diff --git a/autonomy/behaviour/joint_command/config/hardware_mapping.yaml b/autonomy/behaviour/joint_command/config/hardware_mapping.yaml index 9b237bb4..614b9888 100644 --- a/autonomy/behaviour/joint_command/config/hardware_mapping.yaml +++ b/autonomy/behaviour/joint_command/config/hardware_mapping.yaml @@ -2,32 +2,32 @@ left: shoulder: pitch: can_id: 14 - lower_limit: -9.20001220703125 - upper_limit: 10.699996948242188 + lower_limit: -31.900001525878906 + upper_limit: 41.60000038146973 direction: 1 - zero_offset: 185.39999389648438 + zero_offset: 66.5 limit_range: true roll: can_id: 12 direction: -1 - zero_offset: -84.4 - lower_limit: -7.700000000000003 - upper_limit: 3.6999999999999886 + zero_offset: -85.0999984741211 + lower_limit: -3.0 + upper_limit: 1.9000015258789062 limit_range: true yaw: can_id: 11 - lower_limit: -11.300003051757812 - upper_limit: 38.19999694824219 + lower_limit: -212.2999973297119 + upper_limit: 92.39999961853027 direction: 1 - zero_offset: 177.89999389648438 + zero_offset: -21.100000381469727 limit_range: true elbow: pitch: can_id: 10 direction: -1 - zero_offset: -105.5 - lower_limit: -37.400001525878906 - upper_limit: 47.19999694824219 + zero_offset: -77.5 + lower_limit: -55.10000038146973 + upper_limit: 56.30000305175781 limit_range: true roll: can_id: 13 diff --git a/autonomy/behaviour/joint_command/include/joint_command_node.hpp b/autonomy/behaviour/joint_command/include/joint_command_node.hpp index abc72eb1..1a573bde 100644 --- a/autonomy/behaviour/joint_command/include/joint_command_node.hpp +++ b/autonomy/behaviour/joint_command/include/joint_command_node.hpp @@ -46,8 +46,17 @@ class JointCommandNode : public rclcpp::Node { // earlier run the instant the interfacing container came back up). If no fresh ArmPose // arrives within command_timeout_sec_, stop publishing AND clear seeded_from_feedback_, // so the next real command must re-seed from fresh feedback and ramp safely again, - // exactly like a first-ever command after node startup. - double command_timeout_sec_{10.0}; + // exactly like a first-ever command after node startup. Kept short (not the original + // 10s) so stopping the ArmPose publisher (e.g. task_space_real.py) frees the arm almost + // immediately, rather than leaving it held for up to 10 more seconds. + double command_timeout_sec_{0.5}; rclcpp::Time last_armpose_time_; bool have_armpose_time_{false}; + + // Sent once (not spammed every control tick) the instant staleness is detected, so the + // motors actually go limp/free instead of just no-longer-receiving-new-targets -- + // POSITION_LOOP motors hold their last commanded position indefinitely on their own + // onboard control loop; going silent alone does NOT release them. Reset to false again + // on the next fresh ArmPose so a future staleness event re-disables. + bool disable_sent_{false}; }; diff --git a/autonomy/behaviour/joint_command/scripts/feedback_to_udp_bridge.py b/autonomy/behaviour/joint_command/scripts/feedback_to_udp_bridge.py new file mode 100644 index 00000000..9ad9676d --- /dev/null +++ b/autonomy/behaviour/joint_command/scripts/feedback_to_udp_bridge.py @@ -0,0 +1,62 @@ +#!/usr/bin/env python3 +"""ROS2-to-UDP bridge for live_arm_isaacsim.py. + +live_arm_isaacsim.py runs inside the env_isaaclab conda environment, where rclpy can't +be imported (its compiled extension is built for the system ROS Python, not conda's -- +see udp_to_ros_bridge.py for the same constraint in the opposite direction). This script +runs under the SYSTEM python (where rclpy works), subscribes to /interfacing/motorFeedback, +and forwards each message as a UDP packet the Isaac Sim viewer can read without rclpy. + +Packet format (matches live_arm_isaacsim.py's run_simulator unpack): + struct.pack("=id", motor_id, position) -- native int32 motor_id + native double position + +Usage (system python, ROS sourced): + source /opt/ros/jazzy/setup.bash + source /home/rwahib/wato/humanoid/autonomy/install/setup.bash + /usr/bin/python3 feedback_to_udp_bridge.py [--host 127.0.0.1] [--port 5006] +""" + +import argparse +import socket +import struct + +import rclpy +from rclpy.node import Node + +from common_msgs.msg import MotorFeedback + +PACKET_FORMAT = "=id" + + +class UdpBridge(Node): + def __init__(self, host, port): + super().__init__("feedback_to_udp_bridge") + self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + self.dest = (host, port) + self.create_subscription(MotorFeedback, "/interfacing/motorFeedback", self._on_feedback, 20) + self.get_logger().info(f"Forwarding /interfacing/motorFeedback to UDP {host}:{port}") + + def _on_feedback(self, msg: MotorFeedback) -> None: + packet = struct.pack(PACKET_FORMAT, int(msg.motor_id), float(msg.position)) + self.sock.sendto(packet, self.dest) + + +def main(): + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--host", type=str, default="127.0.0.1") + parser.add_argument("--port", type=int, default=5006) + args = parser.parse_args() + + rclpy.init() + node = UdpBridge(args.host, args.port) + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/autonomy/behaviour/joint_command/src/joint_command_node.cpp b/autonomy/behaviour/joint_command/src/joint_command_node.cpp index 80989cda..47e1d622 100644 --- a/autonomy/behaviour/joint_command/src/joint_command_node.cpp +++ b/autonomy/behaviour/joint_command/src/joint_command_node.cpp @@ -14,7 +14,7 @@ JointCommandNode::JointCommandNode() : Node("joint_command_node") { this->declare_parameter("motor_cmd_topic", "/interfacing/motorCMD"); this->declare_parameter("control_type", common_msgs::msg::MotorCmd::POSITION_LOOP); this->declare_parameter("feedback_topic", "/interfacing/motorFeedback"); - this->declare_parameter("command_timeout_sec", 10.0); + this->declare_parameter("command_timeout_sec", 0.5); const std::string arm_side = this->get_parameter("arm_side").as_string(); control_rate_hz_ = this->get_parameter("control_rate_hz").as_double(); @@ -95,6 +95,7 @@ void JointCommandNode::armPoseCallback(const common_msgs::msg::ArmPose::SharedPt have_latest_cmds_ = true; last_armpose_time_ = this->get_clock()->now(); have_armpose_time_ = true; + disable_sent_ = false; } catch (const std::exception& e) { RCLCPP_ERROR(this->get_logger(), "Failed to process ArmPose: %s", e.what()); } @@ -120,6 +121,27 @@ void JointCommandNode::controlTimerCallback() { command_timeout_sec_); have_latest_cmds_ = false; seeded_from_feedback_ = false; + if (!disable_sent_) { + // Going silent alone leaves POSITION_LOOP motors holding their last commanded + // position forever (that's their own onboard control loop, not something this + // node's silence affects) -- send an explicit one-shot DISABLE per motor so the + // arm actually goes free/limp the moment the ArmPose source (e.g. + // task_space_real.py) stops, instead of staying locked in place. + std::vector disable_cmds; + disable_cmds.reserve(latest_cmds_.size()); + for (const auto& cmd : latest_cmds_) { + common_msgs::msg::MotorCmd disable_cmd; + disable_cmd.motor_id = cmd.motor_id; + disable_cmd.control_type = common_msgs::msg::MotorCmd::DISABLE; + disable_cmds.push_back(disable_cmd); + } + for (const auto& cmd : disable_cmds) { + motor_cmd_pub_->publish(cmd); + } + RCLCPP_WARN(this->get_logger(), "Sent DISABLE to %zu motors -- arm is now free to move.", + disable_cmds.size()); + disable_sent_ = true; + } return; } publishMotorCommands(latest_cmds_); diff --git a/autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/live_arm_isaacsim.py b/autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/live_arm_isaacsim.py new file mode 100644 index 00000000..ebb3f275 --- /dev/null +++ b/autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/live_arm_isaacsim.py @@ -0,0 +1,283 @@ +"""Real-time Isaac Sim visualization of the physical bimanual test stand (Isaac Lab +counterpart to live_arm_mjviser.py, same directory). + +Drives one of BIMANUAL_ARM_CFG's two joint chains directly from live +/interfacing/motorFeedback data, same --arm-side/--urdf-side split as +live_arm_mjviser.py: --arm-side picks which hardware_mapping.yaml section / real motors +to read (only "left" is wired/calibrated today); --urdf-side picks which URDF chain to +animate with that feedback. Defaults to --urdf-side right (the unsuffixed joint1..joint6 +chain), matching live_arm_mjviser.py's own validated default invocation in +ARM_BRINGUP.md (--arm-side left --urdf-side right) -- confirmed by direct observation +against the real test stand that the suffixed joint1L..joint6l chain is NOT the visually +correct one here, despite bimanual_arm_cfg.py's docstring claiming otherwise. Read-only: +never calls set_joint_position_target or publishes MotorCmd -- each tick it force-writes +joint state via write_joint_state_to_sim(), the same direct-overwrite semantics +live_arm_mjviser.py uses on data.qpos (no PD lag/interpolation, exact live mirror). It +cannot move the real arm. + +Zero position matches live_arm_mjviser.py: both compute +zero_offset + direction*motor_deg + display_offset from hardware_mapping.yaml's +calibrated zero_offset per joint. This also matches BIMANUAL_ARM_CFG's default/rest pose +for BOTH chains (see bimanual_arm_cfg.py's _load_zero_offsets_deg()), so joints with no +live feedback yet (e.g. wrist_pitch, not wired today) sit at the calibrated zero instead +of a stale Physics Inspector snapshot. task_space_real.py's own sim-zero is matched the +same way, on the OTHER chain (RIGHT_ARM_JOINTS, which it always drives for real hardware +output regardless of which chain looks right in this viewer). + +rclpy can't be imported inside env_isaaclab (its compiled extension targets the system +ROS Python, not conda's -- see udp_to_ros_bridge.py for the same constraint in the +opposite direction). So this script never imports rclpy: it reads motor feedback from a +UDP socket instead, fed by feedback_to_udp_bridge.py running under system ROS Python. + +Terminal 1 (system python, ROS sourced): + source /opt/ros/jazzy/setup.bash + source /home/rwahib/wato/humanoid/autonomy/install/setup.bash + /usr/bin/python3 autonomy/behaviour/joint_command/scripts/feedback_to_udp_bridge.py + +Terminal 2 (env_isaaclab): + conda activate env_isaaclab + cd /home/rwahib/wato/humanoid + python autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/live_arm_isaacsim.py +""" + +import argparse +import os +import socket +import struct +import sys + +from isaaclab.app import AppLauncher + +parser = argparse.ArgumentParser(description="Live Isaac Sim mirror of the real bimanual arm.") +parser.add_argument("--arm-side", default="left", choices=["left", "right"], + help="hardware side: which hardware_mapping.yaml section / real motors to read " + "(only 'left' is wired/calibrated today)") +parser.add_argument("--urdf-side", default="right", choices=["left", "right"], + help="which URDF chain to drive with that feedback: 'right' -> unsuffixed " + "joint1..joint6 (default, matches live_arm_mjviser.py's validated " + "ARM_BRINGUP.md invocation), 'left' -> suffixed joint1L..joint6l " + "(the chain task_space_real.py drives for real hardware output).") +parser.add_argument("--flip", nargs="*", default=[], metavar="LABEL", + help="hardware_mapping labels whose sign to invert (e.g. shoulder_roll) -- " + "same as live_arm_mjviser.py's --flip.") +parser.add_argument("--offset", nargs="*", default=[], metavar="LABEL=DEG", + help="viewer-only constant added to a joint's displayed angle, in degrees " + "(e.g. shoulder_yaw=90) -- same as live_arm_mjviser.py's --offset. " + "Never touches real commands.") +parser.add_argument("--host", type=str, default="127.0.0.1", help="feedback_to_udp_bridge.py host") +parser.add_argument("--port", type=int, default=5006, help="feedback_to_udp_bridge.py port") +AppLauncher.add_app_launcher_args(parser) +args_cli = parser.parse_args() + +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +# Import bimanual_arm_cfg from keyboard teleoperation (same robot model/zero convention +# as task_space_real.py and live_arm_mjviser.py). +_KEYBOARD_TELEOP_DIR = os.path.abspath( + os.path.join(os.path.dirname(__file__), "../../Teleop/keyboard_based_teleoperation") +) +sys.path.insert(0, _KEYBOARD_TELEOP_DIR) + +import yaml # noqa: E402 + +from bimanual_arm_cfg import ( # noqa: E402 + BIMANUAL_ARM_CFG, + _HARDWARE_MAPPING_PATH, + apply_joint_limits, + resolve_joint_name, +) +import isaaclab.sim as sim_utils # noqa: E402 +from isaaclab.assets import AssetBaseCfg # noqa: E402 +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg # noqa: E402 +from isaaclab.utils import configclass # noqa: E402 +import math # noqa: E402 + + +# hardware_mapping.yaml label -> BIMANUAL_ARM_CFG URDF joint, per side. Same dict as +# live_arm_mjviser.py's LABEL_TO_URDF_JOINT (duplicated rather than imported from that +# script, since it pulls in rclpy/mujoco/viser at import time). +LABEL_TO_URDF_JOINT = { + "left": { + "shoulder_pitch": "joint1L", + "shoulder_roll": "joint2l", + "shoulder_yaw": "joint3l", + "elbow_pitch": "joint4l", + "elbow_roll": "joint5l", + "wrist_pitch": "joint6l", + }, + "right": { + "shoulder_pitch": "joint1", + "shoulder_roll": "joint2", + "shoulder_yaw": "joint3", + "elbow_pitch": "joint4", + "elbow_roll": "joint5", + "wrist_pitch": "joint6", + }, +} + + +def load_can_id_map( + mapping_path: str, + hw_side: str, + urdf_side: str, + flip_labels: set = frozenset(), + offset_labels: dict = None, +) -> dict: + """hardware_mapping.yaml -> {can_id: {label, urdf_joint, direction, zero_offset, display_offset, + lower_limit, upper_limit}}. + + Same fields, --flip/--offset semantics, and hw_side/urdf_side split as + live_arm_mjviser.py's load_can_id_map. + """ + with open(mapping_path) as f: + config = yaml.safe_load(f)[hw_side] + + label_to_joint = LABEL_TO_URDF_JOINT[urdf_side] + can_id_map = {} + for group, joints in config.items(): + for name, cfg in joints.items(): + label = f"{group}_{name}" + urdf_joint = label_to_joint.get(label) + if urdf_joint is None: + continue + # MUST negate zero_offset together with direction, not direction alone: + # zero_offset was computed as -home_pos/direction during calibration so that + # zero_offset + direction*home_pos == 0 at the real motor's physical zero pose. + # Flipping direction alone breaks that identity and shifts the displayed pose + # by 2*zero_offset at the real zero -- negating both preserves + # joint_deg(home_pos) == 0 while correctly reversing the sense of motion + # elsewhere (verified: joint_deg_new(raw) == -joint_deg_old(raw) identically). + flip = label in flip_labels + direction = int(cfg["direction"]) * (-1 if flip else 1) + zero_offset = float(cfg["zero_offset"]) * (-1 if flip else 1) + can_id_map[int(cfg["can_id"])] = { + "label": label, + "urdf_joint": urdf_joint, + "direction": direction, + "zero_offset": zero_offset, + "display_offset": float((offset_labels or {}).get(label, 0.0)), + "lower_limit": float(cfg["lower_limit"]), + "upper_limit": float(cfg["upper_limit"]), + } + return can_id_map + + +@configclass +class BareSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg( + prim_path="/World/defaultGroundPlane", + spawn=sim_utils.GroundPlaneCfg(), + init_state=AssetBaseCfg.InitialStateCfg(pos=(0.0, 0.0, -1.05)), + ) + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)), + ) + robot = BIMANUAL_ARM_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + +def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene, + can_id_map: dict, sock: socket.socket) -> None: + robot = scene["robot"] + sim_dt = sim.get_physics_dt() + + scene.update(sim_dt) + apply_joint_limits(robot) + + name_to_id = {name: i for i, name in enumerate(robot.data.joint_names)} + urdf_joint_ids = { + cfg["urdf_joint"]: name_to_id[resolve_joint_name(robot, cfg["urdf_joint"])] + for cfg in can_id_map.values() + } + + print(f"Tracking {len(can_id_map)} motors from {_HARDWARE_MAPPING_PATH} " + f"(hardware={args_cli.arm_side} arm -> driving URDF {args_cli.urdf_side} arm):") + for can_id, cfg in sorted(can_id_map.items()): + flip = " (flipped)" if cfg["label"] in set(args_cli.flip) else "" + off = f" (offset {cfg['display_offset']:+g}°)" if cfg["display_offset"] else "" + print(f" 0x{can_id:02X} -> {cfg['label']:<14} -> {cfg['urdf_joint']}{flip}{off}") + + 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) + scene.write_data_to_sim() + sim.step() + scene.update(sim_dt) + + print("[INFO] Live Isaac Sim mirror running -- move the real arm to see it track here.") + + # Diagnostic only (no filtering/rejection -- this stays a raw, unfiltered mirror by + # design, same as live_arm_mjviser.py). Logs a jump's RAW inputs so a future glitch + # can be traced to its actual source: if the raw `position` printed here is already + # anomalous, it's upstream (CAN/encoder); if raw looks sane but joint_deg doesn't, + # it's a bug in this script's own transform. + _JUMP_WARN_DEG = 15.0 + _last_deg = {} + + while simulation_app.is_running(): + while True: + try: + data, _ = sock.recvfrom(1024) + except BlockingIOError: + break + if len(data) != struct.calcsize("=id"): + continue + motor_id, position = struct.unpack("=id", data) + cfg = can_id_map.get(motor_id) + if cfg is None: + continue + joint_deg = cfg["zero_offset"] + cfg["direction"] * position + cfg["display_offset"] + prev = _last_deg.get(motor_id) + if prev is not None and abs(joint_deg - prev) > _JUMP_WARN_DEG: + print(f"[WARN] {cfg['label']} (motor {motor_id}) jumped {prev:.2f}deg -> " + f"{joint_deg:.2f}deg (raw position={position:.3f}, packet_bytes={len(data)})") + _last_deg[motor_id] = joint_deg + joint_id = urdf_joint_ids[cfg["urdf_joint"]] + joint_position[0, joint_id] = math.radians(joint_deg) + + # Direct state overwrite (not set_joint_position_target): matches + # live_arm_mjviser.py's `data.qpos[...] = ...` -- an exact live mirror with no + # PD lag, since this is read-only visualization, not a commanded target. + robot.write_joint_state_to_sim(joint_position, joint_vel) + scene.write_data_to_sim() + sim.step() + scene.update(sim_dt) + + +def main() -> None: + offset_labels = {} + for item in args_cli.offset: + label, _, val = item.partition("=") + offset_labels[label.strip()] = float(val) + + can_id_map = load_can_id_map( + _HARDWARE_MAPPING_PATH, args_cli.arm_side, args_cli.urdf_side, + set(args_cli.flip), offset_labels + ) + if not can_id_map: + raise RuntimeError( + f"No joints resolved for arm_side={args_cli.arm_side!r} in {_HARDWARE_MAPPING_PATH}" + ) + + sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + sock.bind((args_cli.host, args_cli.port)) + sock.setblocking(False) + print(f"[UDP] Listening for motor feedback on {args_cli.host}:{args_cli.port} " + f"(run feedback_to_udp_bridge.py to feed this)") + + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + sim.set_camera_view([2.5, 2.5, 2.0], [0.0, 0.0, 0.8]) + + scene_cfg = BareSceneCfg(num_envs=1, env_spacing=2.0) + scene = InteractiveScene(scene_cfg) + sim.reset() + + print("[INFO]: Setup complete...") + run_simulator(sim, scene, can_id_map, sock) + + +if __name__ == "__main__": + main() + simulation_app.close() diff --git a/autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/live_arm_mjviser.py b/autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/live_arm_mjviser.py index 03751e7f..4042d9fd 100644 --- a/autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/live_arm_mjviser.py +++ b/autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/live_arm_mjviser.py @@ -26,7 +26,8 @@ unsuffixed chain (joint1, joint2, ..., joint6) is the LEFT arm. This does not change LABEL_TO_URDF_JOINT below: it maps hardware_mapping.yaml's wiring side to whichever URDF chain is physically wired to it, which hasn't changed -- only -what that URDF chain should be CALLED has. +what that URDF chain should be CALLED has. See ARM_BRINGUP.md for the +validated invocation (--arm-side left --urdf-side right). The gripper (hardware_mapping.yaml can_id 0x15, command-frame units 0-100) has no confirmed scale to the prismatic finger joints' travel in meters (joint7l/joint8l), diff --git a/autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/urdf/bimanual_arm/configuration/bimanual_arm_physics.usd b/autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/urdf/bimanual_arm/configuration/bimanual_arm_physics.usd index 8fa52b2d..80ea919b 100644 Binary files a/autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/urdf/bimanual_arm/configuration/bimanual_arm_physics.usd and b/autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/urdf/bimanual_arm/configuration/bimanual_arm_physics.usd differ diff --git a/autonomy/simulation/Task_space_controller/robot_arm_controllers/task_space_real.py b/autonomy/simulation/Task_space_controller/robot_arm_controllers/task_space_real.py index b0c0638b..1d081c52 100644 --- a/autonomy/simulation/Task_space_controller/robot_arm_controllers/task_space_real.py +++ b/autonomy/simulation/Task_space_controller/robot_arm_controllers/task_space_real.py @@ -41,11 +41,12 @@ from bimanual_arm_cfg import ( # noqa: E402 BIMANUAL_ARM_CFG, GRIPPER_OPEN, - RIGHT_ARM_JOINTS, - RIGHT_EE_BODY, - RIGHT_FINGER_TIP_BODIES, - RIGHT_GRIPPER_JOINTS, LEFT_ARM_JOINTS, + LEFT_EE_BODY, + LEFT_FINGER_TIP_BODIES, + LEFT_FINGER_DISTAL_TIP_LOCAL, + LEFT_GRIPPER_JOINTS, + RIGHT_ARM_JOINTS, RIGHT_GRIPPER_JOINTS, apply_joint_limits, compute_tip_ik_jacobian, @@ -97,6 +98,7 @@ def publish_joint_pos_udp(joint_pos_des): q = [math.degrees(v) for v in joint_pos_des[0].tolist()] data = struct.pack("6d", *q) udp_sock.sendto(data, (UDP_HOST, UDP_PORT)) + print(f"[UDP] angles sent (deg): {[f'{v:.2f}' for v in q]}") def _joint_ids(robot, names: list[str]) -> list[int]: @@ -117,15 +119,10 @@ class TableTopSceneCfg(InteractiveSceneCfg): spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)), ) - # Spawned at the active/correct arm's (link7/link8, unsuffixed chain) actual measured - # fingertip midpoint at its default pose -- the old value (0.26, -0.23, 0.15) was - # ~0.31 units from the OTHER (passive, joint1L..6l) arm's resting hand but ~0.95 units - # from this arm's, meaning the IK was starting by reaching almost a meter toward where - # the wrong arm's hand rests. Drag the cube from here once the sim is running. cube = AssetBaseCfg( prim_path="/World/cube", spawn=sim_utils.CuboidCfg(size=[0.1, 0.1, 0.1]), - init_state=AssetBaseCfg.InitialStateCfg(pos=(0.24, 0.64, 0.53)), + init_state=AssetBaseCfg.InitialStateCfg(pos=(-0.234, 0.288, -0.222)), ) robot = BIMANUAL_ARM_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") @@ -138,14 +135,25 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): scene.update(sim_dt) apply_joint_limits(robot) - right_arm_names = [resolve_joint_name(robot, name) for name in RIGHT_ARM_JOINTS] + left_arm_names = [resolve_joint_name(robot, name) for name in LEFT_ARM_JOINTS[:6]] print(f"[INFO] Robot joints: {robot.data.joint_names}") - print(f"[INFO] Left arm IK joints: {right_arm_names}") - print(f"[INFO] Left wrist body (Jacobian anchor): {RIGHT_EE_BODY}") - print("[INFO] IK tracks fingertip center (link7l/link8l mesh distal midpoint)") - + print(f"[INFO] Left arm IK joints: {left_arm_names}") + print(f"[INFO] Left wrist body (Jacobian anchor): {LEFT_EE_BODY}") + print("[INFO] IK tracks fingertip center (link7/link8 mesh distal midpoint)") + + # lambda_val raised well above IsaacLab's default (0.01): compute() takes a full + # one-shot Newton step every frame (joint_pos + J^T(JJ^T + lambda^2 I)^-1 * error), + # not a small gradient step. This 6-joint arm doing a 3-DOF position-only task has 3 + # redundant DOF, so the 3x6 position Jacobian is often poorly-conditioned -- with weak + # damping, a near-singular direction gets amplified by ~1/lambda^2 (10000x at the + # default 0.01), turning even sub-millimeter position error (e.g. float rounding + # between the cube's exact position and the measured fingertip position) into a + # multi-hundred-degree single-step joint jump. Confirmed directly: joint4 hit 447 deg + # and joint6 hit 610 deg from a cube spawned at the exact measured default-pose + # fingertip position. Higher damping trades a bit of tracking speed for stability. diff_ik_cfg = DifferentialIKControllerCfg( - command_type="position", use_relative_mode=False, ik_method="dls" + command_type="position", use_relative_mode=False, ik_method="dls", + ik_params={"lambda_val": 0.2}, ) diff_ik_controller = DifferentialIKController( diff_ik_cfg, num_envs=scene.num_envs, device=sim.device @@ -157,7 +165,7 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): goal_marker = VisualizationMarkers(frame_marker_cfg.replace(prim_path="/Visuals/ee_goal")) robot_entity_cfg = SceneEntityCfg( - "robot", joint_names=right_arm_names, body_names=[RIGHT_EE_BODY] + "robot", joint_names=left_arm_names, body_names=[LEFT_EE_BODY] ) robot_entity_cfg.resolve(scene) @@ -165,26 +173,35 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): robot_entity_cfg.body_ids[0] - 1 if robot.is_fixed_base else robot_entity_cfg.body_ids[0] ) wrist_body_id = robot_entity_cfg.body_ids[0] - finger_body_ids = resolve_body_ids(robot, RIGHT_FINGER_TIP_BODIES) + finger_body_ids = resolve_body_ids(robot, LEFT_FINGER_TIP_BODIES) left_arm_ids = robot_entity_cfg.joint_ids - right_gripper_ids = _joint_ids(robot, RIGHT_GRIPPER_JOINTS) - left_joint_ids = _joint_ids(robot, LEFT_ARM_JOINTS) + left_gripper_ids = _joint_ids(robot, LEFT_GRIPPER_JOINTS) + right_joint_ids = _joint_ids(robot, RIGHT_ARM_JOINTS) right_gripper_ids = _joint_ids(robot, RIGHT_GRIPPER_JOINTS) gripper_open_targets = torch.tensor( - [[GRIPPER_OPEN[name] for name in RIGHT_GRIPPER_JOINTS]], + [[GRIPPER_OPEN[name] for name in LEFT_GRIPPER_JOINTS]], device=sim.device, ) 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) - left_default_pos = joint_position[:, left_joint_ids].clone() + right_default_pos = joint_position[:, right_joint_ids].clone() + right_gripper_default_pos = joint_position[:, right_gripper_ids].clone() scene.write_data_to_sim() sim.step() scene.update(sim_dt) + _startup_tip_pos_w, _ = compute_gripper_tip_pose_w( + robot, wrist_body_id, finger_body_ids, + tip_bodies=LEFT_FINGER_TIP_BODIES, tip_local=LEFT_FINGER_DISTAL_TIP_LOCAL, + ) + print(f"[INFO] Left arm fingertip-center world position at default pose: " + f"{_startup_tip_pos_w[0].tolist()} -- set the cube's init_state pos to this " + f"so the IK starts close to the resting hand.") + diff_ik_controller.reset(env_ids=torch.arange(scene.num_envs, device=sim.device)) # Sim-to-real: accumulate sim time and publish joint targets every 20ms, @@ -209,7 +226,8 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): root_pose_w[:, 0:3], root_pose_w[:, 3:7], ee_pose_w[:, 0:3], ee_pose_w[:, 3:7] ) tip_pos_b, tip_quat_b = compute_gripper_tip_pose_b( - robot, root_pose_w, wrist_body_id, finger_body_ids + robot, root_pose_w, wrist_body_id, finger_body_ids, + tip_bodies=LEFT_FINGER_TIP_BODIES, tip_local=LEFT_FINGER_DISTAL_TIP_LOCAL, ) # set_command needs ee_quat to hold current orientation (used for display only in position mode) @@ -225,12 +243,13 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): joint_pos_des = diff_ik_controller.compute(tip_pos_b, tip_quat_b, jacobian, joint_pos) robot.set_joint_position_target(joint_pos_des, joint_ids=left_arm_ids) - # Hold left gripper open; right arm + gripper at default pose - robot.set_joint_position_target(gripper_open_targets, joint_ids=right_gripper_ids) - robot.set_joint_position_target(left_default_pos, joint_ids=left_joint_ids) + # Hold left (real-hardware) gripper open; right arm + gripper at default pose + robot.set_joint_position_target(gripper_open_targets, joint_ids=left_gripper_ids) + robot.set_joint_position_target(right_default_pos, joint_ids=right_joint_ids) + robot.set_joint_position_target(right_gripper_default_pos, joint_ids=right_gripper_ids) robot.set_joint_velocity_target( - torch.zeros(1, len(right_gripper_ids), device=sim.device), - joint_ids=right_gripper_ids, + torch.zeros(1, len(left_gripper_ids), device=sim.device), + joint_ids=left_gripper_ids, ) # Send desired left-arm joint targets to the real arm via the UDP bridge, @@ -245,7 +264,10 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): sim.step() scene.update(sim_dt) - tip_pos_w, tip_quat_w = compute_gripper_tip_pose_w(robot, wrist_body_id, finger_body_ids) + tip_pos_w, tip_quat_w = compute_gripper_tip_pose_w( + robot, wrist_body_id, finger_body_ids, + tip_bodies=LEFT_FINGER_TIP_BODIES, tip_local=LEFT_FINGER_DISTAL_TIP_LOCAL, + ) ee_marker.visualize(tip_pos_w, tip_quat_w) goal_marker.visualize(cube_pos_w, cube_quat_w) diff --git a/autonomy/simulation/Teleop/keyboard_based_teleoperation/bimanual_arm_cfg.py b/autonomy/simulation/Teleop/keyboard_based_teleoperation/bimanual_arm_cfg.py index 87890848..2d07eed6 100644 --- a/autonomy/simulation/Teleop/keyboard_based_teleoperation/bimanual_arm_cfg.py +++ b/autonomy/simulation/Teleop/keyboard_based_teleoperation/bimanual_arm_cfg.py @@ -47,6 +47,12 @@ _THIS_DIR = os.path.abspath(os.path.dirname(__file__)) _BIMANUAL_ROOT = os.path.abspath(os.path.join(_THIS_DIR, "..", "..", "Humanoid_Wato", "wato_bimanual_arm")) _ARM_USD_PATH = os.path.join(_BIMANUAL_ROOT, "urdf", "bimanual_arm", "bimanual_arm.usd") +# Real-hardware calibration data (direction/zero_offset/limits per joint), read by +# live_arm_isaacsim.py and live_arm_mjviser.py to project live CAN feedback onto this +# URDF's joint coordinates. +_HARDWARE_MAPPING_PATH = os.path.abspath( + os.path.join(_THIS_DIR, "..", "..", "..", "behaviour", "joint_command", "config", "hardware_mapping.yaml") +) def _deg(degrees: float) -> float: @@ -100,6 +106,7 @@ def _deg(degrees: float) -> float: LEFT_ARM_JOINTS = ["joint1", "joint2", "joint3", "joint4", "joint5", "joint6", "joint7", "joint8"] RIGHT_ARM_JOINTS = ["joint1L", "joint2l", "joint3l", "joint4l", "joint5l", "joint6l"] +LEFT_GRIPPER_JOINTS = ["joint7", "joint8"] RIGHT_GRIPPER_JOINTS = ["joint7l", "joint8l"] # Jacobian anchor is the wrist link; IK pose target is the fingertip center (see below). RIGHT_EE_BODY = "link6l" @@ -124,8 +131,8 @@ def _deg(degrees: float) -> float: # Gripper finger targets (joint7: [-0.05, 0], joint8: [0, 0.05]) # Synchronized pair mimics single GL40 motor driving both fingers via linkage. -GRIPPER_OPEN = {"joint7l": -0.05, "joint8l": 0.05} -GRIPPER_CLOSED = {"joint7l": 0.0, "joint8l": 0.0} +GRIPPER_OPEN = {"joint7l": -0.05, "joint8l": 0.05, "joint7": -0.05, "joint8": 0.05} +GRIPPER_CLOSED = {"joint7l": 0.0, "joint8l": 0.0, "joint7": 0.0, "joint8": 0.0} # 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.