diff --git a/README.md b/README.md index 46f6106..9aee155 100644 --- a/README.md +++ b/README.md @@ -22,7 +22,7 @@ Below is a typical setup workflow. > AutoDataGen uses `autosim` as a Python package to organize the code. `autosim` can be installed as a submodule within an environment that already includes Isaac Lab. ```bash -conda create -n AutoDataGen python=3.11 +conda create -n AutoDataGen python=3.12 conda activate AutoDataGen @@ -35,19 +35,20 @@ git submodule update --init --recursive ### IsaacLab Installation -`AutoDataGen` depends on Isaac Lab. You can follow the official installation guide [here](https://isaac-sim.github.io/IsaacLab/v2.2.1/source/setup/installation/pip_installation.html), or use the commands below. If you already have an environment with Isaac Lab installed, you can reuse it and skip this step. +`AutoDataGen` depends on Isaac Lab. You can follow the official installation guide [here](https://isaac-sim.github.io/IsaacLab/v3.0.0-beta/source/setup/installation/pip_installation.html), or use the commands below. If you already have an environment with Isaac Lab installed, you can reuse it and skip this step. ```bash +# Install uv +pip install uv + # Install CUDA toolkit conda install -c "nvidia/label/cuda-12.8.1" cuda-toolkit # Install PyTorch -pip install -U torch==2.7.0 torchvision==0.22.0 --index-url https://download.pytorch.org/whl/cu128 +uv pip install -U torch==2.10.0 torchvision==0.25.0 --index-url https://download.pytorch.org/whl/cu128 # Install IsaacSim -pip install --upgrade pip -pip install "isaacsim[all,extscache]==5.1.0" --extra-index-url https://pypi.nvidia.com - +uv pip install "isaacsim[all,extscache]==6.0.0" --extra-index-url https://pypi.nvidia.com # Install IsaacLab sudo apt install cmake build-essential @@ -65,7 +66,7 @@ Some skills in `autosim` depend on cuRobo. You can follow the official [document ```bash cd dependencies/curobo -pip install -e . --no-build-isolation +uv pip install -e . --no-build-isolation ``` ### autosim Installation @@ -73,7 +74,7 @@ pip install -e . --no-build-isolation Finally, install `autosim` into your environment: ```bash -pip install -e source/autosim +uv pip install -e source/autosim ``` ## Quick Start @@ -85,7 +86,7 @@ After completing the installation and configuration steps above, you can directl First, install the `autosim_examples` package: ```bash -pip install -e source/autosim_examples +uv pip install -e source/autosim_examples ``` Then launch the example with: @@ -94,7 +95,8 @@ Then launch the example with: cd autosim python examples/run_autosim_example.py \ - --pipeline_id AutoSimPipeline-FrankaCubeLift-v0 + --pipeline_id AutoSimPipeline-FrankaCubeLift-v0 \ + --viz kit ``` After launching, you will see in the Isaac Sim UI that the manipulator automatically executes the Cube Lift task. @@ -154,7 +156,7 @@ Then you can launch it with: ```bash cd lw_benchhub -python scripts/autosim/run_autosim_example.py --pipeline_id=LWBenchhub-Autosim-CoffeeSetupMugPipeline-v0 +python scripts/autosim/run_autosim_example.py --pipeline_id=LWBenchhub-Autosim-CoffeeSetupMugPipeline-v0 --viz kit ``` We also support tasks such as CheesyBread, CloseOven, OpenFridge, and KettleBoiling. You can find the corresponding [implementations](https://github.com/LightwheelAI/LW-BenchHub/tree/main/lw_benchhub/autosim) in LW-BenchHub. diff --git a/dependencies/IsaacLab b/dependencies/IsaacLab index 3c6e67b..a4a7602 160000 --- a/dependencies/IsaacLab +++ b/dependencies/IsaacLab @@ -1 +1 @@ -Subproject commit 3c6e67bb5c7ada942a6d1884ab69338f57596f77 +Subproject commit a4a7602f29e755e2673fe0022ea35566df6dd7d5 diff --git a/examples/run_autosim_example.py b/examples/run_autosim_example.py index 6b56d4f..acdb7b1 100644 --- a/examples/run_autosim_example.py +++ b/examples/run_autosim_example.py @@ -9,6 +9,7 @@ parser.add_argument( "--pipeline_id", type=str, default="AutoSimPipeline-FrankaCubeLift-v0", help="Name of the autosim pipeline." ) +parser.add_argument("--num_runs", type=int, default=10, help="Number of times to run the pipeline.") # append AppLauncher cli args AppLauncher.add_app_launcher_args(parser) @@ -28,7 +29,10 @@ def main(): pipeline = make_pipeline(args_cli.pipeline_id) - pipeline.run() + + for i in range(args_cli.num_runs): + print(f"====== run {i + 1} times =======") + pipeline.run() if __name__ == "__main__": diff --git a/examples/visualization/curobo_collision_spheres.py b/examples/visualization/curobo_collision_spheres.py index 0e90363..1b8778e 100644 --- a/examples/visualization/curobo_collision_spheres.py +++ b/examples/visualization/curobo_collision_spheres.py @@ -1,7 +1,7 @@ """Visualize cuRobo robot self-collision spheres during pipeline execution. This script runs an AutoSim pipeline and updates collision sphere + EE frame -visualization after every simulation step. +visualization after every simulation step using debug draw lines (no USD prims created). Usage ----- @@ -18,17 +18,17 @@ Defaults -------- * Environment ID: 0 -* Sphere color: green (0.2, 0.9, 0.2) -* Sphere opacity: 0.4 +* Sphere color: green (0.2, 0.9, 0.2, 0.8) * EE frame scale: 0.1 Notes ----- * Pipeline execution logic is inlined so visualization can hook into every step. * Spheres with radius <= 0 are disabled placeholders and are skipped. -* VisualizationMarkers groups spheres by radius (one USD prototype per unique radius). +* Each sphere is drawn as a 3-axis cross (X/Y/Z lines) scaled by sphere radius. +* EE frame is drawn as three RGB axis lines (R=X, G=Y, B=Z). * ``--hold_on_reach_plan`` intercepts only reach skills after planning succeeds: - it draws the full planned end-effector path, updates markers for inspection, + it draws the full planned end-effector path, updates debug lines for inspection, renders the UI for ``--reach_plan_hold_seconds``, then continues normal execution. """ @@ -67,13 +67,11 @@ app_launcher = AppLauncher(vars(args_cli)) simulation_app = app_launcher.app -import isaaclab.sim as sim_utils -from isaaclab.markers import VisualizationMarkers, VisualizationMarkersCfg -from isaaclab.markers.config import FRAME_MARKER_CFG import autosim_examples # noqa: F401 from autosim import make_pipeline from autosim.core.registration import SkillRegistry +from autosim.utils.data_util import as_torch, convert_quat from autosim.utils.debug_util import clear_debug_drawing, draw_line @@ -88,7 +86,7 @@ def _build_curobo_q(pipeline, env_id: int) -> torch.Tensor: robot = pipeline._robot isaaclab_names = list(robot.data.joint_names) - isaaclab_q = robot.data.joint_pos[env_id] + isaaclab_q = as_torch(robot.data.joint_pos)[env_id] q = torch.zeros(len(planner.target_joint_names), dtype=isaaclab_q.dtype, device=isaaclab_q.device) for i, name in enumerate(planner.target_joint_names): @@ -116,9 +114,9 @@ def _get_spheres_world(pipeline, env_id: int, q_curobo: torch.Tensor | None = No spheres_root = kin_state.robot_spheres[0].detach() # [N, 4] - root_pose = robot.data.root_pose_w[env_id].detach() + root_pose = as_torch(robot.data.root_pose_w)[env_id].detach() robot_root_pos = root_pose[:3] - robot_root_quat = root_pose[3:] # wxyz + robot_root_quat = root_pose[3:] # xyzw (IsaacLab v3.0+) device, dtype = root_pose.device, root_pose.dtype xyz = spheres_root[:, :3].to(device=device, dtype=dtype) @@ -127,7 +125,7 @@ def _get_spheres_world(pipeline, env_id: int, q_curobo: torch.Tensor | None = No n = xyz.shape[0] robot_root_pos_b = robot_root_pos.unsqueeze(0).expand(n, -1) robot_root_quat_b = robot_root_quat.unsqueeze(0).expand(n, -1) - identity = torch.tensor([1.0, 0.0, 0.0, 0.0], device=device, dtype=dtype).unsqueeze(0).expand(n, -1) + identity = torch.tensor([0.0, 0.0, 0.0, 1.0], device=device, dtype=dtype).unsqueeze(0).expand(n, -1) centers_w, _ = PoseUtils.combine_frame_transforms(robot_root_pos_b, robot_root_quat_b, xyz, identity) @@ -138,7 +136,7 @@ def _get_spheres_world(pipeline, env_id: int, q_curobo: torch.Tensor | None = No def _get_ee_pose_world(pipeline, env_id: int, q_curobo: torch.Tensor | None = None) -> torch.Tensor: - """Return EE pose in world frame as [x, y, z, qw, qx, qy, qz] via cuRobo FK.""" + """Return EE pose in world frame as [x, y, z, qx, qy, qz, qw] via cuRobo FK.""" import isaaclab.utils.math as PoseUtils planner = pipeline._motion_planner @@ -150,63 +148,86 @@ def _get_ee_pose_world(pipeline, env_id: int, q_curobo: torch.Tensor | None = No q_curobo = planner._to_curobo_device(q_curobo) ee_pose_root = planner.get_ee_pose(q_curobo) - root_pose = robot.data.root_pose_w[env_id].detach() + root_pose = as_torch(robot.data.root_pose_w)[env_id].detach() rr_pos = root_pose[:3].unsqueeze(0) - rr_quat = root_pose[3:].unsqueeze(0) # wxyz + rr_quat = root_pose[3:].unsqueeze(0) # xyzw device, dtype = root_pose.device, root_pose.dtype ee_pos_root = ee_pose_root.position.view(1, 3).to(device=device, dtype=dtype) - ee_quat_root = ee_pose_root.quaternion.view(1, 4).to(device=device, dtype=dtype) # wxyz + ee_quat_root = convert_quat(ee_pose_root.quaternion.view(1, 4), to="xyzw").to( + device=device, dtype=dtype + ) # cuRobo wxyz → xyzw ee_pos_w, ee_quat_w = PoseUtils.combine_frame_transforms(rr_pos, rr_quat, ee_pos_root, ee_quat_root) - return torch.cat([ee_pos_w, ee_quat_w], dim=-1).squeeze(0) # [7] + return torch.cat([ee_pos_w, ee_quat_w], dim=-1).squeeze(0) -def _create_ee_marker(scale: float) -> VisualizationMarkers: - """Create a frame-axis marker for the EE pose.""" - cfg = FRAME_MARKER_CFG.copy() - cfg.markers["frame"].scale = (scale, scale, scale) - cfg = cfg.replace(prim_path="/World/debug/ee_frame") - return VisualizationMarkers(cfg) +def _draw_ee_frame(pose_w: torch.Tensor, scale: float = 0.1) -> None: + """Draw EE frame axes using debug lines (R=X, G=Y, B=Z).""" + import isaaclab.utils.math as PoseUtils + + pos = pose_w[:3] + quat = pose_w[3:] # xyzw + rot = PoseUtils.matrix_from_quat(quat.unsqueeze(0)).squeeze(0) # (3, 3) + origin = tuple(pos.cpu().tolist()) + for axis_idx, color in enumerate([(1.0, 0.0, 0.0, 1.0), (0.0, 1.0, 0.0, 1.0), (0.0, 0.0, 1.0, 1.0)]): + axis_dir = rot[:, axis_idx] * scale + tip = tuple((pos + axis_dir).cpu().tolist()) + draw_line(origin, tip, color=color, size=3.0) -def _update_ee_marker(vm: VisualizationMarkers, pose_w: torch.Tensor) -> None: - pos = pose_w[:3].unsqueeze(0) # [1, 3] - quat = pose_w[3:].unsqueeze(0) # [1, 4] wxyz - vm.visualize(translations=pos, orientations=quat, marker_indices=[0]) +_CIRCLE_PTS = 16 # segments per great-circle arc +_CIRCLE_ANGLES = np.linspace(0, 2 * np.pi, _CIRCLE_PTS + 1) +_COS = np.cos(_CIRCLE_ANGLES).astype(np.float32) +_SIN = np.sin(_CIRCLE_ANGLES).astype(np.float32) -def _create_markers(unique_radii: np.ndarray, color: list[float], alpha: float) -> VisualizationMarkers: - """Build a VisualizationMarkers with one sphere prototype per unique radius.""" - markers_cfg: dict[str, sim_utils.SphereCfg] = {} - for i, r in enumerate(unique_radii): - markers_cfg[f"sphere_{i}"] = sim_utils.SphereCfg( - radius=float(r), - visual_material=sim_utils.PreviewSurfaceCfg( - diffuse_color=tuple(color), - opacity=alpha, - ), + +def _draw_sphere_wireframe(cx: float, cy: float, cz: float, r: float, color: tuple, size: float = 2.0) -> None: + """Draw a sphere as three orthogonal great circles (XY, YZ, XZ planes).""" + # XY plane + for j in range(_CIRCLE_PTS): + draw_line( + (cx + r * _COS[j], cy + r * _SIN[j], cz), + (cx + r * _COS[j + 1], cy + r * _SIN[j + 1], cz), + color=color, + size=size, + ) + # YZ plane + for j in range(_CIRCLE_PTS): + draw_line( + (cx, cy + r * _COS[j], cz + r * _SIN[j]), + (cx, cy + r * _COS[j + 1], cz + r * _SIN[j + 1]), + color=color, + size=size, + ) + # XZ plane + for j in range(_CIRCLE_PTS): + draw_line( + (cx + r * _COS[j], cy, cz + r * _SIN[j]), + (cx + r * _COS[j + 1], cy, cz + r * _SIN[j + 1]), + color=color, + size=size, ) - cfg = VisualizationMarkersCfg(prim_path="/World/debug/collision_spheres", markers=markers_cfg) - return VisualizationMarkers(cfg) -def _update_markers( - vm: VisualizationMarkers, - positions: np.ndarray, - radii: np.ndarray, - unique_radii: np.ndarray, -) -> None: - radius_to_idx = {float(r): i for i, r in enumerate(unique_radii)} - marker_indices = np.array([radius_to_idx[float(r)] for r in radii], dtype=np.int32) - translations = torch.from_numpy(positions).float() - vm.visualize(translations=translations, marker_indices=marker_indices.tolist()) +def _draw_spheres(positions: np.ndarray, radii: np.ndarray, color: tuple = (0.2, 0.9, 0.2, 0.8)) -> None: + """Draw collision spheres as three orthogonal wireframe circles.""" + for i in range(len(positions)): + _draw_sphere_wireframe( + float(positions[i, 0]), + float(positions[i, 1]), + float(positions[i, 2]), + float(radii[i]), + color, + ) -def _update_visualization(pipeline, env_id, vm_spheres, vm_ee, unique_radii): +def _update_visualization(pipeline, env_id, color: tuple = (0.2, 0.9, 0.2, 0.8), ee_scale: float = 0.1): + clear_debug_drawing() positions, radii = _get_spheres_world(pipeline, env_id) - _update_markers(vm_spheres, positions, radii, unique_radii) - _update_ee_marker(vm_ee, _get_ee_pose_world(pipeline, env_id)) + _draw_spheres(positions, radii, color=color) + _draw_ee_frame(_get_ee_pose_world(pipeline, env_id), scale=ee_scale) def _draw_trajectory_path(pipeline, env_id: int, trajectory, stride: int = 1) -> list[torch.Tensor]: @@ -242,7 +263,7 @@ def _draw_trajectory_path(pipeline, env_id: int, trajectory, stride: int = 1) -> return poses_w -def _visualize_planned_reach(pipeline, env_id, trajectory, vm_spheres, vm_ee, unique_radii, duration_s: float): +def _visualize_planned_reach(pipeline, env_id, trajectory, duration_s: float): """Draw a reach plan, render briefly, then return to normal execution. This is a pre-execution debugging path: after the full reach plan is available, @@ -253,8 +274,8 @@ def _visualize_planned_reach(pipeline, env_id, trajectory, vm_spheres, vm_ee, un _draw_trajectory_path(pipeline, env_id, trajectory) if len(trajectory.position) > 0: positions, radii = _get_spheres_world(pipeline, env_id, trajectory.position[0]) - _update_markers(vm_spheres, positions, radii, unique_radii) - _update_ee_marker(vm_ee, _get_ee_pose_world(pipeline, env_id, trajectory.position[-1])) + _draw_spheres(positions, radii) + _draw_ee_frame(_get_ee_pose_world(pipeline, env_id, trajectory.position[-1])) print( f"Planned reach trajectory with {len(trajectory.position)} waypoints. " f"Rendering for {duration_s:.1f}s before execution." @@ -302,9 +323,9 @@ def _print_link_pose_in_root_frame( print(f"[IsaacLab:{isaaclab_link_name}] link not found. Available: {body_names}") else: idx = body_names.index(isaaclab_link_name) - body_state = robot.data.body_state_w[env_id, idx].detach() - root_pos_w = robot.data.root_pos_w[env_id].detach() - root_quat_w = robot.data.root_quat_w[env_id].detach() # wxyz + body_state = as_torch(robot.data.body_state_w)[env_id, idx].detach() + root_pos_w = as_torch(robot.data.root_pos_w)[env_id].detach() + root_quat_w = as_torch(robot.data.root_quat_w)[env_id].detach() # xyzw pos_il, quat_il = PoseUtils.subtract_frame_transforms( root_pos_w.unsqueeze(0), root_quat_w.unsqueeze(0), @@ -312,18 +333,16 @@ def _print_link_pose_in_root_frame( body_state[3:7].unsqueeze(0), ) pos_il = pos_il.squeeze(0).cpu() - quat_il = quat_il.squeeze(0).cpu() # wxyz - print(f"[IsaacLab:{isaaclab_link_name}] (root frame) pos={pos_il.tolist()} quat(wxyz)={quat_il.tolist()}") + quat_il = quat_il.squeeze(0).cpu() # xyzw + print(f"[IsaacLab:{isaaclab_link_name}] (root frame) pos={pos_il.tolist()} quat(xyzw)={quat_il.tolist()}") -def _execute_single_skill_with_viz( - pipeline, skill, goal, vm_spheres, vm_ee, unique_radii, env_id, curobo_link_name=None, isaaclab_link_name=None -): +def _execute_single_skill_with_viz(pipeline, skill, goal, env_id, curobo_link_name=None, isaaclab_link_name=None): """Execute one skill with visualization hooks. - Normal mode matches AutoSimPipeline's step loop and updates markers after each - simulation step. With ``--hold_on_reach_plan``, reach skills pause briefly after - successful planning so the planned trajectory can be inspected before execution. + Normal mode updates debug-line sphere + EE visualization after each simulation step. + With ``--hold_on_reach_plan``, reach skills pause briefly after successful planning + so the planned trajectory can be inspected before execution. """ world_state = pipeline._build_world_state() plan_success = skill.plan(world_state, goal) @@ -338,9 +357,6 @@ def _execute_single_skill_with_viz( pipeline, env_id, skill._trajectory, - vm_spheres, - vm_ee, - unique_radii, args_cli.reach_plan_hold_seconds, ) @@ -358,7 +374,7 @@ def _execute_single_skill_with_viz( pipeline._generated_actions.append(action) pipeline._env.sim.render() - _update_visualization(pipeline, env_id, vm_spheres, vm_ee, unique_radii) + _update_visualization(pipeline, env_id) if curobo_link_name is not None or isaaclab_link_name is not None: _print_link_pose_in_root_frame(pipeline, env_id, curobo_link_name, isaaclab_link_name) @@ -384,27 +400,17 @@ def _execute_single_skill_with_viz( def main(): env_id = 0 - color = [0.2, 0.9, 0.2] - alpha = 0.4 - ee_scale = 0.1 pipeline = make_pipeline(args_cli.pipeline_id) pipeline.initialize() - # Build markers using the initial robot pose (before reset). The unique radius set - # defines the USD sphere prototypes reused for all later marker updates. - positions, radii = _get_spheres_world(pipeline, env_id) - unique_radii = np.unique(radii) - vm_spheres = _create_markers(unique_radii, color, alpha) - vm_ee = _create_ee_marker(scale=ee_scale) - # Decompose the task before reset, matching the normal AutoSimPipeline.run() order. decompose_result = pipeline.decompose() # After reset, execute the decomposed skills with this script's visualization hooks. pipeline._check_skill_extra_cfg() pipeline.reset_env() - _update_visualization(pipeline, env_id, vm_spheres, vm_ee, unique_radii) + _update_visualization(pipeline, env_id) try: for subtask in decompose_result.subtasks: @@ -422,9 +428,6 @@ def main(): pipeline, skill, goal, - vm_spheres, - vm_ee, - unique_radii, env_id, curobo_link_name=args_cli.curobo_link_name, isaaclab_link_name=args_cli.isaaclab_link_name, diff --git a/examples/visualization/curobo_world_obstacles.py b/examples/visualization/curobo_world_obstacles.py index abb9d3f..3384480 100644 --- a/examples/visualization/curobo_world_obstacles.py +++ b/examples/visualization/curobo_world_obstacles.py @@ -40,6 +40,7 @@ import autosim_examples # noqa: F401 from autosim import make_pipeline +from autosim.utils.data_util import as_torch, convert_quat from autosim.utils.debug_util import clear_debug_drawing, draw_line @@ -128,11 +129,11 @@ def _draw_oriented_box(*, pose_w: _Pose7, half_dims_xyz: torch.Tensor, color, th def _compose_robot_to_world(*, robot_root_pose_w: torch.Tensor, pose_r: _Pose7) -> _Pose7: """Compose pose in robot root frame to world frame.""" rr_pos_w = robot_root_pose_w[:3].view(1, 3) - rr_quat_w = robot_root_pose_w[3:].view(1, 4) + rr_quat_w = robot_root_pose_w[3:].view(1, 4) # xyzw (IsaacLab v3.0) pos_r = pose_r.pos.view(1, 3) - quat_r = pose_r.quat.view(1, 4) + quat_r = convert_quat(pose_r.quat.view(1, 4), to="xyzw") # cuRobo wxyz → xyzw pos_w, quat_w = PoseUtils.combine_frame_transforms(rr_pos_w, rr_quat_w, pos_r, quat_r) - return _Pose7(pos=pos_w.view(3), quat=quat_w.view(4)) + return _Pose7(pos=pos_w.view(3), quat=convert_quat(quat_w.view(4), to="wxyz")) # xyzw → wxyz def _is_obstacle_enabled(collision_checker, obstacle_name: str, env_idx: int = 0) -> bool: @@ -215,7 +216,7 @@ def _visualize_world_obstacles( if world_model is None: raise RuntimeError("cuRobo collision checker has no world model loaded.") - robot_root_pose_w = pipeline._robot.data.root_pose_w[env_id].detach() + robot_root_pose_w = as_torch(pipeline._robot.data.root_pose_w)[env_id].detach() device = robot_root_pose_w.device dtype = robot_root_pose_w.dtype diff --git a/examples/visualization/reach_target_pose.py b/examples/visualization/reach_target_pose.py index f88041f..ec85697 100644 --- a/examples/visualization/reach_target_pose.py +++ b/examples/visualization/reach_target_pose.py @@ -14,7 +14,7 @@ { "object_reach_target_poses": { "": [ - [x, y, z, qw, qx, qy, qz], + [x, y, z, qx, qy, qz, qw], ... ], ... @@ -23,7 +23,7 @@ Notes ----- -* Poses are in the object frame: [x, y, z, qw, qx, qy, qz]. +* Poses are in the object frame: [x, y, z, qx, qy, qz, qw]. * `--live_poll_interval_s` controls how often the file is checked (default: 0.2s). """ @@ -68,6 +68,7 @@ import autosim_examples # noqa: F401 from autosim import make_pipeline +from autosim.utils.data_util import as_torch from autosim.utils.debug_util import visualize_reach_target_poses @@ -110,7 +111,7 @@ def _apply_live_poses(*, poses_path: str, pipeline) -> None: for obj_name, pose_list in object_reach_target_poses.items(): if obj_name not in env.scene.keys(): continue - obj_pose_w = env.scene[obj_name].data.root_pose_w[0] # [7] + obj_pose_w = as_torch(env.scene[obj_name].data.root_pose_w)[0] # [7] device = obj_pose_w.device dtype = obj_pose_w.dtype env_extra_info.object_reach_target_poses[obj_name] = [ @@ -128,7 +129,7 @@ def _snapshot_object_poses_w(*, env, object_names: list[str]) -> dict[str, list[ if obj_name not in env.scene.keys(): print(f"[reach_target_pose] Skip missing scene object: {obj_name}") continue - pose_w = env.scene[obj_name].data.root_pose_w[0] + pose_w = as_torch(env.scene[obj_name].data.root_pose_w)[0] poses_w[obj_name] = [float(v) for v in pose_w.detach().cpu().tolist()] return poses_w diff --git a/source/autosim/autosim/calibration/plan_sweep.py b/source/autosim/autosim/calibration/plan_sweep.py index 18280e0..923c264 100644 --- a/source/autosim/autosim/calibration/plan_sweep.py +++ b/source/autosim/autosim/calibration/plan_sweep.py @@ -9,6 +9,7 @@ from autosim.core.pipeline import AutoSimPipeline from autosim.core.registration import SkillRegistry from autosim.skills.reach import ReachSkill +from autosim.utils.data_util import as_torch from .pose_sampler import OffsetSampler, PoseSampler @@ -38,15 +39,15 @@ def _fmt_pose(vals: list[float]) -> str: def _quat_mul(q1: torch.Tensor, q2: torch.Tensor) -> torch.Tensor: - """Multiply quaternions in wxyz format elementwise over the leading dimensions.""" - w1, x1, y1, z1 = q1.unbind(-1) - w2, x2, y2, z2 = q2.unbind(-1) + """Multiply quaternions in xyzw format elementwise over the leading dimensions.""" + x1, y1, z1, w1 = q1.unbind(-1) + x2, y2, z2, w2 = q2.unbind(-1) return torch.stack( [ - w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2, w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2, w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2, w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2, + w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2, ], dim=-1, ) @@ -60,7 +61,7 @@ def _uniform_yaw_rotations(device: torch.device, dtype: torch.dtype, num_rotatio for idx in range(num_rotations): yaw = (2.0 * math.pi * idx) / num_rotations half = yaw * 0.5 - rotations.append(torch.tensor([math.cos(half), 0.0, 0.0, math.sin(half)], device=device, dtype=dtype)) + rotations.append(torch.tensor([0.0, 0.0, math.sin(half), math.cos(half)], device=device, dtype=dtype)) return rotations @@ -174,7 +175,7 @@ def _sweep_all_rotations( env_extra_info = pipeline._env_extra_info obj = env.scene[obj_name] - original_pose_w = obj.data.root_pose_w[pipeline._env_id].clone() + original_pose_w = as_torch(obj.data.root_pose_w)[pipeline._env_id].clone() base_quat_w = original_pose_w[3:].clone() env_ids = torch.tensor([pipeline._env_id], device=original_pose_w.device, dtype=torch.int32) results: list[dict[str, Any]] = [] @@ -319,14 +320,14 @@ def _sweep( poses_oe = cfg.sampling.sample(base_pose_oe) k = int(poses_oe.shape[0]) - obj_pose_w = env.scene[obj_name].data.root_pose_w[env_id] + obj_pose_w = as_torch(env.scene[obj_name].data.root_pose_w)[env_id] obj_pos_w = obj_pose_w[:3].view(1, 3).repeat(k, 1) obj_quat_w = obj_pose_w[3:].view(1, 4).repeat(k, 1) target_pos_w, target_quat_w = PoseUtils.combine_frame_transforms( obj_pos_w, obj_quat_w, poses_oe[:, :3], poses_oe[:, 3:] ) - robot_root_pose_w = robot.data.root_pose_w[env_id] + robot_root_pose_w = as_torch(robot.data.root_pose_w)[env_id] rr_pos_w = robot_root_pose_w[:3].view(1, 3).repeat(k, 1) rr_quat_w = robot_root_pose_w[3:].view(1, 4).repeat(k, 1) target_pos_r, target_quat_r = PoseUtils.subtract_frame_transforms(rr_pos_w, rr_quat_w, target_pos_w, target_quat_w) diff --git a/source/autosim/autosim/calibration/pose_sampler.py b/source/autosim/autosim/calibration/pose_sampler.py index 2254e9f..9043a79 100644 --- a/source/autosim/autosim/calibration/pose_sampler.py +++ b/source/autosim/autosim/calibration/pose_sampler.py @@ -10,7 +10,7 @@ def sample(self, base_pose_oe: torch.Tensor) -> torch.Tensor: Sample candidate poses around a base pose in object frame. Args: - base_pose_oe: Tensor shape [7] in object frame [x,y,z,qw,qx,qy,qz] + base_pose_oe: Tensor shape [7] in object frame [x,y,z,qx,qy,qz,qw] Returns: poses_oe: Tensor [K, 7] of sampled poses in object frame @@ -55,7 +55,7 @@ def sample(self, base_pose_oe: torch.Tensor) -> torch.Tensor: base_quat = base_pose_oe[3:].view(1, 4).repeat(k, 1) half = dyaw * 0.5 q_delta = torch.stack( - [torch.cos(half), torch.zeros_like(half), torch.zeros_like(half), torch.sin(half)], dim=-1 + [torch.zeros_like(half), torch.zeros_like(half), torch.sin(half), torch.cos(half)], dim=-1 ) quat = self._quat_mul(base_quat, q_delta) @@ -63,14 +63,14 @@ def sample(self, base_pose_oe: torch.Tensor) -> torch.Tensor: @staticmethod def _quat_mul(q1: torch.Tensor, q2: torch.Tensor) -> torch.Tensor: - w1, x1, y1, z1 = q1.unbind(-1) - w2, x2, y2, z2 = q2.unbind(-1) + x1, y1, z1, w1 = q1.unbind(-1) + x2, y2, z2, w2 = q2.unbind(-1) return torch.stack( [ - w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2, w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2, w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2, w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2, + w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2, ], dim=-1, ) diff --git a/source/autosim/autosim/capabilities/motion_planning/curobo/curobo_planner.py b/source/autosim/autosim/capabilities/motion_planning/curobo/curobo_planner.py index 346911f..702302e 100644 --- a/source/autosim/autosim/capabilities/motion_planning/curobo/curobo_planner.py +++ b/source/autosim/autosim/capabilities/motion_planning/curobo/curobo_planner.py @@ -25,6 +25,7 @@ from pxr import UsdGeom, UsdPhysics from autosim.core.logger import AutoSimLogger +from autosim.utils.data_util import as_torch, convert_quat if TYPE_CHECKING: from .curobo_planner_cfg import CuroboPlannerCfg @@ -35,7 +36,7 @@ class CuroboPlanner: # Identity offset for primitives that should directly follow link pose _IDENTITY_OFFSET_POS = torch.zeros(3) - _IDENTITY_OFFSET_QUAT = torch.tensor([1.0, 0.0, 0.0, 0.0]) # w, x, y, z + _IDENTITY_OFFSET_QUAT = torch.tensor([0.0, 0.0, 0.0, 1.0]) # x, y, z, w (xyzw) def __init__( self, @@ -54,7 +55,9 @@ def __init__( # Cache frequently used paths self._env_prim_path = f"/World/envs/env_{self._env_id}" - self._env_scene_prefix = f"{self._env_prim_path}/Scene" + self._env_scene_prefix = ( + f"{self._env_prim_path}/{self.cfg.env_scene_prefix}" if self.cfg.env_scene_prefix else self._env_prim_path + ) # Initialize logger log_level = logging.DEBUG if self.cfg.debug_planner else logging.INFO @@ -333,8 +336,8 @@ def _get_articulated_link_poses_from_usd( xform_cache = UsdGeom.XformCache() world_only_subffixes_paths = [f"{self._env_prim_path}/{sub}" for sub in self.cfg.world_only_subffixes or []] - robot_root_pos_w = self._robot.data.root_pos_w[self._env_id].unsqueeze(0) # [1, 3] - robot_root_quat_w = self._robot.data.root_quat_w[self._env_id].unsqueeze(0) # [1, 4] + robot_root_pos_w = as_torch(self._robot.data.root_pos_w)[self._env_id].unsqueeze(0) # [1, 3] + robot_root_quat_w = as_torch(self._robot.data.root_quat_w)[self._env_id].unsqueeze(0) # [1, 4] result = {} @@ -357,7 +360,7 @@ def _get_articulated_link_poses_from_usd( link_mat, _ = get_prim_world_pose(xform_cache, child_prim) link_pose = Pose.from_matrix(torch.tensor(link_mat, dtype=torch.float32).unsqueeze(0)) link_pos_w = link_pose.position # [1, 3] - link_quat_w = link_pose.quaternion # [1, 4] + link_quat_w = convert_quat(link_pose.quaternion, to="xyzw") # cuRobo wxyz → xyzw link_pos_in_robot, link_quat_in_robot = subtract_frame_transforms( robot_root_pos_w, @@ -391,8 +394,8 @@ def _get_articulated_link_poses( articulations = self._env.scene.articulations world_only_subffixes_paths = [f"{self._env_prim_path}/{sub}" for sub in self.cfg.world_only_subffixes or []] - robot_root_pos_w = self._robot.data.root_pos_w - robot_root_quat_w = self._robot.data.root_quat_w + robot_root_pos_w = as_torch(self._robot.data.root_pos_w) + robot_root_quat_w = as_torch(self._robot.data.root_quat_w) result = {} @@ -400,7 +403,7 @@ def _get_articulated_link_poses( if f"{self._env_scene_prefix}/{obj_name}" not in world_only_subffixes_paths: continue - body_pos_w, body_quat_w = articulation.data.body_pos_w, articulation.data.body_quat_w + body_pos_w, body_quat_w = as_torch(articulation.data.body_pos_w), as_torch(articulation.data.body_quat_w) body_count = body_pos_w.shape[1] body_pos_in_robot, body_quat_in_robot = subtract_frame_transforms( robot_root_pos_w.repeat(1, body_count, 1), @@ -466,7 +469,8 @@ def _build_articulated_primitive_offsets(self) -> None: for child_prim_name in child_prim_names: child_obstacle = self.motion_gen.world_coll_checker.world_model.get_obstacle(child_prim_name) child_pose = Pose.from_list(child_obstacle.pose, tensor_args=self.tensor_args) - child_pos, child_quat = child_pose.position.squeeze(0), child_pose.quaternion.squeeze(0) + child_pos = child_pose.position.squeeze(0) + child_quat = convert_quat(child_pose.quaternion.squeeze(0), to="xyzw") # cuRobo wxyz → xyzw offset_pos, offset_quat = subtract_frame_transforms( link_pos.unsqueeze(0), link_quat.unsqueeze(0), child_pos.unsqueeze(0), child_quat.unsqueeze(0) @@ -519,7 +523,7 @@ def _sync_articulated_obstacles(self) -> None: primitive_name, Pose( position=self._to_curobo_device(primitive_pos), - quaternion=self._to_curobo_device(primitive_quat), + quaternion=self._to_curobo_device(convert_quat(primitive_quat, to="wxyz")), # xyzw → wxyz ), env_idx=self._env_id, update_cpu_reference=True, @@ -577,8 +581,21 @@ def _refine_curobo_world_collision(self) -> None: - Articulated object link poses (if articulated offsets are cached) Called automatically before each planning operation. + + NOTE: collision geometry is derived from articulation.data (direct PhysX buffer reads), + not from the viewport rendering. When use_fabric=False, the rendered visual state may + be slightly inconsistent with the true physics state due to USD Stage sync uncertainty, + while articulation.data always reflects the correct current state. It is recommended to + use use_fabric=True to keep the visual output consistent with the collision geometry used here. """ + use_fabric = self._env.cfg.sim.use_fabric + if not use_fabric: + self._logger.warning( + f"use_fabric in your isaaclab env: {use_fabric}. curobo articulated collision may be inaccurate, it's" + " recommended to use use_fabric=True" + ) + if self.cfg.enable_dynamic_world_sync: self._sync_dynamic_objects() self._sync_articulated_obstacles() @@ -599,7 +616,8 @@ def _sync_dynamic_objects(self) -> int: return 0 rigid_objects = self._env.scene.rigid_objects - robot_root_pos_in_world, robot_root_quat_in_world = self._robot.data.root_pos_w, self._robot.data.root_quat_w + robot_root_pos_in_world = as_torch(self._robot.data.root_pos_w) + robot_root_quat_in_world = as_torch(self._robot.data.root_quat_w) updated_count = 0 @@ -623,13 +641,15 @@ def _sync_dynamic_objects(self) -> int: for object_name, world_obstacle_names in object_mappings.items(): obj = rigid_objects[object_name] # NOTE: cuRobo world model is in the robot-root frame - obj_pos_in_world, obj_quat_in_world = obj.data.root_pos_w, obj.data.root_quat_w + obj_pos_in_world, obj_quat_in_world = as_torch(obj.data.root_pos_w), as_torch(obj.data.root_quat_w) obj_pos_in_robot_root, obj_quat_in_robot_root = subtract_frame_transforms( robot_root_pos_in_world, robot_root_quat_in_world, obj_pos_in_world, obj_quat_in_world ) obj_pose = Pose( position=self._to_curobo_device(obj_pos_in_robot_root[self._env_id]), - quaternion=self._to_curobo_device(obj_quat_in_robot_root[self._env_id]), + quaternion=self._to_curobo_device( + convert_quat(obj_quat_in_robot_root[self._env_id], to="wxyz") # xyzw → wxyz + ), ) # Determine if this object should be enabled @@ -666,7 +686,7 @@ def plan_motion( Args: target_pos: Target position [x, y, z] - target_quat: Target quaternion [qw, qx, qy, qz] + target_quat: Target quaternion [qx, qy, qz, qw] current_q: Current joint positions current_qd: Current joint velocities link_goals: Optional dictionary mapping link names to target poses for other links @@ -699,7 +719,7 @@ def plan_motion( # build the target pose goal = Pose( position=self._to_curobo_device(target_pos), - quaternion=self._to_curobo_device(target_quat), + quaternion=self._to_curobo_device(convert_quat(target_quat, to="wxyz")), # xyzw → wxyz ) # build the current state @@ -718,7 +738,10 @@ def plan_motion( if link_goals is not None: # Use provided link goals link_poses = { - link_name: Pose(position=self._to_curobo_device(pose[:3]), quaternion=self._to_curobo_device(pose[3:])) + link_name: Pose( + position=self._to_curobo_device(pose[:3]), + quaternion=self._to_curobo_device(convert_quat(pose[3:], to="wxyz")), # xyzw → wxyz + ) for link_name, pose in link_goals.items() } @@ -770,11 +793,11 @@ def plan_motion_batch( Args: target_pos: Tensor of shape [K, 3], in robot root frame. - target_quat: Tensor of shape [K, 4] in [qw, qx, qy, qz], in robot root frame. + target_quat: Tensor of shape [K, 4] in [qx, qy, qz, qw], in robot root frame. current_q: Tensor of shape [dof], current joint positions. current_qd: Tensor of shape [dof], current joint velocities. Defaults to zeros. link_goals: Optional dict mapping extra link names to tensors of shape [K, 7] - ([x, y, z, qw, qx, qy, qz], robot root frame) for multi-arm robots. Each entry + ([x, y, z, qx, qy, qz, qw], robot root frame) for multi-arm robots. Each entry specifies the simultaneous target pose of that link for every sample in the batch. Returns: @@ -816,7 +839,7 @@ def plan_motion_batch( goal = Pose( position=self._to_curobo_device(target_pos), - quaternion=self._to_curobo_device(target_quat), + quaternion=self._to_curobo_device(convert_quat(target_quat, to="wxyz")), # xyzw → wxyz ) start_state = JointState( @@ -832,7 +855,7 @@ def plan_motion_batch( link_poses = { ee_name: Pose( position=self._to_curobo_device(poses[:, :3]), - quaternion=self._to_curobo_device(poses[:, 3:]), + quaternion=self._to_curobo_device(convert_quat(poses[:, 3:], to="wxyz")), # xyzw → wxyz ) for ee_name, poses in link_goals.items() } @@ -855,9 +878,9 @@ def solve_ik_batch( Args: target_pos: Tensor of shape [K, 3], in robot root frame. - target_quat: Tensor of shape [K, 4] in [qw, qx, qy, qz], in robot root frame. + target_quat: Tensor of shape [K, 4] in [qx, qy, qz, qw], in robot root frame. link_goals: Optional dict mapping extra link names to tensors of shape [K, 7] - ([x, y, z, qw, qx, qy, qz], robot root frame) for multi-arm robots. + ([x, y, z, qx, qy, qz, qw], robot root frame) for multi-arm robots. Returns: IKResult from cuRobo. Check result.success[k], result.position_error[k], @@ -879,14 +902,14 @@ def solve_ik_batch( goal = Pose( position=self._to_curobo_device(target_pos), - quaternion=self._to_curobo_device(target_quat), + quaternion=self._to_curobo_device(convert_quat(target_quat, to="wxyz")), # xyzw → wxyz ) link_poses = None if link_goals is not None: link_poses = { ee_name: Pose( position=self._to_curobo_device(poses[:, :3]), - quaternion=self._to_curobo_device(poses[:, 3:]), + quaternion=self._to_curobo_device(convert_quat(poses[:, 3:], to="wxyz")), # xyzw → wxyz ) for ee_name, poses in link_goals.items() } diff --git a/source/autosim/autosim/capabilities/motion_planning/curobo/curobo_planner_cfg.py b/source/autosim/autosim/capabilities/motion_planning/curobo/curobo_planner_cfg.py index f8ca3f8..d5bfd7b 100644 --- a/source/autosim/autosim/capabilities/motion_planning/curobo/curobo_planner_cfg.py +++ b/source/autosim/autosim/capabilities/motion_planning/curobo/curobo_planner_cfg.py @@ -77,3 +77,6 @@ class CuroboPlannerCfg: """Enable detailed motion planning debug information.""" cuda_device: int | None = 0 """Preferred CUDA device index; None uses torch.cuda.current_device() (respects CUDA_VISIBLE_DEVICES).""" + + env_scene_prefix: str | None = "Scene" + """Prefix of the environment scene.""" diff --git a/source/autosim/autosim/capabilities/navigation/occupancy_map.py b/source/autosim/autosim/capabilities/navigation/occupancy_map.py index 28fed16..ebaf4e8 100644 --- a/source/autosim/autosim/capabilities/navigation/occupancy_map.py +++ b/source/autosim/autosim/capabilities/navigation/occupancy_map.py @@ -26,6 +26,7 @@ from autosim.core.logger import AutoSimLogger from autosim.core.types import MapBounds, OccupancyMap +from autosim.utils.data_util import as_torch _logger = AutoSimLogger("OccupancyMap") @@ -334,9 +335,9 @@ def _collect_candidate_prim_paths( # ----------------------------------------------------------------------------- -def _matrix4d_from_pos_quat(pos: np.ndarray, quat_wxyz: np.ndarray) -> Gf.Matrix4d: +def _matrix4d_from_pos_quat(pos: np.ndarray, quat_xyzw: np.ndarray) -> Gf.Matrix4d: rot = Gf.Rotation( - Gf.Quatd(float(quat_wxyz[0]), Gf.Vec3d(float(quat_wxyz[1]), float(quat_wxyz[2]), float(quat_wxyz[3]))) + Gf.Quatd(float(quat_xyzw[3]), Gf.Vec3d(float(quat_xyzw[0]), float(quat_xyzw[1]), float(quat_xyzw[2]))) ) return Gf.Matrix4d().SetTransform(rot, Gf.Vec3d(float(pos[0]), float(pos[1]), float(pos[2]))) @@ -391,7 +392,7 @@ def _prim_world_matrix_from_body_frame( prim: Usd.Prim, body_prim: Usd.Prim, body_pos_w: np.ndarray, - body_quat_wxyz: np.ndarray, + body_quat_xyzw: np.ndarray, xform_cache: UsdGeom.XformCache, ) -> Gf.Matrix4d: """World transform: T_world_prim = T_world_body(scene) @ T_body_prim(usd_fixed).""" @@ -401,7 +402,7 @@ def _prim_world_matrix_from_body_frame( # Rigid offset of collision prim in link/body frame; invariant to joint angle. prim_in_body = prim_usd * body_usd.GetInverse() - return _matrix4d_from_pos_quat(body_pos_w, body_quat_wxyz) * prim_in_body + return _matrix4d_from_pos_quat(body_pos_w, body_quat_xyzw) * prim_in_body def _link_name_under_prefix(path_str: str, prefix_norm: str) -> str: @@ -441,7 +442,7 @@ def _get_prim_world_matrix_for_scene_asset( return None link_idx = asset.body_names.index(link_name) body_pos, body_quat = _to_pose_numpy( - asset.data.body_pos_w[env_id, link_idx], asset.data.body_quat_w[env_id, link_idx] + as_torch(asset.data.body_pos_w)[env_id, link_idx], as_torch(asset.data.body_quat_w)[env_id, link_idx] ) return _prim_world_matrix_from_body_frame(prim, body_prim, body_pos, body_quat, xform_cache) @@ -450,7 +451,9 @@ def _get_prim_world_matrix_for_scene_asset( if not body_prim.IsValid(): _logger.warning(f"USD root prim missing at '{prefix_norm}' for rigid object '{obj_name}'") return None - body_pos, body_quat = _to_pose_numpy(asset.data.root_pos_w[env_id], asset.data.root_quat_w[env_id]) + body_pos, body_quat = _to_pose_numpy( + as_torch(asset.data.root_pos_w)[env_id], as_torch(asset.data.root_quat_w)[env_id] + ) return _prim_world_matrix_from_body_frame(prim, body_prim, body_pos, body_quat, xform_cache) _logger.debug(f"Scene asset '{obj_name}' is not Articulation/RigidObject; skipping '{path_str}'") diff --git a/source/autosim/autosim/core/pipeline.py b/source/autosim/autosim/core/pipeline.py index b562bd3..9066985 100644 --- a/source/autosim/autosim/core/pipeline.py +++ b/source/autosim/autosim/core/pipeline.py @@ -14,6 +14,7 @@ CuroboSkillExtraCfg, NavigateSkillExtraCfg, ) +from autosim.utils.data_util import as_torch from .action_adapter import ActionAdapterBase, ActionAdapterCfg from .decomposer import Decomposer, DecomposerCfg @@ -103,6 +104,9 @@ def initialize(self) -> None: # set the initialized flag self._initialized = True + # target objects to be manipulated + self._target_objects: list[str] = None + def run(self) -> PipelineOutput: """Run the AutoSim pipeline.""" @@ -147,6 +151,7 @@ def decompose(self) -> DecomposeResult: else: decompose_result: DecomposeResult = self._decomposer.decompose(self._env_extra_info) self._decomposer.write_cache(self._env_extra_info.task_name, decompose_result) + self._decompose_result = decompose_result return decompose_result def execute_skill_sequence(self, decompose_result: DecomposeResult): @@ -242,31 +247,33 @@ def _execute_single_skill(self, skill: Skill, goal: SkillGoal) -> tuple[bool, in def _build_world_state(self) -> WorldState: """Build the world state.""" - joint_pos_limits = self._robot.data.joint_pos_limits[self._env_id, :, :] + joint_pos_limits = as_torch(self._robot.data.joint_pos_limits)[self._env_id, :, :] lower, upper = joint_pos_limits[:, 0], joint_pos_limits[:, 1] - robot_joint_pos = torch.clamp(self._robot.data.joint_pos[self._env_id, :], min=lower, max=upper) + robot_joint_pos = torch.clamp(as_torch(self._robot.data.joint_pos)[self._env_id, :], min=lower, max=upper) - robot_joint_vel = self._robot.data.joint_vel[self._env_id, :] - robot_ee_pose = self._robot.data.body_link_pose_w[self._env_id, self._eef_link_idx] + robot_joint_vel = as_torch(self._robot.data.joint_vel)[self._env_id, :] + robot_ee_pose = as_torch(self._robot.data.body_link_pose_w)[self._env_id, self._eef_link_idx] - robot_base_pose = self._robot.data.body_link_pose_w[ + robot_base_pose = as_torch(self._robot.data.body_link_pose_w)[ self._env_id, self._robot_base_link_idx - ] # [x, y, z, qw, qx, qy, qz] - w, x, y, z = robot_base_pose[3:7] + ] # [x, y, z, qx, qy, qz, qw] + x, y, z, w = robot_base_pose[3:7] sin_yaw = 2 * (w * z + x * y) cos_yaw = 1 - 2 * (y**2 + z**2) yaw = torch.atan2(sin_yaw, cos_yaw) robot_base_pose = torch.stack((robot_base_pose[0], robot_base_pose[1], yaw)) # [x, y, yaw] - robot_root_pose = self._robot.data.root_pose_w[self._env_id] + robot_root_pose = as_torch(self._robot.data.root_pose_w)[self._env_id] sim_joint_names = self._robot.data.joint_names objects_dict = dict() - for obj_name in self._env.scene.keys(): + if self._target_objects is None: + self._target_objects = self._decompose_result.get_target_objects() + for obj_name in self._target_objects: obj = self._env.scene[obj_name] if hasattr(obj, "data") and hasattr(obj.data, "root_pose_w") and obj_name != self._robot_name: - objects_dict[obj_name] = obj.data.root_pose_w[self._env_id] + objects_dict[obj_name] = as_torch(obj.data.root_pose_w)[self._env_id] return WorldState( robot_joint_pos=robot_joint_pos, diff --git a/source/autosim/autosim/core/types.py b/source/autosim/autosim/core/types.py index bc0b7ec..2b5ee98 100644 --- a/source/autosim/autosim/core/types.py +++ b/source/autosim/autosim/core/types.py @@ -88,7 +88,7 @@ class EnvExtraInfo: object_reach_target_poses: dict[str, list[torch.Tensor]] = field(default_factory=dict) """The reach target poses in the objects frame. - Each object maps to a list of candidate reach target poses ``[x, y, z, qw, qx, qy, qz]``. + Each object maps to a list of candidate reach target poses ``[x, y, z, qx, qy, qz, qw]``. When a reach skill is executed, the candidate closest to the robot end-effector's current pose in the object frame is selected. """ @@ -129,11 +129,11 @@ class WorldState: robot_joint_vel: torch.Tensor """The joint velocities of the robot.""" robot_ee_pose: torch.Tensor - """The end - effector pose of the robot in the world frame. [x, y, z, qw, qx, qy, qz]""" + """The end - effector pose of the robot in the world frame. [x, y, z, qx, qy, qz, qw]""" robot_base_pose: torch.Tensor """The base pose of the robot in the world frame. [x, y, yaw]""" robot_root_pose: torch.Tensor - """The root pose of the robot in the world frame. [x, y, z, qw, qx, qy, qz]""" + """The root pose of the robot in the world frame. [x, y, z, qx, qy, qz, qw]""" sim_joint_names: list[str] """The joint names of the robot.""" objects: dict[str, torch.Tensor] = field(default_factory=dict) @@ -233,7 +233,7 @@ class DecomposeResult: """The objects of the task.""" fixtures: list[FixtureInfo] """The fixtures of the task.""" - interactive_elements: list[str] + interactive_elements: list[str] | list[dict] """The interactive elements of the task.""" subtasks: list[SubtaskResult] """The subtasks of the task.""" @@ -244,6 +244,18 @@ class DecomposeResult: skill_sequence: list[str] """The sequence of skills in the task.""" + def get_target_objects(self) -> list[str]: + """Return deduplicated target object names across all skills, excluding 'none'/empty.""" + seen = set() + result = [] + for subtask in self.subtasks: + for skill in subtask.skills: + name = skill.target_object + if name and name.lower() != "none" and name not in seen: + seen.add(name) + result.append(name) + return result + """NAVIGATION RELATED TYPES""" diff --git a/source/autosim/autosim/skills/navigate.py b/source/autosim/autosim/skills/navigate.py index 69d0f52..7e0eefb 100644 --- a/source/autosim/autosim/skills/navigate.py +++ b/source/autosim/autosim/skills/navigate.py @@ -16,6 +16,7 @@ SkillOutput, WorldState, ) +from autosim.utils.data_util import as_torch from autosim.utils.debug_util import debug_visualize_goal_sampling @@ -99,8 +100,8 @@ def extract_goal_from_info( if target_object_name in per_obj_yaw: self.cfg.extra_cfg.yaw_tolerance = per_obj_yaw[target_object_name] - obj_pos_w = target_object.data.root_pos_w[0].cpu().numpy() - self._logger.info(f"Object pose in world frame: {target_object.data.root_pose_w[0]}") + obj_pos_w = as_torch(target_object.data.root_pos_w)[0].cpu().numpy() + self._logger.info(f"Object pose in world frame: {as_torch(target_object.data.root_pose_w)[0]}") is_free = (self._occupancy_map.occupancy_map == 0).cpu().numpy() if np.any(is_free): @@ -230,7 +231,7 @@ def step(self, state: WorldState) -> SkillOutput: # If we are not in the final approach, try to face the object if not is_final_approach: - obj_tensor = state.objects[self._target_object_name] # [x, y, z, qw, qx, qy, qz] + obj_tensor = state.objects[self._target_object_name] # [x, y, z, qx, qy, qz, qw] obj_pos = obj_tensor[:2] # [x, y] in world frame dx_obj = obj_pos[0] - current_pose[0] diff --git a/source/autosim/autosim/skills/reach.py b/source/autosim/autosim/skills/reach.py index 349d9ab..872a52f 100644 --- a/source/autosim/autosim/skills/reach.py +++ b/source/autosim/autosim/skills/reach.py @@ -15,6 +15,7 @@ SkillOutput, WorldState, ) +from autosim.utils.data_util import as_torch, convert_quat from .base_skill import CuroboSkillBase, CuroboSkillExtraCfg @@ -69,10 +70,10 @@ def _get_current_primary_and_extra_link_poses( primary_pose_in_robot_root = current_link_poses[self._planner.motion_gen.kinematics.ee_link] primary_link_pose_in_robot_root = ( primary_pose_in_robot_root.position, - primary_pose_in_robot_root.quaternion, + convert_quat(primary_pose_in_robot_root.quaternion, to="xyzw"), # cuRobo wxyz → xyzw ) extra_link_poses_in_robot_root = { - link_name: (pose.position, pose.quaternion) + link_name: (pose.position, convert_quat(pose.quaternion, to="xyzw")) # cuRobo wxyz → xyzw for link_name, pose in current_link_poses.items() if link_name != self._planner.motion_gen.kinematics.ee_link } @@ -140,7 +141,7 @@ def _build_extra_target_poses( """Build link-level extra target poses based on configuration. This is the dispatcher for `extra_target_mode`. It returns a dict mapping link names to pose - tensors in `[x, y, z, qw, qx, qy, qz]` (single-sample), used as additional link goals/constraints + tensors in `[x, y, z, qx, qy, qz, qw]` (single-sample), used as additional link goals/constraints during planning. """ @@ -166,7 +167,9 @@ def _build_keep_current_extra_target_poses(self, activate_q: torch.Tensor) -> di for link_name, pose in self._planner.get_link_poses( activate_q, self.cfg.extra_cfg.extra_target_link_names ).items(): - extra_target_poses[link_name] = torch.cat((pose.position, pose.quaternion), dim=-1).squeeze(0) + extra_target_poses[link_name] = torch.cat( + (pose.position, convert_quat(pose.quaternion, to="xyzw")), dim=-1 # cuRobo wxyz → xyzw + ).squeeze(0) return extra_target_poses @@ -235,7 +238,7 @@ def _compute_goal_from_offset( SkillGoal with target poses in robot root frame. """ - object_pose_in_env = env.scene[target_object].data.root_pose_w + object_pose_in_env = as_torch(env.scene[target_object].data.root_pose_w) object_pos_in_env = object_pose_in_env[:, :3] object_quat_in_env = object_pose_in_env[:, 3:] @@ -250,8 +253,8 @@ def _compute_goal_from_offset( self.visualize_debug_target_pose() robot = env.scene[env_extra_info.robot_name] - robot_root_pos_in_env = robot.data.root_pose_w[:, :3] - robot_root_quat_in_env = robot.data.root_pose_w[:, 3:] + robot_root_pos_in_env = as_torch(robot.data.root_pose_w)[:, :3] + robot_root_quat_in_env = as_torch(robot.data.root_pose_w)[:, 3:] reach_target_pos_in_root, reach_target_quat_in_root = PoseUtils.subtract_frame_transforms( robot_root_pos_in_env, robot_root_quat_in_env, reach_target_pos_in_env, reach_target_quat_in_env @@ -262,7 +265,7 @@ def _compute_goal_from_offset( ) activate_q, _ = self._build_activate_joint_state( - robot.data.joint_names, robot.data.joint_pos[0], robot.data.joint_vel[0] + robot.data.joint_names, as_torch(robot.data.joint_pos)[0], as_torch(robot.data.joint_vel)[0] ) extra_target_poses = self._build_extra_target_poses(activate_q, target_pose, env_extra_info) @@ -302,8 +305,8 @@ def _select_best_candidate( robot = env.scene[env_extra_info.robot_name] ee_link_idx = robot.data.body_names.index(env_extra_info.ee_link_name) - ee_pose_w = robot.data.body_link_pose_w[0, ee_link_idx] # [7] - obj_pose_w = env.scene[target_object].data.root_pose_w[0] # [7] + ee_pose_w = as_torch(robot.data.body_link_pose_w)[0, ee_link_idx] # [7] + obj_pose_w = as_torch(env.scene[target_object].data.root_pose_w)[0] # [7] ee_pos_oe, ee_quat_oe = PoseUtils.subtract_frame_transforms( obj_pose_w[:3].unsqueeze(0), @@ -347,7 +350,7 @@ def _compute_corrective_goal(self) -> SkillGoal | None: def extract_goal_from_info( self, skill_info: SkillInfo, env: ManagerBasedEnv, env_extra_info: EnvExtraInfo ) -> SkillGoal: - """Return the target pose[x, y, z, qw, qx, qy, qz] in the robot root frame. + """Return the target pose[x, y, z, qx, qy, qz, qw] in the robot root frame. IMPORTANT: the robot root frame is not the same as the robot base frame. """ @@ -442,7 +445,9 @@ def step(self, state: WorldState) -> SkillOutput: activate_q, _ = self._build_activate_joint_state(state.sim_joint_names, joint_pos, None) all_link_poses = self._planner.get_link_poses(activate_q, link_names=None) info["link_poses_in_robot_root_frame"] = { - name: torch.cat([pose.position.squeeze(0), pose.quaternion.squeeze(0)]) + name: torch.cat( + [pose.position.squeeze(0), convert_quat(pose.quaternion.squeeze(0), to="xyzw")] + ) # cuRobo wxyz → xyzw for name, pose in all_link_poses.items() } diff --git a/source/autosim/autosim/skills/relative_reach.py b/source/autosim/autosim/skills/relative_reach.py index b83a59f..9bc3b77 100644 --- a/source/autosim/autosim/skills/relative_reach.py +++ b/source/autosim/autosim/skills/relative_reach.py @@ -13,6 +13,7 @@ SkillOutput, WorldState, ) +from autosim.utils.data_util import convert_quat from .base_skill import CuroboSkillExtraCfg from .reach import ReachSkill @@ -111,7 +112,8 @@ def execute_plan(self, state: WorldState, goal: SkillGoal) -> bool: raise ValueError("activate_qd should not be None when planning relative reach trajectories.") ee_pose = self._planner.get_ee_pose(activate_q) - target_pos, target_quat = ee_pose.position.clone(), ee_pose.quaternion.clone() + target_pos = ee_pose.position.clone() + target_quat = convert_quat(ee_pose.quaternion.clone(), to="xyzw") # cuRobo wxyz → xyzw self._logger.info(f"ee pos in robot root frame: {target_pos}, ee quat in robot root frame: {target_quat}") # move the eef along the move axis by the move offset based on eef frame, and convert to robot root frame to get target pose @@ -120,7 +122,7 @@ def execute_plan(self, state: WorldState, goal: SkillGoal) -> bool: self.cfg.extra_cfg.get_direction_vector(goal.info["move_axis"]) * self.cfg.extra_cfg.move_offset ) offset_pos_in_ee = move_offset_vector.to(isaaclab_device).unsqueeze(0) - offset_quat_in_ee = torch.tensor([1.0, 0.0, 0.0, 0.0], device=isaaclab_device).unsqueeze(0) + offset_quat_in_ee = torch.tensor([0.0, 0.0, 0.0, 1.0], device=isaaclab_device).unsqueeze(0) ee_pos_in_robot_root, ee_quat_in_robot_root = target_pos.to(isaaclab_device), target_quat.to(isaaclab_device) offset_pos_in_robot_root, offset_quat_in_robot_root = PoseUtils.combine_frame_transforms( diff --git a/source/autosim/autosim/skills/retract.py b/source/autosim/autosim/skills/retract.py index 4650c96..82dc72e 100644 --- a/source/autosim/autosim/skills/retract.py +++ b/source/autosim/autosim/skills/retract.py @@ -12,6 +12,7 @@ SkillOutput, WorldState, ) +from autosim.utils.data_util import convert_quat from .base_skill import CuroboSkillBase, CuroboSkillExtraCfg @@ -133,7 +134,9 @@ def step(self, state: WorldState) -> SkillOutput: activate_q, _ = self._build_activate_joint_state(state.sim_joint_names, joint_pos, None) all_link_poses = self._planner.get_link_poses(activate_q, link_names=None) info["link_poses_in_robot_root_frame"] = { - name: torch.cat([pose.position.squeeze(0), pose.quaternion.squeeze(0)]) + name: torch.cat( + [pose.position.squeeze(0), convert_quat(pose.quaternion.squeeze(0), to="xyzw")] + ) # cuRobo wxyz → xyzw for name, pose in all_link_poses.items() } diff --git a/source/autosim/autosim/skills/rotate.py b/source/autosim/autosim/skills/rotate.py index 967861a..3bcb7bb 100644 --- a/source/autosim/autosim/skills/rotate.py +++ b/source/autosim/autosim/skills/rotate.py @@ -18,6 +18,7 @@ SkillOutput, WorldState, ) +from autosim.utils.data_util import as_torch, convert_quat from .base_skill import CuroboSkillExtraCfg from .reach import ReachSkill @@ -48,12 +49,12 @@ def _parse_axis_vector(rotate_axis: str) -> torch.Tensor: def _axis_angle_to_quat(axis: torch.Tensor, angle: float) -> torch.Tensor: - """Convert axis-angle to quaternion [qw, qx, qy, qz].""" + """Convert axis-angle to quaternion [qx, qy, qz, qw] (xyzw).""" half = torch.as_tensor(angle / 2.0, device=axis.device, dtype=axis.dtype) s = torch.sin(half) w = torch.cos(half) - return torch.cat([w.unsqueeze(0), axis * s]) + return torch.cat([axis * s, w.unsqueeze(0)]) @configclass @@ -108,11 +109,11 @@ def extract_goal_from_info( if self.cfg.extra_cfg.rotate_frame == "object": axis_local = _parse_axis_vector(self.cfg.extra_cfg.rotate_axis).to(env.device) - obj_pose_w = env.scene[skill_info.target_object].data.root_pose_w[0] # [7] - obj_quat_w = obj_pose_w[3:].unsqueeze(0) # [1, 4] + obj_pose_w = as_torch(env.scene[skill_info.target_object].data.root_pose_w)[0] # [7] xyzw + obj_quat_w = obj_pose_w[3:].unsqueeze(0) # [1, 4] xyzw robot = env.scene[env_extra_info.robot_name] - robot_quat_w = robot.data.root_pose_w[0, 3:].unsqueeze(0) # [1, 4] + robot_quat_w = as_torch(robot.data.root_pose_w)[0, 3:].unsqueeze(0) # [1, 4] xyzw # object frame -> world frame -> robot root frame axis_in_world = PoseUtils.quat_apply(obj_quat_w, axis_local.unsqueeze(0)).squeeze(0) @@ -152,7 +153,7 @@ def execute_plan(self, state: WorldState, goal: SkillGoal) -> bool: for i in range(steps): ee_pose = self._planner.get_ee_pose(current_q) ee_pos = ee_pose.position.clone() - ee_quat = ee_pose.quaternion.clone() + ee_quat = convert_quat(ee_pose.quaternion.clone(), to="xyzw") # cuRobo wxyz → xyzw if self.cfg.extra_cfg.rotate_frame == "ee": axis_local = _parse_axis_vector(self.cfg.extra_cfg.rotate_axis).to( @@ -170,13 +171,13 @@ def execute_plan(self, state: WorldState, goal: SkillGoal) -> bool: self._target_poses["target_pose"] = torch.cat([target_pos.unsqueeze(0), target_quat.unsqueeze(0)], dim=-1) self._logger.info( - f"Rotate step {i+1}/{steps} ({self.cfg.extra_cfg.rotate_frame} frame): " + f"Rotate step {i + 1}/{steps} ({self.cfg.extra_cfg.rotate_frame} frame): " f"axis_in_root={axis_in_root}, step_angle={step_angle:.4f}" ) traj = self._planner.plan_motion(target_pos, target_quat, current_q, current_qd) if traj is None: - self._logger.warning(f"Rotate planning failed at step {i+1}/{steps}") + self._logger.warning(f"Rotate planning failed at step {i + 1}/{steps}") if not trajectories: self._trajectory = None return False diff --git a/source/autosim/autosim/utils/data_util.py b/source/autosim/autosim/utils/data_util.py new file mode 100644 index 0000000..5e67c1e --- /dev/null +++ b/source/autosim/autosim/utils/data_util.py @@ -0,0 +1,74 @@ +from typing import Any, Literal + +import numpy as np +import torch + +try: + import warp as wp +except Exception: # pragma: no cover + wp = None # type: ignore[assignment] + + +def as_torch(x: Any) -> torch.Tensor: + """View sim buffers as ``torch.Tensor`` (no-op if already a tensor).""" + if isinstance(x, torch.Tensor): + return x + if wp is None: + raise RuntimeError("warp is required to convert non-tensor sim buffers to torch") + try: + return wp.to_torch(x) # type: ignore[no-any-return] + except AttributeError as exc: + if "is_cpu" not in str(exc): + raise + device = getattr(x, "device", None) + if isinstance(device, torch.device): + if device.type == "cpu": + return torch.as_tensor(np.asarray(x)) + return torch.as_tensor(x, device=device) + raise + + +def convert_quat(quat: torch.Tensor | np.ndarray, to: Literal["xyzw", "wxyz"] = "xyzw") -> torch.Tensor | np.ndarray: + """Converts quaternion from one convention to another. + + The convention to convert TO is specified as an optional argument. If to == 'xyzw', + then the input is in 'wxyz' format, and vice-versa. + + Args: + quat: The quaternion of shape (..., 4). + to: Convention to convert quaternion to.. Defaults to "xyzw". + + Returns: + The converted quaternion in specified convention. + + Raises: + ValueError: Invalid input argument `to`, i.e. not "xyzw" or "wxyz". + ValueError: Invalid shape of input `quat`, i.e. not (..., 4,). + """ + # check input is correct + if quat.shape[-1] != 4: + msg = f"Expected input quaternion shape mismatch: {quat.shape} != (..., 4)." + raise ValueError(msg) + if to not in ["xyzw", "wxyz"]: + msg = f"Expected input argument `to` to be 'xyzw' or 'wxyz'. Received: {to}." + raise ValueError(msg) + # check if input is numpy array (we support this backend since some classes use numpy) + if isinstance(quat, np.ndarray): + # use numpy functions + if to == "xyzw": + # wxyz -> xyzw + return np.roll(quat, -1, axis=-1) + else: + # xyzw -> wxyz + return np.roll(quat, 1, axis=-1) + else: + # convert to torch (sanity check) + if not isinstance(quat, torch.Tensor): + quat = torch.tensor(quat, dtype=float) + # convert to specified quaternion type + if to == "xyzw": + # wxyz -> xyzw + return quat.roll(-1, dims=-1) + else: + # xyzw -> wxyz + return quat.roll(1, dims=-1) diff --git a/source/autosim/autosim/utils/debug_util.py b/source/autosim/autosim/utils/debug_util.py index a0a456d..4535e82 100644 --- a/source/autosim/autosim/utils/debug_util.py +++ b/source/autosim/autosim/utils/debug_util.py @@ -8,6 +8,7 @@ from isaaclab.utils.math import combine_frame_transforms from autosim.core.types import OccupancyMap +from autosim.utils.data_util import as_torch markers: dict[str, VisualizationMarkers] = {} @@ -39,7 +40,7 @@ def _collect_world_poses(obj_poses: dict[str, list[torch.Tensor]], env: ManagerB """ world_poses = [] for obj_name, pose_list in obj_poses.items(): - obj_pose_w = env.scene[obj_name].data.root_pose_w[0] # [7] + obj_pose_w = as_torch(env.scene[obj_name].data.root_pose_w)[0] # [7] obj_pos_w = obj_pose_w[:3].unsqueeze(0) # [1, 3] obj_quat_w = obj_pose_w[3:].unsqueeze(0) # [1, 4] for pose in pose_list: diff --git a/source/autosim_examples/autosim_examples/autosim/action_adapters/franka_adapter.py b/source/autosim_examples/autosim_examples/autosim/action_adapters/franka_adapter.py index 15d36d2..a2e92cd 100644 --- a/source/autosim_examples/autosim_examples/autosim/action_adapters/franka_adapter.py +++ b/source/autosim_examples/autosim_examples/autosim/action_adapters/franka_adapter.py @@ -7,6 +7,7 @@ from autosim import ActionAdapterBase from autosim.core.types import SkillOutput +from autosim.utils.data_util import as_torch if TYPE_CHECKING: from .franka_adapter_cfg import FrankaAbsAdapterCfg @@ -24,7 +25,7 @@ def _apply_reach(self, skill_output: SkillOutput, env: ManagerBasedEnv) -> torch target_joint_pos = skill_output.action # [joint_positions with isaaclab joint order] robot = env.scene["robot"] - default_joint_pos = robot.data.default_joint_pos[0, :] + default_joint_pos = as_torch(robot.data.default_joint_pos)[0, :] last_action = env.action_manager.action action = last_action[0, :].clone() diff --git a/source/autosim_examples/autosim_examples/autosim/pipelines/franka_lift_cube.py b/source/autosim_examples/autosim_examples/autosim/pipelines/franka_lift_cube.py index db4967a..076020e 100644 --- a/source/autosim_examples/autosim_examples/autosim/pipelines/franka_lift_cube.py +++ b/source/autosim_examples/autosim_examples/autosim/pipelines/franka_lift_cube.py @@ -26,6 +26,7 @@ def __post_init__(self): self.motion_planner.robot_config_file = "franka.yml" self.motion_planner.world_ignore_subffixes = [] self.motion_planner.world_only_subffixes = [] + self.motion_planner.env_scene_prefix = None class FrankaCubeLiftPipeline(AutoSimPipeline): @@ -56,7 +57,7 @@ def get_env_extra_info(self) -> EnvExtraInfo: ee_link_name="panda_hand", object_reach_target_poses={ "cube": [ - torch.tensor([0.0, 0.0, 0.10, 0.0, 1.0, 0.0, 0.0]), + torch.tensor([0.0, 0.0, 0.10, 1.0, 0.0, 0.0, 0.0]), # [x, y, z, qx, qy, qz, qw]: 绕 x 轴 180° ], }, ) diff --git a/source/autosim_examples/autosim_examples/isaaclab_task/franka_lift_cube_cfg.py b/source/autosim_examples/autosim_examples/isaaclab_task/franka_lift_cube_cfg.py index 7293504..cac6593 100644 --- a/source/autosim_examples/autosim_examples/isaaclab_task/franka_lift_cube_cfg.py +++ b/source/autosim_examples/autosim_examples/isaaclab_task/franka_lift_cube_cfg.py @@ -13,8 +13,11 @@ from isaaclab.sim.spawners.from_files.from_files_cfg import GroundPlaneCfg, UsdFileCfg from isaaclab.utils import configclass from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR +from isaaclab_physx.physics import PhysxCfg from isaaclab_tasks.manager_based.manipulation.stack.mdp import franka_stack_events +from autosim.utils.data_util import as_torch + from isaaclab_assets.robots.franka import FRANKA_PANDA_CFG # isort: skip @@ -35,14 +38,14 @@ class FrankaCubeLiftSceneCfg(InteractiveSceneCfg): # table table = AssetBaseCfg( prim_path="{ENV_REGEX_NS}/Table", - init_state=AssetBaseCfg.InitialStateCfg(pos=[0.5, 0, 0], rot=[0.707, 0, 0, 0.707]), + init_state=AssetBaseCfg.InitialStateCfg(pos=[0.5, 0, 0], rot=[0, 0, 0.707, 0.707]), spawn=UsdFileCfg(usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/SeattleLabTable/table_instanceable.usd"), ) # cube cube: RigidObjectCfg = RigidObjectCfg( prim_path="{ENV_REGEX_NS}/Cube", - init_state=RigidObjectCfg.InitialStateCfg(pos=[0.4, 0.0, 0.0203], rot=[1, 0, 0, 0]), + init_state=RigidObjectCfg.InitialStateCfg(pos=[0.4, 0.0, 0.0203], rot=[0, 0, 0, 1]), spawn=UsdFileCfg( usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/blue_block.usd", scale=(1.0, 1.0, 1.0), @@ -112,7 +115,7 @@ def __post_init__(self): def cube_lifted(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("cube")) -> torch.Tensor: """Terminate when the cube is lifted.""" - return env.scene[asset_cfg.name].data.root_pos_w[:, 2] > 0.10 + return as_torch(env.scene[asset_cfg.name].data.root_pos_w)[:, 2] > 0.10 @configclass @@ -142,8 +145,8 @@ class EventCfg: func=franka_stack_events.randomize_object_pose, mode="reset", params={ - # "pose_range": {"x": (0.4, 0.6), "y": (-0.10, 0.10), "z": (0.0203, 0.0203), "yaw": (-1.0, 1.0)}, - "pose_range": {"x": (0.5, 0.5), "y": (0.0, 0.0), "z": (0.0203, 0.0203), "yaw": (0.0, 0.0)}, + "pose_range": {"x": (0.4, 0.6), "y": (-0.10, 0.10), "z": (0.0203, 0.0203), "yaw": (-1.0, 1.0)}, + # "pose_range": {"x": (0.5, 0.5), "y": (0.0, 0.0), "z": (0.0203, 0.0203), "yaw": (0.0, 0.0)}, "asset_cfgs": [SceneEntityCfg("cube")], }, ) @@ -172,10 +175,11 @@ def __post_init__(self): self.episode_length_s = 30.0 # simulation settings self.sim.dt = 0.01 # 100Hz - self.sim.render_interval = 2 - - self.sim.physx.bounce_threshold_velocity = 0.2 - self.sim.physx.bounce_threshold_velocity = 0.01 - self.sim.physx.gpu_found_lost_aggregate_pairs_capacity = 1024 * 1024 * 4 - self.sim.physx.gpu_total_aggregate_pairs_capacity = 16 * 1024 - self.sim.physx.friction_correlation_distance = 0.00625 + self.sim.render_interval = self.decimation + + self.sim.physics = PhysxCfg( + bounce_threshold_velocity=0.01, + gpu_found_lost_aggregate_pairs_capacity=1024 * 1024 * 4, + gpu_total_aggregate_pairs_capacity=16 * 1024, + friction_correlation_distance=0.00625, + )