Skip to content

Integrating IK with our model instead of placeholder model #41

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
wilsonchenghy merged 2 commits into from
Mar 24, 2026
Merged

Integrating IK with our model instead of placeholder model #41

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
374d288
your message
RamyWahib Mar 24, 2026
3c1096e
new changes
RamyWahib Mar 24, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
98 changes: 98 additions & 0 deletions .../Humanoid_Wato/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/modelCfg/humanoid_arm_only.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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,
),

},
)
102 changes: 35 additions & 67 deletions autonomy/simulation/Wato_additional_scripts/robot_arm_controllers/task_space_test.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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
Expand All @@ -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
Expand Down Expand Up @@ -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):

Expand All @@ -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)
Expand All @@ -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
Expand Down
Loading