diff --git a/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/modelCfg/bimanual_arm.py b/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/modelCfg/bimanual_arm.py new file mode 100644 index 00000000..aa30174a --- /dev/null +++ b/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/modelCfg/bimanual_arm.py @@ -0,0 +1,295 @@ +""" +Bimanual arm articulation config for keyboard teleoperation. + +Robot model: Humanoid_Wato/wato_bimanual_arm (armDouble.SLDASM.usd) +Joint limits/poses: Isaac Sim Physics Inspector (/World/armDouble_SLDASM/root_joint) +Motor specs: https://watonomous.github.io/humanoid-docs/mechanical/index.html + + Shoulder joints 1-2 AK10-9 V3.0 18 Nm rated / 53 Nm peak + Elbow joints 3-5 AK80-9 V3.0 9 Nm rated / 22 Nm peak + Wrist joint 6 GL40 KV70 0.25 Nm rated / 0.73 Nm peak + Gripper GL40 KV70 0.25 Nm rated / 0.73 Nm peak + (rotary motor + linkage → prismatic finger travel in URDF) + +Only the left arm is actuated for teleop. The right arm is held at the +Physics Inspector default pose below. + +Gripper actuation note +---------------------- +On hardware, ONE GL40 rotary motor closes/opens the gripper through a +mechanical linkage (screw / hinge stack). The URDF instead exposes TWO +independent prismatic joints (joint7 + joint8, joint7l + joint8l). + +In sim we: + 1. Drive both prismatic joints with synchronized position targets (open/closed pair). + 2. Use HIGH stiffness/damping so fingers stay locked during arm motion. + 3. Do NOT copy 0.25 Nm directly — effort_limit_sim on prismatic DOFs is a + linear force cap (Newtons), not motor torque. Tune by grasp/hold behaviour. +""" +import math +import os + +import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets.articulation import ArticulationCfg + +_THIS_DIR = os.path.abspath(os.path.dirname(__file__)) +_BIMANUAL_ROOT = os.path.abspath(os.path.join(_THIS_DIR, "..", "..", "..", "..", "wato_bimanual_arm")) +_ARM_URDF_PATH = os.path.join(_BIMANUAL_ROOT, "urdf", "armDouble.SLDASM.urdf") + + +def _deg(degrees: float) -> float: + return degrees * math.pi / 180.0 + + +# --- Joint limits from Physics Inspector ------------------------------------ +_REVOLUTE_LIMIT = (-math.pi, math.pi) +# Set physical limits so the gripper cannot close past touching (-0.082 / 0.082) or open past 14cm gap (-0.155 / 0.155) +_JOINT7_LIMIT = (-0.155, -0.082) +_JOINT8_LIMIT = (0.082, 0.155) + +JOINT_POS_LIMITS = { + "joint1": _REVOLUTE_LIMIT, + "joint2": _REVOLUTE_LIMIT, + "joint3": _REVOLUTE_LIMIT, + "joint4": _REVOLUTE_LIMIT, + "joint5": _REVOLUTE_LIMIT, + "joint6": _REVOLUTE_LIMIT, + "joint7": _JOINT7_LIMIT, + "joint8": _JOINT8_LIMIT, + "joint1L": _REVOLUTE_LIMIT, + "joint2l": _REVOLUTE_LIMIT, + "joint3l": _REVOLUTE_LIMIT, + "joint4l": _REVOLUTE_LIMIT, + "joint5l": _REVOLUTE_LIMIT, + "joint6l": _REVOLUTE_LIMIT, + "joint7l": _JOINT7_LIMIT, + "joint8l": _JOINT8_LIMIT, +} + +# --- Default poses from Physics Inspector (revolute: deg -> rad) ------------ +_DEFAULT_JOINT_POS = { + # Right arm — held fixed during teleop + "joint1": _deg(-140.8), + "joint2": _deg(55.7), + "joint3": _deg(-66.0), + "joint4": _deg(111.4), + "joint5": _deg(34.8), + "joint6": _deg(3.5), + "joint7": -0.14, + "joint8": 0.14, + # Left arm — teleoperated + "joint1L": _deg(139.2), + "joint2l": _deg(66.1), + "joint3l": _deg(147.9), + "joint4l": _deg(-76.5), + "joint5l": _deg(-76.5), + "joint6l": _deg(-22.6), + "joint7l": -0.14, + "joint8l": 0.14, +} + +RIGHT_ARM_JOINTS = ["joint1", "joint2", "joint3", "joint4", "joint5", "joint6", "joint7", "joint8"] +LEFT_ARM_JOINTS = ["joint1L", "joint2l", "joint3l", "joint4l", "joint5l", "joint6l"] +LEFT_GRIPPER_JOINTS = ["joint7l", "joint8l"] +# Jacobian anchor is the wrist link; IK pose target is the fingertip center (see below). +LEFT_EE_BODY = "link6l" +LEFT_FINGER_TIP_BODIES = ("link7l", "link8l") +# Distal mesh points in each finger link frame (link7l.STL +X, link8l.STL -X). +LEFT_FINGER_DISTAL_TIP_LOCAL = { + "link7l": (0.13211595, -0.04057075, -0.00434997), + "link8l": (-0.13211595, -0.04057075, -0.00435003), +} + +# Gripper finger targets (joint7: [-0.155, -0.082], joint8: [0.082, 0.155]) +# Synchronized pair mimics single GL40 motor driving both fingers via linkage. +GRIPPER_OPEN = {"joint7l": -0.14, "joint8l": 0.14} +GRIPPER_CLOSED = {"joint7l": -0.082, "joint8l": 0.082} + +# Prismatic gripper PD — tuned for hold during arm motion (not from motor datasheet). +# If fingers bounce when the shoulder moves, raise stiffness; if jittery, raise damping. +_GRIPPER_STIFFNESS = 800.0 # doubled — grip must survive arm retraction forces +_GRIPPER_DAMPING = 80.0 # doubled — damp oscillations during pull +_GRIPPER_EFFORT_LIMIT = 200.0 # raised from 30N — must overcome contact force during retraction +_GRIPPER_VELOCITY_LIMIT = 0.5 # raised from 0.2 m/s — allows faster retraction + + +def apply_joint_limits(robot) -> None: + """Apply Physics Inspector joint limits to the articulation.""" + limits = robot.data.joint_pos_limits.clone() + updated = [] + + for joint_idx, joint_name in enumerate(robot.data.joint_names): + if joint_name not in JOINT_POS_LIMITS: + continue + lo, hi = JOINT_POS_LIMITS[joint_name] + limits[:, joint_idx, 0] = lo + limits[:, joint_idx, 1] = hi + updated.append(joint_name) + + if updated: + robot.write_joint_position_limit_to_sim(limits, warn_limit_violation=False) + print(f"[INFO] Applied joint limits for {len(updated)} joints.") + + +def resolve_body_ids(robot, names: tuple[str, ...] | list[str]) -> list[int]: + """Map body names to articulation body indices.""" + body_names = list(robot.data.body_names) + name_to_id = {name: idx for idx, name in enumerate(body_names)} + missing = [name for name in names if name not in name_to_id] + if missing: + raise KeyError(f"Body names {missing} not found in {body_names}") + return [name_to_id[name] for name in names] + + +def compute_gripper_tip_pos_w(robot, finger_body_ids: list[int]): + """World-frame midpoint between the distal tips of link7l and link8l.""" + import torch + from isaaclab.utils.math import quat_apply + + dtype = robot.data.body_pos_w.dtype + device = robot.data.body_pos_w.device + tips = [] + for body_name, body_id in zip(LEFT_FINGER_TIP_BODIES, finger_body_ids): + local = torch.tensor([LEFT_FINGER_DISTAL_TIP_LOCAL[body_name]], device=device, dtype=dtype) + body_pos = robot.data.body_pos_w[:, body_id] + body_quat = robot.data.body_quat_w[:, body_id] + tips.append(body_pos + quat_apply(body_quat, local)) + return (tips[0] + tips[1]) * 0.5 + + +def compute_gripper_tip_pose_w(robot, wrist_body_id: int, finger_body_ids: list[int]): + """Gripper-tip center pose in world frame (position from fingers, orientation from wrist).""" + tip_pos_w = compute_gripper_tip_pos_w(robot, finger_body_ids) + tip_quat_w = robot.data.body_quat_w[:, wrist_body_id] + return tip_pos_w, tip_quat_w + + +def compute_gripper_tip_pose_b(robot, root_pose_w, wrist_body_id: int, finger_body_ids: list[int]): + """Gripper-tip center pose in the robot root frame.""" + from isaaclab.utils.math import subtract_frame_transforms + + tip_pos_w, tip_quat_w = compute_gripper_tip_pose_w(robot, wrist_body_id, finger_body_ids) + return subtract_frame_transforms( + root_pose_w[:, 0:3], root_pose_w[:, 3:7], tip_pos_w, tip_quat_w + ) + + +def jacobian_world_to_root(robot, jacobian_w): + """Rotate PhysX world-frame Jacobian into the articulation root frame.""" + import torch + from isaaclab.utils.math import matrix_from_quat, quat_inv + + base_rot = matrix_from_quat(quat_inv(robot.data.root_quat_w)) + jacobian_b = jacobian_w.clone() + jacobian_b[:, :3, :] = torch.bmm(base_rot, jacobian_b[:, :3, :]) + jacobian_b[:, 3:, :] = torch.bmm(base_rot, jacobian_b[:, 3:, :]) + return jacobian_b + + +def adjust_jacobian_for_gripper_tip(jacobian_b, wrist_pos_b, tip_pos_b): + """Map link6l root-frame Jacobian to the fingertip center.""" + import torch + from isaaclab.utils.math import skew_symmetric_matrix + + offset_b = tip_pos_b - wrist_pos_b + tip_jacobian = jacobian_b.clone() + tip_jacobian[:, 0:3, :] += torch.bmm(-skew_symmetric_matrix(offset_b), jacobian_b[:, 3:, :]) + return tip_jacobian + + +def compute_tip_ik_jacobian(robot, jacobian_w, wrist_pos_b, tip_pos_b): + """World-frame link6l Jacobian -> root frame -> fingertip center.""" + return adjust_jacobian_for_gripper_tip( + jacobian_world_to_root(robot, jacobian_w), wrist_pos_b, tip_pos_b + ) + + +def resolve_joint_name(robot, name: str) -> str: + """Match a config joint name to the articulation's actual joint name.""" + names = list(robot.data.joint_names) + if name in names: + return name + # Physics Inspector may show joint2L while URDF uses joint2l + if len(name) > 1 and name[-1] in ("l", "L"): + alt = name[:-1] + ("L" if name[-1] == "l" else "l") + if alt in names: + return alt + raise KeyError(f"Joint '{name}' not found in {names}") + + +_ARM_URDF_PATH = os.path.join(_BIMANUAL_ROOT, "urdf", "armDouble.SLDASM.urdf") + +BIMANUAL_ARM_CFG = ArticulationCfg( + spawn=sim_utils.UrdfFileCfg( + asset_path=_ARM_URDF_PATH, + fix_base=True, + make_instanceable=True, + joint_drive=sim_utils.UrdfFileCfg.JointDriveCfg( + drive_type="force", + target_type="position", + gains=sim_utils.UrdfFileCfg.JointDriveCfg.PDGainsCfg( + stiffness=400.0, + damping=40.0, + ), + ), + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + max_depenetration_velocity=5.0, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + ), + ), + init_state=ArticulationCfg.InitialStateCfg(joint_pos=_DEFAULT_JOINT_POS), + actuators={ + # AK10-9 V3.0 — shoulder joints 1-2 + "left_shoulder": ImplicitActuatorCfg( + joint_names_expr=["joint1L", "joint2l"], + stiffness=757.6, + damping=60.3, + effort_limit_sim=18.0, + velocity_limit_sim=3.0, + ), + # AK80-9 V3.0 — elbow joints 3-5 + "left_elbow": ImplicitActuatorCfg( + joint_names_expr=["joint3l", "joint4l", "joint5l"], + stiffness=615.5, + damping=43.5, + effort_limit_sim=9.0, + velocity_limit_sim=3.0, + ), + # GL40 KV70 — wrist joint 6 (increased effort limit so wrist doesn't yield during pulling) + "left_wrist": ImplicitActuatorCfg( + joint_names_expr=["joint6l"], + stiffness=170.5, + damping=9.0, + effort_limit_sim=10.0, + velocity_limit_sim=3.0, + ), + # GL40 KV70 rotary → linkage → two prismatic fingers (see module docstring) + "left_gripper": ImplicitActuatorCfg( + joint_names_expr=["joint7l", "joint8l"], + stiffness=_GRIPPER_STIFFNESS, + damping=_GRIPPER_DAMPING, + effort_limit_sim=_GRIPPER_EFFORT_LIMIT, + velocity_limit_sim=_GRIPPER_VELOCITY_LIMIT, + ), + # Right arm revolute joints — hold Physics Inspector default pose + "right_arm": ImplicitActuatorCfg( + joint_names_expr=["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"], + stiffness=500.0, + damping=50.0, + effort_limit_sim=18.0, + velocity_limit_sim=3.0, + ), + # Right gripper — same coupled-prismatic hold as left + "right_gripper": ImplicitActuatorCfg( + joint_names_expr=["joint7", "joint8"], + stiffness=_GRIPPER_STIFFNESS, + damping=_GRIPPER_DAMPING, + effort_limit_sim=_GRIPPER_EFFORT_LIMIT, + velocity_limit_sim=_GRIPPER_VELOCITY_LIMIT, + ), + }, +) diff --git a/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/bimanual_cabinet/__init__.py b/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/bimanual_cabinet/__init__.py new file mode 100644 index 00000000..7ea0d715 --- /dev/null +++ b/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/bimanual_cabinet/__init__.py @@ -0,0 +1,6 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Manipulation environments to open drawers in a cabinet.""" diff --git a/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/bimanual_cabinet/bimanual_cabinet_env_cfg.py b/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/bimanual_cabinet/bimanual_cabinet_env_cfg.py new file mode 100644 index 00000000..5e467992 --- /dev/null +++ b/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/bimanual_cabinet/bimanual_cabinet_env_cfg.py @@ -0,0 +1,443 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.envs import ManagerBasedRLEnvCfg +from isaaclab.managers import EventTermCfg as EventTerm +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.managers import RewardTermCfg as RewTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.managers import TerminationTermCfg as DoneTerm +from isaaclab.managers import CurriculumTermCfg as CurrTerm +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sensors import FrameTransformerCfg, ContactSensorCfg +from isaaclab.sensors.frame_transformer import OffsetCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR + +from . import mdp +from HumanoidRLPackage.HumanoidRLSetup.modelCfg.bimanual_arm import BIMANUAL_ARM_CFG + +## +# Pre-defined configs +## +from isaaclab.markers.config import FRAME_MARKER_CFG # isort: skip + + +FRAME_MARKER_SMALL_CFG = FRAME_MARKER_CFG.copy() +FRAME_MARKER_SMALL_CFG.markers["frame"].scale = (0.10, 0.10, 0.10) + + +## +# Scene definition +## + + +@configclass +class CabinetSceneCfg(InteractiveSceneCfg): + """Configuration for the cabinet scene with a robot and a cabinet. + + This is the abstract base implementation, the exact scene is defined in the derived classes + which need to set the robot and end-effector frames + """ + + robot = BIMANUAL_ARM_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + cabinet = ArticulationCfg( + prim_path="{ENV_REGEX_NS}/Cabinet", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Sektion_Cabinet/sektion_cabinet_instanceable.usd", + activate_contact_sensors=True, # needed so the handle reports contact forces + scale=(1.15, 1.15, 1.15), # Reverted to ORIGINAL size + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.75, 0.0, 0.6), # Reverted to ORIGINAL position + rot=(0.0, 0.0, 0.0, 1.0), + joint_pos={ + "door_left_joint": 0.0, + "door_right_joint": 0.0, + "drawer_bottom_joint": 0.0, + "drawer_top_joint": 0.0, + }, + ), + actuators={ + "drawers": ImplicitActuatorCfg( + joint_names_expr=["drawer_top_joint", "drawer_bottom_joint"], + effort_limit_sim=87.0, + # Zero stiffness so the drawer is a passive sliding mechanism + stiffness=0.0, + damping=0.05, + ), + "doors": ImplicitActuatorCfg( + joint_names_expr=["door_left_joint", "door_right_joint"], + effort_limit_sim=87.0, + stiffness=10.0, + damping=2.5, + ), + }, + ) + + # Frame definitions for the cabinet. + cabinet_frame = FrameTransformerCfg( + prim_path="{ENV_REGEX_NS}/Cabinet/sektion", + debug_vis=True, + visualizer_cfg=FRAME_MARKER_SMALL_CFG.replace(prim_path="/Visuals/CabinetFrameTransformer"), + target_frames=[ + FrameTransformerCfg.FrameCfg( + prim_path="{ENV_REGEX_NS}/Cabinet/drawer_handle_top", + name="drawer_handle_top", + offset=OffsetCfg( + # Z lowered from +0.01 (top rim) to -0.01 so the whole approach/align/ + # proximity ladder aims the TCP at the bar CENTER/underside instead of the + # top rim. This is the primary fix for the "hook the top" behavior — the + # gripper was being guided to the top because the aim point sat there. + pos=(0.3485, 0.0, -0.01), + rot=(0.5, 0.5, -0.5, -0.5), # align with end-effector frame + ), + ), + ], + ) + + # Contact sensors on the two inner fingers, filtered to ONLY report contact with + # the drawer handle. Used by the pull rewards to confirm the claw is truly gripping. + contact_link7 = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Robot/link7", + filter_prim_paths_expr=["{ENV_REGEX_NS}/Cabinet/drawer_handle_top"], + history_length=0, + track_air_time=False, + ) + contact_link8 = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Robot/link8", + filter_prim_paths_expr=["{ENV_REGEX_NS}/Cabinet/drawer_handle_top"], + history_length=0, + track_air_time=False, + ) + + # plane + plane = AssetBaseCfg( + prim_path="/World/GroundPlane", + init_state=AssetBaseCfg.InitialStateCfg(), + spawn=sim_utils.GroundPlaneCfg(), + collision_group=-1, + ) + + # lights + light = AssetBaseCfg( + prim_path="/World/light", + spawn=sim_utils.DomeLightCfg(color=(0.75, 0.75, 0.75), intensity=3000.0), + ) + + +## +# MDP settings +## + + +@configclass +class ActionsCfg: + """Action specifications for the MDP.""" + + arm_action = mdp.JointPositionActionCfg( + asset_name="robot", + joint_names=["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"], + scale=0.8, + use_default_offset=True, + ) + gripper_action = mdp.BinaryJointPositionActionCfg( + asset_name="robot", + joint_names=["joint7", "joint8"], + # OPEN = wide (~11.5cm gap) so the bar can pass between the fingers on approach. + # CLOSE = hard against physical limits (±0.082) — the tightest clamp allowed. + # Using the limit value directly ensures maximum clamping force without fighting the physics. + open_command_expr={"joint7": -0.14, "joint8": 0.14}, + close_command_expr={"joint7": -0.082, "joint8": 0.082}, + ) + # Dummy action to actively hold the left arm at its resting pose + left_arm_hold = mdp.JointPositionActionCfg( + asset_name="robot", + joint_names=["joint1L", "joint2l", "joint3l", "joint4l", "joint5l", "joint6l", "joint7l", "joint8l"], + scale=0.0, # Neural network output is multiplied by zero + use_default_offset=True, # Always targets the default resting position + ) + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP.""" + + @configclass + class PolicyCfg(ObsGroup): + """Observations for policy group.""" + + joint_pos = ObsTerm(func=mdp.joint_pos_rel) + joint_vel = ObsTerm(func=mdp.joint_vel_rel) + cabinet_joint_pos = ObsTerm( + func=mdp.joint_pos_rel, + params={"asset_cfg": SceneEntityCfg("cabinet", joint_names=["drawer_top_joint"])}, + ) + cabinet_joint_vel = ObsTerm( + func=mdp.joint_vel_rel, + params={"asset_cfg": SceneEntityCfg("cabinet", joint_names=["drawer_top_joint"])}, + ) + rel_ee_drawer_distance = ObsTerm(func=mdp.rel_ee_drawer_distance) + + actions = ObsTerm(func=mdp.last_action) + + def __post_init__(self): + self.enable_corruption = True + self.concatenate_terms = True + + # observation groups + policy: PolicyCfg = PolicyCfg() + + +@configclass +class EventCfg: + """Configuration for events.""" + + robot_physics_material = EventTerm( + func=mdp.randomize_rigid_body_material, + mode="startup", + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=".*"), + "static_friction_range": (0.8, 1.25), + "dynamic_friction_range": (0.8, 1.25), + "restitution_range": (0.0, 0.0), + "num_buckets": 16, + }, + ) + + cabinet_physics_material = EventTerm( + func=mdp.randomize_rigid_body_material, + mode="startup", + params={ + "asset_cfg": SceneEntityCfg("cabinet", body_names="drawer_handle_top"), + # Lowered from (2.0, 2.5) — high friction let the robot drag the drawer + # open by pressing down on TOP of the bar. Lower friction forces it to + # mechanically HOOK a finger behind the bar to pull. Tune upward if the + # grip slips too much once hooking is learned. + # Higher friction so a clamped pinch grip can transfer pulling force. + # The contact-based rewards block cheats, so high friction is safe again. + "static_friction_range": (1.5, 2.0), + "dynamic_friction_range": (1.2, 1.6), + "restitution_range": (0.0, 0.0), + "num_buckets": 16, + }, + ) + + reset_all = EventTerm(func=mdp.reset_scene_to_default, mode="reset") + + reset_robot_joints = EventTerm( + func=mdp.reset_joints_by_offset, + mode="reset", + params={ + "asset_cfg": SceneEntityCfg( + "robot", + joint_names=["joint1", "joint2", "joint3", "joint4", "joint5", "joint6", "joint7", "joint8"] + ), + "position_range": (-0.1, 0.1), + "velocity_range": (0.0, 0.0), + }, + ) + + +@configclass +class RewardsCfg: + """Reward terms for the MDP.""" + + # ========================================================================= + # CLEAN REWARD LADDER + # Each behavior is rewarded EXACTLY ONCE. Weights escalate monotonically by + # stage, and drawer-opening rewards are GATED behind a correct straddle grip + # so the drawer cannot be opened by cheating (top-drape / friction drag). + # Dynamic range is kept sane (0.5 .. 300) to avoid one reward swamping the rest. + # ========================================================================= + + # ── STAGE 1: Reach the handle (dense, small) ───────────────────────────── + approach_ee_handle = RewTerm(func=mdp.approach_ee_handle, weight=2.0, params={"threshold": 0.2}) + align_ee_handle = RewTerm(func=mdp.align_ee_handle, weight=0.5) + + # ── STAGE 2: Position the OPEN claw near the handle (dense, medium) ─────── + # Breadcrumb: either finger getting near (OR logic) — keeps a gradient alive early. + single_claw_proximity = RewTerm( + func=mdp.single_claw_proximity, + weight=5.0, + params={"contact_radius": 0.06}, + ) + # Lightly encourage an OPEN claw ONLY during the far approach so the bar can pass + # between the fingers. Weight cut from 100 → 15: it must NOT fight the claw CLOSING + # to clamp the bar once in position (that conflict stalled pulling entirely). + open_claw_approach = RewTerm( + func=mdp.open_claw_approach_reward, + weight=15.0, + params={ + "gripper_cfg": SceneEntityCfg("robot", joint_names=["joint7", "joint8"]), + "open_target": 0.1, + "aperture_sigma": 0.04, + "proximity_radius": 0.15, + }, + ) + # Rotate the approach so the fingers move toward OPPOSITE sides of the bar. + approach_angle_reward = RewTerm( + func=mdp.approach_angle_reward, + weight=10.0, + params={"proximity_radius": 0.15}, + ) + + # ── STAGE 3: Bring BOTH fingers in together (milestone, medium-high) ────── + # Stepping stone between single-finger proximity and full straddle. + dual_approach_bonus = RewTerm( + func=mdp.dual_approach_bonus, + weight=150.0, # Boosted from 80 — stronger gradient for the 5cm→2cm push + params={"near_threshold": 0.06}, + ) + + # ── STAGE 4: Get both fingers to opposite sides of the bar (positioning) ── + # Guides the two fingers to straddle the bar (over/under or either side) so their + # INNER faces can make contact. No behind/hook bias — any opposite-side config works. + dual_claw_straddle = RewTerm( + func=mdp.dual_claw_straddle, + weight=150.0, + params={"contact_radius": 0.08}, + ) + + # ── STAGE 5: GOOD GRIP — both INNER edges touching the handle (contact sensor) ── + # NOTE: Small weight — this is a GUIDING signal only, not a reward to be farmed. + # The real payoff is in Stage 6 where grip quality multiplies the pull reward. + # If this were large, the robot would learn to just hold the grip and never pull. + inner_edge_grip = RewTerm( + func=mdp.inner_edge_grip_reward, + weight=1.0, # Reduced from 10 — grip farming dominated pull reward at 10 + params={ + "force_threshold": 1.0, + "both_bonus": 4.0, + "center_sigma": 0.04, + }, + ) + + # ── STAGE 6: Pull the drawer (goal — ALL gated by a GOOD GRIP: both inner edges) ── + # No pull reward fires unless BOTH inner edges are in contact (good_grip_gate). + # + # Extremely steep, continuously-accelerating pull reward: C*(exp(k*f) - 1) with + # C=1000, k=10, calibrated so every f=0.0001 (~0.039mm of drawer travel) is worth + # 1.0 raw point right from the start, then keeps accelerating hard as the drawer + # opens further (see pull_distance_reward docstring for the full milestone table). + # weight=1.0 preserves that "0.0001f = 1 point" calibration directly in the + # logged Episode_Reward/pull_distance_reward metric. + pull_distance_reward = RewTerm( + func=mdp.pull_distance_reward, + weight=1.0, # Raised from 0.1 — keeps the "f=0.0001 -> 1pt" calibration exact + params={ + "asset_cfg": SceneEntityCfg("cabinet", joint_names=["drawer_top_joint"]), + "max_open": 0.39, + "force_threshold": 1.0, + # Both inner edges gripping gives 4× the pull reward (1 + 3.0). + # Single-edge pull still earns the full base reward (hook-and-pull stays valid). + "dual_grip_bonus": 3.0, + }, + ) + # Velocity reward — momentum multiplier makes one sustained pull worth 4× jerky tugs. + continuous_pull_reward = RewTerm( + func=mdp.continuous_pull_reward, + weight=500.0, # Up from 200 — continuous pull is the target behavior + params={ + "asset_cfg": SceneEntityCfg("cabinet", joint_names=["drawer_top_joint"]), + "force_threshold": 1.0, + "momentum_bonus": 3.0, # up to 4× multiplier for sustained pulls + "velocity_threshold": 0.02, # m/s — min velocity to count as "pulling" + }, + ) + # Posture bonus: upright wrist while holding a good grip and pulling. + upright_pull_bonus = RewTerm( + func=mdp.upright_pull_bonus, + weight=0.2, + params={ + "asset_cfg": SceneEntityCfg("cabinet", joint_names=["drawer_top_joint"]), + "threshold": 0.01, + "force_threshold": 1.0, + }, + ) + + # ── PENALTIES (small, proportional) ────────────────────────────────────── + # Cosmetic smoothness — scale down as the drawer opens so they never block pulling. + action_rate_l2 = RewTerm( + func=mdp.conditional_action_rate_l2, + weight=-0.05, + params={"asset_cfg": SceneEntityCfg("cabinet", joint_names=["drawer_top_joint"])}, + ) + joint_vel = RewTerm( + func=mdp.conditional_joint_vel_l2, + weight=-0.01, + params={ + "asset_cfg": SceneEntityCfg("cabinet", joint_names=["drawer_top_joint"]), + "robot_cfg": SceneEntityCfg("robot", joint_names=["joint1", "joint2", "joint3", "joint4", "joint5", "joint6", "joint7", "joint8"]) + }, + ) + + # ── DEBUG (weight 0 — prints INNER vs OUTER edge contact counts per iteration) ── + debug_inner_edge = RewTerm(func=mdp.debug_inner_edge, weight=0.0) + + +@configclass +class TerminationsCfg: + """Termination terms for the MDP.""" + + time_out = DoneTerm(func=mdp.time_out, time_out=True) + success = DoneTerm( + func=mdp.joint_pos_out_of_manual_limit, + params={"asset_cfg": SceneEntityCfg("cabinet", joint_names=["drawer_top_joint"]), "bounds": (0.0, 0.39)} + ) + +@configclass +class CurriculumCfg: + """Curriculum terms for the MDP.""" + pass + + +## +# Environment configuration +## + + +@configclass +class CabinetEnvCfg(ManagerBasedRLEnvCfg): + """Configuration for the cabinet environment.""" + + # Scene settings + scene: CabinetSceneCfg = CabinetSceneCfg(num_envs=4096, env_spacing=2.0) + # Basic settings + observations: ObservationsCfg = ObservationsCfg() + actions: ActionsCfg = ActionsCfg() + # MDP settings + rewards: RewardsCfg = RewardsCfg() + terminations: TerminationsCfg = TerminationsCfg() + events: EventCfg = EventCfg() + curriculum: CurriculumCfg = CurriculumCfg() + + def __post_init__(self): + """Post initialization.""" + # general settings + self.decimation = 1 + self.episode_length_s = 8.0 + self.viewer.eye = (-2.0, 2.0, 2.0) + self.viewer.lookat = (0.8, 0.0, 0.5) + # simulation settings + self.sim.dt = 1 / 60 # 60Hz + self.sim.render_interval = self.decimation + self.sim.physx.bounce_threshold_velocity = 0.2 + self.sim.physx.bounce_threshold_velocity = 0.01 + self.sim.physx.friction_correlation_distance = 0.00625 + self.scene.robot.init_state.pos = (-0.1, 0.0, 0.4) # Reverted to ORIGINAL position + # Enable contact reporting on the robot so the finger ContactSensors work + self.scene.robot.spawn.activate_contact_sensors = True + + +# PYTHONPATH=$(pwd) /home/hy/IsaacLab/isaaclab.sh -p HumanoidRLPackage/rsl_rl_scripts/train.py --task=Isaac-Open-Drawer-Humanoid-Arm-v0 --headless + +# PYTHONPATH=$(pwd) /home/hy/IsaacLab/isaaclab.sh -p HumanoidRLPackage/rsl_rl_scripts/play.py --task=Isaac-Open-Drawer-Humanoid-Arm-v0 --num_envs=1 diff --git a/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/bimanual_cabinet/config/HumanoidRLEnv/__init__.py b/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/bimanual_cabinet/config/HumanoidRLEnv/__init__.py new file mode 100644 index 00000000..235b81c3 --- /dev/null +++ b/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/bimanual_cabinet/config/HumanoidRLEnv/__init__.py @@ -0,0 +1,23 @@ +import gymnasium as gym + +from . import agents + +gym.register( + id="Isaac-Open-Drawer-Bimanual-Arm-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.joint_pos_env_cfg:BimanualArmCabinetEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:BimanualArmCabinetPPORunnerCfg", + }, +) + +gym.register( + id="Isaac-Open-Drawer-Bimanual-Arm-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.joint_pos_env_cfg:BimanualArmCabinetEnvCfg_PLAY", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:BimanualArmCabinetPPORunnerCfg", + }, +) diff --git a/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/bimanual_cabinet/config/HumanoidRLEnv/agents/__init__.py b/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/bimanual_cabinet/config/HumanoidRLEnv/agents/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/bimanual_cabinet/config/HumanoidRLEnv/agents/rsl_rl_ppo_cfg.py b/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/bimanual_cabinet/config/HumanoidRLEnv/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 00000000..f2ac5a04 --- /dev/null +++ b/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/bimanual_cabinet/config/HumanoidRLEnv/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,36 @@ +from isaaclab.utils import configclass +from isaaclab_rl.rsl_rl import ( + RslRlOnPolicyRunnerCfg, + RslRlPpoActorCriticCfg, + RslRlPpoAlgorithmCfg, +) + + +@configclass +class BimanualArmCabinetPPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 192 # doubled from 96 — longer rollouts capture full pull sequences + max_iterations = 500 + save_interval = 50 + experiment_name = "bimanual_cabinet" + empirical_normalization = True + policy = RslRlPpoActorCriticCfg( + init_noise_std=1.0, + actor_hidden_dims=[256, 128, 64], + critic_hidden_dims=[256, 128, 64], + activation="elu", + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=0.5, # reduced from 1.0 — critic was dominating the optimizer + # when value loss is 10k× larger than surrogate loss + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=0.0, + num_learning_epochs=5, # reverted from 8 — 8 epochs caused policy collapse + num_mini_batches=4, + learning_rate=5.0e-4, + schedule="adaptive", + gamma=0.99, + lam=0.95, + desired_kl=0.016, # tightened from 0.02 — smaller steps when critic is unstable + max_grad_norm=1.0, + ) diff --git a/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/bimanual_cabinet/config/HumanoidRLEnv/joint_pos_env_cfg.py b/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/bimanual_cabinet/config/HumanoidRLEnv/joint_pos_env_cfg.py new file mode 100644 index 00000000..73013a80 --- /dev/null +++ b/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/bimanual_cabinet/config/HumanoidRLEnv/joint_pos_env_cfg.py @@ -0,0 +1,18 @@ +from isaaclab.utils import configclass + +from HumanoidRLPackage.HumanoidRLSetup.tasks.bimanual_cabinet.bimanual_cabinet_env_cfg import CabinetEnvCfg + + +@configclass +class BimanualArmCabinetEnvCfg(CabinetEnvCfg): + """Cabinet open-drawer task for the bimanual arm.""" + pass + + +@configclass +class BimanualArmCabinetEnvCfg_PLAY(BimanualArmCabinetEnvCfg): + def __post_init__(self): + super().__post_init__() + self.scene.num_envs = 50 + self.scene.env_spacing = 2.5 + self.observations.policy.enable_corruption = False diff --git a/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/bimanual_cabinet/config/__init__.py b/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/bimanual_cabinet/config/__init__.py new file mode 100644 index 00000000..d7c38f5c --- /dev/null +++ b/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/bimanual_cabinet/config/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configurations for the cabinet environments.""" + +# We leave this file empty since we don't want to expose any configs in this package directly. +# We still need this file to import the "config" module in the parent package. diff --git a/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/bimanual_cabinet/mdp/__init__.py b/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/bimanual_cabinet/mdp/__init__.py new file mode 100644 index 00000000..79a9af2f --- /dev/null +++ b/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/bimanual_cabinet/mdp/__init__.py @@ -0,0 +1,11 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""This sub-module contains the functions that are specific to the cabinet environments.""" + +from isaaclab.envs.mdp import * # noqa: F401, F403 + +from .observations import * # noqa: F401, F403 +from .rewards import * # noqa: F401, F403 diff --git a/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/bimanual_cabinet/mdp/observations.py b/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/bimanual_cabinet/mdp/observations.py new file mode 100644 index 00000000..5016ded6 --- /dev/null +++ b/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/bimanual_cabinet/mdp/observations.py @@ -0,0 +1,66 @@ +from __future__ import annotations + +from typing import TYPE_CHECKING + +import torch + +import isaaclab.utils.math as math_utils +from isaaclab.assets import ArticulationData +from isaaclab.sensors import FrameTransformerData + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +def _robot_ee_pos(env: ManagerBasedRLEnv) -> torch.Tensor | None: + """End-effector position from robot articulation (body DIP_INDEX). Returns None if not ready.""" + robot = env.scene["robot"] + ids, _ = robot.find_bodies("link7", preserve_order=True) + if not ids: + return None + return robot.data.body_pos_w[:, ids[0], :] + + +def rel_ee_object_distance(env: ManagerBasedRLEnv) -> torch.Tensor: + """The distance between the end-effector and the object.""" + ee_tf_data: FrameTransformerData = env.scene["ee_frame"].data + object_data: ArticulationData = env.scene["object"].data + + return object_data.root_pos_w - ee_tf_data.target_pos_w[..., 0, :] + + +def rel_ee_drawer_distance(env: ManagerBasedRLEnv) -> torch.Tensor: + """The distance between the end-effector and the object.""" + ee_pos = _robot_ee_pos(env) + if ee_pos is None: + return torch.zeros(env.num_envs, 3, device=env.device) + cabinet_tf_data: FrameTransformerData = env.scene["cabinet_frame"].data + handle_pos = cabinet_tf_data.target_pos_w[..., 0, :] + return handle_pos - ee_pos + + +def fingertips_pos(env: ManagerBasedRLEnv) -> torch.Tensor: + """The position of the fingertips relative to the environment origins.""" + ee_tf_data: FrameTransformerData = env.scene["ee_frame"].data + fingertips_pos = ee_tf_data.target_pos_w[..., 1:, :] - env.scene.env_origins.unsqueeze(1) + + return fingertips_pos.view(env.num_envs, -1) + + +def ee_pos(env: ManagerBasedRLEnv) -> torch.Tensor: + """The position of the end-effector relative to the environment origins.""" + ee_tf_data: FrameTransformerData = env.scene["ee_frame"].data + ee_pos = ee_tf_data.target_pos_w[..., 0, :] - env.scene.env_origins + + return ee_pos + + +def ee_quat(env: ManagerBasedRLEnv, make_quat_unique: bool = True) -> torch.Tensor: + """The orientation of the end-effector in the environment frame. + + If :attr:`make_quat_unique` is True, the quaternion is made unique by ensuring the real part is positive. + """ + ee_tf_data: FrameTransformerData = env.scene["ee_frame"].data + ee_quat = ee_tf_data.target_quat_w[..., 0, :] + # make first element of quaternion positive + return math_utils.quat_unique(ee_quat) if make_quat_unique else ee_quat diff --git a/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/bimanual_cabinet/mdp/rewards.py b/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/bimanual_cabinet/mdp/rewards.py new file mode 100644 index 00000000..d3f4996c --- /dev/null +++ b/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/bimanual_cabinet/mdp/rewards.py @@ -0,0 +1,797 @@ +from __future__ import annotations + +from typing import TYPE_CHECKING + +import torch + +from isaaclab.managers import SceneEntityCfg +from isaaclab.utils.math import matrix_from_quat + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +def _robot_ee_pose(env: ManagerBasedRLEnv): + """(ee_tcp_pos, ee_tcp_quat, lfinger_pos, rfinger_pos) from robot articulation bodies.""" + robot = env.scene["robot"] + # For bimanual arm, the right hand fingers are link7 and link8 + ee_ids, _ = robot.find_bodies("link7", preserve_order=True) + thumb_ids, _ = robot.find_bodies("link8", preserve_order=True) + if not ee_ids or not thumb_ids: + return None + lfinger_pos = robot.data.body_pos_w[:, ee_ids[0], :] + rfinger_pos = robot.data.body_pos_w[:, thumb_ids[0], :] + # The true End-Effector TCP is exactly halfway between the two fingers! + ee_tcp_pos = (lfinger_pos + rfinger_pos) / 2.0 + ee_tcp_quat = robot.data.body_quat_w[:, ee_ids[0], :] + return (ee_tcp_pos, ee_tcp_quat, lfinger_pos, rfinger_pos) + + +def approach_ee_handle(env: ManagerBasedRLEnv, threshold: float) -> torch.Tensor: + r"""Reward the robot for reaching the drawer handle using inverse-square law. + + It uses a piecewise function to reward the robot for reaching the handle. + + .. math:: + + reward = \begin{cases} + 2 * (1 / (1 + distance^2))^2 & \text{if } distance \leq threshold \\ + (1 / (1 + distance^2))^2 & \text{otherwise} + \end{cases} + + """ + pose = _robot_ee_pose(env) + if pose is None: + return torch.zeros(env.num_envs, device=env.device) + ee_tcp_pos, _, _, _ = pose + handle_pos = env.scene["cabinet_frame"].data.target_pos_w[..., 0, :] + + distance = torch.norm(handle_pos - ee_tcp_pos, dim=-1, p=2) + reward = 1.0 / (1.0 + distance**2) + reward = torch.pow(reward, 2) + return torch.where(distance <= threshold, 2 * reward, reward) + + +def align_ee_handle(env: ManagerBasedRLEnv) -> torch.Tensor: + """Reward for aligning the end-effector with the handle.""" + pose = _robot_ee_pose(env) + if pose is None: + return torch.zeros(env.num_envs, device=env.device) + _, ee_tcp_quat, _, _ = pose + handle_quat = env.scene["cabinet_frame"].data.target_quat_w[..., 0, :] + + ee_tcp_rot_mat = matrix_from_quat(ee_tcp_quat) + handle_mat = matrix_from_quat(handle_quat) + + handle_x, handle_y = handle_mat[..., 0], handle_mat[..., 1] + ee_tcp_x, ee_tcp_z = ee_tcp_rot_mat[..., 0], ee_tcp_rot_mat[..., 2] + + align_z = torch.bmm(ee_tcp_z.unsqueeze(1), -handle_x.unsqueeze(-1)).squeeze(-1).squeeze(-1) + align_x = torch.bmm(ee_tcp_x.unsqueeze(1), -handle_y.unsqueeze(-1)).squeeze(-1).squeeze(-1) + return 0.5 * (torch.sign(align_z) * align_z**2 + torch.sign(align_x) * align_x**2) + + +def align_grasp_around_handle(env: ManagerBasedRLEnv) -> torch.Tensor: + """Bonus for correct hand orientation around the handle.""" + handle_pos = env.scene["cabinet_frame"].data.target_pos_w[..., 0, :] + pose = _robot_ee_pose(env) + if pose is None: + return torch.zeros(env.num_envs, device=env.device, dtype=torch.bool) + _, _, lfinger_pos, rfinger_pos = pose + + is_graspable = (rfinger_pos[:, 2] < handle_pos[:, 2]) & (lfinger_pos[:, 2] > handle_pos[:, 2]) + return is_graspable + + +def approach_gripper_handle(env: ManagerBasedRLEnv, offset: float = 0.04) -> torch.Tensor: + """Reward the robot's gripper reaching the drawer handle with the right pose.""" + handle_pos = env.scene["cabinet_frame"].data.target_pos_w[..., 0, :] + pose = _robot_ee_pose(env) + if pose is None: + return torch.zeros(env.num_envs, device=env.device) + _, _, lfinger_pos, rfinger_pos = pose + + lfinger_dist = torch.abs(lfinger_pos[:, 2] - handle_pos[:, 2]) + rfinger_dist = torch.abs(rfinger_pos[:, 2] - handle_pos[:, 2]) + is_graspable = (rfinger_pos[:, 2] < handle_pos[:, 2]) & (lfinger_pos[:, 2] > handle_pos[:, 2]) + + return is_graspable * ((offset - lfinger_dist) + (offset - rfinger_dist)) + + +def grasp_handle( + env: ManagerBasedRLEnv, threshold: float, open_joint_pos: float, asset_cfg: SceneEntityCfg +) -> torch.Tensor: + """Reward for closing the fingers when being close to the handle.""" + pose = _robot_ee_pose(env) + if pose is None: + return torch.zeros(env.num_envs, device=env.device) + ee_tcp_pos, _, _, _ = pose + handle_pos = env.scene["cabinet_frame"].data.target_pos_w[..., 0, :] + gripper_joint_pos = env.scene[asset_cfg.name].data.joint_pos[:, asset_cfg.joint_ids] + + distance = torch.norm(handle_pos - ee_tcp_pos, dim=-1, p=2) + is_close = distance <= threshold + + # The physics engine will now naturally clamp gripper_joint_pos to 0.015! + return is_close * torch.sum(open_joint_pos - torch.abs(gripper_joint_pos), dim=-1) + + +def _claw_distances(env: ManagerBasedRLEnv): + """Return (d_link7, d_link8, lfinger_pos, rfinger_pos, handle_pos) or None.""" + pose = _robot_ee_pose(env) + if pose is None: + return None + _, _, lfinger_pos, rfinger_pos = pose + handle_pos = env.scene["cabinet_frame"].data.target_pos_w[..., 0, :] + d_link7 = torch.norm(handle_pos - lfinger_pos, dim=-1, p=2) + d_link8 = torch.norm(handle_pos - rfinger_pos, dim=-1, p=2) + return d_link7, d_link8, lfinger_pos, rfinger_pos, handle_pos + + +# Module-level tracking: closest each claw has gotten within the current PPO iteration. +_last_claw_print_step: int = -1 +_min_d7_this_iter: float = float("inf") +_min_d8_this_iter: float = float("inf") +_last_f_print_step: int = -1 +_max_f_this_iter: float = 0.0 +_mag7_at_max_f: float = 0.0 +_mag8_at_max_f: float = 0.0 +_sum_f_this_iter: float = 0.0 +_count_f_this_iter: int = 0 + + +def single_claw_proximity(env: ManagerBasedRLEnv, contact_radius: float = 0.06) -> torch.Tensor: + """Breadcrumb reward: points for each individual claw being near the handle (OR logic). + + Gives a soft Gaussian signal for link7 and link8 independently so the arm + learns to bring EITHER finger close before we demand both. + Prints the closest each claw got during the iteration, once per PPO iteration. + """ + global _last_claw_print_step, _min_d7_this_iter, _min_d8_this_iter + + result = _claw_distances(env) + if result is None: + return torch.zeros(env.num_envs, device=env.device) + + d_link7, d_link8, _, _, _ = result + + k = 3.0 / contact_radius + score_link7 = torch.exp(-k * d_link7) + score_link8 = torch.exp(-k * d_link8) + + # Track the closest any env got this iteration + _min_d7_this_iter = min(_min_d7_this_iter, d_link7.min().item()) + _min_d8_this_iter = min(_min_d8_this_iter, d_link8.min().item()) + + # One print per PPO iteration. + # common_step_counter increments by 1 per env.step() call (not by num_envs). + # RSL-RL collects num_steps_per_env=96 steps per iteration, so counter advances by 96. + log_interval = 96 + if env.common_step_counter > 0 and env.common_step_counter - _last_claw_print_step >= log_interval: + print( + f"[Claw best] iter_end={env.common_step_counter} | " + f"link7 closest: {_min_d7_this_iter:.3f}m " + f"link8 closest: {_min_d8_this_iter:.3f}m", + flush=True, + ) + _last_claw_print_step = env.common_step_counter + _min_d7_this_iter = float("inf") + _min_d8_this_iter = float("inf") + + # Sum (not product) so either finger getting close earns points + return score_link7 + score_link8 + + +def approach_angle_reward(env: ManagerBasedRLEnv, proximity_radius: float = 0.15) -> torch.Tensor: + """Continuous gradient that rewards increasing the spread angle between the two fingers. + + Fires whenever fingers are within proximity_radius of the handle, even when they + are still on the SAME side. This fills the gradient dead-zone of dual_claw_straddle + (which gives ~0 gradient when cos_angle > 0). + + Reward = prox_gate * (1 - cos_angle) / 2 + - cos_angle = -1 (perfect opposite) → reward = 1.0 + - cos_angle = 0 (perpendicular) → reward = 0.5 + - cos_angle = +1 (same side) → reward = 0.0 + + The gradient always pushes the policy to rotate the approach so fingers move + toward opposite sides of the handle, regardless of current configuration. + """ + result = _claw_distances(env) + if result is None: + return torch.zeros(env.num_envs, device=env.device) + + d_link7, d_link8, lfinger_pos, rfinger_pos, handle_pos = result + + # Soft proximity gate — fires when AVERAGE finger distance is within proximity_radius + avg_dist = (d_link7 + d_link8) * 0.5 + prox_gate = torch.exp(-3.0 * avg_dist / proximity_radius) + + # Direction vectors from handle center to each finger + vec7 = lfinger_pos - handle_pos + vec8 = rfinger_pos - handle_pos + + # Project onto plane perpendicular to handle bar length (handle local Y-axis) + # so hovering over top at opposite ends of the bar does not count as opposite sides + handle_quat = env.scene["cabinet_frame"].data.target_quat_w[..., 0, :] + handle_mat = matrix_from_quat(handle_quat) + handle_y = handle_mat[..., 1] # (N, 3) + + vec7_perp = vec7 - (vec7 * handle_y).sum(dim=-1, keepdim=True) * handle_y + vec8_perp = vec8 - (vec8 * handle_y).sum(dim=-1, keepdim=True) * handle_y + + u7 = vec7_perp / (torch.norm(vec7_perp, dim=-1, keepdim=True) + 1e-6) + u8 = vec8_perp / (torch.norm(vec8_perp, dim=-1, keepdim=True) + 1e-6) + + cos_angle = (u7 * u8).sum(dim=-1) # (N,) in [-1, +1] + + # Map to [0, 1]: 1.0 = perfect opposite, 0.0 = same direction + angle_reward = (1.0 - cos_angle) * 0.5 + + return prox_gate * angle_reward + + +def dual_claw_straddle(env: ManagerBasedRLEnv, contact_radius: float = 0.04) -> torch.Tensor: + """Reward both claws being close AND on OPPOSITE sides of the handle center. + + Uses a direction-agnostic dot-product check: + - Compute the unit vector from the handle to each finger: u7, u8 + - If dot(u7, u8) < 0, the fingers are on opposite sides of the handle + along WHATEVER axis they happen to be on (any line through the origin). + - This is correct regardless of which direction the robot approaches from. + + Gate 1 (proximity): exp(-k * average finger distance), k = 3.0 / contact_radius. + Gate 2 (opposite-side): dot(u7, u8) < 0 → angle between them > 90°. + Perfect opposite sides → cos = -1 → score = 1.0 + Perpendicular → cos = 0 → score = 0.5 + Same side → cos = +1 → score ≈ 0.0 + Veto: if BOTH fingers are above the bar top (top-drape), the score is zeroed. + """ + result = _claw_distances(env) + if result is None: + return torch.zeros(env.num_envs, device=env.device) + + d_link7, d_link8, lfinger_pos, rfinger_pos, handle_pos = result + + # ── Gate 1: proximity — use LINEAR gate instead of product-of-exponentials ── + # Product gate collapses near zero when both fingers are at 5cm (score ≈ 0.003). + # Instead: reward = exp(-k * AVERAGE distance), so both fingers at 5cm gives + # exp(-3.0 * 0.048 / 0.10) = exp(-1.44) = 0.24 — a usable gradient. + avg_dist = (d_link7 + d_link8) * 0.5 + k = 3.0 / contact_radius + dual_proximity = torch.exp(-k * avg_dist) + + # ── Gate 2: direction-agnostic opposite-side check in handle cross-section ── + # Vectors from handle center to each finger + vec7 = lfinger_pos - handle_pos # (N, 3) + vec8 = rfinger_pos - handle_pos # (N, 3) + + # Project out handle bar length direction (handle local Y-axis) + handle_quat = env.scene["cabinet_frame"].data.target_quat_w[..., 0, :] + handle_mat = matrix_from_quat(handle_quat) + handle_y = handle_mat[..., 1] # (N, 3) + + vec7_perp = vec7 - (vec7 * handle_y).sum(dim=-1, keepdim=True) * handle_y + vec8_perp = vec8 - (vec8 * handle_y).sum(dim=-1, keepdim=True) * handle_y + + # Normalize (add small epsilon to avoid division by zero) + u7 = vec7_perp / (torch.norm(vec7_perp, dim=-1, keepdim=True) + 1e-6) # (N, 3) + u8 = vec8_perp / (torch.norm(vec8_perp, dim=-1, keepdim=True) + 1e-6) # (N, 3) + + # Cosine of the angle between the two finger directions in cross-section plane + cos_angle = (u7 * u8).sum(dim=-1) # (N,) — range [-1, +1] + + # Sigmoid gate: scores near 1.0 when cos < 0 (opposite sides), 0.0 when same side. + # Sharpness factor 8 gives a clear threshold around cos = 0. + opposite_side_score = torch.sigmoid(-8.0 * cos_angle) + + # ── Top-drape veto ─────────────────────────────────────────────────────── + # If BOTH fingers are above the bar, it's a top-drape — zero the straddle score. + # One finger above + one below is a legitimate vertical straddle and is allowed. + both_above = (lfinger_pos[:, 2] > handle_pos[:, 2] + 0.01) & \ + (rfinger_pos[:, 2] > handle_pos[:, 2] + 0.01) + anti_cheat = (~both_above).float() + + return dual_proximity * opposite_side_score * anti_cheat + + +def _finger_handle_contact(env: ManagerBasedRLEnv, force_threshold: float = 1.0): + """Return (contact7, contact8) boolean tensors: is each finger touching the handle? + + Reads the ContactSensor force_matrix_w for link7 and link8 (filtered to the handle + body only), so contact with anything OTHER than the handle does not count. + A finger is 'in contact' when the contact force magnitude with the handle exceeds + force_threshold newtons. + """ + def _contact_mag(sensor_name: str) -> torch.Tensor: + sensor = env.scene.sensors[sensor_name] + fmat = sensor.data.force_matrix_w # (N, num_bodies, num_filters, 3) or None + if fmat is None: + return torch.zeros(env.num_envs, device=env.device) + # Sum force magnitude across all bodies/filters for this sensor + return torch.norm(fmat, dim=-1).reshape(env.num_envs, -1).sum(dim=-1) + + mag7 = _contact_mag("contact_link7") + mag8 = _contact_mag("contact_link8") + return mag7 > force_threshold, mag8 > force_threshold + + +def _finger_contact_force(env: ManagerBasedRLEnv): + """Return (f7, f8) net contact force VECTORS (N,3) on link7/link8 from the handle.""" + def _force(name: str) -> torch.Tensor: + sensor = env.scene.sensors[name] + fmat = sensor.data.force_matrix_w # (N, bodies, filters, 3) or None + if fmat is None: + return torch.zeros(env.num_envs, 3, device=env.device) + return fmat.reshape(env.num_envs, -1, 3).sum(dim=1) + return _force("contact_link7"), _force("contact_link8") + + +def _inner_edge_contact( + env: ManagerBasedRLEnv, + force_threshold: float = 1.0, + dir_margin: float = 0.2, + top_margin: float = 0.02, +): + """Return (inner7, inner8) booleans: is each finger's INNER edge touching the handle? + + Contact sensors alone cannot tell WHICH face touched, so we use the contact FORCE + DIRECTION to be certain: + - The inner faces of link7 and link8 face EACH OTHER. + - When the handle presses a finger's INNER face, the reaction force on that finger + points OUTWARD — away from the other finger (direction p_self - p_other). + - When it presses the OUTER (back) face, the force points INWARD (toward the other + finger) → REJECTED. + A contact counts as inner ONLY when force magnitude > force_threshold AND the force + direction projects onto the outward axis by more than dir_margin. This guarantees we + never mistake an outer-edge press for a grip. + + TOP-DRAPE GUARD: a finger draped over the TOP of the bar and pulled backward presses + its inner face against the BACK of the bar, which produces an outward-pointing force + and would otherwise pass the direction test — this is the "hook the top" cheat seen + in training. We additionally require the contacting finger to sit at or below the + bar top (finger_z <= handle_z + top_margin). A finger clearly above the bar top is + draping on top, not hooking behind/under the bar, so its contact is REJECTED. + """ + pose = _robot_ee_pose(env) + if pose is None: + z = torch.zeros(env.num_envs, dtype=torch.bool, device=env.device) + return z, z + _, _, p7, p8 = pose + f7, f8 = _finger_contact_force(env) + + mag7 = torch.norm(f7, dim=-1) + mag8 = torch.norm(f8, dim=-1) + + # Outward axis for each finger (away from the other finger) + out7 = p7 - p8 + out8 = p8 - p7 + out7 = out7 / (torch.norm(out7, dim=-1, keepdim=True) + 1e-6) + out8 = out8 / (torch.norm(out8, dim=-1, keepdim=True) + 1e-6) + + f7u = f7 / (mag7.unsqueeze(-1) + 1e-6) + f8u = f8 / (mag8.unsqueeze(-1) + 1e-6) + + # >0 means the contact force points outward → the INNER face is what touched + proj7 = (f7u * out7).sum(dim=-1) + proj8 = (f8u * out8).sum(dim=-1) + + # Top-drape guard: reject contacts from a finger sitting above the bar top. + handle_pos = env.scene["cabinet_frame"].data.target_pos_w[..., 0, :] + below_top7 = p7[:, 2] <= handle_pos[:, 2] + top_margin + below_top8 = p8[:, 2] <= handle_pos[:, 2] + top_margin + + inner7 = (mag7 > force_threshold) & (proj7 > dir_margin) & below_top7 + inner8 = (mag8 > force_threshold) & (proj8 > dir_margin) & below_top8 + return inner7, inner8 + + +def good_grip_gate(env: ManagerBasedRLEnv, force_threshold: float = 1.0) -> torch.Tensor: + """1.0 when EITHER inner edge contacts the handle (relaxed grip), else 0.0. + + RELAXED (H2): only ONE inner edge needs to touch the handle to count as a good + enough grip to start pulling. Requiring BOTH inner edges simultaneously was too + strict — the robot could reliably get one finger's inner face on the bar but not + both at once, so the pull rewards never unlocked. With one inner edge the robot + can hook and pull from there. Outer-edge contact still never qualifies (the + force-direction test in _inner_edge_contact rejects it). + """ + inner7, inner8 = _inner_edge_contact(env, force_threshold) + return (inner7 | inner8).float() + + +def inner_edge_grip_reward( + env: ManagerBasedRLEnv, + force_threshold: float = 1.0, + both_bonus: float = 4.0, + center_sigma: float = 0.04, +) -> torch.Tensor: + """Reward INNER-edge contact only, with a CENTER-GRIP multiplier. + + +1 per inner edge touching (outer-edge earns NOTHING). + When BOTH inner edges touch, the both_bonus fires AND is scaled by a Gaussian + centered on the handle's midpoint along its bar axis (local Y). Gripping at the + exact center of the bar earns full center multiplier (×2); gripping at the end + cap earns ×1 (no bonus). This steers the claw from the image — hooking on the + end edge — to gripping mid-bar where the pull force is most stable. + + IMPORTANT: The weight on this reward must stay small (≤10). It is a GUIDING signal + only. If it grows large, the robot will farm this reward by holding a perfect grip + all episode without ever pulling. All real reward should come from the pull tier. + """ + inner7, inner8 = _inner_edge_contact(env, force_threshold) + c7 = inner7.float() + c8 = inner8.float() + both = c7 * c8 # 1.0 when both inner edges touch + + # ── Center-grip multiplier ──────────────────────────────────────────────── + pose = _robot_ee_pose(env) + if pose is None: + center_mul = torch.ones(env.num_envs, device=env.device) + else: + _, _, p7, p8 = pose + tcp = (p7 + p8) * 0.5 + handle_pos = env.scene["cabinet_frame"].data.target_pos_w[..., 0, :] + handle_quat = env.scene["cabinet_frame"].data.target_quat_w[..., 0, :] + handle_mat = matrix_from_quat(handle_quat) + handle_y = handle_mat[..., 1] + off_center = ((tcp - handle_pos) * handle_y).sum(dim=-1) + center_score = torch.exp(-(off_center ** 2) / (2.0 * center_sigma ** 2)) + # Center multiplier — LOGARITHMIC (diminishing returns after a certain point). + # At the center (off_center=0): center_score=1.0 → log(1 + 2×1.0) = log(3) ≈ 1.1 + # At one sigma off: center_score=0.61 → log(1 + 2×0.61) = log(2.22) ≈ 0.8 + # At two sigmas off: center_score=0.14 → log(1 + 2×0.14) = log(1.28) ≈ 0.25 + # After the inflection, each extra improvement earns less — no need to obsess over + # perfect center alignment. + center_mul = torch.log(1.0 + 2.0 * center_score) / torch.log(torch.tensor(3.0, device=env.device)) + + # Grip reward is CAPPED — it is a constant recognition of a good grip, not a + # per-step income stream. Once established, the robot earns no more from holding. + # Pull rewards are 100-1000× larger, so the only way to earn significantly more + # is to actually pull the drawer open. + single = torch.clamp(c7 + c8, max=2.0) # max 2.0: both fingers near + both_reward = both_bonus * both * center_mul # max = 4 × 2 = 8.0 at center + + # Hard cap per step so this reward cannot exceed 10.0 regardless of configuration + return torch.clamp(single + both_reward, max=10.0) + + +def first_pull_bonus( + env: ManagerBasedRLEnv, + asset_cfg: SceneEntityCfg, + threshold: float = 0.01, + force_threshold: float = 1.0, +) -> torch.Tensor: + """FLAT bonus: constant the instant a GOOD GRIP (both inner edges) + pull begins. + + Returns a flat 1.0 (× weight) when both inner edges grip AND the drawer moved past + `threshold`. Does not scale — the y-intercept of the pull reward line. + """ + grip = good_grip_gate(env, force_threshold) > 0.5 + drawer_pos = env.scene[asset_cfg.name].data.joint_pos[:, asset_cfg.joint_ids[0]] + opened = drawer_pos > threshold + return (grip & opened).float() + + +def inner_grip_strength(env: ManagerBasedRLEnv, force_threshold: float = 1.0, force_ceiling: float = 30.0) -> torch.Tensor: + """Saturating grip multiplier in [0, 1] based on INNER-EDGE contact FORCE. + + GREATER EMPHASIS: the pull reward now scales with how firmly the inner edge grips — + a light touch earns little, a firm inner-edge grip earns the full multiplier. + CAPPED: saturates at 1.0 once the inner-edge contact force reaches `force_ceiling`, + so beyond a solid grip the multiplier stops increasing (no runaway). + Only forces from edges that pass the inner-edge force-direction test are counted, + so pressing the top/outer face contributes nothing. + """ + inner7, inner8 = _inner_edge_contact(env, force_threshold) + f7, f8 = _finger_contact_force(env) + mag7 = torch.norm(f7, dim=-1) + mag8 = torch.norm(f8, dim=-1) + inner_force = mag7 * inner7.float() + mag8 * inner8.float() + return torch.clamp(inner_force / force_ceiling, max=1.0) + + +def pull_distance_reward( + env: ManagerBasedRLEnv, + asset_cfg: SceneEntityCfg, + max_open: float = 0.39, + force_threshold: float = 1.0, + dual_grip_bonus: float = 1.0, +) -> torch.Tensor: + """Extremely steep, continuously-accelerating pull reward using C*(exp(k*f) - 1). + + Shape: C * (exp(k*f) - 1) where f = drawer_pos / max_open, C=1000, k=10. + Calibrated so the slope AT THE ORIGIN is exactly 10,000 points per unit of f — + i.e. every f=0.0001 of drawer opening is worth 1 point right from the first + micron of pull. Because the curve is exponential, the score doesn't just grow + linearly at that rate — it keeps accelerating, so later progress is worth far + more than early progress: + - f=0.0001 (0.039mm): 1.0 pts (the calibration point — 1 pt per 0.0001f) + - f=0.001 (0.39mm): 10.05 pts + - f=0.01 (3.9mm): 105.17 pts + - f=0.1 (3.9cm): 1,718.28 pts + - f=0.5 (19.5cm): 147,413 pts + - f=1.0 (full, 39cm): 22,025,466 pts — continuously ramps up to a huge payoff + + The grip multiplier is a saturating function of inner-edge contact force so the + robot only needs a good-enough grip, not a perfect one. + """ + # Compute inner-edge contact once and reuse for both grip multipliers below. + inner7, inner8 = _inner_edge_contact(env, force_threshold) + + # Grip multiplier scales with inner-edge contact FORCE, saturating at force_ceiling. + # Firmer inner-edge grip → more pull reward, capped once the grip is solid. + force_ceiling = 30.0 + f7_vec, f8_vec = _finger_contact_force(env) + mag7 = torch.norm(f7_vec, dim=-1) + mag8 = torch.norm(f8_vec, dim=-1) + inner_force = mag7 * inner7.float() + mag8 * inner8.float() + grip_mul = torch.clamp(inner_force / force_ceiling, max=1.0) + + # ── DUAL-GRIP MULTIPLIER ────────────────────────────────────────────────── + # Pulling with BOTH inner edges gripping earns the MOST points. A single inner + # edge still earns the full base pull reward (one-finger hook remains valid), but + # when both inner edges are in contact the whole pull reward is multiplied by + # (1 + dual_grip_bonus). With dual_grip_bonus=3.0 this gives 4× the reward for a + # proper two-finger grip, making it the unambiguously best strategy. + both_inner = (inner7 & inner8).float() + dual_mul = 1.0 + dual_grip_bonus * both_inner + + drawer_pos = env.scene[asset_cfg.name].data.joint_pos[:, asset_cfg.joint_ids[0]] + + global _last_f_print_step, _max_f_this_iter, _mag7_at_max_f, _mag8_at_max_f, _sum_f_this_iter, _count_f_this_iter + + f = torch.clamp(drawer_pos / max_open, min=0.0, max=1.0) + # C=1000, k=10 → slope at the origin is C*k = 10,000 pts per unit of f, i.e. + # every f=0.0001 (0.039mm at max_open=0.39m) is worth ~1.0 point immediately. + # Because it's exponential in f, the reward keeps accelerating well past that: + # f=0.0001 (0.039mm): 1.0 pts (calibration point) + # f=0.001 (0.39mm): 10.05 pts + # f=0.01 (3.9mm): 105.17 pts + # f=0.1 (3.9cm): 1,718.28 pts + # f=0.5 (19.5cm): 147,413 pts + # f=1.0 (full): 22,025,466 pts — scores continuously climb very high + C = 1000.0 + k = 10.0 + distance_score = C * (torch.exp(k * f) - 1.0) + + # Track for debug print — use the already-computed contact forces (f7_vec, f8_vec). + gripped_f = f * (grip_mul > 0.0).float() + best_gripped_idx = gripped_f.argmax().item() + best_gripped_f = gripped_f[best_gripped_idx].item() + if best_gripped_f > _max_f_this_iter: + _max_f_this_iter = best_gripped_f + _mag7_at_max_f = mag7[best_gripped_idx].item() + _mag8_at_max_f = mag8[best_gripped_idx].item() + + _sum_f_this_iter += f.mean().item() + _count_f_this_iter += 1 + + log_interval = 96 + if env.common_step_counter > 0 and env.common_step_counter - _last_f_print_step >= log_interval: + avg_f = _sum_f_this_iter / max(1, _count_f_this_iter) + print( + f"[Pull best] iter_end={env.common_step_counter} | " + f"max f: {_max_f_this_iter:.4f} ({_max_f_this_iter * max_open * 100:.1f}cm) " + f"[F7={_mag7_at_max_f:.1f}N F8={_mag8_at_max_f:.1f}N] | " + f"avg f: {avg_f:.4f} ({avg_f * max_open * 100:.2f}cm)", + flush=True, + ) + _last_f_print_step = env.common_step_counter + _max_f_this_iter = 0.0 + _mag7_at_max_f = 0.0 + _mag8_at_max_f = 0.0 + _sum_f_this_iter = 0.0 + _count_f_this_iter = 0 + + return grip_mul * distance_score * dual_mul + + +def continuous_pull_reward( + env: ManagerBasedRLEnv, + asset_cfg: SceneEntityCfg, + force_threshold: float = 1.0, + momentum_bonus: float = 3.0, + velocity_threshold: float = 0.02, +) -> torch.Tensor: + """Reward SUSTAINED pulling in ONE swing: good grip × drawer velocity × momentum multiplier. + + The momentum multiplier rewards keeping velocity above `velocity_threshold` without + dropping. It accumulates per-env — envs that maintain continuous pulling get a + progressively larger multiplier, while envs that pause and restart reset to 1.0. + This makes one smooth long pull worth far more than many short tugs. + + multiplier per env = 1.0 + momentum_bonus × (sustained_steps / max_steps_per_iter) + where sustained_steps increments each step velocity is above threshold while gripping. + """ + grip = good_grip_gate(env, force_threshold) + drawer_vel = env.scene[asset_cfg.name].data.joint_vel[:, asset_cfg.joint_ids[0]] + pulling_vel = torch.clamp(drawer_vel, min=0.0) + + # Track how many consecutive steps each env has been pulling above threshold + if not hasattr(env, '_sustained_pull_steps'): + env._sustained_pull_steps = torch.zeros(env.num_envs, device=env.device) + + # Increment where gripping + pulling above threshold, reset where not + active = (grip > 0.5) & (pulling_vel > velocity_threshold) + env._sustained_pull_steps = torch.where( + active, + env._sustained_pull_steps + 1.0, + torch.zeros_like(env._sustained_pull_steps) + ) + + # Momentum multiplier: grows with sustained pull, capped at 1 + momentum_bonus + max_steps = 192.0 # one PPO iteration worth of steps (num_steps_per_env) + momentum_mul = 1.0 + momentum_bonus * torch.clamp(env._sustained_pull_steps / max_steps, max=1.0) + + return grip * pulling_vel * momentum_mul + + +def upright_pull_bonus( + env: ManagerBasedRLEnv, + asset_cfg: SceneEntityCfg, + threshold: float = 0.01, + force_threshold: float = 1.0, +) -> torch.Tensor: + """Bonus for upright wrist orientation while holding a GOOD GRIP and pulling.""" + grip = good_grip_gate(env, force_threshold) + drawer_pos = env.scene[asset_cfg.name].data.joint_pos[:, asset_cfg.joint_ids[0]] + opened = (drawer_pos > threshold).float() + + pose = _robot_ee_pose(env) + if pose is None: + return torch.zeros(env.num_envs, device=env.device) + _, ee_tcp_quat, _, _ = pose + ee_rot = matrix_from_quat(ee_tcp_quat) + ee_z = ee_rot[..., 2] + world_z = torch.zeros_like(ee_z) + world_z[:, 2] = 1.0 + upright_score = (ee_z * world_z).sum(dim=-1) ** 2 # peak 1.0 when vertical + + return grip * opened * upright_score + + +# Debug print state for inner-edge contact monitoring +_last_grip_print_step: int = -1 + + +def debug_inner_edge(env: ManagerBasedRLEnv) -> torch.Tensor: + """Weight-0 debug: prints INNER vs OUTER edge contact counts once per PPO iteration. + + Lets you confirm the inner-edge identification is correct — outer contacts are + reported separately so you can be sure a grip is genuinely inner-edge. + """ + global _last_grip_print_step + inner7, inner8 = _inner_edge_contact(env) + any7, any8 = _finger_handle_contact(env) # ANY contact (inner OR outer) + outer7 = any7 & (~inner7) + outer8 = any8 & (~inner8) + + log_interval = 96 + if env.common_step_counter - _last_grip_print_step >= log_interval: + print( + f"[Grip] inner7={int(inner7.sum())} inner8={int(inner8.sum())} " + f"BOTH_inner={int((inner7 & inner8).sum())} | " + f"outer7={int(outer7.sum())} outer8={int(outer8.sum())} " + f"(of {env.num_envs} envs)", + flush=True, + ) + _last_grip_print_step = env.common_step_counter + return torch.zeros(env.num_envs, device=env.device) + + +def open_claw_approach_reward( + env: ManagerBasedRLEnv, + gripper_cfg: SceneEntityCfg, + open_target: float = 0.05, + aperture_sigma: float = 0.02, + proximity_radius: float = 0.12, +) -> torch.Tensor: + """Reward keeping the claw OPEN while approaching the handle. + + Replaces reliance on hard joint limits to keep the gripper open. Rewards the + gripper joints sitting near their open targets (joint7 ≈ -open_target, + joint8 ≈ +open_target), gated by proximity to the handle so it only matters + when the robot is actually going in for the grasp. + + A wide-open claw lets the handle bar pass BETWEEN the fingers instead of the + robot draping a closed/pinched hand over the top. + """ + result = _claw_distances(env) + if result is None: + return torch.zeros(env.num_envs, device=env.device) + d7, d8, _, _, _ = result + + # Proximity gate — fires as the average fingertip nears the handle + avg_dist = (d7 + d8) * 0.5 + prox_gate = torch.exp(-3.0 * avg_dist / proximity_radius) + + # Aperture score — Gaussian bell peaking when joints are at the open targets + gripper_pos = env.scene[gripper_cfg.name].data.joint_pos[:, gripper_cfg.joint_ids] # (N, 2) + j7, j8 = gripper_pos[:, 0], gripper_pos[:, 1] + err = (j7 - (-open_target)) ** 2 + (j8 - open_target) ** 2 + open_score = torch.exp(-err / (2.0 * aperture_sigma ** 2)) + + return prox_gate * open_score + + +def conditional_action_rate_l2(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg) -> torch.Tensor: + """Action rate penalty that scales down as the drawer opens.""" + action_rate = torch.sum(torch.square(env.action_manager.action - env.action_manager.prev_action), dim=1) + drawer_pos = env.scene[asset_cfg.name].data.joint_pos[:, asset_cfg.joint_ids[0]] + multiplier = torch.clamp(1.0 - (drawer_pos / 0.35), min=0.1, max=1.0) + return action_rate * multiplier + + +def conditional_joint_vel_l2(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg, robot_cfg: SceneEntityCfg) -> torch.Tensor: + """Joint velocity penalty that scales down as the drawer opens.""" + joint_vel = torch.sum(torch.square(env.scene[robot_cfg.name].data.joint_vel), dim=1) + drawer_pos = env.scene[asset_cfg.name].data.joint_pos[:, asset_cfg.joint_ids[0]] + multiplier = torch.clamp(1.0 - (drawer_pos / 0.35), min=0.1, max=1.0) + return joint_vel * multiplier + + +def print_stage_curriculum(env: ManagerBasedRLEnv, env_ids: torch.Tensor, term_name: str, weight: float, num_steps: int) -> float: + """A wrapper for modify_reward_weight that prints a message exactly when the stage transitions.""" + from isaaclab.envs.mdp import modify_reward_weight + + if env.common_step_counter >= num_steps and env.common_step_counter < num_steps + env.num_envs * 2: + if len(env_ids) > 0 and env_ids[0] == 0: + print(f"\n=======================================================") + print(f"CURRICULUM UNLOCKED: STAGE 2") + print(f"=======================================================") + print(f"The AI has mastered reaching! Now forcing it to pull...") + print(f"Updating reward term '{term_name}' to {weight}") + print(f"=======================================================\n") + + return modify_reward_weight(env, env_ids, term_name, weight, num_steps) + + +def dual_approach_bonus(env: ManagerBasedRLEnv, near_threshold: float = 0.06) -> torch.Tensor: + """Stepping-stone bonus: both fingers simultaneously within near_threshold of the handle. + + Fires as a discrete bonus when BOTH claws cross the threshold at the same time. + This bridges the gap between single_claw_proximity (OR logic, ~5cm) and + dual_claw_straddle (requires both close + opposite sides, ~2cm). + Without this, the agent has no gradient for the 5cm→2cm push with both fingers. + """ + result = _claw_distances(env) + if result is None: + return torch.zeros(env.num_envs, device=env.device) + d7, d8, _, _, _ = result + + # Soft AND: both must be within threshold — product of two Gaussian gates + k = 3.0 / near_threshold + gate7 = torch.exp(-k * d7) + gate8 = torch.exp(-k * d8) + return gate7 * gate8 # Only near 1.0 when BOTH fingers are close + + +def asymmetry_penalty(env: ManagerBasedRLEnv, contact_radius: float = 0.08, penalty_threshold: float = 0.05) -> torch.Tensor: + """Penalty for asymmetric finger distances — discourages single-finger solutions. + + When one finger is much closer to the handle than the other, apply a penalty. + This specifically targets the local optimum where link8 gets close (0.004m) + while link7 stays far (0.115m). + + Penalty = prox_gate * max(0, |d7 - d8| - penalty_threshold)^2 + """ + result = _claw_distances(env) + if result is None: + return torch.zeros(env.num_envs, device=env.device) + + d_link7, d_link8, _, _, _ = result + + # Only apply penalty when at least one finger is approaching + min_distance = torch.minimum(d_link7, d_link8) + prox_gate = torch.exp(-3.0 * min_distance / contact_radius) + + # Penalty increases quadratically with distance asymmetry + distance_diff = torch.abs(d_link7 - d_link8) + asymmetry_excess = torch.clamp(distance_diff - penalty_threshold, min=0.0) + + return prox_gate * (asymmetry_excess ** 2) + + +# (debug_link_distances removed — replaced by debug_inner_edge, which prints inner vs +# outer edge contact counts per iteration.) \ No newline at end of file diff --git a/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/rsl_rl_scripts/diagnose_policy_rollout.py b/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/rsl_rl_scripts/diagnose_policy_rollout.py index 3d33b58b..0327195e 100644 --- a/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/rsl_rl_scripts/diagnose_policy_rollout.py +++ b/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/rsl_rl_scripts/diagnose_policy_rollout.py @@ -31,7 +31,15 @@ import torch from packaging import version -from rsl_rl.runners import DistillationRunner, OnPolicyRunner +from rsl_rl.runners import OnPolicyRunner + +try: + # DistillationRunner was added in a later rsl-rl-lib release. Older versions + # (e.g. the one pinned in some Isaac Sim containers) don't export it, so import + # it optionally and only fail if a task actually requests class_name="DistillationRunner". + from rsl_rl.runners import DistillationRunner +except ImportError: + DistillationRunner = None from isaaclab.envs import DirectMARLEnv, multi_agent_to_single_agent from isaaclab.utils.assets import retrieve_file_path @@ -115,12 +123,23 @@ def main() -> None: base_env = env.unwrapped robot = base_env.scene["robot"] - if agent_cfg.class_name == "OnPolicyRunner": + # class_name was added to RslRlOnPolicyRunnerCfg in a later isaaclab_rl release + # (to select between OnPolicyRunner/DistillationRunner). Older installs don't + # have this field at all, so default to "OnPolicyRunner" — the only runner + # trained by train.py in this repo. + runner_class_name = getattr(agent_cfg, "class_name", "OnPolicyRunner") + if runner_class_name == "OnPolicyRunner": runner = OnPolicyRunner(env, agent_cfg.to_dict(), log_dir=None, device=agent_cfg.device) - elif agent_cfg.class_name == "DistillationRunner": + elif runner_class_name == "DistillationRunner": + if DistillationRunner is None: + raise ImportError( + "agent_cfg.class_name is 'DistillationRunner' but the installed rsl-rl-lib " + f"version ({installed_rsl_rl_version}) does not provide it. Upgrade rsl-rl-lib " + "or switch this task's runner config to OnPolicyRunner." + ) runner = DistillationRunner(env, agent_cfg.to_dict(), log_dir=None, device=agent_cfg.device) else: - raise ValueError(f"Unsupported runner class: {agent_cfg.class_name}") + raise ValueError(f"Unsupported runner class: {runner_class_name}") runner.load(resume_path) policy = runner.get_inference_policy(device=base_env.device) @@ -168,7 +187,11 @@ def main() -> None: pair_width_stats = {} foot_rel_vel_x_abs_sum = None + # Some isaaclab_rl versions return just the obs tensor from get_observations(); + # others return a (obs, extras) tuple (gymnasium reset() convention). Handle both. obs = env.get_observations() + if isinstance(obs, tuple): + obs = obs[0] for step_idx in range(args_cli.steps): with torch.inference_mode(): actions = policy(obs) diff --git a/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/rsl_rl_scripts/play.py b/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/rsl_rl_scripts/play.py index a75dbdfa..8d94c240 100644 --- a/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/rsl_rl_scripts/play.py +++ b/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/rsl_rl_scripts/play.py @@ -54,7 +54,15 @@ import torch from packaging import version -from rsl_rl.runners import DistillationRunner, OnPolicyRunner +from rsl_rl.runners import OnPolicyRunner + +try: + # DistillationRunner was added in a later rsl-rl-lib release. Older versions + # (e.g. the one pinned in some Isaac Sim containers) don't export it, so import + # it optionally and only fail if a task actually requests class_name="DistillationRunner". + from rsl_rl.runners import DistillationRunner +except ImportError: + DistillationRunner = None from isaaclab.envs import DirectMARLEnv, multi_agent_to_single_agent from isaaclab.utils.assets import retrieve_file_path @@ -66,7 +74,12 @@ export_policy_as_jit, export_policy_as_onnx, ) -from isaaclab_rl.utils.pretrained_checkpoint import get_published_pretrained_checkpoint +try: + # Not present in all isaaclab_rl versions/installs; only needed for + # --use_pretrained_checkpoint (downloading a checkpoint from Nucleus). + from isaaclab_rl.utils.pretrained_checkpoint import get_published_pretrained_checkpoint +except ImportError: + get_published_pretrained_checkpoint = None import HumanoidRLPackage.HumanoidRLSetup.tasks # noqa: F401 from isaaclab_tasks.utils import get_checkpoint_path, parse_env_cfg @@ -90,6 +103,13 @@ def main(): log_root_path = os.path.abspath(log_root_path) print(f"[INFO] Loading experiment from directory: {log_root_path}") if args_cli.use_pretrained_checkpoint: + if get_published_pretrained_checkpoint is None: + raise ImportError( + "--use_pretrained_checkpoint was passed but " + "isaaclab_rl.utils.pretrained_checkpoint is not available in this " + "isaaclab_rl install. Omit --use_pretrained_checkpoint to load a " + "local checkpoint instead." + ) resume_path = get_published_pretrained_checkpoint("rsl_rl", train_task_name) if not resume_path: print("[INFO] Unfortunately a pre-trained checkpoint is currently unavailable for this task.") @@ -127,12 +147,23 @@ def main(): print(f"[INFO]: Loading model checkpoint from: {resume_path}") # load previously trained model - if agent_cfg.class_name == "OnPolicyRunner": + # class_name was added to RslRlOnPolicyRunnerCfg in a later isaaclab_rl release + # (to select between OnPolicyRunner/DistillationRunner). Older installs don't + # have this field at all, so default to "OnPolicyRunner" — the only runner + # trained by train.py in this repo. + runner_class_name = getattr(agent_cfg, "class_name", "OnPolicyRunner") + if runner_class_name == "OnPolicyRunner": runner = OnPolicyRunner(env, agent_cfg.to_dict(), log_dir=None, device=agent_cfg.device) - elif agent_cfg.class_name == "DistillationRunner": + elif runner_class_name == "DistillationRunner": + if DistillationRunner is None: + raise ImportError( + "agent_cfg.class_name is 'DistillationRunner' but the installed rsl-rl-lib " + f"version ({installed_rsl_rl_version}) does not provide it. Upgrade rsl-rl-lib " + "or switch this task's runner config to OnPolicyRunner." + ) runner = DistillationRunner(env, agent_cfg.to_dict(), log_dir=None, device=agent_cfg.device) else: - raise ValueError(f"Unsupported runner class: {agent_cfg.class_name}") + raise ValueError(f"Unsupported runner class: {runner_class_name}") runner.load(resume_path) # obtain the trained policy for inference @@ -158,7 +189,11 @@ def main(): dt = env.unwrapped.step_dt # reset environment + # Some isaaclab_rl versions return just the obs tensor from get_observations(); + # others return a (obs, extras) tuple (gymnasium reset() convention). Handle both. obs = env.get_observations() + if isinstance(obs, tuple): + obs = obs[0] timestep = 0 # simulate environment while simulation_app.is_running(): diff --git a/autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/config/joint_names_armDouble.SLDASM.yaml b/autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/config/joint_names_armDouble.SLDASM.yaml new file mode 100644 index 00000000..0fce7f20 --- /dev/null +++ b/autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/config/joint_names_armDouble.SLDASM.yaml @@ -0,0 +1 @@ +controller_joint_names: ['', 'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7', 'joint8', 'joint1L', 'joint2l', 'joint3l', 'joint4l', 'joint5l', 'joint6l', 'joint7l', 'joint8l', ] diff --git a/autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/urdf/armDouble.SLDASM.csv b/autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/urdf/armDouble.SLDASM.csv new file mode 100644 index 00000000..4dfd150c --- /dev/null +++ b/autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/urdf/armDouble.SLDASM.csv @@ -0,0 +1,18 @@ +Link Name,Center of Mass X,Center of Mass Y,Center of Mass Z,Center of Mass Roll,Center of Mass Pitch,Center of Mass Yaw,Mass,Moment Ixx,Moment Ixy,Moment Ixz,Moment Iyy,Moment Iyz,Moment Izz,Visual X,Visual Y,Visual Z,Visual Roll,Visual Pitch,Visual Yaw,Mesh Filename,Color Red,Color Green,Color Blue,Color Alpha,Collision X,Collision Y,Collision Z,Collision Roll,Collision Pitch,Collision Yaw,Collision Mesh Filename,Material Name,SW Components,Coordinate System,Axis Name,Joint Name,Joint Type,Joint Origin X,Joint Origin Y,Joint Origin Z,Joint Origin Roll,Joint Origin Pitch,Joint Origin Yaw,Parent,Joint Axis X,Joint Axis Y,Joint Axis Z,Limit Effort,Limit Velocity,Limit Lower,Limit Upper,Calibration rising,Calibration falling,Dynamics Damping,Dynamics Friction,Safety Soft Upper,Safety Soft Lower,Safety K Position,Safety K Velocity +base_link,-0.0393699098618405,0.00029338017895416,0.246714430384919,0,0,0,7.72080270470275,0.0927735748472098,1.72190184687839E-09,-0.00256316384233891,0.0438526593789028,7.29494949417166E-09,0.0733982205361372,0,0,0,0,0,0,package://armDouble.SLDASM/meshes/base_link.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://armDouble.SLDASM/meshes/base_link.STL,,baseLink-1,globalCoords,,,,0,0,0,0,0,0,,0,0,0,,,,,,,,,,,, +link1,-0.00648795787235046,0.0599988979727498,-0.000882924223042036,0,0,0,0.715719482519865,0.00134468828759061,0.000142459036926116,1.68789460042738E-05,0.00092992094802874,-6.11976161352999E-06,0.00084923811617113,0,0,0,0,0,0,package://armDouble.SLDASM/meshes/link1.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://armDouble.SLDASM/meshes/link1.STL,,link1-2,link1Coords,joint1Axis,joint1,revolute,0,0.16355,0.5172,0,0,0,base_link,0,-1,0,0,0,0,0,,,,,,,, +link2,8.25653640154633E-05,0.0610024235098449,-0.000118788152262406,0,0,0,0.393289380591223,0.00186278175847507,5.68529001036362E-05,-3.35596070412285E-06,0.000565515410487103,7.65008310205558E-06,0.00158017038765737,0,0,0,0,0,0,package://armDouble.SLDASM/meshes/link2.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://armDouble.SLDASM/meshes/link2.STL,,link2-2,link2Coords,joint2Axis,joint2,revolute,0.0315,0.084,0,0,0,0,link1,1,0,0,0,0,0,0,,,,,,,, +link3,-0.000312733131787979,0.0674576772753304,-0.00923774935502242,0,0,0,0.18680148468641,0.00038742015948435,-1.14856421400225E-05,-1.20447943757034E-06,0.000201366367368852,2.34610012720774E-05,0.000353464970682036,0,0,0,0,0,0,package://armDouble.SLDASM/meshes/link3.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://armDouble.SLDASM/meshes/link3.STL,,link3-2,link3Coords,joint3Axis,joint3,revolute,-0.017,0.171,0,0,0,0,link2,0,-1,0,0,0,0,0,,,,,,,, +link4,-0.0147100175958424,0.0552519220910238,-0.000438676492228174,0,0,0,0.171198731790723,0.000159971058394579,2.85342883021733E-05,-4.41162886967172E-06,0.000199888569467379,1.56879644128418E-06,0.000173071251570806,0,0,0,0,0,0,package://armDouble.SLDASM/meshes/link4.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://armDouble.SLDASM/meshes/link4.STL,,link4-2,link4Coords,joint4Axis,joint4,revolute,0.02,0.12,0,0,0,0,link3,-1,0,0,0,0,0,0,,,,,,,, +link5,0.0101064742185897,0.0501218669985956,-1.58020471687781E-05,0,0,0,0.149874419924705,0.000350578321442484,-5.78554405242181E-05,1.62694798799168E-08,0.000111057036297084,2.37323142178646E-10,0.000317256455304642,0,0,0,0,0,0,package://armDouble.SLDASM/meshes/link5.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://armDouble.SLDASM/meshes/link5.STL,,link5-2,link5Coords,joint5Axis,joint5,revolute,-0.0263,0.1055,0,0,0,0,link4,0,-1,0,0,0,0,0,,,,,,,, +link6,0.030004294635101,0.05544345003385,-7.85291217074713E-05,0,0,0,0.108391369555053,2.83157324789146E-05,4.28967707823919E-07,-2.5805118659042E-06,7.00884254274417E-05,4.16303393714129E-09,6.28400614742533E-05,0,0,0,0,0,0,package://armDouble.SLDASM/meshes/link6.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://armDouble.SLDASM/meshes/link6.STL,,link6-2,link6Coords,joint6Axis,joint6,revolute,-0.031,0.112,0,0,0,0,link5,1,0,0,0,0,0,0,,,,,,,, +link7,-0.013235531458122,-0.0229688384643895,-0.00641129891838277,0,0,0,0.0372930675243502,1.98339939365138E-05,2.61463007883355E-06,6.69248604942636E-09,9.05266779211E-06,-4.79077371537686E-07,1.51529276255668E-05,0,0,0,0,0,0,package://armDouble.SLDASM/meshes/link7.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://armDouble.SLDASM/meshes/link7.STL,,link7-2,link7Coords,joint7_8Axis,joint7,prismatic,0.085558,0.10361,0.004349,0,0,3.1416,link6,-1,0,0,0,0,0,0,,,,,,,, +link8,0.0132355314603446,-0.0229688384648348,-0.00228870108114088,0,0,0,0.037293067525095,1.98339939367975E-05,-2.61463007874125E-06,6.69248519891047E-09,9.0526677922033E-06,4.79077372182631E-07,1.51529276257507E-05,0,0,0,0,0,0,package://armDouble.SLDASM/meshes/link8.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://armDouble.SLDASM/meshes/link8.STL,,link8-1,link8Coords,joint7_8Axis,joint8,prismatic,-0.016558,0.10361,0.004349,0,0,3.1416,link6,-1,0,0,0,0,0,0,,,,,,,, +link1L,-0.00648795077346556,-0.0599988624740439,-0.000882917001239103,0,0,0,0.715718711523841,0.00134468450604772,-0.000142458935582621,1.68787907445742E-05,0.000929920629691007,6.12004958205212E-06,0.000849234420590723,0,0,0,0,0,0,package://armDouble.SLDASM/meshes/link1L.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://armDouble.SLDASM/meshes/link1L.STL,,Mirrorlink1-4,link1L,Axis1,joint1L,revolute,0,-0.16355,0.5172,0,0,0,base_link,0,1,0,0,0,0,0,,,,,,,, +link2l,8.29943297666613E-05,-0.061002585097665,-0.000119871032069119,0,0,0,0.393301841759098,0.00186280275161714,-5.68504461785252E-05,-3.34910038364677E-06,0.000565528641792776,-7.66061942969892E-06,0.001580181793701,0,0,0,0,0,0,package://armDouble.SLDASM/meshes/link2l.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://armDouble.SLDASM/meshes/link2l.STL,,Mirrorlink2-4,link2L,Axis2,joint2l,revolute,0.0315,-0.084,0,0,0,0,link1L,-1,0,0,0,0,0,0,,,,,,,, +link3l,-0.000310436147900828,-0.0674576465286665,-0.00923772776547804,0,0,0,0.186801161897531,0.000387418957820586,1.15116718289572E-05,-1.19792220831277E-06,0.000201365186246964,-2.3460921987413E-05,0.000353463850903654,0,0,0,0,0,0,package://armDouble.SLDASM/meshes/link3l.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://armDouble.SLDASM/meshes/link3l.STL,,Mirrorlink3-4,link3L,Axis3,joint3l,revolute,-0.017,-0.171,0,0,0,0,link2l,0,1,0,0,0,0,0,,,,,,,, +link4l,-0.0147100322439385,-0.0552519615606349,-0.000438485566351399,0,0,0,0.171198562353126,0.000159970911212196,-2.85342137820626E-05,-4.4129325970901E-06,0.000199888757533217,-1.56860037980322E-06,0.000173071281299426,0,0,0,0,0,0,package://armDouble.SLDASM/meshes/link4l.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://armDouble.SLDASM/meshes/link4l.STL,,Mirrorlink4-4,link4L,Axis4,joint4l,revolute,0.02,-0.12,0,0,0,0,link3l,-1,0,0,0,0,0,0,,,,,,,, +link5l,0.0101064733615455,-0.050121865274715,-1.58512277795841E-05,0,0,0,0.14987441741057,0.000350578305563921,5.78554443851511E-05,1.68834227820353E-08,0.000111057041333189,-2.57741312202463E-11,0.000317256450472475,0,0,0,0,0,0,package://armDouble.SLDASM/meshes/link5l.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://armDouble.SLDASM/meshes/link5l.STL,,Mirrorlink5-4,link5L,Axis5,joint5l,revolute,-0.0263,-0.1055,0,0,0,0,link4l,0,1,0,0,0,0,0,,,,,,,, +link6l,0.0300042990781323,-0.0554434240177758,-7.85286280942099E-05,0,0,0,0.108391490900061,2.83157430629336E-05,-4.28967494688143E-07,-2.58051654768112E-06,7.00884223072284E-05,-4.15923690642316E-09,6.2840066426229E-05,0,0,0,0,0,0,package://armDouble.SLDASM/meshes/link6l.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://armDouble.SLDASM/meshes/link6l.STL,,Mirrorlink6-4,link6L,Axis6,joint6l,revolute,-0.031,-0.112,0,0,0,0,link5l,1,0,0,0,0,0,0,,,,,,,, +link7l,0.115351328901149,-0.022969245102013,-0.00641129179837308,0,0,0,0.0372932549660656,1.98346130416805E-05,-2.61481896083632E-06,-6.69769931650367E-09,9.05271732114762E-06,-4.79102693142355E-07,1.51535820699155E-05,0,0,0,0,0,0,package://armDouble.SLDASM/meshes/link7l.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://armDouble.SLDASM/meshes/link7l.STL,,Mirrorlink7-4,link7L,Axis7,joint7l,prismatic,-0.016558,-0.10361,0.004349,0,0,0,link6l,1,0,0,0,0,0,0,,,,,,,, +link8l,-0.115351328676707,-0.0229692455511172,-0.00228870743972476,0,0,0,0.0372932577290872,1.98346146821473E-05,2.61481907321854E-06,-6.69768756447685E-09,9.05271830636071E-06,4.79102082124769E-07,1.51535828052253E-05,0,0,0,0,0,0,package://armDouble.SLDASM/meshes/link8l.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://armDouble.SLDASM/meshes/link8l.STL,,Mirrorlink8-3,link8L,Axis7,joint8l,prismatic,0.085558,-0.10361,0.004349,0,0,0,link6l,1,0,0,0,0,0,0,,,,,,,, diff --git a/autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/urdf/armDouble.SLDASM.urdf b/autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/urdf/armDouble.SLDASM.urdf new file mode 100644 index 00000000..4bf1c79d --- /dev/null +++ b/autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/urdf/armDouble.SLDASM.urdf @@ -0,0 +1,911 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/urdf/armDouble.SLDASM/configuration/armDouble.SLDASM_base.usd b/autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/urdf/armDouble.SLDASM/configuration/armDouble.SLDASM_base.usd new file mode 100644 index 00000000..5dc5706b Binary files /dev/null and b/autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/urdf/armDouble.SLDASM/configuration/armDouble.SLDASM_base.usd differ diff --git a/autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/urdf/armDouble.SLDASM/configuration/armDouble.SLDASM_physics.usd b/autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/urdf/armDouble.SLDASM/configuration/armDouble.SLDASM_physics.usd new file mode 100644 index 00000000..8fa52b2d Binary files /dev/null and b/autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/urdf/armDouble.SLDASM/configuration/armDouble.SLDASM_physics.usd differ diff --git a/autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/urdf/armDouble.SLDASM/configuration/armDouble.SLDASM_sensor.usd b/autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/urdf/armDouble.SLDASM/configuration/armDouble.SLDASM_sensor.usd new file mode 100644 index 00000000..5c806af3 Binary files /dev/null and b/autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/urdf/armDouble.SLDASM/configuration/armDouble.SLDASM_sensor.usd differ