From 374d2886dafa4b84a13e807425880eb70aeebabe Mon Sep 17 00:00:00 2001 From: Ramy Wahib Date: Mon, 23 Mar 2026 22:41:55 -0400 Subject: [PATCH 1/2] your message --- .../modelCfg/humanoid_arm_only.py | 134 ++++++++++++++++++ .../robot_arm_controllers/task_space_test.py | 107 ++++++-------- 2 files changed, 176 insertions(+), 65 deletions(-) create mode 100644 autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/modelCfg/humanoid_arm_only.py diff --git a/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/modelCfg/humanoid_arm_only.py b/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/modelCfg/humanoid_arm_only.py new file mode 100644 index 0000000..7334b95 --- /dev/null +++ b/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/modelCfg/humanoid_arm_only.py @@ -0,0 +1,134 @@ +import os + +import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets.articulation import ArticulationCfg + +_HUMANOID_WATO_ROOT = os.path.abspath( + os.path.join(os.path.dirname(__file__), "..", "..", "..", "..") +) +_MODEL_ASSETS = os.path.join(_HUMANOID_WATO_ROOT, "ModelAssets") + +HAND_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=os.path.join(_MODEL_ASSETS, "hand.usd"), + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + max_depenetration_velocity=5.0, + ), + activate_contact_sensors=False, + ), + init_state = ArticulationCfg.InitialStateCfg( + joint_pos={ + "Revolute_1": 0.0, + "Revolute_2": 0.0, + "Revolute_3": 0.0, + "Revolute_4": 0.0, + "Revolute_5": 0.0, + "Revolute_6": 0.0, + "Revolute_7": 0.0, + "Revolute_8": 0.0, + "Revolute_9": 0.0, + "Revolute_10": 0.0, + "Revolute_11": 0.0, + "Revolute_12": 0.0, + "Revolute_13": 0.0, + "Revolute_14": 0.0, + "Revolute_15": 0.0, + } + ), + actuators={ + "arm": ImplicitActuatorCfg( + joint_names_expr=[".*"], + stiffness=0.5, + damping=0.5, + velocity_limit_sim=3.0, + ), + }, +) + +# Hand Arm +_ARM_USD_PATH = os.path.join(_MODEL_ASSETS, "arm.usd") +ARM_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=_ARM_USD_PATH, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + max_depenetration_velocity=5.0, + ), + activate_contact_sensors=False, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=True + ), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + "shoulder_flexion_extension": 0.0, + "shoulder_abduction_adduction": 0.0, + "shoulder_rotation": 0.0, + "elbow_flexion_extension": 0.0, + "forearm_rotation": 0.0, + "wrist_extension": 0.0, + "mcp_index": 0.0, + "pip_index": 0.0, + "dip_index": 0.0, + "mcp_middle": 0.0, + "pip_middle": 0.0, + "dip_middle": 0.0, + "mcp_ring": 0.0, + "pip_ring": 0.0, + "dip_ring": 0.0, + "mcp_pinky": 0.0, + "pip_pinky": 0.0, + "dip_pinky": 0.0, + "cmc_thumb": 0.0, + "mcp_thumb": 0.79, # within limits [0.785, 2.531] + "ip_thumb": 0.0, + } + ), + actuators={ + # J≈1.20 kg·m², f=4.0 Hz, ζ=1.0 → k=757.6, d=60.3 + "shoulder_fe": ImplicitActuatorCfg( + joint_names_expr=["shoulder_flexion_extension"], + stiffness=757.6, + damping=60.3, + velocity_limit_sim=3.0, + ), + # J≈0.95 kg·m², f=4.0 Hz, ζ=1.0 → k=600.0, d=47.7 + "shoulder_aa": ImplicitActuatorCfg( + joint_names_expr=["shoulder_abduction_adduction"], + stiffness=600.0, + damping=47.7, + velocity_limit_sim=3.0, + ), + # J≈0.77 kg·m², f=4.5 Hz, ζ=1.0 → k=615.5, d=43.5 + "shoulder_rot": ImplicitActuatorCfg( + joint_names_expr=["shoulder_rotation"], + stiffness=615.5, + damping=43.5, + velocity_limit_sim=3.0, + ), + # J≈0.77 kg·m², f=4.5 Hz, ζ=1.0 → k=615.5, d=43.5 + "elbow": ImplicitActuatorCfg( + joint_names_expr=["elbow_flexion_extension"], + stiffness=615.5, + damping=43.5, + velocity_limit_sim=3.0, + ), + # J≈0.45 kg·m², f=5.0 Hz, ζ=1.0 → k=444.1, d=28.3 + "forearm": ImplicitActuatorCfg( + joint_names_expr=["forearm_rotation"], + stiffness=444.1, + damping=28.3, + velocity_limit_sim=3.0, + ), + # J≈0.12 kg·m², f=6.0 Hz, ζ=1.0 → k=170.5, d=9.0 + "wrist": ImplicitActuatorCfg( + joint_names_expr=["wrist_extension"], + stiffness=170.5, + damping=9.0, + velocity_limit_sim=3.0, + ), + + }, +) \ No newline at end of file diff --git a/autonomy/simulation/Wato_additional_scripts/robot_arm_controllers/task_space_test.py b/autonomy/simulation/Wato_additional_scripts/robot_arm_controllers/task_space_test.py index 29bb172..68fe23b 100644 --- a/autonomy/simulation/Wato_additional_scripts/robot_arm_controllers/task_space_test.py +++ b/autonomy/simulation/Wato_additional_scripts/robot_arm_controllers/task_space_test.py @@ -1,4 +1,19 @@ -from isaaclab_assets import UR10_CFG, FRANKA_PANDA_HIGH_PD_CFG +import torch +import argparse +import sys +import os + +from isaaclab.app import AppLauncher + +parser = argparse.ArgumentParser(description="Robot Arm with Task Space IK Control") +parser.add_argument("--robot", type=str, default="ur10", help="Name of the robot.") +AppLauncher.add_app_launcher_args(parser) +args_cli = parser.parse_args() + +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +# ALL imports go AFTER this line from isaaclab.utils.math import subtract_frame_transforms from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR from isaaclab.utils import configclass @@ -9,23 +24,10 @@ from isaaclab.controllers import DifferentialIKController, DifferentialIKControllerCfg from isaaclab.assets import AssetBaseCfg import isaaclab.sim as sim_utils -import torch -import argparse -from isaaclab.app import AppLauncher - -parser = argparse.ArgumentParser( - description="Robot Arm with Task Space IK Control") -parser.add_argument( - "--robot", - type=str, - default="franka_panda", - help="Name of the robot.") -AppLauncher.add_app_launcher_args(parser) -args_cli = parser.parse_args() - -app_launcher = AppLauncher(args_cli) -simulation_app = app_launcher.app +sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), + "../../Humanoid_Wato/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/modelCfg"))) +from humanoid_arm_only import ARM_CFG ## # Pre-defined configs @@ -54,15 +56,15 @@ class TableTopSceneCfg(InteractiveSceneCfg): 0.75))) # mount - table = AssetBaseCfg( - prim_path="{ENV_REGEX_NS}/Table", - spawn=sim_utils.UsdFileCfg( - usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/Stand/stand_instanceable.usd", - scale=( - 2.0, - 2.0, - 2.0)), - ) + # table = AssetBaseCfg( + # prim_path="{ENV_REGEX_NS}/Table", + # spawn=sim_utils.UsdFileCfg( + # usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/Stand/stand_instanceable.usd", + # scale=( + # 2.0, + # 2.0, + # 2.0)), + # ) # Cube cube = AssetBaseCfg( @@ -70,20 +72,14 @@ class TableTopSceneCfg(InteractiveSceneCfg): spawn=sim_utils.CuboidCfg( size=[0.1, 0.1, 0.1] ), - init_state=AssetBaseCfg.InitialStateCfg(pos=(0.5, 0.0, 0.5)), + init_state=AssetBaseCfg.InitialStateCfg(pos=(-0.3, 0.0, 0.3)), ) # articulation - if args_cli.robot == "franka_panda": - robot = FRANKA_PANDA_HIGH_PD_CFG.replace( - prim_path="{ENV_REGEX_NS}/Robot") - elif args_cli.robot == "ur10": - robot = UR10_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") - else: - raise ValueError( - f"Robot {args_cli.robot} is not supported. Valid: franka_panda, ur10") - # robot = UR10_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") - + robot = ARM_CFG.replace( + prim_path="{ENV_REGEX_NS}/Robot", + + ) def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): @@ -106,18 +102,12 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): prim_path="/Visuals/ee_goal")) # Specify robot-specific parameters - if args_cli.robot == "franka_panda": - robot_entity_cfg = SceneEntityCfg( - "robot", - joint_names=["panda_joint.*"], - body_names=["panda_hand"]) - elif args_cli.robot == "ur10": - robot_entity_cfg = SceneEntityCfg( - "robot", joint_names=[".*"], body_names=["ee_link"]) - else: - raise ValueError( - f"Robot {args_cli.robot} is not supported. Valid: franka_panda, ur10") - # robot_entity_cfg = SceneEntityCfg("robot", joint_names=[".*"], body_names=["ee_link"]) + robot_entity_cfg = SceneEntityCfg( + "robot", + joint_names=["shoulder_flexion_extension", "shoulder_abduction_adduction", + "shoulder_rotation", "elbow_flexion_extension", + "forearm_rotation", "wrist_extension"], + body_names=["PALM_GAVIN_1DoF_Hinge_v2_1"]) # Resolving the scene entities robot_entity_cfg.resolve(scene) @@ -134,22 +124,9 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): sim_dt = sim.get_physics_dt() # May have to set initial position first - if args_cli.robot == "franka_panda": - joint_position = robot.data.default_joint_pos.clone() - joint_vel = robot.data.default_joint_vel.clone() - robot.write_joint_state_to_sim(joint_position, joint_vel) - else: - joint_position_index = 0 - joint_position_list = [ - [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], - ] - joint_position = torch.tensor( - joint_position_list[joint_position_index], - device=sim.device) - joint_vel = robot.data.default_joint_vel.clone() - joint_pos_des = joint_position.unsqueeze( - 0)[:, robot_entity_cfg.joint_ids].clone() - robot.write_joint_state_to_sim(joint_pos_des, joint_vel) + joint_position = robot.data.default_joint_pos.clone() + joint_vel = robot.data.default_joint_vel.clone() + robot.write_joint_state_to_sim(joint_position, joint_vel) while simulation_app.is_running(): # Get cube/target_point coordinates From 3c1096e835c7710b05b9a7d913f6d4fd4b4ae5bf Mon Sep 17 00:00:00 2001 From: Ramy Wahib Date: Tue, 24 Mar 2026 18:26:12 -0400 Subject: [PATCH 2/2] new changes --- .../modelCfg/humanoid_arm_only.py | 40 +------------------ .../robot_arm_controllers/task_space_test.py | 13 +----- 2 files changed, 4 insertions(+), 49 deletions(-) diff --git a/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/modelCfg/humanoid_arm_only.py b/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/modelCfg/humanoid_arm_only.py index 7334b95..ba13f6f 100644 --- a/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/modelCfg/humanoid_arm_only.py +++ b/autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/modelCfg/humanoid_arm_only.py @@ -1,3 +1,5 @@ +"""This cfg is only used by the task_space_test.py script and +the humanoid arm is only exposing the 6 DOF Arm for control""" import os import isaaclab.sim as sim_utils @@ -9,44 +11,6 @@ ) _MODEL_ASSETS = os.path.join(_HUMANOID_WATO_ROOT, "ModelAssets") -HAND_CFG = ArticulationCfg( - spawn=sim_utils.UsdFileCfg( - usd_path=os.path.join(_MODEL_ASSETS, "hand.usd"), - rigid_props=sim_utils.RigidBodyPropertiesCfg( - disable_gravity=False, - max_depenetration_velocity=5.0, - ), - activate_contact_sensors=False, - ), - init_state = ArticulationCfg.InitialStateCfg( - joint_pos={ - "Revolute_1": 0.0, - "Revolute_2": 0.0, - "Revolute_3": 0.0, - "Revolute_4": 0.0, - "Revolute_5": 0.0, - "Revolute_6": 0.0, - "Revolute_7": 0.0, - "Revolute_8": 0.0, - "Revolute_9": 0.0, - "Revolute_10": 0.0, - "Revolute_11": 0.0, - "Revolute_12": 0.0, - "Revolute_13": 0.0, - "Revolute_14": 0.0, - "Revolute_15": 0.0, - } - ), - actuators={ - "arm": ImplicitActuatorCfg( - joint_names_expr=[".*"], - stiffness=0.5, - damping=0.5, - velocity_limit_sim=3.0, - ), - }, -) - # Hand Arm _ARM_USD_PATH = os.path.join(_MODEL_ASSETS, "arm.usd") ARM_CFG = ArticulationCfg( diff --git a/autonomy/simulation/Wato_additional_scripts/robot_arm_controllers/task_space_test.py b/autonomy/simulation/Wato_additional_scripts/robot_arm_controllers/task_space_test.py index 68fe23b..4a91472 100644 --- a/autonomy/simulation/Wato_additional_scripts/robot_arm_controllers/task_space_test.py +++ b/autonomy/simulation/Wato_additional_scripts/robot_arm_controllers/task_space_test.py @@ -55,17 +55,6 @@ class TableTopSceneCfg(InteractiveSceneCfg): 0.75, 0.75))) - # mount - # table = AssetBaseCfg( - # prim_path="{ENV_REGEX_NS}/Table", - # spawn=sim_utils.UsdFileCfg( - # usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/Stand/stand_instanceable.usd", - # scale=( - # 2.0, - # 2.0, - # 2.0)), - # ) - # Cube cube = AssetBaseCfg( prim_path="/World/cube", @@ -73,6 +62,8 @@ class TableTopSceneCfg(InteractiveSceneCfg): size=[0.1, 0.1, 0.1] ), init_state=AssetBaseCfg.InitialStateCfg(pos=(-0.3, 0.0, 0.3)), + # Making this the initial state is important because at certain initial states, + # mainly on the positive x-axis, the IK breaks and the simulation bugs out a lot ) # articulation