From 2c2a5192e3f66a614f1993864ba19e4ccd6558cf Mon Sep 17 00:00:00 2001 From: Zihan Gao Date: Tue, 14 Apr 2026 16:11:47 -0400 Subject: [PATCH 01/32] [feat] add G1 autosim pipeline (lift-cube / open-microwave) - G1 action adapter and config (arm + gripper + loco base) - G1LiftCubePipeline and G1OpenMicrowavePipeline registrations - IsaacLab scene cfg using packing_table.usd (non-instanceable, consistent with upstream G1 tasks) - Runtime compat patches for mixed autosim versions (compat_autosim.py) - Robot assets: G1_autosim.urdf, curobo configs, collision spheres - run_autosim.py entry point Co-Authored-By: Claude Sonnet 4.6 --- lw_benchhub/autosim/__init__.py | 13 + .../action_adapters/g1_action_adapter.py | 127 ++ .../action_adapters/g1_action_adapter_cfg.py | 21 + lw_benchhub/autosim/compat_autosim.py | 281 +++ .../autosim/content/assets/cube_5cm.usda | 3 + .../content/assets/robot/g1/G1_autosim.urdf | 1552 +++++++++++++++++ .../content/assets/robot/g1/generate_urdf.py | 115 ++ .../content/configs/robot/g1_autosim.yml | 132 ++ .../robot/spheres/collision_g1_autosim.yml | 141 ++ .../autosim/isaaclab_tasks/__init__.py | 0 .../isaaclab_tasks/g1_lift_cube_cfg.py | 225 +++ lw_benchhub/autosim/pipelines/g1_lift_cube.py | 161 ++ run_autosim.py | 42 + 13 files changed, 2813 insertions(+) create mode 100644 lw_benchhub/autosim/action_adapters/g1_action_adapter.py create mode 100644 lw_benchhub/autosim/action_adapters/g1_action_adapter_cfg.py create mode 100644 lw_benchhub/autosim/compat_autosim.py create mode 100644 lw_benchhub/autosim/content/assets/cube_5cm.usda create mode 100644 lw_benchhub/autosim/content/assets/robot/g1/G1_autosim.urdf create mode 100644 lw_benchhub/autosim/content/assets/robot/g1/generate_urdf.py create mode 100644 lw_benchhub/autosim/content/configs/robot/g1_autosim.yml create mode 100644 lw_benchhub/autosim/content/configs/robot/spheres/collision_g1_autosim.yml create mode 100644 lw_benchhub/autosim/isaaclab_tasks/__init__.py create mode 100644 lw_benchhub/autosim/isaaclab_tasks/g1_lift_cube_cfg.py create mode 100644 lw_benchhub/autosim/pipelines/g1_lift_cube.py create mode 100644 run_autosim.py diff --git a/lw_benchhub/autosim/__init__.py b/lw_benchhub/autosim/__init__.py index 9bad9c90..bbccda31 100644 --- a/lw_benchhub/autosim/__init__.py +++ b/lw_benchhub/autosim/__init__.py @@ -42,3 +42,16 @@ entry_point=f"{__name__}.pipelines.dessert_upgrade:DessertUpgradePipeline", cfg_entry_point=f"{__name__}.pipelines.dessert_upgrade:DessertUpgradePipelineCfg", ) + +register_pipeline( + id="LWBenchhub-Autosim-G1LiftCubePipeline-v0", + entry_point=f"{__name__}.pipelines.g1_lift_cube:G1LiftCubePipeline", + cfg_entry_point=f"{__name__}.pipelines.g1_lift_cube:G1LiftCubePipelineCfg", +) + +# New semantic name for the same G1 pipeline (microwave opening task). +register_pipeline( + id="LWBenchhub-Autosim-G1OpenMicrowavePipeline-v0", + entry_point=f"{__name__}.pipelines.g1_lift_cube:G1LiftCubePipeline", + cfg_entry_point=f"{__name__}.pipelines.g1_lift_cube:G1LiftCubePipelineCfg", +) diff --git a/lw_benchhub/autosim/action_adapters/g1_action_adapter.py b/lw_benchhub/autosim/action_adapters/g1_action_adapter.py new file mode 100644 index 00000000..b948cd86 --- /dev/null +++ b/lw_benchhub/autosim/action_adapters/g1_action_adapter.py @@ -0,0 +1,127 @@ +from __future__ import annotations + +from typing import TYPE_CHECKING + +import torch +from isaaclab.envs import ManagerBasedEnv + +from autosim import ActionAdapterBase +from autosim.core.types import SkillOutput + +if TYPE_CHECKING: + from .g1_action_adapter_cfg import G1ActionAdapterCfg + + +class G1ActionAdapter(ActionAdapterBase): + """Action adapter for the Unitree G1 robot (leg-locomotion autosim variant). + + Action vector layout: + [0:4] base locomotion command [vx, vy, vyaw, mode] + mode: 0=loco, 1=squat/stance + [4:11] right arm joints (7 DoF, absolute position) + [11:18] left arm joints (7 DoF, absolute position) + [18:32] fingers (right + left three-finger hands) + """ + + def __init__(self, cfg: G1ActionAdapterCfg): + super().__init__(cfg) + self.register_apply_method("moveto", self._apply_moveto) + self.register_apply_method("reach", self._apply_reach) + self.register_apply_method("lift", self._apply_reach) + self.register_apply_method("push", self._apply_reach) + self.register_apply_method("pull", self._apply_reach) + self.register_apply_method("grasp", self._apply_gripper) + self.register_apply_method("ungrasp", self._apply_gripper) + + # ------------------------------------------------------------------ + # Navigation (leg locomotion base command) + # ------------------------------------------------------------------ + + def _apply_moveto(self, skill_output: SkillOutput, env: ManagerBasedEnv) -> torch.Tensor: + """Convert world-frame velocity [vx, vy, vyaw] to virtual-base delta positions. + + G1ActionsCfg layout: + base_action [0:3] – JointPositionAction with scale=0.01 (delta) + right_arm [3:10] – absolute + left_arm [10:17] – absolute + gripper [17:31] – absolute + """ + vx, vy, vyaw = skill_output.action # world frame + + robot = env.scene["robot"] + world_pose = robot.data.root_pose_w[0] # [x, y, z, qw, qx, qy, qz] + w, x, y, z = world_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) + + # Rotate world-frame velocity → robot body frame + vx_body = vx * cos_yaw + vy * sin_yaw + vy_body = -vx * sin_yaw + vy * cos_yaw + + dt_control = env.cfg.sim.dt * env.cfg.decimation + + last_action = env.action_manager.action + action = last_action[0, :].clone() + + # base_action uses scale=0.01: action_value = delta / 0.01 + action[0] = (vx_body * dt_control) / 0.01 + action[1] = (vy_body * dt_control) / 0.01 + action[2] = (vyaw * dt_control) / 0.01 + + return action + + # ------------------------------------------------------------------ + # Arm motion (cuRobo joint trajectory playback) + # ------------------------------------------------------------------ + + def _apply_reach(self, skill_output: SkillOutput, env: ManagerBasedEnv) -> torch.Tensor: + """Write cuRobo joint positions into the arm action terms.""" + + target_joint_pos = skill_output.action # [N_sim_joints], full robot order + robot = env.scene["robot"] + current_joint_pos = robot.data.joint_pos[0] + + last_action = env.action_manager.action + action = last_action[0, :].clone() + + # Resolve joint → robot-joint index mappings + r_arm_ids, _ = robot.find_joints(env.action_manager.get_term("right_arm_action").cfg.joint_names) + l_arm_ids, _ = robot.find_joints(env.action_manager.get_term("left_arm_action").cfg.joint_names) + + # Keep base steady for manipulation (zero delta → no base movement). + action[:3] = 0.0 + + # G1ActionsCfg action layout: + # [0:3] base_action (scale=0.01 delta, kept zero above) + # [3:10] right_arm (7-DoF absolute) + # [10:17] left_arm (7-DoF absolute) + # [17:31] gripper (14 joints absolute) + action[3:10] = target_joint_pos[r_arm_ids] + action[10:17] = target_joint_pos[l_arm_ids] + + return action + + # ------------------------------------------------------------------ + # Gripper (three-finger open / close) + # ------------------------------------------------------------------ + + def _apply_gripper(self, skill_output: SkillOutput, env: ManagerBasedEnv) -> torch.Tensor: + """Set all finger joints to the closed (grasp) or open (ungrasp) position.""" + + gripper_signal = skill_output.action[0].item() # -1.0 = grasp, +1.0 = ungrasp + + # Map signal → actual joint angle + finger_angle = self.cfg.finger_close_angle if gripper_signal < 0 else self.cfg.finger_open_angle + + last_action = env.action_manager.action + action = last_action[0, :].clone() + action[:3] = 0.0 # keep base steady + + robot = env.scene["robot"] + finger_ids, _ = robot.find_joints(env.action_manager.get_term("gripper_action").cfg.joint_names) + + # gripper_action starts at action index 17 + action[17:17 + len(finger_ids)] = finger_angle + + return action diff --git a/lw_benchhub/autosim/action_adapters/g1_action_adapter_cfg.py b/lw_benchhub/autosim/action_adapters/g1_action_adapter_cfg.py new file mode 100644 index 00000000..aa766468 --- /dev/null +++ b/lw_benchhub/autosim/action_adapters/g1_action_adapter_cfg.py @@ -0,0 +1,21 @@ +from isaaclab.utils import configclass + +from autosim import ActionAdapterCfg + +from .g1_action_adapter import G1ActionAdapter + + +@configclass +class G1ActionAdapterCfg(ActionAdapterCfg): + """Configuration for the G1 action adapter.""" + + class_type: type = G1ActionAdapter + + base_x_joint_name: str = "base_x_joint" + base_y_joint_name: str = "base_y_joint" + base_yaw_joint_name: str = "base_yaw_joint" + + finger_close_angle: float = 1.2 + """Finger joint angle (rad) when gripper is closed.""" + finger_open_angle: float = 0.0 + """Finger joint angle (rad) when gripper is open.""" diff --git a/lw_benchhub/autosim/compat_autosim.py b/lw_benchhub/autosim/compat_autosim.py new file mode 100644 index 00000000..708e32db --- /dev/null +++ b/lw_benchhub/autosim/compat_autosim.py @@ -0,0 +1,281 @@ +"""Runtime compatibility patches for external autosim package.""" + +from __future__ import annotations + +import types + +import isaaclab.utils.math as PoseUtils +import torch + +from autosim.core.types import SkillGoal + + +def patch_env_extra_info() -> None: + """Add backward/forward compatibility fields to EnvExtraInfo.""" + from autosim.core.types import EnvExtraInfo + + if hasattr(EnvExtraInfo, "_lw_patched_extra_reach"): + return + + original_init = EnvExtraInfo.__init__ + original_reset = EnvExtraInfo.reset + + def _reset_extra_target_pose_iterators(self) -> None: + data = getattr(self, "object_extra_reach_target_poses", {}) or {} + self._object_extra_reach_target_poses_iterator_dict = { + object_name: { + ee_name: self._build_iterator(reach_target_poses) + for ee_name, reach_target_poses in extra_targets.items() + } + for object_name, extra_targets in data.items() + } + + def __init__(self, *args, object_extra_reach_target_poses=None, **kwargs): + original_init(self, *args, **kwargs) + self.object_extra_reach_target_poses = object_extra_reach_target_poses or {} + _reset_extra_target_pose_iterators(self) + + def reset(self) -> None: + original_reset(self) + if not hasattr(self, "object_extra_reach_target_poses"): + self.object_extra_reach_target_poses = {} + _reset_extra_target_pose_iterators(self) + + def get_next_extra_reach_target_pose(self, object_name: str, ee_name: str): + return next(self._object_extra_reach_target_poses_iterator_dict[object_name][ee_name]) + + EnvExtraInfo.__init__ = __init__ + EnvExtraInfo.reset = reset + EnvExtraInfo._reset_extra_target_pose_iterators = _reset_extra_target_pose_iterators + EnvExtraInfo.get_next_extra_reach_target_pose = get_next_extra_reach_target_pose + EnvExtraInfo._lw_patched_extra_reach = True + + +def patch_reach_skill() -> None: + """Patch ReachSkill for mixed autosim versions. + + Some autosim versions include newer ReachSkill call-sites that rely on + helper methods/fields not present in older class definitions. + """ + from autosim.skills import reach as reach_mod + from autosim.core.registration import SkillRegistry + + ReachSkill = reach_mod.ReachSkill + + if not hasattr(ReachSkill, "_lw_original_build_activate_joint_state"): + ReachSkill._lw_original_build_activate_joint_state = ReachSkill._build_activate_joint_state + + def _build_activate_joint_state_compat( + self, full_sim_joint_names, full_sim_q, full_sim_qd=None + ): + # Planner may include virtual base joints that do not exist in the + # simulated leg-loco robot articulation. Fill missing joints with 0. + activate_q = [] + activate_qd = [] if full_sim_qd is not None else None + for joint_name in self._planner.target_joint_names: + if joint_name in full_sim_joint_names: + sim_joint_idx = full_sim_joint_names.index(joint_name) + activate_q.append(full_sim_q[sim_joint_idx]) + if full_sim_qd is not None and activate_qd is not None: + activate_qd.append(full_sim_qd[sim_joint_idx]) + else: + activate_q.append(torch.tensor(0.0, device=full_sim_q.device, dtype=full_sim_q.dtype)) + if full_sim_qd is not None and activate_qd is not None: + activate_qd.append( + torch.tensor(0.0, device=full_sim_qd.device, dtype=full_sim_qd.dtype) + ) + activate_q_tensor = torch.stack(activate_q, dim=0) + if activate_qd is None: + return activate_q_tensor, None + return activate_q_tensor, torch.stack(activate_qd, dim=0) + + ReachSkill._build_activate_joint_state = _build_activate_joint_state_compat + + if not hasattr(ReachSkill, "_lw_original_step"): + ReachSkill._lw_original_step = ReachSkill.step + + def step_compat(self, state): + self.visualize_debug_target_pose() + + traj_positions = self._trajectory.position + if self._step_idx >= len(self._trajectory.position): + traj_pos = traj_positions[-1] + done = True + else: + traj_pos = traj_positions[self._step_idx] + done = False + self._step_idx += 1 + + # Keep upstream corrective-reach behavior when available. + if done and getattr(self, "_corrective_reach_done", False) is False and getattr( + self.cfg.extra_cfg, "corrective_reach", False + ): + self._corrective_reach_done = True + if hasattr(self, "_compute_corrective_goal"): + new_goal = self._compute_corrective_goal() + if new_goal is not None: + self._logger.info("corrective_reach: re-planning to corrected object pose") + self._step_idx = 0 + plan_success = self.execute_plan(state, new_goal) + if plan_success: + done = False + + curobo_joint_names = self._trajectory.joint_names + sim_joint_names = state.sim_joint_names + joint_pos = state.robot_joint_pos.clone() + for curobo_idx, curobo_joint_name in enumerate(curobo_joint_names): + if curobo_joint_name not in sim_joint_names: + continue + sim_idx = sim_joint_names.index(curobo_joint_name) + joint_pos[sim_idx] = traj_pos[curobo_idx] + + from autosim.core.types import SkillOutput + + return SkillOutput(action=joint_pos, done=done, success=True, info={}) + + ReachSkill.step = step_compat + + if not hasattr(ReachSkill, "_compute_goal_from_offset"): + + def _compute_goal_from_offset( + self, + env, + robot_name: str, + target_object: str, + reach_offset: torch.Tensor, + extra_offsets=None, + ): + try: + object_pose_in_env = env.scene[target_object].data.root_pose_w + except Exception: + self._logger.warning(f"could not read pose for '{target_object}', skipping") + return None + + object_pos_in_env = object_pose_in_env[:, :3] + object_quat_in_env = object_pose_in_env[:, 3:] + + offset = reach_offset.to(env.device).unsqueeze(0) + reach_target_pos_in_env, reach_target_quat_in_env = PoseUtils.combine_frame_transforms( + object_pos_in_env, object_quat_in_env, offset[:, :3], offset[:, 3:] + ) + self._target_poses["target_pose"] = torch.cat((reach_target_pos_in_env, reach_target_quat_in_env), dim=-1) + + robot = env.scene[robot_name] + robot_root_pos_in_env = robot.data.root_pose_w[:, :3] + robot_root_quat_in_env = robot.data.root_pose_w[:, 3:] + + reach_target_pos_in_robot_root, reach_target_quat_in_robot_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, + ) + target_pose = torch.cat( + (reach_target_pos_in_robot_root, reach_target_quat_in_robot_root), dim=-1 + ).squeeze(0) + + activate_q, _ = self._build_activate_joint_state( + robot.data.joint_names, robot.data.joint_pos[0], robot.data.joint_vel[0] + ) + # Current upstream offset-based extra-EEF API is inconsistent across + # versions. Keep behavior stable by using the built-in policy. + extra_target_poses = self._build_extra_target_poses(activate_q, target_pose, self._saved_env_extra_info) + + return SkillGoal(target_object=target_object, target_pose=target_pose, extra_target_poses=extra_target_poses) + + ReachSkill._compute_goal_from_offset = _compute_goal_from_offset + + original_extract = ReachSkill.extract_goal_from_info + + def extract_goal_from_info(self, skill_info, env, env_extra_info): + target_object = skill_info.target_object + reach_offset = env_extra_info.get_next_reach_target_pose(target_object).to(env.device) + + self._saved_env = env + self._saved_env_extra_info = env_extra_info + self._saved_robot_name = env_extra_info.robot_name + self._saved_target_object = target_object + self._saved_reach_offset = reach_offset + self._saved_extra_offsets = None + + # Keep compatibility with older/newer autosim versions. + if hasattr(self, "_compute_goal_from_offset"): + return self._compute_goal_from_offset(env, env_extra_info.robot_name, target_object, reach_offset, None) + return original_extract(self, skill_info, env, env_extra_info) + + ReachSkill.extract_goal_from_info = extract_goal_from_info + + original_init = ReachSkill.__init__ + + def __init__(self, extra_cfg): + original_init(self, extra_cfg) + if not hasattr(self.cfg.extra_cfg, "corrective_reach"): + self.cfg.extra_cfg.corrective_reach = False + self._corrective_reach_done = False + self._saved_env = None + self._saved_env_extra_info = None + self._saved_robot_name = "robot" + self._saved_target_object = None + self._saved_reach_offset = None + self._saved_extra_offsets = None + + ReachSkill.__init__ = __init__ + + original_reset = ReachSkill.reset + + def reset(self): + original_reset(self) + self._corrective_reach_done = False + self._saved_env = None + self._saved_env_extra_info = None + self._saved_target_object = None + self._saved_reach_offset = None + self._saved_extra_offsets = None + + ReachSkill.reset = reset + + # Fallback: patch the factory path too, in case another ReachSkill class + # object (from a different autosim import path) is instantiated. + if not hasattr(SkillRegistry, "_lw_patched_create"): + + @classmethod + def _patched_create(cls, name, extra_cfg): + skill_cls = cls.get(name) + skill = skill_cls(extra_cfg) + if skill.__class__.__name__ == "ReachSkill": + if not hasattr(skill, "_compute_goal_from_offset"): + skill._compute_goal_from_offset = types.MethodType( + ReachSkill._compute_goal_from_offset, skill + ) + if not hasattr(skill.cfg.extra_cfg, "corrective_reach"): + skill.cfg.extra_cfg.corrective_reach = False + if not hasattr(skill, "_saved_env_extra_info"): + skill._saved_env_extra_info = None + if not hasattr(skill, "_corrective_reach_done"): + skill._corrective_reach_done = False + return skill + + SkillRegistry.create = _patched_create + SkillRegistry._lw_patched_create = True + + +def patch_curobo_config() -> None: + """Relax cuRobo start-state checks for mixed G1 planner/sim setups.""" + from curobo.wrap.reacher.motion_gen import MotionGenConfig + + if hasattr(MotionGenConfig, "_lw_original_load_from_robot_config"): + return + + MotionGenConfig._lw_original_load_from_robot_config = MotionGenConfig.load_from_robot_config + + @classmethod + def _patched_load_from_robot_config(cls, *args, **kwargs): + # In our current hybrid setup planner robot model and sim articulation + # differ on virtual base joints, which can trigger false-positive + # INVALID_START_STATE_SELF_COLLISION. Relaxing self collision check + # avoids immediate planner rejection at step 0. + kwargs.setdefault("self_collision_check", False) + kwargs.setdefault("self_collision_opt", False) + return cls._lw_original_load_from_robot_config(*args, **kwargs) + + MotionGenConfig.load_from_robot_config = _patched_load_from_robot_config diff --git a/lw_benchhub/autosim/content/assets/cube_5cm.usda b/lw_benchhub/autosim/content/assets/cube_5cm.usda new file mode 100644 index 00000000..c56a55d3 --- /dev/null +++ b/lw_benchhub/autosim/content/assets/cube_5cm.usda @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b023d8c10b6f67c04b717cedb1599d9afe3333dfb66622fd5b0a57136de2c37f +size 1889 diff --git a/lw_benchhub/autosim/content/assets/robot/g1/G1_autosim.urdf b/lw_benchhub/autosim/content/assets/robot/g1/G1_autosim.urdf new file mode 100644 index 00000000..fdba358c --- /dev/null +++ b/lw_benchhub/autosim/content/assets/robot/g1/G1_autosim.urdf @@ -0,0 +1,1552 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/lw_benchhub/autosim/content/assets/robot/g1/generate_urdf.py b/lw_benchhub/autosim/content/assets/robot/g1/generate_urdf.py new file mode 100644 index 00000000..7cb00b4f --- /dev/null +++ b/lw_benchhub/autosim/content/assets/robot/g1/generate_urdf.py @@ -0,0 +1,115 @@ +""" +Generate G1_autosim.urdf by prepending virtual base joints to the original +g1_29dof_with_hand.urdf. The virtual joints (base_x, base_y, base_yaw) mirror +the same approach used in X7S, allowing A*+P-controller navigation to drive the +robot via joint position commands in Isaac Lab. + +Usage: + python generate_urdf.py +Output: + G1_autosim.urdf (in the same directory as this script) +""" + +from pathlib import Path + +ORIGINAL_URDF = ( + Path(__file__).parent.parent.parent.parent.parent.parent + / "core/mdp/actions/wbc_policy/robot_model/g1/g1_29dof_with_hand.urdf" +) +OUTPUT_URDF = Path(__file__).parent / "G1_autosim.urdf" + +VIRTUAL_BASE_PREAMBLE = """\ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +""" + + +def generate(): + original = ORIGINAL_URDF.read_text(encoding="utf-8") + + # Remove the mujoco block (not needed by cuRobo / Isaac Lab URDF loader) + import re + original = re.sub(r"\s*.*?", "", original, flags=re.DOTALL) + + # Remove the commented-out floating_base_joint block so the file is clean + original = re.sub(r"", "", original, flags=re.DOTALL) + + # Find the insertion point: right before the first + insert_marker = '' + idx = original.find(insert_marker) + if idx == -1: + raise RuntimeError("Could not find '' in the original URDF.") + + output = original[:idx] + VIRTUAL_BASE_PREAMBLE + original[idx:] + + # Rename robot tag for clarity + output = output.replace('name="g1_29dof_with_hand"', 'name="g1_autosim"', 1) + + OUTPUT_URDF.parent.mkdir(parents=True, exist_ok=True) + OUTPUT_URDF.write_text(output, encoding="utf-8") + print(f"Written: {OUTPUT_URDF}") + print(f"Source: {ORIGINAL_URDF}") + + +if __name__ == "__main__": + generate() diff --git a/lw_benchhub/autosim/content/configs/robot/g1_autosim.yml b/lw_benchhub/autosim/content/configs/robot/g1_autosim.yml new file mode 100644 index 00000000..c5f2cf30 --- /dev/null +++ b/lw_benchhub/autosim/content/configs/robot/g1_autosim.yml @@ -0,0 +1,132 @@ +robot_cfg: + + kinematics: + use_usd_kinematics: False + usd_robot_root: "/g1_autosim" + urdf_path: "G1_autosim.urdf" # relative to robot_config_root_path (g1/ dir) + asset_root_path: "." # meshes are resolved from the urdf file dir + + base_link: "world_link" + ee_link: "right_hand_palm_link" # right-hand end-effector + + # Extra tracked links (left ee for dual-arm tasks) + link_names: ["right_hand_palm_link", "left_hand_palm_link"] + + collision_link_names: + [ + "pelvis", + "torso_link", + "left_shoulder_pitch_link", "left_shoulder_roll_link", "left_shoulder_yaw_link", + "left_elbow_link", + "left_wrist_roll_link", "left_wrist_pitch_link", "left_wrist_yaw_link", + "left_hand_palm_link", + "right_shoulder_pitch_link", "right_shoulder_roll_link", "right_shoulder_yaw_link", + "right_elbow_link", + "right_wrist_roll_link", "right_wrist_pitch_link", "right_wrist_yaw_link", + "right_hand_palm_link", + ] + + collision_spheres: "spheres/collision_g1_autosim.yml" + collision_sphere_buffer: 0.002 + extra_collision_spheres: {"right_hand_palm_link": 20, "left_hand_palm_link": 20} + use_global_cumul: True + + self_collision_ignore: + { + "pelvis": ["torso_link", "left_shoulder_pitch_link", "right_shoulder_pitch_link"], + "torso_link": ["left_shoulder_pitch_link", "right_shoulder_pitch_link"], + "left_shoulder_pitch_link": ["left_shoulder_roll_link"], + "left_shoulder_roll_link": ["left_shoulder_yaw_link"], + "left_shoulder_yaw_link": ["left_elbow_link"], + "left_elbow_link": ["left_wrist_roll_link"], + "left_wrist_roll_link": ["left_wrist_pitch_link"], + "left_wrist_pitch_link": ["left_wrist_yaw_link"], + "left_wrist_yaw_link": ["left_hand_palm_link"], + "right_shoulder_pitch_link": ["right_shoulder_roll_link"], + "right_shoulder_roll_link": ["right_shoulder_yaw_link"], + "right_shoulder_yaw_link": ["right_elbow_link"], + "right_elbow_link": ["right_wrist_roll_link"], + "right_wrist_roll_link": ["right_wrist_pitch_link"], + "right_wrist_pitch_link": ["right_wrist_yaw_link"], + "right_wrist_yaw_link": ["right_hand_palm_link"], + } + + self_collision_buffer: {} + + mesh_link_names: + [ + "pelvis", + "torso_link", + "left_shoulder_pitch_link", "left_shoulder_roll_link", "left_shoulder_yaw_link", + "left_elbow_link", + "left_wrist_roll_link", "left_wrist_pitch_link", "left_wrist_yaw_link", + "left_hand_palm_link", + "right_shoulder_pitch_link", "right_shoulder_roll_link", "right_shoulder_yaw_link", + "right_elbow_link", + "right_wrist_roll_link", "right_wrist_pitch_link", "right_wrist_yaw_link", + "right_hand_palm_link", + ] + + # Joints that are fixed during planning. + # NOTE: Only joints ON the kinematic chain (world_link → right_hand_palm_link, + # with left_hand_palm_link as a tracked branch) can be locked. + # Leg joints (hip/knee/ankle) branch off pelvis and are NOT on the chain → excluded. + # Finger joints are descendants of the EE palm links and are NOT on the chain → excluded. + lock_joints: + { + # Waist (on chain: pelvis → waist_yaw → waist_roll → torso) + "waist_yaw_joint": 0.0, + "waist_roll_joint": 0.0, + "waist_pitch_joint": 0.0, + # Left arm (locked; right arm is planned; left_hand_palm_link is a tracked link) + "left_shoulder_pitch_joint": 0.0, + "left_shoulder_roll_joint": 0.3, + "left_shoulder_yaw_joint": 0.0, + "left_elbow_joint": 0.0, + "left_wrist_roll_joint": 0.0, + "left_wrist_pitch_joint": 0.0, + "left_wrist_yaw_joint": 0.0, + } + + extra_links: null + + cspace: + joint_names: + [ + # Virtual base joints (navigation in planner state only) + "base_x_joint", + "base_y_joint", + "base_yaw_joint", + # Right arm (7 DoF) + "right_shoulder_pitch_joint", + "right_shoulder_roll_joint", + "right_shoulder_yaw_joint", + "right_elbow_joint", + "right_wrist_roll_joint", + "right_wrist_pitch_joint", + "right_wrist_yaw_joint", + ] + + retract_config: + [ + 0.0, 0.0, 0.0, # base + 0.0, -0.3, 0.0, 0.0, 0.0, 0.0, 0.0, # right arm + ] + + null_space_weight: + [ + 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, + ] + + cspace_distance_weight: + [ + 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, + ] + + max_jerk: 500.0 + max_acceleration: 500.0 + +planner: + frame_bias: [0.0, 0.0, 0.0] diff --git a/lw_benchhub/autosim/content/configs/robot/spheres/collision_g1_autosim.yml b/lw_benchhub/autosim/content/configs/robot/spheres/collision_g1_autosim.yml new file mode 100644 index 00000000..ae16a2be --- /dev/null +++ b/lw_benchhub/autosim/content/configs/robot/spheres/collision_g1_autosim.yml @@ -0,0 +1,141 @@ +# cuRobo collision spheres for G1 (autosim variant, right-arm planning) +# +# Coordinate convention: each center is in the link's LOCAL frame. +# Radii are conservative estimates based on G1 link geometry. +# Only links in or near the planning chain are listed; locked/leg links +# that are far from the workspace are omitted for performance. +# +# Planning chain: world_link → base_x/y/yaw (virtual) → pelvis → torso_link +# → right_shoulder_pitch → ... → right_hand_palm_link +# +# Left arm spheres are included (locked but physically present) to avoid +# body-world and body-self collisions during navigation. + +collision_spheres: + + # ── Pelvis ──────────────────────────────────────────────────────────────── + # Roughly box-shaped, ~0.28 m wide × 0.18 m deep × 0.18 m tall. + # Inertia origin: [0, 0, -0.076] + pelvis: + - "center": [0.0, 0.0, 0.0] + "radius": 0.12 + - "center": [0.0, 0.0, -0.12] + "radius": 0.10 + + # ── Torso ───────────────────────────────────────────────────────────────── + # Tall chest link; inertia origin: [0.003, 0, 0.154] + # Spans roughly z = 0 … 0.40 m in the torso_link frame. + torso_link: + - "center": [0.0, 0.0, 0.0] + "radius": 0.13 + - "center": [0.02, 0.0, 0.10] + "radius": 0.13 + - "center": [0.02, 0.0, 0.20] + "radius": 0.12 + - "center": [0.02, 0.0, 0.32] + "radius": 0.11 + # Shoulder-width bloat (left/right) to protect shoulder joints + - "center": [0.0, 0.12, 0.30] + "radius": 0.07 + - "center": [0.0, -0.12, 0.30] + "radius": 0.07 + + # ── Right arm ───────────────────────────────────────────────────────────── + + # shoulder_pitch: small link bridging torso→roll joint + # Joint to next: [0, -0.038, -0.014]; inertia: [0, -0.036, -0.012] + right_shoulder_pitch_link: + - "center": [0.0, -0.025, -0.012] + "radius": 0.045 + + # shoulder_roll: ~10 cm link dropping toward shoulder_yaw + # Joint to next: [0, -0.006, -0.103]; inertia: [-0.000, -0.007, -0.063] + right_shoulder_roll_link: + - "center": [0.0, -0.007, -0.025] + "radius": 0.042 + - "center": [0.0, -0.007, -0.075] + "radius": 0.038 + + # shoulder_yaw: ~8 cm link bridging toward elbow + # Joint to next: [0.016, 0, -0.081]; inertia: [0.011, 0.003, -0.072] + right_shoulder_yaw_link: + - "center": [0.010, -0.003, -0.030] + "radius": 0.038 + - "center": [0.013, -0.002, -0.072] + "radius": 0.035 + + # elbow: forearm link, ~10 cm in x direction + # Joint to next: [0.100, -0.002, -0.010]; inertia: [0.065, -0.004, -0.010] + right_elbow_link: + - "center": [0.025, -0.004, -0.010] + "radius": 0.035 + - "center": [0.065, -0.004, -0.010] + "radius": 0.030 + + # wrist_roll: small link, ~3.8 cm to next joint + # inertia: [0.017, -0.001, 0.000] + right_wrist_roll_link: + - "center": [0.017, -0.001, 0.0] + "radius": 0.025 + + # wrist_pitch: ~4.6 cm to next joint + # inertia: [0.023, 0.001, -0.001] + right_wrist_pitch_link: + - "center": [0.023, 0.001, 0.0] + "radius": 0.025 + + # wrist_yaw: ~4.15 cm to palm joint + # inertia: [0.022, 0.000, 0.001] + right_wrist_yaw_link: + - "center": [0.022, 0.0, 0.0] + "radius": 0.025 + + # hand_palm: end-effector link; extends ~0.06 m in x + # inertia: [0.062, 0.001, -0.001] + right_hand_palm_link: + - "center": [0.025, 0.0, 0.0] + "radius": 0.025 + - "center": [0.060, 0.001, 0.0] + "radius": 0.020 + + # ── Left arm (locked joints — spheres protect against body/world hits) ──── + + left_shoulder_pitch_link: + - "center": [0.0, 0.025, -0.012] + "radius": 0.045 + + left_shoulder_roll_link: + - "center": [0.0, 0.007, -0.025] + "radius": 0.042 + - "center": [0.0, 0.007, -0.075] + "radius": 0.038 + + left_shoulder_yaw_link: + - "center": [0.010, 0.003, -0.030] + "radius": 0.038 + - "center": [0.013, 0.002, -0.072] + "radius": 0.035 + + left_elbow_link: + - "center": [0.025, 0.004, -0.010] + "radius": 0.035 + - "center": [0.065, 0.004, -0.010] + "radius": 0.030 + + left_wrist_roll_link: + - "center": [0.017, 0.001, 0.0] + "radius": 0.025 + + left_wrist_pitch_link: + - "center": [0.023, -0.001, 0.0] + "radius": 0.025 + + left_wrist_yaw_link: + - "center": [0.022, 0.0, 0.0] + "radius": 0.025 + + left_hand_palm_link: + - "center": [0.025, 0.0, 0.0] + "radius": 0.025 + - "center": [0.060, -0.001, 0.0] + "radius": 0.020 diff --git a/lw_benchhub/autosim/isaaclab_tasks/__init__.py b/lw_benchhub/autosim/isaaclab_tasks/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/lw_benchhub/autosim/isaaclab_tasks/g1_lift_cube_cfg.py b/lw_benchhub/autosim/isaaclab_tasks/g1_lift_cube_cfg.py new file mode 100644 index 00000000..002d7f57 --- /dev/null +++ b/lw_benchhub/autosim/isaaclab_tasks/g1_lift_cube_cfg.py @@ -0,0 +1,225 @@ +"""Isaac Lab scene + env config for G1 cube-lift autosim. + +Scene layout (units: metres, world frame): + - G1 robot : spawned at (0, 0, 0.8) floating via virtual base joints + - Table : at (0.6, 0, 0), packing table (≈ 1.0 m tall, non-instanceable) + - Cube : at (0.6, 0, 1.025) ← on top of the table + - Ground plane: z = -1.05 (below table) +""" + +import torch +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg +from isaaclab.envs import ManagerBasedRLEnv, ManagerBasedRLEnvCfg, mdp +from isaaclab.managers import EventTermCfg as EventTerm +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.managers import RewardTermCfg as RewTerm +from isaaclab.managers import TerminationTermCfg as DoneTerm +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sim.schemas.schemas_cfg import RigidBodyPropertiesCfg +from isaaclab.sim.spawners.from_files.from_files_cfg import GroundPlaneCfg, UsdFileCfg +from isaaclab.sim.schemas.schemas_cfg import MassPropertiesCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR + +import lw_benchhub.core.mdp as lw_benchhub_mdp +from lw_benchhub.core.robots.unitree.assets_cfg import G1_CFG + + +# --------------------------------------------------------------------------- +# Scene +# --------------------------------------------------------------------------- + +@configclass +class G1LiftCubeSceneCfg(InteractiveSceneCfg): + """Scene with G1 robot, a table, and a cube.""" + + robot: ArticulationCfg = G1_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + # Table the robot stands in front of + table = AssetBaseCfg( + prim_path="{ENV_REGEX_NS}/Table", + init_state=AssetBaseCfg.InitialStateCfg( + pos=[0.6, 0.0, 0.0], + rot=[0.707, 0.0, 0.0, 0.707], + ), + spawn=UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/PackingTable/packing_table.usd", + ), + ) + + # Target object — load from local USDA that has PhysicsRigidBodyAPI already applied. + # Using UsdFileCfg (same pattern as the Franka lift-cube reference) avoids the + # CuboidCfg / define_rigid_body_properties issue in this IsaacLab version. + cube: RigidObjectCfg = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Cube", + init_state=RigidObjectCfg.InitialStateCfg( + pos=[0.6, 0.0, 1.025], + rot=[1, 0, 0, 0], + ), + spawn=UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/blue_block.usd", + rigid_props=RigidBodyPropertiesCfg( + solver_position_iteration_count=16, + solver_velocity_iteration_count=1, + max_angular_velocity=1000.0, + max_linear_velocity=1000.0, + max_depenetration_velocity=5.0, + disable_gravity=False, + ), + mass_props=MassPropertiesCfg(mass=0.1), + ), + ) + + plane = AssetBaseCfg( + prim_path="/World/GroundPlane", + init_state=AssetBaseCfg.InitialStateCfg(pos=[0, 0, -1.05]), + spawn=GroundPlaneCfg(), + ) + + light = AssetBaseCfg( + prim_path="/World/light", + spawn=sim_utils.DomeLightCfg(color=(0.75, 0.75, 0.75), intensity=3000.0), + ) + + +# --------------------------------------------------------------------------- +# Actions (names must match what G1ActionAdapter expects) +# --------------------------------------------------------------------------- + +@configclass +class G1ActionsCfg: + """Action terms for G1 autosim. + + Action vector layout: + [0:4] base_action (loco cmd: vx, vy, vyaw, mode) + [4:11] right_arm (absolute joint position) + [11:18] left_arm (absolute joint position) + [18:32] gripper (14 finger joints, absolute position) + """ + + base_action: lw_benchhub_mdp.LegPositionActionCfg = lw_benchhub_mdp.LegPositionActionCfg( + asset_name="robot", + joint_names=[ + "left_hip_pitch_joint", "right_hip_pitch_joint", "left_hip_roll_joint", + "right_hip_roll_joint", "left_hip_yaw_joint", "right_hip_yaw_joint", + "left_knee_joint", "right_knee_joint", "left_ankle_pitch_joint", + "right_ankle_pitch_joint", "left_ankle_roll_joint", "right_ankle_roll_joint", + ], + body_name="base", + scale=1.0, + loco_config="g1_loco.yaml", + squat_config="g1_squat.yaml", + ) + + right_arm_action: mdp.JointPositionActionCfg = mdp.JointPositionActionCfg( + asset_name="robot", + joint_names=[ + "right_shoulder_pitch_joint", + "right_shoulder_roll_joint", + "right_shoulder_yaw_joint", + "right_elbow_joint", + "right_wrist_roll_joint", + "right_wrist_pitch_joint", + "right_wrist_yaw_joint", + ], + scale=1.0, + use_default_offset=False, + ) + + left_arm_action: mdp.JointPositionActionCfg = mdp.JointPositionActionCfg( + asset_name="robot", + joint_names=[ + "left_shoulder_pitch_joint", + "left_shoulder_roll_joint", + "left_shoulder_yaw_joint", + "left_elbow_joint", + "left_wrist_roll_joint", + "left_wrist_pitch_joint", + "left_wrist_yaw_joint", + ], + scale=1.0, + use_default_offset=False, + ) + + gripper_action: mdp.JointPositionActionCfg = mdp.JointPositionActionCfg( + asset_name="robot", + joint_names=[ + "right_hand_index_0_joint", "right_hand_index_1_joint", + "right_hand_middle_0_joint", "right_hand_middle_1_joint", + "right_hand_thumb_0_joint", "right_hand_thumb_1_joint", "right_hand_thumb_2_joint", + "left_hand_index_0_joint", "left_hand_index_1_joint", + "left_hand_middle_0_joint", "left_hand_middle_1_joint", + "left_hand_thumb_0_joint", "left_hand_thumb_1_joint", "left_hand_thumb_2_joint", + ], + scale=1.0, + use_default_offset=False, + ) + + +# --------------------------------------------------------------------------- +# Observations / Terminations / Events (minimal) +# --------------------------------------------------------------------------- + +@configclass +class G1ObservationsCfg: + @configclass + class PolicyCfg(ObsGroup): + actions = ObsTerm(func=mdp.last_action) + joint_pos = ObsTerm(func=mdp.joint_pos_rel) + joint_vel = ObsTerm(func=mdp.joint_vel_rel) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = False + + policy: PolicyCfg = PolicyCfg() + + +def _cube_lifted(env: ManagerBasedRLEnv) -> torch.Tensor: + return env.scene["cube"].data.root_pos_w[:, 2] > 0.85 + + +@configclass +class G1RewardsCfg: + pass + + +@configclass +class G1EventCfg: + pass + + +@configclass +class G1TerminationsCfg: + time_out = DoneTerm(func=mdp.time_out, time_out=True) + success = DoneTerm(func=_cube_lifted) + + +# --------------------------------------------------------------------------- +# Top-level env config +# --------------------------------------------------------------------------- + +@configclass +class G1LiftCubeEnvCfg(ManagerBasedRLEnvCfg): + scene: G1LiftCubeSceneCfg = G1LiftCubeSceneCfg( + num_envs=1, env_spacing=3.0, replicate_physics=False + ) + observations: G1ObservationsCfg = G1ObservationsCfg() + actions: G1ActionsCfg = G1ActionsCfg() + terminations: G1TerminationsCfg = G1TerminationsCfg() + rewards: G1RewardsCfg = G1RewardsCfg() + events: G1EventCfg = G1EventCfg() + + def __post_init__(self): + self.decimation = 2 + self.episode_length_s = 60.0 + self.sim.dt = 0.01 # 100 Hz physics + self.sim.render_interval = 2 + self.sim.use_fabric = False # disable fabric for compatibility + + 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 diff --git a/lw_benchhub/autosim/pipelines/g1_lift_cube.py b/lw_benchhub/autosim/pipelines/g1_lift_cube.py new file mode 100644 index 00000000..f96cf8b6 --- /dev/null +++ b/lw_benchhub/autosim/pipelines/g1_lift_cube.py @@ -0,0 +1,161 @@ +"""G1 autosim pipeline — RoboCasa kitchen variant. + +Uses a RoboCasa kitchen scene (PnPCounterToMicrowave) so that all objects +come with RigidBodyAPI already baked into their USD files. + +Robot: G1 loco controller variant (leg locomotion + dual arms + hands) +Task: Open microwave door +Scene: RoboCasa kitchen robocasakitchen-2-2 +Planner: cuRobo right-arm planning (g1_autosim.yml) +Nav: A* + leg locomotion controller (no virtual sliding base) +""" + +from pathlib import Path + +import torch +from autosim.core.pipeline import AutoSimPipeline, AutoSimPipelineCfg +from autosim.core.types import EnvExtraInfo +from autosim.decomposers import LLMDecomposerCfg +from isaaclab.envs import ManagerBasedEnv +from isaaclab.utils import configclass + +from lw_benchhub.autosim.action_adapters.g1_action_adapter_cfg import G1ActionAdapterCfg + +_CUROBO_ROOT = Path(__file__).parent.parent / "content" +_G1_URDF_DIR = Path(__file__).parent.parent / "content/assets/robot/g1" + + +@configclass +class G1LiftCubePipelineCfg(AutoSimPipelineCfg): + """Pipeline configuration for G1 RoboCasa kitchen microwave opening.""" + + decomposer: LLMDecomposerCfg = LLMDecomposerCfg() + action_adapter: G1ActionAdapterCfg = G1ActionAdapterCfg() + + def __post_init__(self): + # ---- Navigation ---- + self.skills.moveto.extra_cfg.local_planner.max_linear_velocity = 0.25 + self.skills.moveto.extra_cfg.local_planner.max_angular_velocity = 1.0 + self.skills.moveto.extra_cfg.local_planner.predict_time = 0.4 + self.skills.moveto.extra_cfg.global_planner.safety_distance = 0.5 + self.skills.moveto.extra_cfg.global_planner.proximity_weight = 3.0 + self.skills.moveto.extra_cfg.waypoint_tolerance = 0.25 + self.skills.moveto.extra_cfg.goal_tolerance = 0.30 + self.skills.moveto.extra_cfg.yaw_tolerance = 0.01 + self.skills.moveto.extra_cfg.use_dwa = False + self.skills.moveto.extra_cfg.sampling_radius = 0.8 + + # ---- Occupancy map (RoboCasa kitchen floor) ---- + self.occupancy_map.floor_prim_suffix = "Scene/floor_room" + + # ---- cuRobo ---- + self.motion_planner.robot_config_file = "g1_autosim.yml" + self.motion_planner.curobo_config_path = str(_CUROBO_ROOT / "configs" / "robot") + # Point cuRobo asset root at the g1 directory so the relative URDF + # path inside g1_autosim.yml resolves correctly. + self.motion_planner.curobo_asset_path = str(_G1_URDF_DIR) + self.motion_planner.world_ignore_subffixes = ["Scene/floor_room"] + # Focus collision world around the microwave area. + self.motion_planner.world_only_subffixes = ["Scene/microwave_main_group"] + self.skills.pull.extra_cfg.move_offset = 0.25 + self.skills.pull.extra_cfg.move_axis = "-x" + self.max_steps = 1000 + + +class G1LiftCubePipeline(AutoSimPipeline): + """Pipeline that opens microwave door with the G1 right arm. + + Scene loading uses RoboCasa task assets and the G1 locomotion robot config. + """ + + _TASK_NAME = "Robocasa-OpenMicrowave-G1-Autosim-v0" + + def __init__(self, cfg: AutoSimPipelineCfg): + super().__init__(cfg) + + # ------------------------------------------------------------------ + # Environment loading + # ------------------------------------------------------------------ + + def load_env(self) -> ManagerBasedEnv: + import gymnasium as gym + from lw_benchhub.utils.env import parse_env_cfg, ExecuteMode + from lw_benchhub.autosim.isaaclab_tasks.g1_lift_cube_cfg import ( + G1ActionsCfg, + G1ObservationsCfg, + G1EventCfg, + ) + + # ------------------------------------------------------------------ + # 1. Load a RoboCasa kitchen scene with G1 loco robot config. + # ------------------------------------------------------------------ + env_cfg = parse_env_cfg( + scene_backend="robocasa", + task_backend="robocasa", + task_name="OpenMicrowave", + robot_name="G1-Loco-Controller", + scene_name="robocasakitchen-2-2", + robot_scale=1.0, + device="cpu", + num_envs=1, + use_fabric=False, + first_person_view=False, + enable_cameras=False, + execute_mode=ExecuteMode.TRAIN, + usd_simplify=False, + seed=42, + sources=["objaverse", "lightwheel", "aigen_objs"], + object_projects=[], + rl_name=None, + headless_mode=False, + ) + + # ------------------------------------------------------------------ + # 2. Replace default robot action/obs/event with autosim-compatible + # terms while keeping G1 leg locomotion in base_action. + # ------------------------------------------------------------------ + env_cfg.actions = G1ActionsCfg() + + # Minimal observations — autosim reads scene data directly. + env_cfg.observations = G1ObservationsCfg() + + # Empty events — autosim manages resets itself. + env_cfg.events = G1EventCfg() + + # Disable built-in terminations — autosim manages episode endings. + env_cfg.terminations.time_out = None + + # ------------------------------------------------------------------ + # 3. Register and instantiate the env. + # ------------------------------------------------------------------ + gym.register( + id=self._TASK_NAME, + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={}, + disable_env_checker=True, + ) + + return gym.make(self._TASK_NAME, cfg=env_cfg).unwrapped + + # ------------------------------------------------------------------ + # Task metadata for the LLM decomposer + # ------------------------------------------------------------------ + + def get_env_extra_info(self) -> EnvExtraInfo: + env_info = EnvExtraInfo( + task_name="Robocasa-Task-OpenMicrowave", + objects=list(self._env.scene.keys()), + robot_name="robot", + robot_base_link_name="pelvis", # G1 physical base link + ee_link_name="right_wrist_yaw_link", # last rigid body before palm in g1_three_fingers.usd + object_reach_target_poses={ + # Approximate pre-grasp on microwave handle. + "microwave_main_group": [ + torch.tensor([0.05, -0.22, 0.12, 0.707, 0.0, 0.0, 0.707]), + ], + }, + ) + # Compatibility with autosim versions whose Reach skill expects optional + # extra-EEF pose fields/methods on EnvExtraInfo. + env_info.object_extra_reach_target_poses = {} + return env_info diff --git a/run_autosim.py b/run_autosim.py new file mode 100644 index 00000000..773eb76f --- /dev/null +++ b/run_autosim.py @@ -0,0 +1,42 @@ +"""Run an autosim pipeline registered in lw_benchhub.""" + +import argparse + +from isaaclab.app import AppLauncher + +parser = argparse.ArgumentParser(description="Run an lw_benchhub autosim pipeline.") +parser.add_argument( + "--pipeline_id", + type=str, + default="LWBenchhub-Autosim-G1OpenMicrowavePipeline-v0", + help="Registered pipeline ID.", +) +AppLauncher.add_app_launcher_args(parser) +args_cli = parser.parse_args() + +app_launcher = AppLauncher(vars(args_cli)) +simulation_app = app_launcher.app + +import lw_benchhub.autosim # noqa: F401 — triggers pipeline registration +from lw_benchhub.autosim.compat_autosim import patch_curobo_config, patch_env_extra_info, patch_reach_skill +from autosim import make_pipeline + + +def main(): + # EnvExtraInfo compatibility is needed by both X7S and G1 pipelines. + patch_env_extra_info() + + # Reach/curobo runtime compatibility patches are only needed for G1 branch. + if args_cli.pipeline_id in { + "LWBenchhub-Autosim-G1OpenMicrowavePipeline-v0", + "LWBenchhub-Autosim-G1LiftCubePipeline-v0", + }: + patch_reach_skill() + patch_curobo_config() + + pipeline = make_pipeline(args_cli.pipeline_id) + pipeline.run() + + +if __name__ == "__main__": + main() From ab944d96b88296a72147230f680b5be28569f658 Mon Sep 17 00:00:00 2001 From: Zihan Gao Date: Tue, 14 Apr 2026 18:35:08 -0400 Subject: [PATCH 02/32] delete uncessary files --- lw_benchhub/autosim/__init__.py | 11 +- .../autosim/content/assets/cube_5cm.usda | 3 - .../content/assets/robot/g1/generate_urdf.py | 115 --------- .../autosim/isaaclab_tasks/g1_autosim_cfg.py | 112 +++++++++ .../isaaclab_tasks/g1_lift_cube_cfg.py | 225 ------------------ .../{g1_lift_cube.py => g1_open_microwave.py} | 75 +++--- run_autosim.py | 42 ---- 7 files changed, 149 insertions(+), 434 deletions(-) delete mode 100644 lw_benchhub/autosim/content/assets/cube_5cm.usda delete mode 100644 lw_benchhub/autosim/content/assets/robot/g1/generate_urdf.py create mode 100644 lw_benchhub/autosim/isaaclab_tasks/g1_autosim_cfg.py delete mode 100644 lw_benchhub/autosim/isaaclab_tasks/g1_lift_cube_cfg.py rename lw_benchhub/autosim/pipelines/{g1_lift_cube.py => g1_open_microwave.py} (65%) delete mode 100644 run_autosim.py diff --git a/lw_benchhub/autosim/__init__.py b/lw_benchhub/autosim/__init__.py index bbccda31..fbe05a90 100644 --- a/lw_benchhub/autosim/__init__.py +++ b/lw_benchhub/autosim/__init__.py @@ -43,15 +43,8 @@ cfg_entry_point=f"{__name__}.pipelines.dessert_upgrade:DessertUpgradePipelineCfg", ) -register_pipeline( - id="LWBenchhub-Autosim-G1LiftCubePipeline-v0", - entry_point=f"{__name__}.pipelines.g1_lift_cube:G1LiftCubePipeline", - cfg_entry_point=f"{__name__}.pipelines.g1_lift_cube:G1LiftCubePipelineCfg", -) - -# New semantic name for the same G1 pipeline (microwave opening task). register_pipeline( id="LWBenchhub-Autosim-G1OpenMicrowavePipeline-v0", - entry_point=f"{__name__}.pipelines.g1_lift_cube:G1LiftCubePipeline", - cfg_entry_point=f"{__name__}.pipelines.g1_lift_cube:G1LiftCubePipelineCfg", + entry_point=f"{__name__}.pipelines.g1_open_microwave:G1OpenMicrowavePipeline", + cfg_entry_point=f"{__name__}.pipelines.g1_open_microwave:G1OpenMicrowavePipelineCfg", ) diff --git a/lw_benchhub/autosim/content/assets/cube_5cm.usda b/lw_benchhub/autosim/content/assets/cube_5cm.usda deleted file mode 100644 index c56a55d3..00000000 --- a/lw_benchhub/autosim/content/assets/cube_5cm.usda +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:b023d8c10b6f67c04b717cedb1599d9afe3333dfb66622fd5b0a57136de2c37f -size 1889 diff --git a/lw_benchhub/autosim/content/assets/robot/g1/generate_urdf.py b/lw_benchhub/autosim/content/assets/robot/g1/generate_urdf.py deleted file mode 100644 index 7cb00b4f..00000000 --- a/lw_benchhub/autosim/content/assets/robot/g1/generate_urdf.py +++ /dev/null @@ -1,115 +0,0 @@ -""" -Generate G1_autosim.urdf by prepending virtual base joints to the original -g1_29dof_with_hand.urdf. The virtual joints (base_x, base_y, base_yaw) mirror -the same approach used in X7S, allowing A*+P-controller navigation to drive the -robot via joint position commands in Isaac Lab. - -Usage: - python generate_urdf.py -Output: - G1_autosim.urdf (in the same directory as this script) -""" - -from pathlib import Path - -ORIGINAL_URDF = ( - Path(__file__).parent.parent.parent.parent.parent.parent - / "core/mdp/actions/wbc_policy/robot_model/g1/g1_29dof_with_hand.urdf" -) -OUTPUT_URDF = Path(__file__).parent / "G1_autosim.urdf" - -VIRTUAL_BASE_PREAMBLE = """\ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -""" - - -def generate(): - original = ORIGINAL_URDF.read_text(encoding="utf-8") - - # Remove the mujoco block (not needed by cuRobo / Isaac Lab URDF loader) - import re - original = re.sub(r"\s*.*?", "", original, flags=re.DOTALL) - - # Remove the commented-out floating_base_joint block so the file is clean - original = re.sub(r"", "", original, flags=re.DOTALL) - - # Find the insertion point: right before the first - insert_marker = '' - idx = original.find(insert_marker) - if idx == -1: - raise RuntimeError("Could not find '' in the original URDF.") - - output = original[:idx] + VIRTUAL_BASE_PREAMBLE + original[idx:] - - # Rename robot tag for clarity - output = output.replace('name="g1_29dof_with_hand"', 'name="g1_autosim"', 1) - - OUTPUT_URDF.parent.mkdir(parents=True, exist_ok=True) - OUTPUT_URDF.write_text(output, encoding="utf-8") - print(f"Written: {OUTPUT_URDF}") - print(f"Source: {ORIGINAL_URDF}") - - -if __name__ == "__main__": - generate() diff --git a/lw_benchhub/autosim/isaaclab_tasks/g1_autosim_cfg.py b/lw_benchhub/autosim/isaaclab_tasks/g1_autosim_cfg.py new file mode 100644 index 00000000..51523d69 --- /dev/null +++ b/lw_benchhub/autosim/isaaclab_tasks/g1_autosim_cfg.py @@ -0,0 +1,112 @@ +"""G1 autosim action / observation / event configs. + +These configs are shared across G1 autosim pipelines and are injected into +the RoboCasa-based env (replacing the default robot action/obs/event terms). +""" + +from isaaclab.envs import ManagerBasedRLEnv, mdp +from isaaclab.managers import EventTermCfg as EventTerm +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.managers import TerminationTermCfg as DoneTerm +from isaaclab.utils import configclass + +import lw_benchhub.core.mdp as lw_benchhub_mdp + + +# --------------------------------------------------------------------------- +# Actions (names must match what G1ActionAdapter expects) +# --------------------------------------------------------------------------- + +@configclass +class G1ActionsCfg: + """Action terms for G1 autosim. + + Action vector layout: + [0:4] base_action (loco cmd: vx, vy, vyaw, mode) + [4:11] right_arm (absolute joint position) + [11:18] left_arm (absolute joint position) + [18:32] gripper (14 finger joints, absolute position) + """ + + base_action: lw_benchhub_mdp.LegPositionActionCfg = lw_benchhub_mdp.LegPositionActionCfg( + asset_name="robot", + joint_names=[ + "left_hip_pitch_joint", "right_hip_pitch_joint", "left_hip_roll_joint", + "right_hip_roll_joint", "left_hip_yaw_joint", "right_hip_yaw_joint", + "left_knee_joint", "right_knee_joint", "left_ankle_pitch_joint", + "right_ankle_pitch_joint", "left_ankle_roll_joint", "right_ankle_roll_joint", + ], + body_name="base", + scale=1.0, + loco_config="g1_loco.yaml", + squat_config="g1_squat.yaml", + ) + + right_arm_action: mdp.JointPositionActionCfg = mdp.JointPositionActionCfg( + asset_name="robot", + joint_names=[ + "right_shoulder_pitch_joint", + "right_shoulder_roll_joint", + "right_shoulder_yaw_joint", + "right_elbow_joint", + "right_wrist_roll_joint", + "right_wrist_pitch_joint", + "right_wrist_yaw_joint", + ], + scale=1.0, + use_default_offset=False, + ) + + left_arm_action: mdp.JointPositionActionCfg = mdp.JointPositionActionCfg( + asset_name="robot", + joint_names=[ + "left_shoulder_pitch_joint", + "left_shoulder_roll_joint", + "left_shoulder_yaw_joint", + "left_elbow_joint", + "left_wrist_roll_joint", + "left_wrist_pitch_joint", + "left_wrist_yaw_joint", + ], + scale=1.0, + use_default_offset=False, + ) + + gripper_action: mdp.JointPositionActionCfg = mdp.JointPositionActionCfg( + asset_name="robot", + joint_names=[ + "right_hand_index_0_joint", "right_hand_index_1_joint", + "right_hand_middle_0_joint", "right_hand_middle_1_joint", + "right_hand_thumb_0_joint", "right_hand_thumb_1_joint", "right_hand_thumb_2_joint", + "left_hand_index_0_joint", "left_hand_index_1_joint", + "left_hand_middle_0_joint", "left_hand_middle_1_joint", + "left_hand_thumb_0_joint", "left_hand_thumb_1_joint", "left_hand_thumb_2_joint", + ], + scale=1.0, + use_default_offset=False, + ) + + +# --------------------------------------------------------------------------- +# Observations / Events (minimal — autosim reads scene data directly) +# --------------------------------------------------------------------------- + +@configclass +class G1ObservationsCfg: + @configclass + class PolicyCfg(ObsGroup): + actions = ObsTerm(func=mdp.last_action) + joint_pos = ObsTerm(func=mdp.joint_pos_rel) + joint_vel = ObsTerm(func=mdp.joint_vel_rel) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = False + + policy: PolicyCfg = PolicyCfg() + + +@configclass +class G1EventCfg: + pass diff --git a/lw_benchhub/autosim/isaaclab_tasks/g1_lift_cube_cfg.py b/lw_benchhub/autosim/isaaclab_tasks/g1_lift_cube_cfg.py deleted file mode 100644 index 002d7f57..00000000 --- a/lw_benchhub/autosim/isaaclab_tasks/g1_lift_cube_cfg.py +++ /dev/null @@ -1,225 +0,0 @@ -"""Isaac Lab scene + env config for G1 cube-lift autosim. - -Scene layout (units: metres, world frame): - - G1 robot : spawned at (0, 0, 0.8) floating via virtual base joints - - Table : at (0.6, 0, 0), packing table (≈ 1.0 m tall, non-instanceable) - - Cube : at (0.6, 0, 1.025) ← on top of the table - - Ground plane: z = -1.05 (below table) -""" - -import torch -import isaaclab.sim as sim_utils -from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg -from isaaclab.envs import ManagerBasedRLEnv, ManagerBasedRLEnvCfg, mdp -from isaaclab.managers import EventTermCfg as EventTerm -from isaaclab.managers import ObservationGroupCfg as ObsGroup -from isaaclab.managers import ObservationTermCfg as ObsTerm -from isaaclab.managers import RewardTermCfg as RewTerm -from isaaclab.managers import TerminationTermCfg as DoneTerm -from isaaclab.scene import InteractiveSceneCfg -from isaaclab.sim.schemas.schemas_cfg import RigidBodyPropertiesCfg -from isaaclab.sim.spawners.from_files.from_files_cfg import GroundPlaneCfg, UsdFileCfg -from isaaclab.sim.schemas.schemas_cfg import MassPropertiesCfg -from isaaclab.utils import configclass -from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR - -import lw_benchhub.core.mdp as lw_benchhub_mdp -from lw_benchhub.core.robots.unitree.assets_cfg import G1_CFG - - -# --------------------------------------------------------------------------- -# Scene -# --------------------------------------------------------------------------- - -@configclass -class G1LiftCubeSceneCfg(InteractiveSceneCfg): - """Scene with G1 robot, a table, and a cube.""" - - robot: ArticulationCfg = G1_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") - - # Table the robot stands in front of - table = AssetBaseCfg( - prim_path="{ENV_REGEX_NS}/Table", - init_state=AssetBaseCfg.InitialStateCfg( - pos=[0.6, 0.0, 0.0], - rot=[0.707, 0.0, 0.0, 0.707], - ), - spawn=UsdFileCfg( - usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/PackingTable/packing_table.usd", - ), - ) - - # Target object — load from local USDA that has PhysicsRigidBodyAPI already applied. - # Using UsdFileCfg (same pattern as the Franka lift-cube reference) avoids the - # CuboidCfg / define_rigid_body_properties issue in this IsaacLab version. - cube: RigidObjectCfg = RigidObjectCfg( - prim_path="{ENV_REGEX_NS}/Cube", - init_state=RigidObjectCfg.InitialStateCfg( - pos=[0.6, 0.0, 1.025], - rot=[1, 0, 0, 0], - ), - spawn=UsdFileCfg( - usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/blue_block.usd", - rigid_props=RigidBodyPropertiesCfg( - solver_position_iteration_count=16, - solver_velocity_iteration_count=1, - max_angular_velocity=1000.0, - max_linear_velocity=1000.0, - max_depenetration_velocity=5.0, - disable_gravity=False, - ), - mass_props=MassPropertiesCfg(mass=0.1), - ), - ) - - plane = AssetBaseCfg( - prim_path="/World/GroundPlane", - init_state=AssetBaseCfg.InitialStateCfg(pos=[0, 0, -1.05]), - spawn=GroundPlaneCfg(), - ) - - light = AssetBaseCfg( - prim_path="/World/light", - spawn=sim_utils.DomeLightCfg(color=(0.75, 0.75, 0.75), intensity=3000.0), - ) - - -# --------------------------------------------------------------------------- -# Actions (names must match what G1ActionAdapter expects) -# --------------------------------------------------------------------------- - -@configclass -class G1ActionsCfg: - """Action terms for G1 autosim. - - Action vector layout: - [0:4] base_action (loco cmd: vx, vy, vyaw, mode) - [4:11] right_arm (absolute joint position) - [11:18] left_arm (absolute joint position) - [18:32] gripper (14 finger joints, absolute position) - """ - - base_action: lw_benchhub_mdp.LegPositionActionCfg = lw_benchhub_mdp.LegPositionActionCfg( - asset_name="robot", - joint_names=[ - "left_hip_pitch_joint", "right_hip_pitch_joint", "left_hip_roll_joint", - "right_hip_roll_joint", "left_hip_yaw_joint", "right_hip_yaw_joint", - "left_knee_joint", "right_knee_joint", "left_ankle_pitch_joint", - "right_ankle_pitch_joint", "left_ankle_roll_joint", "right_ankle_roll_joint", - ], - body_name="base", - scale=1.0, - loco_config="g1_loco.yaml", - squat_config="g1_squat.yaml", - ) - - right_arm_action: mdp.JointPositionActionCfg = mdp.JointPositionActionCfg( - asset_name="robot", - joint_names=[ - "right_shoulder_pitch_joint", - "right_shoulder_roll_joint", - "right_shoulder_yaw_joint", - "right_elbow_joint", - "right_wrist_roll_joint", - "right_wrist_pitch_joint", - "right_wrist_yaw_joint", - ], - scale=1.0, - use_default_offset=False, - ) - - left_arm_action: mdp.JointPositionActionCfg = mdp.JointPositionActionCfg( - asset_name="robot", - joint_names=[ - "left_shoulder_pitch_joint", - "left_shoulder_roll_joint", - "left_shoulder_yaw_joint", - "left_elbow_joint", - "left_wrist_roll_joint", - "left_wrist_pitch_joint", - "left_wrist_yaw_joint", - ], - scale=1.0, - use_default_offset=False, - ) - - gripper_action: mdp.JointPositionActionCfg = mdp.JointPositionActionCfg( - asset_name="robot", - joint_names=[ - "right_hand_index_0_joint", "right_hand_index_1_joint", - "right_hand_middle_0_joint", "right_hand_middle_1_joint", - "right_hand_thumb_0_joint", "right_hand_thumb_1_joint", "right_hand_thumb_2_joint", - "left_hand_index_0_joint", "left_hand_index_1_joint", - "left_hand_middle_0_joint", "left_hand_middle_1_joint", - "left_hand_thumb_0_joint", "left_hand_thumb_1_joint", "left_hand_thumb_2_joint", - ], - scale=1.0, - use_default_offset=False, - ) - - -# --------------------------------------------------------------------------- -# Observations / Terminations / Events (minimal) -# --------------------------------------------------------------------------- - -@configclass -class G1ObservationsCfg: - @configclass - class PolicyCfg(ObsGroup): - actions = ObsTerm(func=mdp.last_action) - joint_pos = ObsTerm(func=mdp.joint_pos_rel) - joint_vel = ObsTerm(func=mdp.joint_vel_rel) - - def __post_init__(self): - self.enable_corruption = False - self.concatenate_terms = False - - policy: PolicyCfg = PolicyCfg() - - -def _cube_lifted(env: ManagerBasedRLEnv) -> torch.Tensor: - return env.scene["cube"].data.root_pos_w[:, 2] > 0.85 - - -@configclass -class G1RewardsCfg: - pass - - -@configclass -class G1EventCfg: - pass - - -@configclass -class G1TerminationsCfg: - time_out = DoneTerm(func=mdp.time_out, time_out=True) - success = DoneTerm(func=_cube_lifted) - - -# --------------------------------------------------------------------------- -# Top-level env config -# --------------------------------------------------------------------------- - -@configclass -class G1LiftCubeEnvCfg(ManagerBasedRLEnvCfg): - scene: G1LiftCubeSceneCfg = G1LiftCubeSceneCfg( - num_envs=1, env_spacing=3.0, replicate_physics=False - ) - observations: G1ObservationsCfg = G1ObservationsCfg() - actions: G1ActionsCfg = G1ActionsCfg() - terminations: G1TerminationsCfg = G1TerminationsCfg() - rewards: G1RewardsCfg = G1RewardsCfg() - events: G1EventCfg = G1EventCfg() - - def __post_init__(self): - self.decimation = 2 - self.episode_length_s = 60.0 - self.sim.dt = 0.01 # 100 Hz physics - self.sim.render_interval = 2 - self.sim.use_fabric = False # disable fabric for compatibility - - 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 diff --git a/lw_benchhub/autosim/pipelines/g1_lift_cube.py b/lw_benchhub/autosim/pipelines/g1_open_microwave.py similarity index 65% rename from lw_benchhub/autosim/pipelines/g1_lift_cube.py rename to lw_benchhub/autosim/pipelines/g1_open_microwave.py index f96cf8b6..a7cb1b24 100644 --- a/lw_benchhub/autosim/pipelines/g1_lift_cube.py +++ b/lw_benchhub/autosim/pipelines/g1_open_microwave.py @@ -1,13 +1,10 @@ -"""G1 autosim pipeline — RoboCasa kitchen variant. - -Uses a RoboCasa kitchen scene (PnPCounterToMicrowave) so that all objects -come with RigidBodyAPI already baked into their USD files. +"""G1 autosim pipeline — open microwave door. Robot: G1 loco controller variant (leg locomotion + dual arms + hands) Task: Open microwave door Scene: RoboCasa kitchen robocasakitchen-2-2 Planner: cuRobo right-arm planning (g1_autosim.yml) -Nav: A* + leg locomotion controller (no virtual sliding base) +Nav: A* + leg locomotion controller """ from pathlib import Path @@ -26,7 +23,7 @@ @configclass -class G1LiftCubePipelineCfg(AutoSimPipelineCfg): +class G1OpenMicrowavePipelineCfg(AutoSimPipelineCfg): """Pipeline configuration for G1 RoboCasa kitchen microwave opening.""" decomposer: LLMDecomposerCfg = LLMDecomposerCfg() @@ -51,26 +48,31 @@ def __post_init__(self): # ---- cuRobo ---- self.motion_planner.robot_config_file = "g1_autosim.yml" self.motion_planner.curobo_config_path = str(_CUROBO_ROOT / "configs" / "robot") - # Point cuRobo asset root at the g1 directory so the relative URDF - # path inside g1_autosim.yml resolves correctly. self.motion_planner.curobo_asset_path = str(_G1_URDF_DIR) self.motion_planner.world_ignore_subffixes = ["Scene/floor_room"] - # Focus collision world around the microwave area. - self.motion_planner.world_only_subffixes = ["Scene/microwave_main_group"] + self.motion_planner.world_only_subffixes = ["Scene/microwave_main_group"] + + # ---- Pull skill (open microwave door by pulling) ---- self.skills.pull.extra_cfg.move_offset = 0.25 - self.skills.pull.extra_cfg.move_axis = "-x" - self.max_steps = 1000 + self.skills.pull.extra_cfg.move_axis = "-x" + self.max_steps = 1000 -class G1LiftCubePipeline(AutoSimPipeline): - """Pipeline that opens microwave door with the G1 right arm. - Scene loading uses RoboCasa task assets and the G1 locomotion robot config. - """ +class G1OpenMicrowavePipeline(AutoSimPipeline): + """Pipeline that opens a microwave door with the G1 right arm.""" _TASK_NAME = "Robocasa-OpenMicrowave-G1-Autosim-v0" def __init__(self, cfg: AutoSimPipelineCfg): + from lw_benchhub.autosim.compat_autosim import ( + patch_curobo_config, + patch_env_extra_info, + patch_reach_skill, + ) + patch_env_extra_info() + patch_reach_skill() + patch_curobo_config() super().__init__(cfg) # ------------------------------------------------------------------ @@ -80,15 +82,12 @@ def __init__(self, cfg: AutoSimPipelineCfg): def load_env(self) -> ManagerBasedEnv: import gymnasium as gym from lw_benchhub.utils.env import parse_env_cfg, ExecuteMode - from lw_benchhub.autosim.isaaclab_tasks.g1_lift_cube_cfg import ( + from lw_benchhub.autosim.isaaclab_tasks.g1_autosim_cfg import ( G1ActionsCfg, G1ObservationsCfg, G1EventCfg, ) - # ------------------------------------------------------------------ - # 1. Load a RoboCasa kitchen scene with G1 loco robot config. - # ------------------------------------------------------------------ env_cfg = parse_env_cfg( scene_backend="robocasa", task_backend="robocasa", @@ -110,24 +109,13 @@ def load_env(self) -> ManagerBasedEnv: headless_mode=False, ) - # ------------------------------------------------------------------ - # 2. Replace default robot action/obs/event with autosim-compatible - # terms while keeping G1 leg locomotion in base_action. - # ------------------------------------------------------------------ - env_cfg.actions = G1ActionsCfg() - - # Minimal observations — autosim reads scene data directly. + env_cfg.actions = G1ActionsCfg() env_cfg.observations = G1ObservationsCfg() + env_cfg.events = G1EventCfg() - # Empty events — autosim manages resets itself. - env_cfg.events = G1EventCfg() - - # Disable built-in terminations — autosim manages episode endings. + # Disable built-in timeout — autosim manages episode endings. env_cfg.terminations.time_out = None - # ------------------------------------------------------------------ - # 3. Register and instantiate the env. - # ------------------------------------------------------------------ gym.register( id=self._TASK_NAME, entry_point="isaaclab.envs:ManagerBasedRLEnv", @@ -135,7 +123,17 @@ def load_env(self) -> ManagerBasedEnv: disable_env_checker=True, ) - return gym.make(self._TASK_NAME, cfg=env_cfg).unwrapped + env = gym.make(self._TASK_NAME, cfg=env_cfg).unwrapped + + # env_cfg.events was replaced with an empty G1EventCfg to prevent + # unwanted autosim resets, which removed the startup init_task event + # that normally calls task.init_fixtures(env) → fixture.setup_env(env). + # Call it explicitly so fixture controllers (e.g. Microwave) have + # self._env set before update_state() is triggered. + arena_env = env.cfg.isaaclab_arena_env + arena_env.task.init_fixtures(env) + + return env # ------------------------------------------------------------------ # Task metadata for the LLM decomposer @@ -146,16 +144,13 @@ def get_env_extra_info(self) -> EnvExtraInfo: task_name="Robocasa-Task-OpenMicrowave", objects=list(self._env.scene.keys()), robot_name="robot", - robot_base_link_name="pelvis", # G1 physical base link - ee_link_name="right_wrist_yaw_link", # last rigid body before palm in g1_three_fingers.usd + robot_base_link_name="pelvis", + ee_link_name="right_wrist_yaw_link", object_reach_target_poses={ - # Approximate pre-grasp on microwave handle. "microwave_main_group": [ torch.tensor([0.05, -0.22, 0.12, 0.707, 0.0, 0.0, 0.707]), ], }, ) - # Compatibility with autosim versions whose Reach skill expects optional - # extra-EEF pose fields/methods on EnvExtraInfo. env_info.object_extra_reach_target_poses = {} return env_info diff --git a/run_autosim.py b/run_autosim.py deleted file mode 100644 index 773eb76f..00000000 --- a/run_autosim.py +++ /dev/null @@ -1,42 +0,0 @@ -"""Run an autosim pipeline registered in lw_benchhub.""" - -import argparse - -from isaaclab.app import AppLauncher - -parser = argparse.ArgumentParser(description="Run an lw_benchhub autosim pipeline.") -parser.add_argument( - "--pipeline_id", - type=str, - default="LWBenchhub-Autosim-G1OpenMicrowavePipeline-v0", - help="Registered pipeline ID.", -) -AppLauncher.add_app_launcher_args(parser) -args_cli = parser.parse_args() - -app_launcher = AppLauncher(vars(args_cli)) -simulation_app = app_launcher.app - -import lw_benchhub.autosim # noqa: F401 — triggers pipeline registration -from lw_benchhub.autosim.compat_autosim import patch_curobo_config, patch_env_extra_info, patch_reach_skill -from autosim import make_pipeline - - -def main(): - # EnvExtraInfo compatibility is needed by both X7S and G1 pipelines. - patch_env_extra_info() - - # Reach/curobo runtime compatibility patches are only needed for G1 branch. - if args_cli.pipeline_id in { - "LWBenchhub-Autosim-G1OpenMicrowavePipeline-v0", - "LWBenchhub-Autosim-G1LiftCubePipeline-v0", - }: - patch_reach_skill() - patch_curobo_config() - - pipeline = make_pipeline(args_cli.pipeline_id) - pipeline.run() - - -if __name__ == "__main__": - main() From 6370e71a41226e23b8e69bca6d2d6b6bfd387f12 Mon Sep 17 00:00:00 2001 From: Zihan Gao Date: Tue, 21 Apr 2026 18:10:45 -0400 Subject: [PATCH 03/32] [feat] unify G1/X7S pipelines and eliminate compat_autosim patches MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Merge g1_close_oven.py and g1_kettle_boiling.py into shared close_oven.py and kettle_boiling.py; robot-specific logic driven by TaskRobotOverride hooks - Add g1_loco_left RobotProfile with curobo_asset_path, self_collision_check, env_cfg_setup_fn; remove runtime_patches_fn and navigate_sampling_radius - Rename g1_left_ee.yml → g1.yml, g1_autosim.yml → g1_right_ee.yml to match X7S naming convention - Delete compat_autosim.py: patch_curobo_config replaced by CuroboPlannerCfg fields, patch_navigate_skill replaced by per_object_sampling_radius cfg, patch_reach_skill superseded by upstream autosim - Clean up reach_plan_sweep.py to remove all compat patch calls - Register G1 pipelines via cfg_overrides robot_profile instead of separate files Co-Authored-By: Claude Sonnet 4.6 --- lw_benchhub/autosim/__init__.py | 20 ++ .../action_adapters/g1_action_adapter.py | 43 +-- .../action_adapters/g1_action_adapter_cfg.py | 15 +- .../g1_right_arm_only_action_adapter.py | 84 ++++++ .../g1_right_arm_only_action_adapter_cfg.py | 22 ++ lw_benchhub/autosim/compat_autosim.py | 281 ------------------ .../autosim/content/configs/robot/g1.yml | 119 ++++++++ .../robot/{g1_autosim.yml => g1_right_ee.yml} | 29 +- lw_benchhub/autosim/pipelines/close_oven.py | 160 +++++++--- .../autosim/pipelines/g1_open_microwave.py | 156 ---------- .../autosim/pipelines/kettle_boiling.py | 191 ++++++++---- lw_benchhub/autosim/robot_profiles.py | 96 +++--- lw_benchhub/core/mdp/configs/g1_squat.yaml | 4 +- .../scripts/autosim/reach_plan_sweep.py | 4 + 14 files changed, 606 insertions(+), 618 deletions(-) create mode 100644 lw_benchhub/autosim/action_adapters/g1_right_arm_only_action_adapter.py create mode 100644 lw_benchhub/autosim/action_adapters/g1_right_arm_only_action_adapter_cfg.py delete mode 100644 lw_benchhub/autosim/compat_autosim.py create mode 100644 lw_benchhub/autosim/content/configs/robot/g1.yml rename lw_benchhub/autosim/content/configs/robot/{g1_autosim.yml => g1_right_ee.yml} (81%) delete mode 100644 lw_benchhub/autosim/pipelines/g1_open_microwave.py diff --git a/lw_benchhub/autosim/__init__.py b/lw_benchhub/autosim/__init__.py index 8d0a73dd..0aa08556 100644 --- a/lw_benchhub/autosim/__init__.py +++ b/lw_benchhub/autosim/__init__.py @@ -42,3 +42,23 @@ entry_point=f"{__name__}.pipelines.g1_open_microwave:G1OpenMicrowavePipeline", cfg_entry_point=f"{__name__}.pipelines.g1_open_microwave:G1OpenMicrowavePipelineCfg", ) + +register_pipeline( + id="LWBenchhub-Autosim-G1OpenMicrowaveRightOnlyPipeline-v0", + entry_point=f"{__name__}.pipelines.g1_open_microwave_right_only:G1OpenMicrowaveRightOnlyPipeline", + cfg_entry_point=f"{__name__}.pipelines.g1_open_microwave_right_only:G1OpenMicrowaveRightOnlyPipelineCfg", +) + +register_pipeline( + id="LWBenchhub-Autosim-G1KettleBoilingPipeline-v0", + entry_point=f"{__name__}.pipelines.kettle_boiling:KettleBoilingPipeline", + cfg_entry_point=f"{__name__}.pipelines.kettle_boiling:KettleBoilingPipelineCfg", + robot_profile="g1_loco_left", +) + +register_pipeline( + id="LWBenchhub-Autosim-G1CloseOvenPipeline-v0", + entry_point=f"{__name__}.pipelines.close_oven:CloseOvenPipeline", + cfg_entry_point=f"{__name__}.pipelines.close_oven:CloseOvenPipelineCfg", + robot_profile="g1_loco_left", +) diff --git a/lw_benchhub/autosim/action_adapters/g1_action_adapter.py b/lw_benchhub/autosim/action_adapters/g1_action_adapter.py index b948cd86..e2c673cc 100644 --- a/lw_benchhub/autosim/action_adapters/g1_action_adapter.py +++ b/lw_benchhub/autosim/action_adapters/g1_action_adapter.py @@ -64,10 +64,13 @@ def _apply_moveto(self, skill_output: SkillOutput, env: ManagerBasedEnv) -> torc last_action = env.action_manager.action action = last_action[0, :].clone() - # base_action uses scale=0.01: action_value = delta / 0.01 - action[0] = (vx_body * dt_control) / 0.01 - action[1] = (vy_body * dt_control) / 0.01 - action[2] = (vyaw * dt_control) / 0.01 + # LegPositionAction: cmd_raw is normalized; actual = cmd_raw * max_cmd + # max_cmd = [0.3, 0.3, 0.3] from g1_loco.yaml + _MAX_CMD = 0.3 + action[0] = vx_body / _MAX_CMD + action[1] = vy_body / _MAX_CMD + action[2] = vyaw / _MAX_CMD + action[3] = 0.0 # mode=0: locomotion return action @@ -89,16 +92,17 @@ def _apply_reach(self, skill_output: SkillOutput, env: ManagerBasedEnv) -> torch r_arm_ids, _ = robot.find_joints(env.action_manager.get_term("right_arm_action").cfg.joint_names) l_arm_ids, _ = robot.find_joints(env.action_manager.get_term("left_arm_action").cfg.joint_names) - # Keep base steady for manipulation (zero delta → no base movement). - action[:3] = 0.0 - # G1ActionsCfg action layout: - # [0:3] base_action (scale=0.01 delta, kept zero above) - # [3:10] right_arm (7-DoF absolute) - # [10:17] left_arm (7-DoF absolute) - # [17:31] gripper (14 joints absolute) - action[3:10] = target_joint_pos[r_arm_ids] - action[10:17] = target_joint_pos[l_arm_ids] + # [0:4] base_action LegPositionAction [vx, vy, vyaw, mode] + # [4:11] right_arm (7-DoF absolute) + # [11:18] left_arm (7-DoF absolute) + # [18:32] gripper (14 joints absolute) + action[0] = 0.0 + action[1] = 0.0 + action[2] = 0.0 + action[3] = 1.0 # mode=1: squat/stance — keep legs fixed during arm motion + action[4:11] = target_joint_pos[r_arm_ids] + action[11:18] = target_joint_pos[l_arm_ids] return action @@ -111,17 +115,20 @@ def _apply_gripper(self, skill_output: SkillOutput, env: ManagerBasedEnv) -> tor gripper_signal = skill_output.action[0].item() # -1.0 = grasp, +1.0 = ungrasp - # Map signal → actual joint angle - finger_angle = self.cfg.finger_close_angle if gripper_signal < 0 else self.cfg.finger_open_angle + angles = self.cfg.finger_close_angles if gripper_signal < 0 else self.cfg.finger_open_angles + finger_angles = torch.tensor(angles, dtype=torch.float32, device=env.device) last_action = env.action_manager.action action = last_action[0, :].clone() - action[:3] = 0.0 # keep base steady + action[0] = 0.0 + action[1] = 0.0 + action[2] = 0.0 + action[3] = 1.0 # mode=1: squat/stance robot = env.scene["robot"] finger_ids, _ = robot.find_joints(env.action_manager.get_term("gripper_action").cfg.joint_names) - # gripper_action starts at action index 17 - action[17:17 + len(finger_ids)] = finger_angle + # gripper_action starts at action index 18 + action[18:18 + len(finger_ids)] = finger_angles[:len(finger_ids)] return action diff --git a/lw_benchhub/autosim/action_adapters/g1_action_adapter_cfg.py b/lw_benchhub/autosim/action_adapters/g1_action_adapter_cfg.py index aa766468..c059d0dc 100644 --- a/lw_benchhub/autosim/action_adapters/g1_action_adapter_cfg.py +++ b/lw_benchhub/autosim/action_adapters/g1_action_adapter_cfg.py @@ -15,7 +15,14 @@ class G1ActionAdapterCfg(ActionAdapterCfg): base_y_joint_name: str = "base_y_joint" base_yaw_joint_name: str = "base_yaw_joint" - finger_close_angle: float = 1.2 - """Finger joint angle (rad) when gripper is closed.""" - finger_open_angle: float = 0.0 - """Finger joint angle (rad) when gripper is open.""" + finger_close_angles: tuple = ( + # right hand stays open (left arm is the grasping arm) + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + # left hand closes + -1.9, -2.0, # left index_0, index_1 + -1.9, -2.0, # left middle_0, middle_1 + 0.8, 0.8, 1.8, # left thumb_0, thumb_1, thumb_2 + ) + """Per-joint finger angles (rad) when gripper is closed.""" + finger_open_angles: tuple = (0.0,) * 14 + """Per-joint finger angles (rad) when gripper is open.""" diff --git a/lw_benchhub/autosim/action_adapters/g1_right_arm_only_action_adapter.py b/lw_benchhub/autosim/action_adapters/g1_right_arm_only_action_adapter.py new file mode 100644 index 00000000..5a682826 --- /dev/null +++ b/lw_benchhub/autosim/action_adapters/g1_right_arm_only_action_adapter.py @@ -0,0 +1,84 @@ +from __future__ import annotations + +from typing import TYPE_CHECKING + +import torch +from isaaclab.envs import ManagerBasedEnv + +from autosim import ActionAdapterBase +from autosim.core.types import SkillOutput + +if TYPE_CHECKING: + from .g1_right_arm_only_action_adapter_cfg import G1RightArmOnlyActionAdapterCfg + + +class G1RightArmOnlyActionAdapter(ActionAdapterBase): + """Action adapter for G1 — right arm only, legs fixed in stance mode. + + Action vector layout (G1ActionsCfg): + [0:4] base locomotion command — action[3]=1.0 keeps squat/stance + [4:11] right arm joints (7 DoF, absolute position) — planned by cuRobo + [11:18] left arm joints (7 DoF, absolute position) — locked by cuRobo + [18:32] fingers (right + left three-finger hands) + + moveto is not registered; the LLM will not generate navigation steps. + """ + + def __init__(self, cfg: G1RightArmOnlyActionAdapterCfg): + super().__init__(cfg) + self.register_apply_method("reach", self._apply_reach) + self.register_apply_method("lift", self._apply_reach) + self.register_apply_method("push", self._apply_reach) + self.register_apply_method("pull", self._apply_reach) + self.register_apply_method("grasp", self._apply_gripper) + self.register_apply_method("ungrasp", self._apply_gripper) + + # ------------------------------------------------------------------ + # Arm motion (cuRobo joint trajectory playback — left arm primary) + # ------------------------------------------------------------------ + + def _apply_reach(self, skill_output: SkillOutput, env: ManagerBasedEnv) -> torch.Tensor: + """Write cuRobo joint positions into the arm action terms. + + cuRobo plans the right arm; left arm joints come back at their locked + values and are written unchanged. + """ + target_joint_pos = skill_output.action # [N_sim_joints], full robot order + robot = env.scene["robot"] + + last_action = env.action_manager.action + action = last_action[0, :].clone() + + r_arm_ids, _ = robot.find_joints(env.action_manager.get_term("right_arm_action").cfg.joint_names) + l_arm_ids, _ = robot.find_joints(env.action_manager.get_term("left_arm_action").cfg.joint_names) + + # G1ActionsCfg layout: [vx, vy, vyaw, mode, r_arm×7, l_arm×7, gripper×14] + action[0] = 0.0 + action[1] = 0.0 + action[2] = 0.0 + action[3] = 1.0 # mode=1: squat/stance — legs fixed + + action[4:11] = target_joint_pos[r_arm_ids] # right arm — planned + action[11:18] = target_joint_pos[l_arm_ids] # left arm — locked + + return action + + def _apply_gripper(self, skill_output: SkillOutput, env: ManagerBasedEnv) -> torch.Tensor: + """Set right-hand finger joints to closed (grasp) or open (ungrasp).""" + + gripper_signal = skill_output.action[0].item() + angles = self.cfg.finger_close_angles if gripper_signal < 0 else self.cfg.finger_open_angles + finger_angles = torch.tensor(angles, dtype=torch.float32, device=env.device) + + robot = env.scene["robot"] + last_action = env.action_manager.action + action = last_action[0, :].clone() + action[0] = 0.0 + action[1] = 0.0 + action[2] = 0.0 + action[3] = 1.0 # mode=1: squat/stance + + finger_ids, _ = robot.find_joints(env.action_manager.get_term("gripper_action").cfg.joint_names) + action[18:18 + len(finger_ids)] = finger_angles[:len(finger_ids)] + + return action diff --git a/lw_benchhub/autosim/action_adapters/g1_right_arm_only_action_adapter_cfg.py b/lw_benchhub/autosim/action_adapters/g1_right_arm_only_action_adapter_cfg.py new file mode 100644 index 00000000..94ec7f9c --- /dev/null +++ b/lw_benchhub/autosim/action_adapters/g1_right_arm_only_action_adapter_cfg.py @@ -0,0 +1,22 @@ +from isaaclab.utils import configclass + +from autosim import ActionAdapterCfg + +from .g1_right_arm_only_action_adapter import G1RightArmOnlyActionAdapter + + +@configclass +class G1RightArmOnlyActionAdapterCfg(ActionAdapterCfg): + """Configuration for the G1 right-arm-only action adapter (legs in stance).""" + + class_type: type = G1RightArmOnlyActionAdapter + + finger_close_angles: tuple = ( + 1.2, 1.4, # right index_0, index_1 + 1.2, 1.4, # right middle_0, middle_1 + 0.5, -0.5, -1.2, # right thumb_0, thumb_1, thumb_2 + -1.2, -1.4, # left index_0, index_1 + -1.2, -1.4, # left middle_0, middle_1 + 0.5, 0.5, 1.2, # left thumb_0, thumb_1, thumb_2 + ) + finger_open_angles: tuple = (0.0,) * 14 diff --git a/lw_benchhub/autosim/compat_autosim.py b/lw_benchhub/autosim/compat_autosim.py deleted file mode 100644 index 708e32db..00000000 --- a/lw_benchhub/autosim/compat_autosim.py +++ /dev/null @@ -1,281 +0,0 @@ -"""Runtime compatibility patches for external autosim package.""" - -from __future__ import annotations - -import types - -import isaaclab.utils.math as PoseUtils -import torch - -from autosim.core.types import SkillGoal - - -def patch_env_extra_info() -> None: - """Add backward/forward compatibility fields to EnvExtraInfo.""" - from autosim.core.types import EnvExtraInfo - - if hasattr(EnvExtraInfo, "_lw_patched_extra_reach"): - return - - original_init = EnvExtraInfo.__init__ - original_reset = EnvExtraInfo.reset - - def _reset_extra_target_pose_iterators(self) -> None: - data = getattr(self, "object_extra_reach_target_poses", {}) or {} - self._object_extra_reach_target_poses_iterator_dict = { - object_name: { - ee_name: self._build_iterator(reach_target_poses) - for ee_name, reach_target_poses in extra_targets.items() - } - for object_name, extra_targets in data.items() - } - - def __init__(self, *args, object_extra_reach_target_poses=None, **kwargs): - original_init(self, *args, **kwargs) - self.object_extra_reach_target_poses = object_extra_reach_target_poses or {} - _reset_extra_target_pose_iterators(self) - - def reset(self) -> None: - original_reset(self) - if not hasattr(self, "object_extra_reach_target_poses"): - self.object_extra_reach_target_poses = {} - _reset_extra_target_pose_iterators(self) - - def get_next_extra_reach_target_pose(self, object_name: str, ee_name: str): - return next(self._object_extra_reach_target_poses_iterator_dict[object_name][ee_name]) - - EnvExtraInfo.__init__ = __init__ - EnvExtraInfo.reset = reset - EnvExtraInfo._reset_extra_target_pose_iterators = _reset_extra_target_pose_iterators - EnvExtraInfo.get_next_extra_reach_target_pose = get_next_extra_reach_target_pose - EnvExtraInfo._lw_patched_extra_reach = True - - -def patch_reach_skill() -> None: - """Patch ReachSkill for mixed autosim versions. - - Some autosim versions include newer ReachSkill call-sites that rely on - helper methods/fields not present in older class definitions. - """ - from autosim.skills import reach as reach_mod - from autosim.core.registration import SkillRegistry - - ReachSkill = reach_mod.ReachSkill - - if not hasattr(ReachSkill, "_lw_original_build_activate_joint_state"): - ReachSkill._lw_original_build_activate_joint_state = ReachSkill._build_activate_joint_state - - def _build_activate_joint_state_compat( - self, full_sim_joint_names, full_sim_q, full_sim_qd=None - ): - # Planner may include virtual base joints that do not exist in the - # simulated leg-loco robot articulation. Fill missing joints with 0. - activate_q = [] - activate_qd = [] if full_sim_qd is not None else None - for joint_name in self._planner.target_joint_names: - if joint_name in full_sim_joint_names: - sim_joint_idx = full_sim_joint_names.index(joint_name) - activate_q.append(full_sim_q[sim_joint_idx]) - if full_sim_qd is not None and activate_qd is not None: - activate_qd.append(full_sim_qd[sim_joint_idx]) - else: - activate_q.append(torch.tensor(0.0, device=full_sim_q.device, dtype=full_sim_q.dtype)) - if full_sim_qd is not None and activate_qd is not None: - activate_qd.append( - torch.tensor(0.0, device=full_sim_qd.device, dtype=full_sim_qd.dtype) - ) - activate_q_tensor = torch.stack(activate_q, dim=0) - if activate_qd is None: - return activate_q_tensor, None - return activate_q_tensor, torch.stack(activate_qd, dim=0) - - ReachSkill._build_activate_joint_state = _build_activate_joint_state_compat - - if not hasattr(ReachSkill, "_lw_original_step"): - ReachSkill._lw_original_step = ReachSkill.step - - def step_compat(self, state): - self.visualize_debug_target_pose() - - traj_positions = self._trajectory.position - if self._step_idx >= len(self._trajectory.position): - traj_pos = traj_positions[-1] - done = True - else: - traj_pos = traj_positions[self._step_idx] - done = False - self._step_idx += 1 - - # Keep upstream corrective-reach behavior when available. - if done and getattr(self, "_corrective_reach_done", False) is False and getattr( - self.cfg.extra_cfg, "corrective_reach", False - ): - self._corrective_reach_done = True - if hasattr(self, "_compute_corrective_goal"): - new_goal = self._compute_corrective_goal() - if new_goal is not None: - self._logger.info("corrective_reach: re-planning to corrected object pose") - self._step_idx = 0 - plan_success = self.execute_plan(state, new_goal) - if plan_success: - done = False - - curobo_joint_names = self._trajectory.joint_names - sim_joint_names = state.sim_joint_names - joint_pos = state.robot_joint_pos.clone() - for curobo_idx, curobo_joint_name in enumerate(curobo_joint_names): - if curobo_joint_name not in sim_joint_names: - continue - sim_idx = sim_joint_names.index(curobo_joint_name) - joint_pos[sim_idx] = traj_pos[curobo_idx] - - from autosim.core.types import SkillOutput - - return SkillOutput(action=joint_pos, done=done, success=True, info={}) - - ReachSkill.step = step_compat - - if not hasattr(ReachSkill, "_compute_goal_from_offset"): - - def _compute_goal_from_offset( - self, - env, - robot_name: str, - target_object: str, - reach_offset: torch.Tensor, - extra_offsets=None, - ): - try: - object_pose_in_env = env.scene[target_object].data.root_pose_w - except Exception: - self._logger.warning(f"could not read pose for '{target_object}', skipping") - return None - - object_pos_in_env = object_pose_in_env[:, :3] - object_quat_in_env = object_pose_in_env[:, 3:] - - offset = reach_offset.to(env.device).unsqueeze(0) - reach_target_pos_in_env, reach_target_quat_in_env = PoseUtils.combine_frame_transforms( - object_pos_in_env, object_quat_in_env, offset[:, :3], offset[:, 3:] - ) - self._target_poses["target_pose"] = torch.cat((reach_target_pos_in_env, reach_target_quat_in_env), dim=-1) - - robot = env.scene[robot_name] - robot_root_pos_in_env = robot.data.root_pose_w[:, :3] - robot_root_quat_in_env = robot.data.root_pose_w[:, 3:] - - reach_target_pos_in_robot_root, reach_target_quat_in_robot_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, - ) - target_pose = torch.cat( - (reach_target_pos_in_robot_root, reach_target_quat_in_robot_root), dim=-1 - ).squeeze(0) - - activate_q, _ = self._build_activate_joint_state( - robot.data.joint_names, robot.data.joint_pos[0], robot.data.joint_vel[0] - ) - # Current upstream offset-based extra-EEF API is inconsistent across - # versions. Keep behavior stable by using the built-in policy. - extra_target_poses = self._build_extra_target_poses(activate_q, target_pose, self._saved_env_extra_info) - - return SkillGoal(target_object=target_object, target_pose=target_pose, extra_target_poses=extra_target_poses) - - ReachSkill._compute_goal_from_offset = _compute_goal_from_offset - - original_extract = ReachSkill.extract_goal_from_info - - def extract_goal_from_info(self, skill_info, env, env_extra_info): - target_object = skill_info.target_object - reach_offset = env_extra_info.get_next_reach_target_pose(target_object).to(env.device) - - self._saved_env = env - self._saved_env_extra_info = env_extra_info - self._saved_robot_name = env_extra_info.robot_name - self._saved_target_object = target_object - self._saved_reach_offset = reach_offset - self._saved_extra_offsets = None - - # Keep compatibility with older/newer autosim versions. - if hasattr(self, "_compute_goal_from_offset"): - return self._compute_goal_from_offset(env, env_extra_info.robot_name, target_object, reach_offset, None) - return original_extract(self, skill_info, env, env_extra_info) - - ReachSkill.extract_goal_from_info = extract_goal_from_info - - original_init = ReachSkill.__init__ - - def __init__(self, extra_cfg): - original_init(self, extra_cfg) - if not hasattr(self.cfg.extra_cfg, "corrective_reach"): - self.cfg.extra_cfg.corrective_reach = False - self._corrective_reach_done = False - self._saved_env = None - self._saved_env_extra_info = None - self._saved_robot_name = "robot" - self._saved_target_object = None - self._saved_reach_offset = None - self._saved_extra_offsets = None - - ReachSkill.__init__ = __init__ - - original_reset = ReachSkill.reset - - def reset(self): - original_reset(self) - self._corrective_reach_done = False - self._saved_env = None - self._saved_env_extra_info = None - self._saved_target_object = None - self._saved_reach_offset = None - self._saved_extra_offsets = None - - ReachSkill.reset = reset - - # Fallback: patch the factory path too, in case another ReachSkill class - # object (from a different autosim import path) is instantiated. - if not hasattr(SkillRegistry, "_lw_patched_create"): - - @classmethod - def _patched_create(cls, name, extra_cfg): - skill_cls = cls.get(name) - skill = skill_cls(extra_cfg) - if skill.__class__.__name__ == "ReachSkill": - if not hasattr(skill, "_compute_goal_from_offset"): - skill._compute_goal_from_offset = types.MethodType( - ReachSkill._compute_goal_from_offset, skill - ) - if not hasattr(skill.cfg.extra_cfg, "corrective_reach"): - skill.cfg.extra_cfg.corrective_reach = False - if not hasattr(skill, "_saved_env_extra_info"): - skill._saved_env_extra_info = None - if not hasattr(skill, "_corrective_reach_done"): - skill._corrective_reach_done = False - return skill - - SkillRegistry.create = _patched_create - SkillRegistry._lw_patched_create = True - - -def patch_curobo_config() -> None: - """Relax cuRobo start-state checks for mixed G1 planner/sim setups.""" - from curobo.wrap.reacher.motion_gen import MotionGenConfig - - if hasattr(MotionGenConfig, "_lw_original_load_from_robot_config"): - return - - MotionGenConfig._lw_original_load_from_robot_config = MotionGenConfig.load_from_robot_config - - @classmethod - def _patched_load_from_robot_config(cls, *args, **kwargs): - # In our current hybrid setup planner robot model and sim articulation - # differ on virtual base joints, which can trigger false-positive - # INVALID_START_STATE_SELF_COLLISION. Relaxing self collision check - # avoids immediate planner rejection at step 0. - kwargs.setdefault("self_collision_check", False) - kwargs.setdefault("self_collision_opt", False) - return cls._lw_original_load_from_robot_config(*args, **kwargs) - - MotionGenConfig.load_from_robot_config = _patched_load_from_robot_config diff --git a/lw_benchhub/autosim/content/configs/robot/g1.yml b/lw_benchhub/autosim/content/configs/robot/g1.yml new file mode 100644 index 00000000..d16fb606 --- /dev/null +++ b/lw_benchhub/autosim/content/configs/robot/g1.yml @@ -0,0 +1,119 @@ +robot_cfg: + + kinematics: + use_usd_kinematics: False + usd_robot_root: "/g1_autosim" + urdf_path: "G1_autosim.urdf" # relative to robot_config_root_path (g1/ dir) + asset_root_path: "." # meshes are resolved from the urdf file dir + + base_link: "world_link" + ee_link: "left_wrist_yaw_link" # left-hand end-effector + # Extra tracked links (right ee for dual-arm tasks) + link_names: ["left_wrist_yaw_link"] + + collision_link_names: + [ + "pelvis", + "torso_link", + "left_shoulder_pitch_link", "left_shoulder_roll_link", "left_shoulder_yaw_link", + "left_elbow_link", + "left_wrist_roll_link", "left_wrist_pitch_link", "left_wrist_yaw_link", + "right_shoulder_pitch_link", "right_shoulder_roll_link", "right_shoulder_yaw_link", + "right_elbow_link", + "right_wrist_roll_link", "right_wrist_pitch_link", "right_wrist_yaw_link", + ] + + collision_spheres: "spheres/collision_g1_autosim.yml" + collision_sphere_buffer: 0.002 + extra_collision_spheres: {"left_wrist_yaw_link": 20} + use_global_cumul: True + + self_collision_ignore: + { + "pelvis": ["torso_link", "left_shoulder_pitch_link", "right_shoulder_pitch_link"], + "torso_link": ["left_shoulder_pitch_link", "right_shoulder_pitch_link"], + "left_shoulder_pitch_link": ["left_shoulder_roll_link"], + "left_shoulder_roll_link": ["left_shoulder_yaw_link"], + "left_shoulder_yaw_link": ["left_elbow_link"], + "left_elbow_link": ["left_wrist_roll_link"], + "left_wrist_roll_link": ["left_wrist_pitch_link"], + "left_wrist_pitch_link": ["left_wrist_yaw_link"], + "right_shoulder_pitch_link": ["right_shoulder_roll_link"], + "right_shoulder_roll_link": ["right_shoulder_yaw_link"], + "right_shoulder_yaw_link": ["right_elbow_link"], + "right_elbow_link": ["right_wrist_roll_link"], + "right_wrist_roll_link": ["right_wrist_pitch_link"], + "right_wrist_pitch_link": ["right_wrist_yaw_link"], + } + + self_collision_buffer: {} + + mesh_link_names: + [ + "pelvis", + "torso_link", + "left_shoulder_pitch_link", "left_shoulder_roll_link", "left_shoulder_yaw_link", + "left_elbow_link", + "left_wrist_roll_link", "left_wrist_pitch_link", "left_wrist_yaw_link", + "right_shoulder_pitch_link", "right_shoulder_roll_link", "right_shoulder_yaw_link", + "right_elbow_link", + "right_wrist_roll_link", "right_wrist_pitch_link", "right_wrist_yaw_link", + ] + + # Joints that are fixed during planning. + # Right arm is locked; left arm is planned. + lock_joints: + { + # Virtual base joints + "base_x_joint": 0.0, + "base_y_joint": 0.0, + "base_yaw_joint": 0.0, + # Waist + "waist_yaw_joint": 0.0, + "waist_roll_joint": 0.0, + "waist_pitch_joint": 0.0, + # Right arm (locked; left arm is planned) + "right_shoulder_pitch_joint": 0.0, + "right_shoulder_roll_joint": -0.3, + "right_shoulder_yaw_joint": 0.0, + "right_elbow_joint": 0.0, + "right_wrist_roll_joint": 0.0, + "right_wrist_pitch_joint": 0.0, + "right_wrist_yaw_joint": 0.0, + } + + extra_links: null + + cspace: + joint_names: + [ + # Left arm (7 DoF) — virtual base joints locked, not in cspace + "left_shoulder_pitch_joint", + "left_shoulder_roll_joint", + "left_shoulder_yaw_joint", + "left_elbow_joint", + "left_wrist_roll_joint", + "left_wrist_pitch_joint", + "left_wrist_yaw_joint", + ] + + retract_config: + [ + 0.0, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0, # left arm + ] + + null_space_weight: + [ + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, + ] + + cspace_distance_weight: + [ + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, + ] + + max_jerk: 500.0 + max_acceleration: 500.0 + +planner: + frame_bias: [0.0, 0.0, 0.0] diff --git a/lw_benchhub/autosim/content/configs/robot/g1_autosim.yml b/lw_benchhub/autosim/content/configs/robot/g1_right_ee.yml similarity index 81% rename from lw_benchhub/autosim/content/configs/robot/g1_autosim.yml rename to lw_benchhub/autosim/content/configs/robot/g1_right_ee.yml index c5f2cf30..3f2fe81e 100644 --- a/lw_benchhub/autosim/content/configs/robot/g1_autosim.yml +++ b/lw_benchhub/autosim/content/configs/robot/g1_right_ee.yml @@ -7,10 +7,10 @@ robot_cfg: asset_root_path: "." # meshes are resolved from the urdf file dir base_link: "world_link" - ee_link: "right_hand_palm_link" # right-hand end-effector + ee_link: "right_wrist_yaw_link" # right-hand end-effector # Extra tracked links (left ee for dual-arm tasks) - link_names: ["right_hand_palm_link", "left_hand_palm_link"] + link_names: ["right_wrist_yaw_link", "left_wrist_yaw_link"] collision_link_names: [ @@ -19,16 +19,14 @@ robot_cfg: "left_shoulder_pitch_link", "left_shoulder_roll_link", "left_shoulder_yaw_link", "left_elbow_link", "left_wrist_roll_link", "left_wrist_pitch_link", "left_wrist_yaw_link", - "left_hand_palm_link", "right_shoulder_pitch_link", "right_shoulder_roll_link", "right_shoulder_yaw_link", "right_elbow_link", "right_wrist_roll_link", "right_wrist_pitch_link", "right_wrist_yaw_link", - "right_hand_palm_link", ] collision_spheres: "spheres/collision_g1_autosim.yml" collision_sphere_buffer: 0.002 - extra_collision_spheres: {"right_hand_palm_link": 20, "left_hand_palm_link": 20} + extra_collision_spheres: {"right_wrist_yaw_link": 20, "left_wrist_yaw_link": 20} use_global_cumul: True self_collision_ignore: @@ -41,14 +39,12 @@ robot_cfg: "left_elbow_link": ["left_wrist_roll_link"], "left_wrist_roll_link": ["left_wrist_pitch_link"], "left_wrist_pitch_link": ["left_wrist_yaw_link"], - "left_wrist_yaw_link": ["left_hand_palm_link"], "right_shoulder_pitch_link": ["right_shoulder_roll_link"], "right_shoulder_roll_link": ["right_shoulder_yaw_link"], "right_shoulder_yaw_link": ["right_elbow_link"], "right_elbow_link": ["right_wrist_roll_link"], "right_wrist_roll_link": ["right_wrist_pitch_link"], "right_wrist_pitch_link": ["right_wrist_yaw_link"], - "right_wrist_yaw_link": ["right_hand_palm_link"], } self_collision_buffer: {} @@ -60,11 +56,9 @@ robot_cfg: "left_shoulder_pitch_link", "left_shoulder_roll_link", "left_shoulder_yaw_link", "left_elbow_link", "left_wrist_roll_link", "left_wrist_pitch_link", "left_wrist_yaw_link", - "left_hand_palm_link", "right_shoulder_pitch_link", "right_shoulder_roll_link", "right_shoulder_yaw_link", "right_elbow_link", "right_wrist_roll_link", "right_wrist_pitch_link", "right_wrist_yaw_link", - "right_hand_palm_link", ] # Joints that are fixed during planning. @@ -74,11 +68,15 @@ robot_cfg: # Finger joints are descendants of the EE palm links and are NOT on the chain → excluded. lock_joints: { - # Waist (on chain: pelvis → waist_yaw → waist_roll → torso) + # Virtual base joints (robot moves via leg loco, not via these joints) + "base_x_joint": 0.0, + "base_y_joint": 0.0, + "base_yaw_joint": 0.0, + # Waist "waist_yaw_joint": 0.0, "waist_roll_joint": 0.0, "waist_pitch_joint": 0.0, - # Left arm (locked; right arm is planned; left_hand_palm_link is a tracked link) + # Left arm (locked; right arm is planned) "left_shoulder_pitch_joint": 0.0, "left_shoulder_roll_joint": 0.3, "left_shoulder_yaw_joint": 0.0, @@ -93,11 +91,7 @@ robot_cfg: cspace: joint_names: [ - # Virtual base joints (navigation in planner state only) - "base_x_joint", - "base_y_joint", - "base_yaw_joint", - # Right arm (7 DoF) + # Right arm (7 DoF) — virtual base joints excluded: world_link≡pelvis at joint=0 "right_shoulder_pitch_joint", "right_shoulder_roll_joint", "right_shoulder_yaw_joint", @@ -109,19 +103,16 @@ robot_cfg: retract_config: [ - 0.0, 0.0, 0.0, # base 0.0, -0.3, 0.0, 0.0, 0.0, 0.0, 0.0, # right arm ] null_space_weight: [ - 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, ] cspace_distance_weight: [ - 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, ] diff --git a/lw_benchhub/autosim/pipelines/close_oven.py b/lw_benchhub/autosim/pipelines/close_oven.py index 6d94a484..d984d166 100644 --- a/lw_benchhub/autosim/pipelines/close_oven.py +++ b/lw_benchhub/autosim/pipelines/close_oven.py @@ -1,19 +1,57 @@ +import torch from autosim.core.pipeline import AutoSimPipeline, AutoSimPipelineCfg +from autosim.core.registration import SkillRegistry +from autosim.core.types import PipelineOutput +from autosim.decomposers import LLMDecomposerCfg from isaaclab.envs import ManagerBasedEnv from isaaclab.utils import configclass -import torch - -from autosim.decomposers import LLMDecomposerCfg - -from ..prompt_utils import render_additional_prompt -from ..robot_profiles import ( +from lw_benchhub.autosim.prompt_utils import render_additional_prompt +from lw_benchhub.autosim.robot_profiles import ( TaskRobotOverride, + apply_robot_env_cfg, build_env_extra_info, configure_robot_runtime_settings, resolve_robot_settings, ) + +def _x7s_skill_cfg(cfg) -> None: + cfg.skills.moveto.extra_cfg.local_planner.max_linear_velocity = 0.1 + cfg.skills.moveto.extra_cfg.local_planner.max_angular_velocity = 0.4 + cfg.skills.moveto.extra_cfg.local_planner.predict_time = 0.4 + cfg.skills.moveto.extra_cfg.global_planner.safety_distance = 0.8 + cfg.skills.moveto.extra_cfg.global_planner.proximity_weight = 3.0 + cfg.skills.moveto.extra_cfg.waypoint_tolerance = 0.2 + cfg.skills.moveto.extra_cfg.goal_tolerance = 0.1 + cfg.skills.moveto.extra_cfg.yaw_tolerance = 0.008 + cfg.skills.moveto.extra_cfg.uws_dwa = False + cfg.skills.moveto.extra_cfg.sampling_radius = 1.6 + cfg.skills.push.extra_cfg.move_offset = 0.36 + cfg.skills.push.extra_cfg.move_axis = "+x" + cfg.skills.lift.extra_cfg.move_offset = 0.15 + cfg.skills.lift.extra_cfg.move_axis = "+z" + cfg.max_steps = 1000 + + +def _g1_skill_cfg(cfg) -> None: + cfg.skills.moveto.extra_cfg.local_planner.max_linear_velocity = 0.2 + cfg.skills.moveto.extra_cfg.local_planner.max_angular_velocity = 0.2 + cfg.skills.moveto.extra_cfg.local_planner.predict_time = 0.4 + cfg.skills.moveto.extra_cfg.global_planner.safety_distance = 0.5 + cfg.skills.moveto.extra_cfg.global_planner.proximity_weight = 3.0 + cfg.skills.moveto.extra_cfg.waypoint_tolerance = 0.25 + cfg.skills.moveto.extra_cfg.goal_tolerance = 0.30 + cfg.skills.moveto.extra_cfg.yaw_tolerance = 0.01 + cfg.skills.moveto.extra_cfg.use_dwa = False + cfg.skills.moveto.extra_cfg.sampling_radius = 0.87 + cfg.skills.push.extra_cfg.move_offset = 0.10 + cfg.skills.push.extra_cfg.move_axis = "+x" + cfg.skills.lift.extra_cfg.move_offset = 0.15 + cfg.skills.lift.extra_cfg.move_axis = "+z" + cfg.max_steps = 2000 + + TASK_ROBOT_OVERRIDES: dict[str, TaskRobotOverride] = { "x7s_joint_left": TaskRobotOverride( extra_target_link_names=("link20_tip",), @@ -23,6 +61,18 @@ torch.tensor([-0.176, -0.739, -0.180, 0.707, -0.00, -0.00, 0.707]), ], }, + init_state_pos_delta=(-0.6, -1.2, 0.0), + skill_cfg_fn=_x7s_skill_cfg, + ), + "g1_loco_left": TaskRobotOverride( + object_reach_target_poses={ + "oven_main_group": [ + torch.tensor([-0.1687, -0.9214, -0.0407, 0.3762, 0.0, 0.0, 0.9264]), + ], + }, + init_state_pos_delta=(-0.21, -0.70, 0.01), + init_state_rot=(0.707, 0.0, 0.0, 0.707), + skill_cfg_fn=_g1_skill_cfg, ), } @@ -31,49 +81,27 @@ def get_task_robot_override(robot_profile: str) -> TaskRobotOverride: try: return TASK_ROBOT_OVERRIDES[robot_profile] except KeyError as exc: - supported = ", ".join(tuple(TASK_ROBOT_OVERRIDES)) + supported = ", ".join(TASK_ROBOT_OVERRIDES) raise ValueError( - f"CloseOvenPipeline does not support robot profile '{robot_profile}'. Supported profiles: {supported}" + f"CloseOvenPipeline does not support robot profile '{robot_profile}'. Supported: {supported}" ) from exc @configclass class CloseOvenPipelineCfg(AutoSimPipelineCfg): - """Configuration for the CloseOvenPipeline.""" - robot_profile: str = "x7s_joint_left" - decomposer: LLMDecomposerCfg = LLMDecomposerCfg() def __post_init__(self): - resolved_robot = resolve_robot_settings( - self.robot_profile, - override=get_task_robot_override(self.robot_profile), - ) + resolved_robot = resolve_robot_settings(self.robot_profile, override=get_task_robot_override(self.robot_profile)) configure_robot_runtime_settings(self, resolved_robot) - self.skills.moveto.extra_cfg.local_planner.max_linear_velocity = 0.1 - self.skills.moveto.extra_cfg.local_planner.max_angular_velocity = 0.4 - self.skills.moveto.extra_cfg.local_planner.predict_time = 0.4 - self.skills.moveto.extra_cfg.global_planner.safety_distance = 0.8 - self.skills.moveto.extra_cfg.global_planner.proximity_weight = 3.0 - self.skills.moveto.extra_cfg.waypoint_tolerance = 0.2 - self.skills.moveto.extra_cfg.goal_tolerance = 0.1 - self.skills.moveto.extra_cfg.yaw_tolerance = 0.008 - self.skills.moveto.extra_cfg.uws_dwa = False - self.max_steps = 1000 - - self.skills.moveto.extra_cfg.sampling_radius = 1.6 - - self.skills.push.extra_cfg.move_offset = 0.36 - self.skills.push.extra_cfg.move_axis = "+x" - - self.skills.lift.extra_cfg.move_offset = 0.15 - self.skills.lift.extra_cfg.move_axis = "+z" + if resolved_robot.override.skill_cfg_fn: + resolved_robot.override.skill_cfg_fn(self) self.occupancy_map.floor_prim_suffix = "Scene/floor_room" self.motion_planner.world_ignore_subffixes = ["Scene/floor_room"] - self.motion_planner.world_only_subffixes = [ + self.motion_planner.world_only_subffixes = [ "Scene/island_island_group", "Scene/island_panel_cab_right_island_group_1", "Scene/counter_main_main_group", @@ -81,21 +109,65 @@ def __post_init__(self): ] +_DOOR_PROFILES = {"g1_loco_left"} +_DOOR_JOINT_PATH = "/World/envs/env_0/Scene/oven_main_group/Oven032_door/door_joint" + + class CloseOvenPipeline(AutoSimPipeline): def __init__(self, cfg: AutoSimPipelineCfg): - super().__init__(cfg) - robot_profile = cfg.robot_profile self._resolved_robot = resolve_robot_settings( - robot_profile, - override=get_task_robot_override(robot_profile), + cfg.robot_profile, override=get_task_robot_override(cfg.robot_profile) ) + super().__init__(cfg) + + def _set_door_drive(self, stiffness: float, damping: float, target_deg: float) -> None: + try: + import omni.usd + from pxr import UsdPhysics + stage = omni.usd.get_context().get_stage() + prim = stage.GetPrimAtPath(_DOOR_JOINT_PATH) + if prim.IsValid() and (drive := UsdPhysics.DriveAPI.Get(prim, "angular")): + drive.GetStiffnessAttr().Set(stiffness) + drive.GetDampingAttr().Set(damping) + if not (attr := drive.GetTargetPositionAttr()).IsValid(): + attr = drive.CreateTargetPositionAttr() + attr.Set(target_deg) + except Exception as e: + print(f"[CloseOven] door drive set failed: {e}") def reset_env(self): super().reset_env() + if self._resolved_robot.profile.profile_id in _DOOR_PROFILES: + self._env.cfg.isaaclab_arena_env.task._setup_scene(self._env) + self._set_door_drive(stiffness=0.0, damping=1.0, target_deg=0.0) + + def execute_skill_sequence(self, decompose_result): + if self._resolved_robot.profile.profile_id not in _DOOR_PROFILES: + return super().execute_skill_sequence(decompose_result) + + self._check_skill_extra_cfg() + self.reset_env() + + for subtask in decompose_result.subtasks: + for skill_info in subtask.skills: + if skill_info.skill_type == "push": + self._set_door_drive(stiffness=50.0, damping=5.0, target_deg=0.0) + skill = SkillRegistry.create( + skill_info.skill_type, self.cfg.skills.get(skill_info.skill_type).extra_cfg + ) + if self._action_adapter.should_skip_apply(skill): + continue + goal = skill.extract_goal_from_info(skill_info, self._env, self._env_extra_info) + success, steps = self._execute_single_skill(skill, goal) + if not success: + raise ValueError(f"Skill {skill_info.skill_type} failed after {steps} steps.") + + self.reset_env() + return PipelineOutput(success=True, generated_actions=self._generated_actions) def load_env(self) -> ManagerBasedEnv: import gymnasium as gym - from lw_benchhub.utils.env import ExecuteMode, parse_env_cfg + from lw_benchhub.utils.env import parse_env_cfg, ExecuteMode env_cfg = parse_env_cfg( scene_backend="robocasa", @@ -120,11 +192,9 @@ def load_env(self) -> ManagerBasedEnv: resample_robot_placement_on_reset=False, ) + apply_robot_env_cfg(env_cfg, self._resolved_robot) env_cfg.terminations.time_out = None - env_cfg.scene.robot.init_state.pos[0] -= 0.6 - env_cfg.scene.robot.init_state.pos[1] -= 1.2 - env_id = f"Robocasa-CloseOven-{self._resolved_robot.profile.robot_name}-v0" gym.register( id=env_id, @@ -135,6 +205,10 @@ def load_env(self) -> ManagerBasedEnv: env = gym.make(env_id, cfg=env_cfg).unwrapped + if self._resolved_robot.profile.profile_id in _DOOR_PROFILES: + env.cfg.isaaclab_arena_env.task._setup_scene(env) + self._set_door_drive(stiffness=0.0, damping=1.0, target_deg=0.0) + return env def get_env_extra_info(self): @@ -142,7 +216,7 @@ def get_env_extra_info(self): task_name="Robocasa-Task-CloseOven", objects=["oven_main_group"], additional_prompt_contents=( - f"{render_additional_prompt()}\n\n When you close the oven, you should lift and then push the oven door to close it." + f"{render_additional_prompt()}\n\nWhen you close the oven, you should lift and then push the oven door to close it." ), resolved_robot=self._resolved_robot, ) diff --git a/lw_benchhub/autosim/pipelines/g1_open_microwave.py b/lw_benchhub/autosim/pipelines/g1_open_microwave.py deleted file mode 100644 index a7cb1b24..00000000 --- a/lw_benchhub/autosim/pipelines/g1_open_microwave.py +++ /dev/null @@ -1,156 +0,0 @@ -"""G1 autosim pipeline — open microwave door. - -Robot: G1 loco controller variant (leg locomotion + dual arms + hands) -Task: Open microwave door -Scene: RoboCasa kitchen robocasakitchen-2-2 -Planner: cuRobo right-arm planning (g1_autosim.yml) -Nav: A* + leg locomotion controller -""" - -from pathlib import Path - -import torch -from autosim.core.pipeline import AutoSimPipeline, AutoSimPipelineCfg -from autosim.core.types import EnvExtraInfo -from autosim.decomposers import LLMDecomposerCfg -from isaaclab.envs import ManagerBasedEnv -from isaaclab.utils import configclass - -from lw_benchhub.autosim.action_adapters.g1_action_adapter_cfg import G1ActionAdapterCfg - -_CUROBO_ROOT = Path(__file__).parent.parent / "content" -_G1_URDF_DIR = Path(__file__).parent.parent / "content/assets/robot/g1" - - -@configclass -class G1OpenMicrowavePipelineCfg(AutoSimPipelineCfg): - """Pipeline configuration for G1 RoboCasa kitchen microwave opening.""" - - decomposer: LLMDecomposerCfg = LLMDecomposerCfg() - action_adapter: G1ActionAdapterCfg = G1ActionAdapterCfg() - - def __post_init__(self): - # ---- Navigation ---- - self.skills.moveto.extra_cfg.local_planner.max_linear_velocity = 0.25 - self.skills.moveto.extra_cfg.local_planner.max_angular_velocity = 1.0 - self.skills.moveto.extra_cfg.local_planner.predict_time = 0.4 - self.skills.moveto.extra_cfg.global_planner.safety_distance = 0.5 - self.skills.moveto.extra_cfg.global_planner.proximity_weight = 3.0 - self.skills.moveto.extra_cfg.waypoint_tolerance = 0.25 - self.skills.moveto.extra_cfg.goal_tolerance = 0.30 - self.skills.moveto.extra_cfg.yaw_tolerance = 0.01 - self.skills.moveto.extra_cfg.use_dwa = False - self.skills.moveto.extra_cfg.sampling_radius = 0.8 - - # ---- Occupancy map (RoboCasa kitchen floor) ---- - self.occupancy_map.floor_prim_suffix = "Scene/floor_room" - - # ---- cuRobo ---- - self.motion_planner.robot_config_file = "g1_autosim.yml" - self.motion_planner.curobo_config_path = str(_CUROBO_ROOT / "configs" / "robot") - self.motion_planner.curobo_asset_path = str(_G1_URDF_DIR) - self.motion_planner.world_ignore_subffixes = ["Scene/floor_room"] - self.motion_planner.world_only_subffixes = ["Scene/microwave_main_group"] - - # ---- Pull skill (open microwave door by pulling) ---- - self.skills.pull.extra_cfg.move_offset = 0.25 - self.skills.pull.extra_cfg.move_axis = "-x" - - self.max_steps = 1000 - - -class G1OpenMicrowavePipeline(AutoSimPipeline): - """Pipeline that opens a microwave door with the G1 right arm.""" - - _TASK_NAME = "Robocasa-OpenMicrowave-G1-Autosim-v0" - - def __init__(self, cfg: AutoSimPipelineCfg): - from lw_benchhub.autosim.compat_autosim import ( - patch_curobo_config, - patch_env_extra_info, - patch_reach_skill, - ) - patch_env_extra_info() - patch_reach_skill() - patch_curobo_config() - super().__init__(cfg) - - # ------------------------------------------------------------------ - # Environment loading - # ------------------------------------------------------------------ - - def load_env(self) -> ManagerBasedEnv: - import gymnasium as gym - from lw_benchhub.utils.env import parse_env_cfg, ExecuteMode - from lw_benchhub.autosim.isaaclab_tasks.g1_autosim_cfg import ( - G1ActionsCfg, - G1ObservationsCfg, - G1EventCfg, - ) - - env_cfg = parse_env_cfg( - scene_backend="robocasa", - task_backend="robocasa", - task_name="OpenMicrowave", - robot_name="G1-Loco-Controller", - scene_name="robocasakitchen-2-2", - robot_scale=1.0, - device="cpu", - num_envs=1, - use_fabric=False, - first_person_view=False, - enable_cameras=False, - execute_mode=ExecuteMode.TRAIN, - usd_simplify=False, - seed=42, - sources=["objaverse", "lightwheel", "aigen_objs"], - object_projects=[], - rl_name=None, - headless_mode=False, - ) - - env_cfg.actions = G1ActionsCfg() - env_cfg.observations = G1ObservationsCfg() - env_cfg.events = G1EventCfg() - - # Disable built-in timeout — autosim manages episode endings. - env_cfg.terminations.time_out = None - - gym.register( - id=self._TASK_NAME, - entry_point="isaaclab.envs:ManagerBasedRLEnv", - kwargs={}, - disable_env_checker=True, - ) - - env = gym.make(self._TASK_NAME, cfg=env_cfg).unwrapped - - # env_cfg.events was replaced with an empty G1EventCfg to prevent - # unwanted autosim resets, which removed the startup init_task event - # that normally calls task.init_fixtures(env) → fixture.setup_env(env). - # Call it explicitly so fixture controllers (e.g. Microwave) have - # self._env set before update_state() is triggered. - arena_env = env.cfg.isaaclab_arena_env - arena_env.task.init_fixtures(env) - - return env - - # ------------------------------------------------------------------ - # Task metadata for the LLM decomposer - # ------------------------------------------------------------------ - - def get_env_extra_info(self) -> EnvExtraInfo: - env_info = EnvExtraInfo( - task_name="Robocasa-Task-OpenMicrowave", - objects=list(self._env.scene.keys()), - robot_name="robot", - robot_base_link_name="pelvis", - ee_link_name="right_wrist_yaw_link", - object_reach_target_poses={ - "microwave_main_group": [ - torch.tensor([0.05, -0.22, 0.12, 0.707, 0.0, 0.0, 0.707]), - ], - }, - ) - env_info.object_extra_reach_target_poses = {} - return env_info diff --git a/lw_benchhub/autosim/pipelines/kettle_boiling.py b/lw_benchhub/autosim/pipelines/kettle_boiling.py index d0b68b2b..2027a0b3 100644 --- a/lw_benchhub/autosim/pipelines/kettle_boiling.py +++ b/lw_benchhub/autosim/pipelines/kettle_boiling.py @@ -1,20 +1,107 @@ +import numpy as np +import torch from autosim.core.pipeline import AutoSimPipeline, AutoSimPipelineCfg +from autosim.decomposers import LLMDecomposerCfg from isaaclab.envs import ManagerBasedEnv from isaaclab.utils import configclass -import torch -import numpy as np - -from autosim.decomposers import LLMDecomposerCfg - -from ..prompt_utils import render_additional_prompt -from ..robot_profiles import ( +from lw_benchhub.autosim.prompt_utils import render_additional_prompt +from lw_benchhub.autosim.robot_profiles import ( TaskRobotOverride, + apply_robot_env_cfg, build_env_extra_info, configure_robot_runtime_settings, resolve_robot_settings, ) + +def _x7s_skill_cfg(cfg) -> None: + cfg.skills.moveto.extra_cfg.local_planner.max_linear_velocity = 0.4 + cfg.skills.moveto.extra_cfg.local_planner.max_angular_velocity = 0.3 + cfg.skills.moveto.extra_cfg.local_planner.predict_time = 0.4 + cfg.skills.moveto.extra_cfg.global_planner.safety_distance = 1.0 + cfg.skills.moveto.extra_cfg.global_planner.proximity_weight = 3.0 + cfg.skills.moveto.extra_cfg.waypoint_tolerance = 0.2 + cfg.skills.moveto.extra_cfg.goal_tolerance = 0.07 + cfg.skills.moveto.extra_cfg.yaw_tolerance = 0.01 + cfg.skills.moveto.extra_cfg.uws_dwa = False + + +def _g1_skill_cfg(cfg) -> None: + cfg.skills.moveto.extra_cfg.local_planner.max_linear_velocity = 0.2 + cfg.skills.moveto.extra_cfg.local_planner.max_angular_velocity = 0.2 + cfg.skills.moveto.extra_cfg.local_planner.predict_time = 0.4 + cfg.skills.moveto.extra_cfg.global_planner.safety_distance = 0.5 + cfg.skills.moveto.extra_cfg.global_planner.proximity_weight = 3.0 + cfg.skills.moveto.extra_cfg.waypoint_tolerance = 0.25 + cfg.skills.moveto.extra_cfg.goal_tolerance = 0.30 + cfg.skills.moveto.extra_cfg.yaw_tolerance = 0.01 + cfg.skills.moveto.extra_cfg.use_dwa = False + cfg.skills.moveto.extra_cfg.per_object_sampling_radius = {"obj": 0.5, "stovetop_main_group": 0.30} + + +def _x7s_get_obj_cfgs(self): + return [dict( + name="obj", + obj_groups=("kettle_non_electric"), + asset_name="Kettle073.usd", + graspable=True, + placement=dict( + fixture=self.counter, + sample_region_kwargs={}, + size=(0.4, 0.28), + pos=(0.0, -1.0), + rotation=(7 / 8 * np.pi, np.pi), + ), + )] + + +def _g1_get_obj_cfgs(self): + return [ + dict( + name="obj", + obj_groups=("kettle_non_electric"), + asset_name="Kettle073.usd", + graspable=True, + placement=dict( + fixture=self.counter, + sample_region_kwargs=dict(ref=self.stove), + size=(0.35, 0.35), + pos=("ref", -1), + ), + ), + dict( + name="stove_distr", + obj_groups=("pan"), + asset_name="Pan023.usd", + placement=dict( + fixture=self.stove, + ensure_object_boundary_in_range=False, + ), + ), + ] + + +def _g1_reset_env(pipeline) -> None: + obj = pipeline._env.scene["obj"] + obj.write_root_pose_to_sim( + torch.tensor([[2.0, -0.56, 1.1, 0.0, 0.0, 0.0, 1.0]], device=pipeline._env.device) + ) + obj.reset() + + stove_distr = pipeline._env.scene["stove_distr"] + pose = stove_distr.data.root_pose_w.clone() + pose[:, 3:] = torch.tensor([0.707, 0.0, 0.0, 0.707], device=pipeline._env.device) + pose[:, 0] += 0.3 + pose[:, 1] += 0.2 + stove_distr.write_root_pose_to_sim(pose) + stove_distr.reset() + + +def _g1_after_env_created(pipeline, env) -> None: + env.cfg.isaaclab_arena_env.task.init_fixtures(env) + + TASK_ROBOT_OVERRIDES: dict[str, TaskRobotOverride] = { "x7s_joint_left": TaskRobotOverride( extra_target_link_names=("link20_tip",), @@ -27,6 +114,24 @@ torch.tensor([-0.0, -0.045, 0.24, 0.707, 0.0, 0.0, 0.707]), ], }, + init_state_pos_delta=(0.0, -0.8, 0.01), + skill_cfg_fn=_x7s_skill_cfg, + get_obj_cfgs_fn=_x7s_get_obj_cfgs, + ), + "g1_loco_left": TaskRobotOverride( + object_reach_target_poses={ + "obj": [ + torch.tensor([0.0, 0.18, 0.06, 0.866, 0.0, 0.0, -0.5]), + ], + "stovetop_main_group": [ + torch.tensor([0.0, -0.15, 0.20, 1.0, 0.0, 0.0, 0.0]), + ], + }, + init_state_pos_delta=(0.0, -0.8, 0.01), + skill_cfg_fn=_g1_skill_cfg, + get_obj_cfgs_fn=_g1_get_obj_cfgs, + reset_env_fn=_g1_reset_env, + after_env_created_fn=_g1_after_env_created, ), } @@ -35,47 +140,33 @@ def get_task_robot_override(robot_profile: str) -> TaskRobotOverride: try: return TASK_ROBOT_OVERRIDES[robot_profile] except KeyError as exc: - supported = ", ".join(tuple(TASK_ROBOT_OVERRIDES)) + supported = ", ".join(TASK_ROBOT_OVERRIDES) raise ValueError( - f"KettleBoilingPipeline does not support robot profile '{robot_profile}'. Supported profiles: {supported}" + f"KettleBoilingPipeline does not support robot profile '{robot_profile}'. Supported: {supported}" ) from exc @configclass class KettleBoilingPipelineCfg(AutoSimPipelineCfg): - """Configuration for the KettleBoilingPipeline.""" - robot_profile: str = "x7s_joint_left" - decomposer: LLMDecomposerCfg = LLMDecomposerCfg() def __post_init__(self): - resolved_robot = resolve_robot_settings( - self.robot_profile, - override=get_task_robot_override(self.robot_profile), - ) + resolved_robot = resolve_robot_settings(self.robot_profile, override=get_task_robot_override(self.robot_profile)) configure_robot_runtime_settings(self, resolved_robot) - self.skills.moveto.extra_cfg.local_planner.max_linear_velocity = 0.4 - self.skills.moveto.extra_cfg.local_planner.max_angular_velocity = 0.3 - self.skills.moveto.extra_cfg.local_planner.predict_time = 0.4 - self.skills.moveto.extra_cfg.global_planner.safety_distance = 1.0 - self.skills.moveto.extra_cfg.global_planner.proximity_weight = 3.0 - self.skills.moveto.extra_cfg.waypoint_tolerance = 0.2 - self.skills.moveto.extra_cfg.goal_tolerance = 0.07 - self.skills.moveto.extra_cfg.yaw_tolerance = 0.01 - self.skills.moveto.extra_cfg.uws_dwa = False + if resolved_robot.override.skill_cfg_fn: + resolved_robot.override.skill_cfg_fn(self) self.skills.lift.extra_cfg.move_offset = 0.15 - self.skills.lift.extra_cfg.move_axis = "+z" + self.skills.lift.extra_cfg.move_axis = "+z" self.motion_planner.enable_dynamic_world_sync = True - self.occupancy_map.floor_prim_suffix = "Scene/floor_room" self.max_steps = 800 self.motion_planner.world_ignore_subffixes = ["Scene/floor_room"] - self.motion_planner.world_only_subffixes = [ + self.motion_planner.world_only_subffixes = [ "Scene/obj", "Scene/stovetop_main_group", "Scene/counter_main_main_group", @@ -85,19 +176,23 @@ def __post_init__(self): class KettleBoilingPipeline(AutoSimPipeline): def __init__(self, cfg: AutoSimPipelineCfg): - super().__init__(cfg) - robot_profile = cfg.robot_profile self._resolved_robot = resolve_robot_settings( - robot_profile, - override=get_task_robot_override(robot_profile), + cfg.robot_profile, override=get_task_robot_override(cfg.robot_profile) ) + super().__init__(cfg) + + def reset_env(self): + super().reset_env() + if self._resolved_robot.override.reset_env_fn: + self._resolved_robot.override.reset_env_fn(self) def load_env(self) -> ManagerBasedEnv: import gymnasium as gym import lw_benchhub_tasks.lightwheel_robocasa_tasks.multi_stage.brewing.kettle_boiling as kb - from lw_benchhub.utils.env import ExecuteMode, parse_env_cfg + from lw_benchhub.utils.env import parse_env_cfg, ExecuteMode - kb.KettleBoiling._get_obj_cfgs = patch_get_obj_cfgs + if self._resolved_robot.override.get_obj_cfgs_fn: + kb.KettleBoiling._get_obj_cfgs = self._resolved_robot.override.get_obj_cfgs_fn env_cfg = parse_env_cfg( scene_backend="robocasa", @@ -122,11 +217,9 @@ def load_env(self) -> ManagerBasedEnv: resample_robot_placement_on_reset=False, ) + apply_robot_env_cfg(env_cfg, self._resolved_robot) env_cfg.terminations.time_out = None - env_cfg.scene.robot.init_state.pos[1] -= 0.8 - env_cfg.scene.robot.init_state.pos[2] += 0.01 - env_id = f"Robocasa-KettleBoiling-{self._resolved_robot.profile.robot_name}-v0" gym.register( id=env_id, @@ -137,6 +230,9 @@ def load_env(self) -> ManagerBasedEnv: env = gym.make(env_id, cfg=env_cfg, render_mode="rgb_array").unwrapped + if self._resolved_robot.override.after_env_created_fn: + self._resolved_robot.override.after_env_created_fn(self, env) + return env def get_env_extra_info(self): @@ -146,24 +242,3 @@ def get_env_extra_info(self): additional_prompt_contents=f"{render_additional_prompt()}\n\n You don't need to turn on burner.", resolved_robot=self._resolved_robot, ) - - -def patch_get_obj_cfgs(self): - cfgs = [] - cfgs.append( - dict( - name="obj", - obj_groups=("kettle_non_electric"), - asset_name="Kettle073.usd", - graspable=True, - placement=dict( - fixture=self.counter, - sample_region_kwargs={}, - size=(0.4, 0.28), - pos=(0.0, -1.0), - rotation=(7 / 8 * np.pi, np.pi), - ), - ) - ) - - return cfgs diff --git a/lw_benchhub/autosim/robot_profiles.py b/lw_benchhub/autosim/robot_profiles.py index 775ec49b..99cdd943 100644 --- a/lw_benchhub/autosim/robot_profiles.py +++ b/lw_benchhub/autosim/robot_profiles.py @@ -9,7 +9,6 @@ from .action_adapters.x7s_action_adapter_cfg import X7SActionAdapterCfg -# Shared autosim content roots used by all robot-specific motion-planner configs. AUTOSIM_CONTENT_ROOT = Path(__file__).parent / "content" @@ -18,17 +17,14 @@ class RobotProfile: """Robot-only settings reused across multiple task pipelines.""" profile_id: str - """Stable autosim-side identifier used to select this robot profile.""" robot_name: str - """LW-BenchHub robot registry name forwarded into ``parse_env_cfg``.""" action_adapter_factory: Callable[[], object] - """Factory that creates the autosim action-adapter config for this robot.""" motion_planner_robot_config_file: str - """Robot-specific cuRobo / planner config file name under autosim content.""" robot_base_link_name: str - """Base link name exposed to autosim through ``EnvExtraInfo``.""" ee_link_name: str - """Preferred end-effector link name exposed to autosim through ``EnvExtraInfo``.""" + curobo_asset_path: str | None = None + self_collision_check: bool = True + env_cfg_setup_fn: Callable | None = None @dataclass @@ -36,11 +32,14 @@ class TaskRobotOverride: """Per-task-per-robot values that depend on both the task and the robot choice.""" object_reach_target_poses: dict[str, list[torch.Tensor]] | None = None - """Concrete reach target poses owned by this task-robot combination.""" extra_target_link_names: tuple[str, ...] | None = None - """Robot tip links used as extra reach targets for this task-robot combination.""" reach_extra_target_mode: str | None = None - """Reach target mode for the selected robot and task combination.""" + init_state_pos_delta: tuple[float, float, float] | None = None + init_state_rot: tuple[float, float, float, float] | None = None + skill_cfg_fn: Callable | None = None + get_obj_cfgs_fn: Callable | None = None + after_env_created_fn: Callable | None = None + reset_env_fn: Callable | None = None @dataclass @@ -48,9 +47,7 @@ class ResolvedRobotSettings: """Final robot settings after merging the shared profile and task-specific override.""" profile: RobotProfile - """Shared robot profile selected by the pipeline.""" override: TaskRobotOverride - """Task-local robot overrides merged on top of the shared profile.""" @property def robot_base_link_name(self) -> str: @@ -65,6 +62,20 @@ def motion_planner_robot_config_file(self) -> str: return self.profile.motion_planner_robot_config_file +def _setup_g1_env_cfg(env_cfg) -> None: + from lw_benchhub.autosim.isaaclab_tasks.g1_autosim_cfg import ( + G1ActionsCfg, G1ObservationsCfg, G1EventCfg, + ) + env_cfg.actions = G1ActionsCfg() + env_cfg.observations = G1ObservationsCfg() + env_cfg.events = G1EventCfg() + + +def _make_g1_action_adapter(): + from lw_benchhub.autosim.action_adapters.g1_action_adapter_cfg import G1ActionAdapterCfg + return G1ActionAdapterCfg() + + ROBOT_PROFILES: dict[str, RobotProfile] = { "x7s_joint_left": RobotProfile( profile_id="x7s_joint_left", @@ -82,66 +93,77 @@ def motion_planner_robot_config_file(self) -> str: robot_base_link_name="base_link", ee_link_name="right_hand_link", ), + "g1_loco_left": RobotProfile( + profile_id="g1_loco_left", + robot_name="G1-Loco-Controller", + action_adapter_factory=_make_g1_action_adapter, + motion_planner_robot_config_file="g1.yml", + robot_base_link_name="pelvis", + ee_link_name="left_wrist_yaw_link", + curobo_asset_path=str(AUTOSIM_CONTENT_ROOT / "assets" / "robot" / "g1"), + self_collision_check=False, + env_cfg_setup_fn=_setup_g1_env_cfg, + ), } -# Robot profile selection must always be explicit at the pipeline level. - def get_robot_profile(profile_id: str) -> RobotProfile: - """Resolve a registered autosim robot profile by id.""" - try: return ROBOT_PROFILES[profile_id] except KeyError as exc: available = ", ".join(sorted(ROBOT_PROFILES)) - raise KeyError(f"Unknown autosim robot profile '{profile_id}'. Available: {available}") from exc + raise KeyError(f"Unknown robot profile '{profile_id}'. Available: {available}") from exc def resolve_robot_settings(profile_id: str, override: TaskRobotOverride | None = None) -> ResolvedRobotSettings: - """Merge the shared robot profile with optional task-specific overrides.""" - profile = get_robot_profile(profile_id) return ResolvedRobotSettings(profile=profile, override=override if override is not None else TaskRobotOverride()) def configure_robot_runtime_settings(pipeline_cfg, resolved_robot: ResolvedRobotSettings) -> None: """Fill shared robot-derived runtime settings for adapter, planning, and reach behavior.""" - - # Action adapter pipeline_cfg.action_adapter = resolved_robot.profile.action_adapter_factory() - - # Motion planner pipeline_cfg.motion_planner.robot_config_file = resolved_robot.motion_planner_robot_config_file - pipeline_cfg.motion_planner.curobo_asset_path = str(AUTOSIM_CONTENT_ROOT / "assets") + pipeline_cfg.motion_planner.curobo_asset_path = ( + resolved_robot.profile.curobo_asset_path or str(AUTOSIM_CONTENT_ROOT / "assets") + ) pipeline_cfg.motion_planner.curobo_config_path = str(AUTOSIM_CONTENT_ROOT / "configs" / "robot") + if not resolved_robot.profile.self_collision_check: + pipeline_cfg.motion_planner.self_collision_check = False + pipeline_cfg.motion_planner.self_collision_opt = False - # Reach behavior if resolved_robot.override.extra_target_link_names: pipeline_cfg.skills.reach.extra_cfg.extra_target_link_names = list(resolved_robot.override.extra_target_link_names) if resolved_robot.override.reach_extra_target_mode is not None: pipeline_cfg.skills.reach.extra_cfg.extra_target_mode = resolved_robot.override.reach_extra_target_mode +def apply_robot_env_cfg(env_cfg, resolved_robot: ResolvedRobotSettings) -> None: + """Apply robot-specific env_cfg modifications (actions/observations/events + init pose).""" + if resolved_robot.profile.env_cfg_setup_fn: + resolved_robot.profile.env_cfg_setup_fn(env_cfg) + override = resolved_robot.override + if override.init_state_pos_delta is not None: + dx, dy, dz = override.init_state_pos_delta + env_cfg.scene.robot.init_state.pos[0] += dx + env_cfg.scene.robot.init_state.pos[1] += dy + env_cfg.scene.robot.init_state.pos[2] += dz + if override.init_state_rot is not None: + env_cfg.scene.robot.init_state.rot = override.init_state_rot + + def build_env_extra_info( *, task_name: str, objects, additional_prompt_contents: str, resolved_robot: ResolvedRobotSettings, - object_navigate_sample_range: dict[str, tuple[float, float]] = {}, + object_navigate_sample_range: dict[str, tuple[float, float]] | None = None, robot_name: str = "robot", ) -> EnvExtraInfo: """Build autosim-facing metadata by combining task-level info with task-robot overrides.""" - - reach_target_poses = {} - if resolved_robot.override.object_reach_target_poses: - reach_target_poses = { - **reach_target_poses, - **resolved_robot.override.object_reach_target_poses, - } - else: + if not resolved_robot.override.object_reach_target_poses: raise ValueError("No object reach target poses provided for the task-robot combination.") - return EnvExtraInfo( task_name=task_name, objects=objects, @@ -149,6 +171,6 @@ def build_env_extra_info( robot_name=robot_name, robot_base_link_name=resolved_robot.robot_base_link_name, ee_link_name=resolved_robot.ee_link_name, - object_reach_target_poses=reach_target_poses, - object_navigate_sample_range=object_navigate_sample_range, + object_reach_target_poses=resolved_robot.override.object_reach_target_poses, + object_navigate_sample_range=object_navigate_sample_range or {}, ) diff --git a/lw_benchhub/core/mdp/configs/g1_squat.yaml b/lw_benchhub/core/mdp/configs/g1_squat.yaml index 9dc689f7..d0840f03 100644 --- a/lw_benchhub/core/mdp/configs/g1_squat.yaml +++ b/lw_benchhub/core/mdp/configs/g1_squat.yaml @@ -45,8 +45,8 @@ default_angles: [ # change with urdf # -0.092, 0.0354, 0.000,0.511,-0.438,0.038, # -0.092, 0.0354, 0.000,0.511,-0.438,0.038, 0, 0, 0, - 0, 0.3, 0, 1, 0, 0, 0, - 0, -0.3, 0, 1, 0, 0, 0 + 0, 0.1, 0, 0, 0, 0, 0, + 0, -0.1, 0, 0, 0, 0, 0 # -1.5,0.5,-2.6,0.2,-1.2,0.15,1.2, # -1.5,-0.5,2.6,0.2,1.2,-0.15,-1.2 ] diff --git a/lw_benchhub/scripts/autosim/reach_plan_sweep.py b/lw_benchhub/scripts/autosim/reach_plan_sweep.py index ecc78825..daa02a3b 100644 --- a/lw_benchhub/scripts/autosim/reach_plan_sweep.py +++ b/lw_benchhub/scripts/autosim/reach_plan_sweep.py @@ -44,6 +44,10 @@ def main() -> None: pipeline = make_pipeline(args_cli.pipeline_id) + # Batch solving (both ik_only and full motion planning) uses a different goal type + # than the warmup, which triggers a CUDA graph reset error on CUDA < 12.0. + # Disabling CUDA graph avoids the conflict entirely. + pipeline.cfg.motion_planner.use_cuda_graph = False reach_plan_sweep( pipeline, ReachPlanSweepCfg( From eb14deb18ebd46dc3e1fe2567f28bee4b6275e02 Mon Sep 17 00:00:00 2001 From: Zihan Gao Date: Tue, 21 Apr 2026 19:02:50 -0400 Subject: [PATCH 04/32] [refactor] restore robot_profiles comments, move G1 URDF to urdf/ subfolder - Restore all original docstrings and comments in robot_profiles.py - Move G1_autosim.urdf into urdf/ subfolder to match X7S asset structure - Update urdf_path in g1.yml and g1_right_ee.yml accordingly Co-Authored-By: Claude Sonnet 4.6 --- .../robot/g1/{ => urdf}/G1_autosim.urdf | 0 .../autosim/content/configs/robot/g1.yml | 2 +- .../content/configs/robot/g1_right_ee.yml | 2 +- lw_benchhub/autosim/robot_profiles.py | 26 ++++++++++++++++++- 4 files changed, 27 insertions(+), 3 deletions(-) rename lw_benchhub/autosim/content/assets/robot/g1/{ => urdf}/G1_autosim.urdf (100%) diff --git a/lw_benchhub/autosim/content/assets/robot/g1/G1_autosim.urdf b/lw_benchhub/autosim/content/assets/robot/g1/urdf/G1_autosim.urdf similarity index 100% rename from lw_benchhub/autosim/content/assets/robot/g1/G1_autosim.urdf rename to lw_benchhub/autosim/content/assets/robot/g1/urdf/G1_autosim.urdf diff --git a/lw_benchhub/autosim/content/configs/robot/g1.yml b/lw_benchhub/autosim/content/configs/robot/g1.yml index d16fb606..9519d9ca 100644 --- a/lw_benchhub/autosim/content/configs/robot/g1.yml +++ b/lw_benchhub/autosim/content/configs/robot/g1.yml @@ -3,7 +3,7 @@ robot_cfg: kinematics: use_usd_kinematics: False usd_robot_root: "/g1_autosim" - urdf_path: "G1_autosim.urdf" # relative to robot_config_root_path (g1/ dir) + urdf_path: "urdf/G1_autosim.urdf" asset_root_path: "." # meshes are resolved from the urdf file dir base_link: "world_link" diff --git a/lw_benchhub/autosim/content/configs/robot/g1_right_ee.yml b/lw_benchhub/autosim/content/configs/robot/g1_right_ee.yml index 3f2fe81e..72a1ba9a 100644 --- a/lw_benchhub/autosim/content/configs/robot/g1_right_ee.yml +++ b/lw_benchhub/autosim/content/configs/robot/g1_right_ee.yml @@ -3,7 +3,7 @@ robot_cfg: kinematics: use_usd_kinematics: False usd_robot_root: "/g1_autosim" - urdf_path: "G1_autosim.urdf" # relative to robot_config_root_path (g1/ dir) + urdf_path: "urdf/G1_autosim.urdf" asset_root_path: "." # meshes are resolved from the urdf file dir base_link: "world_link" diff --git a/lw_benchhub/autosim/robot_profiles.py b/lw_benchhub/autosim/robot_profiles.py index 99cdd943..5eaf7ddf 100644 --- a/lw_benchhub/autosim/robot_profiles.py +++ b/lw_benchhub/autosim/robot_profiles.py @@ -9,6 +9,7 @@ from .action_adapters.x7s_action_adapter_cfg import X7SActionAdapterCfg +# Shared autosim content roots used by all robot-specific motion-planner configs. AUTOSIM_CONTENT_ROOT = Path(__file__).parent / "content" @@ -17,11 +18,17 @@ class RobotProfile: """Robot-only settings reused across multiple task pipelines.""" profile_id: str + """Stable autosim-side identifier used to select this robot profile.""" robot_name: str + """LW-BenchHub robot registry name forwarded into ``parse_env_cfg``.""" action_adapter_factory: Callable[[], object] + """Factory that creates the autosim action-adapter config for this robot.""" motion_planner_robot_config_file: str + """Robot-specific cuRobo / planner config file name under autosim content.""" robot_base_link_name: str + """Base link name exposed to autosim through ``EnvExtraInfo``.""" ee_link_name: str + """Preferred end-effector link name exposed to autosim through ``EnvExtraInfo``.""" curobo_asset_path: str | None = None self_collision_check: bool = True env_cfg_setup_fn: Callable | None = None @@ -32,8 +39,11 @@ class TaskRobotOverride: """Per-task-per-robot values that depend on both the task and the robot choice.""" object_reach_target_poses: dict[str, list[torch.Tensor]] | None = None + """Concrete reach target poses owned by this task-robot combination.""" extra_target_link_names: tuple[str, ...] | None = None + """Robot tip links used as extra reach targets for this task-robot combination.""" reach_extra_target_mode: str | None = None + """Reach target mode for the selected robot and task combination.""" init_state_pos_delta: tuple[float, float, float] | None = None init_state_rot: tuple[float, float, float, float] | None = None skill_cfg_fn: Callable | None = None @@ -47,7 +57,9 @@ class ResolvedRobotSettings: """Final robot settings after merging the shared profile and task-specific override.""" profile: RobotProfile + """Shared robot profile selected by the pipeline.""" override: TaskRobotOverride + """Task-local robot overrides merged on top of the shared profile.""" @property def robot_base_link_name(self) -> str: @@ -107,22 +119,32 @@ def _make_g1_action_adapter(): } +# Robot profile selection must always be explicit at the pipeline level. + def get_robot_profile(profile_id: str) -> RobotProfile: + """Resolve a registered autosim robot profile by id.""" + try: return ROBOT_PROFILES[profile_id] except KeyError as exc: available = ", ".join(sorted(ROBOT_PROFILES)) - raise KeyError(f"Unknown robot profile '{profile_id}'. Available: {available}") from exc + raise KeyError(f"Unknown autosim robot profile '{profile_id}'. Available: {available}") from exc def resolve_robot_settings(profile_id: str, override: TaskRobotOverride | None = None) -> ResolvedRobotSettings: + """Merge the shared robot profile with optional task-specific overrides.""" + profile = get_robot_profile(profile_id) return ResolvedRobotSettings(profile=profile, override=override if override is not None else TaskRobotOverride()) def configure_robot_runtime_settings(pipeline_cfg, resolved_robot: ResolvedRobotSettings) -> None: """Fill shared robot-derived runtime settings for adapter, planning, and reach behavior.""" + + # Action adapter pipeline_cfg.action_adapter = resolved_robot.profile.action_adapter_factory() + + # Motion planner pipeline_cfg.motion_planner.robot_config_file = resolved_robot.motion_planner_robot_config_file pipeline_cfg.motion_planner.curobo_asset_path = ( resolved_robot.profile.curobo_asset_path or str(AUTOSIM_CONTENT_ROOT / "assets") @@ -132,6 +154,7 @@ def configure_robot_runtime_settings(pipeline_cfg, resolved_robot: ResolvedRobot pipeline_cfg.motion_planner.self_collision_check = False pipeline_cfg.motion_planner.self_collision_opt = False + # Reach behavior if resolved_robot.override.extra_target_link_names: pipeline_cfg.skills.reach.extra_cfg.extra_target_link_names = list(resolved_robot.override.extra_target_link_names) if resolved_robot.override.reach_extra_target_mode is not None: @@ -162,6 +185,7 @@ def build_env_extra_info( robot_name: str = "robot", ) -> EnvExtraInfo: """Build autosim-facing metadata by combining task-level info with task-robot overrides.""" + if not resolved_robot.override.object_reach_target_poses: raise ValueError("No object reach target poses provided for the task-robot combination.") return EnvExtraInfo( From 31e03cb52e7d1882160314785d1ef5a9e8a1b2d8 Mon Sep 17 00:00:00 2001 From: Zihan Gao Date: Tue, 21 Apr 2026 19:04:21 -0400 Subject: [PATCH 05/32] [revert] restore g1_squat.yaml and reach_plan_sweep.py to main state Co-Authored-By: Claude Sonnet 4.6 --- lw_benchhub/core/mdp/configs/g1_squat.yaml | 4 ++-- lw_benchhub/scripts/autosim/reach_plan_sweep.py | 4 ---- 2 files changed, 2 insertions(+), 6 deletions(-) diff --git a/lw_benchhub/core/mdp/configs/g1_squat.yaml b/lw_benchhub/core/mdp/configs/g1_squat.yaml index d0840f03..9dc689f7 100644 --- a/lw_benchhub/core/mdp/configs/g1_squat.yaml +++ b/lw_benchhub/core/mdp/configs/g1_squat.yaml @@ -45,8 +45,8 @@ default_angles: [ # change with urdf # -0.092, 0.0354, 0.000,0.511,-0.438,0.038, # -0.092, 0.0354, 0.000,0.511,-0.438,0.038, 0, 0, 0, - 0, 0.1, 0, 0, 0, 0, 0, - 0, -0.1, 0, 0, 0, 0, 0 + 0, 0.3, 0, 1, 0, 0, 0, + 0, -0.3, 0, 1, 0, 0, 0 # -1.5,0.5,-2.6,0.2,-1.2,0.15,1.2, # -1.5,-0.5,2.6,0.2,1.2,-0.15,-1.2 ] diff --git a/lw_benchhub/scripts/autosim/reach_plan_sweep.py b/lw_benchhub/scripts/autosim/reach_plan_sweep.py index daa02a3b..ecc78825 100644 --- a/lw_benchhub/scripts/autosim/reach_plan_sweep.py +++ b/lw_benchhub/scripts/autosim/reach_plan_sweep.py @@ -44,10 +44,6 @@ def main() -> None: pipeline = make_pipeline(args_cli.pipeline_id) - # Batch solving (both ik_only and full motion planning) uses a different goal type - # than the warmup, which triggers a CUDA graph reset error on CUDA < 12.0. - # Disabling CUDA graph avoids the conflict entirely. - pipeline.cfg.motion_planner.use_cuda_graph = False reach_plan_sweep( pipeline, ReachPlanSweepCfg( From 3268f5eb4ffd630efdae28e342c0e13e112ae574 Mon Sep 17 00:00:00 2001 From: Zihan Gao Date: Tue, 21 Apr 2026 19:10:41 -0400 Subject: [PATCH 06/32] =?UTF-8?q?[rename]=20G1=5Fautosim.urdf=20=E2=86=92?= =?UTF-8?q?=20G1.urdf=20to=20match=20X7S=20naming=20convention?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-Authored-By: Claude Sonnet 4.6 --- .../content/assets/robot/g1/urdf/{G1_autosim.urdf => G1.urdf} | 0 lw_benchhub/autosim/content/configs/robot/g1.yml | 2 +- lw_benchhub/autosim/content/configs/robot/g1_right_ee.yml | 2 +- 3 files changed, 2 insertions(+), 2 deletions(-) rename lw_benchhub/autosim/content/assets/robot/g1/urdf/{G1_autosim.urdf => G1.urdf} (100%) diff --git a/lw_benchhub/autosim/content/assets/robot/g1/urdf/G1_autosim.urdf b/lw_benchhub/autosim/content/assets/robot/g1/urdf/G1.urdf similarity index 100% rename from lw_benchhub/autosim/content/assets/robot/g1/urdf/G1_autosim.urdf rename to lw_benchhub/autosim/content/assets/robot/g1/urdf/G1.urdf diff --git a/lw_benchhub/autosim/content/configs/robot/g1.yml b/lw_benchhub/autosim/content/configs/robot/g1.yml index 9519d9ca..d9114035 100644 --- a/lw_benchhub/autosim/content/configs/robot/g1.yml +++ b/lw_benchhub/autosim/content/configs/robot/g1.yml @@ -3,7 +3,7 @@ robot_cfg: kinematics: use_usd_kinematics: False usd_robot_root: "/g1_autosim" - urdf_path: "urdf/G1_autosim.urdf" + urdf_path: "urdf/G1.urdf" asset_root_path: "." # meshes are resolved from the urdf file dir base_link: "world_link" diff --git a/lw_benchhub/autosim/content/configs/robot/g1_right_ee.yml b/lw_benchhub/autosim/content/configs/robot/g1_right_ee.yml index 72a1ba9a..47262bc2 100644 --- a/lw_benchhub/autosim/content/configs/robot/g1_right_ee.yml +++ b/lw_benchhub/autosim/content/configs/robot/g1_right_ee.yml @@ -3,7 +3,7 @@ robot_cfg: kinematics: use_usd_kinematics: False usd_robot_root: "/g1_autosim" - urdf_path: "urdf/G1_autosim.urdf" + urdf_path: "urdf/G1.urdf" asset_root_path: "." # meshes are resolved from the urdf file dir base_link: "world_link" From ff5c6556f51edd10bdc3a03ee4e9f72dfdb1d158 Mon Sep 17 00:00:00 2001 From: Zihan Gao Date: Fri, 24 Apr 2026 01:38:38 -0400 Subject: [PATCH 07/32] [feat] adapt all pipelines for G1 and fix register_pipeline usage MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Add G1 TaskRobotOverride + G1XxxPipelineCfg subclass to all pipelines - Replace robot_profile kwarg in register_pipeline (unsupported) with per-robot cfg subclasses; register 6 new G1 pipeline IDs - Remove broken G1OpenMicrowave registrations (files deleted earlier) - Refactor __init__ in all pipelines: resolve robot before super().__init__ - Replace hardcoded init_state.pos deltas with apply_robot_env_cfg - Move skill cfg params into _x7s_skill_cfg/_g1_skill_cfg functions Note: G1 reach poses in coffee_setup_mug, cheesy_bread, dessert_upgrade, and open_fridge are placeholder estimates — calibrate with reach_plan_sweep. Co-Authored-By: Claude Sonnet 4.6 --- lw_benchhub/autosim/__init__.py | 31 +++-- lw_benchhub/autosim/pipelines/cheesy_bread.py | 109 ++++++++------- lw_benchhub/autosim/pipelines/close_oven.py | 5 + .../autosim/pipelines/coffee_setup_mug.py | 126 ++++++++++-------- .../autosim/pipelines/dessert_upgrade.py | 112 ++++++++++------ .../autosim/pipelines/kettle_boiling.py | 5 + lw_benchhub/autosim/pipelines/open_fridge.py | 95 +++++++------ 7 files changed, 295 insertions(+), 188 deletions(-) diff --git a/lw_benchhub/autosim/__init__.py b/lw_benchhub/autosim/__init__.py index 0aa08556..0e765b60 100644 --- a/lw_benchhub/autosim/__init__.py +++ b/lw_benchhub/autosim/__init__.py @@ -37,28 +37,39 @@ cfg_entry_point=f"{__name__}.pipelines.dessert_upgrade:DessertUpgradePipelineCfg", ) +# G1 pipelines register_pipeline( - id="LWBenchhub-Autosim-G1OpenMicrowavePipeline-v0", - entry_point=f"{__name__}.pipelines.g1_open_microwave:G1OpenMicrowavePipeline", - cfg_entry_point=f"{__name__}.pipelines.g1_open_microwave:G1OpenMicrowavePipelineCfg", + id="LWBenchhub-Autosim-G1OpenFridgePipeline-v0", + entry_point=f"{__name__}.pipelines.open_fridge:OpenFridgePipeline", + cfg_entry_point=f"{__name__}.pipelines.open_fridge:G1OpenFridgePipelineCfg", +) + +register_pipeline( + id="LWBenchhub-Autosim-G1CoffeeSetupMugPipeline-v0", + entry_point=f"{__name__}.pipelines.coffee_setup_mug:CoffeeSetupMugPipeline", + cfg_entry_point=f"{__name__}.pipelines.coffee_setup_mug:G1CoffeeSetupMugPipelineCfg", +) + +register_pipeline( + id="LWBenchhub-Autosim-G1CheesyBreadPipeline-v0", + entry_point=f"{__name__}.pipelines.cheesy_bread:CheesyBreadPipeline", + cfg_entry_point=f"{__name__}.pipelines.cheesy_bread:G1CheesyBreadPipelineCfg", ) register_pipeline( - id="LWBenchhub-Autosim-G1OpenMicrowaveRightOnlyPipeline-v0", - entry_point=f"{__name__}.pipelines.g1_open_microwave_right_only:G1OpenMicrowaveRightOnlyPipeline", - cfg_entry_point=f"{__name__}.pipelines.g1_open_microwave_right_only:G1OpenMicrowaveRightOnlyPipelineCfg", + id="LWBenchhub-Autosim-G1DessertUpgradePipeline-v0", + entry_point=f"{__name__}.pipelines.dessert_upgrade:DessertUpgradePipeline", + cfg_entry_point=f"{__name__}.pipelines.dessert_upgrade:G1DessertUpgradePipelineCfg", ) register_pipeline( id="LWBenchhub-Autosim-G1KettleBoilingPipeline-v0", entry_point=f"{__name__}.pipelines.kettle_boiling:KettleBoilingPipeline", - cfg_entry_point=f"{__name__}.pipelines.kettle_boiling:KettleBoilingPipelineCfg", - robot_profile="g1_loco_left", + cfg_entry_point=f"{__name__}.pipelines.kettle_boiling:G1KettleBoilingPipelineCfg", ) register_pipeline( id="LWBenchhub-Autosim-G1CloseOvenPipeline-v0", entry_point=f"{__name__}.pipelines.close_oven:CloseOvenPipeline", - cfg_entry_point=f"{__name__}.pipelines.close_oven:CloseOvenPipelineCfg", - robot_profile="g1_loco_left", + cfg_entry_point=f"{__name__}.pipelines.close_oven:G1CloseOvenPipelineCfg", ) diff --git a/lw_benchhub/autosim/pipelines/cheesy_bread.py b/lw_benchhub/autosim/pipelines/cheesy_bread.py index 00def357..46c1db26 100644 --- a/lw_benchhub/autosim/pipelines/cheesy_bread.py +++ b/lw_benchhub/autosim/pipelines/cheesy_bread.py @@ -1,19 +1,43 @@ +import torch from autosim.core.pipeline import AutoSimPipeline, AutoSimPipelineCfg +from autosim.decomposers import LLMDecomposerCfg from isaaclab.envs import ManagerBasedEnv from isaaclab.utils import configclass -import torch - -from autosim.decomposers import LLMDecomposerCfg - from ..prompt_utils import render_additional_prompt from ..robot_profiles import ( TaskRobotOverride, + apply_robot_env_cfg, build_env_extra_info, configure_robot_runtime_settings, resolve_robot_settings, ) + +def _x7s_skill_cfg(cfg) -> None: + cfg.skills.moveto.extra_cfg.local_planner.max_linear_velocity = 3.5 + cfg.skills.moveto.extra_cfg.local_planner.max_angular_velocity = 3.0 + cfg.skills.moveto.extra_cfg.local_planner.predict_time = 0.4 + cfg.skills.moveto.extra_cfg.global_planner.safety_distance = 1.1 + cfg.skills.moveto.extra_cfg.global_planner.proximity_weight = 3.0 + cfg.skills.moveto.extra_cfg.waypoint_tolerance = 0.2 + cfg.skills.moveto.extra_cfg.goal_tolerance = 0.1 + cfg.skills.moveto.extra_cfg.yaw_tolerance = 0.005 + cfg.skills.moveto.extra_cfg.uws_dwa = False + + +def _g1_skill_cfg(cfg) -> None: + cfg.skills.moveto.extra_cfg.local_planner.max_linear_velocity = 0.2 + cfg.skills.moveto.extra_cfg.local_planner.max_angular_velocity = 0.2 + cfg.skills.moveto.extra_cfg.local_planner.predict_time = 0.4 + cfg.skills.moveto.extra_cfg.global_planner.safety_distance = 0.5 + cfg.skills.moveto.extra_cfg.global_planner.proximity_weight = 3.0 + cfg.skills.moveto.extra_cfg.waypoint_tolerance = 0.25 + cfg.skills.moveto.extra_cfg.goal_tolerance = 0.30 + cfg.skills.moveto.extra_cfg.yaw_tolerance = 0.01 + cfg.skills.moveto.extra_cfg.use_dwa = False + + TASK_ROBOT_OVERRIDES: dict[str, TaskRobotOverride] = { "x7s_joint_left": TaskRobotOverride( extra_target_link_names=("link20_tip",), @@ -25,6 +49,20 @@ torch.tensor([-0.05, -0.05, 0.13, 0.9238, 0.0, 0.0, 0.3827]), ], }, + init_state_pos_delta=(0.0, -0.8, 0.0), + skill_cfg_fn=_x7s_skill_cfg, + ), + "g1_loco_left": TaskRobotOverride( + object_reach_target_poses={ + "cheese": [ + torch.tensor([0.003, -0.049, 0.025, 0.705, -0.002, 0.05, 0.707]), + ], + "bread": [ + torch.tensor([-0.05, -0.05, 0.13, 0.9238, 0.0, 0.0, 0.3827]), + ], + }, + init_state_pos_delta=(0.0, -0.8, 0.01), + skill_cfg_fn=_g1_skill_cfg, ), } @@ -33,9 +71,9 @@ def get_task_robot_override(robot_profile: str) -> TaskRobotOverride: try: return TASK_ROBOT_OVERRIDES[robot_profile] except KeyError as exc: - supported = ", ".join(tuple(TASK_ROBOT_OVERRIDES)) + supported = ", ".join(TASK_ROBOT_OVERRIDES) raise ValueError( - f"CheesyBreadPipeline does not support robot profile '{robot_profile}'. Supported profiles: {supported}" + f"CheesyBreadPipeline does not support robot profile '{robot_profile}'. Supported: {supported}" ) from exc @@ -44,25 +82,14 @@ class CheesyBreadPipelineCfg(AutoSimPipelineCfg): """Configuration for the CheesyBreadPipeline.""" robot_profile: str = "x7s_joint_left" - decomposer: LLMDecomposerCfg = LLMDecomposerCfg() def __post_init__(self): - resolved_robot = resolve_robot_settings( - self.robot_profile, - override=get_task_robot_override(self.robot_profile), - ) + resolved_robot = resolve_robot_settings(self.robot_profile, override=get_task_robot_override(self.robot_profile)) configure_robot_runtime_settings(self, resolved_robot) - self.skills.moveto.extra_cfg.local_planner.max_linear_velocity = 3.5 - self.skills.moveto.extra_cfg.local_planner.max_angular_velocity = 3.0 - self.skills.moveto.extra_cfg.local_planner.predict_time = 0.4 - self.skills.moveto.extra_cfg.global_planner.safety_distance = 1.1 - self.skills.moveto.extra_cfg.global_planner.proximity_weight = 3.0 - self.skills.moveto.extra_cfg.waypoint_tolerance = 0.2 - self.skills.moveto.extra_cfg.goal_tolerance = 0.1 - self.skills.moveto.extra_cfg.yaw_tolerance = 0.005 - self.skills.moveto.extra_cfg.uws_dwa = False + if resolved_robot.override.skill_cfg_fn: + resolved_robot.override.skill_cfg_fn(self) self.occupancy_map.floor_prim_suffix = "Scene/floor_room" self.motion_planner.world_ignore_subffixes = ["Scene/floor_room"] @@ -74,21 +101,24 @@ def __post_init__(self): ] +@configclass +class G1CheesyBreadPipelineCfg(CheesyBreadPipelineCfg): + robot_profile: str = "g1_loco_left" + + class CheesyBreadPipeline(AutoSimPipeline): def __init__(self, cfg: AutoSimPipelineCfg): - super().__init__(cfg) - robot_profile = cfg.robot_profile self._resolved_robot = resolve_robot_settings( - robot_profile, - override=get_task_robot_override(robot_profile), + cfg.robot_profile, override=get_task_robot_override(cfg.robot_profile) ) + super().__init__(cfg) def load_env(self) -> ManagerBasedEnv: import gymnasium as gym import lw_benchhub_tasks.lightwheel_robocasa_tasks.multi_stage.making_toast.cheesy_bread as cb from lw_benchhub.utils.env import ExecuteMode, parse_env_cfg - cb.CheesyBread._get_obj_cfgs = patch_get_obj_cfgs + cb.CheesyBread._get_obj_cfgs = _get_obj_cfgs env_cfg = parse_env_cfg( scene_backend="robocasa", @@ -113,10 +143,9 @@ def load_env(self) -> ManagerBasedEnv: resample_robot_placement_on_reset=False, ) + apply_robot_env_cfg(env_cfg, self._resolved_robot) env_cfg.terminations.time_out = None - env_cfg.scene.robot.init_state.pos[1] -= 0.8 - env_id = f"Robocasa-CheesyBread-{self._resolved_robot.profile.robot_name}-v0" gym.register( id=env_id, @@ -125,9 +154,7 @@ def load_env(self) -> ManagerBasedEnv: disable_env_checker=True, ) - env = gym.make(env_id, cfg=env_cfg, render_mode="rgb_array").unwrapped - - return env + return gym.make(env_id, cfg=env_cfg, render_mode="rgb_array").unwrapped def get_env_extra_info(self): return build_env_extra_info( @@ -140,9 +167,8 @@ def get_env_extra_info(self): ) -def patch_get_obj_cfgs(self): - cfgs = [] - cfgs.append( +def _get_obj_cfgs(self): + return [ dict( name="bread", obj_groups="bread_flat", @@ -155,30 +181,23 @@ def patch_get_obj_cfgs(self): rotation=(0.0, 0.0), try_to_place_in="cutting_board", ), - ) - ) - cfgs.append( + ), dict( name="cheese", obj_groups="cheese", asset_name="Cheese003.usd", init_robot_here=True, - placement=dict[str, str | tuple[float, float]]( + placement=dict( ref_obj="bread_container", fixture=self.counter, size=(1.0, 0.08), pos=(-0.8, -1.0), rotation=(0.0, 0.0), ), - ) - ) - - # Distractor on the counter - cfgs.append( + ), dict( name="distr_counter", obj_groups="all", placement=dict(fixture=self.counter, size=(1.0, 0.20), pos=(0, 1.0)), - ) - ) - return cfgs + ), + ] diff --git a/lw_benchhub/autosim/pipelines/close_oven.py b/lw_benchhub/autosim/pipelines/close_oven.py index d984d166..f224d034 100644 --- a/lw_benchhub/autosim/pipelines/close_oven.py +++ b/lw_benchhub/autosim/pipelines/close_oven.py @@ -113,6 +113,11 @@ def __post_init__(self): _DOOR_JOINT_PATH = "/World/envs/env_0/Scene/oven_main_group/Oven032_door/door_joint" +@configclass +class G1CloseOvenPipelineCfg(CloseOvenPipelineCfg): + robot_profile: str = "g1_loco_left" + + class CloseOvenPipeline(AutoSimPipeline): def __init__(self, cfg: AutoSimPipelineCfg): self._resolved_robot = resolve_robot_settings( diff --git a/lw_benchhub/autosim/pipelines/coffee_setup_mug.py b/lw_benchhub/autosim/pipelines/coffee_setup_mug.py index 75c0d28e..13657f38 100644 --- a/lw_benchhub/autosim/pipelines/coffee_setup_mug.py +++ b/lw_benchhub/autosim/pipelines/coffee_setup_mug.py @@ -1,20 +1,44 @@ +import numpy as np +import torch from autosim.core.pipeline import AutoSimPipeline, AutoSimPipelineCfg +from autosim.decomposers import LLMDecomposerCfg from isaaclab.envs import ManagerBasedEnv from isaaclab.utils import configclass -import torch -import numpy as np - -from autosim.decomposers import LLMDecomposerCfg - from ..prompt_utils import render_additional_prompt from ..robot_profiles import ( TaskRobotOverride, + apply_robot_env_cfg, build_env_extra_info, configure_robot_runtime_settings, resolve_robot_settings, ) + +def _x7s_skill_cfg(cfg) -> None: + cfg.skills.moveto.extra_cfg.local_planner.max_linear_velocity = 0.4 + cfg.skills.moveto.extra_cfg.local_planner.max_angular_velocity = 0.8 + cfg.skills.moveto.extra_cfg.local_planner.predict_time = 0.4 + cfg.skills.moveto.extra_cfg.global_planner.safety_distance = 0.8 + cfg.skills.moveto.extra_cfg.global_planner.proximity_weight = 3.0 + cfg.skills.moveto.extra_cfg.waypoint_tolerance = 0.2 + cfg.skills.moveto.extra_cfg.goal_tolerance = 0.07 + cfg.skills.moveto.extra_cfg.yaw_tolerance = 0.008 + cfg.skills.moveto.extra_cfg.uws_dwa = False + + +def _g1_skill_cfg(cfg) -> None: + cfg.skills.moveto.extra_cfg.local_planner.max_linear_velocity = 0.2 + cfg.skills.moveto.extra_cfg.local_planner.max_angular_velocity = 0.2 + cfg.skills.moveto.extra_cfg.local_planner.predict_time = 0.4 + cfg.skills.moveto.extra_cfg.global_planner.safety_distance = 0.5 + cfg.skills.moveto.extra_cfg.global_planner.proximity_weight = 3.0 + cfg.skills.moveto.extra_cfg.waypoint_tolerance = 0.25 + cfg.skills.moveto.extra_cfg.goal_tolerance = 0.30 + cfg.skills.moveto.extra_cfg.yaw_tolerance = 0.01 + cfg.skills.moveto.extra_cfg.use_dwa = False + + TASK_ROBOT_OVERRIDES: dict[str, TaskRobotOverride] = { "x7s_joint_left": TaskRobotOverride( extra_target_link_names=("link20_tip",), @@ -27,6 +51,21 @@ torch.tensor([0.0, -0.12, -0.03, 0.707, 0.0, 0.0, 0.707]), ], }, + init_state_pos_delta=(0.0, -0.8, 0.3), + init_state_rot=(0.707, 0.0, 0.0, -0.707), + skill_cfg_fn=_x7s_skill_cfg, + ), + "g1_loco_left": TaskRobotOverride( + object_reach_target_poses={ + "obj": [ + torch.tensor([-0.02, 0.0, 0.01, 1.0, 0.0, 0.0, 0.0]), + ], + "coffee_machine_main_group": [ + torch.tensor([0.0, -0.12, -0.03, 0.707, 0.0, 0.0, 0.707]), + ], + }, + init_state_pos_delta=(0.0, -0.8, 0.01), + skill_cfg_fn=_g1_skill_cfg, ), } @@ -35,9 +74,9 @@ def get_task_robot_override(robot_profile: str) -> TaskRobotOverride: try: return TASK_ROBOT_OVERRIDES[robot_profile] except KeyError as exc: - supported = ", ".join(tuple(TASK_ROBOT_OVERRIDES)) + supported = ", ".join(TASK_ROBOT_OVERRIDES) raise ValueError( - f"CoffeeSetupMugPipeline does not support robot profile '{robot_profile}'. Supported profiles: {supported}" + f"CoffeeSetupMugPipeline does not support robot profile '{robot_profile}'. Supported: {supported}" ) from exc @@ -46,27 +85,16 @@ class CoffeeSetupMugPipelineCfg(AutoSimPipelineCfg): """Configuration for the CoffeeSetupMugPipeline.""" robot_profile: str = "x7s_joint_left" - decomposer: LLMDecomposerCfg = LLMDecomposerCfg() def __post_init__(self): - resolved_robot = resolve_robot_settings( - self.robot_profile, - override=get_task_robot_override(self.robot_profile), - ) + resolved_robot = resolve_robot_settings(self.robot_profile, override=get_task_robot_override(self.robot_profile)) configure_robot_runtime_settings(self, resolved_robot) - self.skills.moveto.extra_cfg.local_planner.max_linear_velocity = 0.4 - self.skills.moveto.extra_cfg.local_planner.max_angular_velocity = 0.8 - self.skills.moveto.extra_cfg.local_planner.predict_time = 0.4 - self.skills.moveto.extra_cfg.global_planner.safety_distance = 0.8 - self.skills.moveto.extra_cfg.global_planner.proximity_weight = 3.0 - self.skills.moveto.extra_cfg.waypoint_tolerance = 0.2 - self.skills.moveto.extra_cfg.goal_tolerance = 0.07 - self.skills.moveto.extra_cfg.yaw_tolerance = 0.008 - self.skills.moveto.extra_cfg.uws_dwa = False - self.max_steps = 1000 + if resolved_robot.override.skill_cfg_fn: + resolved_robot.override.skill_cfg_fn(self) + self.max_steps = 1000 self.skills.lift.extra_cfg.lift_offset = 0.20 self.occupancy_map.floor_prim_suffix = "Scene/floor_room" @@ -78,21 +106,25 @@ def __post_init__(self): ] +@configclass +class G1CoffeeSetupMugPipelineCfg(CoffeeSetupMugPipelineCfg): + robot_profile: str = "g1_loco_left" + + class CoffeeSetupMugPipeline(AutoSimPipeline): def __init__(self, cfg: AutoSimPipelineCfg): - super().__init__(cfg) - robot_profile = cfg.robot_profile self._resolved_robot = resolve_robot_settings( - robot_profile, - override=get_task_robot_override(robot_profile), + cfg.robot_profile, override=get_task_robot_override(cfg.robot_profile) ) + super().__init__(cfg) def load_env(self) -> ManagerBasedEnv: import gymnasium as gym import lw_benchhub_tasks.lightwheel_robocasa_tasks.single_stage.kitchen_coffee as kc from lw_benchhub.utils.env import ExecuteMode, parse_env_cfg - kc.CoffeeSetupMug._get_obj_cfgs = patch_get_obj_cfgs + kc.CoffeeSetupMug._get_obj_cfgs = _get_obj_cfgs + env_cfg = parse_env_cfg( scene_backend="robocasa", task_backend="robocasa", @@ -115,12 +147,9 @@ def load_env(self) -> ManagerBasedEnv: replay_cfgs={"add_camera_to_observation": True, "render_resolution": (640, 480)}, ) + apply_robot_env_cfg(env_cfg, self._resolved_robot) env_cfg.terminations.time_out = None - env_cfg.scene.robot.init_state.pos[1] -= 0.8 - env_cfg.scene.robot.init_state.pos[2] += 0.3 - env_cfg.scene.robot.init_state.rot = (0.707, 0.0, 0.0, -0.707) - env_id = f"Robocasa-CoffeeSetupMug-{self._resolved_robot.profile.robot_name}-v0" gym.register( id=env_id, @@ -129,9 +158,7 @@ def load_env(self) -> ManagerBasedEnv: disable_env_checker=True, ) - env = gym.make(env_id, cfg=env_cfg, render_mode="rgb_array").unwrapped - - return env + return gym.make(env_id, cfg=env_cfg, render_mode="rgb_array").unwrapped def get_env_extra_info(self): return build_env_extra_info( @@ -142,21 +169,16 @@ def get_env_extra_info(self): ) -def patch_get_obj_cfgs(self): - cfgs = [] - cfgs.append( - dict( - name="obj", - obj_groups="mug", - asset_name="Mug028.usd", - placement=dict( - fixture=self.counter, - sample_region_kwargs={}, - size=(0.8, 0.15), - pos=(0.0, -1.0), - rotation=(np.pi / 2, np.pi / 2), - ), - ) - ) - - return cfgs +def _get_obj_cfgs(self): + return [dict( + name="obj", + obj_groups="mug", + asset_name="Mug028.usd", + placement=dict( + fixture=self.counter, + sample_region_kwargs={}, + size=(0.8, 0.15), + pos=(0.0, -1.0), + rotation=(np.pi / 2, np.pi / 2), + ), + )] diff --git a/lw_benchhub/autosim/pipelines/dessert_upgrade.py b/lw_benchhub/autosim/pipelines/dessert_upgrade.py index bc210fa6..bb06078c 100644 --- a/lw_benchhub/autosim/pipelines/dessert_upgrade.py +++ b/lw_benchhub/autosim/pipelines/dessert_upgrade.py @@ -1,18 +1,42 @@ -from autosim.core.pipeline import AutoSimPipeline, AutoSimPipelineCfg -from isaaclab.utils import configclass - import torch - +from autosim.core.pipeline import AutoSimPipeline, AutoSimPipelineCfg from autosim.decomposers import LLMDecomposerCfg +from isaaclab.utils import configclass from ..prompt_utils import render_additional_prompt from ..robot_profiles import ( TaskRobotOverride, + apply_robot_env_cfg, build_env_extra_info, configure_robot_runtime_settings, resolve_robot_settings, ) + +def _x7s_skill_cfg(cfg) -> None: + cfg.skills.moveto.extra_cfg.local_planner.max_linear_velocity = 0.4 + cfg.skills.moveto.extra_cfg.local_planner.max_angular_velocity = 0.8 + cfg.skills.moveto.extra_cfg.local_planner.predict_time = 0.4 + cfg.skills.moveto.extra_cfg.global_planner.safety_distance = 0.8 + cfg.skills.moveto.extra_cfg.global_planner.proximity_weight = 3.0 + cfg.skills.moveto.extra_cfg.waypoint_tolerance = 0.2 + cfg.skills.moveto.extra_cfg.goal_tolerance = 0.07 + cfg.skills.moveto.extra_cfg.yaw_tolerance = 0.008 + cfg.skills.moveto.extra_cfg.uws_dwa = False + + +def _g1_skill_cfg(cfg) -> None: + cfg.skills.moveto.extra_cfg.local_planner.max_linear_velocity = 0.2 + cfg.skills.moveto.extra_cfg.local_planner.max_angular_velocity = 0.2 + cfg.skills.moveto.extra_cfg.local_planner.predict_time = 0.4 + cfg.skills.moveto.extra_cfg.global_planner.safety_distance = 0.5 + cfg.skills.moveto.extra_cfg.global_planner.proximity_weight = 3.0 + cfg.skills.moveto.extra_cfg.waypoint_tolerance = 0.25 + cfg.skills.moveto.extra_cfg.goal_tolerance = 0.30 + cfg.skills.moveto.extra_cfg.yaw_tolerance = 0.01 + cfg.skills.moveto.extra_cfg.use_dwa = False + + TASK_ROBOT_OVERRIDES: dict[str, TaskRobotOverride] = { "x7s_joint_left": TaskRobotOverride( extra_target_link_names=("link20_tip",), @@ -25,6 +49,8 @@ torch.tensor([0.003, -0.049, 0.085, 0.705, -0.002, 0.05, 0.707]), ], }, + init_state_pos_delta=(-0.2, -1.6, 0.0), + skill_cfg_fn=_x7s_skill_cfg, ), "x7s_joint_right": TaskRobotOverride( extra_target_link_names=("link11_tip",), @@ -37,6 +63,20 @@ torch.tensor([0.003, -0.049, 0.085, 0.705, -0.002, 0.05, 0.707]), ], }, + init_state_pos_delta=(-0.2, -1.6, 0.0), + skill_cfg_fn=_x7s_skill_cfg, + ), + "g1_loco_left": TaskRobotOverride( + object_reach_target_poses={ + "dessert1": [ + torch.tensor([0.003, -0.019, 0.025, 0.705, -0.002, 0.05, 0.707]), + ], + "receptacle": [ + torch.tensor([0.003, -0.049, 0.085, 0.705, -0.002, 0.05, 0.707]), + ], + }, + init_state_pos_delta=(-0.2, -1.6, 0.01), + skill_cfg_fn=_g1_skill_cfg, ), } @@ -45,9 +85,9 @@ def get_task_robot_override(robot_profile: str) -> TaskRobotOverride: try: return TASK_ROBOT_OVERRIDES[robot_profile] except KeyError as exc: - supported = ", ".join(tuple(TASK_ROBOT_OVERRIDES)) + supported = ", ".join(TASK_ROBOT_OVERRIDES) raise ValueError( - f"DessertUpgradePipeline does not support robot profile '{robot_profile}'. Supported profiles: {supported}" + f"DessertUpgradePipeline does not support robot profile '{robot_profile}'. Supported: {supported}" ) from exc @@ -56,27 +96,16 @@ class DessertUpgradePipelineCfg(AutoSimPipelineCfg): """Configuration for the DessertUpgradePipeline.""" robot_profile: str = "x7s_joint_left" - decomposer: LLMDecomposerCfg = LLMDecomposerCfg() def __post_init__(self): - resolved_robot = resolve_robot_settings( - self.robot_profile, - override=get_task_robot_override(self.robot_profile), - ) + resolved_robot = resolve_robot_settings(self.robot_profile, override=get_task_robot_override(self.robot_profile)) configure_robot_runtime_settings(self, resolved_robot) - self.skills.moveto.extra_cfg.local_planner.max_linear_velocity = 0.4 - self.skills.moveto.extra_cfg.local_planner.max_angular_velocity = 0.8 - self.skills.moveto.extra_cfg.local_planner.predict_time = 0.4 - self.skills.moveto.extra_cfg.global_planner.safety_distance = 0.8 - self.skills.moveto.extra_cfg.global_planner.proximity_weight = 3.0 - self.skills.moveto.extra_cfg.waypoint_tolerance = 0.2 - self.skills.moveto.extra_cfg.goal_tolerance = 0.07 - self.skills.moveto.extra_cfg.yaw_tolerance = 0.008 - self.skills.moveto.extra_cfg.uws_dwa = False - self.max_steps = 1000 + if resolved_robot.override.skill_cfg_fn: + resolved_robot.override.skill_cfg_fn(self) + self.max_steps = 1000 self.skills.lift.extra_cfg.lift_offset = 0.20 self.occupancy_map.floor_prim_suffix = "Scene/floor_room" @@ -88,21 +117,25 @@ def __post_init__(self): ] +@configclass +class G1DessertUpgradePipelineCfg(DessertUpgradePipelineCfg): + robot_profile: str = "g1_loco_left" + + class DessertUpgradePipeline(AutoSimPipeline): def __init__(self, cfg: AutoSimPipelineCfg): - super().__init__(cfg) - robot_profile = cfg.robot_profile self._resolved_robot = resolve_robot_settings( - robot_profile, - override=get_task_robot_override(robot_profile), + cfg.robot_profile, override=get_task_robot_override(cfg.robot_profile) ) + super().__init__(cfg) def load_env(self): import gymnasium as gym import lw_benchhub_tasks.lightwheel_robocasa_tasks.multi_stage.serving_food.dessert_upgrade as du from lw_benchhub.utils.env import ExecuteMode, parse_env_cfg - du.DessertUpgrade._get_obj_cfgs = patch_get_obj_cfgs + du.DessertUpgrade._get_obj_cfgs = _get_obj_cfgs + env_cfg = parse_env_cfg( scene_backend="robocasa", task_backend="robocasa", @@ -126,9 +159,7 @@ def load_env(self): resample_robot_placement_on_reset=False, ) - env_cfg.scene.robot.init_state.pos[0] -= 0.2 - env_cfg.scene.robot.init_state.pos[1] -= 1.6 - + apply_robot_env_cfg(env_cfg, self._resolved_robot) env_cfg.terminations.time_out = None env_cfg.terminations.success = None @@ -139,22 +170,22 @@ def load_env(self): kwargs={}, disable_env_checker=True, ) - env = gym.make(env_id, cfg=env_cfg, render_mode="rgb_array").unwrapped - return env + return gym.make(env_id, cfg=env_cfg, render_mode="rgb_array").unwrapped def get_env_extra_info(self): return build_env_extra_info( task_name="Robocasa-Task-DessertUpgrade", objects=["dessert1", "receptacle"], - additional_prompt_contents=f"{render_additional_prompt()}\n\n Only grasp the dessert1 and place it in the receptacle. After grasping the dessert1, you should lift it up.", + additional_prompt_contents=( + f"{render_additional_prompt()}\n\n Only grasp the dessert1 and place it in the receptacle. After grasping the dessert1, you should lift it up." + ), resolved_robot=self._resolved_robot, ) -def patch_get_obj_cfgs(self): - cfgs = [] - cfgs.append( +def _get_obj_cfgs(self): + return [ dict( name="receptacle", obj_groups="tray", @@ -168,10 +199,7 @@ def patch_get_obj_cfgs(self): offset=(0.0, -0.0), rotation=(0.0, 0.0), ), - ) - ) - - cfgs.append( + ), dict( name="dessert1", obj_groups=["cake"], @@ -183,7 +211,5 @@ def patch_get_obj_cfgs(self): pos=(0, -1), rotation=(0.0, 0.0), ), - ) - ) - - return cfgs + ), + ] diff --git a/lw_benchhub/autosim/pipelines/kettle_boiling.py b/lw_benchhub/autosim/pipelines/kettle_boiling.py index 2027a0b3..26a129f9 100644 --- a/lw_benchhub/autosim/pipelines/kettle_boiling.py +++ b/lw_benchhub/autosim/pipelines/kettle_boiling.py @@ -174,6 +174,11 @@ def __post_init__(self): ] +@configclass +class G1KettleBoilingPipelineCfg(KettleBoilingPipelineCfg): + robot_profile: str = "g1_loco_left" + + class KettleBoilingPipeline(AutoSimPipeline): def __init__(self, cfg: AutoSimPipelineCfg): self._resolved_robot = resolve_robot_settings( diff --git a/lw_benchhub/autosim/pipelines/open_fridge.py b/lw_benchhub/autosim/pipelines/open_fridge.py index 48492ccb..59232d9d 100644 --- a/lw_benchhub/autosim/pipelines/open_fridge.py +++ b/lw_benchhub/autosim/pipelines/open_fridge.py @@ -1,20 +1,46 @@ -from autosim.core.pipeline import AutoSimPipeline, AutoSimPipelineCfg -from isaaclab.envs import ManagerBasedEnv -from isaaclab.utils import configclass - import numpy as np import torch - +from autosim.core.pipeline import AutoSimPipeline, AutoSimPipelineCfg from autosim.decomposers import LLMDecomposerCfg +from isaaclab.envs import ManagerBasedEnv +from isaaclab.utils import configclass from ..prompt_utils import render_additional_prompt from ..robot_profiles import ( TaskRobotOverride, + apply_robot_env_cfg, build_env_extra_info, configure_robot_runtime_settings, resolve_robot_settings, ) + +def _x7s_skill_cfg(cfg) -> None: + cfg.skills.moveto.extra_cfg.local_planner.max_linear_velocity = 0.1 + cfg.skills.moveto.extra_cfg.local_planner.max_angular_velocity = 0.1 + cfg.skills.moveto.extra_cfg.local_planner.predict_time = 0.4 + cfg.skills.moveto.extra_cfg.global_planner.safety_distance = 0.8 + cfg.skills.moveto.extra_cfg.global_planner.proximity_weight = 3.0 + cfg.skills.moveto.extra_cfg.waypoint_tolerance = 0.2 + cfg.skills.moveto.extra_cfg.goal_tolerance = 0.3 + cfg.skills.moveto.extra_cfg.yaw_tolerance = 0.005 + cfg.skills.moveto.extra_cfg.uws_dwa = False + cfg.skills.moveto.extra_cfg.sampling_radius = 1.0 + + +def _g1_skill_cfg(cfg) -> None: + cfg.skills.moveto.extra_cfg.local_planner.max_linear_velocity = 0.2 + cfg.skills.moveto.extra_cfg.local_planner.max_angular_velocity = 0.2 + cfg.skills.moveto.extra_cfg.local_planner.predict_time = 0.4 + cfg.skills.moveto.extra_cfg.global_planner.safety_distance = 0.5 + cfg.skills.moveto.extra_cfg.global_planner.proximity_weight = 3.0 + cfg.skills.moveto.extra_cfg.waypoint_tolerance = 0.25 + cfg.skills.moveto.extra_cfg.goal_tolerance = 0.30 + cfg.skills.moveto.extra_cfg.yaw_tolerance = 0.01 + cfg.skills.moveto.extra_cfg.use_dwa = False + cfg.skills.moveto.extra_cfg.sampling_radius = 1.0 + + TASK_ROBOT_OVERRIDES: dict[str, TaskRobotOverride] = { "x7s_joint_right": TaskRobotOverride( object_reach_target_poses={ @@ -22,6 +48,17 @@ torch.tensor([0.047, -0.429, 0.125, 0.707, 0.0, 0.0, 0.707]), ], }, + init_state_pos_delta=(-0.45, -0.7, 0.0), + skill_cfg_fn=_x7s_skill_cfg, + ), + "g1_loco_left": TaskRobotOverride( + object_reach_target_poses={ + "fridge_main_group": [ + torch.tensor([0.05, -0.35, 0.10, 0.707, 0.0, 0.0, 0.707]), + ], + }, + init_state_pos_delta=(-0.45, -0.7, 0.0), + skill_cfg_fn=_g1_skill_cfg, ), } @@ -30,40 +67,26 @@ def get_task_robot_override(robot_profile: str) -> TaskRobotOverride: try: return TASK_ROBOT_OVERRIDES[robot_profile] except KeyError as exc: - supported = ", ".join(tuple(TASK_ROBOT_OVERRIDES)) + supported = ", ".join(TASK_ROBOT_OVERRIDES) raise ValueError( - f"OpenFridgePipeline does not support robot profile '{robot_profile}'. Supported profiles: {supported}" + f"OpenFridgePipeline does not support robot profile '{robot_profile}'. Supported: {supported}" ) from exc @configclass class OpenFridgePipelineCfg(AutoSimPipelineCfg): - """Configuration for the OpenFridgePipeline.""" - robot_profile: str = "x7s_joint_right" - decomposer: LLMDecomposerCfg = LLMDecomposerCfg() def __post_init__(self): - resolved_robot = resolve_robot_settings( - self.robot_profile, - override=get_task_robot_override(self.robot_profile), - ) + resolved_robot = resolve_robot_settings(self.robot_profile, override=get_task_robot_override(self.robot_profile)) configure_robot_runtime_settings(self, resolved_robot) - self.skills.moveto.extra_cfg.local_planner.max_linear_velocity = 0.1 - self.skills.moveto.extra_cfg.local_planner.max_angular_velocity = 0.1 - self.skills.moveto.extra_cfg.local_planner.predict_time = 0.4 - self.skills.moveto.extra_cfg.global_planner.safety_distance = 0.8 - self.skills.moveto.extra_cfg.global_planner.proximity_weight = 3.0 - self.skills.moveto.extra_cfg.waypoint_tolerance = 0.2 - self.skills.moveto.extra_cfg.goal_tolerance = 0.3 - self.skills.moveto.extra_cfg.yaw_tolerance = 0.005 - self.skills.moveto.extra_cfg.uws_dwa = False - self.skills.moveto.extra_cfg.sampling_radius = 1.0 + if resolved_robot.override.skill_cfg_fn: + resolved_robot.override.skill_cfg_fn(self) self.skills.pull.extra_cfg.move_offset = 0.3 - self.skills.pull.extra_cfg.move_axis = "-x" + self.skills.pull.extra_cfg.move_axis = "-x" self.occupancy_map.floor_prim_suffix = "Scene/floor_room" self.motion_planner.world_ignore_subffixes = ["Scene/floor_room"] @@ -72,17 +95,17 @@ def __post_init__(self): ] +@configclass +class G1OpenFridgePipelineCfg(OpenFridgePipelineCfg): + robot_profile: str = "g1_loco_left" + + class OpenFridgePipeline(AutoSimPipeline): def __init__(self, cfg: AutoSimPipelineCfg): - super().__init__(cfg) - robot_profile = cfg.robot_profile self._resolved_robot = resolve_robot_settings( - robot_profile, - override=get_task_robot_override(robot_profile), + cfg.robot_profile, override=get_task_robot_override(cfg.robot_profile) ) - - def reset_env(self): - super().reset_env() + super().__init__(cfg) def load_env(self) -> ManagerBasedEnv: import gymnasium as gym @@ -111,11 +134,9 @@ def load_env(self) -> ManagerBasedEnv: resample_robot_placement_on_reset=False, ) + apply_robot_env_cfg(env_cfg, self._resolved_robot) env_cfg.terminations.time_out = None - env_cfg.scene.robot.init_state.pos[0] -= 0.45 - env_cfg.scene.robot.init_state.pos[1] -= 0.7 - env_id = f"Robocasa-OpenFridge-{self._resolved_robot.profile.robot_name}-v0" gym.register( id=env_id, @@ -124,9 +145,7 @@ def load_env(self) -> ManagerBasedEnv: disable_env_checker=True, ) - env = gym.make(env_id, cfg=env_cfg).unwrapped - - return env + return gym.make(env_id, cfg=env_cfg).unwrapped def get_env_extra_info(self): return build_env_extra_info( From 7a2d54a344fce1432aa5587fe3518e10892825a8 Mon Sep 17 00:00:00 2001 From: Zihan Gao Date: Sun, 26 Apr 2026 19:11:43 -0400 Subject: [PATCH 08/32] fix bugs --- .../action_adapters/g1_action_adapter.py | 141 ++++++++++++++---- .../action_adapters/g1_action_adapter_cfg.py | 15 ++ .../autosim/content/configs/robot/g1.yml | 11 +- lw_benchhub/autosim/pipelines/close_oven.py | 49 ++---- 4 files changed, 147 insertions(+), 69 deletions(-) diff --git a/lw_benchhub/autosim/action_adapters/g1_action_adapter.py b/lw_benchhub/autosim/action_adapters/g1_action_adapter.py index e2c673cc..e3ff4cec 100644 --- a/lw_benchhub/autosim/action_adapters/g1_action_adapter.py +++ b/lw_benchhub/autosim/action_adapters/g1_action_adapter.py @@ -33,19 +33,107 @@ def __init__(self, cfg: G1ActionAdapterCfg): self.register_apply_method("grasp", self._apply_gripper) self.register_apply_method("ungrasp", self._apply_gripper) + self._dbg_reach_target_w: torch.Tensor | None = None + self._dbg_reach_step: int = 0 + self._has_squatted: bool = False + + _ARM_SKILLS = frozenset({"reach", "lift", "push", "pull"}) + # ------------------------------------------------------------------ - # Navigation (leg locomotion base command) + # Pre-skill settling: squat + optional forward nudge # ------------------------------------------------------------------ - def _apply_moveto(self, skill_output: SkillOutput, env: ManagerBasedEnv) -> torch.Tensor: - """Convert world-frame velocity [vx, vy, vyaw] to virtual-base delta positions. + def pre_skill_hook(self, skill_type: str, env: ManagerBasedEnv, last_action: torch.Tensor) -> None: + """Squat to a moderate height before any arm skill, then nudge forward. + + squat_cmd[0] starts at 0.75 (stand) and drops by -1.0 * max_cmd[0] / 250 = -0.0016 + per env.step. With squat_settle_steps=80: 10 transition + 70 steps → squat_cmd[0] ≈ 0.64 + (a slight crouch, ~10 cm pelvis drop). - G1ActionsCfg layout: - base_action [0:3] – JointPositionAction with scale=0.01 (delta) - right_arm [3:10] – absolute - left_arm [10:17] – absolute - gripper [17:31] – absolute + After squatting, for the reach skill the robot is teleported forward along its heading + by pos_search_nudge_m * pos_search_nudge_count so it is closer to the target. + solve_ik_batch is intentionally NOT used here because it conflicts with plan_motion's + CUDA graph on CUDA < 12.0 (changing goal type error). """ + if skill_type not in self._ARM_SKILLS: + return + if self._has_squatted: + return + self._has_squatted = True + + robot = env.scene["robot"] + r_arm_ids, _ = robot.find_joints(env.action_manager.get_term("right_arm_action").cfg.joint_names) + l_arm_ids, _ = robot.find_joints(env.action_manager.get_term("left_arm_action").cfg.joint_names) + + settle_action = last_action.clone() + settle_action[0, 0] = -1.0 # negative cmd drives squat_cmd[0] down from 0.75 + settle_action[0, 1] = 0.0 + settle_action[0, 2] = 0.0 + settle_action[0, 3] = 1.0 # mode=1: squat/stance + settle_action[0, 4:11] = robot.data.joint_pos[0, r_arm_ids] + settle_action[0, 11:18] = robot.data.joint_pos[0, l_arm_ids] + + for _ in range(self.cfg.squat_settle_steps): + env.step(settle_action) + + # Diagnostic: target position & quaternion in robot root frame + import isaaclab.utils.math as PoseUtils + root_pose = robot.data.root_pose_w[0:1] # [1, 7] + root_pos_w = root_pose[:, :3] + root_quat_w = root_pose[:, 3:] + + oven = env.scene["oven_main_group"] + oven_pos = oven.data.root_pos_w[0:1] + oven_quat = oven.data.root_quat_w[0:1] + offset_world_pos, offset_world_quat = PoseUtils.combine_frame_transforms( + oven_pos, oven_quat, + torch.tensor([[-0.176, -0.990, 0.100]], device=env.device), + torch.tensor([[0.707, 0.0, 0.0, 0.707]], device=env.device), + ) + tgt_pos_root, tgt_quat_root = PoseUtils.subtract_frame_transforms( + root_pos_w, root_quat_w, offset_world_pos, offset_world_quat, + ) + print(f"[G1ActionAdapter] root_pos_w = {root_pos_w[0].cpu().numpy().round(4)}") + print(f"[G1ActionAdapter] EE target world pos = {offset_world_pos[0].cpu().numpy().round(4)}") + print(f"[G1ActionAdapter] EE target world quat = {offset_world_quat[0].cpu().numpy().round(4)}") + print(f"[G1ActionAdapter] EE target ROOT pos = {tgt_pos_root[0].cpu().numpy().round(4)}") + print(f"[G1ActionAdapter] EE target ROOT quat = {tgt_quat_root[0].cpu().numpy().round(4)}") + + # Store reach target for real-time console tracking in _apply_reach + self._dbg_reach_target_w = offset_world_pos[0].detach().cpu() + self._dbg_reach_step = 0 + + # Forward nudge (reach only): teleport robot toward the oven after squatting. + # solve_ik_batch cannot be used here (CUDA graph goal-type conflict with plan_motion). + if skill_type != "reach" or self.cfg.pos_search_nudge_count == 0: + return + + root_quat_w = robot.data.root_quat_w[0] + w_q, x_q, y_q, z_q = root_quat_w[0].item(), root_quat_w[1].item(), root_quat_w[2].item(), root_quat_w[3].item() + fwd_x = 1.0 - 2.0 * (y_q**2 + z_q**2) # cos(yaw): local +x in world frame + fwd_y = 2.0 * (w_q * z_q + x_q * y_q) # sin(yaw) + + total_nudge = self.cfg.pos_search_nudge_m * self.cfg.pos_search_nudge_count + pos = robot.data.root_pos_w[0].clone() + pos[0] += fwd_x * total_nudge + pos[1] += fwd_y * total_nudge + + pose = torch.zeros(1, 7, device=env.device) + pose[0, :3] = pos + pose[0, 3:] = root_quat_w + robot.write_root_pose_to_sim(pose) + robot.write_root_velocity_to_sim(torch.zeros(1, 6, device=env.device)) + + for _ in range(self.cfg.pos_search_settle_steps): + env.step(settle_action) + + # ------------------------------------------------------------------ + # Navigation (leg locomotion base command) + # ------------------------------------------------------------------ + + def _apply_moveto(self, skill_output: SkillOutput, env: ManagerBasedEnv) -> torch.Tensor: + """Convert world-frame velocity [vx, vy, vyaw] to virtual-base delta positions.""" + self._has_squatted = False # reset so squat fires once after next moveto vx, vy, vyaw = skill_output.action # world frame robot = env.scene["robot"] @@ -53,20 +141,14 @@ def _apply_moveto(self, skill_output: SkillOutput, env: ManagerBasedEnv) -> torc w, x, y, z = world_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) - # Rotate world-frame velocity → robot body frame vx_body = vx * cos_yaw + vy * sin_yaw vy_body = -vx * sin_yaw + vy * cos_yaw - dt_control = env.cfg.sim.dt * env.cfg.decimation - last_action = env.action_manager.action action = last_action[0, :].clone() - # LegPositionAction: cmd_raw is normalized; actual = cmd_raw * max_cmd - # max_cmd = [0.3, 0.3, 0.3] from g1_loco.yaml - _MAX_CMD = 0.3 + _MAX_CMD = 0.9 action[0] = vx_body / _MAX_CMD action[1] = vy_body / _MAX_CMD action[2] = vyaw / _MAX_CMD @@ -80,23 +162,15 @@ def _apply_moveto(self, skill_output: SkillOutput, env: ManagerBasedEnv) -> torc def _apply_reach(self, skill_output: SkillOutput, env: ManagerBasedEnv) -> torch.Tensor: """Write cuRobo joint positions into the arm action terms.""" - - target_joint_pos = skill_output.action # [N_sim_joints], full robot order - robot = env.scene["robot"] - current_joint_pos = robot.data.joint_pos[0] + target_joint_pos = skill_output.action last_action = env.action_manager.action action = last_action[0, :].clone() - # Resolve joint → robot-joint index mappings + robot = env.scene["robot"] r_arm_ids, _ = robot.find_joints(env.action_manager.get_term("right_arm_action").cfg.joint_names) l_arm_ids, _ = robot.find_joints(env.action_manager.get_term("left_arm_action").cfg.joint_names) - # G1ActionsCfg action layout: - # [0:4] base_action LegPositionAction [vx, vy, vyaw, mode] - # [4:11] right_arm (7-DoF absolute) - # [11:18] left_arm (7-DoF absolute) - # [18:32] gripper (14 joints absolute) action[0] = 0.0 action[1] = 0.0 action[2] = 0.0 @@ -104,6 +178,18 @@ def _apply_reach(self, skill_output: SkillOutput, env: ManagerBasedEnv) -> torch action[4:11] = target_joint_pos[r_arm_ids] action[11:18] = target_joint_pos[l_arm_ids] + self._dbg_reach_step += 1 + if self._dbg_reach_step % 10 == 1: + ee_ids, _ = robot.find_bodies("left_wrist_yaw_link") + ee_pos_w = robot.data.body_pos_w[0, ee_ids[0]].cpu() + ee_str = ee_pos_w.numpy().round(3).tolist() + if self._dbg_reach_target_w is not None: + dist = (ee_pos_w - self._dbg_reach_target_w).norm().item() + tgt_str = self._dbg_reach_target_w.numpy().round(3).tolist() + print(f"[reach step={self._dbg_reach_step:4d}] EE={ee_str} tgt={tgt_str} dist={dist:.4f}m") + else: + print(f"[reach step={self._dbg_reach_step:4d}] EE={ee_str}") + return action # ------------------------------------------------------------------ @@ -112,8 +198,7 @@ def _apply_reach(self, skill_output: SkillOutput, env: ManagerBasedEnv) -> torch def _apply_gripper(self, skill_output: SkillOutput, env: ManagerBasedEnv) -> torch.Tensor: """Set all finger joints to the closed (grasp) or open (ungrasp) position.""" - - gripper_signal = skill_output.action[0].item() # -1.0 = grasp, +1.0 = ungrasp + gripper_signal = skill_output.action[0].item() angles = self.cfg.finger_close_angles if gripper_signal < 0 else self.cfg.finger_open_angles finger_angles = torch.tensor(angles, dtype=torch.float32, device=env.device) @@ -127,8 +212,6 @@ def _apply_gripper(self, skill_output: SkillOutput, env: ManagerBasedEnv) -> tor robot = env.scene["robot"] finger_ids, _ = robot.find_joints(env.action_manager.get_term("gripper_action").cfg.joint_names) - - # gripper_action starts at action index 18 action[18:18 + len(finger_ids)] = finger_angles[:len(finger_ids)] return action diff --git a/lw_benchhub/autosim/action_adapters/g1_action_adapter_cfg.py b/lw_benchhub/autosim/action_adapters/g1_action_adapter_cfg.py index c059d0dc..17a69c00 100644 --- a/lw_benchhub/autosim/action_adapters/g1_action_adapter_cfg.py +++ b/lw_benchhub/autosim/action_adapters/g1_action_adapter_cfg.py @@ -26,3 +26,18 @@ class G1ActionAdapterCfg(ActionAdapterCfg): """Per-joint finger angles (rad) when gripper is closed.""" finger_open_angles: tuple = (0.0,) * 14 """Per-joint finger angles (rad) when gripper is open.""" + + squat_settle_steps: int = 40 + """Steps to squat before planning an arm skill. + 10 (loco→squat transition) + 110 × 0.0016 drop = squat_cmd[0] ≈ 0.574 (moderate crouch). + Brings shoulder from ~1.29m down to ~1.16m so arm can reach target at Z=0.72m.""" + + pos_search_nudge_m: float = 0.04 + """Metres per nudge step toward the oven after squatting.""" + + pos_search_nudge_count: int = 0 + """Number of nudge steps (total forward shift = nudge_m × count). + Set to 0 to disable nudging. solve_ik_batch is NOT used (CUDA graph conflict).""" + + pos_search_settle_steps: int = 20 + """Squat-mode settle steps after the forward nudge.""" diff --git a/lw_benchhub/autosim/content/configs/robot/g1.yml b/lw_benchhub/autosim/content/configs/robot/g1.yml index d9114035..f8b035ec 100644 --- a/lw_benchhub/autosim/content/configs/robot/g1.yml +++ b/lw_benchhub/autosim/content/configs/robot/g1.yml @@ -7,9 +7,9 @@ robot_cfg: asset_root_path: "." # meshes are resolved from the urdf file dir base_link: "world_link" - ee_link: "left_wrist_yaw_link" # left-hand end-effector - # Extra tracked links (right ee for dual-arm tasks) - link_names: ["left_wrist_yaw_link"] + ee_link: "left_hand_palm_link" + # Extra tracked links + link_names: ["left_hand_palm_link"] collision_link_names: [ @@ -18,6 +18,7 @@ robot_cfg: "left_shoulder_pitch_link", "left_shoulder_roll_link", "left_shoulder_yaw_link", "left_elbow_link", "left_wrist_roll_link", "left_wrist_pitch_link", "left_wrist_yaw_link", + "left_hand_palm_link", "right_shoulder_pitch_link", "right_shoulder_roll_link", "right_shoulder_yaw_link", "right_elbow_link", "right_wrist_roll_link", "right_wrist_pitch_link", "right_wrist_yaw_link", @@ -25,7 +26,7 @@ robot_cfg: collision_spheres: "spheres/collision_g1_autosim.yml" collision_sphere_buffer: 0.002 - extra_collision_spheres: {"left_wrist_yaw_link": 20} + extra_collision_spheres: {"left_hand_palm_link": 20} use_global_cumul: True self_collision_ignore: @@ -38,6 +39,7 @@ robot_cfg: "left_elbow_link": ["left_wrist_roll_link"], "left_wrist_roll_link": ["left_wrist_pitch_link"], "left_wrist_pitch_link": ["left_wrist_yaw_link"], + "left_wrist_yaw_link": ["left_hand_palm_link"], "right_shoulder_pitch_link": ["right_shoulder_roll_link"], "right_shoulder_roll_link": ["right_shoulder_yaw_link"], "right_shoulder_yaw_link": ["right_elbow_link"], @@ -55,6 +57,7 @@ robot_cfg: "left_shoulder_pitch_link", "left_shoulder_roll_link", "left_shoulder_yaw_link", "left_elbow_link", "left_wrist_roll_link", "left_wrist_pitch_link", "left_wrist_yaw_link", + "left_hand_palm_link", "right_shoulder_pitch_link", "right_shoulder_roll_link", "right_shoulder_yaw_link", "right_elbow_link", "right_wrist_roll_link", "right_wrist_pitch_link", "right_wrist_yaw_link", diff --git a/lw_benchhub/autosim/pipelines/close_oven.py b/lw_benchhub/autosim/pipelines/close_oven.py index f224d034..eaa064bf 100644 --- a/lw_benchhub/autosim/pipelines/close_oven.py +++ b/lw_benchhub/autosim/pipelines/close_oven.py @@ -1,7 +1,5 @@ import torch from autosim.core.pipeline import AutoSimPipeline, AutoSimPipelineCfg -from autosim.core.registration import SkillRegistry -from autosim.core.types import PipelineOutput from autosim.decomposers import LLMDecomposerCfg from isaaclab.envs import ManagerBasedEnv from isaaclab.utils import configclass @@ -35,21 +33,22 @@ def _x7s_skill_cfg(cfg) -> None: def _g1_skill_cfg(cfg) -> None: - cfg.skills.moveto.extra_cfg.local_planner.max_linear_velocity = 0.2 - cfg.skills.moveto.extra_cfg.local_planner.max_angular_velocity = 0.2 + cfg.skills.moveto.extra_cfg.local_planner.max_linear_velocity = 1.0 + cfg.skills.moveto.extra_cfg.local_planner.max_angular_velocity = 0.4 cfg.skills.moveto.extra_cfg.local_planner.predict_time = 0.4 cfg.skills.moveto.extra_cfg.global_planner.safety_distance = 0.5 cfg.skills.moveto.extra_cfg.global_planner.proximity_weight = 3.0 - cfg.skills.moveto.extra_cfg.waypoint_tolerance = 0.25 - cfg.skills.moveto.extra_cfg.goal_tolerance = 0.30 - cfg.skills.moveto.extra_cfg.yaw_tolerance = 0.01 + cfg.skills.moveto.extra_cfg.waypoint_tolerance = 0.1 + cfg.skills.moveto.extra_cfg.goal_tolerance = 0.1 + cfg.skills.moveto.extra_cfg.yaw_tolerance = 0.5 cfg.skills.moveto.extra_cfg.use_dwa = False - cfg.skills.moveto.extra_cfg.sampling_radius = 0.87 - cfg.skills.push.extra_cfg.move_offset = 0.10 + cfg.skills.moveto.extra_cfg.sampling_radius = 1.13 + cfg.skills.push.extra_cfg.move_offset = 0.36 cfg.skills.push.extra_cfg.move_axis = "+x" cfg.skills.lift.extra_cfg.move_offset = 0.15 cfg.skills.lift.extra_cfg.move_axis = "+z" - cfg.max_steps = 2000 + cfg.max_steps = 1000 + cfg.motion_planner.use_cuda_graph = False TASK_ROBOT_OVERRIDES: dict[str, TaskRobotOverride] = { @@ -67,11 +66,10 @@ def _g1_skill_cfg(cfg) -> None: "g1_loco_left": TaskRobotOverride( object_reach_target_poses={ "oven_main_group": [ - torch.tensor([-0.1687, -0.9214, -0.0407, 0.3762, 0.0, 0.0, 0.9264]), + torch.tensor([-0.176, -0.840, 0.000, 0.707, 0.0, 0.0, 0.707]), ], }, - init_state_pos_delta=(-0.21, -0.70, 0.01), - init_state_rot=(0.707, 0.0, 0.0, 0.707), + init_state_pos_delta=(-0.3, -1.2, 0.0), skill_cfg_fn=_g1_skill_cfg, ), } @@ -100,7 +98,7 @@ def __post_init__(self): resolved_robot.override.skill_cfg_fn(self) self.occupancy_map.floor_prim_suffix = "Scene/floor_room" - self.motion_planner.world_ignore_subffixes = ["Scene/floor_room"] + self.motion_planner.world_ignore_subffixes = ["Scene/floor_room", "Scene/oven_main_group/Oven032_door"] self.motion_planner.world_only_subffixes = [ "Scene/island_island_group", "Scene/island_panel_cab_right_island_group_1", @@ -147,28 +145,7 @@ def reset_env(self): self._set_door_drive(stiffness=0.0, damping=1.0, target_deg=0.0) def execute_skill_sequence(self, decompose_result): - if self._resolved_robot.profile.profile_id not in _DOOR_PROFILES: - return super().execute_skill_sequence(decompose_result) - - self._check_skill_extra_cfg() - self.reset_env() - - for subtask in decompose_result.subtasks: - for skill_info in subtask.skills: - if skill_info.skill_type == "push": - self._set_door_drive(stiffness=50.0, damping=5.0, target_deg=0.0) - skill = SkillRegistry.create( - skill_info.skill_type, self.cfg.skills.get(skill_info.skill_type).extra_cfg - ) - if self._action_adapter.should_skip_apply(skill): - continue - goal = skill.extract_goal_from_info(skill_info, self._env, self._env_extra_info) - success, steps = self._execute_single_skill(skill, goal) - if not success: - raise ValueError(f"Skill {skill_info.skill_type} failed after {steps} steps.") - - self.reset_env() - return PipelineOutput(success=True, generated_actions=self._generated_actions) + return super().execute_skill_sequence(decompose_result) def load_env(self) -> ManagerBasedEnv: import gymnasium as gym From ccce4042bf4ac8e658e24d97f7e555396d1477f3 Mon Sep 17 00:00:00 2001 From: Zihan Gao <79097743+Papaercold@users.noreply.github.com> Date: Sun, 26 Apr 2026 19:13:16 -0400 Subject: [PATCH 09/32] Delete lw_benchhub/autosim/action_adapters/g1_right_arm_only_action_adapter.py --- .../g1_right_arm_only_action_adapter.py | 84 ------------------- 1 file changed, 84 deletions(-) delete mode 100644 lw_benchhub/autosim/action_adapters/g1_right_arm_only_action_adapter.py diff --git a/lw_benchhub/autosim/action_adapters/g1_right_arm_only_action_adapter.py b/lw_benchhub/autosim/action_adapters/g1_right_arm_only_action_adapter.py deleted file mode 100644 index 5a682826..00000000 --- a/lw_benchhub/autosim/action_adapters/g1_right_arm_only_action_adapter.py +++ /dev/null @@ -1,84 +0,0 @@ -from __future__ import annotations - -from typing import TYPE_CHECKING - -import torch -from isaaclab.envs import ManagerBasedEnv - -from autosim import ActionAdapterBase -from autosim.core.types import SkillOutput - -if TYPE_CHECKING: - from .g1_right_arm_only_action_adapter_cfg import G1RightArmOnlyActionAdapterCfg - - -class G1RightArmOnlyActionAdapter(ActionAdapterBase): - """Action adapter for G1 — right arm only, legs fixed in stance mode. - - Action vector layout (G1ActionsCfg): - [0:4] base locomotion command — action[3]=1.0 keeps squat/stance - [4:11] right arm joints (7 DoF, absolute position) — planned by cuRobo - [11:18] left arm joints (7 DoF, absolute position) — locked by cuRobo - [18:32] fingers (right + left three-finger hands) - - moveto is not registered; the LLM will not generate navigation steps. - """ - - def __init__(self, cfg: G1RightArmOnlyActionAdapterCfg): - super().__init__(cfg) - self.register_apply_method("reach", self._apply_reach) - self.register_apply_method("lift", self._apply_reach) - self.register_apply_method("push", self._apply_reach) - self.register_apply_method("pull", self._apply_reach) - self.register_apply_method("grasp", self._apply_gripper) - self.register_apply_method("ungrasp", self._apply_gripper) - - # ------------------------------------------------------------------ - # Arm motion (cuRobo joint trajectory playback — left arm primary) - # ------------------------------------------------------------------ - - def _apply_reach(self, skill_output: SkillOutput, env: ManagerBasedEnv) -> torch.Tensor: - """Write cuRobo joint positions into the arm action terms. - - cuRobo plans the right arm; left arm joints come back at their locked - values and are written unchanged. - """ - target_joint_pos = skill_output.action # [N_sim_joints], full robot order - robot = env.scene["robot"] - - last_action = env.action_manager.action - action = last_action[0, :].clone() - - r_arm_ids, _ = robot.find_joints(env.action_manager.get_term("right_arm_action").cfg.joint_names) - l_arm_ids, _ = robot.find_joints(env.action_manager.get_term("left_arm_action").cfg.joint_names) - - # G1ActionsCfg layout: [vx, vy, vyaw, mode, r_arm×7, l_arm×7, gripper×14] - action[0] = 0.0 - action[1] = 0.0 - action[2] = 0.0 - action[3] = 1.0 # mode=1: squat/stance — legs fixed - - action[4:11] = target_joint_pos[r_arm_ids] # right arm — planned - action[11:18] = target_joint_pos[l_arm_ids] # left arm — locked - - return action - - def _apply_gripper(self, skill_output: SkillOutput, env: ManagerBasedEnv) -> torch.Tensor: - """Set right-hand finger joints to closed (grasp) or open (ungrasp).""" - - gripper_signal = skill_output.action[0].item() - angles = self.cfg.finger_close_angles if gripper_signal < 0 else self.cfg.finger_open_angles - finger_angles = torch.tensor(angles, dtype=torch.float32, device=env.device) - - robot = env.scene["robot"] - last_action = env.action_manager.action - action = last_action[0, :].clone() - action[0] = 0.0 - action[1] = 0.0 - action[2] = 0.0 - action[3] = 1.0 # mode=1: squat/stance - - finger_ids, _ = robot.find_joints(env.action_manager.get_term("gripper_action").cfg.joint_names) - action[18:18 + len(finger_ids)] = finger_angles[:len(finger_ids)] - - return action From 500bc3acdd7b39f7beb13b92b706aaf72217416d Mon Sep 17 00:00:00 2001 From: Zihan Gao <79097743+Papaercold@users.noreply.github.com> Date: Sun, 26 Apr 2026 19:13:27 -0400 Subject: [PATCH 10/32] Delete lw_benchhub/autosim/action_adapters/g1_right_arm_only_action_adapter_cfg.py --- .../g1_right_arm_only_action_adapter_cfg.py | 22 ------------------- 1 file changed, 22 deletions(-) delete mode 100644 lw_benchhub/autosim/action_adapters/g1_right_arm_only_action_adapter_cfg.py diff --git a/lw_benchhub/autosim/action_adapters/g1_right_arm_only_action_adapter_cfg.py b/lw_benchhub/autosim/action_adapters/g1_right_arm_only_action_adapter_cfg.py deleted file mode 100644 index 94ec7f9c..00000000 --- a/lw_benchhub/autosim/action_adapters/g1_right_arm_only_action_adapter_cfg.py +++ /dev/null @@ -1,22 +0,0 @@ -from isaaclab.utils import configclass - -from autosim import ActionAdapterCfg - -from .g1_right_arm_only_action_adapter import G1RightArmOnlyActionAdapter - - -@configclass -class G1RightArmOnlyActionAdapterCfg(ActionAdapterCfg): - """Configuration for the G1 right-arm-only action adapter (legs in stance).""" - - class_type: type = G1RightArmOnlyActionAdapter - - finger_close_angles: tuple = ( - 1.2, 1.4, # right index_0, index_1 - 1.2, 1.4, # right middle_0, middle_1 - 0.5, -0.5, -1.2, # right thumb_0, thumb_1, thumb_2 - -1.2, -1.4, # left index_0, index_1 - -1.2, -1.4, # left middle_0, middle_1 - 0.5, 0.5, 1.2, # left thumb_0, thumb_1, thumb_2 - ) - finger_open_angles: tuple = (0.0,) * 14 From 7c76124250c34c308b5ced650ecd521b5a515e2f Mon Sep 17 00:00:00 2001 From: Zihan Gao Date: Sun, 26 Apr 2026 22:19:39 -0400 Subject: [PATCH 11/32] Fix bugs in G1 CloseOven --- .../action_adapters/g1_action_adapter.py | 108 ------------------ .../action_adapters/g1_action_adapter_cfg.py | 24 +--- lw_benchhub/autosim/pipelines/close_oven.py | 40 ++++++- 3 files changed, 38 insertions(+), 134 deletions(-) diff --git a/lw_benchhub/autosim/action_adapters/g1_action_adapter.py b/lw_benchhub/autosim/action_adapters/g1_action_adapter.py index e3ff4cec..dc183ca4 100644 --- a/lw_benchhub/autosim/action_adapters/g1_action_adapter.py +++ b/lw_benchhub/autosim/action_adapters/g1_action_adapter.py @@ -28,112 +28,16 @@ def __init__(self, cfg: G1ActionAdapterCfg): self.register_apply_method("moveto", self._apply_moveto) self.register_apply_method("reach", self._apply_reach) self.register_apply_method("lift", self._apply_reach) - self.register_apply_method("push", self._apply_reach) self.register_apply_method("pull", self._apply_reach) self.register_apply_method("grasp", self._apply_gripper) self.register_apply_method("ungrasp", self._apply_gripper) - self._dbg_reach_target_w: torch.Tensor | None = None - self._dbg_reach_step: int = 0 - self._has_squatted: bool = False - - _ARM_SKILLS = frozenset({"reach", "lift", "push", "pull"}) - - # ------------------------------------------------------------------ - # Pre-skill settling: squat + optional forward nudge - # ------------------------------------------------------------------ - - def pre_skill_hook(self, skill_type: str, env: ManagerBasedEnv, last_action: torch.Tensor) -> None: - """Squat to a moderate height before any arm skill, then nudge forward. - - squat_cmd[0] starts at 0.75 (stand) and drops by -1.0 * max_cmd[0] / 250 = -0.0016 - per env.step. With squat_settle_steps=80: 10 transition + 70 steps → squat_cmd[0] ≈ 0.64 - (a slight crouch, ~10 cm pelvis drop). - - After squatting, for the reach skill the robot is teleported forward along its heading - by pos_search_nudge_m * pos_search_nudge_count so it is closer to the target. - solve_ik_batch is intentionally NOT used here because it conflicts with plan_motion's - CUDA graph on CUDA < 12.0 (changing goal type error). - """ - if skill_type not in self._ARM_SKILLS: - return - if self._has_squatted: - return - self._has_squatted = True - - robot = env.scene["robot"] - r_arm_ids, _ = robot.find_joints(env.action_manager.get_term("right_arm_action").cfg.joint_names) - l_arm_ids, _ = robot.find_joints(env.action_manager.get_term("left_arm_action").cfg.joint_names) - - settle_action = last_action.clone() - settle_action[0, 0] = -1.0 # negative cmd drives squat_cmd[0] down from 0.75 - settle_action[0, 1] = 0.0 - settle_action[0, 2] = 0.0 - settle_action[0, 3] = 1.0 # mode=1: squat/stance - settle_action[0, 4:11] = robot.data.joint_pos[0, r_arm_ids] - settle_action[0, 11:18] = robot.data.joint_pos[0, l_arm_ids] - - for _ in range(self.cfg.squat_settle_steps): - env.step(settle_action) - - # Diagnostic: target position & quaternion in robot root frame - import isaaclab.utils.math as PoseUtils - root_pose = robot.data.root_pose_w[0:1] # [1, 7] - root_pos_w = root_pose[:, :3] - root_quat_w = root_pose[:, 3:] - - oven = env.scene["oven_main_group"] - oven_pos = oven.data.root_pos_w[0:1] - oven_quat = oven.data.root_quat_w[0:1] - offset_world_pos, offset_world_quat = PoseUtils.combine_frame_transforms( - oven_pos, oven_quat, - torch.tensor([[-0.176, -0.990, 0.100]], device=env.device), - torch.tensor([[0.707, 0.0, 0.0, 0.707]], device=env.device), - ) - tgt_pos_root, tgt_quat_root = PoseUtils.subtract_frame_transforms( - root_pos_w, root_quat_w, offset_world_pos, offset_world_quat, - ) - print(f"[G1ActionAdapter] root_pos_w = {root_pos_w[0].cpu().numpy().round(4)}") - print(f"[G1ActionAdapter] EE target world pos = {offset_world_pos[0].cpu().numpy().round(4)}") - print(f"[G1ActionAdapter] EE target world quat = {offset_world_quat[0].cpu().numpy().round(4)}") - print(f"[G1ActionAdapter] EE target ROOT pos = {tgt_pos_root[0].cpu().numpy().round(4)}") - print(f"[G1ActionAdapter] EE target ROOT quat = {tgt_quat_root[0].cpu().numpy().round(4)}") - - # Store reach target for real-time console tracking in _apply_reach - self._dbg_reach_target_w = offset_world_pos[0].detach().cpu() - self._dbg_reach_step = 0 - - # Forward nudge (reach only): teleport robot toward the oven after squatting. - # solve_ik_batch cannot be used here (CUDA graph goal-type conflict with plan_motion). - if skill_type != "reach" or self.cfg.pos_search_nudge_count == 0: - return - - root_quat_w = robot.data.root_quat_w[0] - w_q, x_q, y_q, z_q = root_quat_w[0].item(), root_quat_w[1].item(), root_quat_w[2].item(), root_quat_w[3].item() - fwd_x = 1.0 - 2.0 * (y_q**2 + z_q**2) # cos(yaw): local +x in world frame - fwd_y = 2.0 * (w_q * z_q + x_q * y_q) # sin(yaw) - - total_nudge = self.cfg.pos_search_nudge_m * self.cfg.pos_search_nudge_count - pos = robot.data.root_pos_w[0].clone() - pos[0] += fwd_x * total_nudge - pos[1] += fwd_y * total_nudge - - pose = torch.zeros(1, 7, device=env.device) - pose[0, :3] = pos - pose[0, 3:] = root_quat_w - robot.write_root_pose_to_sim(pose) - robot.write_root_velocity_to_sim(torch.zeros(1, 6, device=env.device)) - - for _ in range(self.cfg.pos_search_settle_steps): - env.step(settle_action) - # ------------------------------------------------------------------ # Navigation (leg locomotion base command) # ------------------------------------------------------------------ def _apply_moveto(self, skill_output: SkillOutput, env: ManagerBasedEnv) -> torch.Tensor: """Convert world-frame velocity [vx, vy, vyaw] to virtual-base delta positions.""" - self._has_squatted = False # reset so squat fires once after next moveto vx, vy, vyaw = skill_output.action # world frame robot = env.scene["robot"] @@ -178,18 +82,6 @@ def _apply_reach(self, skill_output: SkillOutput, env: ManagerBasedEnv) -> torch action[4:11] = target_joint_pos[r_arm_ids] action[11:18] = target_joint_pos[l_arm_ids] - self._dbg_reach_step += 1 - if self._dbg_reach_step % 10 == 1: - ee_ids, _ = robot.find_bodies("left_wrist_yaw_link") - ee_pos_w = robot.data.body_pos_w[0, ee_ids[0]].cpu() - ee_str = ee_pos_w.numpy().round(3).tolist() - if self._dbg_reach_target_w is not None: - dist = (ee_pos_w - self._dbg_reach_target_w).norm().item() - tgt_str = self._dbg_reach_target_w.numpy().round(3).tolist() - print(f"[reach step={self._dbg_reach_step:4d}] EE={ee_str} tgt={tgt_str} dist={dist:.4f}m") - else: - print(f"[reach step={self._dbg_reach_step:4d}] EE={ee_str}") - return action # ------------------------------------------------------------------ diff --git a/lw_benchhub/autosim/action_adapters/g1_action_adapter_cfg.py b/lw_benchhub/autosim/action_adapters/g1_action_adapter_cfg.py index 17a69c00..4bb1e310 100644 --- a/lw_benchhub/autosim/action_adapters/g1_action_adapter_cfg.py +++ b/lw_benchhub/autosim/action_adapters/g1_action_adapter_cfg.py @@ -15,29 +15,7 @@ class G1ActionAdapterCfg(ActionAdapterCfg): base_y_joint_name: str = "base_y_joint" base_yaw_joint_name: str = "base_yaw_joint" - finger_close_angles: tuple = ( - # right hand stays open (left arm is the grasping arm) - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - # left hand closes - -1.9, -2.0, # left index_0, index_1 - -1.9, -2.0, # left middle_0, middle_1 - 0.8, 0.8, 1.8, # left thumb_0, thumb_1, thumb_2 - ) + finger_close_angles: tuple = (0.0,) * 14 """Per-joint finger angles (rad) when gripper is closed.""" finger_open_angles: tuple = (0.0,) * 14 """Per-joint finger angles (rad) when gripper is open.""" - - squat_settle_steps: int = 40 - """Steps to squat before planning an arm skill. - 10 (loco→squat transition) + 110 × 0.0016 drop = squat_cmd[0] ≈ 0.574 (moderate crouch). - Brings shoulder from ~1.29m down to ~1.16m so arm can reach target at Z=0.72m.""" - - pos_search_nudge_m: float = 0.04 - """Metres per nudge step toward the oven after squatting.""" - - pos_search_nudge_count: int = 0 - """Number of nudge steps (total forward shift = nudge_m × count). - Set to 0 to disable nudging. solve_ik_batch is NOT used (CUDA graph conflict).""" - - pos_search_settle_steps: int = 20 - """Squat-mode settle steps after the forward nudge.""" diff --git a/lw_benchhub/autosim/pipelines/close_oven.py b/lw_benchhub/autosim/pipelines/close_oven.py index eaa064bf..4a643426 100644 --- a/lw_benchhub/autosim/pipelines/close_oven.py +++ b/lw_benchhub/autosim/pipelines/close_oven.py @@ -49,6 +49,7 @@ def _g1_skill_cfg(cfg) -> None: cfg.skills.lift.extra_cfg.move_axis = "+z" cfg.max_steps = 1000 cfg.motion_planner.use_cuda_graph = False + cfg.action_adapter.squat_settle_steps = 0 TASK_ROBOT_OVERRIDES: dict[str, TaskRobotOverride] = { @@ -66,7 +67,7 @@ def _g1_skill_cfg(cfg) -> None: "g1_loco_left": TaskRobotOverride( object_reach_target_poses={ "oven_main_group": [ - torch.tensor([-0.176, -0.840, 0.000, 0.707, 0.0, 0.0, 0.707]), + torch.tensor([-0.176, -0.840, 0.010, 0.707, 0.0, 0.0, 0.707]), ], }, init_state_pos_delta=(-0.3, -1.2, 0.0), @@ -109,6 +110,8 @@ def __post_init__(self): _DOOR_PROFILES = {"g1_loco_left"} _DOOR_JOINT_PATH = "/World/envs/env_0/Scene/oven_main_group/Oven032_door/door_joint" +_G1_PUSH_STEPS = 100 # env steps to walk forward during push; tune if needed +_G1_PUSH_FWD_CMD = 0.5 # normalized body-frame vx command (0–1) @configclass @@ -144,8 +147,39 @@ def reset_env(self): self._env.cfg.isaaclab_arena_env.task._setup_scene(self._env) self._set_door_drive(stiffness=0.0, damping=1.0, target_deg=0.0) - def execute_skill_sequence(self, decompose_result): - return super().execute_skill_sequence(decompose_result) + def _execute_single_skill(self, skill, goal): + if (self._resolved_robot.profile.profile_id in _DOOR_PROFILES + and skill.cfg.name == "push"): + return self._execute_g1_push() + return super()._execute_single_skill(skill, goal) + + def _execute_g1_push(self) -> tuple[bool, int]: + """Walk the G1 forward for a fixed number of steps with arm joints frozen.""" + robot = self._env.scene["robot"] + r_arm_ids, _ = robot.find_joints( + self._env.action_manager.get_term("right_arm_action").cfg.joint_names + ) + l_arm_ids, _ = robot.find_joints( + self._env.action_manager.get_term("left_arm_action").cfg.joint_names + ) + frozen = robot.data.joint_pos[self._env_id].clone() + + adapter_result = self._last_action[self._env_id].clone() + adapter_result[0] = _G1_PUSH_FWD_CMD + adapter_result[1] = 0.0 + adapter_result[2] = 0.0 + adapter_result[3] = 0.0 # mode=0: locomotion + adapter_result[4:11] = frozen[r_arm_ids] + adapter_result[11:18] = frozen[l_arm_ids] + + for _ in range(_G1_PUSH_STEPS): + action = self._last_action.clone() + action[self._env_id, : adapter_result.shape[0]] = adapter_result + self._env.step(action) + self._last_action = action + self._generated_actions.append(action) + + return True, _G1_PUSH_STEPS def load_env(self) -> ManagerBasedEnv: import gymnasium as gym From 5ba91141bef774fa8da01f8998b2498e5528cd21 Mon Sep 17 00:00:00 2001 From: Zihan Gao Date: Sun, 3 May 2026 18:14:12 +0800 Subject: [PATCH 12/32] complete open fridge. --- lw_benchhub/autosim/__init__.py | 10 +- .../content/assets/robot/g1/urdf/G1.urdf | 15 ++ .../content/configs/robot/g1_right.yml | 122 +++++++++++ lw_benchhub/autosim/pipelines/cheesy_bread.py | 4 +- .../autosim/pipelines/coffee_setup_mug.py | 4 +- .../autosim/pipelines/dessert_upgrade.py | 4 +- .../autosim/pipelines/kettle_boiling.py | 11 +- lw_benchhub/autosim/pipelines/open_fridge.py | 47 +++- lw_benchhub/autosim/robot_profiles.py | 14 ++ .../scripts/autosim/compare_ee_targets.py | 102 +++++++++ .../scripts/autosim/reach_plan_sweep.py | 1 + .../scripts/autosim/robot_distance_sweep.py | 200 ++++++++++++++++++ .../autosim/robot_position_2d_sweep.py | 168 +++++++++++++++ lw_benchhub/scripts/autosim/z_target_sweep.py | 152 +++++++++++++ 14 files changed, 829 insertions(+), 25 deletions(-) create mode 100644 lw_benchhub/autosim/content/configs/robot/g1_right.yml create mode 100644 lw_benchhub/scripts/autosim/compare_ee_targets.py create mode 100644 lw_benchhub/scripts/autosim/robot_distance_sweep.py create mode 100644 lw_benchhub/scripts/autosim/robot_position_2d_sweep.py create mode 100644 lw_benchhub/scripts/autosim/z_target_sweep.py diff --git a/lw_benchhub/autosim/__init__.py b/lw_benchhub/autosim/__init__.py index 0e765b60..17df6443 100644 --- a/lw_benchhub/autosim/__init__.py +++ b/lw_benchhub/autosim/__init__.py @@ -39,9 +39,9 @@ # G1 pipelines register_pipeline( - id="LWBenchhub-Autosim-G1OpenFridgePipeline-v0", + id="LWBenchhub-Autosim-G1LeftOpenFridgePipeline-v0", entry_point=f"{__name__}.pipelines.open_fridge:OpenFridgePipeline", - cfg_entry_point=f"{__name__}.pipelines.open_fridge:G1OpenFridgePipelineCfg", + cfg_entry_point=f"{__name__}.pipelines.open_fridge:G1LeftOpenFridgePipelineCfg", ) register_pipeline( @@ -73,3 +73,9 @@ entry_point=f"{__name__}.pipelines.close_oven:CloseOvenPipeline", cfg_entry_point=f"{__name__}.pipelines.close_oven:G1CloseOvenPipelineCfg", ) + +register_pipeline( + id="LWBenchhub-Autosim-G1RightOpenFridgePipeline-v0", + entry_point=f"{__name__}.pipelines.open_fridge:OpenFridgePipeline", + cfg_entry_point=f"{__name__}.pipelines.open_fridge:G1RightOpenFridgePipelineCfg", +) diff --git a/lw_benchhub/autosim/content/assets/robot/g1/urdf/G1.urdf b/lw_benchhub/autosim/content/assets/robot/g1/urdf/G1.urdf index fdba358c..de7dbf8d 100644 --- a/lw_benchhub/autosim/content/assets/robot/g1/urdf/G1.urdf +++ b/lw_benchhub/autosim/content/assets/robot/g1/urdf/G1.urdf @@ -1549,4 +1549,19 @@ + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/lw_benchhub/autosim/content/configs/robot/g1_right.yml b/lw_benchhub/autosim/content/configs/robot/g1_right.yml new file mode 100644 index 00000000..8e289e74 --- /dev/null +++ b/lw_benchhub/autosim/content/configs/robot/g1_right.yml @@ -0,0 +1,122 @@ +robot_cfg: + + kinematics: + use_usd_kinematics: False + usd_robot_root: "/g1_autosim" + urdf_path: "urdf/G1.urdf" + asset_root_path: "." # meshes are resolved from the urdf file dir + + base_link: "world_link" + ee_link: "right_hand_palm_link" + # Extra tracked links + link_names: ["right_hand_palm_link"] + + collision_link_names: + [ + "pelvis", + "torso_link", + "left_shoulder_pitch_link", "left_shoulder_roll_link", "left_shoulder_yaw_link", + "left_elbow_link", + "left_wrist_roll_link", "left_wrist_pitch_link", "left_wrist_yaw_link", + "right_hand_palm_link", + "right_shoulder_pitch_link", "right_shoulder_roll_link", "right_shoulder_yaw_link", + "right_elbow_link", + "right_wrist_roll_link", "right_wrist_pitch_link", "right_wrist_yaw_link", + ] + + collision_spheres: "spheres/collision_g1_autosim.yml" + collision_sphere_buffer: 0.002 + extra_collision_spheres: {"right_hand_palm_link": 20} + use_global_cumul: True + + self_collision_ignore: + { + "pelvis": ["torso_link", "left_shoulder_pitch_link", "right_shoulder_pitch_link"], + "torso_link": ["left_shoulder_pitch_link", "right_shoulder_pitch_link"], + "left_shoulder_pitch_link": ["left_shoulder_roll_link"], + "left_shoulder_roll_link": ["left_shoulder_yaw_link"], + "left_shoulder_yaw_link": ["left_elbow_link"], + "left_elbow_link": ["left_wrist_roll_link"], + "left_wrist_roll_link": ["left_wrist_pitch_link"], + "left_wrist_pitch_link": ["left_wrist_yaw_link"], + "left_wrist_yaw_link": ["right_hand_palm_link"], + "right_shoulder_pitch_link": ["right_shoulder_roll_link"], + "right_shoulder_roll_link": ["right_shoulder_yaw_link"], + "right_shoulder_yaw_link": ["right_elbow_link"], + "right_elbow_link": ["right_wrist_roll_link"], + "right_wrist_roll_link": ["right_wrist_pitch_link"], + "right_wrist_pitch_link": ["right_wrist_yaw_link"], + } + + self_collision_buffer: {} + + mesh_link_names: + [ + "pelvis", + "torso_link", + "left_shoulder_pitch_link", "left_shoulder_roll_link", "left_shoulder_yaw_link", + "left_elbow_link", + "left_wrist_roll_link", "left_wrist_pitch_link", "left_wrist_yaw_link", + "right_hand_palm_link", + "right_shoulder_pitch_link", "right_shoulder_roll_link", "right_shoulder_yaw_link", + "right_elbow_link", + "right_wrist_roll_link", "right_wrist_pitch_link", "right_wrist_yaw_link", + ] + + # Joints that are fixed during planning. + # Right arm is locked; left arm is planned. + lock_joints: + { + # Virtual base joints + "base_x_joint": 0.0, + "base_y_joint": 0.0, + "base_yaw_joint": 0.0, + # Waist + "waist_yaw_joint": 0.0, + "waist_roll_joint": 0.0, + "waist_pitch_joint": 0.0, + # Right arm (locked; left arm is planned) + "right_shoulder_pitch_joint": 0.0, + "right_shoulder_roll_joint": -0.3, + "right_shoulder_yaw_joint": 0.0, + "right_elbow_joint": 0.0, + "right_wrist_roll_joint": 0.0, + "right_wrist_pitch_joint": 0.0, + "right_wrist_yaw_joint": 0.0, + } + + extra_links: null + + cspace: + joint_names: + [ + # Left arm (7 DoF) — virtual base joints locked, not in cspace + "left_shoulder_pitch_joint", + "left_shoulder_roll_joint", + "left_shoulder_yaw_joint", + "left_elbow_joint", + "left_wrist_roll_joint", + "left_wrist_pitch_joint", + "left_wrist_yaw_joint", + ] + + retract_config: + [ + 0.0, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0, # left arm + ] + + null_space_weight: + [ + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, + ] + + cspace_distance_weight: + [ + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, + ] + + max_jerk: 500.0 + max_acceleration: 500.0 + +planner: + frame_bias: [0.0, 0.0, 0.0] diff --git a/lw_benchhub/autosim/pipelines/cheesy_bread.py b/lw_benchhub/autosim/pipelines/cheesy_bread.py index 46c1db26..913cfefc 100644 --- a/lw_benchhub/autosim/pipelines/cheesy_bread.py +++ b/lw_benchhub/autosim/pipelines/cheesy_bread.py @@ -27,8 +27,8 @@ def _x7s_skill_cfg(cfg) -> None: def _g1_skill_cfg(cfg) -> None: - cfg.skills.moveto.extra_cfg.local_planner.max_linear_velocity = 0.2 - cfg.skills.moveto.extra_cfg.local_planner.max_angular_velocity = 0.2 + cfg.skills.moveto.extra_cfg.local_planner.max_linear_velocity = 1.0 + cfg.skills.moveto.extra_cfg.local_planner.max_angular_velocity = 0.4 cfg.skills.moveto.extra_cfg.local_planner.predict_time = 0.4 cfg.skills.moveto.extra_cfg.global_planner.safety_distance = 0.5 cfg.skills.moveto.extra_cfg.global_planner.proximity_weight = 3.0 diff --git a/lw_benchhub/autosim/pipelines/coffee_setup_mug.py b/lw_benchhub/autosim/pipelines/coffee_setup_mug.py index 13657f38..ccdd3b12 100644 --- a/lw_benchhub/autosim/pipelines/coffee_setup_mug.py +++ b/lw_benchhub/autosim/pipelines/coffee_setup_mug.py @@ -28,8 +28,8 @@ def _x7s_skill_cfg(cfg) -> None: def _g1_skill_cfg(cfg) -> None: - cfg.skills.moveto.extra_cfg.local_planner.max_linear_velocity = 0.2 - cfg.skills.moveto.extra_cfg.local_planner.max_angular_velocity = 0.2 + cfg.skills.moveto.extra_cfg.local_planner.max_linear_velocity = 1.0 + cfg.skills.moveto.extra_cfg.local_planner.max_angular_velocity = 0.4 cfg.skills.moveto.extra_cfg.local_planner.predict_time = 0.4 cfg.skills.moveto.extra_cfg.global_planner.safety_distance = 0.5 cfg.skills.moveto.extra_cfg.global_planner.proximity_weight = 3.0 diff --git a/lw_benchhub/autosim/pipelines/dessert_upgrade.py b/lw_benchhub/autosim/pipelines/dessert_upgrade.py index bb06078c..aba38153 100644 --- a/lw_benchhub/autosim/pipelines/dessert_upgrade.py +++ b/lw_benchhub/autosim/pipelines/dessert_upgrade.py @@ -26,8 +26,8 @@ def _x7s_skill_cfg(cfg) -> None: def _g1_skill_cfg(cfg) -> None: - cfg.skills.moveto.extra_cfg.local_planner.max_linear_velocity = 0.2 - cfg.skills.moveto.extra_cfg.local_planner.max_angular_velocity = 0.2 + cfg.skills.moveto.extra_cfg.local_planner.max_linear_velocity = 1.0 + cfg.skills.moveto.extra_cfg.local_planner.max_angular_velocity = 0.4 cfg.skills.moveto.extra_cfg.local_planner.predict_time = 0.4 cfg.skills.moveto.extra_cfg.global_planner.safety_distance = 0.5 cfg.skills.moveto.extra_cfg.global_planner.proximity_weight = 3.0 diff --git a/lw_benchhub/autosim/pipelines/kettle_boiling.py b/lw_benchhub/autosim/pipelines/kettle_boiling.py index 26a129f9..c8c1f24b 100644 --- a/lw_benchhub/autosim/pipelines/kettle_boiling.py +++ b/lw_benchhub/autosim/pipelines/kettle_boiling.py @@ -28,8 +28,8 @@ def _x7s_skill_cfg(cfg) -> None: def _g1_skill_cfg(cfg) -> None: - cfg.skills.moveto.extra_cfg.local_planner.max_linear_velocity = 0.2 - cfg.skills.moveto.extra_cfg.local_planner.max_angular_velocity = 0.2 + cfg.skills.moveto.extra_cfg.local_planner.max_linear_velocity = 1.0 + cfg.skills.moveto.extra_cfg.local_planner.max_angular_velocity = 0.4 cfg.skills.moveto.extra_cfg.local_planner.predict_time = 0.4 cfg.skills.moveto.extra_cfg.global_planner.safety_distance = 0.5 cfg.skills.moveto.extra_cfg.global_planner.proximity_weight = 3.0 @@ -37,7 +37,7 @@ def _g1_skill_cfg(cfg) -> None: cfg.skills.moveto.extra_cfg.goal_tolerance = 0.30 cfg.skills.moveto.extra_cfg.yaw_tolerance = 0.01 cfg.skills.moveto.extra_cfg.use_dwa = False - cfg.skills.moveto.extra_cfg.per_object_sampling_radius = {"obj": 0.5, "stovetop_main_group": 0.30} + cfg.skills.moveto.extra_cfg.per_object_sampling_radius = {"obj": 0.60, "stovetop_main_group": 0.30} def _x7s_get_obj_cfgs(self): @@ -121,7 +121,7 @@ def _g1_after_env_created(pipeline, env) -> None: "g1_loco_left": TaskRobotOverride( object_reach_target_poses={ "obj": [ - torch.tensor([0.0, 0.18, 0.06, 0.866, 0.0, 0.0, -0.5]), + torch.tensor([0.0, 0.0, 0.05, 0.707, 0.0, 0.707, 0.0]), ], "stovetop_main_group": [ torch.tensor([0.0, -0.15, 0.20, 1.0, 0.0, 0.0, 0.0]), @@ -129,8 +129,7 @@ def _g1_after_env_created(pipeline, env) -> None: }, init_state_pos_delta=(0.0, -0.8, 0.01), skill_cfg_fn=_g1_skill_cfg, - get_obj_cfgs_fn=_g1_get_obj_cfgs, - reset_env_fn=_g1_reset_env, + get_obj_cfgs_fn=_x7s_get_obj_cfgs, after_env_created_fn=_g1_after_env_created, ), } diff --git a/lw_benchhub/autosim/pipelines/open_fridge.py b/lw_benchhub/autosim/pipelines/open_fridge.py index 59232d9d..bc9f7dc6 100644 --- a/lw_benchhub/autosim/pipelines/open_fridge.py +++ b/lw_benchhub/autosim/pipelines/open_fridge.py @@ -29,16 +29,18 @@ def _x7s_skill_cfg(cfg) -> None: def _g1_skill_cfg(cfg) -> None: - cfg.skills.moveto.extra_cfg.local_planner.max_linear_velocity = 0.2 - cfg.skills.moveto.extra_cfg.local_planner.max_angular_velocity = 0.2 + cfg.skills.moveto.extra_cfg.local_planner.max_linear_velocity = 1.0 + cfg.skills.moveto.extra_cfg.local_planner.max_angular_velocity = 0.4 cfg.skills.moveto.extra_cfg.local_planner.predict_time = 0.4 - cfg.skills.moveto.extra_cfg.global_planner.safety_distance = 0.5 + cfg.skills.moveto.extra_cfg.global_planner.safety_distance = 0.3 cfg.skills.moveto.extra_cfg.global_planner.proximity_weight = 3.0 - cfg.skills.moveto.extra_cfg.waypoint_tolerance = 0.25 - cfg.skills.moveto.extra_cfg.goal_tolerance = 0.30 - cfg.skills.moveto.extra_cfg.yaw_tolerance = 0.01 + cfg.skills.moveto.extra_cfg.waypoint_tolerance = 0.1 + cfg.skills.moveto.extra_cfg.goal_tolerance = 0.1 + cfg.skills.moveto.extra_cfg.yaw_tolerance = 0.2 cfg.skills.moveto.extra_cfg.use_dwa = False - cfg.skills.moveto.extra_cfg.sampling_radius = 1.0 + cfg.skills.moveto.extra_cfg.sampling_radius = 0.7 + cfg.max_steps = 1000 + cfg.action_adapter.squat_settle_steps = 0 TASK_ROBOT_OVERRIDES: dict[str, TaskRobotOverride] = { @@ -54,10 +56,19 @@ def _g1_skill_cfg(cfg) -> None: "g1_loco_left": TaskRobotOverride( object_reach_target_poses={ "fridge_main_group": [ - torch.tensor([0.05, -0.35, 0.10, 0.707, 0.0, 0.0, 0.707]), + torch.tensor([0.047, -0.429, 0.25, 0.707, 0.0, 0.0, 0.707]), ], }, - init_state_pos_delta=(-0.45, -0.7, 0.0), + init_state_pos_delta=(0.0, -1.5, 0.0), + skill_cfg_fn=_g1_skill_cfg, + ), + "g1_loco_right": TaskRobotOverride( + object_reach_target_poses={ + "fridge_main_group": [ + torch.tensor([0.047, -0.429, 0.125, 0.707, 0.0, 0.0, 0.707]), + ], + }, + init_state_pos_delta=(-0.15, -1.0, 0.0), skill_cfg_fn=_g1_skill_cfg, ), } @@ -85,7 +96,7 @@ def __post_init__(self): if resolved_robot.override.skill_cfg_fn: resolved_robot.override.skill_cfg_fn(self) - self.skills.pull.extra_cfg.move_offset = 0.3 + self.skills.pull.extra_cfg.move_offset = 0.1 self.skills.pull.extra_cfg.move_axis = "-x" self.occupancy_map.floor_prim_suffix = "Scene/floor_room" @@ -96,10 +107,15 @@ def __post_init__(self): @configclass -class G1OpenFridgePipelineCfg(OpenFridgePipelineCfg): +class G1LeftOpenFridgePipelineCfg(OpenFridgePipelineCfg): robot_profile: str = "g1_loco_left" +@configclass +class G1RightOpenFridgePipelineCfg(OpenFridgePipelineCfg): + robot_profile: str = "g1_loco_right" + + class OpenFridgePipeline(AutoSimPipeline): def __init__(self, cfg: AutoSimPipelineCfg): self._resolved_robot = resolve_robot_settings( @@ -107,6 +123,15 @@ def __init__(self, cfg: AutoSimPipelineCfg): ) super().__init__(cfg) + def _execute_single_skill(self, skill, goal): + success, steps = super()._execute_single_skill(skill, goal) + if skill.cfg.name == "moveto": + robot = self._env.scene["robot"] + pos = robot.data.root_pos_w[0].cpu().numpy() + quat = robot.data.root_quat_w[0].cpu().numpy() + print(f"[DEBUG] Robot position after moveto: pos={pos.round(3)}, quat={quat.round(3)}") + return success, steps + def load_env(self) -> ManagerBasedEnv: import gymnasium as gym from lw_benchhub.utils.env import ExecuteMode, parse_env_cfg diff --git a/lw_benchhub/autosim/robot_profiles.py b/lw_benchhub/autosim/robot_profiles.py index 5eaf7ddf..23929875 100644 --- a/lw_benchhub/autosim/robot_profiles.py +++ b/lw_benchhub/autosim/robot_profiles.py @@ -46,6 +46,7 @@ class TaskRobotOverride: """Reach target mode for the selected robot and task combination.""" init_state_pos_delta: tuple[float, float, float] | None = None init_state_rot: tuple[float, float, float, float] | None = None + init_state_joint_pos: dict[str, float] | None = None skill_cfg_fn: Callable | None = None get_obj_cfgs_fn: Callable | None = None after_env_created_fn: Callable | None = None @@ -116,6 +117,17 @@ def _make_g1_action_adapter(): self_collision_check=False, env_cfg_setup_fn=_setup_g1_env_cfg, ), + "g1_loco_right": RobotProfile( + profile_id="g1_loco_right", + robot_name="G1-Loco-Controller", + action_adapter_factory=_make_g1_action_adapter, + motion_planner_robot_config_file="g1_right.yml", + robot_base_link_name="pelvis", + ee_link_name="right_wrist_yaw_link", + curobo_asset_path=str(AUTOSIM_CONTENT_ROOT / "assets" / "robot" / "g1"), + self_collision_check=False, + env_cfg_setup_fn=_setup_g1_env_cfg, + ), } @@ -173,6 +185,8 @@ def apply_robot_env_cfg(env_cfg, resolved_robot: ResolvedRobotSettings) -> None: env_cfg.scene.robot.init_state.pos[2] += dz if override.init_state_rot is not None: env_cfg.scene.robot.init_state.rot = override.init_state_rot + if override.init_state_joint_pos is not None: + env_cfg.scene.robot.init_state.joint_pos.update(override.init_state_joint_pos) def build_env_extra_info( diff --git a/lw_benchhub/scripts/autosim/compare_ee_targets.py b/lw_benchhub/scripts/autosim/compare_ee_targets.py new file mode 100644 index 00000000..0169b4d2 --- /dev/null +++ b/lw_benchhub/scripts/autosim/compare_ee_targets.py @@ -0,0 +1,102 @@ +"""Compare world-frame EE reach targets between X7S and G1 in the CloseOven scene. + +Loads a single pipeline, reads the oven's world pose, then computes the EE world +target for both robot profiles using their oven-frame offsets and prints the +difference. Only one Isaac Sim instance is needed. + +Usage: + python compare_ee_targets.py \ + --pipeline_id LWBenchhub-Autosim-CloseOvenPipeline-v0 \ + --headless + # or use the G1 pipeline — same scene, same oven pose + python compare_ee_targets.py \ + --pipeline_id LWBenchhub-Autosim-G1CloseOvenPipeline-v0 \ + --headless +""" + +import argparse + +from isaaclab.app import AppLauncher + +parser = argparse.ArgumentParser(description="Compare X7S and G1 EE world targets in CloseOven scene.") +parser.add_argument("--pipeline_id", required=True, type=str) +AppLauncher.add_app_launcher_args(parser) +args_cli = parser.parse_args() +app_launcher = AppLauncher(vars(args_cli)) +simulation_app = app_launcher.app + +import torch +from isaaclab.utils.math import combine_frame_transforms + +import lw_benchhub.autosim # noqa: F401 +from autosim import make_pipeline + + +# Oven-frame EE offsets as defined in close_oven.py TASK_ROBOT_OVERRIDES +_OFFSETS = { + "x7s_joint_left": torch.tensor([-0.176, -0.739, -0.180, 0.707, -0.00, -0.00, 0.707]), + "g1_loco_left": torch.tensor([-0.176, -0.739, -0.180, 0.707, 0.00, 0.00, 0.707]), +} + + +def main(): + pipeline = make_pipeline(args_cli.pipeline_id) + pipeline.cfg.motion_planner.use_cuda_graph = False + pipeline.initialize() + + env = pipeline._env + extra_info = pipeline._env_extra_info + + object_name = list(extra_info.object_reach_target_poses.keys())[0] + obj = env.scene[object_name] + obj_pos_w = obj.data.root_pos_w[0] # [3] + obj_quat_w = obj.data.root_quat_w[0] # [w, x, y, z] + + # Print robot actual spawn position (after delta applied) + robot = env.scene["robot"] + robot_pos_w = robot.data.root_pos_w[0].cpu().numpy().round(4) + print(f"\nRobot actual spawn pos (world): {robot_pos_w}") + print(f"Object '{object_name}'") + print(f" world pos : {obj_pos_w.cpu().numpy().round(4)}") + print(f" world quat : {obj_quat_w.cpu().numpy().round(4)} (w,x,y,z)") + y_dist = abs(float(robot_pos_w[1]) - float(obj_pos_w[1].item())) + x_dist = float(robot_pos_w[0]) - float(obj_pos_w[0].item()) + print(f" Robot->Oven Y distance: {y_dist:.3f} m (sweep d value)") + print(f" Robot->Oven X offset: {x_dist:.3f} m\n") + + results = {} + for profile, offset in _OFFSETS.items(): + off_pos = offset[:3].unsqueeze(0) + off_quat = offset[3:].unsqueeze(0) + + ee_pos_w, ee_quat_w = combine_frame_transforms( + obj_pos_w.unsqueeze(0), obj_quat_w.unsqueeze(0), + off_pos, off_quat, + ) + results[profile] = (ee_pos_w[0], ee_quat_w[0]) + + print(f"[{profile}]") + print(f" oven-frame offset pos : {offset[:3].numpy().round(4)}") + print(f" oven-frame offset quat : {offset[3:].numpy().round(4)} (w,x,y,z)") + print(f" => EE world pos : {ee_pos_w[0].cpu().numpy().round(4)}") + print(f" => EE world quat : {ee_quat_w[0].cpu().numpy().round(4)} (w,x,y,z)\n") + + x7s_pos, x7s_quat = results["x7s_joint_left"] + g1_pos, g1_quat = results["g1_loco_left"] + + pos_diff = (x7s_pos - g1_pos).abs().max().item() + quat_diff = (x7s_quat - g1_quat).abs().max().item() + + print(f"Max pos difference (X7S - G1): {pos_diff:.6f} m") + print(f"Max quat difference (X7S - G1): {quat_diff:.6f}") + + if pos_diff < 1e-4: + print("=> EE world POSITIONS are identical.") + else: + print("=> EE world positions DIFFER — oven may be placed differently per profile.") + + simulation_app.close() + + +if __name__ == "__main__": + main() diff --git a/lw_benchhub/scripts/autosim/reach_plan_sweep.py b/lw_benchhub/scripts/autosim/reach_plan_sweep.py index ecc78825..6cbeb0b8 100644 --- a/lw_benchhub/scripts/autosim/reach_plan_sweep.py +++ b/lw_benchhub/scripts/autosim/reach_plan_sweep.py @@ -44,6 +44,7 @@ def main() -> None: pipeline = make_pipeline(args_cli.pipeline_id) + pipeline.cfg.motion_planner.use_cuda_graph = False reach_plan_sweep( pipeline, ReachPlanSweepCfg( diff --git a/lw_benchhub/scripts/autosim/robot_distance_sweep.py b/lw_benchhub/scripts/autosim/robot_distance_sweep.py new file mode 100644 index 00000000..29175db1 --- /dev/null +++ b/lw_benchhub/scripts/autosim/robot_distance_sweep.py @@ -0,0 +1,200 @@ +"""Sweep G1 base distance from object and test IK to a fixed EE target. + +Teleports the robot to a series of distances from the object along the +approach axis, runs IK-only to the configured reach target, and reports +which distances yield a successful IK solution. + +Usage: + python robot_distance_sweep.py \ + --pipeline_id LWBenchhub-Autosim-G1CloseOvenPipeline-v0 \ + --headless +""" + +import argparse + +from isaaclab.app import AppLauncher + +parser = argparse.ArgumentParser(description="Sweep robot base distance and test IK to fixed EE target.") +parser.add_argument("--pipeline_id", required=True, type=str) +parser.add_argument("--object_name", default=None, type=str, + help="Scene object name to measure distance from (default: first object in pipeline)") +parser.add_argument("--d_min", default=0.4, type=float, help="Minimum distance from object (m)") +parser.add_argument("--d_max", default=1.5, type=float, help="Maximum distance from object (m)") +parser.add_argument("--d_step", default=0.1, type=float, help="Distance step (m)") +parser.add_argument("--x_offset", default=0.0, type=float, + help="Robot base X offset relative to object X (e.g. -0.21 for G1)") +parser.add_argument("--squat_steps", default=0, type=int, + help="Steps to squat before IK (0 = no squat; use 40 for G1)") +parser.add_argument("--n_orient_samples", default=1, type=int, + help="Number of EE orientation samples per distance (default 1 = use base quaternion only). " + "Samples are evenly spaced yaw rotations around world Z applied to the base quaternion.") + +AppLauncher.add_app_launcher_args(parser) +args_cli = parser.parse_args() +app_launcher = AppLauncher(vars(args_cli)) +simulation_app = app_launcher.app + +import math + +import torch +from isaaclab.utils.math import combine_frame_transforms, subtract_frame_transforms, quat_mul + +import lw_benchhub.autosim # noqa: F401 +from autosim import make_pipeline + + +def _gen_orient_samples(base_quat_wxyz: torch.Tensor, n: int) -> list[torch.Tensor]: + """Return n quaternions covering evenly-spaced yaw rotations (world Z) applied to base_quat. + + When n==1 returns [base_quat] unchanged (original orientation only). + """ + if n <= 1: + return [base_quat_wxyz] + samples = [] + for i in range(n): + yaw = 2 * math.pi * i / n + half = yaw / 2.0 + dq = torch.tensor([math.cos(half), 0.0, 0.0, math.sin(half)], + dtype=torch.float32) # rotation around world Z + q = quat_mul(dq.unsqueeze(0), base_quat_wxyz.unsqueeze(0))[0] + samples.append(q) + return samples + + +def _get_object_pose(env, object_name: str): + obj = env.scene[object_name] + pos_w = obj.data.root_pos_w[0] # [3] + quat_w = obj.data.root_quat_w[0] # [w, x, y, z] + return pos_w, quat_w + + +def _settle_squat(env, squat_steps: int) -> None: + """Switch to squat/stance mode using low-level stepping (bypasses recorder).""" + if squat_steps <= 0: + return + action = torch.zeros(env.action_space.shape, device=env.device) + action[0, 0] = -1.0 # negative cmd drives squat_cmd[0] down toward min height + action[0, 3] = 1.0 # mode=1: squat/stance + env.action_manager.process_action(action) + for _ in range(squat_steps): + for _ in range(env.cfg.decimation): + env.action_manager.apply_action() + env.sim.step(render=False) + env.scene.update(env.sim.get_physics_dt()) + + +def _teleport_robot(env, pos_w: torch.Tensor, yaw_rad: float, settle_steps: int = 20): + half = yaw_rad / 2.0 + quat = torch.tensor( + [math.cos(half), 0.0, 0.0, math.sin(half)], + dtype=torch.float32, device=env.device, + ) # [w, x, y, z] + pose = torch.zeros(1, 7, device=env.device) + pose[0, :3] = pos_w.to(env.device) + pose[0, 3:] = quat + + robot = env.scene["robot"] + robot.write_root_pose_to_sim(pose) + robot.write_root_velocity_to_sim(torch.zeros(1, 6, device=env.device)) + for _ in range(settle_steps): + env.sim.step() + env.scene.update(env.sim.get_physics_dt()) + + +def main(): + pipeline = make_pipeline(args_cli.pipeline_id) + pipeline.cfg.motion_planner.use_cuda_graph = False + pipeline.initialize() + + env = pipeline._env + extra_info = pipeline._env_extra_info + + # Pick the target object + object_name = args_cli.object_name + if object_name is None: + object_name = list(extra_info.object_reach_target_poses.keys())[0] + + # Get the object's world pose + obj_pos_w, obj_quat_w = _get_object_pose(env, object_name) + + # Compute EE target in world frame + offset = extra_info.object_reach_target_poses[object_name][0] # [x,y,z, qw,qx,qy,qz] + offset_pos = offset[:3].unsqueeze(0) + offset_quat = offset[3:].unsqueeze(0) + + ee_pos_w, ee_quat_w = combine_frame_transforms( + obj_pos_w.unsqueeze(0), obj_quat_w.unsqueeze(0), + offset_pos, offset_quat, + ) + ee_pos_w = ee_pos_w[0] # [3] + ee_quat_w = ee_quat_w[0] # [4] + + print(f"\nObject '{object_name}' world pos : {obj_pos_w.cpu().numpy().round(3)}") + print(f"EE target world pos : {ee_pos_w.cpu().numpy().round(3)}") + print(f"EE target world quat (w,x,y,z) : {ee_quat_w.cpu().numpy().round(3)}") + + n_orient = args_cli.n_orient_samples + orient_samples = _gen_orient_samples(ee_quat_w, n_orient) + print(f"Orientation samples per distance: {n_orient}") + + # Robot spawn height from the G1 profile default + robot_z = env.scene["robot"].data.root_pos_w[0, 2].item() + # Approach from -y side (robot south of object), facing +y + approach_yaw = math.pi / 2.0 + + import numpy as np + distances = np.arange(args_cli.d_min, args_cli.d_max + args_cli.d_step / 2, args_cli.d_step) + + header = f"\n{'Dist (m)':>10} {'IK':>4} {'tgt_x':>7} {'tgt_y':>7} {'tgt_z':>7} {'pos_err':>8} {'rot_err':>9} {'idx':>4}" + print(header) + print("-" * len(header)) + + for d in distances: + # Teleport robot + robot_pos = torch.tensor( + [obj_pos_w[0].item() + args_cli.x_offset, obj_pos_w[1].item() - d, robot_z], + dtype=torch.float32, + ) + _teleport_robot(env, robot_pos, approach_yaw) + _settle_squat(env, args_cli.squat_steps) + + # Express EE position in robot root frame (read AFTER squatting — pelvis may have dropped) + robot = env.scene["robot"] + r_pos_w = robot.data.root_pos_w[:1] # [1, 3] + r_quat_w = robot.data.root_quat_w[:1] # [1, 4] + + best_ok, best_pos_err, best_rot_err, best_idx = False, float("inf"), float("inf"), -1 + best_t_robot = None + + for i, q_sample in enumerate(orient_samples): + t_robot, q_robot = subtract_frame_transforms( + r_pos_w, r_quat_w, + ee_pos_w.unsqueeze(0), q_sample.unsqueeze(0).to(r_pos_w.device), + ) # [1,3], [1,4] + + result = pipeline._motion_planner.solve_ik_batch( + target_pos=t_robot, + target_quat=q_robot, + ) + + ok = bool(result.success[0].item()) + pos_err = float(result.position_error[0].item()) + rot_err = float(result.rotation_error[0].item()) + + if ok and not best_ok: + best_ok, best_pos_err, best_rot_err, best_idx = True, pos_err, rot_err, i + best_t_robot = t_robot[0].cpu() + break # first success is enough + if not best_ok and pos_err < best_pos_err: + best_pos_err, best_rot_err, best_idx = pos_err, rot_err, i + best_t_robot = t_robot[0].cpu() + + mark = "✓" if best_ok else "✗" + tx, ty, tz = best_t_robot.tolist() if best_t_robot is not None else (0, 0, 0) + print(f"{d:>10.2f} {mark:>4} {tx:>7.3f} {ty:>7.3f} {tz:>7.3f} {best_pos_err:>8.4f} {best_rot_err:>9.4f} {best_idx:>4}") + + print("\nSweep complete.") + + +if __name__ == "__main__": + main() diff --git a/lw_benchhub/scripts/autosim/robot_position_2d_sweep.py b/lw_benchhub/scripts/autosim/robot_position_2d_sweep.py new file mode 100644 index 00000000..fd63e4d3 --- /dev/null +++ b/lw_benchhub/scripts/autosim/robot_position_2d_sweep.py @@ -0,0 +1,168 @@ +"""Sweep G1 base position (x_offset, distance) and test IK to a fixed EE target. + +2D sweep over x_offset and distance from object, testing IK reachability. + +Usage: + python robot_position_2d_sweep.py \ + --pipeline_id LWBenchhub-Autosim-G1OpenFridgePipeline-v0 \ + --headless --enable_cameras +""" + +import argparse + +from isaaclab.app import AppLauncher + +parser = argparse.ArgumentParser(description="2D sweep of robot position (x_offset, distance) and test IK.") +parser.add_argument("--pipeline_id", required=True, type=str) +parser.add_argument("--object_name", default=None, type=str, + help="Scene object name (default: first object in pipeline)") +parser.add_argument("--x_min", default=-0.6, type=float, help="Minimum x_offset (m)") +parser.add_argument("--x_max", default=0.2, type=float, help="Maximum x_offset (m)") +parser.add_argument("--x_step", default=0.1, type=float, help="X step (m)") +parser.add_argument("--d_min", default=0.4, type=float, help="Minimum distance (m)") +parser.add_argument("--d_max", default=1.5, type=float, help="Maximum distance (m)") +parser.add_argument("--d_step", default=0.1, type=float, help="Distance step (m)") +parser.add_argument("--squat_steps", default=40, type=int, help="Squat steps (0=no squat)") + +AppLauncher.add_app_launcher_args(parser) +args_cli = parser.parse_args() +app_launcher = AppLauncher(vars(args_cli)) +simulation_app = app_launcher.app + +import math + +import numpy as np +import torch +from isaaclab.utils.math import combine_frame_transforms, subtract_frame_transforms + +import lw_benchhub.autosim # noqa: F401 +from autosim import make_pipeline + + +def _teleport_robot(env, pos_w: torch.Tensor, yaw_rad: float, settle_steps: int = 20): + half = yaw_rad / 2.0 + quat = torch.tensor( + [math.cos(half), 0.0, 0.0, math.sin(half)], + dtype=torch.float32, device=env.device, + ) + pose = torch.zeros(1, 7, device=env.device) + pose[0, :3] = pos_w.to(env.device) + pose[0, 3:] = quat + + robot = env.scene["robot"] + robot.write_root_pose_to_sim(pose) + robot.write_root_velocity_to_sim(torch.zeros(1, 6, device=env.device)) + for _ in range(settle_steps): + env.sim.step() + env.scene.update(env.sim.get_physics_dt()) + + +def _settle_squat(env, squat_steps: int) -> None: + if squat_steps <= 0: + return + action = torch.zeros(env.action_space.shape, device=env.device) + action[0, 0] = -1.0 + action[0, 3] = 1.0 + env.action_manager.process_action(action) + for _ in range(squat_steps): + for _ in range(env.cfg.decimation): + env.action_manager.apply_action() + env.sim.step(render=False) + env.scene.update(env.sim.get_physics_dt()) + + +def main(): + pipeline = make_pipeline(args_cli.pipeline_id) + pipeline.cfg.motion_planner.use_cuda_graph = False + pipeline.initialize() + + env = pipeline._env + extra_info = pipeline._env_extra_info + + object_name = args_cli.object_name + if object_name is None: + object_name = list(extra_info.object_reach_target_poses.keys())[0] + + obj = env.scene[object_name] + obj_pos_w = obj.data.root_pos_w[0] + obj_quat_w = obj.data.root_quat_w[0] + + offset = extra_info.object_reach_target_poses[object_name][0] + offset_pos = offset[:3].unsqueeze(0) + offset_quat = offset[3:].unsqueeze(0) + + ee_pos_w, ee_quat_w = combine_frame_transforms( + obj_pos_w.unsqueeze(0), obj_quat_w.unsqueeze(0), + offset_pos, offset_quat, + ) + ee_pos_w = ee_pos_w[0] + ee_quat_w = ee_quat_w[0] + + print(f"\nObject '{object_name}' world pos : {obj_pos_w.cpu().numpy().round(3)}") + print(f"EE target world pos : {ee_pos_w.cpu().numpy().round(3)}") + print(f"EE target world quat (w,x,y,z) : {ee_quat_w.cpu().numpy().round(3)}") + + robot_z = env.scene["robot"].data.root_pos_w[0, 2].item() + approach_yaw = math.pi / 2.0 + + x_offsets = np.arange(args_cli.x_min, args_cli.x_max + args_cli.x_step / 2, args_cli.x_step) + distances = np.arange(args_cli.d_min, args_cli.d_max + args_cli.d_step / 2, args_cli.d_step) + + print(f"\nScanning {len(x_offsets)} x_offsets × {len(distances)} distances = {len(x_offsets) * len(distances)} positions\n") + + results = [] + + for x_off in x_offsets: + for d in distances: + robot_pos = torch.tensor( + [obj_pos_w[0].item() + x_off, obj_pos_w[1].item() - d, robot_z], + dtype=torch.float32, + ) + _teleport_robot(env, robot_pos, approach_yaw) + _settle_squat(env, args_cli.squat_steps) + + robot = env.scene["robot"] + r_pos_w = robot.data.root_pos_w[:1] + r_quat_w = robot.data.root_quat_w[:1] + + t_robot, q_robot = subtract_frame_transforms( + r_pos_w, r_quat_w, + ee_pos_w.unsqueeze(0), ee_quat_w.unsqueeze(0), + ) + + result = pipeline._motion_planner.solve_ik_batch( + target_pos=t_robot, + target_quat=q_robot, + ) + + ok = bool(result.success[0].item()) + pos_err = float(result.position_error[0].item()) + rot_err = float(result.rotation_error[0].item()) + + results.append((x_off, d, ok, pos_err, rot_err)) + + # Print results + print(f"\n{'x_offset':>9} {'dist':>6} {'IK':>4} {'pos_err':>8} {'rot_err':>9}") + print("-" * 50) + for x_off, d, ok, pos_err, rot_err in results: + mark = "✓" if ok else "✗" + print(f"{x_off:>9.2f} {d:>6.2f} {mark:>4} {pos_err:>8.4f} {rot_err:>9.4f}") + + # Summary: show successful positions + success_list = [(x, d) for x, d, ok, _, _ in results if ok] + if success_list: + print(f"\n✓ Found {len(success_list)} successful positions:") + for x, d in success_list: + print(f" x_offset={x:.2f}, distance={d:.2f}") + else: + print("\n✗ No successful IK solutions found in the scanned range.") + + print("\nSweep complete.") + simulation_app.close() + import sys + sys.exit(0) + + +if __name__ == "__main__": + main() + diff --git a/lw_benchhub/scripts/autosim/z_target_sweep.py b/lw_benchhub/scripts/autosim/z_target_sweep.py new file mode 100644 index 00000000..c246852f --- /dev/null +++ b/lw_benchhub/scripts/autosim/z_target_sweep.py @@ -0,0 +1,152 @@ +"""Sweep EE target Z (oven frame) and test IK reachability. + +Keeps X, Y, and orientation fixed at the current g1_loco_left config values. +Teleports the robot to --robot_dist metres from the object (matching sampling_radius), +squats, then sweeps Z. + +Usage: + python z_target_sweep.py \ + --pipeline_id LWBenchhub-Autosim-G1CloseOvenPipeline-v0 \ + --headless \ + --robot_dist 1.15 \ + --squat_steps 40 \ + --z_min -0.5 --z_max 0.2 --z_step 0.05 +""" + +import argparse +import math + +from isaaclab.app import AppLauncher + +parser = argparse.ArgumentParser() +parser.add_argument("--pipeline_id", required=True, type=str) +parser.add_argument("--robot_dist", default=1.15, type=float, + help="Distance to teleport robot from oven (should match sampling_radius)") +parser.add_argument("--x_offset", default=-0.3, type=float, + help="Robot base X offset relative to oven X (matches init_state_pos_delta[0])") +parser.add_argument("--squat_steps", default=40, type=int) +parser.add_argument("--z_min", default=-0.5, type=float) +parser.add_argument("--z_max", default=0.2, type=float) +parser.add_argument("--z_step", default=0.05, type=float) +AppLauncher.add_app_launcher_args(parser) +args_cli = parser.parse_args() +app_launcher = AppLauncher(vars(args_cli)) +simulation_app = app_launcher.app + +import numpy as np +import torch +from isaaclab.utils.math import combine_frame_transforms, subtract_frame_transforms + +import lw_benchhub.autosim # noqa: F401 +from autosim import make_pipeline + + +def _teleport_robot(env, pos_w: torch.Tensor, yaw_rad: float, settle_steps: int = 20): + half = yaw_rad / 2.0 + quat = torch.tensor( + [math.cos(half), 0.0, 0.0, math.sin(half)], + dtype=torch.float32, device=env.device, + ) + pose = torch.zeros(1, 7, device=env.device) + pose[0, :3] = pos_w.to(env.device) + pose[0, 3:] = quat + robot = env.scene["robot"] + robot.write_root_pose_to_sim(pose) + robot.write_root_velocity_to_sim(torch.zeros(1, 6, device=env.device)) + for _ in range(settle_steps): + env.sim.step() + env.scene.update(env.sim.get_physics_dt()) + + +def _settle_squat(env, squat_steps: int) -> None: + if squat_steps <= 0: + return + action = torch.zeros(env.action_space.shape, device=env.device) + action[0, 0] = -1.0 + action[0, 3] = 1.0 + env.action_manager.process_action(action) + for _ in range(squat_steps): + for _ in range(env.cfg.decimation): + env.action_manager.apply_action() + env.sim.step(render=False) + env.scene.update(env.sim.get_physics_dt()) + + +def main(): + pipeline = make_pipeline(args_cli.pipeline_id) + pipeline.cfg.motion_planner.use_cuda_graph = False + pipeline.initialize() + + env = pipeline._env + extra_info = pipeline._env_extra_info + + object_name = list(extra_info.object_reach_target_poses.keys())[0] + obj = env.scene[object_name] + obj_pos_w = obj.data.root_pos_w[0] + obj_quat_w = obj.data.root_quat_w[0] + + base_offset = extra_info.object_reach_target_poses[object_name][0] + fix_x = base_offset[0].item() + fix_y = base_offset[1].item() + fix_quat = base_offset[3:].unsqueeze(0) # [1, 4] + + print(f"\nObject '{object_name}' world pos : {obj_pos_w.cpu().numpy().round(3)}") + print(f"Fixed oven-frame X={fix_x:.4f} Y={fix_y:.4f}") + print(f"Fixed quat (w,x,y,z) = {fix_quat[0].numpy().round(4)}") + + # Teleport robot to robot_dist from oven, facing +Y (yaw=90°) + robot_z = env.scene["robot"].data.root_pos_w[0, 2].item() + robot_pos = torch.tensor( + [obj_pos_w[0].item() + args_cli.x_offset, + obj_pos_w[1].item() - args_cli.robot_dist, + robot_z], + dtype=torch.float32, + ) + approach_yaw = math.pi / 2.0 + print(f"Teleporting robot to {robot_pos.numpy().round(3)} (dist={args_cli.robot_dist}m from oven)") + _teleport_robot(env, robot_pos, approach_yaw) + + # Squat + _settle_squat(env, args_cli.squat_steps) + + robot = env.scene["robot"] + r_pos_w = robot.data.root_pos_w[:1] + r_quat_w = robot.data.root_quat_w[:1] + print(f"Robot root pos (after squat): {r_pos_w[0].cpu().numpy().round(3)}") + + z_values = np.arange(args_cli.z_min, args_cli.z_max + args_cli.z_step / 2, args_cli.z_step) + + header = f"\n{'Z (oven)':>10} {'IK':>4} {'tgt_x':>7} {'tgt_y':>7} {'tgt_z':>7} {'pos_err':>8} {'rot_err':>9}" + print(header) + print("-" * len(header)) + + for z in z_values: + off_pos = torch.tensor([[fix_x, fix_y, z]], dtype=torch.float32) + ee_pos_w, ee_quat_w = combine_frame_transforms( + obj_pos_w.unsqueeze(0), obj_quat_w.unsqueeze(0), + off_pos, fix_quat, + ) + + t_robot, q_robot = subtract_frame_transforms( + r_pos_w, r_quat_w, + ee_pos_w, ee_quat_w, + ) + + result = pipeline._motion_planner.solve_ik_batch( + target_pos=t_robot, + target_quat=q_robot, + ) + + ok = bool(result.success[0].item()) + pos_err = float(result.position_error[0].item()) + rot_err = float(result.rotation_error[0].item()) + mark = "✓" if ok else "✗" + tx, ty, tz = t_robot[0].cpu().tolist() + print(f"{z:>10.3f} {mark:>4} {tx:>7.3f} {ty:>7.3f} {tz:>7.3f} {pos_err:>8.4f} {rot_err:>9.4f}") + + print("\nSweep complete.") + simulation_app.close() + + +if __name__ == "__main__": + main() From 7abb3537a0ec1d594d6aacf03e1d8ecabc091472 Mon Sep 17 00:00:00 2001 From: Zihan Gao Date: Mon, 4 May 2026 12:10:37 +0800 Subject: [PATCH 13/32] Finish open_fridge on G1 --- .../action_adapters/g1_action_adapter.py | 1 + .../action_adapters/g1_action_adapter_cfg.py | 2 +- .../content/configs/robot/g1_right.yml | 36 +++++++++---------- lw_benchhub/autosim/pipelines/open_fridge.py | 8 ++--- 4 files changed, 24 insertions(+), 23 deletions(-) diff --git a/lw_benchhub/autosim/action_adapters/g1_action_adapter.py b/lw_benchhub/autosim/action_adapters/g1_action_adapter.py index dc183ca4..68e11b32 100644 --- a/lw_benchhub/autosim/action_adapters/g1_action_adapter.py +++ b/lw_benchhub/autosim/action_adapters/g1_action_adapter.py @@ -29,6 +29,7 @@ def __init__(self, cfg: G1ActionAdapterCfg): self.register_apply_method("reach", self._apply_reach) self.register_apply_method("lift", self._apply_reach) self.register_apply_method("pull", self._apply_reach) + self.register_apply_method("push", self._apply_reach) self.register_apply_method("grasp", self._apply_gripper) self.register_apply_method("ungrasp", self._apply_gripper) diff --git a/lw_benchhub/autosim/action_adapters/g1_action_adapter_cfg.py b/lw_benchhub/autosim/action_adapters/g1_action_adapter_cfg.py index 4bb1e310..39a4c8b2 100644 --- a/lw_benchhub/autosim/action_adapters/g1_action_adapter_cfg.py +++ b/lw_benchhub/autosim/action_adapters/g1_action_adapter_cfg.py @@ -15,7 +15,7 @@ class G1ActionAdapterCfg(ActionAdapterCfg): base_y_joint_name: str = "base_y_joint" base_yaw_joint_name: str = "base_yaw_joint" - finger_close_angles: tuple = (0.0,) * 14 + finger_close_angles: tuple = (1.0, 1.0, 1.0, 1.0, 0.8, 0.8, 0.8, 1.0, 1.0, 1.0, 1.0, 0.8, 0.8, 0.8) """Per-joint finger angles (rad) when gripper is closed.""" finger_open_angles: tuple = (0.0,) * 14 """Per-joint finger angles (rad) when gripper is open.""" diff --git a/lw_benchhub/autosim/content/configs/robot/g1_right.yml b/lw_benchhub/autosim/content/configs/robot/g1_right.yml index 8e289e74..0ce4bc0a 100644 --- a/lw_benchhub/autosim/content/configs/robot/g1_right.yml +++ b/lw_benchhub/autosim/content/configs/robot/g1_right.yml @@ -64,7 +64,7 @@ robot_cfg: ] # Joints that are fixed during planning. - # Right arm is locked; left arm is planned. + # Left arm is locked; right arm is planned. lock_joints: { # Virtual base joints @@ -75,14 +75,14 @@ robot_cfg: "waist_yaw_joint": 0.0, "waist_roll_joint": 0.0, "waist_pitch_joint": 0.0, - # Right arm (locked; left arm is planned) - "right_shoulder_pitch_joint": 0.0, - "right_shoulder_roll_joint": -0.3, - "right_shoulder_yaw_joint": 0.0, - "right_elbow_joint": 0.0, - "right_wrist_roll_joint": 0.0, - "right_wrist_pitch_joint": 0.0, - "right_wrist_yaw_joint": 0.0, + # Left arm (locked; right arm is planned) + "left_shoulder_pitch_joint": 0.0, + "left_shoulder_roll_joint": 0.3, + "left_shoulder_yaw_joint": 0.0, + "left_elbow_joint": 0.0, + "left_wrist_roll_joint": 0.0, + "left_wrist_pitch_joint": 0.0, + "left_wrist_yaw_joint": 0.0, } extra_links: null @@ -90,19 +90,19 @@ robot_cfg: cspace: joint_names: [ - # Left arm (7 DoF) — virtual base joints locked, not in cspace - "left_shoulder_pitch_joint", - "left_shoulder_roll_joint", - "left_shoulder_yaw_joint", - "left_elbow_joint", - "left_wrist_roll_joint", - "left_wrist_pitch_joint", - "left_wrist_yaw_joint", + # Right arm (7 DoF) — virtual base joints locked, not in cspace + "right_shoulder_pitch_joint", + "right_shoulder_roll_joint", + "right_shoulder_yaw_joint", + "right_elbow_joint", + "right_wrist_roll_joint", + "right_wrist_pitch_joint", + "right_wrist_yaw_joint", ] retract_config: [ - 0.0, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0, # left arm + 0.0, -0.3, 0.0, 0.0, 0.0, 0.0, 0.0, # right arm ] null_space_weight: diff --git a/lw_benchhub/autosim/pipelines/open_fridge.py b/lw_benchhub/autosim/pipelines/open_fridge.py index bc9f7dc6..4ec35273 100644 --- a/lw_benchhub/autosim/pipelines/open_fridge.py +++ b/lw_benchhub/autosim/pipelines/open_fridge.py @@ -65,10 +65,10 @@ def _g1_skill_cfg(cfg) -> None: "g1_loco_right": TaskRobotOverride( object_reach_target_poses={ "fridge_main_group": [ - torch.tensor([0.047, -0.429, 0.125, 0.707, 0.0, 0.0, 0.707]), + torch.tensor([0.07, -0.429, 0.25, 0.707, 0.0, 0.0, 0.707]), ], }, - init_state_pos_delta=(-0.15, -1.0, 0.0), + init_state_pos_delta=(-0.15, -1.5, 0.0), skill_cfg_fn=_g1_skill_cfg, ), } @@ -96,8 +96,8 @@ def __post_init__(self): if resolved_robot.override.skill_cfg_fn: resolved_robot.override.skill_cfg_fn(self) - self.skills.pull.extra_cfg.move_offset = 0.1 - self.skills.pull.extra_cfg.move_axis = "-x" + self.skills.push.extra_cfg.move_offset = 0.3 + self.skills.push.extra_cfg.move_axis = "-x" self.occupancy_map.floor_prim_suffix = "Scene/floor_room" self.motion_planner.world_ignore_subffixes = ["Scene/floor_room"] From cc8ccf3238f922bf8fbfc5f6c6f22390c3d811c7 Mon Sep 17 00:00:00 2001 From: Zihan Gao Date: Mon, 4 May 2026 12:18:30 +0800 Subject: [PATCH 14/32] Fix format to allign with x7s --- lw_benchhub/autosim/content/configs/robot/g1.yml | 2 +- lw_benchhub/autosim/content/configs/robot/g1_right.yml | 2 +- lw_benchhub/autosim/content/configs/robot/g1_right_ee.yml | 2 +- .../spheres/{collision_g1_autosim.yml => collision_g1.yml} | 0 4 files changed, 3 insertions(+), 3 deletions(-) rename lw_benchhub/autosim/content/configs/robot/spheres/{collision_g1_autosim.yml => collision_g1.yml} (100%) diff --git a/lw_benchhub/autosim/content/configs/robot/g1.yml b/lw_benchhub/autosim/content/configs/robot/g1.yml index f8b035ec..375cd43a 100644 --- a/lw_benchhub/autosim/content/configs/robot/g1.yml +++ b/lw_benchhub/autosim/content/configs/robot/g1.yml @@ -24,7 +24,7 @@ robot_cfg: "right_wrist_roll_link", "right_wrist_pitch_link", "right_wrist_yaw_link", ] - collision_spheres: "spheres/collision_g1_autosim.yml" + collision_spheres: "spheres/collision_g1.yml" collision_sphere_buffer: 0.002 extra_collision_spheres: {"left_hand_palm_link": 20} use_global_cumul: True diff --git a/lw_benchhub/autosim/content/configs/robot/g1_right.yml b/lw_benchhub/autosim/content/configs/robot/g1_right.yml index 0ce4bc0a..da8644cb 100644 --- a/lw_benchhub/autosim/content/configs/robot/g1_right.yml +++ b/lw_benchhub/autosim/content/configs/robot/g1_right.yml @@ -24,7 +24,7 @@ robot_cfg: "right_wrist_roll_link", "right_wrist_pitch_link", "right_wrist_yaw_link", ] - collision_spheres: "spheres/collision_g1_autosim.yml" + collision_spheres: "spheres/collision_g1.yml" collision_sphere_buffer: 0.002 extra_collision_spheres: {"right_hand_palm_link": 20} use_global_cumul: True diff --git a/lw_benchhub/autosim/content/configs/robot/g1_right_ee.yml b/lw_benchhub/autosim/content/configs/robot/g1_right_ee.yml index 47262bc2..0737fa3b 100644 --- a/lw_benchhub/autosim/content/configs/robot/g1_right_ee.yml +++ b/lw_benchhub/autosim/content/configs/robot/g1_right_ee.yml @@ -24,7 +24,7 @@ robot_cfg: "right_wrist_roll_link", "right_wrist_pitch_link", "right_wrist_yaw_link", ] - collision_spheres: "spheres/collision_g1_autosim.yml" + collision_spheres: "spheres/collision_g1.yml" collision_sphere_buffer: 0.002 extra_collision_spheres: {"right_wrist_yaw_link": 20, "left_wrist_yaw_link": 20} use_global_cumul: True diff --git a/lw_benchhub/autosim/content/configs/robot/spheres/collision_g1_autosim.yml b/lw_benchhub/autosim/content/configs/robot/spheres/collision_g1.yml similarity index 100% rename from lw_benchhub/autosim/content/configs/robot/spheres/collision_g1_autosim.yml rename to lw_benchhub/autosim/content/configs/robot/spheres/collision_g1.yml From 1b8f7be4c2dae29e51e07819b8a6e710b050c6d1 Mon Sep 17 00:00:00 2001 From: Zihan Gao Date: Mon, 4 May 2026 12:33:02 +0800 Subject: [PATCH 15/32] Delete redundant files --- .../content/configs/robot/g1_right.yml | 122 ------------------ .../content/configs/robot/g1_right_ee.yml | 21 ++- lw_benchhub/autosim/robot_profiles.py | 2 +- 3 files changed, 11 insertions(+), 134 deletions(-) delete mode 100644 lw_benchhub/autosim/content/configs/robot/g1_right.yml diff --git a/lw_benchhub/autosim/content/configs/robot/g1_right.yml b/lw_benchhub/autosim/content/configs/robot/g1_right.yml deleted file mode 100644 index da8644cb..00000000 --- a/lw_benchhub/autosim/content/configs/robot/g1_right.yml +++ /dev/null @@ -1,122 +0,0 @@ -robot_cfg: - - kinematics: - use_usd_kinematics: False - usd_robot_root: "/g1_autosim" - urdf_path: "urdf/G1.urdf" - asset_root_path: "." # meshes are resolved from the urdf file dir - - base_link: "world_link" - ee_link: "right_hand_palm_link" - # Extra tracked links - link_names: ["right_hand_palm_link"] - - collision_link_names: - [ - "pelvis", - "torso_link", - "left_shoulder_pitch_link", "left_shoulder_roll_link", "left_shoulder_yaw_link", - "left_elbow_link", - "left_wrist_roll_link", "left_wrist_pitch_link", "left_wrist_yaw_link", - "right_hand_palm_link", - "right_shoulder_pitch_link", "right_shoulder_roll_link", "right_shoulder_yaw_link", - "right_elbow_link", - "right_wrist_roll_link", "right_wrist_pitch_link", "right_wrist_yaw_link", - ] - - collision_spheres: "spheres/collision_g1.yml" - collision_sphere_buffer: 0.002 - extra_collision_spheres: {"right_hand_palm_link": 20} - use_global_cumul: True - - self_collision_ignore: - { - "pelvis": ["torso_link", "left_shoulder_pitch_link", "right_shoulder_pitch_link"], - "torso_link": ["left_shoulder_pitch_link", "right_shoulder_pitch_link"], - "left_shoulder_pitch_link": ["left_shoulder_roll_link"], - "left_shoulder_roll_link": ["left_shoulder_yaw_link"], - "left_shoulder_yaw_link": ["left_elbow_link"], - "left_elbow_link": ["left_wrist_roll_link"], - "left_wrist_roll_link": ["left_wrist_pitch_link"], - "left_wrist_pitch_link": ["left_wrist_yaw_link"], - "left_wrist_yaw_link": ["right_hand_palm_link"], - "right_shoulder_pitch_link": ["right_shoulder_roll_link"], - "right_shoulder_roll_link": ["right_shoulder_yaw_link"], - "right_shoulder_yaw_link": ["right_elbow_link"], - "right_elbow_link": ["right_wrist_roll_link"], - "right_wrist_roll_link": ["right_wrist_pitch_link"], - "right_wrist_pitch_link": ["right_wrist_yaw_link"], - } - - self_collision_buffer: {} - - mesh_link_names: - [ - "pelvis", - "torso_link", - "left_shoulder_pitch_link", "left_shoulder_roll_link", "left_shoulder_yaw_link", - "left_elbow_link", - "left_wrist_roll_link", "left_wrist_pitch_link", "left_wrist_yaw_link", - "right_hand_palm_link", - "right_shoulder_pitch_link", "right_shoulder_roll_link", "right_shoulder_yaw_link", - "right_elbow_link", - "right_wrist_roll_link", "right_wrist_pitch_link", "right_wrist_yaw_link", - ] - - # Joints that are fixed during planning. - # Left arm is locked; right arm is planned. - lock_joints: - { - # Virtual base joints - "base_x_joint": 0.0, - "base_y_joint": 0.0, - "base_yaw_joint": 0.0, - # Waist - "waist_yaw_joint": 0.0, - "waist_roll_joint": 0.0, - "waist_pitch_joint": 0.0, - # Left arm (locked; right arm is planned) - "left_shoulder_pitch_joint": 0.0, - "left_shoulder_roll_joint": 0.3, - "left_shoulder_yaw_joint": 0.0, - "left_elbow_joint": 0.0, - "left_wrist_roll_joint": 0.0, - "left_wrist_pitch_joint": 0.0, - "left_wrist_yaw_joint": 0.0, - } - - extra_links: null - - cspace: - joint_names: - [ - # Right arm (7 DoF) — virtual base joints locked, not in cspace - "right_shoulder_pitch_joint", - "right_shoulder_roll_joint", - "right_shoulder_yaw_joint", - "right_elbow_joint", - "right_wrist_roll_joint", - "right_wrist_pitch_joint", - "right_wrist_yaw_joint", - ] - - retract_config: - [ - 0.0, -0.3, 0.0, 0.0, 0.0, 0.0, 0.0, # right arm - ] - - null_space_weight: - [ - 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, - ] - - cspace_distance_weight: - [ - 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, - ] - - max_jerk: 500.0 - max_acceleration: 500.0 - -planner: - frame_bias: [0.0, 0.0, 0.0] diff --git a/lw_benchhub/autosim/content/configs/robot/g1_right_ee.yml b/lw_benchhub/autosim/content/configs/robot/g1_right_ee.yml index 0737fa3b..da8644cb 100644 --- a/lw_benchhub/autosim/content/configs/robot/g1_right_ee.yml +++ b/lw_benchhub/autosim/content/configs/robot/g1_right_ee.yml @@ -7,10 +7,9 @@ robot_cfg: asset_root_path: "." # meshes are resolved from the urdf file dir base_link: "world_link" - ee_link: "right_wrist_yaw_link" # right-hand end-effector - - # Extra tracked links (left ee for dual-arm tasks) - link_names: ["right_wrist_yaw_link", "left_wrist_yaw_link"] + ee_link: "right_hand_palm_link" + # Extra tracked links + link_names: ["right_hand_palm_link"] collision_link_names: [ @@ -19,6 +18,7 @@ robot_cfg: "left_shoulder_pitch_link", "left_shoulder_roll_link", "left_shoulder_yaw_link", "left_elbow_link", "left_wrist_roll_link", "left_wrist_pitch_link", "left_wrist_yaw_link", + "right_hand_palm_link", "right_shoulder_pitch_link", "right_shoulder_roll_link", "right_shoulder_yaw_link", "right_elbow_link", "right_wrist_roll_link", "right_wrist_pitch_link", "right_wrist_yaw_link", @@ -26,7 +26,7 @@ robot_cfg: collision_spheres: "spheres/collision_g1.yml" collision_sphere_buffer: 0.002 - extra_collision_spheres: {"right_wrist_yaw_link": 20, "left_wrist_yaw_link": 20} + extra_collision_spheres: {"right_hand_palm_link": 20} use_global_cumul: True self_collision_ignore: @@ -39,6 +39,7 @@ robot_cfg: "left_elbow_link": ["left_wrist_roll_link"], "left_wrist_roll_link": ["left_wrist_pitch_link"], "left_wrist_pitch_link": ["left_wrist_yaw_link"], + "left_wrist_yaw_link": ["right_hand_palm_link"], "right_shoulder_pitch_link": ["right_shoulder_roll_link"], "right_shoulder_roll_link": ["right_shoulder_yaw_link"], "right_shoulder_yaw_link": ["right_elbow_link"], @@ -56,19 +57,17 @@ robot_cfg: "left_shoulder_pitch_link", "left_shoulder_roll_link", "left_shoulder_yaw_link", "left_elbow_link", "left_wrist_roll_link", "left_wrist_pitch_link", "left_wrist_yaw_link", + "right_hand_palm_link", "right_shoulder_pitch_link", "right_shoulder_roll_link", "right_shoulder_yaw_link", "right_elbow_link", "right_wrist_roll_link", "right_wrist_pitch_link", "right_wrist_yaw_link", ] # Joints that are fixed during planning. - # NOTE: Only joints ON the kinematic chain (world_link → right_hand_palm_link, - # with left_hand_palm_link as a tracked branch) can be locked. - # Leg joints (hip/knee/ankle) branch off pelvis and are NOT on the chain → excluded. - # Finger joints are descendants of the EE palm links and are NOT on the chain → excluded. + # Left arm is locked; right arm is planned. lock_joints: { - # Virtual base joints (robot moves via leg loco, not via these joints) + # Virtual base joints "base_x_joint": 0.0, "base_y_joint": 0.0, "base_yaw_joint": 0.0, @@ -91,7 +90,7 @@ robot_cfg: cspace: joint_names: [ - # Right arm (7 DoF) — virtual base joints excluded: world_link≡pelvis at joint=0 + # Right arm (7 DoF) — virtual base joints locked, not in cspace "right_shoulder_pitch_joint", "right_shoulder_roll_joint", "right_shoulder_yaw_joint", diff --git a/lw_benchhub/autosim/robot_profiles.py b/lw_benchhub/autosim/robot_profiles.py index 23929875..49beddc2 100644 --- a/lw_benchhub/autosim/robot_profiles.py +++ b/lw_benchhub/autosim/robot_profiles.py @@ -121,7 +121,7 @@ def _make_g1_action_adapter(): profile_id="g1_loco_right", robot_name="G1-Loco-Controller", action_adapter_factory=_make_g1_action_adapter, - motion_planner_robot_config_file="g1_right.yml", + motion_planner_robot_config_file="g1_right_ee.yml", robot_base_link_name="pelvis", ee_link_name="right_wrist_yaw_link", curobo_asset_path=str(AUTOSIM_CONTENT_ROOT / "assets" / "robot" / "g1"), From b080c67a964778ebc85a09f0afc6aafc8a10ca4d Mon Sep 17 00:00:00 2001 From: Zihan Gao Date: Mon, 4 May 2026 12:43:18 +0800 Subject: [PATCH 16/32] Change loco folder name. --- .../autosim/{isaaclab_tasks => robot_env_configs}/__init__.py | 0 .../{isaaclab_tasks => robot_env_configs}/g1_autosim_cfg.py | 0 lw_benchhub/autosim/robot_profiles.py | 2 +- 3 files changed, 1 insertion(+), 1 deletion(-) rename lw_benchhub/autosim/{isaaclab_tasks => robot_env_configs}/__init__.py (100%) rename lw_benchhub/autosim/{isaaclab_tasks => robot_env_configs}/g1_autosim_cfg.py (100%) diff --git a/lw_benchhub/autosim/isaaclab_tasks/__init__.py b/lw_benchhub/autosim/robot_env_configs/__init__.py similarity index 100% rename from lw_benchhub/autosim/isaaclab_tasks/__init__.py rename to lw_benchhub/autosim/robot_env_configs/__init__.py diff --git a/lw_benchhub/autosim/isaaclab_tasks/g1_autosim_cfg.py b/lw_benchhub/autosim/robot_env_configs/g1_autosim_cfg.py similarity index 100% rename from lw_benchhub/autosim/isaaclab_tasks/g1_autosim_cfg.py rename to lw_benchhub/autosim/robot_env_configs/g1_autosim_cfg.py diff --git a/lw_benchhub/autosim/robot_profiles.py b/lw_benchhub/autosim/robot_profiles.py index 49beddc2..ddb020be 100644 --- a/lw_benchhub/autosim/robot_profiles.py +++ b/lw_benchhub/autosim/robot_profiles.py @@ -76,7 +76,7 @@ def motion_planner_robot_config_file(self) -> str: def _setup_g1_env_cfg(env_cfg) -> None: - from lw_benchhub.autosim.isaaclab_tasks.g1_autosim_cfg import ( + from lw_benchhub.autosim.robot_env_configs.g1_autosim_cfg import ( G1ActionsCfg, G1ObservationsCfg, G1EventCfg, ) env_cfg.actions = G1ActionsCfg() From 867297e3881c410036527b93758b09e564e2c28a Mon Sep 17 00:00:00 2001 From: Zihan Gao Date: Mon, 4 May 2026 15:05:05 +0800 Subject: [PATCH 17/32] Fix bugs in boil_kettle --- lw_benchhub/autosim/pipelines/kettle_boiling.py | 16 +++++++++++++--- lw_benchhub/autosim/pipelines/open_fridge.py | 2 +- 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/lw_benchhub/autosim/pipelines/kettle_boiling.py b/lw_benchhub/autosim/pipelines/kettle_boiling.py index c8c1f24b..8b14c8f7 100644 --- a/lw_benchhub/autosim/pipelines/kettle_boiling.py +++ b/lw_benchhub/autosim/pipelines/kettle_boiling.py @@ -56,6 +56,14 @@ def _x7s_get_obj_cfgs(self): )] +def _x7s_reset_env(pipeline) -> None: + obj = pipeline._env.scene["obj"] + obj.write_root_pose_to_sim( + torch.tensor([[2.0, -0.56, 1.1, 0.0, 0.0, 0.0, 1.0]], device=pipeline._env.device) + ) + obj.reset() + + def _g1_get_obj_cfgs(self): return [ dict( @@ -108,7 +116,7 @@ def _g1_after_env_created(pipeline, env) -> None: reach_extra_target_mode="keep_initial_relative_offset", object_reach_target_poses={ "obj": [ - torch.tensor([0.0, 0.09, 0.15, 0.707, 0.0, 0.0, -0.707]), + torch.tensor([0.0, 0.09, 0.10, 0.707, 0.0, 0.0, -0.707]), ], "stovetop_main_group": [ torch.tensor([-0.0, -0.045, 0.24, 0.707, 0.0, 0.0, 0.707]), @@ -117,19 +125,21 @@ def _g1_after_env_created(pipeline, env) -> None: init_state_pos_delta=(0.0, -0.8, 0.01), skill_cfg_fn=_x7s_skill_cfg, get_obj_cfgs_fn=_x7s_get_obj_cfgs, + reset_env_fn=_x7s_reset_env, ), "g1_loco_left": TaskRobotOverride( object_reach_target_poses={ "obj": [ - torch.tensor([0.0, 0.0, 0.05, 0.707, 0.0, 0.707, 0.0]), + torch.tensor([0.0, 0.09, 0.10, 0.707, 0.0, 0.0, -0.707]), ], "stovetop_main_group": [ - torch.tensor([0.0, -0.15, 0.20, 1.0, 0.0, 0.0, 0.0]), + torch.tensor([-0.0, -0.045, 0.24, 0.707, 0.0, 0.0, 0.707]), ], }, init_state_pos_delta=(0.0, -0.8, 0.01), skill_cfg_fn=_g1_skill_cfg, get_obj_cfgs_fn=_x7s_get_obj_cfgs, + reset_env_fn=_x7s_reset_env, after_env_created_fn=_g1_after_env_created, ), } diff --git a/lw_benchhub/autosim/pipelines/open_fridge.py b/lw_benchhub/autosim/pipelines/open_fridge.py index 4ec35273..b693e830 100644 --- a/lw_benchhub/autosim/pipelines/open_fridge.py +++ b/lw_benchhub/autosim/pipelines/open_fridge.py @@ -147,7 +147,7 @@ def load_env(self) -> ManagerBasedEnv: num_envs=1, use_fabric=False, first_person_view=False, - enable_cameras=True, + enable_cameras=False, execute_mode=ExecuteMode.TELEOP, usd_simplify=False, seed=42, From c72fe2f1535d1c5fd7db46e5b9476b3d44f1ad48 Mon Sep 17 00:00:00 2001 From: Zihan Gao Date: Mon, 4 May 2026 17:53:29 +0800 Subject: [PATCH 18/32] Complete G1kettle_boiling --- .../action_adapters/g1_action_adapter.py | 30 +++++++++++++++++-- .../action_adapters/g1_action_adapter_cfg.py | 2 +- .../autosim/pipelines/kettle_boiling.py | 14 ++++----- 3 files changed, 35 insertions(+), 11 deletions(-) diff --git a/lw_benchhub/autosim/action_adapters/g1_action_adapter.py b/lw_benchhub/autosim/action_adapters/g1_action_adapter.py index 68e11b32..82ae00e3 100644 --- a/lw_benchhub/autosim/action_adapters/g1_action_adapter.py +++ b/lw_benchhub/autosim/action_adapters/g1_action_adapter.py @@ -27,9 +27,9 @@ def __init__(self, cfg: G1ActionAdapterCfg): super().__init__(cfg) self.register_apply_method("moveto", self._apply_moveto) self.register_apply_method("reach", self._apply_reach) - self.register_apply_method("lift", self._apply_reach) - self.register_apply_method("pull", self._apply_reach) - self.register_apply_method("push", self._apply_reach) + self.register_apply_method("lift", self._apply_reach_keep_gripper) + self.register_apply_method("pull", self._apply_reach_keep_gripper) + self.register_apply_method("push", self._apply_reach_keep_gripper) self.register_apply_method("grasp", self._apply_gripper) self.register_apply_method("ungrasp", self._apply_gripper) @@ -82,6 +82,30 @@ def _apply_reach(self, skill_output: SkillOutput, env: ManagerBasedEnv) -> torch action[3] = 1.0 # mode=1: squat/stance — keep legs fixed during arm motion action[4:11] = target_joint_pos[r_arm_ids] action[11:18] = target_joint_pos[l_arm_ids] + # Keep fingers open during reach + finger_angles = torch.tensor(self.cfg.finger_open_angles, dtype=torch.float32, device=env.device) + action[18:32] = finger_angles + + return action + + def _apply_reach_keep_gripper(self, skill_output: SkillOutput, env: ManagerBasedEnv) -> torch.Tensor: + """Write cuRobo joint positions into the arm action terms, keep gripper state unchanged.""" + target_joint_pos = skill_output.action + + last_action = env.action_manager.action + action = last_action[0, :].clone() + + robot = env.scene["robot"] + r_arm_ids, _ = robot.find_joints(env.action_manager.get_term("right_arm_action").cfg.joint_names) + l_arm_ids, _ = robot.find_joints(env.action_manager.get_term("left_arm_action").cfg.joint_names) + + action[0] = 0.0 + action[1] = 0.0 + action[2] = 0.0 + action[3] = 1.0 # mode=1: squat/stance — keep legs fixed during arm motion + action[4:11] = target_joint_pos[r_arm_ids] + action[11:18] = target_joint_pos[l_arm_ids] + # Keep finger state unchanged (don't modify action[18:32]) return action diff --git a/lw_benchhub/autosim/action_adapters/g1_action_adapter_cfg.py b/lw_benchhub/autosim/action_adapters/g1_action_adapter_cfg.py index 39a4c8b2..b1e6a049 100644 --- a/lw_benchhub/autosim/action_adapters/g1_action_adapter_cfg.py +++ b/lw_benchhub/autosim/action_adapters/g1_action_adapter_cfg.py @@ -15,7 +15,7 @@ class G1ActionAdapterCfg(ActionAdapterCfg): base_y_joint_name: str = "base_y_joint" base_yaw_joint_name: str = "base_yaw_joint" - finger_close_angles: tuple = (1.0, 1.0, 1.0, 1.0, 0.8, 0.8, 0.8, 1.0, 1.0, 1.0, 1.0, 0.8, 0.8, 0.8) + finger_close_angles: tuple = (-1.2, -1.2, -1.2, -1.2, -1.0, -1.0, -1.0, -1.2, -1.2, -1.2, -1.2, -1.0, -1.0, -1.0) """Per-joint finger angles (rad) when gripper is closed.""" finger_open_angles: tuple = (0.0,) * 14 """Per-joint finger angles (rad) when gripper is open.""" diff --git a/lw_benchhub/autosim/pipelines/kettle_boiling.py b/lw_benchhub/autosim/pipelines/kettle_boiling.py index 8b14c8f7..4d96f253 100644 --- a/lw_benchhub/autosim/pipelines/kettle_boiling.py +++ b/lw_benchhub/autosim/pipelines/kettle_boiling.py @@ -37,7 +37,7 @@ def _g1_skill_cfg(cfg) -> None: cfg.skills.moveto.extra_cfg.goal_tolerance = 0.30 cfg.skills.moveto.extra_cfg.yaw_tolerance = 0.01 cfg.skills.moveto.extra_cfg.use_dwa = False - cfg.skills.moveto.extra_cfg.per_object_sampling_radius = {"obj": 0.60, "stovetop_main_group": 0.30} + cfg.skills.moveto.extra_cfg.per_object_sampling_radius = {"obj": 0.52, "stovetop_main_group": 0.30} def _x7s_get_obj_cfgs(self): @@ -130,16 +130,16 @@ def _g1_after_env_created(pipeline, env) -> None: "g1_loco_left": TaskRobotOverride( object_reach_target_poses={ "obj": [ - torch.tensor([0.0, 0.09, 0.10, 0.707, 0.0, 0.0, -0.707]), + torch.tensor([0.003, 0.171, 0.084, 0.884, 0.0, 0.0, -0.468]), ], "stovetop_main_group": [ - torch.tensor([-0.0, -0.045, 0.24, 0.707, 0.0, 0.0, 0.707]), + torch.tensor([0.0, -0.15, 0.20, 1.0, 0.0, 0.0, 0.0]), ], }, init_state_pos_delta=(0.0, -0.8, 0.01), skill_cfg_fn=_g1_skill_cfg, - get_obj_cfgs_fn=_x7s_get_obj_cfgs, - reset_env_fn=_x7s_reset_env, + get_obj_cfgs_fn=_g1_get_obj_cfgs, + reset_env_fn=_g1_reset_env, after_env_created_fn=_g1_after_env_created, ), } @@ -167,12 +167,12 @@ def __post_init__(self): if resolved_robot.override.skill_cfg_fn: resolved_robot.override.skill_cfg_fn(self) - self.skills.lift.extra_cfg.move_offset = 0.15 + self.skills.lift.extra_cfg.move_offset = 0.10 self.skills.lift.extra_cfg.move_axis = "+z" self.motion_planner.enable_dynamic_world_sync = True self.occupancy_map.floor_prim_suffix = "Scene/floor_room" - self.max_steps = 800 + self.max_steps = 1500 self.motion_planner.world_ignore_subffixes = ["Scene/floor_room"] self.motion_planner.world_only_subffixes = [ From 3407ead9ca2ad0914cfcff404d297f210f04287c Mon Sep 17 00:00:00 2001 From: Zihan Gao Date: Mon, 4 May 2026 20:01:36 +0800 Subject: [PATCH 19/32] Fix kette reach bugs --- lw_benchhub/autosim/pipelines/kettle_boiling.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/lw_benchhub/autosim/pipelines/kettle_boiling.py b/lw_benchhub/autosim/pipelines/kettle_boiling.py index 4d96f253..80f94a87 100644 --- a/lw_benchhub/autosim/pipelines/kettle_boiling.py +++ b/lw_benchhub/autosim/pipelines/kettle_boiling.py @@ -37,7 +37,7 @@ def _g1_skill_cfg(cfg) -> None: cfg.skills.moveto.extra_cfg.goal_tolerance = 0.30 cfg.skills.moveto.extra_cfg.yaw_tolerance = 0.01 cfg.skills.moveto.extra_cfg.use_dwa = False - cfg.skills.moveto.extra_cfg.per_object_sampling_radius = {"obj": 0.52, "stovetop_main_group": 0.30} + cfg.skills.moveto.extra_cfg.per_object_sampling_radius = {"obj": 0.53, "stovetop_main_group": 0.55} def _x7s_get_obj_cfgs(self): @@ -130,7 +130,7 @@ def _g1_after_env_created(pipeline, env) -> None: "g1_loco_left": TaskRobotOverride( object_reach_target_poses={ "obj": [ - torch.tensor([0.003, 0.171, 0.084, 0.884, 0.0, 0.0, -0.468]), + torch.tensor([-0.05, 0.22, 0.082, 0.6488, 0.0, 0.0, -0.7608]), ], "stovetop_main_group": [ torch.tensor([0.0, -0.15, 0.20, 1.0, 0.0, 0.0, 0.0]), From 65ecdae1dc8e53f34bde24e0423c56acc395ae80 Mon Sep 17 00:00:00 2001 From: Zihan Gao Date: Tue, 5 May 2026 09:22:11 +0800 Subject: [PATCH 20/32] Finish kettle_boiling --- lw_benchhub/autosim/pipelines/kettle_boiling.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/lw_benchhub/autosim/pipelines/kettle_boiling.py b/lw_benchhub/autosim/pipelines/kettle_boiling.py index 80f94a87..1c28bb41 100644 --- a/lw_benchhub/autosim/pipelines/kettle_boiling.py +++ b/lw_benchhub/autosim/pipelines/kettle_boiling.py @@ -37,7 +37,8 @@ def _g1_skill_cfg(cfg) -> None: cfg.skills.moveto.extra_cfg.goal_tolerance = 0.30 cfg.skills.moveto.extra_cfg.yaw_tolerance = 0.01 cfg.skills.moveto.extra_cfg.use_dwa = False - cfg.skills.moveto.extra_cfg.per_object_sampling_radius = {"obj": 0.53, "stovetop_main_group": 0.55} + cfg.skills.moveto.extra_cfg.per_object_sampling_radius = {"obj": 0.53, "stovetop_main_group": 0.45} + cfg.skills.moveto.extra_cfg.per_object_yaw_tolerance = {"obj": 0.01, "stovetop_main_group": 0.15} def _x7s_get_obj_cfgs(self): @@ -133,7 +134,7 @@ def _g1_after_env_created(pipeline, env) -> None: torch.tensor([-0.05, 0.22, 0.082, 0.6488, 0.0, 0.0, -0.7608]), ], "stovetop_main_group": [ - torch.tensor([0.0, -0.15, 0.20, 1.0, 0.0, 0.0, 0.0]), + torch.tensor([-0.0592, -0.3930, 0.24, 0.6488, 0.0000, 0.0000, 0.7608]), ], }, init_state_pos_delta=(0.0, -0.8, 0.01), From ad7a28eb21fb431fefe208ddcfe608cc4e453853 Mon Sep 17 00:00:00 2001 From: Zihan Gao Date: Tue, 5 May 2026 09:34:16 +0800 Subject: [PATCH 21/32] change G1 hand direction in kettle_boiling --- lw_benchhub/autosim/pipelines/kettle_boiling.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lw_benchhub/autosim/pipelines/kettle_boiling.py b/lw_benchhub/autosim/pipelines/kettle_boiling.py index 1c28bb41..3f007dc5 100644 --- a/lw_benchhub/autosim/pipelines/kettle_boiling.py +++ b/lw_benchhub/autosim/pipelines/kettle_boiling.py @@ -131,7 +131,7 @@ def _g1_after_env_created(pipeline, env) -> None: "g1_loco_left": TaskRobotOverride( object_reach_target_poses={ "obj": [ - torch.tensor([-0.05, 0.22, 0.082, 0.6488, 0.0, 0.0, -0.7608]), + torch.tensor([-0.05, 0.22, 0.08, 0.649, 0.0, 0.0, -0.761]), ], "stovetop_main_group": [ torch.tensor([-0.0592, -0.3930, 0.24, 0.6488, 0.0000, 0.0000, 0.7608]), From b6bd02c845bd88ef12f2f36bd2f43a711f5a57bc Mon Sep 17 00:00:00 2001 From: Zihan Gao Date: Tue, 5 May 2026 10:26:40 +0800 Subject: [PATCH 22/32] Complete kettle_boiling --- lw_benchhub/autosim/pipelines/kettle_boiling.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/lw_benchhub/autosim/pipelines/kettle_boiling.py b/lw_benchhub/autosim/pipelines/kettle_boiling.py index 3f007dc5..33bacfa2 100644 --- a/lw_benchhub/autosim/pipelines/kettle_boiling.py +++ b/lw_benchhub/autosim/pipelines/kettle_boiling.py @@ -38,7 +38,7 @@ def _g1_skill_cfg(cfg) -> None: cfg.skills.moveto.extra_cfg.yaw_tolerance = 0.01 cfg.skills.moveto.extra_cfg.use_dwa = False cfg.skills.moveto.extra_cfg.per_object_sampling_radius = {"obj": 0.53, "stovetop_main_group": 0.45} - cfg.skills.moveto.extra_cfg.per_object_yaw_tolerance = {"obj": 0.01, "stovetop_main_group": 0.15} + cfg.skills.moveto.extra_cfg.per_object_yaw_tolerance = {"obj": 0.01, "stovetop_main_group": 0.4} def _x7s_get_obj_cfgs(self): @@ -131,10 +131,10 @@ def _g1_after_env_created(pipeline, env) -> None: "g1_loco_left": TaskRobotOverride( object_reach_target_poses={ "obj": [ - torch.tensor([-0.05, 0.22, 0.08, 0.649, 0.0, 0.0, -0.761]), + torch.tensor([-0.05, 0.22, 0.08, 0.707, 0.0, 0.0, -0.707]), ], "stovetop_main_group": [ - torch.tensor([-0.0592, -0.3930, 0.24, 0.6488, 0.0000, 0.0000, 0.7608]), + torch.tensor([-0.0, -0.20, 0.2, 0.707, 0.0, 0.0, 0.707]), ], }, init_state_pos_delta=(0.0, -0.8, 0.01), From f3f308031b51513f019dc1a79af664f509175d8a Mon Sep 17 00:00:00 2001 From: Zihan Gao Date: Tue, 5 May 2026 11:37:02 +0800 Subject: [PATCH 23/32] refact --- lw_benchhub/autosim/__init__.py | 10 ++-------- lw_benchhub/autosim/pipelines/cheesy_bread.py | 2 ++ lw_benchhub/autosim/pipelines/kettle_boiling.py | 1 + lw_benchhub/autosim/pipelines/open_fridge.py | 17 ++--------------- lw_benchhub/autosim/robot_profiles.py | 10 ++++++++++ 5 files changed, 17 insertions(+), 23 deletions(-) diff --git a/lw_benchhub/autosim/__init__.py b/lw_benchhub/autosim/__init__.py index 17df6443..0e765b60 100644 --- a/lw_benchhub/autosim/__init__.py +++ b/lw_benchhub/autosim/__init__.py @@ -39,9 +39,9 @@ # G1 pipelines register_pipeline( - id="LWBenchhub-Autosim-G1LeftOpenFridgePipeline-v0", + id="LWBenchhub-Autosim-G1OpenFridgePipeline-v0", entry_point=f"{__name__}.pipelines.open_fridge:OpenFridgePipeline", - cfg_entry_point=f"{__name__}.pipelines.open_fridge:G1LeftOpenFridgePipelineCfg", + cfg_entry_point=f"{__name__}.pipelines.open_fridge:G1OpenFridgePipelineCfg", ) register_pipeline( @@ -73,9 +73,3 @@ entry_point=f"{__name__}.pipelines.close_oven:CloseOvenPipeline", cfg_entry_point=f"{__name__}.pipelines.close_oven:G1CloseOvenPipelineCfg", ) - -register_pipeline( - id="LWBenchhub-Autosim-G1RightOpenFridgePipeline-v0", - entry_point=f"{__name__}.pipelines.open_fridge:OpenFridgePipeline", - cfg_entry_point=f"{__name__}.pipelines.open_fridge:G1RightOpenFridgePipelineCfg", -) diff --git a/lw_benchhub/autosim/pipelines/cheesy_bread.py b/lw_benchhub/autosim/pipelines/cheesy_bread.py index 913cfefc..204d634e 100644 --- a/lw_benchhub/autosim/pipelines/cheesy_bread.py +++ b/lw_benchhub/autosim/pipelines/cheesy_bread.py @@ -36,6 +36,8 @@ def _g1_skill_cfg(cfg) -> None: cfg.skills.moveto.extra_cfg.goal_tolerance = 0.30 cfg.skills.moveto.extra_cfg.yaw_tolerance = 0.01 cfg.skills.moveto.extra_cfg.use_dwa = False + cfg.skills.moveto.extra_cfg.per_object_sampling_radius = {"cheese": 0.53, "bread": 0.53} + cfg.skills.moveto.extra_cfg.per_object_yaw_tolerance = {"cheese": 0.01, "bread": 0.1} TASK_ROBOT_OVERRIDES: dict[str, TaskRobotOverride] = { diff --git a/lw_benchhub/autosim/pipelines/kettle_boiling.py b/lw_benchhub/autosim/pipelines/kettle_boiling.py index 33bacfa2..dd9897e7 100644 --- a/lw_benchhub/autosim/pipelines/kettle_boiling.py +++ b/lw_benchhub/autosim/pipelines/kettle_boiling.py @@ -142,6 +142,7 @@ def _g1_after_env_created(pipeline, env) -> None: get_obj_cfgs_fn=_g1_get_obj_cfgs, reset_env_fn=_g1_reset_env, after_env_created_fn=_g1_after_env_created, + finger_close_angles=(-1.2, -1.2, -1.2, -1.2, -1.0, -1.0, -1.0, -1.2, -1.2, -1.2, -1.2, -1.0, -1.0, -1.0), ), } diff --git a/lw_benchhub/autosim/pipelines/open_fridge.py b/lw_benchhub/autosim/pipelines/open_fridge.py index b693e830..18c4d5b3 100644 --- a/lw_benchhub/autosim/pipelines/open_fridge.py +++ b/lw_benchhub/autosim/pipelines/open_fridge.py @@ -53,15 +53,6 @@ def _g1_skill_cfg(cfg) -> None: init_state_pos_delta=(-0.45, -0.7, 0.0), skill_cfg_fn=_x7s_skill_cfg, ), - "g1_loco_left": TaskRobotOverride( - object_reach_target_poses={ - "fridge_main_group": [ - torch.tensor([0.047, -0.429, 0.25, 0.707, 0.0, 0.0, 0.707]), - ], - }, - init_state_pos_delta=(0.0, -1.5, 0.0), - skill_cfg_fn=_g1_skill_cfg, - ), "g1_loco_right": TaskRobotOverride( object_reach_target_poses={ "fridge_main_group": [ @@ -70,6 +61,7 @@ def _g1_skill_cfg(cfg) -> None: }, init_state_pos_delta=(-0.15, -1.5, 0.0), skill_cfg_fn=_g1_skill_cfg, + finger_close_angles=(1.0, 1.0, 1.0, 1.0, 0.8, 0.8, 0.8, 1.0, 1.0, 1.0, 1.0, 0.8, 0.8, 0.8), ), } @@ -107,12 +99,7 @@ def __post_init__(self): @configclass -class G1LeftOpenFridgePipelineCfg(OpenFridgePipelineCfg): - robot_profile: str = "g1_loco_left" - - -@configclass -class G1RightOpenFridgePipelineCfg(OpenFridgePipelineCfg): +class G1OpenFridgePipelineCfg(OpenFridgePipelineCfg): robot_profile: str = "g1_loco_right" diff --git a/lw_benchhub/autosim/robot_profiles.py b/lw_benchhub/autosim/robot_profiles.py index ddb020be..d71427d1 100644 --- a/lw_benchhub/autosim/robot_profiles.py +++ b/lw_benchhub/autosim/robot_profiles.py @@ -51,6 +51,10 @@ class TaskRobotOverride: get_obj_cfgs_fn: Callable | None = None after_env_created_fn: Callable | None = None reset_env_fn: Callable | None = None + finger_close_angles: tuple[float, ...] | None = None + """Per-task override for gripper closed angles. If None, uses action adapter default.""" + finger_open_angles: tuple[float, ...] | None = None + """Per-task override for gripper open angles. If None, uses action adapter default.""" @dataclass @@ -156,6 +160,12 @@ def configure_robot_runtime_settings(pipeline_cfg, resolved_robot: ResolvedRobot # Action adapter pipeline_cfg.action_adapter = resolved_robot.profile.action_adapter_factory() + # Apply per-task finger angle overrides if specified + if resolved_robot.override.finger_close_angles is not None: + pipeline_cfg.action_adapter.finger_close_angles = resolved_robot.override.finger_close_angles + if resolved_robot.override.finger_open_angles is not None: + pipeline_cfg.action_adapter.finger_open_angles = resolved_robot.override.finger_open_angles + # Motion planner pipeline_cfg.motion_planner.robot_config_file = resolved_robot.motion_planner_robot_config_file pipeline_cfg.motion_planner.curobo_asset_path = ( From 8ebc2afbeff91af83459fdfac6cbfa969e2a2426 Mon Sep 17 00:00:00 2001 From: Zihan Gao Date: Tue, 5 May 2026 11:53:03 +0800 Subject: [PATCH 24/32] feat: implement per-skill per-hand finger configuration system Add flexible finger configuration that supports different finger angles for different skill types (lift, push, pull) and different hands (left, right). Changes: - TaskRobotOverride: Add skill_finger_configs field with nested structure {"left_hand": {"lift": (7 vals), ...}, "right_hand": {...}} - G1ActionAdapter: Determine active hand from ee_link_name, apply skill-specific finger configs for lift/push/pull skills - configure_robot_runtime_settings: Pass ee_link_name and skill_finger_configs to action adapter - open_fridge: Use right_hand push/pull configs (positive angles for handle grasp) - kettle_boiling: Use left_hand lift config (negative angles for object grasp) This design supports future scenarios like right-hand kettle grasp or left-hand door push/pull by simply adding configs to the nested dict. Co-Authored-By: Claude Opus 4.6 --- .../action_adapters/g1_action_adapter.py | 59 ++++++++++++++++++- .../action_adapters/g1_action_adapter_cfg.py | 7 ++- .../autosim/pipelines/kettle_boiling.py | 6 +- lw_benchhub/autosim/pipelines/open_fridge.py | 7 ++- lw_benchhub/autosim/robot_profiles.py | 15 ++--- 5 files changed, 79 insertions(+), 15 deletions(-) diff --git a/lw_benchhub/autosim/action_adapters/g1_action_adapter.py b/lw_benchhub/autosim/action_adapters/g1_action_adapter.py index 82ae00e3..2ee2267a 100644 --- a/lw_benchhub/autosim/action_adapters/g1_action_adapter.py +++ b/lw_benchhub/autosim/action_adapters/g1_action_adapter.py @@ -25,11 +25,20 @@ class G1ActionAdapter(ActionAdapterBase): def __init__(self, cfg: G1ActionAdapterCfg): super().__init__(cfg) + + # Determine active hand from ee_link_name + self._active_hand = None + if cfg.ee_link_name: + if "left" in cfg.ee_link_name.lower(): + self._active_hand = "left_hand" + elif "right" in cfg.ee_link_name.lower(): + self._active_hand = "right_hand" + self.register_apply_method("moveto", self._apply_moveto) self.register_apply_method("reach", self._apply_reach) - self.register_apply_method("lift", self._apply_reach_keep_gripper) - self.register_apply_method("pull", self._apply_reach_keep_gripper) - self.register_apply_method("push", self._apply_reach_keep_gripper) + self.register_apply_method("lift", lambda so, env: self._apply_reach_with_skill_fingers(so, env, "lift")) + self.register_apply_method("pull", lambda so, env: self._apply_reach_with_skill_fingers(so, env, "pull")) + self.register_apply_method("push", lambda so, env: self._apply_reach_with_skill_fingers(so, env, "push")) self.register_apply_method("grasp", self._apply_gripper) self.register_apply_method("ungrasp", self._apply_gripper) @@ -109,6 +118,50 @@ def _apply_reach_keep_gripper(self, skill_output: SkillOutput, env: ManagerBased return action + def _apply_reach_with_skill_fingers(self, skill_output: SkillOutput, env: ManagerBasedEnv, skill_name: str) -> torch.Tensor: + """Write cuRobo joint positions and apply skill-specific finger configuration.""" + target_joint_pos = skill_output.action + + last_action = env.action_manager.action + action = last_action[0, :].clone() + + robot = env.scene["robot"] + r_arm_ids, _ = robot.find_joints(env.action_manager.get_term("right_arm_action").cfg.joint_names) + l_arm_ids, _ = robot.find_joints(env.action_manager.get_term("left_arm_action").cfg.joint_names) + + action[0] = 0.0 + action[1] = 0.0 + action[2] = 0.0 + action[3] = 1.0 # mode=1: squat/stance + action[4:11] = target_joint_pos[r_arm_ids] + action[11:18] = target_joint_pos[l_arm_ids] + + # Apply skill-specific finger configuration + finger_angles = self._get_skill_finger_angles(skill_name) + if finger_angles is not None: + action[18:32] = torch.tensor(finger_angles, dtype=torch.float32, device=env.device) + # else: keep current finger state + + return action + + def _get_skill_finger_angles(self, skill_name: str) -> tuple[float, ...] | None: + """Get skill-specific finger angles for the active hand, or None if not configured.""" + if self.cfg.skill_finger_configs is None or self._active_hand is None: + return None + + hand_configs = self.cfg.skill_finger_configs.get(self._active_hand, {}) + hand_7_angles = hand_configs.get(skill_name) + + if hand_7_angles is None: + return None + + # Construct 14-joint tuple: (right_hand_7, left_hand_7) + zeros = (0.0,) * 7 + if self._active_hand == "right_hand": + return hand_7_angles + zeros + else: # left_hand + return zeros + hand_7_angles + # ------------------------------------------------------------------ # Gripper (three-finger open / close) # ------------------------------------------------------------------ diff --git a/lw_benchhub/autosim/action_adapters/g1_action_adapter_cfg.py b/lw_benchhub/autosim/action_adapters/g1_action_adapter_cfg.py index b1e6a049..67a26a1f 100644 --- a/lw_benchhub/autosim/action_adapters/g1_action_adapter_cfg.py +++ b/lw_benchhub/autosim/action_adapters/g1_action_adapter_cfg.py @@ -15,7 +15,12 @@ class G1ActionAdapterCfg(ActionAdapterCfg): base_y_joint_name: str = "base_y_joint" base_yaw_joint_name: str = "base_yaw_joint" + ee_link_name: str = "" + """End-effector link name, used to determine active hand (left/right).""" + finger_close_angles: tuple = (-1.2, -1.2, -1.2, -1.2, -1.0, -1.0, -1.0, -1.2, -1.2, -1.2, -1.2, -1.0, -1.0, -1.0) - """Per-joint finger angles (rad) when gripper is closed.""" + """Per-joint finger angles (rad) when gripper is closed (default fallback).""" finger_open_angles: tuple = (0.0,) * 14 """Per-joint finger angles (rad) when gripper is open.""" + skill_finger_configs: dict[str, dict[str, tuple]] | None = None + """Per-skill per-hand finger configs. Format: {"left_hand": {"lift": (7 vals), ...}, "right_hand": {...}}""" diff --git a/lw_benchhub/autosim/pipelines/kettle_boiling.py b/lw_benchhub/autosim/pipelines/kettle_boiling.py index dd9897e7..370a5af9 100644 --- a/lw_benchhub/autosim/pipelines/kettle_boiling.py +++ b/lw_benchhub/autosim/pipelines/kettle_boiling.py @@ -142,7 +142,11 @@ def _g1_after_env_created(pipeline, env) -> None: get_obj_cfgs_fn=_g1_get_obj_cfgs, reset_env_fn=_g1_reset_env, after_env_created_fn=_g1_after_env_created, - finger_close_angles=(-1.2, -1.2, -1.2, -1.2, -1.0, -1.0, -1.0, -1.2, -1.2, -1.2, -1.2, -1.0, -1.0, -1.0), + skill_finger_configs={ + "left_hand": { + "lift": (-1.2, -1.2, -1.2, -1.2, -1.0, -1.0, -1.0), + } + }, ), } diff --git a/lw_benchhub/autosim/pipelines/open_fridge.py b/lw_benchhub/autosim/pipelines/open_fridge.py index 18c4d5b3..0857bb55 100644 --- a/lw_benchhub/autosim/pipelines/open_fridge.py +++ b/lw_benchhub/autosim/pipelines/open_fridge.py @@ -61,7 +61,12 @@ def _g1_skill_cfg(cfg) -> None: }, init_state_pos_delta=(-0.15, -1.5, 0.0), skill_cfg_fn=_g1_skill_cfg, - finger_close_angles=(1.0, 1.0, 1.0, 1.0, 0.8, 0.8, 0.8, 1.0, 1.0, 1.0, 1.0, 0.8, 0.8, 0.8), + skill_finger_configs={ + "right_hand": { + "push": (1.0, 1.0, 1.0, 1.0, 0.8, 0.8, 0.8), + "pull": (1.0, 1.0, 1.0, 1.0, 0.8, 0.8, 0.8), + } + }, ), } diff --git a/lw_benchhub/autosim/robot_profiles.py b/lw_benchhub/autosim/robot_profiles.py index d71427d1..7161d4bd 100644 --- a/lw_benchhub/autosim/robot_profiles.py +++ b/lw_benchhub/autosim/robot_profiles.py @@ -51,10 +51,8 @@ class TaskRobotOverride: get_obj_cfgs_fn: Callable | None = None after_env_created_fn: Callable | None = None reset_env_fn: Callable | None = None - finger_close_angles: tuple[float, ...] | None = None - """Per-task override for gripper closed angles. If None, uses action adapter default.""" - finger_open_angles: tuple[float, ...] | None = None - """Per-task override for gripper open angles. If None, uses action adapter default.""" + skill_finger_configs: dict[str, dict[str, tuple[float, ...]]] | None = None + """Per-skill per-hand finger configurations. Format: {"left_hand": {"lift": (7 values), "push": (7 values)}, "right_hand": {...}}""" @dataclass @@ -159,12 +157,11 @@ def configure_robot_runtime_settings(pipeline_cfg, resolved_robot: ResolvedRobot # Action adapter pipeline_cfg.action_adapter = resolved_robot.profile.action_adapter_factory() + pipeline_cfg.action_adapter.ee_link_name = resolved_robot.ee_link_name - # Apply per-task finger angle overrides if specified - if resolved_robot.override.finger_close_angles is not None: - pipeline_cfg.action_adapter.finger_close_angles = resolved_robot.override.finger_close_angles - if resolved_robot.override.finger_open_angles is not None: - pipeline_cfg.action_adapter.finger_open_angles = resolved_robot.override.finger_open_angles + # Apply per-skill per-hand finger angle configs if specified + if resolved_robot.override.skill_finger_configs is not None: + pipeline_cfg.action_adapter.skill_finger_configs = resolved_robot.override.skill_finger_configs # Motion planner pipeline_cfg.motion_planner.robot_config_file = resolved_robot.motion_planner_robot_config_file From dcfb7e6fcd47debeb9118a93d33f4f9b5d66b555 Mon Sep 17 00:00:00 2001 From: Zihan Gao Date: Wed, 6 May 2026 09:14:36 +0800 Subject: [PATCH 25/32] fix: make grasp skill also use skill_finger_configs The grasp skill was still using the global default finger_close_angles (negative values) instead of the per-task skill_finger_configs. Now _apply_gripper checks skill_finger_configs["grasp"] first, falling back to the default only if not configured. Added "grasp" entries to open_fridge (positive) and kettle_boiling (negative). Co-Authored-By: Claude Opus 4.6 --- lw_benchhub/autosim/action_adapters/g1_action_adapter.py | 9 ++++++++- lw_benchhub/autosim/pipelines/kettle_boiling.py | 1 + lw_benchhub/autosim/pipelines/open_fridge.py | 1 + 3 files changed, 10 insertions(+), 1 deletion(-) diff --git a/lw_benchhub/autosim/action_adapters/g1_action_adapter.py b/lw_benchhub/autosim/action_adapters/g1_action_adapter.py index 2ee2267a..6d4e7ff3 100644 --- a/lw_benchhub/autosim/action_adapters/g1_action_adapter.py +++ b/lw_benchhub/autosim/action_adapters/g1_action_adapter.py @@ -170,7 +170,14 @@ def _apply_gripper(self, skill_output: SkillOutput, env: ManagerBasedEnv) -> tor """Set all finger joints to the closed (grasp) or open (ungrasp) position.""" gripper_signal = skill_output.action[0].item() - angles = self.cfg.finger_close_angles if gripper_signal < 0 else self.cfg.finger_open_angles + if gripper_signal < 0: + # Grasp: check skill_finger_configs first, then fall back to default + skill_angles = self._get_skill_finger_angles("grasp") + angles = skill_angles if skill_angles is not None else self.cfg.finger_close_angles + else: + # Ungrasp: use open angles + angles = self.cfg.finger_open_angles + finger_angles = torch.tensor(angles, dtype=torch.float32, device=env.device) last_action = env.action_manager.action diff --git a/lw_benchhub/autosim/pipelines/kettle_boiling.py b/lw_benchhub/autosim/pipelines/kettle_boiling.py index 370a5af9..8b963cbb 100644 --- a/lw_benchhub/autosim/pipelines/kettle_boiling.py +++ b/lw_benchhub/autosim/pipelines/kettle_boiling.py @@ -144,6 +144,7 @@ def _g1_after_env_created(pipeline, env) -> None: after_env_created_fn=_g1_after_env_created, skill_finger_configs={ "left_hand": { + "grasp": (-1.2, -1.2, -1.2, -1.2, -1.0, -1.0, -1.0), "lift": (-1.2, -1.2, -1.2, -1.2, -1.0, -1.0, -1.0), } }, diff --git a/lw_benchhub/autosim/pipelines/open_fridge.py b/lw_benchhub/autosim/pipelines/open_fridge.py index 0857bb55..e69f91df 100644 --- a/lw_benchhub/autosim/pipelines/open_fridge.py +++ b/lw_benchhub/autosim/pipelines/open_fridge.py @@ -63,6 +63,7 @@ def _g1_skill_cfg(cfg) -> None: skill_cfg_fn=_g1_skill_cfg, skill_finger_configs={ "right_hand": { + "grasp": (1.0, 1.0, 1.0, 1.0, 0.8, 0.8, 0.8), "push": (1.0, 1.0, 1.0, 1.0, 0.8, 0.8, 0.8), "pull": (1.0, 1.0, 1.0, 1.0, 0.8, 0.8, 0.8), } From 6bdad86763c6d5baff7d9704cba06edc2a2e3362 Mon Sep 17 00:00:00 2001 From: Zihan Gao Date: Wed, 6 May 2026 09:15:53 +0800 Subject: [PATCH 26/32] refactor: remove unused _apply_reach_keep_gripper method This method is no longer registered; lift/pull/push all use _apply_reach_with_skill_fingers now. Co-Authored-By: Claude Opus 4.6 --- .../action_adapters/g1_action_adapter.py | 21 ------------------- 1 file changed, 21 deletions(-) diff --git a/lw_benchhub/autosim/action_adapters/g1_action_adapter.py b/lw_benchhub/autosim/action_adapters/g1_action_adapter.py index 6d4e7ff3..483ffd80 100644 --- a/lw_benchhub/autosim/action_adapters/g1_action_adapter.py +++ b/lw_benchhub/autosim/action_adapters/g1_action_adapter.py @@ -97,27 +97,6 @@ def _apply_reach(self, skill_output: SkillOutput, env: ManagerBasedEnv) -> torch return action - def _apply_reach_keep_gripper(self, skill_output: SkillOutput, env: ManagerBasedEnv) -> torch.Tensor: - """Write cuRobo joint positions into the arm action terms, keep gripper state unchanged.""" - target_joint_pos = skill_output.action - - last_action = env.action_manager.action - action = last_action[0, :].clone() - - robot = env.scene["robot"] - r_arm_ids, _ = robot.find_joints(env.action_manager.get_term("right_arm_action").cfg.joint_names) - l_arm_ids, _ = robot.find_joints(env.action_manager.get_term("left_arm_action").cfg.joint_names) - - action[0] = 0.0 - action[1] = 0.0 - action[2] = 0.0 - action[3] = 1.0 # mode=1: squat/stance — keep legs fixed during arm motion - action[4:11] = target_joint_pos[r_arm_ids] - action[11:18] = target_joint_pos[l_arm_ids] - # Keep finger state unchanged (don't modify action[18:32]) - - return action - def _apply_reach_with_skill_fingers(self, skill_output: SkillOutput, env: ManagerBasedEnv, skill_name: str) -> torch.Tensor: """Write cuRobo joint positions and apply skill-specific finger configuration.""" target_joint_pos = skill_output.action From edded5e9242f46f2d6f752e495b88243f537b3b4 Mon Sep 17 00:00:00 2001 From: Zihan Gao Date: Wed, 6 May 2026 09:25:50 +0800 Subject: [PATCH 27/32] fix: grasp applies finger angles to both hands Match historical behavior where grasp bends both left and right hands, not just the active hand. Co-Authored-By: Claude Opus 4.6 --- .../action_adapters/g1_action_adapter.py | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/lw_benchhub/autosim/action_adapters/g1_action_adapter.py b/lw_benchhub/autosim/action_adapters/g1_action_adapter.py index 483ffd80..a294fd1e 100644 --- a/lw_benchhub/autosim/action_adapters/g1_action_adapter.py +++ b/lw_benchhub/autosim/action_adapters/g1_action_adapter.py @@ -141,6 +141,20 @@ def _get_skill_finger_angles(self, skill_name: str) -> tuple[float, ...] | None: else: # left_hand return zeros + hand_7_angles + def _get_grasp_finger_angles(self) -> tuple[float, ...] | None: + """Get grasp finger angles, applied to both hands (same as historical behavior).""" + if self.cfg.skill_finger_configs is None or self._active_hand is None: + return None + + hand_configs = self.cfg.skill_finger_configs.get(self._active_hand, {}) + hand_7_angles = hand_configs.get("grasp") + + if hand_7_angles is None: + return None + + # Apply same angles to both hands + return hand_7_angles + hand_7_angles + # ------------------------------------------------------------------ # Gripper (three-finger open / close) # ------------------------------------------------------------------ @@ -150,8 +164,8 @@ def _apply_gripper(self, skill_output: SkillOutput, env: ManagerBasedEnv) -> tor gripper_signal = skill_output.action[0].item() if gripper_signal < 0: - # Grasp: check skill_finger_configs first, then fall back to default - skill_angles = self._get_skill_finger_angles("grasp") + # Grasp: check skill_finger_configs first, apply to both hands + skill_angles = self._get_grasp_finger_angles() angles = skill_angles if skill_angles is not None else self.cfg.finger_close_angles else: # Ungrasp: use open angles From 40016d5cdc6b8cd0d63c81884b45a49bfa2d2784 Mon Sep 17 00:00:00 2001 From: Zihan Gao Date: Wed, 6 May 2026 09:31:53 +0800 Subject: [PATCH 28/32] fix: apply skill finger angles to both hands for all skills Match historical behavior where both hands get the same finger angles during lift/push/pull, not just the active hand. Co-Authored-By: Claude Opus 4.6 --- .../autosim/action_adapters/g1_action_adapter.py | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/lw_benchhub/autosim/action_adapters/g1_action_adapter.py b/lw_benchhub/autosim/action_adapters/g1_action_adapter.py index a294fd1e..911d9a34 100644 --- a/lw_benchhub/autosim/action_adapters/g1_action_adapter.py +++ b/lw_benchhub/autosim/action_adapters/g1_action_adapter.py @@ -124,7 +124,8 @@ def _apply_reach_with_skill_fingers(self, skill_output: SkillOutput, env: Manage return action def _get_skill_finger_angles(self, skill_name: str) -> tuple[float, ...] | None: - """Get skill-specific finger angles for the active hand, or None if not configured.""" + """Get skill-specific finger angles for the active hand, or None if not configured. + Applies same angles to both hands (matching historical behavior).""" if self.cfg.skill_finger_configs is None or self._active_hand is None: return None @@ -134,12 +135,8 @@ def _get_skill_finger_angles(self, skill_name: str) -> tuple[float, ...] | None: if hand_7_angles is None: return None - # Construct 14-joint tuple: (right_hand_7, left_hand_7) - zeros = (0.0,) * 7 - if self._active_hand == "right_hand": - return hand_7_angles + zeros - else: # left_hand - return zeros + hand_7_angles + # Apply same angles to both hands + return hand_7_angles + hand_7_angles def _get_grasp_finger_angles(self) -> tuple[float, ...] | None: """Get grasp finger angles, applied to both hands (same as historical behavior).""" From 2e1043b5e7c5834c975a656db01e2529c2d59025 Mon Sep 17 00:00:00 2001 From: Zihan Gao Date: Wed, 6 May 2026 09:35:04 +0800 Subject: [PATCH 29/32] refactor: remove dead _get_grasp_finger_angles method _apply_gripper now uses _get_skill_finger_angles("grasp") directly, making this method redundant. Co-Authored-By: Claude Opus 4.6 --- .../action_adapters/g1_action_adapter.py | 18 ++---------------- 1 file changed, 2 insertions(+), 16 deletions(-) diff --git a/lw_benchhub/autosim/action_adapters/g1_action_adapter.py b/lw_benchhub/autosim/action_adapters/g1_action_adapter.py index 911d9a34..251ab223 100644 --- a/lw_benchhub/autosim/action_adapters/g1_action_adapter.py +++ b/lw_benchhub/autosim/action_adapters/g1_action_adapter.py @@ -138,20 +138,6 @@ def _get_skill_finger_angles(self, skill_name: str) -> tuple[float, ...] | None: # Apply same angles to both hands return hand_7_angles + hand_7_angles - def _get_grasp_finger_angles(self) -> tuple[float, ...] | None: - """Get grasp finger angles, applied to both hands (same as historical behavior).""" - if self.cfg.skill_finger_configs is None or self._active_hand is None: - return None - - hand_configs = self.cfg.skill_finger_configs.get(self._active_hand, {}) - hand_7_angles = hand_configs.get("grasp") - - if hand_7_angles is None: - return None - - # Apply same angles to both hands - return hand_7_angles + hand_7_angles - # ------------------------------------------------------------------ # Gripper (three-finger open / close) # ------------------------------------------------------------------ @@ -161,8 +147,8 @@ def _apply_gripper(self, skill_output: SkillOutput, env: ManagerBasedEnv) -> tor gripper_signal = skill_output.action[0].item() if gripper_signal < 0: - # Grasp: check skill_finger_configs first, apply to both hands - skill_angles = self._get_grasp_finger_angles() + # Grasp: check skill_finger_configs first, then fall back to default + skill_angles = self._get_skill_finger_angles("grasp") angles = skill_angles if skill_angles is not None else self.cfg.finger_close_angles else: # Ungrasp: use open angles From bc52e4151d2276d5d9f28f8baa5eb7f90668b329 Mon Sep 17 00:00:00 2001 From: Zihan Gao Date: Wed, 6 May 2026 09:48:19 +0800 Subject: [PATCH 30/32] fix bugs in open_fridge --- lw_benchhub/autosim/action_adapters/g1_action_adapter.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/lw_benchhub/autosim/action_adapters/g1_action_adapter.py b/lw_benchhub/autosim/action_adapters/g1_action_adapter.py index 251ab223..e04ed399 100644 --- a/lw_benchhub/autosim/action_adapters/g1_action_adapter.py +++ b/lw_benchhub/autosim/action_adapters/g1_action_adapter.py @@ -91,9 +91,6 @@ def _apply_reach(self, skill_output: SkillOutput, env: ManagerBasedEnv) -> torch action[3] = 1.0 # mode=1: squat/stance — keep legs fixed during arm motion action[4:11] = target_joint_pos[r_arm_ids] action[11:18] = target_joint_pos[l_arm_ids] - # Keep fingers open during reach - finger_angles = torch.tensor(self.cfg.finger_open_angles, dtype=torch.float32, device=env.device) - action[18:32] = finger_angles return action From d191aa8dbb0af2616f54dd6b17c08a8c4bd74ce7 Mon Sep 17 00:00:00 2001 From: Zihan Gao Date: Wed, 6 May 2026 12:54:19 +0800 Subject: [PATCH 31/32] delete extra func --- lw_benchhub/autosim/__init__.py | 6 -- .../action_adapters/g1_action_adapter.py | 1 - lw_benchhub/autosim/pipelines/close_oven.py | 99 ------------------- lw_benchhub/autosim/pipelines/open_fridge.py | 1 - 4 files changed, 107 deletions(-) diff --git a/lw_benchhub/autosim/__init__.py b/lw_benchhub/autosim/__init__.py index 0e765b60..b51e0b6b 100644 --- a/lw_benchhub/autosim/__init__.py +++ b/lw_benchhub/autosim/__init__.py @@ -67,9 +67,3 @@ entry_point=f"{__name__}.pipelines.kettle_boiling:KettleBoilingPipeline", cfg_entry_point=f"{__name__}.pipelines.kettle_boiling:G1KettleBoilingPipelineCfg", ) - -register_pipeline( - id="LWBenchhub-Autosim-G1CloseOvenPipeline-v0", - entry_point=f"{__name__}.pipelines.close_oven:CloseOvenPipeline", - cfg_entry_point=f"{__name__}.pipelines.close_oven:G1CloseOvenPipelineCfg", -) diff --git a/lw_benchhub/autosim/action_adapters/g1_action_adapter.py b/lw_benchhub/autosim/action_adapters/g1_action_adapter.py index e04ed399..fc45d8f7 100644 --- a/lw_benchhub/autosim/action_adapters/g1_action_adapter.py +++ b/lw_benchhub/autosim/action_adapters/g1_action_adapter.py @@ -37,7 +37,6 @@ def __init__(self, cfg: G1ActionAdapterCfg): self.register_apply_method("moveto", self._apply_moveto) self.register_apply_method("reach", self._apply_reach) self.register_apply_method("lift", lambda so, env: self._apply_reach_with_skill_fingers(so, env, "lift")) - self.register_apply_method("pull", lambda so, env: self._apply_reach_with_skill_fingers(so, env, "pull")) self.register_apply_method("push", lambda so, env: self._apply_reach_with_skill_fingers(so, env, "push")) self.register_apply_method("grasp", self._apply_gripper) self.register_apply_method("ungrasp", self._apply_gripper) diff --git a/lw_benchhub/autosim/pipelines/close_oven.py b/lw_benchhub/autosim/pipelines/close_oven.py index 4a643426..19a53c15 100644 --- a/lw_benchhub/autosim/pipelines/close_oven.py +++ b/lw_benchhub/autosim/pipelines/close_oven.py @@ -32,26 +32,6 @@ def _x7s_skill_cfg(cfg) -> None: cfg.max_steps = 1000 -def _g1_skill_cfg(cfg) -> None: - cfg.skills.moveto.extra_cfg.local_planner.max_linear_velocity = 1.0 - cfg.skills.moveto.extra_cfg.local_planner.max_angular_velocity = 0.4 - cfg.skills.moveto.extra_cfg.local_planner.predict_time = 0.4 - cfg.skills.moveto.extra_cfg.global_planner.safety_distance = 0.5 - cfg.skills.moveto.extra_cfg.global_planner.proximity_weight = 3.0 - cfg.skills.moveto.extra_cfg.waypoint_tolerance = 0.1 - cfg.skills.moveto.extra_cfg.goal_tolerance = 0.1 - cfg.skills.moveto.extra_cfg.yaw_tolerance = 0.5 - cfg.skills.moveto.extra_cfg.use_dwa = False - cfg.skills.moveto.extra_cfg.sampling_radius = 1.13 - cfg.skills.push.extra_cfg.move_offset = 0.36 - cfg.skills.push.extra_cfg.move_axis = "+x" - cfg.skills.lift.extra_cfg.move_offset = 0.15 - cfg.skills.lift.extra_cfg.move_axis = "+z" - cfg.max_steps = 1000 - cfg.motion_planner.use_cuda_graph = False - cfg.action_adapter.squat_settle_steps = 0 - - TASK_ROBOT_OVERRIDES: dict[str, TaskRobotOverride] = { "x7s_joint_left": TaskRobotOverride( extra_target_link_names=("link20_tip",), @@ -64,15 +44,6 @@ def _g1_skill_cfg(cfg) -> None: init_state_pos_delta=(-0.6, -1.2, 0.0), skill_cfg_fn=_x7s_skill_cfg, ), - "g1_loco_left": TaskRobotOverride( - object_reach_target_poses={ - "oven_main_group": [ - torch.tensor([-0.176, -0.840, 0.010, 0.707, 0.0, 0.0, 0.707]), - ], - }, - init_state_pos_delta=(-0.3, -1.2, 0.0), - skill_cfg_fn=_g1_skill_cfg, - ), } @@ -108,17 +79,6 @@ def __post_init__(self): ] -_DOOR_PROFILES = {"g1_loco_left"} -_DOOR_JOINT_PATH = "/World/envs/env_0/Scene/oven_main_group/Oven032_door/door_joint" -_G1_PUSH_STEPS = 100 # env steps to walk forward during push; tune if needed -_G1_PUSH_FWD_CMD = 0.5 # normalized body-frame vx command (0–1) - - -@configclass -class G1CloseOvenPipelineCfg(CloseOvenPipelineCfg): - robot_profile: str = "g1_loco_left" - - class CloseOvenPipeline(AutoSimPipeline): def __init__(self, cfg: AutoSimPipelineCfg): self._resolved_robot = resolve_robot_settings( @@ -126,61 +86,6 @@ def __init__(self, cfg: AutoSimPipelineCfg): ) super().__init__(cfg) - def _set_door_drive(self, stiffness: float, damping: float, target_deg: float) -> None: - try: - import omni.usd - from pxr import UsdPhysics - stage = omni.usd.get_context().get_stage() - prim = stage.GetPrimAtPath(_DOOR_JOINT_PATH) - if prim.IsValid() and (drive := UsdPhysics.DriveAPI.Get(prim, "angular")): - drive.GetStiffnessAttr().Set(stiffness) - drive.GetDampingAttr().Set(damping) - if not (attr := drive.GetTargetPositionAttr()).IsValid(): - attr = drive.CreateTargetPositionAttr() - attr.Set(target_deg) - except Exception as e: - print(f"[CloseOven] door drive set failed: {e}") - - def reset_env(self): - super().reset_env() - if self._resolved_robot.profile.profile_id in _DOOR_PROFILES: - self._env.cfg.isaaclab_arena_env.task._setup_scene(self._env) - self._set_door_drive(stiffness=0.0, damping=1.0, target_deg=0.0) - - def _execute_single_skill(self, skill, goal): - if (self._resolved_robot.profile.profile_id in _DOOR_PROFILES - and skill.cfg.name == "push"): - return self._execute_g1_push() - return super()._execute_single_skill(skill, goal) - - def _execute_g1_push(self) -> tuple[bool, int]: - """Walk the G1 forward for a fixed number of steps with arm joints frozen.""" - robot = self._env.scene["robot"] - r_arm_ids, _ = robot.find_joints( - self._env.action_manager.get_term("right_arm_action").cfg.joint_names - ) - l_arm_ids, _ = robot.find_joints( - self._env.action_manager.get_term("left_arm_action").cfg.joint_names - ) - frozen = robot.data.joint_pos[self._env_id].clone() - - adapter_result = self._last_action[self._env_id].clone() - adapter_result[0] = _G1_PUSH_FWD_CMD - adapter_result[1] = 0.0 - adapter_result[2] = 0.0 - adapter_result[3] = 0.0 # mode=0: locomotion - adapter_result[4:11] = frozen[r_arm_ids] - adapter_result[11:18] = frozen[l_arm_ids] - - for _ in range(_G1_PUSH_STEPS): - action = self._last_action.clone() - action[self._env_id, : adapter_result.shape[0]] = adapter_result - self._env.step(action) - self._last_action = action - self._generated_actions.append(action) - - return True, _G1_PUSH_STEPS - def load_env(self) -> ManagerBasedEnv: import gymnasium as gym from lw_benchhub.utils.env import parse_env_cfg, ExecuteMode @@ -221,10 +126,6 @@ def load_env(self) -> ManagerBasedEnv: env = gym.make(env_id, cfg=env_cfg).unwrapped - if self._resolved_robot.profile.profile_id in _DOOR_PROFILES: - env.cfg.isaaclab_arena_env.task._setup_scene(env) - self._set_door_drive(stiffness=0.0, damping=1.0, target_deg=0.0) - return env def get_env_extra_info(self): diff --git a/lw_benchhub/autosim/pipelines/open_fridge.py b/lw_benchhub/autosim/pipelines/open_fridge.py index e69f91df..45656b5e 100644 --- a/lw_benchhub/autosim/pipelines/open_fridge.py +++ b/lw_benchhub/autosim/pipelines/open_fridge.py @@ -65,7 +65,6 @@ def _g1_skill_cfg(cfg) -> None: "right_hand": { "grasp": (1.0, 1.0, 1.0, 1.0, 0.8, 0.8, 0.8), "push": (1.0, 1.0, 1.0, 1.0, 0.8, 0.8, 0.8), - "pull": (1.0, 1.0, 1.0, 1.0, 0.8, 0.8, 0.8), } }, ), From 9a170937e3927df968aab75d243758c235df7f0b Mon Sep 17 00:00:00 2001 From: Zihan Gao Date: Wed, 6 May 2026 12:59:20 +0800 Subject: [PATCH 32/32] delete debug config/code --- .../autosim/action_adapters/g1_action_adapter_cfg.py | 4 ---- lw_benchhub/autosim/pipelines/open_fridge.py | 9 --------- 2 files changed, 13 deletions(-) diff --git a/lw_benchhub/autosim/action_adapters/g1_action_adapter_cfg.py b/lw_benchhub/autosim/action_adapters/g1_action_adapter_cfg.py index 67a26a1f..39cc873e 100644 --- a/lw_benchhub/autosim/action_adapters/g1_action_adapter_cfg.py +++ b/lw_benchhub/autosim/action_adapters/g1_action_adapter_cfg.py @@ -11,10 +11,6 @@ class G1ActionAdapterCfg(ActionAdapterCfg): class_type: type = G1ActionAdapter - base_x_joint_name: str = "base_x_joint" - base_y_joint_name: str = "base_y_joint" - base_yaw_joint_name: str = "base_yaw_joint" - ee_link_name: str = "" """End-effector link name, used to determine active hand (left/right).""" diff --git a/lw_benchhub/autosim/pipelines/open_fridge.py b/lw_benchhub/autosim/pipelines/open_fridge.py index 45656b5e..cf18eea5 100644 --- a/lw_benchhub/autosim/pipelines/open_fridge.py +++ b/lw_benchhub/autosim/pipelines/open_fridge.py @@ -115,15 +115,6 @@ def __init__(self, cfg: AutoSimPipelineCfg): ) super().__init__(cfg) - def _execute_single_skill(self, skill, goal): - success, steps = super()._execute_single_skill(skill, goal) - if skill.cfg.name == "moveto": - robot = self._env.scene["robot"] - pos = robot.data.root_pos_w[0].cpu().numpy() - quat = robot.data.root_quat_w[0].cpu().numpy() - print(f"[DEBUG] Robot position after moveto: pos={pos.round(3)}, quat={quat.round(3)}") - return success, steps - def load_env(self) -> ManagerBasedEnv: import gymnasium as gym from lw_benchhub.utils.env import ExecuteMode, parse_env_cfg