IK Deployment

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

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};
 };

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()

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<common_msgs::msg::MotorCmd> 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_);