From eb2cbf00317f9968dfd6a77376ca03e5ce5eb7bf Mon Sep 17 00:00:00 2001 From: wilsonchenghy Date: Tue, 24 Mar 2026 21:01:40 -0400 Subject: [PATCH] polish code --- .../arm_assembly/fingertip_ik.py | 154 +++--------------- .../Humanoid_Wato/fingertip_ik_isaac_sim.py | 88 +++------- .../robot_arm_controllers/task_space_test.py | 66 +++----- 3 files changed, 59 insertions(+), 249 deletions(-) diff --git a/autonomy/simulation/Humanoid_Wato/arm_assembly/fingertip_ik.py b/autonomy/simulation/Humanoid_Wato/arm_assembly/fingertip_ik.py index 072ee6e..4b672f0 100644 --- a/autonomy/simulation/Humanoid_Wato/arm_assembly/fingertip_ik.py +++ b/autonomy/simulation/Humanoid_Wato/arm_assembly/fingertip_ik.py @@ -1,24 +1,21 @@ """ Standalone gradient-based IK for arm_assembly (6-DOF arm + 15-DOF hand). Solves for joint angles so that 5 fingertips reach desired world positions -using damped least-squares (no dependencies on other scripts in this repo). +using damped least-squares Joint 0 (shoulder_flexion_extension) issue: - MuJoCo's URDF importer can set continuous joints to range [0,0], which would - lock joint 0 at zero. We bypass that in clip_qpos_to_limits(): when - lower == upper we skip clipping so the IK can move that joint. See - diagnose_jacobian_joint0() and print_joint_limits_urdf() to inspect. + Here we found that MuJoCo's URDF importer can set continuous joints to range [0,0], which would + lock joint 0 at zero. We bypass that in clip_to_joint_limits(): when + lower == upper we skip clipping so the IK can move that joint. """ import os import mujoco import numpy as np from pathlib import Path -# Path to URDF (script next to arm_assembly folder or repo root) _SCRIPT_DIR = Path(__file__).resolve().parent URDF_PATH = _SCRIPT_DIR / "arm_assembly.urdf" -# Fingertip body names (distal link of each finger in arm_assembly.urdf) FINGERTIP_BODIES = [ "IP_THUMB_v1_1", # thumb "DIP_INDEX_v1_1", # index @@ -27,19 +24,16 @@ "DIP_PINKY_v1_1", # pinky ] -# First N DOFs are arm (shoulder, elbow, wrist); rest are hand. Used for weighted IK. -NUM_ARM_DOFS = 6 - def load_model(urdf_path=None): path = Path(urdf_path or URDF_PATH).resolve() if not path.exists(): raise FileNotFoundError(f"URDF not found: {path}") + mesh_dir = path.parent / "meshes" if not mesh_dir.is_dir(): raise FileNotFoundError(f"Mesh directory not found: {mesh_dir}") - # MuJoCo's URDF importer keeps only mesh basename and opens from cwd. - # Use mesh filenames without "meshes/" and chdir to meshes so they are found. + xml = path.read_text() xml = xml.replace('filename="meshes/', 'filename="') old_cwd = os.getcwd() @@ -50,41 +44,9 @@ def load_model(urdf_path=None): os.chdir(old_cwd) -def body_name_to_id(model: mujoco.MjModel, name: str) -> int: - try: - return model.body(name).id - except KeyError: - # List bodies for debugging - names = [model.body(i).name for i in range(model.nbody)] - raise KeyError(f"Body '{name}' not in model. Available: {names}") - - -def print_joint_limits_urdf(model: mujoco.MjModel) -> None: - """Print joint position limits from the MuJoCo model (from URDF). - Use this to compare with USD joint limits (e.g. in Isaac Sim). - """ - print("[joint limits] From URDF (MuJoCo model.jnt_range):") - for i in range(model.njnt): - jnt_type = model.jnt_type[i] - if jnt_type not in (mujoco.mjtJoint.mjJNT_HINGE, mujoco.mjtJoint.mjJNT_SLIDE): - continue - name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, i) or f"joint_{i}" - qposadr = model.jnt_qposadr[i] - if qposadr < 0: - continue - lo, hi = model.jnt_range[i, 0], model.jnt_range[i, 1] - finite = np.isfinite(lo) or np.isfinite(hi) - marker = " <-- joint 0 (shoulder_flexion)" if qposadr == 0 else "" - if finite: - print(f" [qpos {qposadr}] {name}: lower={lo:.4f}, upper={hi:.4f}{marker}") - if qposadr == 0 and lo == hi: - print(" ^ LOCKED at one value (lower=upper). MuJoCo URDF import may have set continuous joints to [0,0]. Fix URDF or USD so this joint has a real range.") - else: - print(f" [qpos {qposadr}] {name}: (no limits / continuous){marker}") - +def clip_to_joint_limits(model: mujoco.MjModel, data: mujoco.MjData) -> None: + """Clamp each hinge/slide joint's scalar position to ``model.jnt_range``. -def clip_qpos_to_limits(model: mujoco.MjModel, data: mujoco.MjData) -> None: - """Clip data.qpos to joint limits (only for limited joints). Bypass: if lower == upper (e.g. MuJoCo imported continuous as [0,0]), skip clipping so the IK can move that joint instead of locking it. """ @@ -92,15 +54,15 @@ def clip_qpos_to_limits(model: mujoco.MjModel, data: mujoco.MjData) -> None: jnt_type = model.jnt_type[i] if jnt_type not in (mujoco.mjtJoint.mjJNT_HINGE, mujoco.mjtJoint.mjJNT_SLIDE): continue - qposadr = model.jnt_qposadr[i] - if qposadr < 0: + pos_idx = model.jnt_qposadr[i] + if pos_idx < 0: continue lo = model.jnt_range[i, 0] hi = model.jnt_range[i, 1] if lo == hi: - continue # bypass: treat as unlimited (e.g. continuous imported as [0,0]) + continue # bypass: treat as unlimited revolute joint if np.isfinite(lo) or np.isfinite(hi): - data.qpos[qposadr] = np.clip(data.qpos[qposadr], lo, hi) + data.qpos[pos_idx] = np.clip(data.qpos[pos_idx], lo, hi) def solve_fingertip_ik( @@ -113,21 +75,12 @@ def solve_fingertip_ik( max_iter: int = 200, tol: float = 1e-4, clip_limits: bool = True, - arm_dofs: int = NUM_ARM_DOFS, - finger_weight: float = 4.0, - joint0_weight: float = 0.4, ) -> tuple[np.ndarray, bool, float]: """ Run damped least-squares IK so that each fingertip reaches its target. - Weighted regularization: arm DOFs (first arm_dofs) are penalized less than - finger DOFs (finger_weight > 1), so the solver prefers moving the arm to reach. - joint0_weight < 1 (default 0.4) further prefers joint 0 (shoulder_flexion) when - error has a Y/Z component, so the arm uses it to reduce large Y (or Y+Z) error. - - Joint 0 rotates about X so it cannot help with pure X error; for Y/Z error it can. targets: dict mapping body name -> (3,) world position. - Returns: (qpos, converged, final_max_error). + Returns: (generalized_positions, converged, final_max_error) — a copy of ``data.qpos``. """ # Build list of (body_name, target_pos) in consistent order tip_names = [b for b in FINGERTIP_BODIES if b in targets] @@ -135,12 +88,7 @@ def solve_fingertip_ik( raise ValueError("targets must contain at least one of %s" % FINGERTIP_BODIES) target_list = [targets[b] for b in tip_names] - # Diagonal regularization: joint 0 preferred when < 1, other arm 1, fingers finger_weight. nv = model.nv - arm_dofs = min(arm_dofs, nv) - W = np.ones(nv, dtype=np.float64) - W[0] = max(1e-3, float(joint0_weight)) - W[arm_dofs:] = finger_weight for _ in range(max_iter): mujoco.mj_forward(model, data) @@ -149,7 +97,7 @@ def solve_fingertip_ik( err_list = [] for body_name, target in zip(tip_names, target_list): - body_id = body_name_to_id(model, body_name) + body_id = model.body(body_name).id pos = data.xpos[body_id].copy() err = target - pos jacp = np.zeros((3, model.nv)) @@ -165,8 +113,7 @@ def solve_fingertip_ik( if max_err < tol: return data.qpos.copy(), True, float(max_err) - # Weighted damped least squares - A = J.T @ J + damping * np.diag(W) + A = J.T @ J + damping * np.eye(nv) rhs = J.T @ err dq = np.linalg.solve(A, rhs) @@ -174,87 +121,24 @@ def solve_fingertip_ik( data.qpos[:nv] += step * dq if clip_limits: - clip_qpos_to_limits(model, data) + clip_to_joint_limits(model, data) return data.qpos.copy(), False, float(max_err) def get_fingertip_positions(model: mujoco.MjModel, data: mujoco.MjData) -> dict[str, np.ndarray]: - """Return current world positions of all 5 fingertips.""" mujoco.mj_forward(model, data) out = {} for name in FINGERTIP_BODIES: - try: - bid = body_name_to_id(model, name) - out[name] = data.xpos[bid].copy() - except KeyError: - pass + id = model.body(name).id + out[name] = data.xpos[id].copy() return out -def diagnose_jacobian_joint0( - model: mujoco.MjModel, - data: mujoco.MjData, - err: np.ndarray | None = None, -) -> None: - """ - At current qpos, build the 15x21 task Jacobian J and explain why joint 0 - (shoulder_flexion_extension) often gets dq[0] ≈ 0. Joint 0 rotates about X, - so J's column 0 has zero X components; pure-X position error contributes - nothing to (J.T @ err)[0]. - - If err is provided (15-D task error: [ex,ey,ez] x 5 fingertips), also prints - error by axis and (J.T @ err) for first 6 joints so you can see if joint 0 - gets a non-zero rhs when error is in Y. - """ - mujoco.mj_forward(model, data) - J_list = [] - for name in FINGERTIP_BODIES: - bid = body_name_to_id(model, name) - jacp = np.zeros((3, model.nv)) - jacr = np.zeros((3, model.nv)) - mujoco.mj_jacBody(model, data, jacp, jacr, bid) - J_list.append(jacp) - J = np.vstack(J_list) - j0 = J[:, 0] - x_rows = [0, 3, 6, 9, 12] - j0_x = j0[x_rows] - j0_yz = np.delete(j0, x_rows) - print("Joint 0 (shoulder_flexion_extension) Jacobian column (15-D task):") - print(" ||J[:,0]|| =", np.linalg.norm(j0)) - print(" X components (rows 0,3,6,9,12):", j0_x, " -> should be ~0 (joint rotates about X)") - print(" Y,Z components norm:", np.linalg.norm(j0_yz)) - err_x = np.zeros(15) - err_x[0::3] = 1.0 - rhs0_x = (J.T @ err_x)[0] - print(" (J.T @ err_pure_X)[0] =", rhs0_x, " -> 0 means joint 0 gets no update for forward (X) reach") - err_y = np.zeros(15) - err_y[1::3] = 1.0 - rhs0_y = (J.T @ err_y)[0] - rhs1_y = (J.T @ err_y)[1] - print(" (J.T @ err_pure_Y)[0] =", rhs0_y, " [1] =", rhs1_y, " -> joint 0 can help for Y error") - print(" So dq[0] stays 0 when error is mostly in world X.") - - if err is not None and err.size == 15: - rhs = J.T @ err - err_x_sum = np.sum(np.abs(err[0::3])) - err_y_sum = np.sum(np.abs(err[1::3])) - err_z_sum = np.sum(np.abs(err[2::3])) - print(" --- With current task error ---") - print(" |error| by axis (sum over 5 tips): X =", err_x_sum, " Y =", err_y_sum, " Z =", err_z_sum) - print(" (J.T @ err)[0:6] (arm joints):", rhs[:6]) - print(" -> If Y error is large but rhs[0] ~ 0, other joints are covering Y; if rhs[0] != 0, joint 0 could step.") - - def main(): model = load_model() data = mujoco.MjData(model) - - # --- Joint limits from URDF (MuJoCo) --- - print("========== Joint limits (URDF / MuJoCo) ==========") - print_joint_limits_urdf(model) - print() - + data.qpos[:] = 0.0 mujoco.mj_forward(model, data) default_positions = get_fingertip_positions(model, data) diff --git a/autonomy/simulation/Humanoid_Wato/fingertip_ik_isaac_sim.py b/autonomy/simulation/Humanoid_Wato/fingertip_ik_isaac_sim.py index 4f4c8d6..2b0bbea 100644 --- a/autonomy/simulation/Humanoid_Wato/fingertip_ik_isaac_sim.py +++ b/autonomy/simulation/Humanoid_Wato/fingertip_ik_isaac_sim.py @@ -1,3 +1,4 @@ +"""21 DOF Humanoid Arm reaching 5 different 5 EE fingertip target positions""" import argparse from isaaclab.app import AppLauncher @@ -40,30 +41,6 @@ class FingertipIKSceneCfg(InteractiveSceneCfg): robot = ARM_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") -def print_joint_limits_from_usd(robot): - """Print joint position limits from the USD-loaded articulation (Isaac Sim). - Use this to check if joint 0 (shoulder_flexion_extension) has limits that could block motion. - """ - names = getattr(robot.data, "joint_names", None) - limits = getattr(robot.data, "default_joint_pos_limits", None) or getattr(robot.data, "joint_pos_limits", None) - if names is None or limits is None: - print("[joint limits] joint_names or (default_)joint_pos_limits not found on robot.data") - return - # limits shape: (num_instances, num_joints, 2) or (num_joints, 2) -> [lower, upper] - lim = limits.cpu().numpy() - if lim.ndim == 3: - lim = lim[0] - low, high = lim[:, 0], lim[:, 1] - print("[joint limits] From USD (robot.data.default_joint_pos_limits or joint_pos_limits):") - for i, name in enumerate(names): - marker = " <-- joint 0 (shoulder_flexion)" if i == 0 else "" - print(f" [{i}] {name}: lower={low[i]:.4f}, upper={high[i]:.4f}{marker}") - if hasattr(robot.data, "soft_joint_pos_limits") and robot.data.soft_joint_pos_limits is not None: - soft = robot.data.soft_joint_pos_limits[0].cpu().numpy() - if soft.ndim == 2: - print("[joint limits] soft_joint_pos_limits (first 6):", soft[:6]) - - def run_simulator( sim: sim_utils.SimulationContext, scene: InteractiveScene, @@ -102,8 +79,7 @@ def run_simulator( ) current_markers = VisualizationMarkers(current_marker_cfg) - # Map fingertip body names (from URDF) to Isaac body indices once. - fingertip_body_ids: list[int | None] = [] + fingertip_body_ids = [] for name in FINGERTIP_BODIES: ids, _ = robot.find_bodies(name, preserve_order=True) fingertip_body_ids.append(ids[0] if ids else None) @@ -114,14 +90,13 @@ def run_simulator( print(f"pose 1/{n_poses}: IK err {pose_sequence[0][2]:.4f} m") while simulation_app.is_running(): - # Advance to next pose when we've held long enough if step_count >= steps_per_pose: step_count = 0 pose_idx = (pose_idx + 1) % n_poses ik_err = pose_sequence[pose_idx][2] print(f"pose {pose_idx + 1}/{n_poses}: IK err {ik_err:.4f} m") - # Interpolate between current and next pose so motion is smooth (no snap/reset) + # Interpolate between current and next pose so motion is smooth next_idx = (pose_idx + 1) % n_poses alpha = step_count / steps_per_pose if steps_per_pose > 0 else 1.0 alpha = min(alpha, 1.0) @@ -131,24 +106,17 @@ def run_simulator( qpos_interp = (1.0 - alpha) * qpos_cur + alpha * qpos_next targets_interp = (1.0 - alpha) * targets_cur + alpha * targets_next - # Write interpolated state every frame so robot and markers stay in sync (no PD lag) joint_pos_tensor = torch.tensor(qpos_interp, dtype=torch.float32, device=sim.device) robot.write_joint_state_to_sim(joint_pos_tensor, joint_vel) targets_world = torch.tensor(targets_interp, dtype=torch.float32, device=sim.device) + scene.env_origins[0] target_markers.visualize(translations=targets_world) - # Visualize current fingertip locations (world frame) next to targets + # Visualize current fingertip locations current_positions = [] for body_id in fingertip_body_ids: - if body_id is None: - current_positions.append([0.0, 0.0, 0.0]) - else: - pos_w = robot.data.body_pos_w[0, body_id, :] # world frame - current_positions.append(pos_w) - current_positions_tensor = torch.stack( - [p if torch.is_tensor(p) else torch.tensor(p, device=sim.device) for p in current_positions], - dim=0, - ).to(device=sim.device, dtype=torch.float32) + pos_w = robot.data.body_pos_w[0, body_id, :] # world frame + current_positions.append(pos_w) + current_positions_tensor = torch.stack([p for p in current_positions], dim=0,).to(device=sim.device, dtype=torch.float32) current_markers.visualize(translations=current_positions_tensor) scene.write_data_to_sim() @@ -156,15 +124,12 @@ def run_simulator( scene.update(sim_dt) step_count += 1 - # After the first sim step of each pose, print per-finger errors in Isaac world frame. + # Log if step_count == 1: targets_np = targets_world.cpu().numpy() max_finger_err = 0.0 print("[Isaac] Per-finger position error at current pose (m):") for j, (name, body_id) in enumerate(zip(FINGERTIP_BODIES, fingertip_body_ids)): - if body_id is None: - print(f" {name}: (body not found)") - continue tip_pos = robot.data.body_pos_w[0, body_id, :].cpu().numpy() target_pos = targets_np[j] err_vec = tip_pos - target_pos @@ -177,46 +142,37 @@ def run_simulator( def main(): import mujoco - # ---- Run IK for several 5-point target sets ---- model = load_model() data = mujoco.MjData(model) data.qpos[:] = 0.0 default_positions = get_fingertip_positions(model, data) - if len(default_positions) != 5: - raise RuntimeError("Could not get all 5 fingertip positions from model.") - # Build a list of (dx, dy, dz) offsets in meters; each motion goes there and back. + # ---- Define Target Motion ---- n = 10 # steps per half (out or back) range_m = 0.14 # motion amplitude - def lin_there_back(start, end): + def linear_out_and_back(start, end): out = np.linspace(start, end, n, dtype=np.float64) back = np.linspace(end, start, n, dtype=np.float64) return np.concatenate([out, back[1:]]) - # 1. Vertical: Z down then up - z_off = lin_there_back(0.0, -range_m) - offsets_vertical = np.column_stack([np.zeros_like(z_off), np.zeros_like(z_off), z_off]) - - # 2. Horizontal X: move in X then back - x_off = lin_there_back(0.0, -range_m) - offsets_x = np.column_stack([x_off, np.zeros_like(x_off), np.zeros_like(x_off)]) - - # 3. Horizontal Y: move in Y then back - y_off = lin_there_back(0.0, range_m) - offsets_y = np.column_stack([np.zeros_like(y_off), y_off, np.zeros_like(y_off)]) - - # 4. Diagonal: X and Z together - d_off = lin_there_back(0.0, -range_m * 0.8) - offsets_diag = np.column_stack([d_off, np.zeros_like(d_off), d_off]) + def offsets_to_endpoint(end_xyz): + s = linear_out_and_back(0.0, 1.0) + return np.outer(s, np.asarray(end_xyz, dtype=np.float64)) - all_offsets = np.vstack([offsets_vertical, offsets_x, offsets_y, offsets_diag]) + motion_ends = [ + (0.0, 0.0, -range_m), # Z + (-range_m, 0.0, 0.0), # X + (0.0, range_m, 0.0), # Y + (-range_m * 0.8, 0.0, -range_m * 0.8), # XZ diagonal + ] + all_offsets = np.vstack([offsets_to_endpoint(e) for e in motion_ends]) + # ---- Precompute Pose with IK ---- pose_sequence = [] n_poses = len(all_offsets) for i, offset in enumerate(all_offsets): targets_mjc = {name: pos + offset for name, pos in default_positions.items()} - data.qpos[:] = 0.0 qpos, converged, max_err = solve_fingertip_ik( model, data, targets_mjc, damping=5e-4, step=0.25, max_iter=300, tol=5e-3, @@ -233,8 +189,6 @@ def lin_there_back(start, end): scene_cfg = FingertipIKSceneCfg(num_envs=1, env_spacing=2.0) scene = InteractiveScene(scene_cfg) sim.reset() - print("[INFO] Isaac Sim ready. Press Play: targets move vertical, then X, then Y, then diagonal; fingers should follow.") - print(f"[INFO] env_origins[0] (marker offset) = {scene.env_origins[0].cpu().numpy()}") run_simulator(sim, scene, pose_sequence, hold_seconds=0.08) 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 4a91472..527a622 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,3 +1,8 @@ +""" +Task space controller on only the 6 DOF of the humanoid arm, there is a cube which represent the target arm ee position, +user can move the cube's position and see the arm's IK controller react to it and follow the cube around +""" + import torch import argparse import sys @@ -13,9 +18,7 @@ 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 from isaaclab.scene import InteractiveScene, InteractiveSceneCfg from isaaclab.markers.config import FRAME_MARKER_CFG @@ -29,23 +32,15 @@ "../../Humanoid_Wato/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/modelCfg"))) from humanoid_arm_only import ARM_CFG -## -# Pre-defined configs -## - @configclass class TableTopSceneCfg(InteractiveSceneCfg): - """Configuration for a cart-pole scene.""" - - # ground plane ground = AssetBaseCfg( prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg(), init_state=AssetBaseCfg.InitialStateCfg(pos=(0.0, 0.0, -1.05)), ) - # lights dome_light = AssetBaseCfg( prim_path="/World/Light", spawn=sim_utils.DomeLightCfg( @@ -55,52 +50,46 @@ class TableTopSceneCfg(InteractiveSceneCfg): 0.75, 0.75))) - # Cube + # Setting the initial state here is important because at certain initial states, + # mainly on the positive x-axis, the IK breaks and the simulation bugs out a lot cube = AssetBaseCfg( prim_path="/World/cube", spawn=sim_utils.CuboidCfg( 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 robot = ARM_CFG.replace( - prim_path="{ENV_REGEX_NS}/Robot", - + prim_path="{ENV_REGEX_NS}/Robot" ) def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): robot = scene["robot"] - # Create controller diff_ik_cfg = DifferentialIKControllerCfg( command_type="pose", use_relative_mode=False, ik_method="dls") diff_ik_controller = DifferentialIKController( diff_ik_cfg, num_envs=scene.num_envs, device=sim.device) - # Markers frame_marker_cfg = FRAME_MARKER_CFG.copy() frame_marker_cfg.markers["frame"].scale = (0.1, 0.1, 0.1) ee_marker = VisualizationMarkers( - frame_marker_cfg.replace( - prim_path="/Visuals/ee_current")) + frame_marker_cfg.replace(prim_path="/Visuals/ee_current") + ) goal_marker = VisualizationMarkers( - frame_marker_cfg.replace( - prim_path="/Visuals/ee_goal")) + frame_marker_cfg.replace(prim_path="/Visuals/ee_goal") + ) - # Specify robot-specific parameters 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"]) + "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) # Obtain the frame index of the end-effector ; For a fixed base robot, the @@ -111,23 +100,18 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): else: ee_jacobi_idx = robot_entity_cfg.body_ids[0] - # Define simulation stepping sim_dt = sim.get_physics_dt() - # May have to set initial position first 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 position, quaternion = scene["cube"].get_world_poses() - # Quaternion is in (w, x, y, z) ik_commands = torch.cat([position, quaternion], dim=1) diff_ik_controller.set_command(ik_commands) - # obtain quantities from simulation jacobian = robot.root_physx_view.get_jacobians()[ :, ee_jacobi_idx, :, robot_entity_cfg.joint_ids] ee_pose_w = robot.data.body_state_w[:, @@ -135,12 +119,10 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): root_pose_w = robot.data.root_state_w[:, 0:7] joint_pos = robot.data.joint_pos[:, robot_entity_cfg.joint_ids] - # compute frame in root frame ee_pos_b, ee_quat_b = subtract_frame_transforms( root_pose_w[:, 0:3], root_pose_w[:, 3:7], ee_pose_w[:, 0:3], ee_pose_w[:, 3:7] ) - # compute the joint commands joint_pos_des = diff_ik_controller.compute( ee_pos_b, ee_quat_b, jacobian, joint_pos) @@ -149,40 +131,30 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): joint_pos_des, joint_ids=robot_entity_cfg.joint_ids) scene.write_data_to_sim() - # perform step sim.step() - # update buffers scene.update(sim_dt) - # obtain quantities from simulation - ee_pose_w = robot.data.body_state_w[:, - robot_entity_cfg.body_ids[0], 0:7] + ee_pose_w = robot.data.body_state_w[:, robot_entity_cfg.body_ids[0], 0:7] - # update marker positions ee_marker.visualize(ee_pose_w[:, 0:3], ee_pose_w[:, 3:7]) goal_marker.visualize( ik_commands[:, 0:3] + scene.env_origins, ik_commands[:, 3:7]) def main(): - # Load kit helper sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) sim = sim_utils.SimulationContext(sim_cfg) - # Set main camera sim.set_camera_view([2.5, 2.5, 2.5], [0.0, 0.0, 0.0]) - # Design scene scene_cfg = TableTopSceneCfg(num_envs=1, env_spacing=2.0) scene = InteractiveScene(scene_cfg) - # Play the simulator sim.reset() print("[INFO]: Setup complete...") - # Run the simulator run_simulator(sim, scene)