diff --git a/lw_benchhub/autosim/__init__.py b/lw_benchhub/autosim/__init__.py index 7aac51e..b51e0b6 100644 --- a/lw_benchhub/autosim/__init__.py +++ b/lw_benchhub/autosim/__init__.py @@ -36,3 +36,34 @@ entry_point=f"{__name__}.pipelines.dessert_upgrade:DessertUpgradePipeline", cfg_entry_point=f"{__name__}.pipelines.dessert_upgrade:DessertUpgradePipelineCfg", ) + +# G1 pipelines +register_pipeline( + 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-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:G1KettleBoilingPipelineCfg", +) 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 0000000..fc45d8f --- /dev/null +++ b/lw_benchhub/autosim/action_adapters/g1_action_adapter.py @@ -0,0 +1,166 @@ +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) + + # 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", lambda so, env: self._apply_reach_with_skill_fingers(so, env, "lift")) + 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) + + # ------------------------------------------------------------------ + # 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.""" + 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) + + vx_body = vx * cos_yaw + vy * sin_yaw + vy_body = -vx * sin_yaw + vy * cos_yaw + + last_action = env.action_manager.action + action = last_action[0, :].clone() + + _MAX_CMD = 0.9 + 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 + + # ------------------------------------------------------------------ + # 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 + + 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] + + 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. + 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 + + 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 + + # Apply same angles to both hands + return hand_7_angles + hand_7_angles + + # ------------------------------------------------------------------ + # 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() + + 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 + 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 + + robot = env.scene["robot"] + 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_action_adapter_cfg.py b/lw_benchhub/autosim/action_adapters/g1_action_adapter_cfg.py new file mode 100644 index 0000000..39cc873 --- /dev/null +++ b/lw_benchhub/autosim/action_adapters/g1_action_adapter_cfg.py @@ -0,0 +1,22 @@ +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 + + 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 (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/content/assets/robot/g1/urdf/G1.urdf b/lw_benchhub/autosim/content/assets/robot/g1/urdf/G1.urdf new file mode 100644 index 0000000..de7dbf8 --- /dev/null +++ b/lw_benchhub/autosim/content/assets/robot/g1/urdf/G1.urdf @@ -0,0 +1,1567 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file 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 0000000..375cd43 --- /dev/null +++ b/lw_benchhub/autosim/content/configs/robot/g1.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: "left_hand_palm_link" + # Extra tracked links + link_names: ["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", + ] + + collision_spheres: "spheres/collision_g1.yml" + collision_sphere_buffer: 0.002 + extra_collision_spheres: {"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"], + } + + 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", + ] + + # 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_right_ee.yml b/lw_benchhub/autosim/content/configs/robot/g1_right_ee.yml new file mode 100644 index 0000000..da8644c --- /dev/null +++ b/lw_benchhub/autosim/content/configs/robot/g1_right_ee.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.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/spheres/collision_g1.yml b/lw_benchhub/autosim/content/configs/robot/spheres/collision_g1.yml new file mode 100644 index 0000000..ae16a2b --- /dev/null +++ b/lw_benchhub/autosim/content/configs/robot/spheres/collision_g1.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/pipelines/cheesy_bread.py b/lw_benchhub/autosim/pipelines/cheesy_bread.py index 00def35..204d634 100644 --- a/lw_benchhub/autosim/pipelines/cheesy_bread.py +++ b/lw_benchhub/autosim/pipelines/cheesy_bread.py @@ -1,19 +1,45 @@ +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 = 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.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] = { "x7s_joint_left": TaskRobotOverride( extra_target_link_names=("link20_tip",), @@ -25,6 +51,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 +73,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 +84,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 +103,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 +145,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 +156,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 +169,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 +183,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 6d94a48..19a53c1 100644 --- a/lw_benchhub/autosim/pipelines/close_oven.py +++ b/lw_benchhub/autosim/pipelines/close_oven.py @@ -1,19 +1,37 @@ +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 ( +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 + + TASK_ROBOT_OVERRIDES: dict[str, TaskRobotOverride] = { "x7s_joint_left": TaskRobotOverride( extra_target_link_names=("link20_tip",), @@ -23,6 +41,8 @@ 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, ), } @@ -31,49 +51,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_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", "Scene/counter_main_main_group", @@ -83,19 +81,14 @@ def __post_init__(self): 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) ) - - def reset_env(self): - super().reset_env() + super().__init__(cfg) 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 +113,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, @@ -142,7 +133,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/coffee_setup_mug.py b/lw_benchhub/autosim/pipelines/coffee_setup_mug.py index 75c0d28..ccdd3b1 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 = 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.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 bc210fa..aba3815 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 = 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.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 d0b68b2..8b963cb 100644 --- a/lw_benchhub/autosim/pipelines/kettle_boiling.py +++ b/lw_benchhub/autosim/pipelines/kettle_boiling.py @@ -1,32 +1,153 @@ +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 = 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.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.4} + + +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 _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( + 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",), 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]), ], }, + 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.05, 0.22, 0.08, 0.707, 0.0, 0.0, -0.707]), + ], + "stovetop_main_group": [ + 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), + 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, + 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), + } + }, ), } @@ -35,47 +156,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_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 = [ + self.motion_planner.world_only_subffixes = [ "Scene/obj", "Scene/stovetop_main_group", "Scene/counter_main_main_group", @@ -83,21 +190,30 @@ def __post_init__(self): ] +@configclass +class G1KettleBoilingPipelineCfg(KettleBoilingPipelineCfg): + robot_profile: str = "g1_loco_left" + + 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 +238,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 +251,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 +263,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/pipelines/open_fridge.py b/lw_benchhub/autosim/pipelines/open_fridge.py index 48492cc..cf18eea 100644 --- a/lw_benchhub/autosim/pipelines/open_fridge.py +++ b/lw_benchhub/autosim/pipelines/open_fridge.py @@ -1,20 +1,48 @@ -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 = 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.3 + 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.2 + cfg.skills.moveto.extra_cfg.use_dwa = False + 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] = { "x7s_joint_right": TaskRobotOverride( object_reach_target_poses={ @@ -22,6 +50,23 @@ 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_right": TaskRobotOverride( + object_reach_target_poses={ + "fridge_main_group": [ + torch.tensor([0.07, -0.429, 0.25, 0.707, 0.0, 0.0, 0.707]), + ], + }, + init_state_pos_delta=(-0.15, -1.5, 0.0), + 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), + } + }, ), } @@ -30,40 +75,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.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"] @@ -72,17 +103,17 @@ def __post_init__(self): ] +@configclass +class G1OpenFridgePipelineCfg(OpenFridgePipelineCfg): + robot_profile: str = "g1_loco_right" + + 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 @@ -99,7 +130,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, @@ -111,11 +142,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 +153,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( diff --git a/lw_benchhub/autosim/robot_env_configs/__init__.py b/lw_benchhub/autosim/robot_env_configs/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/lw_benchhub/autosim/robot_env_configs/g1_autosim_cfg.py b/lw_benchhub/autosim/robot_env_configs/g1_autosim_cfg.py new file mode 100644 index 0000000..51523d6 --- /dev/null +++ b/lw_benchhub/autosim/robot_env_configs/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/robot_profiles.py b/lw_benchhub/autosim/robot_profiles.py index 775ec49..7161d4b 100644 --- a/lw_benchhub/autosim/robot_profiles.py +++ b/lw_benchhub/autosim/robot_profiles.py @@ -29,6 +29,9 @@ class RobotProfile: """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 @@ -41,6 +44,15 @@ class TaskRobotOverride: """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 + 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 + reset_env_fn: Callable | None = None + 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 @@ -65,6 +77,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.robot_env_configs.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,6 +108,28 @@ 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, + ), + "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_ee.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, + ), } @@ -109,11 +157,21 @@ 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-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 - 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: @@ -122,26 +180,35 @@ def configure_robot_runtime_settings(pipeline_cfg, resolved_robot: ResolvedRobot 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 + 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( *, 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 +216,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/scripts/autosim/compare_ee_targets.py b/lw_benchhub/scripts/autosim/compare_ee_targets.py new file mode 100644 index 0000000..0169b4d --- /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 ecc7882..6cbeb0b 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 0000000..29175db --- /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 0000000..fd63e4d --- /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 0000000..c246852 --- /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()