From 3a867fbd4f57b247e18d7a7b32004e42616686e0 Mon Sep 17 00:00:00 2001 From: nitish2006main Date: Wed, 16 Sep 2026 22:35:26 -0400 Subject: [PATCH 01/11] In progress script for Task 280, creating config_loader script --- .gitignore | 1 + src/simulation/real2sim_mirror/config_loader.py | 6 ++++++ 2 files changed, 7 insertions(+) create mode 100644 src/simulation/real2sim_mirror/config_loader.py diff --git a/.gitignore b/.gitignore index e1c8f3c3..9b91f27a 100644 --- a/.gitignore +++ b/.gitignore @@ -34,3 +34,4 @@ src/teleop/quest_teleop/static/marker_uv.json src/simulation/garment_fold_task/ src/simulation/humanoid_rl/logs/ src/simulation/humanoid_rl/outputs/ +venv/ diff --git a/src/simulation/real2sim_mirror/config_loader.py b/src/simulation/real2sim_mirror/config_loader.py new file mode 100644 index 00000000..9372d6a9 --- /dev/null +++ b/src/simulation/real2sim_mirror/config_loader.py @@ -0,0 +1,6 @@ +import yaml + +with open("src/interfacing/joint_command/config/hardware_mapping.yaml", "r") as f: + data = yaml.safe_load(f) + +print(data) \ No newline at end of file From 8e67cddab2ffe01e265eb674f4043f6433fd7aa1 Mon Sep 17 00:00:00 2001 From: nitish2006main Date: Wed, 16 Sep 2026 23:00:31 -0400 Subject: [PATCH 02/11] Implemented hardware mapping lookup table for direct CAN id to joint configuration access --- .../real2sim_mirror/config_loader.py | 22 ++++++++++++++++--- 1 file changed, 19 insertions(+), 3 deletions(-) diff --git a/src/simulation/real2sim_mirror/config_loader.py b/src/simulation/real2sim_mirror/config_loader.py index 9372d6a9..80b98f15 100644 --- a/src/simulation/real2sim_mirror/config_loader.py +++ b/src/simulation/real2sim_mirror/config_loader.py @@ -1,6 +1,22 @@ import yaml -with open("src/interfacing/joint_command/config/hardware_mapping.yaml", "r") as f: - data = yaml.safe_load(f) +def load_hardware_mapping(yaml_file_path): + with open(yaml_file_path, "r") as f: + data = yaml.safe_load(f) -print(data) \ No newline at end of file + lookup_table = {} + + for side, limbs in data.items(): + for limb, joints in limbs.items(): + for joint_type, config in joints.items(): + joint_name = f"{side}_{limb}_{joint_type}" + entry = config.copy() + entry["joint_name"] = joint_name + lookup_table[config["can_id"]] = entry + + return lookup_table + +if __name__ == "__main__": + yaml_file_path = "src/interfacing/joint_command/config/hardware_mapping.yaml" + lookup_table = load_hardware_mapping(yaml_file_path) + print(lookup_table) From 6d21aeb744dd71ea005e8bc6b783dfb910d9a80f Mon Sep 17 00:00:00 2001 From: nitish2006main Date: Thu, 17 Sep 2026 18:07:11 -0400 Subject: [PATCH 03/11] Added ROS2 subscriber node for motor feedback: Task #280 --- src/simulation/real2sim_mirror/__init__.py | 0 .../real2sim_mirror/config_loader.py | 22 +++++++++++++++ .../real2sim_mirror/real2sim_mirror_node.py | 27 +++++++++++++++++++ 3 files changed, 49 insertions(+) create mode 100644 src/simulation/real2sim_mirror/__init__.py create mode 100644 src/simulation/real2sim_mirror/real2sim_mirror_node.py diff --git a/src/simulation/real2sim_mirror/__init__.py b/src/simulation/real2sim_mirror/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/simulation/real2sim_mirror/config_loader.py b/src/simulation/real2sim_mirror/config_loader.py index 80b98f15..779a4cf2 100644 --- a/src/simulation/real2sim_mirror/config_loader.py +++ b/src/simulation/real2sim_mirror/config_loader.py @@ -1,5 +1,7 @@ import yaml +import math +#creating a lookup table for easy access def load_hardware_mapping(yaml_file_path): with open(yaml_file_path, "r") as f: data = yaml.safe_load(f) @@ -16,7 +18,27 @@ def load_hardware_mapping(yaml_file_path): return lookup_table +#angle computation function so rad angles can be sent to mjlabs +def angle_computation(motor_id, raw_position, lookup): + config = lookup[motor_id] + zero_offset = config["zero_offset"] + direction = config["direction"] + + true_angle_deg = (raw_position - zero_offset) * direction + true_angle_rad = math.radians(true_angle_deg) + + if motor_id not in lookup: + print (f"Warning: Unknown Motor ID {motor_id}") + return None + + return true_angle_rad + +def + if __name__ == "__main__": yaml_file_path = "src/interfacing/joint_command/config/hardware_mapping.yaml" lookup_table = load_hardware_mapping(yaml_file_path) print(lookup_table) + + test_angle = angle_computation(14, 1900, lookup_table) + print(test_angle) diff --git a/src/simulation/real2sim_mirror/real2sim_mirror_node.py b/src/simulation/real2sim_mirror/real2sim_mirror_node.py new file mode 100644 index 00000000..85fda027 --- /dev/null +++ b/src/simulation/real2sim_mirror/real2sim_mirror_node.py @@ -0,0 +1,27 @@ +import rclpy +from rclpy.node import Node +from common_msgs.msg import MotorFeedback +from config_loader import load_hardware_mapping, angle_computation + +class Real2SimMirrorNode(Node): + def __init__(self): + super().__init__("real2sim_mirror_node") + self.subscription = self.create_subscription( + MotorFeedback, + "/interfacing/motorFeedback", + self.feedback_callback, + 10 + ) + self.lookup_table = load_hardware_mapping("src/interfacing/joint_command/config/hardware_mapping.yaml") + + def feedback_callback(self, msg): + motor_id = msg.motor_id + raw_position = msg.position + angle = angle_computation(motor_id, raw_position, self.lookup_table) + print(f"{msg.motor_id}: {angle} rad") + +if __name__ == "__main__": + rclpy.init() + node = Real2SimMirrorNode() + rclpy.spin(node) + rclpy.shutdown() \ No newline at end of file From aa009c4e68d9945abd060027b45a6f76c773c09f Mon Sep 17 00:00:00 2001 From: nitish2006main Date: Thu, 17 Sep 2026 18:19:47 -0400 Subject: [PATCH 04/11] Update gitignore --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index 9b91f27a..aaf8b269 100644 --- a/.gitignore +++ b/.gitignore @@ -35,3 +35,4 @@ src/simulation/garment_fold_task/ src/simulation/humanoid_rl/logs/ src/simulation/humanoid_rl/outputs/ venv/ +venv/ From 8e2c1c57094e02f780738373a0990246f724529e Mon Sep 17 00:00:00 2001 From: nitish2006main Date: Thu, 17 Sep 2026 21:48:55 -0400 Subject: [PATCH 05/11] Changed yaml file path in config_loader and real2sim_mirror_node, Task #280 --- modules/docker-compose.interfacing.yaml | 3 ++- modules/docker-compose.simulation_mj.yaml | 18 +++++++++--------- .../real2sim_mirror/config_loader.py | 12 +++++------- .../real2sim_mirror/real2sim_mirror_node.py | 2 +- 4 files changed, 17 insertions(+), 18 deletions(-) diff --git a/modules/docker-compose.interfacing.yaml b/modules/docker-compose.interfacing.yaml index 6a00f5fc..a30a9691 100644 --- a/modules/docker-compose.interfacing.yaml +++ b/modules/docker-compose.interfacing.yaml @@ -12,7 +12,8 @@ services: - "${INTERFACING_IMAGE:?}:${TAG}" - "${INTERFACING_IMAGE:?}:main" image: "${INTERFACING_IMAGE:?}:${TAG}" - command: /bin/bash -c "ros2 launch can can.launch.py" + entrypoint: ./wato_ros_entrypoint.sh + command: -c "source /opt/watonomous/setup.bash && exec ros2 launch can can.launch.py" privileged: true network_mode: host # Allow the container to access the host's network interfaces cap_add: # Grant the capability to configure network interfaces diff --git a/modules/docker-compose.simulation_mj.yaml b/modules/docker-compose.simulation_mj.yaml index 7fe56aa3..a09e91ea 100644 --- a/modules/docker-compose.simulation_mj.yaml +++ b/modules/docker-compose.simulation_mj.yaml @@ -17,18 +17,18 @@ services: ports: - "8080:8080" environment: - - NVIDIA_VISIBLE_DEVICES=all - - NVIDIA_DRIVER_CAPABILITIES=compute,utility,graphics + # - NVIDIA_VISIBLE_DEVICES=all + # - NVIDIA_DRIVER_CAPABILITIES=compute,utility,graphics - ROS_DOMAIN_ID=0 - FASTDDS_BUILTIN_TRANSPORTS=UDPv4 command: sleep infinity - deploy: - resources: - reservations: - devices: - - driver: nvidia - count: 1 - capabilities: [gpu] + # deploy: + # resources: + # reservations: + # devices: + # - driver: nvidia + # count: 1 + # capabilities: [gpu] volumes: - ${MONO_DIR}/src/simulation:/root/ament_ws/src/simulation - ${MONO_DIR}/src/teleop:/root/ament_ws/src/teleop diff --git a/src/simulation/real2sim_mirror/config_loader.py b/src/simulation/real2sim_mirror/config_loader.py index 779a4cf2..f6124d2f 100644 --- a/src/simulation/real2sim_mirror/config_loader.py +++ b/src/simulation/real2sim_mirror/config_loader.py @@ -20,6 +20,10 @@ def load_hardware_mapping(yaml_file_path): #angle computation function so rad angles can be sent to mjlabs def angle_computation(motor_id, raw_position, lookup): + if motor_id not in lookup: + print (f"Warning: Unknown Motor ID {motor_id}") + return None + config = lookup[motor_id] zero_offset = config["zero_offset"] direction = config["direction"] @@ -27,16 +31,10 @@ def angle_computation(motor_id, raw_position, lookup): true_angle_deg = (raw_position - zero_offset) * direction true_angle_rad = math.radians(true_angle_deg) - if motor_id not in lookup: - print (f"Warning: Unknown Motor ID {motor_id}") - return None - return true_angle_rad -def - if __name__ == "__main__": - yaml_file_path = "src/interfacing/joint_command/config/hardware_mapping.yaml" + yaml_file_path = "src/joint_command/config/hardware_mapping.yaml" lookup_table = load_hardware_mapping(yaml_file_path) print(lookup_table) diff --git a/src/simulation/real2sim_mirror/real2sim_mirror_node.py b/src/simulation/real2sim_mirror/real2sim_mirror_node.py index 85fda027..aeeda9cf 100644 --- a/src/simulation/real2sim_mirror/real2sim_mirror_node.py +++ b/src/simulation/real2sim_mirror/real2sim_mirror_node.py @@ -12,7 +12,7 @@ def __init__(self): self.feedback_callback, 10 ) - self.lookup_table = load_hardware_mapping("src/interfacing/joint_command/config/hardware_mapping.yaml") + self.lookup_table = load_hardware_mapping("src/joint_command/config/hardware_mapping.yaml") def feedback_callback(self, msg): motor_id = msg.motor_id From fff030378c245fee7dad694230754b3d2d4e9a08 Mon Sep 17 00:00:00 2001 From: nitish2006main Date: Thu, 17 Sep 2026 22:12:29 -0400 Subject: [PATCH 06/11] Variable name changes: Task 280 --- src/simulation/real2sim_mirror/config_loader.py | 4 ++-- src/simulation/real2sim_mirror/real2sim_mirror_node.py | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/simulation/real2sim_mirror/config_loader.py b/src/simulation/real2sim_mirror/config_loader.py index f6124d2f..4d32b73a 100644 --- a/src/simulation/real2sim_mirror/config_loader.py +++ b/src/simulation/real2sim_mirror/config_loader.py @@ -19,7 +19,7 @@ def load_hardware_mapping(yaml_file_path): return lookup_table #angle computation function so rad angles can be sent to mjlabs -def angle_computation(motor_id, raw_position, lookup): +def angle_computation(motor_id, position, lookup): if motor_id not in lookup: print (f"Warning: Unknown Motor ID {motor_id}") return None @@ -28,7 +28,7 @@ def angle_computation(motor_id, raw_position, lookup): zero_offset = config["zero_offset"] direction = config["direction"] - true_angle_deg = (raw_position - zero_offset) * direction + true_angle_deg = (position - zero_offset) * direction true_angle_rad = math.radians(true_angle_deg) return true_angle_rad diff --git a/src/simulation/real2sim_mirror/real2sim_mirror_node.py b/src/simulation/real2sim_mirror/real2sim_mirror_node.py index aeeda9cf..c793a61c 100644 --- a/src/simulation/real2sim_mirror/real2sim_mirror_node.py +++ b/src/simulation/real2sim_mirror/real2sim_mirror_node.py @@ -16,8 +16,8 @@ def __init__(self): def feedback_callback(self, msg): motor_id = msg.motor_id - raw_position = msg.position - angle = angle_computation(motor_id, raw_position, self.lookup_table) + position = msg.position + angle = angle_computation(motor_id, position, self.lookup_table) print(f"{msg.motor_id}: {angle} rad") if __name__ == "__main__": From 4c7a18bba130a09728ff2722898af7f2b78b2696 Mon Sep 17 00:00:00 2001 From: nitish2006main Date: Thu, 17 Sep 2026 22:37:22 -0400 Subject: [PATCH 07/11] Added MJlabs setup for the real2sim mirror node. Visualization script is almost done. Task 280 --- .../real2sim_mirror/real2sim_mirror_node.py | 32 ++++++++++++++++--- 1 file changed, 28 insertions(+), 4 deletions(-) diff --git a/src/simulation/real2sim_mirror/real2sim_mirror_node.py b/src/simulation/real2sim_mirror/real2sim_mirror_node.py index c793a61c..ac5d382b 100644 --- a/src/simulation/real2sim_mirror/real2sim_mirror_node.py +++ b/src/simulation/real2sim_mirror/real2sim_mirror_node.py @@ -2,6 +2,7 @@ from rclpy.node import Node from common_msgs.msg import MotorFeedback from config_loader import load_hardware_mapping, angle_computation +import mujoco class Real2SimMirrorNode(Node): def __init__(self): @@ -14,14 +15,37 @@ def __init__(self): ) self.lookup_table = load_hardware_mapping("src/joint_command/config/hardware_mapping.yaml") + self.model = mujoco.MjModel.from_xml_path("src/simulation/real2sim_mirror/mirror.xml") + self.data = mujoco.MjData(self.model) + def feedback_callback(self, msg): motor_id = msg.motor_id position = msg.position - angle = angle_computation(motor_id, position, self.lookup_table) - print(f"{msg.motor_id}: {angle} rad") + + angle_rad = angle_computation(motor_id, position, self.lookup_table) + + if angle_rad is None: + return + + joint_name = self.lookup_table[motor_id]["joint_name"] + joint_id = mujoco.mj_name2id( + self.model, + mujoco.mjtObj.mjOBJ_JOINT, + joint_name + ) + + qpos_addr = self.model.jnt.qposadr[joint_id] + self.data.qpos[qpos_addr] = angle_rad + + mujoco.mj_forward(self.model, self.data) + if __name__ == "__main__": rclpy.init() node = Real2SimMirrorNode() - rclpy.spin(node) - rclpy.shutdown() \ No newline at end of file + + try: + rclpy.spin(node) + finally: + node.destroy_node() + rclpy.shutdown() \ No newline at end of file From 838eb8802df7d3d2aec03ec6999435c24262c928 Mon Sep 17 00:00:00 2001 From: nitish2006main Date: Fri, 18 Sep 2026 10:44:13 -0400 Subject: [PATCH 08/11] Removed the 2 python scripts for visualization, and merged it into one. Tested the mirror angle conversion in python, need to implement visualization once correct model is received. Task 280 --- .../arm_visualization_mirror.py | 138 ++++++++++++++++++ .../real2sim_mirror/config_loader.py | 42 ------ .../real2sim_mirror/real2sim_mirror_node.py | 51 ------- 3 files changed, 138 insertions(+), 93 deletions(-) create mode 100644 src/simulation/real2sim_mirror/arm_visualization_mirror.py delete mode 100644 src/simulation/real2sim_mirror/config_loader.py delete mode 100644 src/simulation/real2sim_mirror/real2sim_mirror_node.py diff --git a/src/simulation/real2sim_mirror/arm_visualization_mirror.py b/src/simulation/real2sim_mirror/arm_visualization_mirror.py new file mode 100644 index 00000000..c33d7033 --- /dev/null +++ b/src/simulation/real2sim_mirror/arm_visualization_mirror.py @@ -0,0 +1,138 @@ +import math +import yaml + +import rclpy +from rclpy.node import Node +from common_msgs.msg import MotorFeedback + + +class Real2SimMirrorNode(Node): + def __init__(self): + super().__init__("real2sim_mirror_node") + + self.subscription = self.create_subscription( + MotorFeedback, + "/interfacing/motorFeedback", + self.feedback_callback, + 10, + ) + + self.lookup_table = self.load_hardware_mapping( + "src/joint_command/config/hardware_mapping.yaml" + ) + + # +1 = same direction, -1 = mirrored direction + # These are temporary until we verify the MuJoCo joint axes. + self.mirror_directions = { + "shoulder_pitch": 1, + "shoulder_roll": -1, + "shoulder_yaw": 1, + "elbow_pitch": 1, + "elbow_roll": -1, + "wrist_pitch": 1, + } + + def load_hardware_mapping(self, yaml_file_path): + with open(yaml_file_path, "r") as f: + data = yaml.safe_load(f) + + lookup_table = {} + + for side, limbs in data.items(): + for limb, joints in limbs.items(): + for joint_type, config in joints.items(): + joint_name = f"{side}_{limb}_{joint_type}" + + entry = config.copy() + entry["joint_name"] = joint_name + + lookup_table[config["can_id"]] = entry + + return lookup_table + + def angle_computation(self, motor_id, position_deg): + if motor_id not in self.lookup_table: + self.get_logger().warn( + f"Unknown Motor ID {motor_id}" + ) + return None + + config = self.lookup_table[motor_id] + + true_angle_deg = ( + position_deg - config["zero_offset"] + ) * config["direction"] + + return math.radians(true_angle_deg) + + def mirror_angle(self, angle_rad, joint_name): + joint_type = "_".join(joint_name.split("_")[-1]) + direction = self.mirror_directions[joint_type] + + return angle_rad * direction + + def feedback_callback(self, msg): + motor_id = msg.motor_id + position_deg = msg.position + + angle_rad = self.angle_computation( + motor_id, + position_deg, + ) + + if angle_rad is None: + return + + joint_name = self.lookup_table[motor_id]["joint_name"] + + mirrored_angle_rad = self.mirror_angle( + angle_rad, + joint_name, + ) + + self.get_logger().info( + f"{joint_name}: " + f"{angle_rad:.3f} rad -> " + f"{mirrored_angle_rad:.3f} rad" + ) + +def test(): + lookup_table = Real2SimMirrorNode.load_hardware_mapping( + None, + "src/joint_command/config/hardware_mapping.yaml" + ) + + test_motor_id = 12 + test_position_deg = -80.4 + + config = lookup_table[test_motor_id] + + true_angle_deg = ( + test_position_deg - config["zero_offset"] + ) * config["direction"] + + true_angle_rad = math.radians(true_angle_deg) + + joint_name = config["joint_name"] + + joint_type = "_".join(joint_name.split("_")[1:]) + + mirror_direction = { + "shoulder_pitch": 1, + "shoulder_roll": -1, + "shoulder_yaw": -1, + "elbow_pitch": 1, + "elbow_roll": -1, + "wrist_pitch": 1, + }[joint_type] + + mirrored_angle_rad = true_angle_rad * mirror_direction + + print(f"Joint: {joint_name}") + print(f"Position: {test_position_deg} deg") + print(f"True angle: {true_angle_rad:.3f} rad") + print(f"Mirrored angle: {mirrored_angle_rad:.3f} rad") + + +if __name__ == "__main__": + test() \ No newline at end of file diff --git a/src/simulation/real2sim_mirror/config_loader.py b/src/simulation/real2sim_mirror/config_loader.py deleted file mode 100644 index 4d32b73a..00000000 --- a/src/simulation/real2sim_mirror/config_loader.py +++ /dev/null @@ -1,42 +0,0 @@ -import yaml -import math - -#creating a lookup table for easy access -def load_hardware_mapping(yaml_file_path): - with open(yaml_file_path, "r") as f: - data = yaml.safe_load(f) - - lookup_table = {} - - for side, limbs in data.items(): - for limb, joints in limbs.items(): - for joint_type, config in joints.items(): - joint_name = f"{side}_{limb}_{joint_type}" - entry = config.copy() - entry["joint_name"] = joint_name - lookup_table[config["can_id"]] = entry - - return lookup_table - -#angle computation function so rad angles can be sent to mjlabs -def angle_computation(motor_id, position, lookup): - if motor_id not in lookup: - print (f"Warning: Unknown Motor ID {motor_id}") - return None - - config = lookup[motor_id] - zero_offset = config["zero_offset"] - direction = config["direction"] - - true_angle_deg = (position - zero_offset) * direction - true_angle_rad = math.radians(true_angle_deg) - - return true_angle_rad - -if __name__ == "__main__": - yaml_file_path = "src/joint_command/config/hardware_mapping.yaml" - lookup_table = load_hardware_mapping(yaml_file_path) - print(lookup_table) - - test_angle = angle_computation(14, 1900, lookup_table) - print(test_angle) diff --git a/src/simulation/real2sim_mirror/real2sim_mirror_node.py b/src/simulation/real2sim_mirror/real2sim_mirror_node.py deleted file mode 100644 index ac5d382b..00000000 --- a/src/simulation/real2sim_mirror/real2sim_mirror_node.py +++ /dev/null @@ -1,51 +0,0 @@ -import rclpy -from rclpy.node import Node -from common_msgs.msg import MotorFeedback -from config_loader import load_hardware_mapping, angle_computation -import mujoco - -class Real2SimMirrorNode(Node): - def __init__(self): - super().__init__("real2sim_mirror_node") - self.subscription = self.create_subscription( - MotorFeedback, - "/interfacing/motorFeedback", - self.feedback_callback, - 10 - ) - self.lookup_table = load_hardware_mapping("src/joint_command/config/hardware_mapping.yaml") - - self.model = mujoco.MjModel.from_xml_path("src/simulation/real2sim_mirror/mirror.xml") - self.data = mujoco.MjData(self.model) - - def feedback_callback(self, msg): - motor_id = msg.motor_id - position = msg.position - - angle_rad = angle_computation(motor_id, position, self.lookup_table) - - if angle_rad is None: - return - - joint_name = self.lookup_table[motor_id]["joint_name"] - joint_id = mujoco.mj_name2id( - self.model, - mujoco.mjtObj.mjOBJ_JOINT, - joint_name - ) - - qpos_addr = self.model.jnt.qposadr[joint_id] - self.data.qpos[qpos_addr] = angle_rad - - mujoco.mj_forward(self.model, self.data) - - -if __name__ == "__main__": - rclpy.init() - node = Real2SimMirrorNode() - - try: - rclpy.spin(node) - finally: - node.destroy_node() - rclpy.shutdown() \ No newline at end of file From f28a0f1fcedb1809a680aac8077bd6b352bad7c8 Mon Sep 17 00:00:00 2001 From: nitish2006main Date: Fri, 18 Sep 2026 13:33:28 -0400 Subject: [PATCH 09/11] Mujoco Visualization with proper mirrored arm movement. Will need xml model for mjlabs, then temporary code can be removed. Task 280 --- modules/docker-compose.simulation_mj.yaml | 11 +- .../arm_visualization_mirror.py | 235 ++++++++++++++---- 2 files changed, 197 insertions(+), 49 deletions(-) diff --git a/modules/docker-compose.simulation_mj.yaml b/modules/docker-compose.simulation_mj.yaml index a09e91ea..6d0516b4 100644 --- a/modules/docker-compose.simulation_mj.yaml +++ b/modules/docker-compose.simulation_mj.yaml @@ -17,10 +17,15 @@ services: ports: - "8080:8080" environment: + #commented the two below since I (Nitish) don't have a GPU and can't test them. Uncomment if you have a GPU and want to use it. # - NVIDIA_VISIBLE_DEVICES=all # - NVIDIA_DRIVER_CAPABILITIES=compute,utility,graphics - ROS_DOMAIN_ID=0 - FASTDDS_BUILTIN_TRANSPORTS=UDPv4 + #added the below three for mujoco visualization + - DISPLAY=${DISPLAY} + - WAYLAND_DISPLAY=${WAYLAND_DISPLAY} + - XDG_RUNTIME_DIR=/tmp/runtime-nitish command: sleep infinity # deploy: # resources: @@ -33,4 +38,8 @@ services: - ${MONO_DIR}/src/simulation:/root/ament_ws/src/simulation - ${MONO_DIR}/src/teleop:/root/ament_ws/src/teleop - ${MONO_DIR}/src/common_msgs:/root/ament_ws/src/common_msgs - - ${MONO_DIR}/src/interfacing/joint_command:/root/ament_ws/src/joint_command \ No newline at end of file + - ${MONO_DIR}/src/interfacing/joint_command:/root/ament_ws/src/joint_command + ##added the three below for mujoco visualization + - ${MONO_DIR}/assets:/root/ament_ws/assets:ro + - ${XDG_RUNTIME_DIR}/wayland-0:/tmp/runtime-nitish/wayland-0 + - /tmp/.X11-unix:/tmp/.X11-unix \ No newline at end of file diff --git a/src/simulation/real2sim_mirror/arm_visualization_mirror.py b/src/simulation/real2sim_mirror/arm_visualization_mirror.py index c33d7033..f6fef43e 100644 --- a/src/simulation/real2sim_mirror/arm_visualization_mirror.py +++ b/src/simulation/real2sim_mirror/arm_visualization_mirror.py @@ -1,5 +1,9 @@ import math +import os +import tempfile import yaml +import mujoco +import mujoco.viewer import rclpy from rclpy.node import Node @@ -9,29 +13,78 @@ class Real2SimMirrorNode(Node): def __init__(self): super().__init__("real2sim_mirror_node") + self.urdf_path = ( + "/root/ament_ws/assets/pioneer_bimanual_arm/" + "urdf/pioneer_bimanual_arm.urdf" + ) - self.subscription = self.create_subscription( - MotorFeedback, - "/interfacing/motorFeedback", - self.feedback_callback, - 10, + self.mesh_directory = ( + "/root/ament_ws/assets/pioneer_bimanual_arm/meshes" + ) + + self.hardware_mapping_path = ( + "/root/ament_ws/src/joint_command/" + "config/hardware_mapping.yaml" ) self.lookup_table = self.load_hardware_mapping( "src/joint_command/config/hardware_mapping.yaml" ) - # +1 = same direction, -1 = mirrored direction - # These are temporary until we verify the MuJoCo joint axes. + self.left_joint_names = { + "shoulder_pitch": "joint1L", + "shoulder_yaw": "joint2l", + "shoulder_roll": "joint3l", + "elbow_pitch": "joint4l", + "elbow_roll": "joint5l", + "wrist_pitch": "joint6l", + } + + self.right_joint_names = { + "shoulder_pitch": "joint1", + "shoulder_yaw": "joint2", + "shoulder_roll": "joint3", + "elbow_pitch": "joint4", + "elbow_roll": "joint5", + "wrist_pitch": "joint6", + } + self.mirror_directions = { - "shoulder_pitch": 1, + "shoulder_pitch": -1, "shoulder_roll": -1, - "shoulder_yaw": 1, - "elbow_pitch": 1, + "shoulder_yaw": -1, + "elbow_pitch": -1, "elbow_roll": -1, - "wrist_pitch": 1, + "wrist_pitch": -1, } + #temporary file to store the modified URDF for MuJoCo + self.mujoco_urdf_path = self.create_mujoco_urdf() + + self.model = mujoco.MjModel.from_xml_path( + self.mujoco_urdf_path + ) + # Initialize MuJoCo data structure + self.data = mujoco.MjData(self.model) + + self.left_qpos = {} + self.right_qpos = {} + + self.setup_joint_indices() + + + self.subscription = self.create_subscription( + MotorFeedback, + "/interfacing/motorFeedback", + self.feedback_callback, + 10, + ) + + self.get_logger().info( + "Real2Sim mirror visualization node started." + ) + + #Converts the hardware mapping YAML file into a lookup table for easy access def load_hardware_mapping(self, yaml_file_path): with open(yaml_file_path, "r") as f: data = yaml.safe_load(f) @@ -49,6 +102,75 @@ def load_hardware_mapping(self, yaml_file_path): lookup_table[config["can_id"]] = entry return lookup_table + #Temporary function for Mujoco urdf simulation. + def create_mujoco_urdf(self): + """ + The original URDF uses ROS package:// mesh paths. + + MuJoCo does not resolve those ROS package paths, so create + a temporary copy with the mesh paths replaced by the actual + mounted mesh directory. + """ + + with open(self.urdf_path, "r") as f: + urdf = f.read() + + urdf = urdf.replace( + "package://armv2URDF/meshes/", + self.mesh_directory + "/", + ) + + temp_file = tempfile.NamedTemporaryFile( + mode="w", + suffix=".urdf", + delete=False, + ) + + temp_file.write(urdf) + temp_file.close() + + self.get_logger().info( + f"Created MuJoCo-readable URDF: {temp_file.name}" + ) + + return temp_file.name + #Temporary function for Mujoco urdf simulation. + def setup_joint_indices(self): + for joint_type, joint_name in self.left_joint_names.items(): + joint_id = mujoco.mj_name2id( + self.model, + mujoco.mjtObj.mjOBJ_JOINT, + joint_name, + ) + + if joint_id == -1: + raise RuntimeError( + f"Could not find MuJoCo joint: {joint_name}" + ) + + self.left_qpos[joint_type] = self.model.jnt_qposadr[joint_id] + + for joint_type, joint_name in self.right_joint_names.items(): + joint_id = mujoco.mj_name2id( + self.model, + mujoco.mjtObj.mjOBJ_JOINT, + joint_name, + ) + + if joint_id == -1: + raise RuntimeError( + f"Could not find MuJoCo joint: {joint_name}" + ) + + self.right_qpos[joint_type] = self.model.jnt_qposadr[joint_id] + + self.get_logger().info( + f"Left qpos indices: {self.left_qpos}" + ) + + self.get_logger().info( + f"Right qpos indices: {self.right_qpos}" + ) def angle_computation(self, motor_id, position_deg): if motor_id not in self.lookup_table: @@ -66,7 +188,7 @@ def angle_computation(self, motor_id, position_deg): return math.radians(true_angle_deg) def mirror_angle(self, angle_rad, joint_name): - joint_type = "_".join(joint_name.split("_")[-1]) + joint_type = "_".join(joint_name.split("_")[1:]) direction = self.mirror_directions[joint_type] return angle_rad * direction @@ -85,10 +207,30 @@ def feedback_callback(self, msg): joint_name = self.lookup_table[motor_id]["joint_name"] - mirrored_angle_rad = self.mirror_angle( - angle_rad, - joint_name, + if not joint_name.startswith("left_"): + return + + joint_type = "_".join(joint_name.split("_")[1:]) + + if joint_type not in self.left_qpos: + return + + ##Temporary code for Mujoco urdf simulation. + left_qpos_index = self.left_qpos[joint_type] + self.data.qpos[left_qpos_index] = angle_rad + ## + + mirrored_angle_rad = self.mirror_angle(angle_rad, joint_name) + + ##Temporary code for Mujoco urdf simulation. + right_qpos_index = self.right_qpos[joint_type] + self.data.qpos[right_qpos_index] = mirrored_angle_rad + # Update MuJoCo forward kinematics. + mujoco.mj_forward( + self.model, + self.data, ) + ## self.get_logger().info( f"{joint_name}: " @@ -96,43 +238,40 @@ def feedback_callback(self, msg): f"{mirrored_angle_rad:.3f} rad" ) -def test(): - lookup_table = Real2SimMirrorNode.load_hardware_mapping( - None, - "src/joint_command/config/hardware_mapping.yaml" - ) - - test_motor_id = 12 - test_position_deg = -80.4 - - config = lookup_table[test_motor_id] - - true_angle_deg = ( - test_position_deg - config["zero_offset"] - ) * config["direction"] - - true_angle_rad = math.radians(true_angle_deg) - - joint_name = config["joint_name"] +def main(): + rclpy.init() + + node = Real2SimMirrorNode() + #temporary code for Mujoco urdf simulation, will remove once xml model is found for mjlabs + try: + # ------------------------------------------------------------ + # Start MuJoCo viewer + # ------------------------------------------------------------ + + with mujoco.viewer.launch_passive( + node.model, + node.data, + ) as viewer: + + node.get_logger().info( + "MuJoCo viewer started." + ) - joint_type = "_".join(joint_name.split("_")[1:]) + while rclpy.ok() and viewer.is_running(): + rclpy.spin_once( + node, + timeout_sec=0.01, + ) - mirror_direction = { - "shoulder_pitch": 1, - "shoulder_roll": -1, - "shoulder_yaw": -1, - "elbow_pitch": 1, - "elbow_roll": -1, - "wrist_pitch": 1, - }[joint_type] + viewer.sync() - mirrored_angle_rad = true_angle_rad * mirror_direction + except KeyboardInterrupt: + pass - print(f"Joint: {joint_name}") - print(f"Position: {test_position_deg} deg") - print(f"True angle: {true_angle_rad:.3f} rad") - print(f"Mirrored angle: {mirrored_angle_rad:.3f} rad") + finally: + node.destroy_node() + rclpy.shutdown() if __name__ == "__main__": - test() \ No newline at end of file + main() \ No newline at end of file From e18c51538d29744e76159f49fee96dd9bb9054fa Mon Sep 17 00:00:00 2001 From: nitish2006main Date: Fri, 18 Sep 2026 16:26:32 -0400 Subject: [PATCH 10/11] Used MJViser for real2sim arm mirror visualization, along with creating the .xml model for MJViser --- docker/simulation/mjlabs/mjlabs.Dockerfile | 11 +- models/robot_mjcf.xml | 145 ++++++++++++++++++ modules/docker-compose.simulation_mj.yaml | 2 + .../arm_visualization_mirror.py | 96 ++++-------- 4 files changed, 183 insertions(+), 71 deletions(-) create mode 100644 models/robot_mjcf.xml diff --git a/docker/simulation/mjlabs/mjlabs.Dockerfile b/docker/simulation/mjlabs/mjlabs.Dockerfile index 45fe566d..c6f9ba94 100644 --- a/docker/simulation/mjlabs/mjlabs.Dockerfile +++ b/docker/simulation/mjlabs/mjlabs.Dockerfile @@ -26,8 +26,17 @@ ENV AMENT_WS=/root/ament_ws # Install Rosdep requirements COPY --from=source /tmp/colcon_install_list /tmp/colcon_install_list +#RUN apt-get update -qq && \ + #apt-get install -qq -y --no-install-recommends $(cat /tmp/colcon_install_list) || true + +#Install OpenGL/EGL libraries required for Mujoco and MJViser rendering RUN apt-get update -qq && \ - apt-get install -qq -y --no-install-recommends $(cat /tmp/colcon_install_list) || true + apt-get install -qq -y --no-install-recommends \ + $(cat /tmp/colcon_install_list) \ + libgl1 \ + libglx0 \ + libegl1 \ + || true # Copy in source code from source stage WORKDIR ${AMENT_WS} diff --git a/models/robot_mjcf.xml b/models/robot_mjcf.xml new file mode 100644 index 00000000..39e8aa63 --- /dev/null +++ b/models/robot_mjcf.xml @@ -0,0 +1,145 @@ + + + diff --git a/modules/docker-compose.simulation_mj.yaml b/modules/docker-compose.simulation_mj.yaml index 6d0516b4..0325c565 100644 --- a/modules/docker-compose.simulation_mj.yaml +++ b/modules/docker-compose.simulation_mj.yaml @@ -39,6 +39,8 @@ services: - ${MONO_DIR}/src/teleop:/root/ament_ws/src/teleop - ${MONO_DIR}/src/common_msgs:/root/ament_ws/src/common_msgs - ${MONO_DIR}/src/interfacing/joint_command:/root/ament_ws/src/joint_command + #so that the models folder is mounted into mjlabs docker container + - ${MONO_DIR}/models:/root/ament_ws/models:ro ##added the three below for mujoco visualization - ${MONO_DIR}/assets:/root/ament_ws/assets:ro - ${XDG_RUNTIME_DIR}/wayland-0:/tmp/runtime-nitish/wayland-0 diff --git a/src/simulation/real2sim_mirror/arm_visualization_mirror.py b/src/simulation/real2sim_mirror/arm_visualization_mirror.py index f6fef43e..899aaf67 100644 --- a/src/simulation/real2sim_mirror/arm_visualization_mirror.py +++ b/src/simulation/real2sim_mirror/arm_visualization_mirror.py @@ -1,11 +1,10 @@ import math -import os -import tempfile import yaml import mujoco -import mujoco.viewer - import rclpy +import viser + +from mjviser import ViserMujocoScene from rclpy.node import Node from common_msgs.msg import MotorFeedback @@ -13,23 +12,13 @@ class Real2SimMirrorNode(Node): def __init__(self): super().__init__("real2sim_mirror_node") - self.urdf_path = ( - "/root/ament_ws/assets/pioneer_bimanual_arm/" - "urdf/pioneer_bimanual_arm.urdf" - ) - - self.mesh_directory = ( - "/root/ament_ws/assets/pioneer_bimanual_arm/meshes" - ) self.hardware_mapping_path = ( "/root/ament_ws/src/joint_command/" "config/hardware_mapping.yaml" ) - self.lookup_table = self.load_hardware_mapping( - "src/joint_command/config/hardware_mapping.yaml" - ) + self.lookup_table = self.load_hardware_mapping(self.hardware_mapping_path) self.left_joint_names = { "shoulder_pitch": "joint1L", @@ -58,14 +47,15 @@ def __init__(self): "wrist_pitch": -1, } - #temporary file to store the modified URDF for MuJoCo - self.mujoco_urdf_path = self.create_mujoco_urdf() + self.mjcf_path = "/root/ament_ws/models/robot_mjcf.xml" self.model = mujoco.MjModel.from_xml_path( - self.mujoco_urdf_path + self.mjcf_path ) + # Initialize MuJoCo data structure self.data = mujoco.MjData(self.model) + mujoco.mj_forward(self.model, self.data) self.left_qpos = {} self.right_qpos = {} @@ -102,39 +92,7 @@ def load_hardware_mapping(self, yaml_file_path): lookup_table[config["can_id"]] = entry return lookup_table - #Temporary function for Mujoco urdf simulation. - def create_mujoco_urdf(self): - """ - The original URDF uses ROS package:// mesh paths. - - MuJoCo does not resolve those ROS package paths, so create - a temporary copy with the mesh paths replaced by the actual - mounted mesh directory. - """ - - with open(self.urdf_path, "r") as f: - urdf = f.read() - - urdf = urdf.replace( - "package://armv2URDF/meshes/", - self.mesh_directory + "/", - ) - - temp_file = tempfile.NamedTemporaryFile( - mode="w", - suffix=".urdf", - delete=False, - ) - temp_file.write(urdf) - temp_file.close() - - self.get_logger().info( - f"Created MuJoCo-readable URDF: {temp_file.name}" - ) - - return temp_file.name - #Temporary function for Mujoco urdf simulation. def setup_joint_indices(self): for joint_type, joint_name in self.left_joint_names.items(): joint_id = mujoco.mj_name2id( @@ -215,14 +173,11 @@ def feedback_callback(self, msg): if joint_type not in self.left_qpos: return - ##Temporary code for Mujoco urdf simulation. left_qpos_index = self.left_qpos[joint_type] self.data.qpos[left_qpos_index] = angle_rad - ## mirrored_angle_rad = self.mirror_angle(angle_rad, joint_name) - ##Temporary code for Mujoco urdf simulation. right_qpos_index = self.right_qpos[joint_type] self.data.qpos[right_qpos_index] = mirrored_angle_rad # Update MuJoCo forward kinematics. @@ -230,7 +185,6 @@ def feedback_callback(self, msg): self.model, self.data, ) - ## self.get_logger().info( f"{joint_name}: " @@ -242,28 +196,30 @@ def main(): rclpy.init() node = Real2SimMirrorNode() - #temporary code for Mujoco urdf simulation, will remove once xml model is found for mjlabs try: - # ------------------------------------------------------------ - # Start MuJoCo viewer - # ------------------------------------------------------------ + server = viser.ViserServer(port=8080) - with mujoco.viewer.launch_passive( + scene = ViserMujocoScene( + server, node.model, - node.data, - ) as viewer: + num_envs=1, + ) - node.get_logger().info( - "MuJoCo viewer started." - ) + scene.create_visualization_gui() + + node.get_logger().info( + "MJViser started, Open the printed browser URL" + ) - while rclpy.ok() and viewer.is_running(): - rclpy.spin_once( - node, - timeout_sec=0.01, - ) + while rclpy.ok(): + rclpy.spin_once( + node, + timeout_sec=0.01, + ) - viewer.sync() + scene.update_from_mjdata( + node.data + ) except KeyboardInterrupt: pass From 43809b45b4b1ec6db19c4aec7d64ea876d66ffe2 Mon Sep 17 00:00:00 2001 From: nitish2006main Date: Sat, 19 Sep 2026 12:03:04 -0400 Subject: [PATCH 11/11] Cleaned up r2sim mirror visualization code. Simulation is working, but visually, the meshes in MJViser for the grippers aren't moving --- .../arm_visualization_mirror.py | 129 +++++++++++------- 1 file changed, 81 insertions(+), 48 deletions(-) diff --git a/src/simulation/real2sim_mirror/arm_visualization_mirror.py b/src/simulation/real2sim_mirror/arm_visualization_mirror.py index 899aaf67..ffd257f6 100644 --- a/src/simulation/real2sim_mirror/arm_visualization_mirror.py +++ b/src/simulation/real2sim_mirror/arm_visualization_mirror.py @@ -27,6 +27,7 @@ def __init__(self): "elbow_pitch": "joint4l", "elbow_roll": "joint5l", "wrist_pitch": "joint6l", + "gripper": "joint7l", } self.right_joint_names = { @@ -36,6 +37,7 @@ def __init__(self): "elbow_pitch": "joint4", "elbow_roll": "joint5", "wrist_pitch": "joint6", + "gripper": "joint7", } self.mirror_directions = { @@ -62,7 +64,6 @@ def __init__(self): self.setup_joint_indices() - self.subscription = self.create_subscription( MotorFeedback, "/interfacing/motorFeedback", @@ -70,9 +71,7 @@ def __init__(self): 10, ) - self.get_logger().info( - "Real2Sim mirror visualization node started." - ) + self.get_logger().info("Real2Sim mirror visualization node started.") #Converts the hardware mapping YAML file into a lookup table for easy access def load_hardware_mapping(self, yaml_file_path): @@ -95,11 +94,7 @@ def load_hardware_mapping(self, yaml_file_path): def setup_joint_indices(self): for joint_type, joint_name in self.left_joint_names.items(): - joint_id = mujoco.mj_name2id( - self.model, - mujoco.mjtObj.mjOBJ_JOINT, - joint_name, - ) + joint_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, joint_name,) if joint_id == -1: raise RuntimeError( @@ -109,16 +104,10 @@ def setup_joint_indices(self): self.left_qpos[joint_type] = self.model.jnt_qposadr[joint_id] for joint_type, joint_name in self.right_joint_names.items(): - joint_id = mujoco.mj_name2id( - self.model, - mujoco.mjtObj.mjOBJ_JOINT, - joint_name, - ) + joint_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, joint_name) if joint_id == -1: - raise RuntimeError( - f"Could not find MuJoCo joint: {joint_name}" - ) + raise RuntimeError(f"Could not find MuJoCo joint: {joint_name}") self.right_qpos[joint_type] = self.model.jnt_qposadr[joint_id] @@ -131,19 +120,18 @@ def setup_joint_indices(self): ) def angle_computation(self, motor_id, position_deg): + if motor_id not in self.lookup_table: - self.get_logger().warn( - f"Unknown Motor ID {motor_id}" - ) + self.get_logger().warn(f"Unknown Motor ID {motor_id}") return None config = self.lookup_table[motor_id] - true_angle_deg = ( - position_deg - config["zero_offset"] - ) * config["direction"] + true_angle_deg = (position_deg - config["zero_offset"]) * config["direction"] + + limited_angle_deg = max(config["lower_limit"], min(config["upper_limit"], true_angle_deg)) - return math.radians(true_angle_deg) + return math.radians(limited_angle_deg) def mirror_angle(self, angle_rad, joint_name): joint_type = "_".join(joint_name.split("_")[1:]) @@ -155,10 +143,66 @@ def feedback_callback(self, msg): motor_id = msg.motor_id position_deg = msg.position - angle_rad = self.angle_computation( - motor_id, - position_deg, - ) + if motor_id == 21: + gripper_angle = max(0.0, min(100.0, position_deg)) + + gripper_value = gripper_angle / 100.0 + + # Map the logical gripper value to both finger joints. + joint7_min = -0.0532 + joint7_max = 0.0168 + + joint8_min = -0.0132 + joint8_max = 0.0468 + + joint7_position = (joint7_min + gripper_value * (joint7_max - joint7_min)) + joint8_position = (joint8_min + gripper_value * (joint8_max - joint8_min)) + + # Left gripper + + #Set the position of the left gripper's main prismatic joint + self.data.qpos[self.left_qpos["gripper"]] = joint7_position + + #Find the mujoco joint id for the left gripper second prismatic joint + joint8l_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT,"joint8l") + + if joint8l_id == -1: + raise RuntimeError("Could not find MuJoCo joint: joint8l") + + #Convert the Mujoco joint id into the index used to access that joint's position inside data.qpos. + joint8l_qpos = self.model.jnt_qposadr[joint8l_id] + + #Set the position of the left gripper's second prismatic joint + self.data.qpos[joint8l_qpos] = joint8_position + + # Right gripper + + #Set the position of the right gripper's main prismatic joint + self.data.qpos[self.right_qpos["gripper"]] = joint7_position + + #Find the mujoco joint id for the right gripper second prismatic joint + joint8_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, "joint8") + + if joint8_id == -1: + raise RuntimeError("Could not find MuJoCo joint: joint8") + + #Convert the Mujoco joint id into the index used to access that joint's position inside data.qpos. + joint8_qpos = self.model.jnt_qposadr[joint8_id] + + #Set the position of the right gripper's second prismatic joint + self.data.qpos[joint8_qpos] = joint8_position + + mujoco.mj_forward(self.model, self.data,) + + self.get_logger().info( + f"Gripper: {position_deg:.1f} deg -> " + f"joint7: {joint7_position:.4f} m, " + f"joint8: {joint8_position:.4f} m" + ) + + return + + angle_rad = self.angle_computation(motor_id, position_deg,) if angle_rad is None: return @@ -168,23 +212,24 @@ def feedback_callback(self, msg): if not joint_name.startswith("left_"): return + #Get the joint type from hardware_mapping joint_type = "_".join(joint_name.split("_")[1:]) if joint_type not in self.left_qpos: return left_qpos_index = self.left_qpos[joint_type] + + #Set the left joint angle (rad) self.data.qpos[left_qpos_index] = angle_rad mirrored_angle_rad = self.mirror_angle(angle_rad, joint_name) right_qpos_index = self.right_qpos[joint_type] self.data.qpos[right_qpos_index] = mirrored_angle_rad + # Update MuJoCo forward kinematics. - mujoco.mj_forward( - self.model, - self.data, - ) + mujoco.mj_forward(self.model, self.data,) self.get_logger().info( f"{joint_name}: " @@ -199,27 +244,15 @@ def main(): try: server = viser.ViserServer(port=8080) - scene = ViserMujocoScene( - server, - node.model, - num_envs=1, - ) + scene = ViserMujocoScene(server, node.model, num_envs=1) scene.create_visualization_gui() - node.get_logger().info( - "MJViser started, Open the printed browser URL" - ) + node.get_logger().info("MJViser started, Open the printed browser URL") while rclpy.ok(): - rclpy.spin_once( - node, - timeout_sec=0.01, - ) - - scene.update_from_mjdata( - node.data - ) + rclpy.spin_once(node, timeout_sec=0.01) + scene.update_from_mjdata(node.data) except KeyboardInterrupt: pass