Merge pull request #41 from WATonomous/ramy

Integrating IK with our model instead of  placeholder model

Author: wilsonchenghy

autonomy/simulation/Humanoid_Wato/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/modelCfg/humanoid_arm_only.py

@@ -0,0 +1,98 @@
+"""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
+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 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,
+        ),
+        
+    },
+)

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
@@ -53,37 +55,22 @@ 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",
         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)),
+        # 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
-    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 +93,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 +115,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