Add files using upload-large-folder tool

GR00T-WholeBodyControl/decoupled_wbc/control/envs/g1/g1_body.py

@@ -0,0 +1,67 @@
+from typing import Any, Dict
+
+import gymnasium as gym
+import numpy as np
+
+from decoupled_wbc.control.base.env import Env
+from decoupled_wbc.control.envs.g1.utils.command_sender import BodyCommandSender
+from decoupled_wbc.control.envs.g1.utils.state_processor import BodyStateProcessor
+
+
+class G1Body(Env):
+    def __init__(self, config: Dict[str, Any]):
+        super().__init__()
+        self.body_state_processor = BodyStateProcessor(config=config)
+        self.body_command_sender = BodyCommandSender(config=config)
+
+    def observe(self) -> dict[str, any]:
+        body_state = self.body_state_processor._prepare_low_state()  # (1, 148)
+        assert body_state.shape == (1, 148)
+        body_q = body_state[
+            0, 7 : 7 + 12 + 3 + 7 + 7
+        ]  # leg (12) + waist (3) + left arm (7) + right arm (7)
+        body_dq = body_state[0, 42 : 42 + 12 + 3 + 7 + 7]
+        body_ddq = body_state[0, 112 : 112 + 12 + 3 + 7 + 7]
+        body_tau_est = body_state[0, 77 : 77 + 12 + 3 + 7 + 7]
+        floating_base_pose = body_state[0, 0:7]
+        floating_base_vel = body_state[0, 36:42]
+        floating_base_acc = body_state[0, 106:112]
+        torso_quat = body_state[0, 141:145]
+        torso_ang_vel = body_state[0, 145:148]
+
+        return {
+            "body_q": body_q,
+            "body_dq": body_dq,
+            "body_ddq": body_ddq,
+            "body_tau_est": body_tau_est,
+            "floating_base_pose": floating_base_pose,
+            "floating_base_vel": floating_base_vel,
+            "floating_base_acc": floating_base_acc,
+            "torso_quat": torso_quat,
+            "torso_ang_vel": torso_ang_vel,
+        }
+
+    def queue_action(self, action: dict[str, any]):
+        # action should contain body_q, body_dq, body_tau
+        self.body_command_sender.send_command(
+            action["body_q"], action["body_dq"], action["body_tau"]
+        )
+
+    def observation_space(self) -> gym.Space:
+        return gym.spaces.Dict(
+            {
+                "body_q": gym.spaces.Box(low=-np.inf, high=np.inf, shape=(29,)),
+                "body_dq": gym.spaces.Box(low=-np.inf, high=np.inf, shape=(29,)),
+                "floating_base_pose": gym.spaces.Box(low=-np.inf, high=np.inf, shape=(7,)),
+                "floating_base_vel": gym.spaces.Box(low=-np.inf, high=np.inf, shape=(6,)),
+            }
+        )
+
+    def action_space(self) -> gym.Space:
+        return gym.spaces.Dict(
+            {
+                "body_q": gym.spaces.Box(low=-np.inf, high=np.inf, shape=(29,)),
+                "body_dq": gym.spaces.Box(low=-np.inf, high=np.inf, shape=(29,)),
+                "body_tau": gym.spaces.Box(low=-np.inf, high=np.inf, shape=(29,)),
+            }
+        )

GR00T-WholeBodyControl/decoupled_wbc/control/envs/g1/g1_env.py

@@ -0,0 +1,324 @@
+from copy import deepcopy
+from typing import Dict
+
+import gymnasium as gym
+import numpy as np
+from scipy.spatial.transform import Rotation as R
+
+from decoupled_wbc.control.base.humanoid_env import Hands, HumanoidEnv
+from decoupled_wbc.control.envs.g1.g1_body import G1Body
+from decoupled_wbc.control.envs.g1.g1_hand import G1ThreeFingerHand
+from decoupled_wbc.control.envs.g1.sim.simulator_factory import SimulatorFactory, init_channel
+from decoupled_wbc.control.envs.g1.utils.joint_safety import JointSafetyMonitor
+from decoupled_wbc.control.robot_model.instantiation.g1 import instantiate_g1_robot_model
+from decoupled_wbc.control.robot_model.robot_model import RobotModel
+from decoupled_wbc.control.utils.ros_utils import ROSManager
+
+
+class G1Env(HumanoidEnv):
+    def __init__(
+        self,
+        env_name: str = "default",
+        robot_model: RobotModel = None,
+        wbc_version: str = "v2",
+        config: Dict[str, any] = None,
+        **kwargs,
+    ):
+        super().__init__()
+        self.robot_model = deepcopy(robot_model)  # need to cache FK results
+        self.config = config
+
+        # Initialize safety monitor (visualization disabled)
+        self.safety_monitor = JointSafetyMonitor(
+            robot_model, enable_viz=False, env_type=self.config.get("ENV_TYPE", "real")
+        )
+        self.last_obs = None
+        self.last_safety_ok = True  # Track last safety status from queue_action
+
+        init_channel(config=self.config)
+
+        # Initialize body and hands
+        self._body = G1Body(config=self.config)
+
+        self.with_hands = config.get("with_hands", False)
+
+        # Gravity compensation settings
+        self.enable_gravity_compensation = config.get("enable_gravity_compensation", False)
+        self.gravity_compensation_joints = config.get("gravity_compensation_joints", ["arms"])
+
+        if self.enable_gravity_compensation:
+            print(
+                f"Gravity compensation enabled for joint groups: {self.gravity_compensation_joints}"
+            )
+        if self.with_hands:
+            self._hands = Hands()
+            self._hands.left = G1ThreeFingerHand(is_left=True)
+            self._hands.right = G1ThreeFingerHand(is_left=False)
+
+        # Initialize simulator if in simulation mode
+        self.use_sim = self.config.get("ENV_TYPE") == "sim"
+
+        if self.use_sim:
+            # Create simulator using factory
+
+            kwargs.update(
+                {
+                    "onscreen": self.config.get("ENABLE_ONSCREEN", True),
+                    "offscreen": self.config.get("ENABLE_OFFSCREEN", False),
+                }
+            )
+            self.sim = SimulatorFactory.create_simulator(
+                config=self.config,
+                env_name=env_name,
+                wbc_version=wbc_version,
+                body_ik_solver_settings_type=kwargs.get("body_ik_solver_settings_type", "default"),
+                **kwargs,
+            )
+        else:
+            self.sim = None
+
+            # using the real robot
+            self.calibrate_hands()
+
+        # Initialize ROS 2 node
+        self.ros_manager = ROSManager(node_name="g1_env")
+        self.ros_node = self.ros_manager.node
+
+        self.delay_list = []
+        self.visualize_delay = False
+        self.print_delay_interval = 100
+        self.cnt = 0
+
+    def start_simulator(self):
+        # imag epublish disabled since the sim is running in a sub-thread
+        SimulatorFactory.start_simulator(self.sim, as_thread=True, enable_image_publish=False)
+
+    def step_simulator(self):
+        sim_num_steps = int(self.config["REWARD_DT"] / self.config["SIMULATE_DT"])
+        for _ in range(sim_num_steps):
+            self.sim.sim_env.sim_step()
+        self.sim.sim_env.update_viewer()
+
+    def body(self) -> G1Body:
+        return self._body
+
+    def hands(self) -> Hands:
+        if not self.with_hands:
+            raise RuntimeError(
+                "Hands not initialized. Use --with_hands True to enable hand functionality."
+            )
+        return self._hands
+
+    def observe(self) -> Dict[str, any]:
+        # Get observations from body and hands
+        body_obs = self.body().observe()
+
+        body_q = body_obs["body_q"]
+        body_dq = body_obs["body_dq"]
+        body_ddq = body_obs["body_ddq"]
+        body_tau_est = body_obs["body_tau_est"]
+
+        if self.with_hands:
+            left_hand_obs = self.hands().left.observe()
+            right_hand_obs = self.hands().right.observe()
+            left_hand_q = left_hand_obs["hand_q"]
+            right_hand_q = right_hand_obs["hand_q"]
+            left_hand_dq = left_hand_obs["hand_dq"]
+            right_hand_dq = right_hand_obs["hand_dq"]
+            left_hand_ddq = left_hand_obs["hand_ddq"]
+            right_hand_ddq = right_hand_obs["hand_ddq"]
+            left_hand_tau_est = left_hand_obs["hand_tau_est"]
+            right_hand_tau_est = right_hand_obs["hand_tau_est"]
+
+            # Body and hand joint measurements come in actuator order, so we need to convert them to joint order
+            whole_q = self.robot_model.get_configuration_from_actuated_joints(
+                body_actuated_joint_values=body_q,
+                left_hand_actuated_joint_values=left_hand_q,
+                right_hand_actuated_joint_values=right_hand_q,
+            )
+            whole_dq = self.robot_model.get_configuration_from_actuated_joints(
+                body_actuated_joint_values=body_dq,
+                left_hand_actuated_joint_values=left_hand_dq,
+                right_hand_actuated_joint_values=right_hand_dq,
+            )
+            whole_ddq = self.robot_model.get_configuration_from_actuated_joints(
+                body_actuated_joint_values=body_ddq,
+                left_hand_actuated_joint_values=left_hand_ddq,
+                right_hand_actuated_joint_values=right_hand_ddq,
+            )
+            whole_tau_est = self.robot_model.get_configuration_from_actuated_joints(
+                body_actuated_joint_values=body_tau_est,
+                left_hand_actuated_joint_values=left_hand_tau_est,
+                right_hand_actuated_joint_values=right_hand_tau_est,
+            )
+        else:
+            # Body and hand joint measurements come in actuator order, so we need to convert them to joint order
+            whole_q = self.robot_model.get_configuration_from_actuated_joints(
+                body_actuated_joint_values=body_q,
+            )
+            whole_dq = self.robot_model.get_configuration_from_actuated_joints(
+                body_actuated_joint_values=body_dq,
+            )
+            whole_ddq = self.robot_model.get_configuration_from_actuated_joints(
+                body_actuated_joint_values=body_ddq,
+            )
+            whole_tau_est = self.robot_model.get_configuration_from_actuated_joints(
+                body_actuated_joint_values=body_tau_est,
+            )
+
+        eef_obs = self.get_eef_obs(whole_q)
+
+        obs = {
+            "q": whole_q,
+            "dq": whole_dq,
+            "ddq": whole_ddq,
+            "tau_est": whole_tau_est,
+            "floating_base_pose": body_obs["floating_base_pose"],
+            "floating_base_vel": body_obs["floating_base_vel"],
+            "floating_base_acc": body_obs["floating_base_acc"],
+            "wrist_pose": np.concatenate([eef_obs["left_wrist_pose"], eef_obs["right_wrist_pose"]]),
+            "torso_quat": body_obs["torso_quat"],
+            "torso_ang_vel": body_obs["torso_ang_vel"],
+        }
+
+        if self.use_sim and self.sim:
+            obs.update(self.sim.get_privileged_obs())
+
+        # Store last observation for safety checking
+        self.last_obs = obs
+
+        return obs
+
+    @property
+    def observation_space(self) -> gym.Space:
+        # @todo: check if the low and high bounds are correct for body_obs.
+        q_space = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(self.robot_model.num_dofs,))
+        dq_space = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(self.robot_model.num_dofs,))
+        ddq_space = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(self.robot_model.num_dofs,))
+        tau_est_space = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(self.robot_model.num_dofs,))
+        floating_base_pose_space = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(7,))
+        floating_base_vel_space = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(6,))
+        floating_base_acc_space = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(6,))
+        wrist_pose_space = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(7 + 7,))
+        return gym.spaces.Dict(
+            {
+                "floating_base_pose": floating_base_pose_space,
+                "floating_base_vel": floating_base_vel_space,
+                "floating_base_acc": floating_base_acc_space,
+                "q": q_space,
+                "dq": dq_space,
+                "ddq": ddq_space,
+                "tau_est": tau_est_space,
+                "wrist_pose": wrist_pose_space,
+            }
+        )
+
+    def queue_action(self, action: Dict[str, any]):
+        # Safety check
+        if self.last_obs is not None:
+            safety_result = self.safety_monitor.handle_violations(self.last_obs, action)
+            action = safety_result["action"]
+
+        # Map action from joint order to actuator order
+        body_actuator_q = self.robot_model.get_body_actuated_joints(action["q"])
+
+        self.body().queue_action(
+            {
+                "body_q": body_actuator_q,
+                "body_dq": np.zeros_like(body_actuator_q),
+                "body_tau": np.zeros_like(body_actuator_q),
+            }
+        )
+
+        if self.with_hands:
+            left_hand_actuator_q = self.robot_model.get_hand_actuated_joints(
+                action["q"], side="left"
+            )
+            right_hand_actuator_q = self.robot_model.get_hand_actuated_joints(
+                action["q"], side="right"
+            )
+            self.hands().left.queue_action({"hand_q": left_hand_actuator_q})
+            self.hands().right.queue_action({"hand_q": right_hand_actuator_q})
+
+    def action_space(self) -> gym.Space:
+        return gym.spaces.Box(low=-np.inf, high=np.inf, shape=(self.robot_model.num_dofs,))
+
+    def calibrate_hands(self):
+        """Calibrate the hand joint qpos if real robot"""
+        if self.with_hands:
+            print("calibrating left hand")
+            self.hands().left.calibrate_hand()
+            print("calibrating right hand")
+            self.hands().right.calibrate_hand()
+        else:
+            print("Skipping hand calibration - hands disabled")
+
+    def set_ik_indicator(self, teleop_cmd):
+        """Set the IK indicators for the simulator"""
+        if self.config["SIMULATOR"] == "robocasa":
+            if "left_wrist" in teleop_cmd and "right_wrist" in teleop_cmd:
+                left_wrist_input_pose = teleop_cmd["left_wrist"]
+                right_wrist_input_pose = teleop_cmd["right_wrist"]
+                ik_wrapper = self.sim.env.env.unwrapped.env
+                ik_wrapper.set_target_poses_outside_env(
+                    [left_wrist_input_pose, right_wrist_input_pose]
+                )
+        else:
+            raise NotImplementedError("IK indicators are only implemented for robocasa simulator")
+
+    def set_sync_mode(self, sync_mode: bool, steps_per_action: int = 4):
+        """When set to True, the simulator will wait for the action to be sent to it"""
+        if self.config["SIMULATOR"] == "robocasa":
+            self.sim.set_sync_mode(sync_mode, steps_per_action)
+
+    def reset(self):
+        if self.sim:
+            self.sim.reset()
+
+    def close(self):
+        if self.sim:
+            self.sim.close()
+
+    def robot_model(self) -> RobotModel:
+        return self.robot_model
+
+    def get_reward(self):
+        if self.sim:
+            return self.sim.get_reward()
+
+    def reset_obj_pos(self):
+        if hasattr(self.sim, "base_env") and hasattr(self.sim.base_env, "reset_obj_pos"):
+            self.sim.base_env.reset_obj_pos()
+
+    def get_eef_obs(self, q: np.ndarray) -> Dict[str, np.ndarray]:
+        self.robot_model.cache_forward_kinematics(q)
+        eef_obs = {}
+        for side in ["left", "right"]:
+            wrist_placement = self.robot_model.frame_placement(
+                self.robot_model.supplemental_info.hand_frame_names[side]
+            )
+            wrist_pos, wrist_quat = wrist_placement.translation[:3], R.from_matrix(
+                wrist_placement.rotation
+            ).as_quat(scalar_first=True)
+            eef_obs[f"{side}_wrist_pose"] = np.concatenate([wrist_pos, wrist_quat])
+
+        return eef_obs
+
+    def get_joint_safety_status(self) -> bool:
+        """Get current joint safety status from the last queue_action safety check.
+
+        Returns:
+            bool: True if joints are safe (no shutdown required), False if unsafe
+        """
+        return self.last_safety_ok
+
+    def handle_keyboard_button(self, key):
+        # Only handles keyboard buttons for the mujoco simulator for now.
+        if self.use_sim and self.config.get("SIMULATOR", "mujoco") == "mujoco":
+            self.sim.handle_keyboard_button(key)
+
+
+if __name__ == "__main__":
+    env = G1Env(robot_model=instantiate_g1_robot_model(), wbc_version="gear_wbc")
+    while True:
+        print(env.observe())

GR00T-WholeBodyControl/decoupled_wbc/control/envs/g1/g1_hand.py

@@ -0,0 +1,89 @@
+import time
+
+import gymnasium as gym
+import numpy as np
+
+from decoupled_wbc.control.base.env import Env
+from decoupled_wbc.control.envs.g1.utils.command_sender import HandCommandSender
+from decoupled_wbc.control.envs.g1.utils.state_processor import HandStateProcessor
+
+
+class G1ThreeFingerHand(Env):
+    def __init__(self, is_left: bool = True):
+        super().__init__()
+        self.is_left = is_left
+        self.hand_state_processor = HandStateProcessor(is_left=self.is_left)
+        self.hand_command_sender = HandCommandSender(is_left=self.is_left)
+        self.hand_q_offset = np.zeros(7)
+
+    def observe(self) -> dict[str, any]:
+        hand_state = self.hand_state_processor._prepare_low_state()  # (1, 28)
+        assert hand_state.shape == (1, 28)
+
+        # Apply offset to the hand state
+        hand_state[0, :7] = hand_state[0, :7] + self.hand_q_offset
+
+        hand_q = hand_state[0, :7]
+        hand_dq = hand_state[0, 7:14]
+        hand_ddq = hand_state[0, 21:28]
+        hand_tau_est = hand_state[0, 14:21]
+
+        # Return the state for this specific hand (left or right)
+        return {
+            "hand_q": hand_q,
+            "hand_dq": hand_dq,
+            "hand_ddq": hand_ddq,
+            "hand_tau_est": hand_tau_est,
+        }
+
+    def queue_action(self, action: dict[str, any]):
+        # Apply offset to the hand target
+        action["hand_q"] = action["hand_q"] - self.hand_q_offset
+
+        # action should contain hand_q
+        self.hand_command_sender.send_command(action["hand_q"])
+
+    def observation_space(self) -> gym.Space:
+        return gym.spaces.Dict(
+            {
+                "hand_q": gym.spaces.Box(low=-np.inf, high=np.inf, shape=(7,)),
+                "hand_dq": gym.spaces.Box(low=-np.inf, high=np.inf, shape=(7,)),
+                "hand_ddq": gym.spaces.Box(low=-np.inf, high=np.inf, shape=(7,)),
+                "hand_tau_est": gym.spaces.Box(low=-np.inf, high=np.inf, shape=(7,)),
+            }
+        )
+
+    def action_space(self) -> gym.Space:
+        return gym.spaces.Dict({"hand_q": gym.spaces.Box(low=-np.inf, high=np.inf, shape=(7,))})
+
+    def calibrate_hand(self):
+        hand_obs = self.observe()
+        hand_q = hand_obs["hand_q"]
+
+        hand_q_target = np.zeros_like(hand_q)
+        hand_q_target[0] = hand_q[0]
+
+        # joint limit
+        hand_q0_upper_limit = np.deg2rad(60)  # lower limit is -60
+
+        # move the figure counterclockwise until the limit
+        while True:
+
+            if hand_q_target[0] - hand_q[0] < np.deg2rad(60):
+                hand_q_target[0] += np.deg2rad(10)
+            else:
+                self.hand_q_offset[0] = hand_q0_upper_limit - hand_q[0]
+                break
+
+            self.queue_action({"hand_q": hand_q_target})
+
+            hand_obs = self.observe()
+            hand_q = hand_obs["hand_q"]
+
+            time.sleep(0.1)
+
+        print("done calibration, q0 offset (deg):", np.rad2deg(self.hand_q_offset[0]))
+
+        # done calibrating, set target to zero
+        self.hand_q_target = np.zeros_like(hand_q)
+        self.queue_action({"hand_q": self.hand_q_target})

GR00T-WholeBodyControl/decoupled_wbc/control/envs/g1/sim/base_sim.py

@@ -0,0 +1,772 @@
+import argparse
+import pathlib
+from pathlib import Path
+import threading
+from threading import Lock, Thread
+from typing import Dict
+
+import mujoco
+import mujoco.viewer
+import numpy as np
+import rclpy
+from unitree_sdk2py.core.channel import ChannelFactoryInitialize
+import yaml
+
+from decoupled_wbc.control.envs.g1.sim.image_publish_utils import ImagePublishProcess
+from decoupled_wbc.control.envs.g1.sim.metric_utils import check_contact, check_height
+from decoupled_wbc.control.envs.g1.sim.sim_utilts import get_subtree_body_names
+from decoupled_wbc.control.envs.g1.sim.unitree_sdk2py_bridge import ElasticBand, UnitreeSdk2Bridge
+
+DECOUPLED_WBC_ROOT = Path(__file__).resolve().parent.parent.parent.parent.parent.parent
+
+
+class DefaultEnv:
+    """Base environment class that handles simulation environment setup and step"""
+
+    def __init__(
+        self,
+        config: Dict[str, any],
+        env_name: str = "default",
+        camera_configs: Dict[str, any] = {},
+        onscreen: bool = False,
+        offscreen: bool = False,
+        enable_image_publish: bool = False,
+    ):
+        # global_view is only set up for this specifc scene for now.
+        if config["ROBOT_SCENE"] == "decoupled_wbc/control/robot_model/model_data/g1/scene_29dof.xml":
+            camera_configs["global_view"] = {
+                "height": 400,
+                "width": 400,
+            }
+        self.config = config
+        self.env_name = env_name
+        self.num_body_dof = self.config["NUM_JOINTS"]
+        self.num_hand_dof = self.config["NUM_HAND_JOINTS"]
+        self.sim_dt = self.config["SIMULATE_DT"]
+        self.obs = None
+        self.torques = np.zeros(self.num_body_dof + self.num_hand_dof * 2)
+        self.torque_limit = np.array(self.config["motor_effort_limit_list"])
+        self.camera_configs = camera_configs
+
+        # Thread safety lock
+        self.reward_lock = Lock()
+
+        # Unitree bridge will be initialized by the simulator
+        self.unitree_bridge = None
+
+        # Store display mode
+        self.onscreen = onscreen
+
+        # Initialize scene (defined in subclasses)
+        self.init_scene()
+        self.last_reward = 0
+
+        # Setup offscreen rendering if needed
+        self.offscreen = offscreen
+        if self.offscreen:
+            self.init_renderers()
+        self.image_dt = self.config.get("IMAGE_DT", 0.033333)
+        self.image_publish_process = None
+
+    def start_image_publish_subprocess(self, start_method: str = "spawn", camera_port: int = 5555):
+        # Use spawn method for better GIL isolation, or configured method
+        if len(self.camera_configs) == 0:
+            print(
+                "Warning: No camera configs provided, image publishing subprocess will not be started"
+            )
+            return
+        start_method = self.config.get("MP_START_METHOD", "spawn")
+        self.image_publish_process = ImagePublishProcess(
+            camera_configs=self.camera_configs,
+            image_dt=self.image_dt,
+            zmq_port=camera_port,
+            start_method=start_method,
+            verbose=self.config.get("verbose", False),
+        )
+        self.image_publish_process.start_process()
+
+    def init_scene(self):
+        """Initialize the default robot scene"""
+        self.mj_model = mujoco.MjModel.from_xml_path(
+            str(pathlib.Path(DECOUPLED_WBC_ROOT) / self.config["ROBOT_SCENE"])
+        )
+        self.mj_data = mujoco.MjData(self.mj_model)
+        self.mj_model.opt.timestep = self.sim_dt
+        self.torso_index = mujoco.mj_name2id(self.mj_model, mujoco.mjtObj.mjOBJ_BODY, "torso_link")
+        self.root_body = "pelvis"
+        # Enable the elastic band
+        if self.config["ENABLE_ELASTIC_BAND"]:
+            self.elastic_band = ElasticBand()
+            if "g1" in self.config["ROBOT_TYPE"]:
+                if self.config["enable_waist"]:
+                    self.band_attached_link = self.mj_model.body("pelvis").id
+                else:
+                    self.band_attached_link = self.mj_model.body("torso_link").id
+            elif "h1" in self.config["ROBOT_TYPE"]:
+                self.band_attached_link = self.mj_model.body("torso_link").id
+            else:
+                self.band_attached_link = self.mj_model.body("base_link").id
+
+            if self.onscreen:
+                self.viewer = mujoco.viewer.launch_passive(
+                    self.mj_model,
+                    self.mj_data,
+                    key_callback=self.elastic_band.MujuocoKeyCallback,
+                    show_left_ui=False,
+                    show_right_ui=False,
+                )
+            else:
+                mujoco.mj_forward(self.mj_model, self.mj_data)
+                self.viewer = None
+        else:
+            if self.onscreen:
+                self.viewer = mujoco.viewer.launch_passive(
+                    self.mj_model, self.mj_data, show_left_ui=False, show_right_ui=False
+                )
+            else:
+                mujoco.mj_forward(self.mj_model, self.mj_data)
+                self.viewer = None
+
+        if self.viewer:
+            # viewer camera
+            self.viewer.cam.azimuth = 120  # Horizontal rotation in degrees
+            self.viewer.cam.elevation = -30  # Vertical tilt in degrees
+            self.viewer.cam.distance = 2.0  # Distance from camera to target
+            self.viewer.cam.lookat = np.array([0, 0, 0.5])  # Point the camera is looking at
+
+        # Note that the actuator order is the same as the joint order in the mujoco model.
+        self.body_joint_index = []
+        self.left_hand_index = []
+        self.right_hand_index = []
+        for i in range(self.mj_model.njnt):
+            name = self.mj_model.joint(i).name
+            if any(
+                [
+                    part_name in name
+                    for part_name in ["hip", "knee", "ankle", "waist", "shoulder", "elbow", "wrist"]
+                ]
+            ):
+                self.body_joint_index.append(i)
+            elif "left_hand" in name:
+                self.left_hand_index.append(i)
+            elif "right_hand" in name:
+                self.right_hand_index.append(i)
+
+        assert len(self.body_joint_index) == self.config["NUM_JOINTS"]
+        assert len(self.left_hand_index) == self.config["NUM_HAND_JOINTS"]
+        assert len(self.right_hand_index) == self.config["NUM_HAND_JOINTS"]
+
+        self.body_joint_index = np.array(self.body_joint_index)
+        self.left_hand_index = np.array(self.left_hand_index)
+        self.right_hand_index = np.array(self.right_hand_index)
+
+    def init_renderers(self):
+        # Initialize camera renderers
+        self.renderers = {}
+        for camera_name, camera_config in self.camera_configs.items():
+            renderer = mujoco.Renderer(
+                self.mj_model, height=camera_config["height"], width=camera_config["width"]
+            )
+            self.renderers[camera_name] = renderer
+
+    def compute_body_torques(self) -> np.ndarray:
+        """Compute body torques based on the current robot state"""
+        body_torques = np.zeros(self.num_body_dof)
+        if self.unitree_bridge is not None and self.unitree_bridge.low_cmd:
+            for i in range(self.unitree_bridge.num_body_motor):
+                if self.unitree_bridge.use_sensor:
+                    body_torques[i] = (
+                        self.unitree_bridge.low_cmd.motor_cmd[i].tau
+                        + self.unitree_bridge.low_cmd.motor_cmd[i].kp
+                        * (self.unitree_bridge.low_cmd.motor_cmd[i].q - self.mj_data.sensordata[i])
+                        + self.unitree_bridge.low_cmd.motor_cmd[i].kd
+                        * (
+                            self.unitree_bridge.low_cmd.motor_cmd[i].dq
+                            - self.mj_data.sensordata[i + self.unitree_bridge.num_body_motor]
+                        )
+                    )
+                else:
+                    body_torques[i] = (
+                        self.unitree_bridge.low_cmd.motor_cmd[i].tau
+                        + self.unitree_bridge.low_cmd.motor_cmd[i].kp
+                        * (
+                            self.unitree_bridge.low_cmd.motor_cmd[i].q
+                            - self.mj_data.qpos[self.body_joint_index[i] + 7 - 1]
+                        )
+                        + self.unitree_bridge.low_cmd.motor_cmd[i].kd
+                        * (
+                            self.unitree_bridge.low_cmd.motor_cmd[i].dq
+                            - self.mj_data.qvel[self.body_joint_index[i] + 6 - 1]
+                        )
+                    )
+        return body_torques
+
+    def compute_hand_torques(self) -> np.ndarray:
+        """Compute hand torques based on the current robot state"""
+        left_hand_torques = np.zeros(self.num_hand_dof)
+        right_hand_torques = np.zeros(self.num_hand_dof)
+        if self.unitree_bridge is not None and self.unitree_bridge.low_cmd:
+            for i in range(self.unitree_bridge.num_hand_motor):
+                left_hand_torques[i] = (
+                    self.unitree_bridge.left_hand_cmd.motor_cmd[i].tau
+                    + self.unitree_bridge.left_hand_cmd.motor_cmd[i].kp
+                    * (
+                        self.unitree_bridge.left_hand_cmd.motor_cmd[i].q
+                        - self.mj_data.qpos[self.left_hand_index[i] + 7 - 1]
+                    )
+                    + self.unitree_bridge.left_hand_cmd.motor_cmd[i].kd
+                    * (
+                        self.unitree_bridge.left_hand_cmd.motor_cmd[i].dq
+                        - self.mj_data.qvel[self.left_hand_index[i] + 6 - 1]
+                    )
+                )
+                right_hand_torques[i] = (
+                    self.unitree_bridge.right_hand_cmd.motor_cmd[i].tau
+                    + self.unitree_bridge.right_hand_cmd.motor_cmd[i].kp
+                    * (
+                        self.unitree_bridge.right_hand_cmd.motor_cmd[i].q
+                        - self.mj_data.qpos[self.right_hand_index[i] + 7 - 1]
+                    )
+                    + self.unitree_bridge.right_hand_cmd.motor_cmd[i].kd
+                    * (
+                        self.unitree_bridge.right_hand_cmd.motor_cmd[i].dq
+                        - self.mj_data.qvel[self.right_hand_index[i] + 6 - 1]
+                    )
+                )
+        return np.concatenate((left_hand_torques, right_hand_torques))
+
+    def compute_body_qpos(self) -> np.ndarray:
+        """Compute body joint positions based on the current command"""
+        body_qpos = np.zeros(self.num_body_dof)
+        if self.unitree_bridge is not None and self.unitree_bridge.low_cmd:
+            for i in range(self.unitree_bridge.num_body_motor):
+                body_qpos[i] = self.unitree_bridge.low_cmd.motor_cmd[i].q
+        return body_qpos
+
+    def compute_hand_qpos(self) -> np.ndarray:
+        """Compute hand joint positions based on the current command"""
+        hand_qpos = np.zeros(self.num_hand_dof * 2)
+        if self.unitree_bridge is not None and self.unitree_bridge.low_cmd:
+            for i in range(self.unitree_bridge.num_hand_motor):
+                hand_qpos[i] = self.unitree_bridge.left_hand_cmd.motor_cmd[i].q
+                hand_qpos[i + self.num_hand_dof] = self.unitree_bridge.right_hand_cmd.motor_cmd[i].q
+        return hand_qpos
+
+    def prepare_obs(self) -> Dict[str, any]:
+        """Prepare observation dictionary from the current robot state"""
+        obs = {}
+        obs["floating_base_pose"] = self.mj_data.qpos[:7]
+        obs["floating_base_vel"] = self.mj_data.qvel[:6]
+        obs["floating_base_acc"] = self.mj_data.qacc[:6]
+        obs["secondary_imu_quat"] = self.mj_data.xquat[self.torso_index]
+        obs["secondary_imu_vel"] = self.mj_data.cvel[self.torso_index]
+        obs["body_q"] = self.mj_data.qpos[self.body_joint_index + 7 - 1]
+        obs["body_dq"] = self.mj_data.qvel[self.body_joint_index + 6 - 1]
+        obs["body_ddq"] = self.mj_data.qacc[self.body_joint_index + 6 - 1]
+        obs["body_tau_est"] = self.mj_data.actuator_force[self.body_joint_index - 1]
+        if self.num_hand_dof > 0:
+            obs["left_hand_q"] = self.mj_data.qpos[self.left_hand_index + 7 - 1]
+            obs["left_hand_dq"] = self.mj_data.qvel[self.left_hand_index + 6 - 1]
+            obs["left_hand_ddq"] = self.mj_data.qacc[self.left_hand_index + 6 - 1]
+            obs["left_hand_tau_est"] = self.mj_data.actuator_force[self.left_hand_index - 1]
+            obs["right_hand_q"] = self.mj_data.qpos[self.right_hand_index + 7 - 1]
+            obs["right_hand_dq"] = self.mj_data.qvel[self.right_hand_index + 6 - 1]
+            obs["right_hand_ddq"] = self.mj_data.qacc[self.right_hand_index + 6 - 1]
+            obs["right_hand_tau_est"] = self.mj_data.actuator_force[self.right_hand_index - 1]
+        obs["time"] = self.mj_data.time
+        return obs
+
+    def sim_step(self):
+        self.obs = self.prepare_obs()
+        self.unitree_bridge.PublishLowState(self.obs)
+        if self.unitree_bridge.joystick:
+            self.unitree_bridge.PublishWirelessController()
+        if self.config["ENABLE_ELASTIC_BAND"]:
+            if self.elastic_band.enable:
+                # Get Cartesian pose and velocity of the band_attached_link
+                pose = np.concatenate(
+                    [
+                        self.mj_data.xpos[self.band_attached_link],  # link position in world
+                        self.mj_data.xquat[
+                            self.band_attached_link
+                        ],  # link quaternion in world [w,x,y,z]
+                        np.zeros(6),  # placeholder for velocity
+                    ]
+                )
+
+                # Get velocity in world frame
+                mujoco.mj_objectVelocity(
+                    self.mj_model,
+                    self.mj_data,
+                    mujoco.mjtObj.mjOBJ_BODY,
+                    self.band_attached_link,
+                    pose[7:13],
+                    0,  # 0 for world frame
+                )
+
+                # Reorder velocity from [ang, lin] to [lin, ang]
+                pose[7:10], pose[10:13] = pose[10:13], pose[7:10].copy()
+                self.mj_data.xfrc_applied[self.band_attached_link] = self.elastic_band.Advance(pose)
+            else:
+                # explicitly resetting the force when the band is not enabled
+                self.mj_data.xfrc_applied[self.band_attached_link] = np.zeros(6)
+        body_torques = self.compute_body_torques()
+        hand_torques = self.compute_hand_torques()
+        self.torques[self.body_joint_index - 1] = body_torques
+        if self.num_hand_dof > 0:
+            self.torques[self.left_hand_index - 1] = hand_torques[: self.num_hand_dof]
+            self.torques[self.right_hand_index - 1] = hand_torques[self.num_hand_dof :]
+
+        self.torques = np.clip(self.torques, -self.torque_limit, self.torque_limit)
+
+        if self.config["FREE_BASE"]:
+            self.mj_data.ctrl = np.concatenate((np.zeros(6), self.torques))
+        else:
+            self.mj_data.ctrl = self.torques
+        mujoco.mj_step(self.mj_model, self.mj_data)
+        # self.check_self_collision()
+
+    def kinematics_step(self):
+        """
+        Run kinematics only: compute the qpos of the robot and directly set the qpos.
+        For debugging purposes.
+        """
+        if self.unitree_bridge is not None:
+            self.unitree_bridge.PublishLowState(self.prepare_obs())
+            if self.unitree_bridge.joystick:
+                self.unitree_bridge.PublishWirelessController()
+
+        if self.config["ENABLE_ELASTIC_BAND"]:
+            if self.elastic_band.enable:
+                # Get Cartesian pose and velocity of the band_attached_link
+                pose = np.concatenate(
+                    [
+                        self.mj_data.xpos[self.band_attached_link],  # link position in world
+                        self.mj_data.xquat[
+                            self.band_attached_link
+                        ],  # link quaternion in world [w,x,y,z]
+                        np.zeros(6),  # placeholder for velocity
+                    ]
+                )
+
+                # Get velocity in world frame
+                mujoco.mj_objectVelocity(
+                    self.mj_model,
+                    self.mj_data,
+                    mujoco.mjtObj.mjOBJ_BODY,
+                    self.band_attached_link,
+                    pose[7:13],
+                    0,  # 0 for world frame
+                )
+
+                # Reorder velocity from [ang, lin] to [lin, ang]
+                pose[7:10], pose[10:13] = pose[10:13], pose[7:10].copy()
+
+                self.mj_data.xfrc_applied[self.band_attached_link] = self.elastic_band.Advance(pose)
+            else:
+                # explicitly resetting the force when the band is not enabled
+                self.mj_data.xfrc_applied[self.band_attached_link] = np.zeros(6)
+
+        body_qpos = self.compute_body_qpos()  # (num_body_dof,)
+        hand_qpos = self.compute_hand_qpos()  # (num_hand_dof * 2,)
+
+        self.mj_data.qpos[self.body_joint_index + 7 - 1] = body_qpos
+        self.mj_data.qpos[self.left_hand_index + 7 - 1] = hand_qpos[: self.num_hand_dof]
+        self.mj_data.qpos[self.right_hand_index + 7 - 1] = hand_qpos[self.num_hand_dof :]
+
+        mujoco.mj_kinematics(self.mj_model, self.mj_data)
+        mujoco.mj_comPos(self.mj_model, self.mj_data)
+
+    def apply_perturbation(self, key):
+        """Apply perturbation to the robot"""
+        # Add velocity perturbations in body frame
+        perturbation_x_body = 0.0  # forward/backward in body frame
+        perturbation_y_body = 0.0  # left/right in body frame
+        if key == "up":
+            perturbation_x_body = 1.0  # forward
+        elif key == "down":
+            perturbation_x_body = -1.0  # backward
+        elif key == "left":
+            perturbation_y_body = 1.0  # left
+        elif key == "right":
+            perturbation_y_body = -1.0  # right
+
+        # Transform body frame velocity to world frame using MuJoCo's rotation
+        vel_body = np.array([perturbation_x_body, perturbation_y_body, 0.0])
+        vel_world = np.zeros(3)
+        base_quat = self.mj_data.qpos[3:7]  # [w, x, y, z] quaternion
+
+        # Use MuJoCo's robust quaternion rotation (handles invalid quaternions automatically)
+        mujoco.mju_rotVecQuat(vel_world, vel_body, base_quat)
+
+        # Apply to base linear velocity in world frame
+        self.mj_data.qvel[0] += vel_world[0]  # world X velocity
+        self.mj_data.qvel[1] += vel_world[1]  # world Y velocity
+
+        # Update dynamics after velocity change
+        mujoco.mj_forward(self.mj_model, self.mj_data)
+
+    def update_viewer(self):
+        if self.viewer is not None:
+            self.viewer.sync()
+
+    def update_viewer_camera(self):
+        if self.viewer is not None:
+            if self.viewer.cam.type == mujoco.mjtCamera.mjCAMERA_TRACKING:
+                self.viewer.cam.type = mujoco.mjtCamera.mjCAMERA_FREE
+            else:
+                self.viewer.cam.type = mujoco.mjtCamera.mjCAMERA_TRACKING
+
+    def update_reward(self):
+        """Calculate reward. Should be implemented by subclasses."""
+        with self.reward_lock:
+            self.last_reward = 0
+
+    def get_reward(self):
+        """Thread-safe way to get the last calculated reward."""
+        with self.reward_lock:
+            return self.last_reward
+
+    def set_unitree_bridge(self, unitree_bridge):
+        """Set the unitree bridge from the simulator"""
+        self.unitree_bridge = unitree_bridge
+
+    def get_privileged_obs(self):
+        """Get privileged observation. Should be implemented by subclasses."""
+        return {}
+
+    def update_render_caches(self):
+        """Update render cache and shared memory for subprocess."""
+        render_caches = {}
+        for camera_name, camera_config in self.camera_configs.items():
+            renderer = self.renderers[camera_name]
+            if "params" in camera_config:
+                renderer.update_scene(self.mj_data, camera=camera_config["params"])
+            else:
+                renderer.update_scene(self.mj_data, camera=camera_name)
+            render_caches[camera_name + "_image"] = renderer.render()
+
+        # Update shared memory if image publishing process is available
+        if self.image_publish_process is not None:
+            self.image_publish_process.update_shared_memory(render_caches)
+
+        return render_caches
+
+    def handle_keyboard_button(self, key):
+        if self.elastic_band is not None:
+            self.elastic_band.handle_keyboard_button(key)
+
+        if key == "backspace":
+            self.reset()
+        if key == "v":
+            self.update_viewer_camera()
+        if key in ["up", "down", "left", "right"]:
+            self.apply_perturbation(key)
+
+    def check_fall(self):
+        """Check if the robot has fallen"""
+        self.fall = False
+        if self.mj_data.qpos[2] < 0.2:
+            self.fall = True
+            print(f"Warning: Robot has fallen, height: {self.mj_data.qpos[2]:.3f} m")
+
+        if self.fall:
+            self.reset()
+
+    def check_self_collision(self):
+        """Check for self-collision of the robot"""
+        robot_bodies = get_subtree_body_names(self.mj_model, self.mj_model.body(self.root_body).id)
+        self_collision, contact_bodies = check_contact(
+            self.mj_model, self.mj_data, robot_bodies, robot_bodies, return_all_contact_bodies=True
+        )
+        if self_collision:
+            print(f"Warning: Self-collision detected: {contact_bodies}")
+        return self_collision
+
+    def reset(self):
+        mujoco.mj_resetData(self.mj_model, self.mj_data)
+
+
+class CubeEnv(DefaultEnv):
+    """Environment with a cube object for pick and place tasks"""
+
+    def __init__(
+        self,
+        config: Dict[str, any],
+        onscreen: bool = False,
+        offscreen: bool = False,
+        enable_image_publish: bool = False,
+    ):
+        # Override the robot scene
+        config = config.copy()  # Create a copy to avoid modifying the original
+        config["ROBOT_SCENE"] = "decoupled_wbc/control/robot_model/model_data/g1/pnp_cube_43dof.xml"
+        super().__init__(config, "cube", {}, onscreen, offscreen, enable_image_publish)
+
+    def update_reward(self):
+        """Calculate reward based on gripper contact with cube and cube height"""
+        right_hand_body = [
+            "right_hand_thumb_2_link",
+            "right_hand_middle_1_link",
+            "right_hand_index_1_link",
+        ]
+        gripper_cube_contact = check_contact(
+            self.mj_model, self.mj_data, right_hand_body, "cube_body"
+        )
+        cube_lifted = check_height(self.mj_model, self.mj_data, "cube", 0.85, 2.0)
+
+        with self.reward_lock:
+            self.last_reward = gripper_cube_contact & cube_lifted
+
+
+class BoxEnv(DefaultEnv):
+    """Environment with a box object for manipulation tasks"""
+
+    def __init__(
+        self,
+        config: Dict[str, any],
+        onscreen: bool = False,
+        offscreen: bool = False,
+        enable_image_publish: bool = False,
+    ):
+        # Override the robot scene
+        config = config.copy()  # Create a copy to avoid modifying the original
+        config["ROBOT_SCENE"] = "decoupled_wbc/control/robot_model/model_data/g1/lift_box_43dof.xml"
+        super().__init__(config, "box", {}, onscreen, offscreen, enable_image_publish)
+
+    def reward(self):
+        """Calculate reward based on gripper contact with cube and cube height"""
+        left_hand_body = [
+            "left_hand_thumb_2_link",
+            "left_hand_middle_1_link",
+            "left_hand_index_1_link",
+        ]
+        right_hand_body = [
+            "right_hand_thumb_2_link",
+            "right_hand_middle_1_link",
+            "right_hand_index_1_link",
+        ]
+        gripper_box_contact = check_contact(self.mj_model, self.mj_data, left_hand_body, "box_body")
+        gripper_box_contact &= check_contact(
+            self.mj_model, self.mj_data, right_hand_body, "box_body"
+        )
+        box_lifted = check_height(self.mj_model, self.mj_data, "box", 0.92, 2.0)
+
+        print("gripper_box_contact: ", gripper_box_contact, "box_lifted: ", box_lifted)
+
+        with self.reward_lock:
+            self.last_reward = gripper_box_contact & box_lifted
+            return self.last_reward
+
+
+class BottleEnv(DefaultEnv):
+    """Environment with a cylinder object for manipulation tasks"""
+
+    def __init__(
+        self,
+        config: Dict[str, any],
+        onscreen: bool = False,
+        offscreen: bool = False,
+        enable_image_publish: bool = False,
+    ):
+        # Override the robot scene
+        config = config.copy()  # Create a copy to avoid modifying the original
+        config["ROBOT_SCENE"] = "decoupled_wbc/control/robot_model/model_data/g1/pnp_bottle_43dof.xml"
+        camera_configs = {
+            "egoview": {
+                "height": 400,
+                "width": 400,
+            },
+        }
+        super().__init__(
+            config, "cylinder", camera_configs, onscreen, offscreen, enable_image_publish
+        )
+
+        self.bottle_body = self.mj_model.body("bottle_body")
+        self.bottle_geom = self.mj_model.geom("bottle")
+
+        if self.viewer is not None:
+            self.viewer.cam.type = mujoco.mjtCamera.mjCAMERA_FIXED
+            self.viewer.cam.fixedcamid = self.mj_model.camera("egoview").id
+
+    def update_reward(self):
+        """Calculate reward based on gripper contact with cylinder and cylinder height"""
+        pass
+
+    def get_privileged_obs(self):
+        obs_pos = self.mj_data.xpos[self.bottle_body.id]
+        obs_quat = self.mj_data.xquat[self.bottle_body.id]
+        return {"bottle_pos": obs_pos, "bottle_quat": obs_quat}
+
+
+class BaseSimulator:
+    """Base simulator class that handles initialization and running of simulations"""
+
+    def __init__(self, config: Dict[str, any], env_name: str = "default", **kwargs):
+        self.config = config
+        self.env_name = env_name
+
+        # Initialize ROS 2 node
+        if not rclpy.ok():
+            rclpy.init()
+            self.node = rclpy.create_node("sim_mujoco")
+            self.thread = threading.Thread(target=rclpy.spin, args=(self.node,), daemon=True)
+            self.thread.start()
+        else:
+            self.thread = None
+            executor = rclpy.get_global_executor()
+            self.node = executor.get_nodes()[0]  # will only take the first node
+
+        # Create rate objects for different update frequencies
+        self.sim_dt = self.config["SIMULATE_DT"]
+        self.reward_dt = self.config.get("REWARD_DT", 0.02)
+        self.image_dt = self.config.get("IMAGE_DT", 0.033333)
+        self.viewer_dt = self.config.get("VIEWER_DT", 0.02)
+        self.rate = self.node.create_rate(1 / self.sim_dt)
+
+        # Create the appropriate environment based on name
+        if env_name == "default":
+            self.sim_env = DefaultEnv(config, env_name, **kwargs)
+        elif env_name == "pnp_cube":
+            self.sim_env = CubeEnv(config, **kwargs)
+        elif env_name == "lift_box":
+            self.sim_env = BoxEnv(config, **kwargs)
+        elif env_name == "pnp_bottle":
+            self.sim_env = BottleEnv(config, **kwargs)
+        else:
+            raise ValueError(f"Invalid environment name: {env_name}")
+
+        # Initialize the DDS communication layer - should be safe to call multiple times
+
+        try:
+            if self.config.get("INTERFACE", None):
+                ChannelFactoryInitialize(self.config["DOMAIN_ID"], self.config["INTERFACE"])
+            else:
+                ChannelFactoryInitialize(self.config["DOMAIN_ID"])
+        except Exception as e:
+            # If it fails because it's already initialized, that's okay
+            print(f"Note: Channel factory initialization attempt: {e}")
+
+        # Initialize the unitree bridge and pass it to the environment
+        self.init_unitree_bridge()
+        self.sim_env.set_unitree_bridge(self.unitree_bridge)
+
+        # Initialize additional components
+        self.init_subscriber()
+        self.init_publisher()
+
+        self.sim_thread = None
+
+    def start_as_thread(self):
+        # Create simulation thread
+        self.sim_thread = Thread(target=self.start)
+        self.sim_thread.start()
+
+    def start_image_publish_subprocess(self, start_method: str = "spawn", camera_port: int = 5555):
+        """Start the image publish subprocess"""
+        self.sim_env.start_image_publish_subprocess(start_method, camera_port)
+
+    def init_subscriber(self):
+        """Initialize subscribers. Can be overridden by subclasses."""
+        pass
+
+    def init_publisher(self):
+        """Initialize publishers. Can be overridden by subclasses."""
+        pass
+
+    def init_unitree_bridge(self):
+        """Initialize the unitree SDK bridge"""
+        self.unitree_bridge = UnitreeSdk2Bridge(self.config)
+        if self.config["USE_JOYSTICK"]:
+            self.unitree_bridge.SetupJoystick(
+                device_id=self.config["JOYSTICK_DEVICE"], js_type=self.config["JOYSTICK_TYPE"]
+            )
+
+    def start(self):
+        """Main simulation loop"""
+        sim_cnt = 0
+
+        try:
+            while (
+                self.sim_env.viewer and self.sim_env.viewer.is_running()
+            ) or self.sim_env.viewer is None:
+                # Run simulation step
+                self.sim_env.sim_step()
+
+                # Update viewer at viewer rate
+                if sim_cnt % int(self.viewer_dt / self.sim_dt) == 0:
+                    self.sim_env.update_viewer()
+
+                # Calculate reward at reward rate
+                if sim_cnt % int(self.reward_dt / self.sim_dt) == 0:
+                    self.sim_env.update_reward()
+
+                # Update render caches at image rate
+                if sim_cnt % int(self.image_dt / self.sim_dt) == 0:
+                    self.sim_env.update_render_caches()
+
+                # Sleep to maintain correct rate
+                self.rate.sleep()
+
+                sim_cnt += 1
+        except rclpy.exceptions.ROSInterruptException:
+            # This is expected when ROS shuts down - exit cleanly
+            pass
+        except Exception:
+            self.close()
+
+    def __del__(self):
+        """Clean up resources when simulator is deleted"""
+        self.close()
+
+    def reset(self):
+        """Reset the simulation. Can be overridden by subclasses."""
+        self.sim_env.reset()
+
+    def close(self):
+        """Close the simulation. Can be overridden by subclasses."""
+        try:
+            # Stop image publishing subprocess
+            if self.sim_env.image_publish_process is not None:
+                self.sim_env.image_publish_process.stop()
+
+            # Close viewer
+            if hasattr(self.sim_env, "viewer") and self.sim_env.viewer is not None:
+                self.sim_env.viewer.close()
+
+            # Shutdown ROS
+            if rclpy.ok():
+                rclpy.shutdown()
+        except Exception as e:
+            print(f"Warning during close: {e}")
+
+    def get_privileged_obs(self):
+        obs = self.sim_env.get_privileged_obs()
+        # TODO: add ros2 topic to get privileged obs
+        return obs
+
+    def handle_keyboard_button(self, key):
+        # Only handles keyboard buttons for default env.
+        if self.env_name == "default":
+            self.sim_env.handle_keyboard_button(key)
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser(description="Robot")
+    parser.add_argument(
+        "--config",
+        type=str,
+        default="./decoupled_wbc/control/main/teleop/configs/g1_29dof_gear_wbc.yaml",
+        help="config file",
+    )
+    args = parser.parse_args()
+
+    with open(args.config, "r") as file:
+        config = yaml.load(file, Loader=yaml.FullLoader)
+
+    if config.get("INTERFACE", None):
+        ChannelFactoryInitialize(config["DOMAIN_ID"], config["INTERFACE"])
+    else:
+        ChannelFactoryInitialize(config["DOMAIN_ID"])
+
+    simulation = BaseSimulator(config)
+    simulation.start_as_thread()

GR00T-WholeBodyControl/decoupled_wbc/control/envs/g1/sim/image_publish_utils.py

@@ -0,0 +1,256 @@
+import multiprocessing as mp
+from multiprocessing import shared_memory
+import time
+from typing import Any, Dict
+
+import numpy as np
+
+from decoupled_wbc.control.sensor.sensor_server import ImageMessageSchema, SensorServer
+
+
+def get_multiprocessing_info(verbose: bool = True):
+    """Get information about multiprocessing start methods"""
+
+    if verbose:
+        print(f"Available start methods: {mp.get_all_start_methods()}")
+    return mp.get_start_method()
+
+
+class ImagePublishProcess:
+    """Subprocess for publishing images using shared memory and ZMQ"""
+
+    def __init__(
+        self,
+        camera_configs: Dict[str, Any],
+        image_dt: float,
+        zmq_port: int = 5555,
+        start_method: str = "spawn",
+        verbose: bool = False,
+    ):
+        self.camera_configs = camera_configs
+        self.image_dt = image_dt
+        self.zmq_port = zmq_port
+        self.verbose = verbose
+        self.shared_memory_blocks = {}
+        self.shared_memory_info = {}
+        self.process = None
+
+        # Use specific context to avoid global state pollution
+        self.mp_context = mp.get_context(start_method)
+        if self.verbose:
+            print(f"Using multiprocessing context: {start_method}")
+
+        self.stop_event = self.mp_context.Event()
+        self.data_ready_event = self.mp_context.Event()
+
+        # Ensure events start in correct state
+        self.stop_event.clear()
+        self.data_ready_event.clear()
+
+        if self.verbose:
+            print(f"Initial stop_event state: {self.stop_event.is_set()}")
+            print(f"Initial data_ready_event state: {self.data_ready_event.is_set()}")
+
+        # Calculate shared memory requirements for each camera
+        for camera_name, camera_config in camera_configs.items():
+            height = camera_config["height"]
+            width = camera_config["width"]
+            # RGB image: height * width * 3 (uint8)
+            size = height * width * 3
+
+            # Create shared memory block
+            shm = shared_memory.SharedMemory(create=True, size=size)
+            self.shared_memory_blocks[camera_name] = shm
+            self.shared_memory_info[camera_name] = {
+                "name": shm.name,
+                "size": size,
+                "shape": (height, width, 3),
+                "dtype": np.uint8,
+            }
+
+    def start_process(self):
+        """Start the image publishing subprocess"""
+        if self.verbose:
+            print(f"Starting subprocess with stop_event state: {self.stop_event.is_set()}")
+        self.process = self.mp_context.Process(
+            target=self._image_publish_worker,
+            args=(
+                self.shared_memory_info,
+                self.image_dt,
+                self.zmq_port,
+                self.stop_event,
+                self.data_ready_event,
+                self.verbose,
+            ),
+        )
+        self.process.start()
+        if self.verbose:
+            print(f"Subprocess started, PID: {self.process.pid}")
+
+    def update_shared_memory(self, render_caches: Dict[str, np.ndarray]):
+        """Update shared memory with new rendered images"""
+        images_updated = 0
+        for camera_name in self.camera_configs.keys():
+            image_key = f"{camera_name}_image"
+            if image_key in render_caches:
+                image = render_caches[image_key]
+
+                # Ensure image is uint8 and has correct shape
+                if image.dtype != np.uint8:
+                    image = (image * 255).astype(np.uint8)
+
+                # Get shared memory array
+                shm = self.shared_memory_blocks[camera_name]
+                shared_array = np.ndarray(
+                    self.shared_memory_info[camera_name]["shape"],
+                    dtype=self.shared_memory_info[camera_name]["dtype"],
+                    buffer=shm.buf,
+                )
+
+                # Copy image data to shared memory atomically
+                np.copyto(shared_array, image)
+                images_updated += 1
+
+        # Signal that new data is ready only after all images are written
+        if images_updated > 0:
+            if self.verbose:
+                print(f"Main process: Updated {images_updated} images, setting data_ready_event")
+            self.data_ready_event.set()
+        elif self.verbose:
+            print(
+                "Main process: No images to update. "
+                "please check if camera configs are provided and the renderer is properly initialized"
+            )
+
+    def stop(self):
+        """Stop the image publishing subprocess"""
+        if self.verbose:
+            print("Stopping image publishing subprocess...")
+        self.stop_event.set()
+
+        if self.process and self.process.is_alive():
+            # Give the process time to clean up gracefully
+            self.process.join(timeout=5)
+            if self.process.is_alive():
+                if self.verbose:
+                    print("Subprocess didn't stop gracefully, terminating...")
+                self.process.terminate()
+                self.process.join(timeout=2)
+                if self.process.is_alive():
+                    if self.verbose:
+                        print("Force killing subprocess...")
+                    self.process.kill()
+                    self.process.join()
+
+        # Clean up shared memory
+        for camera_name, shm in self.shared_memory_blocks.items():
+            try:
+                shm.close()
+                shm.unlink()
+                if self.verbose:
+                    print(f"Cleaned up shared memory for {camera_name}")
+            except Exception as e:
+                if self.verbose:
+                    print(f"Warning: Failed to cleanup shared memory for {camera_name}: {e}")
+
+        self.shared_memory_blocks.clear()
+        if self.verbose:
+            print("Image publishing subprocess stopped and cleaned up")
+
+    @staticmethod
+    def _image_publish_worker(
+        shared_memory_info, image_dt, zmq_port, stop_event, data_ready_event, verbose
+    ):
+        """Worker function that runs in the subprocess"""
+        if verbose:
+            print(f"Worker started! PID: {__import__('os').getpid()}")
+            print(f"Worker stop_event state at start: {stop_event.is_set()}")
+            print(f"Worker data_ready_event state at start: {data_ready_event.is_set()}")
+
+        try:
+            # Initialize ZMQ sensor server
+            sensor_server = SensorServer()
+            sensor_server.start_server(port=zmq_port)
+
+            # Connect to shared memory blocks
+            shared_arrays = {}
+            shm_blocks = {}
+            for camera_name, info in shared_memory_info.items():
+                shm = shared_memory.SharedMemory(name=info["name"])
+                shm_blocks[camera_name] = shm
+                shared_arrays[camera_name] = np.ndarray(
+                    info["shape"], dtype=info["dtype"], buffer=shm.buf
+                )
+
+            print(
+                f"Image publishing subprocess started with {len(shared_arrays)} cameras on ZMQ port {zmq_port}"
+            )
+
+            loop_count = 0
+            last_data_time = time.time()
+
+            while not stop_event.is_set():
+                loop_count += 1
+
+                # Wait for new data with shorter timeout for better responsiveness
+                timeout = min(image_dt, 0.1)  # Max 100ms timeout
+                data_available = data_ready_event.wait(timeout=timeout)
+
+                current_time = time.time()
+
+                if data_available:
+                    data_ready_event.clear()
+                    if loop_count % 50 == 0:
+                        print("Image publish frequency: ", 1 / (current_time - last_data_time))
+                    last_data_time = current_time
+
+                    # Collect all camera images and serialize them
+                    try:
+                        from decoupled_wbc.control.sensor.sensor_server import ImageUtils
+
+                        # Copy all images atomically at once
+                        image_copies = {name: arr.copy() for name, arr in shared_arrays.items()}
+
+                        # Create message with all camera images
+                        message_dict = {
+                            "images": image_copies,
+                            "timestamps": {name: current_time for name in image_copies.keys()},
+                        }
+
+                        # Create ImageMessageSchema and serialize
+                        image_msg = ImageMessageSchema(
+                            timestamps=message_dict.get("timestamps"),
+                            images=message_dict.get("images", None),
+                        )
+
+                        # Serialize and send via ZMQ
+                        serialized_data = image_msg.serialize()
+
+                        # Add individual camera images to the message
+                        for camera_name, image_copy in image_copies.items():
+                            serialized_data[f"{camera_name}"] = ImageUtils.encode_image(image_copy)
+
+                        sensor_server.send_message(serialized_data)
+
+                    except Exception as e:
+                        print(f"Error publishing images: {e}")
+
+                elif verbose and loop_count % 10 == 0:
+                    print(f"Subprocess: Still waiting for data... (iteration {loop_count})")
+
+                # Small sleep to prevent busy waiting when no data
+                if not data_available:
+                    time.sleep(0.001)
+
+        except KeyboardInterrupt:
+            print("Image publisher interrupted by user")
+        finally:
+            # Clean up
+            try:
+                for shm in shm_blocks.values():
+                    shm.close()
+                sensor_server.stop_server()
+            except Exception as e:
+                print(f"Error during subprocess cleanup: {e}")
+            if verbose:
+                print("Image publish subprocess stopped")

GR00T-WholeBodyControl/decoupled_wbc/control/envs/g1/sim/metric_utils.py

@@ -0,0 +1,71 @@
+from typing import List, Tuple
+
+import mujoco
+
+from decoupled_wbc.control.envs.g1.sim.sim_utilts import get_body_geom_ids
+
+
+def check_contact(
+    mj_model: mujoco.MjModel,
+    mj_data: mujoco.MjData,
+    bodies_1: List[str] | str,
+    bodies_2: List[str] | str,
+    return_all_contact_bodies: bool = False,
+) -> Tuple[bool, List[Tuple[str, str]]] | bool:
+    """
+    Finds contact between two body groups. Any geom in the body is considered to be in contact.
+    Args:
+        mj_model (MujocoModel): Current simulation object
+        mj_data (MjData): Current simulation data
+        bodies_1 (str or list of int): an individual body name or list of body names.
+        bodies_2 (str or list of int): another individual body name or list of body names.
+    Returns:
+        bool: True if any body in @bodies_1 is in contact with any body in @bodies_2.
+    """
+    if isinstance(bodies_1, str):
+        bodies_1 = [bodies_1]
+    if isinstance(bodies_2, str):
+        bodies_2 = [bodies_2]
+
+    geoms_1 = [get_body_geom_ids(mj_model, mj_model.body(g).id) for g in bodies_1]
+    geoms_1 = [g for geom_list in geoms_1 for g in geom_list]
+    geoms_2 = [get_body_geom_ids(mj_model, mj_model.body(g).id) for g in bodies_2]
+    geoms_2 = [g for geom_list in geoms_2 for g in geom_list]
+    contact_bodies = []
+    for i in range(mj_data.ncon):
+        contact = mj_data.contact[i]
+        # check contact geom in geoms
+        c1_in_g1 = contact.geom1 in geoms_1
+        c2_in_g2 = contact.geom2 in geoms_2 if geoms_2 is not None else True
+        # check contact geom in geoms (flipped)
+        c2_in_g1 = contact.geom2 in geoms_1
+        c1_in_g2 = contact.geom1 in geoms_2 if geoms_2 is not None else True
+        if (c1_in_g1 and c2_in_g2) or (c1_in_g2 and c2_in_g1):
+            contact_bodies.append(
+                (
+                    mj_model.body(mj_model.geom(contact.geom1).bodyid).name,
+                    mj_model.body(mj_model.geom(contact.geom2).bodyid).name,
+                )
+            )
+            if not return_all_contact_bodies:
+                break
+    if return_all_contact_bodies:
+        return len(contact_bodies) > 0, set(contact_bodies)
+    else:
+        return len(contact_bodies) > 0
+
+
+def check_height(
+    mj_model: mujoco.MjModel,
+    mj_data: mujoco.MjData,
+    geom_name: str,
+    lower_bound: float = -float("inf"),
+    upper_bound: float = float("inf"),
+):
+    """
+    Checks if the height of a geom is greater than a given height.
+    """
+    geom_id = mj_model.geom(geom_name).id
+    return (
+        mj_data.geom_xpos[geom_id][2] < upper_bound and mj_data.geom_xpos[geom_id][2] > lower_bound
+    )

GR00T-WholeBodyControl/decoupled_wbc/control/envs/g1/sim/robocasa_sim.py

@@ -0,0 +1,63 @@
+from typing import Any, Dict, Tuple
+
+from unitree_sdk2py.core.channel import ChannelFactoryInitialize
+
+from decoupled_wbc.control.envs.g1.sim.unitree_sdk2py_bridge import UnitreeSdk2Bridge
+from decoupled_wbc.control.envs.robocasa.async_env_server import RoboCasaEnvServer
+from decoupled_wbc.control.robot_model.instantiation import get_robot_type_and_model
+
+
+class RoboCasaG1EnvServer(RoboCasaEnvServer):
+    def __init__(
+        self, env_name: str, wbc_config: Dict[str, Any], env_kwargs: Dict[str, Any], **kwargs
+    ):
+        if UnitreeSdk2Bridge is None:
+            raise ImportError("UnitreeSdk2Bridge is required for RoboCasaG1EnvServer")
+        self.wbc_config = wbc_config
+        _, robot_model = get_robot_type_and_model(
+            "G1",
+            enable_waist_ik=wbc_config["enable_waist"],
+        )
+        if env_kwargs.get("camera_names", None) is None:
+            env_kwargs["camera_names"] = [
+                "robot0_oak_egoview",
+                "robot0_oak_left_monoview",
+                "robot0_oak_right_monoview",
+                "robot0_rs_tppview",
+            ]
+        if env_kwargs.get("render_camera", None) is None:
+            if env_kwargs.get("renderer", "mjviewer") == "mjviewer":
+                env_kwargs["render_camera"] = "robot0_oak_egoview"
+            else:
+                env_kwargs["render_camera"] = [
+                    "robot0_oak_egoview",
+                    "robot0_rs_tppview",
+                ]
+
+        super().__init__(env_name, "G1", robot_model, env_kwargs=env_kwargs, **kwargs)
+
+    def init_channel(self):
+
+        try:
+            if self.wbc_config.get("INTERFACE", None):
+                ChannelFactoryInitialize(self.wbc_config["DOMAIN_ID"], self.wbc_config["INTERFACE"])
+            else:
+                ChannelFactoryInitialize(self.wbc_config["DOMAIN_ID"])
+        except Exception:
+            # If it fails because it's already initialized, that's okay
+            pass
+
+        self.channel_bridge = UnitreeSdk2Bridge(config=self.wbc_config)
+
+    def publish_obs(self):
+        # with self.cache_lock:
+        obs = self.caches["obs"]
+        self.channel_bridge.PublishLowState(obs)
+
+    def get_action(self) -> Tuple[Dict[str, Any], bool, bool]:
+        q, ready, is_new_action = self.channel_bridge.GetAction()
+        return {"q": q}, ready, is_new_action
+
+    def reset(self):
+        super().reset()
+        self.channel_bridge.reset()

GR00T-WholeBodyControl/decoupled_wbc/control/envs/g1/sim/sim_utilts.py

@@ -0,0 +1,96 @@
+"""
+Utility functions for working with Mujoco models.
+copied from https://github.com/kevinzakka/mink/blob/main/mink/utils.py
+"""
+
+from typing import List
+
+import mujoco
+
+
+def get_body_body_ids(model: mujoco.MjModel, body_id: int) -> List[int]:
+    """Get immediate children bodies belonging to a given body.
+
+    Args:
+        model: Mujoco model.
+        body_id: ID of body.
+
+    Returns:
+        A List containing all child body ids.
+    """
+    return [
+        i
+        for i in range(model.nbody)
+        if model.body_parentid[i] == body_id and body_id != i  # Exclude the body itself.
+    ]
+
+
+def get_subtree_body_ids(model: mujoco.MjModel, body_id: int) -> List[int]:
+    """Get all bodies belonging to subtree starting at a given body.
+
+    Args:
+        model: Mujoco model.
+        body_id: ID of body where subtree starts.
+
+    Returns:
+        A List containing all subtree body ids.
+    """
+    body_ids: List[int] = []
+    stack = [body_id]
+    while stack:
+        body_id = stack.pop()
+        body_ids.append(body_id)
+        stack += get_body_body_ids(model, body_id)
+    return body_ids
+
+
+def get_subtree_body_names(model: mujoco.MjModel, body_id: int) -> List[str]:
+    """Get all bodies belonging to subtree starting at a given body.
+    Args:
+        model: Mujoco model.
+        body_id: ID of body where subtree starts.
+
+    Returns:
+        A List containing all subtree body names.
+    """
+    return [model.body(i).name for i in get_subtree_body_ids(model, body_id)]
+
+
+def get_body_geom_ids(model: mujoco.MjModel, body_id: int) -> List[int]:
+    """Get immediate geoms belonging to a given body.
+
+    Here, immediate geoms are those directly attached to the body and not its
+    descendants.
+
+    Args:
+        model: Mujoco model.
+        body_id: ID of body.
+
+    Returns:
+        A list containing all body geom ids.
+    """
+    geom_start = model.body_geomadr[body_id]
+    geom_end = geom_start + model.body_geomnum[body_id]
+    return list(range(geom_start, geom_end))
+
+
+def get_subtree_geom_ids(model: mujoco.MjModel, body_id: int) -> List[int]:
+    """Get all geoms belonging to subtree starting at a given body.
+
+    Here, a subtree is defined as the kinematic tree starting at the body and including
+    all its descendants.
+
+    Args:
+        model: Mujoco model.
+        body_id: ID of body where subtree starts.
+
+    Returns:
+        A list containing all subtree geom ids.
+    """
+    geom_ids: List[int] = []
+    stack = [body_id]
+    while stack:
+        body_id = stack.pop()
+        geom_ids.extend(get_body_geom_ids(model, body_id))
+        stack += get_body_body_ids(model, body_id)
+    return geom_ids

GR00T-WholeBodyControl/decoupled_wbc/control/envs/g1/sim/simulator_factory.py

@@ -0,0 +1,144 @@
+import time
+from typing import Any, Dict
+
+from unitree_sdk2py.core.channel import ChannelFactoryInitialize
+
+from decoupled_wbc.control.envs.g1.sim.base_sim import BaseSimulator
+
+
+def init_channel(config: Dict[str, Any]) -> None:
+    """
+    Initialize the communication channel for simulator/robot communication.
+
+    Args:
+        config: Configuration dictionary containing DOMAIN_ID and optionally INTERFACE
+    """
+    if config.get("INTERFACE", None):
+        ChannelFactoryInitialize(config["DOMAIN_ID"], config["INTERFACE"])
+    else:
+        ChannelFactoryInitialize(config["DOMAIN_ID"])
+
+
+class SimulatorFactory:
+    """Factory class for creating different types of simulators."""
+
+    @staticmethod
+    def create_simulator(config: Dict[str, Any], env_name: str = "default", **kwargs):
+        """
+        Create a simulator based on the configuration.
+
+        Args:
+            config: Configuration dictionary containing SIMULATOR type
+            env_name: Environment name
+            **kwargs: Additional keyword arguments for specific simulators
+        """
+        simulator_type = config.get("SIMULATOR", "mujoco")
+        if simulator_type == "mujoco":
+            return SimulatorFactory._create_mujoco_simulator(config, env_name, **kwargs)
+        elif simulator_type == "robocasa":
+            return SimulatorFactory._create_robocasa_simulator(config, env_name, **kwargs)
+        else:
+            print(
+                f"Warning: Invalid simulator type: {simulator_type}. "
+                "If you are using run_sim_loop, please ignore this warning."
+            )
+            return None
+
+    @staticmethod
+    def _create_mujoco_simulator(config: Dict[str, Any], env_name: str = "default", **kwargs):
+        """Create a MuJoCo simulator instance."""
+        env_kwargs = dict(
+            onscreen=kwargs.pop("onscreen", True),
+            offscreen=kwargs.pop("offscreen", False),
+            enable_image_publish=kwargs.get("enable_image_publish", False),
+        )
+        return BaseSimulator(config=config, env_name=env_name, **env_kwargs)
+
+    @staticmethod
+    def _create_robocasa_simulator(config: Dict[str, Any], env_name: str = "default", **kwargs):
+        """Create a RoboCasa simulator instance."""
+        from decoupled_wbc.control.envs.g1.sim.robocasa_sim import RoboCasaG1EnvServer
+        from decoupled_wbc.control.envs.robocasa.utils.controller_utils import (
+            update_robosuite_controller_configs,
+        )
+        from decoupled_wbc.control.envs.robocasa.utils.sim_utils import change_simulation_timestep
+
+        change_simulation_timestep(config["SIMULATE_DT"])
+
+        # Use default environment if not specified
+        if env_name == "default":
+            env_name = "GroundOnly"
+
+        # Get or create controller configurations
+        controller_configs = kwargs.get("controller_configs")
+        if controller_configs is None:
+            wbc_version = kwargs.get("wbc_version", "gear_wbc")
+            controller_configs = update_robosuite_controller_configs("G1", wbc_version)
+
+        # Build environment kwargs
+        env_kwargs = dict(
+            onscreen=kwargs.pop("onscreen", True),
+            offscreen=kwargs.pop("offscreen", False),
+            camera_names=kwargs.pop("camera_names", None),
+            camera_heights=kwargs.pop("camera_heights", None),
+            camera_widths=kwargs.pop("camera_widths", None),
+            control_freq=kwargs.pop("control_freq", 50),
+            controller_configs=controller_configs,
+            ik_indicator=kwargs.pop("ik_indicator", False),
+            randomize_cameras=kwargs.pop("randomize_cameras", True),
+        )
+
+        kwargs.update(
+            {
+                "verbose": config.pop("verbose", False),
+                "sim_freq": 1 / config.pop("SIMULATE_DT"),
+            }
+        )
+
+        return RoboCasaG1EnvServer(
+            env_name=env_name,
+            wbc_config=config,
+            env_kwargs=env_kwargs,
+            **kwargs,
+        )
+
+    @staticmethod
+    def start_simulator(
+        simulator,
+        as_thread: bool = True,
+        enable_image_publish: bool = False,
+        mp_start_method: str = "spawn",
+        camera_port: int = 5555,
+    ):
+        """
+        Start the simulator either as a thread or as a separate process.
+
+        Args:
+            simulator: The simulator instance to start
+            config: Configuration dictionary
+            as_thread: If True, start as thread; if False, start as subprocess
+            enable_offscreen: If True and not as_thread, start image publishing
+        """
+
+        if as_thread:
+            simulator.start_as_thread()
+        else:
+            # Wrap in try-except to make sure simulator is properly closed upon exit.
+            try:
+                if enable_image_publish:
+                    simulator.start_image_publish_subprocess(
+                        start_method=mp_start_method,
+                        camera_port=camera_port,
+                    )
+                    time.sleep(1)
+                simulator.start()
+            except KeyboardInterrupt:
+                print("+++++Simulator interrupted by user.")
+            except Exception as e:
+                print(f"++++error in simulator: {e} ++++")
+            finally:
+                print("++++closing simulator ++++")
+                simulator.close()
+
+        # Allow simulator to initialize
+        time.sleep(1)

GR00T-WholeBodyControl/decoupled_wbc/control/envs/g1/sim/unitree_sdk2py_bridge.py

@@ -0,0 +1,459 @@
+import sys
+import threading
+from typing import Dict, Tuple
+
+import glfw
+from loguru import logger
+import mujoco
+import numpy as np
+import pygame
+import scipy.spatial.transform
+from termcolor import colored
+from unitree_sdk2py.core.channel import ChannelPublisher, ChannelSubscriber
+from unitree_sdk2py.idl.default import (
+    unitree_go_msg_dds__WirelessController_,
+    unitree_hg_msg_dds__HandCmd_ as HandCmd_default,
+    unitree_hg_msg_dds__HandState_ as HandState_default,
+)
+from unitree_sdk2py.idl.unitree_go.msg.dds_ import WirelessController_
+from unitree_sdk2py.idl.unitree_hg.msg.dds_ import HandCmd_, HandState_, OdoState_
+
+
+class UnitreeSdk2Bridge:
+    """
+    This class is responsible for bridging the Unitree SDK2 with the Gr00t environment.
+    It is responsible for sending and receiving messages to and from the Unitree SDK2.
+    Both the body and hand are supported.
+    """
+
+    def __init__(self, config):
+        # Note that we do not give the mjdata and mjmodel to the UnitreeSdk2Bridge.
+        # It is unsafe and would be unflexible if we use a hand-plugged robot model
+
+        robot_type = config["ROBOT_TYPE"]
+        if "g1" in robot_type or "h1-2" in robot_type:
+            from unitree_sdk2py.idl.default import (
+                unitree_hg_msg_dds__IMUState_ as IMUState_default,
+                unitree_hg_msg_dds__LowCmd_,
+                unitree_hg_msg_dds__LowState_ as LowState_default,
+                unitree_hg_msg_dds__OdoState_ as OdoState_default,
+            )
+            from unitree_sdk2py.idl.unitree_hg.msg.dds_ import IMUState_, LowCmd_, LowState_
+
+            self.low_cmd = unitree_hg_msg_dds__LowCmd_()
+        elif "h1" == robot_type or "go2" == robot_type:
+            from unitree_sdk2py.idl.default import (
+                unitree_go_msg_dds__LowCmd_,
+                unitree_go_msg_dds__LowState_ as LowState_default,
+                unitree_hg_msg_dds__IMUState_ as IMUState_default,
+            )
+            from unitree_sdk2py.idl.unitree_go.msg.dds_ import IMUState_, LowCmd_, LowState_
+
+            self.low_cmd = unitree_go_msg_dds__LowCmd_()
+        else:
+            raise ValueError(f"Invalid robot type '{robot_type}'. Expected 'g1', 'h1', or 'go2'.")
+
+        self.num_body_motor = config["NUM_MOTORS"]
+        self.num_hand_motor = config.get("NUM_HAND_MOTORS", 0)
+        self.use_sensor = config["USE_SENSOR"]
+
+        self.have_imu_ = False
+        self.have_frame_sensor_ = False
+        # if self.use_sensor:
+        #     MOTOR_SENSOR_NUM = 3
+        #     self.dim_motor_sensor = MOTOR_SENSOR_NUM * self.num_motor
+        #     # Check sensor
+        #     for i in range(self.dim_motor_sensor, self.mj_model.nsensor):
+        #         name = mujoco.mj_id2name(self.mj_model, mujoco._enums.mjtObj.mjOBJ_SENSOR, i)
+        #         if name == "imu_quat":
+        #             self.have_imu_ = True
+        #         if name == "frame_pos":
+        #             self.have_frame_sensor_ = True
+
+        # Unitree sdk2 message
+        self.low_state = LowState_default()
+        self.low_state_puber = ChannelPublisher("rt/lowstate", LowState_)
+        self.low_state_puber.Init()
+
+        # Only create odo_state for supported robot types
+        if "g1" in robot_type or "h1-2" in robot_type:
+            self.odo_state = OdoState_default()
+            self.odo_state_puber = ChannelPublisher("rt/odostate", OdoState_)
+            self.odo_state_puber.Init()
+        else:
+            self.odo_state = None
+            self.odo_state_puber = None
+        self.torso_imu_state = IMUState_default()
+        self.torso_imu_puber = ChannelPublisher("rt/secondary_imu", IMUState_)
+        self.torso_imu_puber.Init()
+
+        self.left_hand_state = HandState_default()
+        self.left_hand_state_puber = ChannelPublisher("rt/dex3/left/state", HandState_)
+        self.left_hand_state_puber.Init()
+        self.right_hand_state = HandState_default()
+        self.right_hand_state_puber = ChannelPublisher("rt/dex3/right/state", HandState_)
+        self.right_hand_state_puber.Init()
+
+        self.low_cmd_suber = ChannelSubscriber("rt/lowcmd", LowCmd_)
+        self.low_cmd_suber.Init(self.LowCmdHandler, 1)
+
+        self.left_hand_cmd = HandCmd_default()
+        self.left_hand_cmd_suber = ChannelSubscriber("rt/dex3/left/cmd", HandCmd_)
+        self.left_hand_cmd_suber.Init(self.LeftHandCmdHandler, 1)
+        self.right_hand_cmd = HandCmd_default()
+        self.right_hand_cmd_suber = ChannelSubscriber("rt/dex3/right/cmd", HandCmd_)
+        self.right_hand_cmd_suber.Init(self.RightHandCmdHandler, 1)
+
+        self.low_cmd_lock = threading.Lock()
+        self.left_hand_cmd_lock = threading.Lock()
+        self.right_hand_cmd_lock = threading.Lock()
+
+        self.wireless_controller = unitree_go_msg_dds__WirelessController_()
+        self.wireless_controller_puber = ChannelPublisher(
+            "rt/wirelesscontroller", WirelessController_
+        )
+        self.wireless_controller_puber.Init()
+
+        # joystick
+        self.key_map = {
+            "R1": 0,
+            "L1": 1,
+            "start": 2,
+            "select": 3,
+            "R2": 4,
+            "L2": 5,
+            "F1": 6,
+            "F2": 7,
+            "A": 8,
+            "B": 9,
+            "X": 10,
+            "Y": 11,
+            "up": 12,
+            "right": 13,
+            "down": 14,
+            "left": 15,
+        }
+        self.joystick = None
+
+        self.reset()
+
+    def reset(self):
+        with self.low_cmd_lock:
+            self.low_cmd_received = False
+            self.new_low_cmd = False
+        with self.left_hand_cmd_lock:
+            self.left_hand_cmd_received = False
+            self.new_left_hand_cmd = False
+        with self.right_hand_cmd_lock:
+            self.right_hand_cmd_received = False
+            self.new_right_hand_cmd = False
+
+    def LowCmdHandler(self, msg):
+        with self.low_cmd_lock:
+            self.low_cmd = msg
+            self.low_cmd_received = True
+            self.new_low_cmd = True
+
+    def LeftHandCmdHandler(self, msg):
+        with self.left_hand_cmd_lock:
+            self.left_hand_cmd = msg
+            self.left_hand_cmd_received = True
+            self.new_left_hand_cmd = True
+
+    def RightHandCmdHandler(self, msg):
+        with self.right_hand_cmd_lock:
+            self.right_hand_cmd = msg
+            self.right_hand_cmd_received = True
+            self.new_right_hand_cmd = True
+
+    def cmd_received(self):
+        with self.low_cmd_lock:
+            low_cmd_received = self.low_cmd_received
+        with self.left_hand_cmd_lock:
+            left_hand_cmd_received = self.left_hand_cmd_received
+        with self.right_hand_cmd_lock:
+            right_hand_cmd_received = self.right_hand_cmd_received
+        return low_cmd_received or left_hand_cmd_received or right_hand_cmd_received
+
+    def PublishLowState(self, obs: Dict[str, any]):
+        # publish body state
+        if self.use_sensor:
+            raise NotImplementedError("Sensor data is not implemented yet.")
+        else:
+            for i in range(self.num_body_motor):
+                self.low_state.motor_state[i].q = obs["body_q"][i]
+                self.low_state.motor_state[i].dq = obs["body_dq"][i]
+                self.low_state.motor_state[i].ddq = obs["body_ddq"][i]
+                self.low_state.motor_state[i].tau_est = obs["body_tau_est"][i]
+
+        if self.use_sensor and self.have_frame_sensor_:
+            raise NotImplementedError("Frame sensor data is not implemented yet.")
+        else:
+            # Get data from ground truth
+            self.odo_state.position[:] = obs["floating_base_pose"][:3]
+            self.odo_state.linear_velocity[:] = obs["floating_base_vel"][:3]
+            self.odo_state.orientation[:] = obs["floating_base_pose"][3:7]
+            self.odo_state.angular_velocity[:] = obs["floating_base_vel"][3:6]
+            # quaternion: w, x, y, z
+            self.low_state.imu_state.quaternion[:] = obs["floating_base_pose"][3:7]
+            # angular velocity
+            self.low_state.imu_state.gyroscope[:] = obs["floating_base_vel"][3:6]
+            # linear acceleration
+            self.low_state.imu_state.accelerometer[:] = obs["floating_base_acc"][:3]
+
+            self.torso_imu_state.quaternion[:] = obs["secondary_imu_quat"]
+            self.torso_imu_state.gyroscope[:] = obs["secondary_imu_vel"][3:6]
+
+        # acceleration: x, y, z (only available when frame sensor is enabled)
+        if self.have_frame_sensor_:
+            raise NotImplementedError("Frame sensor data is not implemented yet.")
+        self.low_state.tick = int(obs["time"] * 1e3)
+        self.low_state_puber.Write(self.low_state)
+
+        self.odo_state.tick = int(obs["time"] * 1e3)
+        self.odo_state_puber.Write(self.odo_state)
+
+        self.torso_imu_puber.Write(self.torso_imu_state)
+
+        # publish hand state
+        for i in range(self.num_hand_motor):
+            self.left_hand_state.motor_state[i].q = obs["left_hand_q"][i]
+            self.left_hand_state.motor_state[i].dq = obs["left_hand_dq"][i]
+        self.left_hand_state_puber.Write(self.left_hand_state)
+
+        for i in range(self.num_hand_motor):
+            self.right_hand_state.motor_state[i].q = obs["right_hand_q"][i]
+            self.right_hand_state.motor_state[i].dq = obs["right_hand_dq"][i]
+        self.right_hand_state_puber.Write(self.right_hand_state)
+
+    def GetAction(self) -> Tuple[np.ndarray, bool, bool]:
+        with self.low_cmd_lock:
+            body_q = [self.low_cmd.motor_cmd[i].q for i in range(self.num_body_motor)]
+        with self.left_hand_cmd_lock:
+            left_hand_q = [self.left_hand_cmd.motor_cmd[i].q for i in range(self.num_hand_motor)]
+        with self.right_hand_cmd_lock:
+            right_hand_q = [self.right_hand_cmd.motor_cmd[i].q for i in range(self.num_hand_motor)]
+        with self.low_cmd_lock and self.left_hand_cmd_lock and self.right_hand_cmd_lock:
+            is_new_action = self.new_low_cmd and self.new_left_hand_cmd and self.new_right_hand_cmd
+            if is_new_action:
+                self.new_low_cmd = False
+                self.new_left_hand_cmd = False
+                self.new_right_hand_cmd = False
+
+        return (
+            np.concatenate([body_q[:-7], left_hand_q, body_q[-7:], right_hand_q]),
+            self.cmd_received(),
+            is_new_action,
+        )
+
+    def PublishWirelessController(self):
+        if self.joystick is not None:
+            pygame.event.get()
+            key_state = [0] * 16
+            key_state[self.key_map["R1"]] = self.joystick.get_button(self.button_id["RB"])
+            key_state[self.key_map["L1"]] = self.joystick.get_button(self.button_id["LB"])
+            key_state[self.key_map["start"]] = self.joystick.get_button(self.button_id["START"])
+            key_state[self.key_map["select"]] = self.joystick.get_button(self.button_id["SELECT"])
+            key_state[self.key_map["R2"]] = self.joystick.get_axis(self.axis_id["RT"]) > 0
+            key_state[self.key_map["L2"]] = self.joystick.get_axis(self.axis_id["LT"]) > 0
+            key_state[self.key_map["F1"]] = 0
+            key_state[self.key_map["F2"]] = 0
+            key_state[self.key_map["A"]] = self.joystick.get_button(self.button_id["A"])
+            key_state[self.key_map["B"]] = self.joystick.get_button(self.button_id["B"])
+            key_state[self.key_map["X"]] = self.joystick.get_button(self.button_id["X"])
+            key_state[self.key_map["Y"]] = self.joystick.get_button(self.button_id["Y"])
+            key_state[self.key_map["up"]] = self.joystick.get_hat(0)[1] > 0
+            key_state[self.key_map["right"]] = self.joystick.get_hat(0)[0] > 0
+            key_state[self.key_map["down"]] = self.joystick.get_hat(0)[1] < 0
+            key_state[self.key_map["left"]] = self.joystick.get_hat(0)[0] < 0
+
+            key_value = 0
+            for i in range(16):
+                key_value += key_state[i] << i
+
+            self.wireless_controller.keys = key_value
+            self.wireless_controller.lx = self.joystick.get_axis(self.axis_id["LX"])
+            self.wireless_controller.ly = -self.joystick.get_axis(self.axis_id["LY"])
+            self.wireless_controller.rx = self.joystick.get_axis(self.axis_id["RX"])
+            self.wireless_controller.ry = -self.joystick.get_axis(self.axis_id["RY"])
+
+            self.wireless_controller_puber.Write(self.wireless_controller)
+
+    def SetupJoystick(self, device_id=0, js_type="xbox"):
+        pygame.init()
+        pygame.joystick.init()
+        joystick_count = pygame.joystick.get_count()
+        if joystick_count > 0:
+            self.joystick = pygame.joystick.Joystick(device_id)
+            self.joystick.init()
+        else:
+            print("No gamepad detected.")
+            sys.exit()
+
+        if js_type == "xbox":
+            if sys.platform.startswith("linux"):
+                self.axis_id = {
+                    "LX": 0,  # Left stick axis x
+                    "LY": 1,  # Left stick axis y
+                    "RX": 3,  # Right stick axis x
+                    "RY": 4,  # Right stick axis y
+                    "LT": 2,  # Left trigger
+                    "RT": 5,  # Right trigger
+                    "DX": 6,  # Directional pad x
+                    "DY": 7,  # Directional pad y
+                }
+                self.button_id = {
+                    "X": 2,
+                    "Y": 3,
+                    "B": 1,
+                    "A": 0,
+                    "LB": 4,
+                    "RB": 5,
+                    "SELECT": 6,
+                    "START": 7,
+                    "XBOX": 8,
+                    "LSB": 9,
+                    "RSB": 10,
+                }
+            elif sys.platform == "darwin":
+                self.axis_id = {
+                    "LX": 0,  # Left stick axis x
+                    "LY": 1,  # Left stick axis y
+                    "RX": 2,  # Right stick axis x
+                    "RY": 3,  # Right stick axis y
+                    "LT": 4,  # Left trigger
+                    "RT": 5,  # Right trigger
+                }
+                self.button_id = {
+                    "X": 2,
+                    "Y": 3,
+                    "B": 1,
+                    "A": 0,
+                    "LB": 9,
+                    "RB": 10,
+                    "SELECT": 4,
+                    "START": 6,
+                    "XBOX": 5,
+                    "LSB": 7,
+                    "RSB": 8,
+                    "DYU": 11,
+                    "DYD": 12,
+                    "DXL": 13,
+                    "DXR": 14,
+                }
+            else:
+                print("Unsupported OS. ")
+
+        elif js_type == "switch":
+            # Yuanhang: may differ for different OS, need to be checked
+            self.axis_id = {
+                "LX": 0,  # Left stick axis x
+                "LY": 1,  # Left stick axis y
+                "RX": 2,  # Right stick axis x
+                "RY": 3,  # Right stick axis y
+                "LT": 5,  # Left trigger
+                "RT": 4,  # Right trigger
+                "DX": 6,  # Directional pad x
+                "DY": 7,  # Directional pad y
+            }
+
+            self.button_id = {
+                "X": 3,
+                "Y": 4,
+                "B": 1,
+                "A": 0,
+                "LB": 6,
+                "RB": 7,
+                "SELECT": 10,
+                "START": 11,
+            }
+        else:
+            print("Unsupported gamepad. ")
+
+    def PrintSceneInformation(self):
+        print(" ")
+        logger.info(colored("<<------------- Link ------------->>", "green"))
+        for i in range(self.mj_model.nbody):
+            name = mujoco.mj_id2name(self.mj_model, mujoco._enums.mjtObj.mjOBJ_BODY, i)
+            if name:
+                logger.info(f"link_index: {i}, name: {name}")
+        print(" ")
+
+        logger.info(colored("<<------------- Joint ------------->>", "green"))
+        for i in range(self.mj_model.njnt):
+            name = mujoco.mj_id2name(self.mj_model, mujoco._enums.mjtObj.mjOBJ_JOINT, i)
+            if name:
+                logger.info(f"joint_index: {i}, name: {name}")
+        print(" ")
+
+        logger.info(colored("<<------------- Actuator ------------->>", "green"))
+        for i in range(self.mj_model.nu):
+            name = mujoco.mj_id2name(self.mj_model, mujoco._enums.mjtObj.mjOBJ_ACTUATOR, i)
+            if name:
+                logger.info(f"actuator_index: {i}, name: {name}")
+        print(" ")
+
+        logger.info(colored("<<------------- Sensor ------------->>", "green"))
+        index = 0
+        for i in range(self.mj_model.nsensor):
+            name = mujoco.mj_id2name(self.mj_model, mujoco._enums.mjtObj.mjOBJ_SENSOR, i)
+            if name:
+                logger.info(
+                    f"sensor_index: {index}, name: {name}, dim: {self.mj_model.sensor_dim[i]}"
+                )
+            index = index + self.mj_model.sensor_dim[i]
+        print(" ")
+
+
+class ElasticBand:
+    """
+    ref: https://github.com/unitreerobotics/unitree_mujoco
+    """
+
+    def __init__(self):
+        self.kp_pos = 10000
+        self.kd_pos = 1000
+        self.kp_ang = 1000
+        self.kd_ang = 10
+        self.point = np.array([0, 0, 1])
+        self.length = 0
+        self.enable = True
+
+    def Advance(self, pose):
+        """
+        Args:
+          pose: 13D array containing:
+               - pose[0:3]: position in world frame
+               - pose[3:7]: quaternion [w,x,y,z] in world frame
+               - pose[7:10]: linear velocity in world frame
+               - pose[10:13]: angular velocity in world frame
+        Returns:
+          np.ndarray: 6D vector [fx, fy, fz, tx, ty, tz]
+        """
+        pos = pose[0:3]
+        quat = pose[3:7]
+        lin_vel = pose[7:10]
+        ang_vel = pose[10:13]
+
+        δx = self.point - pos
+        f = self.kp_pos * (δx + np.array([0, 0, self.length])) + self.kd_pos * (0 - lin_vel)
+
+        # --- Orientation PD control for torque ---
+        quat = np.array([quat[1], quat[2], quat[3], quat[0]])  # reorder to [x,y,z,w] for scipy
+        rot = scipy.spatial.transform.Rotation.from_quat(quat)
+        rotvec = rot.as_rotvec()  # axis-angle error
+        torque = -self.kp_ang * rotvec - self.kd_ang * ang_vel
+
+        return np.concatenate([f, torque])
+
+    def MujuocoKeyCallback(self, key):
+        if key == glfw.KEY_7:
+            self.length -= 0.1
+        if key == glfw.KEY_8:
+            self.length += 0.1
+        if key == glfw.KEY_9:
+            self.enable = not self.enable
+
+    def handle_keyboard_button(self, key):
+        if key == "9":
+            self.enable = not self.enable

GR00T-WholeBodyControl/decoupled_wbc/control/envs/g1/utils/command_sender.py

@@ -0,0 +1,146 @@
+from typing import Dict
+
+import numpy as np
+from unitree_sdk2py.core.channel import ChannelPublisher
+from unitree_sdk2py.idl.default import unitree_hg_msg_dds__HandCmd_
+from unitree_sdk2py.idl.unitree_hg.msg.dds_ import HandCmd_
+from unitree_sdk2py.utils.crc import CRC
+
+
+class BodyCommandSender:
+    def __init__(self, config: Dict):
+        self.config = config
+        if self.config["ROBOT_TYPE"] == "h1" or self.config["ROBOT_TYPE"] == "go2":
+            from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowCmd_
+            from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowCmd_
+
+            self.low_cmd = unitree_go_msg_dds__LowCmd_()
+        elif (
+            self.config["ROBOT_TYPE"] == "g1_29dof"
+            or self.config["ROBOT_TYPE"] == "h1-2_21dof"
+            or self.config["ROBOT_TYPE"] == "h1-2_27dof"
+        ):
+            from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_
+            from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_
+
+            self.low_cmd = unitree_hg_msg_dds__LowCmd_()
+        else:
+            raise NotImplementedError(
+                f"Robot type {self.config['ROBOT_TYPE']} is not supported yet"
+            )
+        # init kp kd
+        self.kp_level = 1.0
+        self.waist_kp_level = 1.0
+        self.robot_kp = np.zeros(self.config["NUM_MOTORS"])
+        self.robot_kd = np.zeros(self.config["NUM_MOTORS"])
+        # set kp level
+        for i in range(len(self.config["MOTOR_KP"])):
+            self.robot_kp[i] = self.config["MOTOR_KP"][i] * self.kp_level
+        for i in range(len(self.config["MOTOR_KD"])):
+            self.robot_kd[i] = self.config["MOTOR_KD"][i] * 1.0
+        self.weak_motor_joint_index = []
+        for _, value in self.config["WeakMotorJointIndex"].items():
+            self.weak_motor_joint_index.append(value)
+        # init low cmd publisher
+        self.lowcmd_publisher_ = ChannelPublisher("rt/lowcmd", LowCmd_)
+        self.lowcmd_publisher_.Init()
+        self.InitLowCmd()
+        self.low_state = None
+        self.crc = CRC()
+
+    def InitLowCmd(self):
+        # h1/go2:
+        if self.config["ROBOT_TYPE"] == "h1" or self.config["ROBOT_TYPE"] == "go2":
+            self.low_cmd.head[0] = 0xFE
+            self.low_cmd.head[1] = 0xEF
+        else:
+            pass
+
+        self.low_cmd.level_flag = 0xFF
+        self.low_cmd.gpio = 0
+        for i in range(self.config["NUM_MOTORS"]):
+            if self.is_weak_motor(i):
+                self.low_cmd.motor_cmd[i].mode = 0x01
+            else:
+                self.low_cmd.motor_cmd[i].mode = 0x0A
+            self.low_cmd.motor_cmd[i].q = self.config["UNITREE_LEGGED_CONST"]["PosStopF"]
+            self.low_cmd.motor_cmd[i].kp = 0
+            self.low_cmd.motor_cmd[i].dq = self.config["UNITREE_LEGGED_CONST"]["VelStopF"]
+            self.low_cmd.motor_cmd[i].kd = 0
+            self.low_cmd.motor_cmd[i].tau = 0
+            if (
+                self.config["ROBOT_TYPE"] == "g1_29dof"
+                or self.config["ROBOT_TYPE"] == "h1-2_21dof"
+                or self.config["ROBOT_TYPE"] == "h1-2_27dof"
+            ):
+                self.low_cmd.mode_machine = self.config["UNITREE_LEGGED_CONST"]["MODE_MACHINE"]
+                self.low_cmd.mode_pr = self.config["UNITREE_LEGGED_CONST"]["MODE_PR"]
+            else:
+                pass
+
+    def is_weak_motor(self, motor_index: int) -> bool:
+        return motor_index in self.weak_motor_joint_index
+
+    def send_command(self, cmd_q: np.ndarray, cmd_dq: np.ndarray, cmd_tau: np.ndarray):
+        for i in range(self.config["NUM_MOTORS"]):
+            motor_index = self.config["JOINT2MOTOR"][i]
+            joint_index = self.config["MOTOR2JOINT"][i]
+            # print(f"motor_index: {motor_index}, joint_index: {joint_index}")
+            if joint_index == -1:
+                # send default joint position command
+                self.low_cmd.motor_cmd[motor_index].q = self.config["DEFAULT_MOTOR_ANGLES"][
+                    motor_index
+                ]
+                self.low_cmd.motor_cmd[motor_index].dq = 0.0
+                self.low_cmd.motor_cmd[motor_index].tau = 0.0
+            else:
+                self.low_cmd.motor_cmd[motor_index].q = cmd_q[joint_index]
+                self.low_cmd.motor_cmd[motor_index].dq = cmd_dq[joint_index]
+                self.low_cmd.motor_cmd[motor_index].tau = cmd_tau[joint_index]
+            # kp kd
+            self.low_cmd.motor_cmd[motor_index].kp = self.robot_kp[motor_index]
+            self.low_cmd.motor_cmd[motor_index].kd = self.robot_kd[motor_index]
+
+        self.low_cmd.crc = self.crc.Crc(self.low_cmd)
+        self.lowcmd_publisher_.Write(self.low_cmd)
+
+
+def make_hand_mode(motor_index: int) -> int:
+    status = 0x01
+    timeout = 0x01
+    mode = motor_index & 0x0F
+    mode |= status << 4  # bits [4..6]
+    mode |= timeout << 7  # bit 7
+    return mode
+
+
+class HandCommandSender:
+    def __init__(self, is_left: bool = True):
+        self.is_left = is_left
+        if self.is_left:
+            self.cmd_pub = ChannelPublisher("rt/dex3/left/cmd", HandCmd_)
+        else:
+            self.cmd_pub = ChannelPublisher("rt/dex3/right/cmd", HandCmd_)
+
+        self.cmd_pub.Init()
+        self.cmd = unitree_hg_msg_dds__HandCmd_()
+
+        self.hand_dof = 7
+
+        self.kp = [1.0] * self.hand_dof
+        self.kd = [0.2] * self.hand_dof
+        self.kp[0] = 2.0
+        self.kd[0] = 0.5
+
+    def send_command(self, cmd: np.ndarray):
+        for i in range(self.hand_dof):
+            # Build the bitfield mode (see your C++ example)
+            mode_val = make_hand_mode(i)
+            self.cmd.motor_cmd[i].mode = mode_val
+            self.cmd.motor_cmd[i].q = cmd[i]
+            self.cmd.motor_cmd[i].dq = 0.0
+            self.cmd.motor_cmd[i].tau = 0.0
+            self.cmd.motor_cmd[i].kp = self.kp[i]
+            self.cmd.motor_cmd[i].kd = self.kd[i]
+
+        self.cmd_pub.Write(self.cmd)

GR00T-WholeBodyControl/decoupled_wbc/control/envs/g1/utils/joint_safety.py

@@ -0,0 +1,534 @@
+"""Joint safety monitor for G1 robot.
+
+This module implements safety monitoring for arm and finger joint velocities using
+joint groups defined in the robot model's supplemental info. Leg joints are not monitored.
+"""
+
+from datetime import datetime
+import sys
+import time
+from typing import Dict, List, Optional, Tuple
+
+import numpy as np
+
+from decoupled_wbc.data.viz.rerun_viz import RerunViz
+
+
+class JointSafetyMonitor:
+    """Monitor joint velocities for G1 robot arms and hands."""
+
+    # Velocity limits in rad/s
+    ARM_VELOCITY_LIMIT = 6.0  # rad/s for arm joints
+    HAND_VELOCITY_LIMIT = 50.0  # rad/s for finger joints
+
+    def __init__(self, robot_model, enable_viz: bool = False, env_type: str = "real"):
+        """Initialize joint safety monitor.
+
+        Args:
+            robot_model: The robot model containing joint information
+            enable_viz: If True, enable rerun visualization (default False)
+            env_type: Environment type - "sim" or "real" (default "real")
+        """
+        self.robot_model = robot_model
+        self.safety_margin = 1.0  # Hardcoded safety margin
+        self.enable_viz = enable_viz
+        self.env_type = env_type
+
+        # Startup ramping parameters
+        self.control_frequency = 50  # Hz, hardcoded from run_g1_control_loop.py
+        self.ramp_duration_steps = int(2.0 * self.control_frequency)  # 2 seconds * 50Hz = 100 steps
+        self.startup_counter = 0
+        self.initial_positions = None
+        self.startup_complete = False
+
+        # Initialize velocity and position limits for monitored joints
+        self.velocity_limits = {}
+        self.position_limits = {}
+        self._initialize_limits()
+
+        # Track violations for reporting
+        self.violations = []
+
+        # Initialize visualization
+        self.right_arm_indices = None
+        self.right_arm_joint_names = []
+        self.left_arm_indices = None
+        self.left_arm_joint_names = []
+        self.right_hand_indices = None
+        self.right_hand_joint_names = []
+        self.left_hand_indices = None
+        self.left_hand_joint_names = []
+        try:
+            arm_indices = self.robot_model.get_joint_group_indices("arms")
+            all_joint_names = [self.robot_model.joint_names[i] for i in arm_indices]
+            # Filter for right and left arm joints
+            self.right_arm_joint_names = [
+                name for name in all_joint_names if name.startswith("right_")
+            ]
+            self.right_arm_indices = [
+                self.robot_model.joint_to_dof_index[name] for name in self.right_arm_joint_names
+            ]
+            self.left_arm_joint_names = [
+                name for name in all_joint_names if name.startswith("left_")
+            ]
+            self.left_arm_indices = [
+                self.robot_model.joint_to_dof_index[name] for name in self.left_arm_joint_names
+            ]
+            # Hand joints
+            hand_indices = self.robot_model.get_joint_group_indices("hands")
+            all_hand_names = [self.robot_model.joint_names[i] for i in hand_indices]
+            self.right_hand_joint_names = [
+                name for name in all_hand_names if name.startswith("right_")
+            ]
+            self.right_hand_indices = [
+                self.robot_model.joint_to_dof_index[name] for name in self.right_hand_joint_names
+            ]
+            self.left_hand_joint_names = [
+                name for name in all_hand_names if name.startswith("left_")
+            ]
+            self.left_hand_indices = [
+                self.robot_model.joint_to_dof_index[name] for name in self.left_hand_joint_names
+            ]
+        except ValueError as e:
+            print(f"[JointSafetyMonitor] Warning: Could not initialize arm/hand visualization: {e}")
+        except Exception:
+            pass
+
+        # Use single tensor_key for each plot
+        self.right_arm_pos_key = "right_arm_qpos"
+        self.left_arm_pos_key = "left_arm_qpos"
+        self.right_arm_vel_key = "right_arm_dq"
+        self.left_arm_vel_key = "left_arm_dq"
+        self.right_hand_pos_key = "right_hand_qpos"
+        self.left_hand_pos_key = "left_hand_qpos"
+        self.right_hand_vel_key = "right_hand_dq"
+        self.left_hand_vel_key = "left_hand_dq"
+
+        # Define a consistent color palette for up to 8 joints (tab10 + extra)
+        self.joint_colors = [
+            [31, 119, 180],  # blue
+            [255, 127, 14],  # orange
+            [44, 160, 44],  # green
+            [214, 39, 40],  # red
+            [148, 103, 189],  # purple
+            [140, 86, 75],  # brown
+            [227, 119, 194],  # pink
+            [127, 127, 127],  # gray (for 8th joint if needed)
+        ]
+
+        # Initialize Rerun visualization only if enabled
+        self.viz = None
+        if self.enable_viz:
+            try:
+                self.viz = RerunViz(
+                    image_keys=[],
+                    tensor_keys=[
+                        self.right_arm_pos_key,
+                        self.left_arm_pos_key,
+                        self.right_arm_vel_key,
+                        self.left_arm_vel_key,
+                        self.right_hand_pos_key,
+                        self.left_hand_pos_key,
+                        self.right_hand_vel_key,
+                        self.left_hand_vel_key,
+                    ],
+                    window_size=10.0,
+                    app_name="joint_safety_monitor",
+                )
+            except Exception:
+                self.viz = None
+
+    def _initialize_limits(self):
+        """Initialize velocity and position limits for arm and hand joints using robot model joint groups."""
+        if self.robot_model.supplemental_info is None:
+            raise ValueError("Robot model must have supplemental_info to use joint groups")
+
+        # Get arm joint indices from robot model joint groups
+        try:
+            arm_indices = self.robot_model.get_joint_group_indices("arms")
+            arm_joint_names = [self.robot_model.joint_names[i] for i in arm_indices]
+
+            for joint_name in arm_joint_names:
+                # Set velocity limits
+                vel_limit = self.ARM_VELOCITY_LIMIT * self.safety_margin
+                self.velocity_limits[joint_name] = {"min": -vel_limit, "max": vel_limit}
+
+                # Set position limits from robot model
+                if joint_name in self.robot_model.joint_to_dof_index:
+                    joint_idx = self.robot_model.joint_to_dof_index[joint_name]
+                    # Adjust index for floating base if present
+                    limit_idx = joint_idx - (7 if self.robot_model.is_floating_base_model else 0)
+
+                    if 0 <= limit_idx < len(self.robot_model.lower_joint_limits):
+                        pos_min = self.robot_model.lower_joint_limits[limit_idx]
+                        pos_max = self.robot_model.upper_joint_limits[limit_idx]
+
+                        # Apply safety margin to position limits
+                        pos_range = pos_max - pos_min
+                        margin = pos_range * (1.0 - self.safety_margin) / 2.0
+
+                        self.position_limits[joint_name] = {
+                            "min": pos_min + margin,
+                            "max": pos_max - margin,
+                        }
+        except ValueError as e:
+            print(f"[JointSafetyMonitor] Warning: Could not find 'arms' joint group: {e}")
+
+        # Get hand joint indices from robot model joint groups
+        try:
+            hand_indices = self.robot_model.get_joint_group_indices("hands")
+            hand_joint_names = [self.robot_model.joint_names[i] for i in hand_indices]
+
+            for joint_name in hand_joint_names:
+                # Set velocity limits only for hands (no position limits for now)
+                vel_limit = self.HAND_VELOCITY_LIMIT * self.safety_margin
+                self.velocity_limits[joint_name] = {"min": -vel_limit, "max": vel_limit}
+        except ValueError as e:
+            print(f"[JointSafetyMonitor] Warning: Could not find 'hands' joint group: {e}")
+
+    def check_safety(self, obs: Dict, action: Dict) -> Tuple[bool, List[Dict]]:
+        """Check if current velocities and positions are within safe bounds.
+
+        Args:
+            obs: Observation dictionary containing joint positions and velocities
+            action: Action dictionary containing target positions
+
+        Returns:
+            (is_safe, violations): Tuple of safety status and list of violations
+            Note: is_safe=False only for velocity violations (triggers shutdown)
+                  Position violations are warnings only (don't affect is_safe)
+        """
+        self.violations = []
+        is_safe = True
+        joint_names = self.robot_model.joint_names
+
+        # Check current joint velocities (critical - triggers shutdown)
+        if "dq" in obs:
+            joint_velocities = obs["dq"]
+
+            for i, joint_name in enumerate(joint_names):
+                # Only check monitored joints
+                if joint_name not in self.velocity_limits:
+                    continue
+
+                if i < len(joint_velocities):
+                    velocity = joint_velocities[i]
+                    limits = self.velocity_limits[joint_name]
+
+                    if velocity < limits["min"] or velocity > limits["max"]:
+                        violation = {
+                            "joint": joint_name,
+                            "type": "velocity",
+                            "value": velocity,
+                            "limit_min": limits["min"],
+                            "limit_max": limits["max"],
+                            "exceeded_by": self._calculate_exceeded_percentage(
+                                velocity, limits["min"], limits["max"]
+                            ),
+                            "critical": True,  # Velocity violations are critical
+                        }
+                        self.violations.append(violation)
+                        is_safe = False
+
+        # Check current joint positions (warning only - no shutdown)
+        if "q" in obs:
+            joint_positions = obs["q"]
+
+            for i, joint_name in enumerate(joint_names):
+                # Only check joints with position limits (arms)
+                if joint_name not in self.position_limits:
+                    continue
+
+                if i < len(joint_positions):
+                    position = joint_positions[i]
+                    limits = self.position_limits[joint_name]
+
+                    if position < limits["min"] or position > limits["max"]:
+                        violation = {
+                            "joint": joint_name,
+                            "type": "position",
+                            "value": position,
+                            "limit_min": limits["min"],
+                            "limit_max": limits["max"],
+                            "exceeded_by": self._calculate_exceeded_percentage(
+                                position, limits["min"], limits["max"]
+                            ),
+                            "critical": False,  # Position violations are warnings only
+                        }
+                        self.violations.append(violation)
+                        # Don't set is_safe = False for position violations
+
+        return is_safe, self.violations
+
+    def _calculate_exceeded_percentage(
+        self, value: float, limit_min: float, limit_max: float
+    ) -> float:
+        """Calculate by how much percentage a value exceeds the limits."""
+        if value < limit_min:
+            return abs((value - limit_min) / limit_min) * 100
+        elif value > limit_max:
+            return abs((value - limit_max) / limit_max) * 100
+        return 0.0
+
+    def get_safe_action(self, obs: Dict, original_action: Dict) -> Dict:
+        """Generate a safe action with startup ramping for smooth initialization.
+
+        Args:
+            obs: Observation dictionary containing current joint positions
+            original_action: The original action that may cause violations
+
+        Returns:
+            Safe action with startup ramping applied if within ramp duration
+        """
+        safe_action = original_action.copy()
+
+        # Handle startup ramping for arm joints
+        if not self.startup_complete:
+            if self.initial_positions is None and "q" in obs:
+                # Store initial positions from first observation
+                self.initial_positions = obs["q"].copy()
+
+            if (
+                self.startup_counter < self.ramp_duration_steps
+                and self.initial_positions is not None
+                and "q" in safe_action
+            ):
+                # Ramp factor: 0.0 at start → 1.0 at end
+                ramp_factor = self.startup_counter / self.ramp_duration_steps
+
+                # Apply ramping only to monitored arm joints
+                for joint_name in self.velocity_limits:  # Only monitored arm joints
+                    if joint_name in self.robot_model.joint_to_dof_index:
+                        joint_idx = self.robot_model.joint_to_dof_index[joint_name]
+                        if joint_idx < len(safe_action["q"]) and joint_idx < len(
+                            self.initial_positions
+                        ):
+                            initial_pos = self.initial_positions[joint_idx]
+                            target_pos = original_action["q"][joint_idx]
+                            # Linear interpolation: initial + ramp_factor * (target - initial)
+                            safe_action["q"][joint_idx] = initial_pos + ramp_factor * (
+                                target_pos - initial_pos
+                            )
+
+                # Increment counter for next iteration
+                self.startup_counter += 1
+            else:
+                # Ramping complete - use original actions
+                self.startup_complete = True
+
+        return safe_action
+
+    def get_violation_report(self, violations: Optional[List[Dict]] = None) -> str:
+        """Generate a formatted error report for violations.
+
+        Args:
+            violations: List of violations to report (uses self.violations if None)
+
+        Returns:
+            Formatted error message string
+        """
+        if violations is None:
+            violations = self.violations
+
+        if not violations:
+            return "No violations detected."
+
+        timestamp = datetime.now().strftime("%Y-%m-%d %H:%M:%S.%f")[:-3]
+
+        # Check if these are critical violations or warnings
+        critical_violations = [v for v in violations if v.get("critical", True)]
+        warning_violations = [v for v in violations if not v.get("critical", True)]
+
+        if critical_violations and warning_violations:
+            report = f"Joint safety bounds exceeded!\nTimestamp: {timestamp}\nViolations:\n"
+        elif critical_violations:
+            report = f"Joint safety bounds exceeded!\nTimestamp: {timestamp}\nViolations:\n"
+        else:
+            report = f"Joint position warnings!\nTimestamp: {timestamp}\nWarnings:\n"
+
+        for violation in violations:
+            joint = violation["joint"]
+            vtype = violation["type"]
+            value = violation["value"]
+            exceeded = violation["exceeded_by"]
+            limit_min = violation["limit_min"]
+            limit_max = violation["limit_max"]
+
+            if vtype == "velocity":
+                report += f"  - {joint}: {vtype}={value:.3f} rad/s "
+                report += f"(limit: ±{limit_max:.3f} rad/s) - "
+                report += f"EXCEEDED by {exceeded:.1f}%\n"
+            elif vtype == "position":
+                report += f"  - {joint}: {vtype}={value:.3f} rad "
+                report += f"(limits: [{limit_min:.3f}, {limit_max:.3f}] rad) - "
+                report += f"EXCEEDED by {exceeded:.1f}%\n"
+
+        # Add appropriate action message
+        if critical_violations:
+            report += "Action: Safe mode engaged (kp=0, tau=0). System shutdown initiated.\n"
+            report += "Please restart Docker container to resume operation."
+        else:
+            report += "Action: Position warning only. Robot continues operation."
+
+        return report
+
+    def handle_violations(self, obs: Dict, action: Dict) -> Dict:
+        """Check safety and handle violations appropriately.
+
+        Args:
+            obs: Observation dictionary
+            action: Action dictionary
+
+        Returns:
+            Dict with keys:
+            - 'safe_to_continue': bool - whether robot should continue operation
+            - 'action': Dict - potentially modified safe action
+            - 'shutdown_required': bool - whether system shutdown is needed
+        """
+        is_safe, violations = self.check_safety(obs, action)
+
+        # Apply startup ramping (always, regardless of violations)
+        safe_action = self.get_safe_action(obs, action)
+
+        # Visualize arm and hand joint positions and velocities if enabled
+        if self.enable_viz:
+            if (
+                self.right_arm_indices is not None
+                and self.left_arm_indices is not None
+                and self.right_hand_indices is not None
+                and self.left_hand_indices is not None
+                and "q" in obs
+                and "dq" in obs
+                and self.viz is not None
+            ):
+                try:
+                    right_arm_positions = obs["q"][self.right_arm_indices]
+                    left_arm_positions = obs["q"][self.left_arm_indices]
+                    right_arm_velocities = obs["dq"][self.right_arm_indices]
+                    left_arm_velocities = obs["dq"][self.left_arm_indices]
+                    right_hand_positions = obs["q"][self.right_hand_indices]
+                    left_hand_positions = obs["q"][self.left_hand_indices]
+                    right_hand_velocities = obs["dq"][self.right_hand_indices]
+                    left_hand_velocities = obs["dq"][self.left_hand_indices]
+                    tensor_dict = {
+                        self.right_arm_pos_key: right_arm_positions,
+                        self.left_arm_pos_key: left_arm_positions,
+                        self.right_arm_vel_key: right_arm_velocities,
+                        self.left_arm_vel_key: left_arm_velocities,
+                        self.right_hand_pos_key: right_hand_positions,
+                        self.left_hand_pos_key: left_hand_positions,
+                        self.right_hand_vel_key: right_hand_velocities,
+                        self.left_hand_vel_key: left_hand_velocities,
+                    }
+                    self.viz.plot_tensors(tensor_dict, time.time())
+                except Exception:
+                    pass
+
+        if not violations:
+            return {"safe_to_continue": True, "action": safe_action, "shutdown_required": False}
+
+        # Separate critical (velocity) and warning (position) violations
+        critical_violations = [v for v in violations if v.get("critical", True)]
+        # warning_violations = [v for v in violations if not v.get('critical', True)]
+
+        # Print warnings for position violations
+        # if warning_violations:
+        # warning_msg = self.get_violation_report(warning_violations)
+        # print(f"[SAFETY WARNING] {warning_msg}")
+
+        # Handle critical violations (velocity) - trigger shutdown
+        if not is_safe and critical_violations:
+            error_msg = self.get_violation_report(critical_violations)
+            if self.env_type == "real":
+                print(f"[SAFETY VIOLATION] {error_msg}")
+                self.trigger_system_shutdown()
+
+            return {"safe_to_continue": False, "action": safe_action, "shutdown_required": True}
+
+        # Only position violations - continue with safe action
+        return {"safe_to_continue": True, "action": safe_action, "shutdown_required": False}
+
+    def trigger_system_shutdown(self):
+        """Trigger system shutdown after safety violation."""
+        print("\n[SAFETY] Initiating system shutdown due to safety violation...")
+        sys.exit(1)
+
+
+def main():
+    """Test the joint safety monitor with joint groups."""
+    print("Testing joint safety monitor with joint groups...")
+
+    try:
+        from decoupled_wbc.control.robot_model.instantiation.g1 import instantiate_g1_robot_model
+
+        # Instantiate robot model
+        robot_model = instantiate_g1_robot_model()
+        print(f"Robot model created with {len(robot_model.joint_names)} joints")
+
+        # Create safety monitor
+        safety_monitor = JointSafetyMonitor(robot_model)
+        print("Safety monitor created successfully!")
+        print(f"Monitoring {len(safety_monitor.velocity_limits)} joints")
+
+        # Print monitored joints
+        print("\nVelocity limits:")
+        for joint_name, limits in safety_monitor.velocity_limits.items():
+            print(f"  - {joint_name}: ±{limits['max']:.2f} rad/s")
+
+        print(f"\nPosition limits (arms only): {len(safety_monitor.position_limits)} joints")
+        for joint_name, limits in safety_monitor.position_limits.items():
+            print(f"  - {joint_name}: [{limits['min']:.3f}, {limits['max']:.3f}] rad")
+
+        # Test safety checking with safe values
+        print("\n--- Testing Safety Checking ---")
+
+        # Create mock observation with safe values
+        safe_obs = {
+            "q": np.zeros(robot_model.num_dofs),  # All joints at zero position
+            "dq": np.zeros(robot_model.num_dofs),  # All joints at zero velocity
+        }
+        safe_action = {"q": np.zeros(robot_model.num_dofs)}
+
+        # Test handle_violations method
+        result = safety_monitor.handle_violations(safe_obs, safe_action)
+        print(
+            f"Safe values test: safe_to_continue={result['safe_to_continue']}, "
+            f"shutdown_required={result['shutdown_required']}"
+        )
+
+        # Test with unsafe velocity
+        unsafe_obs = safe_obs.copy()
+        unsafe_obs["dq"] = np.zeros(robot_model.num_dofs)
+        # Set left shoulder pitch velocity to exceed limit
+        left_shoulder_idx = robot_model.dof_index("left_shoulder_pitch_joint")
+        unsafe_obs["dq"][left_shoulder_idx] = 6.0  # Exceeds 5.0 rad/s limit
+
+        print("\nUnsafe velocity test:")
+        result = safety_monitor.handle_violations(unsafe_obs, safe_action)
+        print(
+            f"  safe_to_continue={result['safe_to_continue']}, shutdown_required={result['shutdown_required']}"
+        )
+
+        # Test with unsafe position only
+        unsafe_pos_obs = safe_obs.copy()
+        unsafe_pos_obs["q"] = np.zeros(robot_model.num_dofs)
+        # Set left shoulder pitch position to exceed limit
+        unsafe_pos_obs["q"][left_shoulder_idx] = -4.0  # Exceeds lower limit of -3.089
+
+        print("\nUnsafe position test:")
+        result = safety_monitor.handle_violations(unsafe_pos_obs, safe_action)
+        print(
+            f"  safe_to_continue={result['safe_to_continue']}, shutdown_required={result['shutdown_required']}"
+        )
+
+        print("\nAll tests completed successfully!")
+
+    except Exception as e:
+        print(f"Test failed with error: {e}")
+        import traceback
+
+        traceback.print_exc()
+
+
+if __name__ == "__main__":
+    main()

GR00T-WholeBodyControl/decoupled_wbc/control/envs/g1/utils/state_processor.py

@@ -0,0 +1,143 @@
+import time
+
+import numpy as np
+from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import (
+    MotionSwitcherClient,
+)
+from unitree_sdk2py.core.channel import ChannelSubscriber
+from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowState_ as LowState_go
+from unitree_sdk2py.idl.unitree_hg.msg.dds_ import (
+    HandState_,
+    IMUState_,
+    LowState_ as LowState_hg,
+    OdoState_,
+)
+
+
+class BodyStateProcessor:
+    def __init__(self, config):
+        self.config = config
+
+        # Enter debug mode for real robot
+        if self.config["ENV_TYPE"] == "real":
+            msc = MotionSwitcherClient()
+            msc.SetTimeout(5.0)
+            msc.Init()
+
+            status, result = msc.CheckMode()
+            print(status, result)
+            while result["name"]:
+                msc.ReleaseMode()
+                status, result = msc.CheckMode()
+                print(status, result)
+                time.sleep(1)
+
+        if self.config["ROBOT_TYPE"] == "h1" or self.config["ROBOT_TYPE"] == "go2":
+            self.robot_lowstate_subscriber = ChannelSubscriber("rt/lowstate", LowState_go)
+            self.robot_lowstate_subscriber.Init(None, 0)
+            self.robot_lowstate_subscriber.Init(None, 0)
+        elif (
+            self.config["ROBOT_TYPE"] == "g1_29dof"
+            or self.config["ROBOT_TYPE"] == "h1-2_27dof"
+            or self.config["ROBOT_TYPE"] == "h1-2_21dof"
+        ):
+            self.robot_lowstate_subscriber = ChannelSubscriber("rt/lowstate", LowState_hg)
+            self.robot_lowstate_subscriber.Init(None, 0)
+
+            self.secondary_imu_subscriber = ChannelSubscriber("rt/secondary_imu", IMUState_)
+            self.secondary_imu_subscriber.Init(None, 0)
+
+            # Subscribe to odo state (only available in simulation)
+            if self.config["ENV_TYPE"] == "sim":
+                self.odo_state_subscriber = ChannelSubscriber("rt/odostate", OdoState_)
+                self.odo_state_subscriber.Init(None, 0)
+        else:
+            raise NotImplementedError(f"Robot type {self.config['ROBOT_TYPE']} is not supported")
+
+        self.num_dof = self.config["NUM_JOINTS"]
+        # 3 + 4 + 19
+        self._init_q = np.zeros(3 + 4 + self.num_dof)
+        self.q = self._init_q
+        self.dq = np.zeros(3 + 3 + self.num_dof)
+        self.ddq = np.zeros(3 + 3 + self.num_dof)
+        self.tau_est = np.zeros(3 + 3 + self.num_dof)
+        self.torso_quat = np.zeros(4)
+        self.torso_ang_vel = np.zeros(3)
+        self.temp_first = np.zeros(self.num_dof)
+        self.temp_second = np.zeros(self.num_dof)
+        self.robot_low_state = None
+        self.secondary_imu_state = None
+        self.odo_state = None
+
+    def _prepare_low_state(self) -> np.ndarray:
+        self.robot_low_state = self.robot_lowstate_subscriber.Read()
+        self.secondary_imu_state = self.secondary_imu_subscriber.Read()
+
+        if not self.robot_low_state:
+            print("No low state received")
+            return
+        imu_state = self.robot_low_state.imu_state
+
+        # Use odo_state for position and velocity if available, otherwise set to zero
+        if self.config["ENV_TYPE"] == "sim":
+            self.odo_state = self.odo_state_subscriber.Read()
+            self.q[0:3] = self.odo_state.position
+            self.dq[0:3] = self.odo_state.linear_velocity
+        else:
+            self.q[0:3] = [0.0, 0.0, 0.0]
+            self.dq[0:3] = [0.0, 0.0, 0.0]
+
+        self.q[3:7] = imu_state.quaternion  # w, x, y, z
+        self.dq[3:6] = imu_state.gyroscope
+        self.ddq[0:3] = imu_state.accelerometer
+        unitree_joint_state = self.robot_low_state.motor_state
+        self.torso_quat = self.secondary_imu_state.quaternion
+        self.torso_ang_vel = self.secondary_imu_state.gyroscope
+
+        for i in range(self.num_dof):
+            self.q[7 + i] = unitree_joint_state[self.config["JOINT2MOTOR"][i]].q
+            self.dq[6 + i] = unitree_joint_state[self.config["JOINT2MOTOR"][i]].dq
+            self.tau_est[6 + i] = unitree_joint_state[self.config["JOINT2MOTOR"][i]].tau_est
+
+        robot_state_data = np.concatenate(
+            [self.q, self.dq, self.tau_est, self.ddq, self.torso_quat, self.torso_ang_vel], axis=0
+        ).reshape(1, -1)
+        # (7 + 29) + (6 + 29) + (6 + 29) + (6 + 29) = 141 dim
+
+        return robot_state_data
+
+
+class HandStateProcessor:
+    def __init__(self, is_left: bool = True):
+        self.is_left = is_left
+        if self.is_left:
+            self.state_sub = ChannelSubscriber("rt/dex3/left/state", HandState_)
+        else:
+            self.state_sub = ChannelSubscriber("rt/dex3/right/state", HandState_)
+
+        self.state_sub.Init(None, 0)
+        self.state_sub.Init(None, 0)
+        self.state = None
+        self.num_dof = 7  # for single hand
+
+    def _prepare_low_state(self) -> np.ndarray:
+        self.state = self.state_sub.Read()
+
+        if not self.state:
+            print("No state received")
+            return
+
+        state_data = (
+            np.concatenate(
+                [
+                    [self.state.motor_state[i].q for i in range(self.num_dof)],
+                    [self.state.motor_state[i].dq for i in range(self.num_dof)],
+                    [self.state.motor_state[i].tau_est for i in range(self.num_dof)],
+                    [self.state.motor_state[i].ddq for i in range(self.num_dof)],
+                ],
+                axis=0,
+            )
+            .astype(np.float64)
+            .reshape(1, -1)
+        )
+        return state_data

GR00T-WholeBodyControl/decoupled_wbc/control/envs/robocasa/async_env_server.py

@@ -0,0 +1,305 @@
+from abc import abstractmethod
+import threading
+import time
+from typing import Any, Dict, Tuple
+
+import mujoco
+import numpy as np
+import rclpy
+
+from decoupled_wbc.control.envs.g1.sim.image_publish_utils import ImagePublishProcess
+from decoupled_wbc.control.envs.robocasa.utils.robocasa_env import (
+    Gr00tLocomanipRoboCasaEnv,
+)  # noqa: F401
+from decoupled_wbc.control.robot_model.robot_model import RobotModel
+from decoupled_wbc.control.utils.keyboard_dispatcher import KeyboardListenerSubscriber
+
+
+class RoboCasaEnvServer:
+    """
+    This class is responsible for running the simulation environment loop in a separate thread.
+    It communicates with the main thread via the `publish_obs` and `get_action` methods through `channel_bridge`.
+    It will also handle the viewer sync when `onscreen` is True.
+    """
+
+    def __init__(
+        self,
+        env_name: str,
+        robot_name: str,
+        robot_model: RobotModel,
+        env_kwargs: Dict[str, Any],
+        **kwargs,
+    ):
+        # initialize environment
+        if env_kwargs.get("onscreen", False):
+            env_kwargs["onscreen"] = False
+            self.onscreen = True  # onscreen render in the main thread
+            self.render_camera = env_kwargs.get("render_camera", None)
+        else:
+            self.onscreen = False
+        self.env_name = env_name
+        self.env = Gr00tLocomanipRoboCasaEnv(env_name, robot_name, robot_model, **env_kwargs)
+        self.init_caches()
+        self.cache_lock = threading.Lock()
+
+        # initialize channel
+        self.init_channel()
+
+        # initialize ROS2 node
+        if not rclpy.ok():
+            rclpy.init()
+            self.node = rclpy.create_node("sim_robocasa")
+            self.thread = threading.Thread(target=rclpy.spin, args=(self.node,), daemon=True)
+            self.thread.start()
+        else:
+            self.thread = None
+            executor = rclpy.get_global_executor()
+            self.node = executor.get_nodes()[0]  # will only take the first node
+
+        self.control_freq = env_kwargs.get("control_freq", 1 / 0.02)
+        self.sim_freq = kwargs.get("sim_freq", 1 / 0.005)
+        self.control_rate = self.node.create_rate(self.control_freq)
+
+        self.running = False
+        self.sim_thread = None
+        self.sync_lock = threading.Lock()
+
+        self.sync_mode = kwargs.get("sync_mode", False)
+        self.steps_per_action = kwargs.get("steps_per_action", 1)
+
+        self.image_dt = kwargs.get("image_dt", 0.04)
+        self.image_publish_process = None
+        self.viewer_freq = kwargs.get("viewer_freq", 1 / 0.02)
+        self.viewer = None
+
+        self.verbose = kwargs.get("verbose", True)
+
+        # Initialize keyboard listener for env reset
+        self.keyboard_listener = KeyboardListenerSubscriber()
+
+        self.reset()
+
+    @property
+    def base_env(self):
+        return self.env.env
+
+    def start_image_publish_subprocess(self, start_method: str = "spawn", camera_port: int = 5555):
+        """Initialize image publishing subprocess if cameras are configured"""
+        if len(self.env.camera_names) == 0:
+            print(
+                "Warning: No camera configs provided, image publishing subprocess will not be started"
+            )
+            return
+
+        # Build camera configs from env camera settings
+        camera_configs = {}
+        for env_cam_name in self.env.camera_names:
+            camera_config = self.env.camera_key_mapper.get_camera_config(env_cam_name)
+            mapped_cam_name, cam_width, cam_height = camera_config
+            camera_configs[mapped_cam_name] = {"height": cam_height, "width": cam_width}
+
+        self.image_publish_process = ImagePublishProcess(
+            camera_configs=camera_configs,
+            image_dt=self.image_dt,
+            zmq_port=camera_port,
+            start_method=start_method,
+            verbose=self.verbose,
+        )
+
+        self.image_publish_process.start_process()
+
+    def update_render_caches(self, obs: Dict[str, Any]):
+        """Update render cache and shared memory for subprocess"""
+        if self.image_publish_process is None:
+            return
+
+        # Extract image observations from obs dict
+        render_caches = {
+            k: v for k, v in obs.items() if k.endswith("_image") and isinstance(v, np.ndarray)
+        }
+
+        # Update shared memory if image publishing process is available
+        if render_caches:
+            self.image_publish_process.update_shared_memory(render_caches)
+
+    def init_caches(self):
+        self.caches = {
+            "obs": None,
+            "reward": None,
+            "terminated": None,
+            "truncated": None,
+            "info": None,
+        }
+
+    def reset(self, **kwargs):
+        if self.viewer is not None:
+            self.viewer.close()
+
+        obs, info = self.env.reset(**kwargs)
+        self.caches["obs"] = obs
+        self.caches["reward"] = 0
+        self.caches["terminated"] = False
+        self.caches["truncated"] = False
+        self.caches["info"] = info
+
+        # initialize viewer
+        if self.onscreen:
+            self.viewer = mujoco.viewer.launch_passive(
+                self.base_env.sim.model._model,
+                self.base_env.sim.data._data,
+                show_left_ui=False,
+                show_right_ui=False,
+            )
+            self.viewer.opt.geomgroup[0] = 0  # disable collision visualization
+            if self.render_camera is not None:
+                self.viewer.cam.type = mujoco.mjtCamera.mjCAMERA_FIXED
+                self.viewer.cam.fixedcamid = self.base_env.sim.model._model.cam(
+                    self.render_camera
+                ).id
+
+        # self.episode_state.reset_state()
+        return obs, info
+
+    @abstractmethod
+    def init_channel(self):
+        raise NotImplementedError("init_channel must be implemented by the subclass")
+
+    @abstractmethod
+    def publish_obs(self):
+        raise NotImplementedError("publish_obs must be implemented by the subclass")
+
+    @abstractmethod
+    def get_action(self) -> Tuple[Dict[str, Any], bool, bool]:
+        raise NotImplementedError("get_action must be implemented by the subclass")
+
+    def start_as_thread(self):
+        """Start the simulation thread"""
+        if self.sim_thread is not None and self.sim_thread.is_alive():
+            return
+
+        self.sim_thread = threading.Thread(target=self.start)
+        self.sim_thread.daemon = True
+        self.sim_thread.start()
+
+    def set_sync_mode(self, sync_mode: bool, steps_per_action: int = 4):
+        """Set the sync mode of the environment server"""
+        with self.sync_lock:
+            self.sync_mode = sync_mode
+            self.steps_per_action = steps_per_action
+
+    def _check_keyboard_input(self):
+        """Check for keyboard input and handle state transitions"""
+        key = self.keyboard_listener.read_msg()
+        if key == "k":
+            print("\033[1;32m[Sim env]\033[0m Resetting sim environment")
+            self.reset()
+
+    def start(self):
+        """Function executed by the simulation thread"""
+        iter_idx = 0
+        steps_per_cur_action = 0
+        t_start = time.monotonic()
+
+        self.running = True
+
+        while self.running:
+            # Check keyboard input for state transitions
+            self._check_keyboard_input()
+
+            # Publish observations and get new action
+            self.publish_obs()
+            action, ready, is_new_action = self.get_action()
+            # ready is True if the action is received from the control loop
+            # is_new_action is True if the action is new (not the same as the previous action)
+            with self.sync_lock:
+                sync_mode = self.sync_mode
+                max_steps_per_action = self.steps_per_action
+
+            # Process action if ready and within step limits
+            action_should_apply = ready and (
+                (not sync_mode) or steps_per_cur_action < max_steps_per_action
+            )
+            if action_should_apply:
+                obs, reward, terminated, truncated, info = self.env.step(action)
+                with self.cache_lock:
+                    self.caches["obs"] = obs
+                    self.caches["reward"] = reward
+                    self.caches["terminated"] = terminated
+                    self.caches["truncated"] = truncated
+                    self.caches["info"] = info
+
+                if reward == 1.0 and iter_idx % 50 == 0:
+                    print("\033[92mTask successful. Can save data now.\033[0m")
+
+                iter_idx += 1
+                steps_per_cur_action += 1
+                if self.verbose and sync_mode:
+                    print("steps_per_cur_action: ", steps_per_cur_action)
+
+            # Update render caches at image publishing rate
+            if action_should_apply and iter_idx % int(self.image_dt * self.control_freq) == 0:
+                with self.cache_lock:
+                    obs_copy = self.caches["obs"].copy()
+                self.update_render_caches(obs_copy)
+
+            # Reset step counter for new actions
+            if is_new_action:
+                steps_per_cur_action = 0
+
+            # Update viewer at specified frequency
+            if self.onscreen and iter_idx % (self.control_freq / self.viewer_freq) == 0:
+                self.viewer.sync()
+
+            # Check if we're meeting the desired control frequency
+            if iter_idx % 100 == 0:
+                end_time = time.monotonic()
+                if self.verbose:
+                    print(
+                        f"sim FPS: {100.0 / (end_time - t_start) * (self.sim_freq / self.control_freq)}"
+                    )
+                if (end_time - t_start) > ((110.0 / self.control_freq)):  # for tolerance
+                    print(
+                        f"Warning: Sim runs at "
+                        "{100.0/(end_time - t_start) * (self.sim_freq / self.control_freq):.1f}Hz, "
+                        f"but should run at {self.sim_freq:.1f}Hz"
+                    )
+                t_start = end_time
+
+            # reset obj pos every 200 steps
+            if iter_idx % 200 == 0:
+                if hasattr(self.base_env, "reset_obj_pos"):
+                    self.base_env.reset_obj_pos()
+
+            self.control_rate.sleep()
+
+    def get_privileged_obs(self):
+        """Get privileged observation. Should be implemented by subclasses."""
+        obs = {}
+        with self.cache_lock:
+            if hasattr(self.base_env, "get_privileged_obs_keys"):
+                for key in self.base_env.get_privileged_obs_keys():
+                    obs[key] = self.caches["obs"][key]
+
+            for key in self.caches["obs"].keys():
+                if key.endswith("_image"):
+                    obs[key] = self.caches["obs"][key]
+
+        return obs
+
+    def stop(self):
+        """Stop the simulation thread"""
+        self.running = False
+        if self.sim_thread is not None:
+            self.sim_thread.join(timeout=1.0)  # Wait for thread to finish with timeout
+            self.sim_thread = None
+
+    def close(self):
+        self.stop()
+        if self.image_publish_process is not None:
+            self.image_publish_process.stop()
+        if self.onscreen:
+            self.viewer.close()
+        self.env.close()
+
+    def get_reward(self):
+        return self.base_env.reward()

GR00T-WholeBodyControl/decoupled_wbc/control/envs/robocasa/sync_env.py

@@ -0,0 +1,586 @@
+import sys
+from typing import Any, Dict, Tuple
+
+import gymnasium as gym
+from gymnasium.envs.registration import register
+import numpy as np
+from robocasa.environments.locomanipulation import REGISTERED_LOCOMANIPULATION_ENVS
+from robocasa.models.robots import GR00T_LOCOMANIP_ENVS_ROBOTS
+from robosuite.environments.robot_env import RobotEnv
+from scipy.spatial.transform import Rotation as R
+
+from decoupled_wbc.control.envs.g1.utils.joint_safety import JointSafetyMonitor
+from decoupled_wbc.control.envs.robocasa.utils.controller_utils import (
+    update_robosuite_controller_configs,
+)
+from decoupled_wbc.control.envs.robocasa.utils.robocasa_env import (  # noqa: F401
+    ALLOWED_LANGUAGE_CHARSET,
+    Gr00tLocomanipRoboCasaEnv,
+)
+from decoupled_wbc.control.robot_model.instantiation import get_robot_type_and_model
+from decoupled_wbc.control.utils.n1_utils import (
+    prepare_gym_space_for_eval,
+    prepare_observation_for_eval,
+)
+from decoupled_wbc.data.constants import RS_VIEW_CAMERA_HEIGHT, RS_VIEW_CAMERA_WIDTH
+
+
+class SyncEnv(gym.Env):
+    MAX_MUJOCO_STATE_LEN = 800
+
+    def __init__(self, env_name, robot_name, **kwargs):
+        self.env_name = env_name
+        self.robot_name = robot_name
+        self.onscreen = kwargs.get("onscreen", True)
+        self.enable_gravity_compensation = kwargs.pop("enable_gravity_compensation", False)
+        self.gravity_compensation_joints = kwargs.pop("gravity_compensation_joints", ["arms"])
+        _, self.robot_model = get_robot_type_and_model(
+            robot_name, enable_waist_ik=kwargs.pop("enable_waist", False)
+        )
+
+        env_kwargs = {
+            "onscreen": kwargs.get("onscreen", True),
+            "offscreen": kwargs.get("offscreen", False),
+            "renderer": kwargs.get("renderer", "mjviewer"),
+            "render_camera": kwargs.get("render_camera", "frontview"),
+            "camera_names": kwargs.get("camera_names", ["frontview"]),
+            "camera_heights": kwargs.get("camera_heights", None),
+            "camera_widths": kwargs.get("camera_widths", None),
+            "controller_configs": kwargs["controller_configs"],
+            "control_freq": kwargs.get("control_freq", 50),
+            "translucent_robot": kwargs.get("translucent_robot", True),
+            "ik_indicator": kwargs.get("ik_indicator", False),
+            "randomize_cameras": kwargs.get("randomize_cameras", True),
+        }
+        self.env = Gr00tLocomanipRoboCasaEnv(
+            env_name, robot_name, robot_model=self.robot_model, **env_kwargs
+        )
+        self.init_cache()
+
+        self.reset()
+
+    @property
+    def base_env(self) -> RobotEnv:
+        return self.env.env
+
+    def overwrite_floating_base_action(self, navigate_cmd):
+        if self.base_env.robots[0].robot_model.default_base == "FloatingLeggedBase":
+            self.env.unwrapped.overridden_floating_base_action = navigate_cmd
+
+    def get_mujoco_state_info(self):
+        mujoco_state = self.base_env.sim.get_state().flatten()
+        assert len(mujoco_state) < SyncEnv.MAX_MUJOCO_STATE_LEN
+        padding_width = SyncEnv.MAX_MUJOCO_STATE_LEN - len(mujoco_state)
+        padded_mujoco_state = np.pad(
+            mujoco_state, (0, padding_width), mode="constant", constant_values=0
+        )
+        max_mujoco_state_len = SyncEnv.MAX_MUJOCO_STATE_LEN
+        mujoco_state_len = len(mujoco_state)
+        mujoco_state = padded_mujoco_state.copy()
+        return max_mujoco_state_len, mujoco_state_len, mujoco_state
+
+    def reset_to(self, state: Dict[str, Any]) -> Dict[str, Any] | None:
+        if hasattr(self.base_env, "reset_to"):
+            self.base_env.reset_to(state)
+        else:
+            # todo: maybe update robosuite to have reset_to()
+            env = self.base_env
+            if "model_file" in state:
+                xml = env.edit_model_xml(state["model_file"])
+                env.reset_from_xml_string(xml)
+                env.sim.reset()
+            if "states" in state:
+                env.sim.set_state_from_flattened(state["states"])
+                env.sim.forward()
+
+        obs = self.env.force_update_observation(timestep=0)
+        self.cache["obs"] = obs
+        return
+
+    def get_state(self) -> Dict[str, Any]:
+        return self.base_env.get_state()
+
+    def is_success(self):
+        """
+        Check if the task condition(s) is reached. Should return a dictionary
+        { str: bool } with at least a "task" key for the overall task success,
+        and additional optional keys corresponding to other task criteria.
+        """
+        # First, try to use the base environment's is_success method if it exists
+        if hasattr(self.base_env, "is_success"):
+            return self.base_env.is_success()
+
+        # Fall back to using _check_success if available
+        elif hasattr(self.base_env, "_check_success"):
+            succ = self.base_env._check_success()
+            if isinstance(succ, dict):
+                assert "task" in succ
+                return succ
+            return {"task": succ}
+
+        # If neither method exists, return failure
+        else:
+            return {"task": False}
+
+    def init_cache(self):
+        self.cache = {
+            "obs": None,
+            "reward": None,
+            "terminated": None,
+            "truncated": None,
+            "info": None,
+        }
+
+    def reset(self, seed=None, options=None) -> Tuple[Dict[str, any], Dict[str, any]]:
+        self.init_cache()
+        obs, info = self.env.reset(seed=seed, options=options)
+        self.cache["obs"] = obs
+        self.cache["reward"] = 0
+        self.cache["terminated"] = False
+        self.cache["truncated"] = False
+        self.cache["info"] = info
+        return self.observe(), info
+
+    def observe(self) -> Dict[str, any]:
+        # Get observations from body and hands
+        assert (
+            self.cache["obs"] is not None
+        ), "Observation cache is not initialized, please reset the environment first"
+        raw_obs = self.cache["obs"]
+
+        # Body and hand joint measurements come in actuator order, so we need to convert them to joint order
+        whole_q = self.robot_model.get_configuration_from_actuated_joints(
+            body_actuated_joint_values=raw_obs["body_q"],
+            left_hand_actuated_joint_values=raw_obs["left_hand_q"],
+            right_hand_actuated_joint_values=raw_obs["right_hand_q"],
+        )
+        whole_dq = self.robot_model.get_configuration_from_actuated_joints(
+            body_actuated_joint_values=raw_obs["body_dq"],
+            left_hand_actuated_joint_values=raw_obs["left_hand_dq"],
+            right_hand_actuated_joint_values=raw_obs["right_hand_dq"],
+        )
+        whole_ddq = self.robot_model.get_configuration_from_actuated_joints(
+            body_actuated_joint_values=raw_obs["body_ddq"],
+            left_hand_actuated_joint_values=raw_obs["left_hand_ddq"],
+            right_hand_actuated_joint_values=raw_obs["right_hand_ddq"],
+        )
+        whole_tau_est = self.robot_model.get_configuration_from_actuated_joints(
+            body_actuated_joint_values=raw_obs["body_tau_est"],
+            left_hand_actuated_joint_values=raw_obs["left_hand_tau_est"],
+            right_hand_actuated_joint_values=raw_obs["right_hand_tau_est"],
+        )
+        eef_obs = self.get_eef_obs(whole_q)
+
+        obs = {
+            "q": whole_q,
+            "dq": whole_dq,
+            "ddq": whole_ddq,
+            "tau_est": whole_tau_est,
+            "floating_base_pose": raw_obs["floating_base_pose"],
+            "floating_base_vel": raw_obs["floating_base_vel"],
+            "floating_base_acc": raw_obs["floating_base_acc"],
+            "wrist_pose": np.concatenate([eef_obs["left_wrist_pose"], eef_obs["right_wrist_pose"]]),
+        }
+
+        # Add state keys for model input
+        obs = prepare_observation_for_eval(self.robot_model, obs)
+
+        obs["annotation.human.task_description"] = raw_obs["language.language_instruction"]
+
+        if hasattr(self.base_env, "get_privileged_obs_keys"):
+            for key in self.base_env.get_privileged_obs_keys():
+                obs[key] = raw_obs[key]
+
+        for key in raw_obs.keys():
+            if key.endswith("_image"):
+                obs[key] = raw_obs[key]
+                # TODO: add video.key without _image suffix for evaluation, convert to uint8, remove later
+                obs[f"video.{key.replace('_image', '')}"] = raw_obs[key]
+        return obs
+
+    def step(
+        self, action: Dict[str, any]
+    ) -> Tuple[Dict[str, any], float, bool, bool, Dict[str, any]]:
+        self.queue_action(action)
+        return self.get_step_info()
+
+    def get_observation(self):
+        return self.base_env._get_observations()  # assumes base env is robosuite
+
+    def get_step_info(self) -> Tuple[Dict[str, any], float, bool, bool, Dict[str, any]]:
+        return (
+            self.observe(),
+            self.cache["reward"],
+            self.cache["terminated"],
+            self.cache["truncated"],
+            self.cache["info"],
+        )
+
+    def convert_q_to_actuated_joint_order(self, q: np.ndarray) -> np.ndarray:
+        body_q = self.robot_model.get_body_actuated_joints(q)
+        left_hand_q = self.robot_model.get_hand_actuated_joints(q, side="left")
+        right_hand_q = self.robot_model.get_hand_actuated_joints(q, side="right")
+        whole_q = np.zeros_like(q)
+        whole_q[self.robot_model.get_joint_group_indices("body")] = body_q
+        whole_q[self.robot_model.get_joint_group_indices("left_hand")] = left_hand_q
+        whole_q[self.robot_model.get_joint_group_indices("right_hand")] = right_hand_q
+
+        return whole_q
+
+    def set_ik_indicator(self, teleop_cmd):
+        """Set the IK indicators for the simulator"""
+        if "left_wrist" in teleop_cmd and "right_wrist" in teleop_cmd:
+            left_wrist_input_pose = teleop_cmd["left_wrist"]
+            right_wrist_input_pose = teleop_cmd["right_wrist"]
+            ik_wrapper = self.base_env
+            ik_wrapper.set_target_poses_outside_env([left_wrist_input_pose, right_wrist_input_pose])
+
+    def render(self):
+        if self.base_env.viewer is not None:
+            self.base_env.viewer.update()
+        if self.onscreen:
+            self.base_env.render()
+
+    def queue_action(self, action: Dict[str, any]):
+        # action is in pinocchio joint order, we need to convert it to actuator order
+        action_q = self.convert_q_to_actuated_joint_order(action["q"])
+
+        # Compute gravity compensation torques if enabled
+        tau_q = np.zeros_like(action_q)
+        if self.enable_gravity_compensation and self.robot_model is not None:
+            try:
+                # Get current robot configuration from cache (more efficient than observe())
+                raw_obs = self.cache["obs"]
+
+                # Convert from actuated joint order to joint order for Pinocchio
+                current_q_joint_order = self.robot_model.get_configuration_from_actuated_joints(
+                    body_actuated_joint_values=raw_obs["body_q"],
+                    left_hand_actuated_joint_values=raw_obs["left_hand_q"],
+                    right_hand_actuated_joint_values=raw_obs["right_hand_q"],
+                )
+
+                # Compute gravity compensation in joint order using current robot configuration
+                gravity_torques_joint_order = self.robot_model.compute_gravity_compensation_torques(
+                    current_q_joint_order, joint_groups=self.gravity_compensation_joints
+                )
+
+                # Convert gravity torques to actuated joint order
+                gravity_torques_actuated = self.convert_q_to_actuated_joint_order(
+                    gravity_torques_joint_order
+                )
+
+                # Add gravity compensation to torques
+                tau_q += gravity_torques_actuated
+
+            except Exception as e:
+                print(f"Error applying gravity compensation in sync_env: {e}")
+
+        obs, reward, terminated, truncated, info = self.env.step({"q": action_q, "tau": tau_q})
+        self.cache["obs"] = obs
+        self.cache["reward"] = reward
+        self.cache["terminated"] = terminated
+        self.cache["truncated"] = truncated
+        self.cache["info"] = info
+
+    def queue_state(self, state: Dict[str, any]):
+        # This function is for debugging or cross-playback between sim and real only.
+        state_q = self.convert_q_to_actuated_joint_order(state["q"])
+        obs, reward, terminated, truncated, info = self.env.unwrapped.step_only_kinematics(
+            {"q": state_q}
+        )
+        self.cache["obs"] = obs
+        self.cache["reward"] = reward
+        self.cache["terminated"] = terminated
+        self.cache["truncated"] = truncated
+        self.cache["info"] = info
+
+    @property
+    def observation_space(self) -> gym.Space:
+        # @todo: check if the low and high bounds are correct for body_obs.
+        q_space = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(self.robot_model.num_dofs,))
+        dq_space = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(self.robot_model.num_dofs,))
+        ddq_space = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(self.robot_model.num_dofs,))
+        tau_est_space = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(self.robot_model.num_dofs,))
+        floating_base_pose_space = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(7,))
+        floating_base_vel_space = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(6,))
+        floating_base_acc_space = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(6,))
+        wrist_pose_space = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(7 + 7,))
+
+        obs_space = gym.spaces.Dict(
+            {
+                "floating_base_pose": floating_base_pose_space,
+                "floating_base_vel": floating_base_vel_space,
+                "floating_base_acc": floating_base_acc_space,
+                "q": q_space,
+                "dq": dq_space,
+                "ddq": ddq_space,
+                "tau_est": tau_est_space,
+                "wrist_pose": wrist_pose_space,
+            }
+        )
+
+        obs_space = prepare_gym_space_for_eval(self.robot_model, obs_space)
+
+        obs_space["annotation.human.task_description"] = gym.spaces.Text(
+            max_length=256, charset=ALLOWED_LANGUAGE_CHARSET
+        )
+
+        if hasattr(self.base_env, "get_privileged_obs_keys"):
+            for key, shape in self.base_env.get_privileged_obs_keys().items():
+                space = gym.spaces.Box(low=-np.inf, high=np.inf, shape=shape)
+                obs_space[key] = space
+
+        robocasa_obs_space = self.env.observation_space
+        for key in robocasa_obs_space.keys():
+            if key.endswith("_image"):
+                space = gym.spaces.Box(
+                    low=-np.inf, high=np.inf, shape=robocasa_obs_space[key].shape
+                )
+                obs_space[key] = space
+                # TODO: add video.key without _image suffix for evaluation, remove later
+                space_uint = gym.spaces.Box(low=0, high=255, shape=space.shape, dtype=np.uint8)
+                obs_space[f"video.{key.replace('_image', '')}"] = space_uint
+
+        return obs_space
+
+    def reset_obj_pos(self):
+        # For Tairan's goal-reaching task, a hacky way to reset the object position is needed.
+        if hasattr(self.base_env, "reset_obj_pos"):
+            self.base_env.reset_obj_pos()
+
+    @property
+    def action_space(self) -> gym.Space:
+        return self.env.action_space
+
+    def close(self):
+        self.env.close()
+
+    def __repr__(self):
+        return (
+            f"SyncEnv(env_name={self.env_name}, \n"
+            f"            observation_space={self.observation_space}, \n"
+            f"            action_space={self.action_space})"
+        )
+
+    def get_joint_gains(self):
+        controller = self.base_env.robots[0].composite_controller
+
+        gains = {}
+        key_mapping = {
+            "left": "left_arm",
+            "right": "right_arm",
+            "legs": "legs",
+            "torso": "waist",
+            "head": "neck",
+        }
+        for k in controller.part_controllers.keys():
+            if hasattr(controller.part_controllers[k], "kp"):
+                if k in key_mapping:
+                    gains[key_mapping[k]] = controller.part_controllers[k].kp
+                else:
+                    gains[k] = controller.part_controllers[k].kp
+        gains.update(
+            {
+                "left_hand": self.base_env.sim.model.actuator_gainprm[
+                    self.base_env.robots[0]._ref_actuators_indexes_dict["left_gripper"], 0
+                ],
+                "right_hand": self.base_env.sim.model.actuator_gainprm[
+                    self.base_env.robots[0]._ref_actuators_indexes_dict["right_gripper"], 0
+                ],
+            }
+        )
+        joint_gains = np.zeros(self.robot_model.num_dofs)
+        for k in gains.keys():
+            joint_gains[self.robot_model.get_joint_group_indices(k)] = gains[k]
+        return joint_gains
+
+    def get_joint_damping(self):
+        controller = self.base_env.robots[0].composite_controller
+        damping = {}
+        key_mapping = {
+            "left": "left_arm",
+            "right": "right_arm",
+            "legs": "legs",
+            "torso": "waist",
+            "head": "neck",
+        }
+        for k in controller.part_controllers.keys():
+            if hasattr(controller.part_controllers[k], "kd"):
+                if k in key_mapping:
+                    damping[key_mapping[k]] = controller.part_controllers[k].kd
+                else:
+                    damping[k] = controller.part_controllers[k].kd
+        damping.update(
+            {
+                "left_hand": -self.base_env.sim.model.actuator_biasprm[
+                    self.base_env.robots[0]._ref_actuators_indexes_dict["left_gripper"], 2
+                ],
+                "right_hand": -self.base_env.sim.model.actuator_biasprm[
+                    self.base_env.robots[0]._ref_actuators_indexes_dict["right_gripper"], 2
+                ],
+            }
+        )
+        joint_damping = np.zeros(self.robot_model.num_dofs)
+        for k in damping.keys():
+            joint_damping[self.robot_model.get_joint_group_indices(k)] = damping[k]
+        return joint_damping
+
+    def get_eef_obs(self, q: np.ndarray) -> Dict[str, np.ndarray]:
+        self.robot_model.cache_forward_kinematics(q)
+        eef_obs = {}
+        for side in ["left", "right"]:
+            wrist_placement = self.robot_model.frame_placement(
+                self.robot_model.supplemental_info.hand_frame_names[side]
+            )
+            wrist_pos, wrist_quat = wrist_placement.translation[:3], R.from_matrix(
+                wrist_placement.rotation
+            ).as_quat(scalar_first=True)
+            eef_obs[f"{side}_wrist_pose"] = np.concatenate([wrist_pos, wrist_quat])
+
+        return eef_obs
+
+
+class G1SyncEnv(SyncEnv):
+    def __init__(
+        self,
+        env_name,
+        robot_name,
+        **kwargs,
+    ):
+        renderer = kwargs.get("renderer", "mjviewer")
+        if renderer == "mjviewer":
+            default_render_camera = ["robot0_oak_egoview"]
+        elif renderer in ["mujoco", "rerun"]:
+            default_render_camera = [
+                "robot0_oak_egoview",
+                "robot0_oak_left_monoview",
+                "robot0_oak_right_monoview",
+            ]
+        else:
+            raise NotImplementedError
+        default_camera_names = [
+            "robot0_oak_egoview",
+            "robot0_oak_left_monoview",
+            "robot0_oak_right_monoview",
+        ]
+        default_camera_heights = [
+            RS_VIEW_CAMERA_HEIGHT,
+            RS_VIEW_CAMERA_HEIGHT,
+            RS_VIEW_CAMERA_HEIGHT,
+        ]
+        default_camera_widths = [
+            RS_VIEW_CAMERA_WIDTH,
+            RS_VIEW_CAMERA_WIDTH,
+            RS_VIEW_CAMERA_WIDTH,
+        ]
+
+        kwargs.update(
+            {
+                "onscreen": kwargs.get("onscreen", True),
+                "offscreen": kwargs.get("offscreen", False),
+                "render_camera": kwargs.get("render_camera", default_render_camera),
+                "camera_names": kwargs.get("camera_names", default_camera_names),
+                "camera_heights": kwargs.get("camera_heights", default_camera_heights),
+                "camera_widths": kwargs.get("camera_widths", default_camera_widths),
+                "translucent_robot": kwargs.get("translucent_robot", False),
+            }
+        )
+        super().__init__(env_name=env_name, robot_name=robot_name, **kwargs)
+
+        # Initialize safety monitor (visualization disabled) - G1 specific
+        self.safety_monitor = JointSafetyMonitor(
+            self.robot_model,
+            enable_viz=False,
+            env_type="sim",  # G1SyncEnv is always simulation
+        )
+        self.safety_monitor.ramp_duration_steps = 0
+        self.safety_monitor.startup_complete = True
+        self.safety_monitor.LOWER_BODY_VELOCITY_LIMIT = (
+            1e10  # disable lower body velocity limits since it impacts WBC
+        )
+        self.last_safety_ok = True  # Track last safety status from queue_action
+
+    @property
+    def observation_space(self):
+        obs_space = super().observation_space
+        obs_space["torso_quat"] = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(4,))
+        obs_space["torso_ang_vel"] = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(3,))
+        return obs_space
+
+    def observe(self):
+        obs = super().observe()
+        obs["torso_quat"] = self.cache["obs"]["secondary_imu_quat"]
+        obs["torso_ang_vel"] = self.cache["obs"]["secondary_imu_vel"][3:6]
+        return obs
+
+    def queue_action(self, action: Dict[str, any]):
+        # Safety check before queuing action
+        obs = self.observe()
+        safety_result = self.safety_monitor.handle_violations(obs, action)
+        action = safety_result["action"]
+        # Save safety status for efficient access
+        self.last_safety_ok = not safety_result.get("shutdown_required", False)
+        # Check if shutdown is required
+        if safety_result["shutdown_required"]:
+            self.safety_monitor.trigger_system_shutdown()
+
+        # Call parent queue_action with potentially modified action
+        super().queue_action(action)
+
+    def get_joint_safety_status(self) -> bool:
+        """Get current joint safety status from the last queue_action safety check.
+
+        Returns:
+            bool: True if joints are safe (no shutdown required), False if unsafe
+        """
+        return self.last_safety_ok
+
+
+def create_gym_sync_env_class(env, robot, robot_alias, wbc_version):
+    class_name = f"{env}_{robot}_{wbc_version}"
+    id_name = f"gr00tlocomanip_{robot_alias}/{class_name}"
+
+    if robot_alias.startswith("g1"):
+        env_class_type = G1SyncEnv
+    elif robot_alias.startswith("gr1"):
+        env_class_type = globals().get("GR1SyncEnv", SyncEnv)
+    else:
+        env_class_type = SyncEnv
+
+    controller_configs = update_robosuite_controller_configs(
+        robot=robot,
+        wbc_version=wbc_version,
+    )
+
+    env_class_type = type(
+        class_name,
+        (env_class_type,),
+        {
+            "__init__": lambda self, **kwargs: super(self.__class__, self).__init__(
+                env_name=env,
+                robot_name=robot,
+                controller_configs=controller_configs,
+                **kwargs,
+            )
+        },
+    )
+
+    current_module = sys.modules["decoupled_wbc.control.envs.robocasa.sync_env"]
+    setattr(current_module, class_name, env_class_type)
+    register(
+        id=id_name,  # Unique ID for the environment
+        entry_point=f"decoupled_wbc.control.envs.robocasa.sync_env:{class_name}",
+    )
+
+
+WBC_VERSION = "gear_wbc"
+
+for ENV in REGISTERED_LOCOMANIPULATION_ENVS:
+    for ROBOT, ROBOT_ALIAS in GR00T_LOCOMANIP_ENVS_ROBOTS.items():
+        create_gym_sync_env_class(ENV, ROBOT, ROBOT_ALIAS, WBC_VERSION)
+
+
+if __name__ == "__main__":
+
+    env = gym.make("gr00tlocomanip_g1_sim/PnPBottle_g1_gear_wbc")
+    print(env.observation_space)

GR00T-WholeBodyControl/decoupled_wbc/control/envs/robocasa/utils/cam_key_converter.py

@@ -0,0 +1,73 @@
+from dataclasses import dataclass
+from typing import Dict, Optional, Tuple
+
+from decoupled_wbc.data.constants import RS_VIEW_CAMERA_HEIGHT, RS_VIEW_CAMERA_WIDTH
+
+
+@dataclass
+class CameraConfig:
+    width: int
+    height: int
+    mapped_key: str
+
+
+class CameraKeyMapper:
+    def __init__(self):
+        # Default camera dimensions
+        self.default_width = RS_VIEW_CAMERA_WIDTH
+        self.default_height = RS_VIEW_CAMERA_HEIGHT
+
+        # Camera key mapping with custom dimensions
+        self.camera_configs: Dict[str, CameraConfig] = {
+            # GR1
+            "egoview": CameraConfig(self.default_width, self.default_height, "ego_view"),
+            "frontview": CameraConfig(self.default_width, self.default_height, "front_view"),
+            # G1
+            "robot0_rs_egoview": CameraConfig(self.default_width, self.default_height, "ego_view"),
+            "robot0_rs_tppview": CameraConfig(self.default_width, self.default_height, "tpp_view"),
+            "robot0_oak_egoview": CameraConfig(self.default_width, self.default_height, "ego_view"),
+            "robot0_oak_left_monoview": CameraConfig(
+                self.default_width, self.default_height, "ego_view_left_mono"
+            ),
+            "robot0_oak_right_monoview": CameraConfig(
+                self.default_width, self.default_height, "ego_view_right_mono"
+            ),
+        }
+
+    def get_camera_config(self, key: str) -> Optional[Tuple[str, int, int]]:
+        """
+        Get the mapped camera key and dimensions for a given camera key.
+
+        Args:
+            key: The input camera key
+
+        Returns:
+            Tuple of (mapped_key, width, height) if key exists, None otherwise
+        """
+        config = self.camera_configs.get(key.lower())
+        if config is None:
+            return None
+        return config.mapped_key, config.width, config.height
+
+    def add_camera_config(
+        self, key: str, mapped_key: str, width: int = 256, height: int = 256
+    ) -> None:
+        """
+        Add a new camera configuration or update an existing one.
+
+        Args:
+            key: The camera key to add/update
+            mapped_key: The actual camera key to map to
+            width: Camera width in pixels
+            height: Camera height in pixels
+        """
+        self.camera_configs[key.lower()] = CameraConfig(width, height, mapped_key)
+
+    def get_all_camera_keys(self) -> list:
+        """
+        Get all available camera keys.
+
+        Returns:
+            List of all camera keys
+        """
+        return list(self.camera_configs.keys())

GR00T-WholeBodyControl/decoupled_wbc/control/envs/robocasa/utils/controller_utils.py

@@ -0,0 +1,54 @@
+def get_body_ik_solver_settings_type(robot: str):
+    robot2body_ik_solver_settings_type = {
+        "G1FixedLowerBody": "sim_optimized",
+        "G1FixedBase": "sim_optimized",
+        "G1FloatingBody": "sim_optimized",
+        "G1ArmsOnly": "sim_optimized",
+        "G1ArmsOnlyFloating": "sim_optimized",
+        "G1": "default",
+        "GR1ArmsAndWaistFourierHands": "default",
+        "GR1FixedLowerBody": "default",
+        "GR1ArmsOnlyFourierHands": "default",
+        "GR1ArmsOnly": "default",
+    }
+    return robot2body_ik_solver_settings_type[robot]
+
+
+def update_robosuite_controller_configs(
+    robot: str,
+    wbc_version: str = None,
+    enable_gravity_compensation: bool = False,
+):
+    """
+    Update the robosuite controller configs based on the robot type and wbc version.
+    """
+    body_ik_solver_settings_type = get_body_ik_solver_settings_type(robot)
+    if robot.startswith("G1"):
+        if wbc_version == "gear_wbc":
+            if enable_gravity_compensation:
+                robosuite_controller_configs = (
+                    "robocasa/examples/third_party_controller/default_mink_ik_g1_gear_wbc_gc.json"
+                )
+            else:
+                robosuite_controller_configs = (
+                    "robocasa/examples/third_party_controller/default_mink_ik_g1_gear_wbc.json"
+                )
+        else:
+            if body_ik_solver_settings_type == "default":
+                robosuite_controller_configs = (
+                    "robocasa/examples/third_party_controller/default_mink_ik_g1_wbc.json"
+                )
+            elif body_ik_solver_settings_type == "sim_optimized":
+                robosuite_controller_configs = (
+                    "robocasa/examples/third_party_controller/"
+                    "default_mink_ik_g1_wbc_sim_optimized.json"
+                )
+            else:
+                raise ValueError(
+                    f"Invalid body_ik_solver_settings_type: {body_ik_solver_settings_type}"
+                )
+    elif robot.startswith("GR1"):
+        return "robocasa/examples/third_party_controller/default_mink_ik_gr1_smallkd.json"
+    else:
+        raise ValueError(f"Invalid robot: {robot}")
+    return robosuite_controller_configs

GR00T-WholeBodyControl/decoupled_wbc/control/envs/robocasa/utils/robocasa_env.py

@@ -0,0 +1,443 @@
+import os
+from typing import Any, Dict, List, Tuple
+
+from gymnasium import spaces
+import mujoco
+import numpy as np
+import robocasa
+from robocasa.utils.gym_utils.gymnasium_basic import (
+    RoboCasaEnv,
+    create_env_robosuite,
+)
+from robocasa.wrappers.ik_wrapper import IKWrapper
+from robosuite.controllers import load_composite_controller_config
+from robosuite.utils.log_utils import ROBOSUITE_DEFAULT_LOGGER
+
+from decoupled_wbc.control.envs.robocasa.utils.cam_key_converter import CameraKeyMapper
+from decoupled_wbc.control.envs.robocasa.utils.robot_key_converter import Gr00tObsActionConverter
+from decoupled_wbc.control.robot_model.robot_model import RobotModel
+
+ALLOWED_LANGUAGE_CHARSET = (
+    "abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ0123456789 ,.\n\t[]{}()!?'_:"
+)
+
+
+class Gr00tLocomanipRoboCasaEnv(RoboCasaEnv):
+    def __init__(
+        self,
+        env_name: str,
+        robots_name: str,
+        robot_model: RobotModel,  # gr00t robot model
+        input_space: str = "JOINT_SPACE",  # either "JOINT_SPACE" or "EEF_SPACE"
+        camera_names: List[str] = ["egoview"],
+        camera_heights: List[int] | None = None,
+        camera_widths: List[int] | None = None,
+        onscreen: bool = False,
+        offscreen: bool = False,
+        dump_rollout_dataset_dir: str | None = None,
+        rollout_hdf5: str | None = None,
+        rollout_trainset: int | None = None,
+        controller_configs: str | None = None,
+        ik_indicator: bool = False,
+        **kwargs,
+    ):
+        # ========= Create env =========
+        if controller_configs is None:
+            if "G1" in robots_name:
+                controller_configs = (
+                    "robocasa/examples/third_party_controller/default_mink_ik_g1_wbc.json"
+                )
+            elif "GR1" in robots_name:
+                controller_configs = (
+                    "robocasa/examples/third_party_controller/default_mink_ik_gr1_smallkd.json"
+                )
+            else:
+                assert False, f"Unsupported robot name: {robots_name}"
+        controller_configs = os.path.join(
+            os.path.dirname(robocasa.__file__),
+            "../",
+            controller_configs,
+        )
+        controller_configs = load_composite_controller_config(
+            controller=controller_configs,
+            robot=robots_name.split("_")[0],
+        )
+        if input_space == "JOINT_SPACE":
+            controller_configs["type"] = "BASIC"
+            controller_configs["composite_controller_specific_configs"] = {}
+            controller_configs["control_delta"] = False
+
+        self.camera_key_mapper = CameraKeyMapper()
+        self.camera_names = camera_names
+
+        if camera_widths is None:
+            self.camera_widths = [
+                self.camera_key_mapper.get_camera_config(name)[1] for name in camera_names
+            ]
+        else:
+            self.camera_widths = camera_widths
+        if camera_heights is None:
+            self.camera_heights = [
+                self.camera_key_mapper.get_camera_config(name)[2] for name in camera_names
+            ]
+        else:
+            self.camera_heights = camera_heights
+
+        self.env, self.env_kwargs = create_env_robosuite(
+            env_name=env_name,
+            robots=robots_name.split("_"),
+            controller_configs=controller_configs,
+            camera_names=camera_names,
+            camera_widths=self.camera_widths,
+            camera_heights=self.camera_heights,
+            enable_render=offscreen,
+            onscreen=onscreen,
+            **kwargs,  # Forward kwargs to create_env_robosuite
+        )
+
+        if ik_indicator:
+            self.env = IKWrapper(self.env, ik_indicator=True)
+
+        # ========= create converters first to get total DOFs =========
+        # For now, assume single robot (multi-robot support can be added later)
+        self.obs_action_converter: List[Gr00tObsActionConverter] = [
+            Gr00tObsActionConverter(
+                robot_model=robot_model,
+                robosuite_robot_model=self.env.robots[i],
+            )
+            for i in range(len(self.env.robots))
+        ]
+
+        self.body_dofs = sum(converter.body_dof for converter in self.obs_action_converter)
+        self.gripper_dofs = sum(converter.gripper_dof for converter in self.obs_action_converter)
+        self.total_dofs = self.body_dofs + self.gripper_dofs
+        self.body_nu = sum(converter.body_nu for converter in self.obs_action_converter)
+        self.gripper_nu = sum(converter.gripper_nu for converter in self.obs_action_converter)
+        self.total_nu = self.body_nu + self.gripper_nu
+
+        # ========= create spaces to match total DOFs =========
+        self.get_observation_space()
+        self.get_action_space()
+
+        self.enable_render = offscreen
+        self.render_obs_key = f"{camera_names[0]}_image"
+        self.render_cache = None
+
+        self.dump_rollout_dataset_dir = dump_rollout_dataset_dir
+        self.gr00t_exporter = None
+        self.np_exporter = None
+
+        self.rollout_hdf5 = rollout_hdf5
+        self.rollout_trainset = rollout_trainset
+        self.rollout_initial_state = {}
+
+        self.verbose = False
+        for k, v in self.observation_space.items():
+            self.verbose and print("{OBS}", k, v)
+        for k, v in self.action_space.items():
+            self.verbose and print("{ACTION}", k, v)
+
+        self.overridden_floating_base_action = None
+
+    def get_observation_space(self):
+        self.observation_space = spaces.Dict({})
+
+        # Add all the observation spaces
+        self.observation_space["time"] = spaces.Box(
+            low=-np.inf, high=np.inf, shape=(1,), dtype=np.float32
+        )
+        self.observation_space["floating_base_pose"] = spaces.Box(
+            low=-np.inf, high=np.inf, shape=(7,), dtype=np.float32
+        )
+        self.observation_space["floating_base_vel"] = spaces.Box(
+            low=-np.inf, high=np.inf, shape=(6,), dtype=np.float32
+        )
+        self.observation_space["floating_base_acc"] = spaces.Box(
+            low=-np.inf, high=np.inf, shape=(6,), dtype=np.float32
+        )
+        self.observation_space["body_q"] = spaces.Box(
+            low=-np.inf, high=np.inf, shape=(self.body_dofs,), dtype=np.float32
+        )
+        self.observation_space["body_dq"] = spaces.Box(
+            low=-np.inf, high=np.inf, shape=(self.body_dofs,), dtype=np.float32
+        )
+        self.observation_space["body_ddq"] = spaces.Box(
+            low=-np.inf, high=np.inf, shape=(self.body_dofs,), dtype=np.float32
+        )
+        self.observation_space["body_tau_est"] = spaces.Box(
+            low=-np.inf, high=np.inf, shape=(self.body_nu,), dtype=np.float32
+        )
+        self.observation_space["left_hand_q"] = spaces.Box(
+            low=-np.inf, high=np.inf, shape=(self.gripper_dofs // 2,), dtype=np.float32
+        )
+        self.observation_space["left_hand_dq"] = spaces.Box(
+            low=-np.inf, high=np.inf, shape=(self.gripper_dofs // 2,), dtype=np.float32
+        )
+        self.observation_space["left_hand_ddq"] = spaces.Box(
+            low=-np.inf, high=np.inf, shape=(self.gripper_dofs // 2,), dtype=np.float32
+        )
+        self.observation_space["left_hand_tau_est"] = spaces.Box(
+            low=-np.inf, high=np.inf, shape=(self.gripper_nu // 2,), dtype=np.float32
+        )
+        self.observation_space["right_hand_q"] = spaces.Box(
+            low=-np.inf, high=np.inf, shape=(self.gripper_dofs // 2,), dtype=np.float32
+        )
+        self.observation_space["right_hand_dq"] = spaces.Box(
+            low=-np.inf, high=np.inf, shape=(self.gripper_dofs // 2,), dtype=np.float32
+        )
+        self.observation_space["right_hand_ddq"] = spaces.Box(
+            low=-np.inf, high=np.inf, shape=(self.gripper_dofs // 2,), dtype=np.float32
+        )
+        self.observation_space["right_hand_tau_est"] = spaces.Box(
+            low=-np.inf, high=np.inf, shape=(self.gripper_nu // 2,), dtype=np.float32
+        )
+
+        self.observation_space["language.language_instruction"] = spaces.Text(
+            max_length=256, charset=ALLOWED_LANGUAGE_CHARSET
+        )
+
+        # Add camera observation spaces
+        for camera_name, w, h in zip(self.camera_names, self.camera_widths, self.camera_heights):
+            k = self.camera_key_mapper.get_camera_config(camera_name)[0]
+            self.observation_space[f"{k}_image"] = spaces.Box(
+                low=0, high=255, shape=(h, w, 3), dtype=np.uint8
+            )
+
+        # Add extra privileged observation spaces
+        if hasattr(self.env, "get_privileged_obs_keys"):
+            for key, shape in self.env.get_privileged_obs_keys().items():
+                self.observation_space[key] = spaces.Box(
+                    low=-np.inf, high=np.inf, shape=shape, dtype=np.float32
+                )
+
+        # Add robot-specific observation spaces
+        if hasattr(self.env.robots[0].robot_model, "torso_body"):
+            self.observation_space["secondary_imu_quat"] = spaces.Box(
+                low=-np.inf, high=np.inf, shape=(4,), dtype=np.float32
+            )
+            self.observation_space["secondary_imu_vel"] = spaces.Box(
+                low=-np.inf, high=np.inf, shape=(6,), dtype=np.float32
+            )
+
+    def get_action_space(self):
+        self.action_space = spaces.Dict(
+            {"q": spaces.Box(low=-np.inf, high=np.inf, shape=(self.total_dofs,), dtype=np.float32)}
+        )
+
+    def reset(self, seed=None, options=None):
+        raw_obs, info = super().reset(seed=seed, options=options)
+        obs = self.get_gr00t_observation(raw_obs)
+
+        lang = self.env.get_ep_meta().get("lang", "")
+        ROBOSUITE_DEFAULT_LOGGER.info(f"Instruction: {lang}")
+
+        return obs, info
+
+    def step(
+        self, action: Dict[str, Any]
+    ) -> Tuple[Dict[str, Any], float, bool, bool, Dict[str, Any]]:
+        # action={"q": xxx, "tau": xxx}
+        for k, v in action.items():
+            self.verbose and print("<ACTION>", k, v)
+
+        joint_actoin_vec = action["q"]
+        action_dict = {}
+        for ii, robot in enumerate(self.env.robots):
+            pf = robot.robot_model.naming_prefix
+            _action_dict = self.obs_action_converter[ii].gr00t_to_robocasa_action_dict(
+                joint_actoin_vec
+            )
+            action_dict.update({f"{pf}{k}": v for k, v in _action_dict.items()})
+            if action.get("tau", None) is not None:
+                _torque_dict = self.obs_action_converter[ii].gr00t_to_robocasa_action_dict(
+                    action["tau"]
+                )
+                action_dict.update({f"{pf}{k}_tau": v for k, v in _torque_dict.items()})
+            if self.overridden_floating_base_action is not None:
+                action_dict["robot0_base"] = self.overridden_floating_base_action
+        raw_obs, reward, terminated, truncated, info = super().step(action_dict)
+        obs = self.get_gr00t_observation(raw_obs)
+
+        for k, v in obs.items():
+            self.verbose and print("<OBS>", k, v.shape if k.startswith("video.") else v)
+        self.verbose = False
+
+        return obs, reward, terminated, truncated, info
+
+    def step_only_kinematics(
+        self, action: Dict[str, Any]
+    ) -> Tuple[Dict[str, Any], float, bool, bool, Dict[str, Any]]:
+        joint_actoin_vec = action["q"]
+        for ii, robot in enumerate(self.env.robots):
+            joint_names = np.array(self.env.sim.model.joint_names)[robot._ref_joint_indexes]
+            body_q = self.obs_action_converter[ii].gr00t_to_robocasa_joint_order(
+                joint_names, joint_actoin_vec
+            )
+            self.env.sim.data.qpos[robot._ref_joint_pos_indexes] = body_q
+
+            for side in ["left", "right"]:
+                joint_names = np.array(self.env.sim.model.joint_names)[
+                    robot._ref_joints_indexes_dict[side + "_gripper"]
+                ]
+                gripper_q = self.obs_action_converter[ii].gr00t_to_robocasa_joint_order(
+                    joint_names, joint_actoin_vec
+                )
+                self.env.sim.data.qpos[robot._ref_gripper_joint_pos_indexes[side]] = gripper_q
+
+        mujoco.mj_forward(self.env.sim.model._model, self.env.sim.data._data)
+
+        obs = self.force_update_observation()
+        return obs, 0, False, False, {"success": False}
+
+    def force_update_observation(self, timestep=0):
+        raw_obs = self.env._get_observations(force_update=True, timestep=timestep)
+        obs = self.get_basic_observation(raw_obs)
+        obs = self.get_gr00t_observation(obs)
+        return obs
+
+    def get_basic_observation(self, raw_obs):
+        # this function takes a lot of time, so we disable it for now
+        # raw_obs.update(gather_robot_observations(self.env, format_gripper_space=False))
+
+        # Image are in (H, W, C), flip it upside down
+        def process_img(img):
+            return np.copy(img[::-1, :, :])
+
+        for obs_name, obs_value in raw_obs.items():
+            if obs_name.endswith("_image"):
+                # image observations
+                raw_obs[obs_name] = process_img(obs_value)
+            else:
+                # non-image observations
+                raw_obs[obs_name] = obs_value.astype(np.float32)
+
+        # Return black image if rendering is disabled
+        if not self.enable_render:
+            for ii, name in enumerate(self.camera_names):
+                raw_obs[f"{name}_image"] = np.zeros(
+                    (self.camera_heights[ii], self.camera_widths[ii], 3), dtype=np.uint8
+                )
+
+        self.render_cache = raw_obs[self.render_obs_key]
+        raw_obs["language"] = self.env.get_ep_meta().get("lang", "")
+
+        return raw_obs
+
+    def convert_body_q(self, q: np.ndarray) -> np.ndarray:
+        # q is in the order of the joints
+        robot = self.env.robots[0]
+        joint_names = np.array(self.env.sim.model.joint_names)[robot._ref_joint_indexes]
+        # this joint names are in the order of the obs_vec
+        actuated_q = self.obs_action_converter[0].robocasa_to_gr00t_actuated_order(
+            joint_names, q, "body"
+        )
+        return actuated_q
+
+    def convert_gripper_q(self, q: np.ndarray, side: str = "left") -> np.ndarray:
+        # q is in the order of the joints
+        robot = self.env.robots[0]
+        joint_names = np.array(self.env.sim.model.joint_names)[
+            robot._ref_joints_indexes_dict[side + "_gripper"]
+        ]
+        actuated_q = self.obs_action_converter[0].robocasa_to_gr00t_actuated_order(
+            joint_names, q, side + "_gripper"
+        )
+        return actuated_q
+
+    def convert_gripper_tau(self, tau: np.ndarray, side: str = "left") -> np.ndarray:
+        # tau is in the order of the actuators
+        robot = self.env.robots[0]
+        actuator_idx = robot._ref_actuators_indexes_dict[side + "_gripper"]
+        actuated_joint_names = [
+            self.env.sim.model.joint_id2name(self.env.sim.model.actuator_trnid[i][0])
+            for i in actuator_idx
+        ]
+        actuated_tau = self.obs_action_converter[0].robocasa_to_gr00t_actuated_order(
+            actuated_joint_names, tau, side + "_gripper"
+        )
+        return actuated_tau
+
+    def get_gr00t_observation(self, raw_obs: Dict[str, Any]) -> Dict[str, Any]:
+        obs = {}
+
+        if self.env.sim.model.jnt_type[0] == mujoco.mjtJoint.mjJNT_FREE:
+            # If the first joint is a free joint, use this way to get the floating base data
+            obs["floating_base_pose"] = self.env.sim.data.qpos[:7]
+            obs["floating_base_vel"] = self.env.sim.data.qvel[:6]
+            obs["floating_base_acc"] = self.env.sim.data.qacc[:6]
+        else:
+            # Otherwise, use self.env.sim.model to fetch the floating base pose
+            root_body_id = self.env.sim.model.body_name2id("robot0_base")
+
+            # Get position and orientation from body state
+            root_pos = self.env.sim.data.body_xpos[root_body_id]
+            root_quat = self.env.sim.data.body_xquat[root_body_id]  # quaternion in wxyz format
+
+            # Combine position and quaternion to form 7-DOF pose
+            obs["floating_base_pose"] = np.concatenate([root_pos, root_quat])
+            # set vel and acc to 0
+            obs["floating_base_vel"] = np.zeros(6)
+            obs["floating_base_acc"] = np.zeros(6)
+
+        obs["body_q"] = self.convert_body_q(raw_obs["robot0_joint_pos"])
+        obs["body_dq"] = self.convert_body_q(raw_obs["robot0_joint_vel"])
+        obs["body_ddq"] = self.convert_body_q(raw_obs["robot0_joint_acc"])
+
+        obs["left_hand_q"] = self.convert_gripper_q(raw_obs["robot0_left_gripper_qpos"], "left")
+        obs["left_hand_dq"] = self.convert_gripper_q(raw_obs["robot0_left_gripper_qvel"], "left")
+        obs["left_hand_ddq"] = self.convert_gripper_q(raw_obs["robot0_left_gripper_qacc"], "left")
+        obs["right_hand_q"] = self.convert_gripper_q(raw_obs["robot0_right_gripper_qpos"], "right")
+        obs["right_hand_dq"] = self.convert_gripper_q(raw_obs["robot0_right_gripper_qvel"], "right")
+        obs["right_hand_ddq"] = self.convert_gripper_q(
+            raw_obs["robot0_right_gripper_qacc"], "right"
+        )
+
+        robot = self.env.robots[0]
+        body_tau_idx_list = []
+        left_gripper_tau_idx_list = []
+        right_gripper_tau_idx_list = []
+        for part_name, actuator_idx in robot._ref_actuators_indexes_dict.items():
+            if "left_gripper" in part_name:
+                left_gripper_tau_idx_list.extend(actuator_idx)
+            elif "right_gripper" in part_name:
+                right_gripper_tau_idx_list.extend(actuator_idx)
+            elif "base" in part_name:
+                assert (
+                    len(actuator_idx) == 0 or robot.robot_model.default_base == "FloatingLeggedBase"
+                )
+            else:
+                body_tau_idx_list.extend(actuator_idx)
+
+        body_tau_idx_list = sorted(body_tau_idx_list)
+        left_gripper_tau_idx_list = sorted(left_gripper_tau_idx_list)
+        right_gripper_tau_idx_list = sorted(right_gripper_tau_idx_list)
+        obs["body_tau_est"] = self.convert_body_q(
+            self.env.sim.data.actuator_force[body_tau_idx_list]
+        )
+        obs["right_hand_tau_est"] = self.convert_gripper_tau(
+            self.env.sim.data.actuator_force[right_gripper_tau_idx_list], "right"
+        )
+        obs["left_hand_tau_est"] = self.convert_gripper_tau(
+            self.env.sim.data.actuator_force[left_gripper_tau_idx_list], "left"
+        )
+
+        obs["time"] = self.env.sim.data.time
+
+        # Add camera images
+        for ii, camera_name in enumerate(self.camera_names):
+            mapped_camera_name = self.camera_key_mapper.get_camera_config(camera_name)[0]
+            obs[f"{mapped_camera_name}_image"] = raw_obs[f"{camera_name}_image"]
+
+        # Add privileged observations
+        if hasattr(self.env, "get_privileged_obs_keys"):
+            for key in self.env.get_privileged_obs_keys():
+                obs[key] = raw_obs[key]
+
+        # Add robot-specific observations
+        if hasattr(self.env.robots[0].robot_model, "torso_body"):
+            obs["secondary_imu_quat"] = raw_obs["robot0_torso_link_imu_quat"]
+            obs["secondary_imu_vel"] = raw_obs["robot0_torso_link_imu_vel"]
+
+        obs["language.language_instruction"] = raw_obs["language"]
+
+        return obs

GR00T-WholeBodyControl/decoupled_wbc/control/envs/robocasa/utils/robot_key_converter.py

@@ -0,0 +1,301 @@
+from dataclasses import dataclass
+from typing import Any, Dict, List, Tuple
+
+import numpy as np
+from robocasa.models.robots import remove_mimic_joints
+from robosuite.models.robots import RobotModel as RobosuiteRobotModel
+
+from decoupled_wbc.control.robot_model import RobotModel
+
+
+class Gr00tJointInfo:
+    """
+    Mapping from decoupled_wbc actuated joint names to robocasa joint names.
+    """
+
+    def __init__(self, robot_model: RobosuiteRobotModel):
+        self.robocasa_body_prefix = "robot0_"
+        self.robocasa_gripper_prefix = "gripper0_"
+
+        self.robot_model: RobotModel = robot_model
+        self.body_actuated_joint_names: List[str] = (
+            self.robot_model.supplemental_info.body_actuated_joints
+        )
+        self.left_hand_actuated_joint_names: List[str] = (
+            self.robot_model.supplemental_info.left_hand_actuated_joints
+        )
+        self.right_hand_actuated_joint_names: List[str] = (
+            self.robot_model.supplemental_info.right_hand_actuated_joints
+        )
+
+        self.actuated_joint_names: List[str] = self._get_gr00t_actuated_joint_names()
+        self.body_actuated_joint_to_index: Dict[str, int] = (
+            self._get_gr00t_body_actuated_joint_name_to_index()
+        )
+        self.gripper_actuated_joint_to_index: Tuple[Dict[str, int], Dict[str, int]] = (
+            self._get_gr00t_gripper_actuated_joint_name_to_index()
+        )
+        self.actuated_joint_name_to_index: Dict[str, int] = (
+            self._get_gr00t_actuated_joint_name_to_index()
+        )
+
+    def _get_gr00t_actuated_joint_names(self) -> List[str]:
+        """Get list of gr00t actuated joint names ordered by their indices."""
+        if self.robot_model.supplemental_info is None:
+            raise ValueError("Robot model must have supplemental_info")
+
+        # Get joint names and indices
+        body_names = self.robot_model.supplemental_info.body_actuated_joints
+        left_hand_names = self.robot_model.supplemental_info.left_hand_actuated_joints
+        right_hand_names = self.robot_model.supplemental_info.right_hand_actuated_joints
+
+        body_indices = self.robot_model.get_joint_group_indices("body")
+        left_hand_indices = self.robot_model.get_joint_group_indices("left_hand")
+        right_hand_indices = self.robot_model.get_joint_group_indices("right_hand")
+
+        # Create a dictionary mapping index to name
+        index_to_name = {}
+        for name, idx in zip(body_names, body_indices):
+            index_to_name[idx] = self.robocasa_body_prefix + name
+        for name, idx in zip(left_hand_names, left_hand_indices):
+            index_to_name[idx] = self.robocasa_gripper_prefix + "left_" + name
+        for name, idx in zip(right_hand_names, right_hand_indices):
+            index_to_name[idx] = self.robocasa_gripper_prefix + "right_" + name
+        sorted_indices = sorted(index_to_name.keys())
+        all_actuated_joint_names = [index_to_name[idx] for idx in sorted_indices]
+        return all_actuated_joint_names
+
+    def _get_gr00t_body_actuated_joint_name_to_index(self) -> Dict[str, int]:
+        """Get dictionary mapping gr00t actuated joint names to indices."""
+        if self.robot_model.supplemental_info is None:
+            raise ValueError("Robot model must have supplemental_info")
+        body_names = self.robot_model.supplemental_info.body_actuated_joints
+        body_indices = self.robot_model.get_joint_group_indices("body")
+        sorted_indices = np.argsort(body_indices)
+        sorted_names = [body_names[i] for i in sorted_indices]
+        return {self.robocasa_body_prefix + name: ii for ii, name in enumerate(sorted_names)}
+
+    def _get_gr00t_gripper_actuated_joint_name_to_index(
+        self,
+    ) -> Tuple[Dict[str, int], Dict[str, int]]:
+        """Get dictionary mapping gr00t actuated joint names to indices."""
+        if self.robot_model.supplemental_info is None:
+            raise ValueError("Robot model must have supplemental_info")
+        left_hand_names = self.robot_model.supplemental_info.left_hand_actuated_joints
+        right_hand_names = self.robot_model.supplemental_info.right_hand_actuated_joints
+        left_hand_indices = self.robot_model.get_joint_group_indices("left_hand")
+        right_hand_indices = self.robot_model.get_joint_group_indices("right_hand")
+        sorted_left_hand_indices = np.argsort(left_hand_indices)
+        sorted_right_hand_indices = np.argsort(right_hand_indices)
+        sorted_left_hand_names = [left_hand_names[i] for i in sorted_left_hand_indices]
+        sorted_right_hand_names = [right_hand_names[i] for i in sorted_right_hand_indices]
+        return (
+            {
+                self.robocasa_gripper_prefix + "left_" + name: ii
+                for ii, name in enumerate(sorted_left_hand_names)
+            },
+            {
+                self.robocasa_gripper_prefix + "right_" + name: ii
+                for ii, name in enumerate(sorted_right_hand_names)
+            },
+        )
+
+    def _get_gr00t_actuated_joint_name_to_index(self) -> Dict[str, int]:
+        """Get dictionary mapping gr00t actuated joint names to indices."""
+        return {name: ii for ii, name in enumerate(self.actuated_joint_names)}
+
+
+@dataclass
+class Gr00tObsActionConverter:
+    """
+    Converter to align simulation environment joint action space with real environment joint action space.
+    Handles joint order and range conversion.
+    """
+
+    robot_model: RobotModel
+    robosuite_robot_model: RobosuiteRobotModel
+    robocasa_body_prefix: str = "robot0_"
+    robocasa_gripper_prefix: str = "gripper0_"
+
+    def __post_init__(self):
+        """Initialize converter with robot configuration."""
+
+        self.robot_key = self.robot_model.supplemental_info.name
+        self.gr00t_joint_info = Gr00tJointInfo(self.robot_model)
+        self.robocasa_joint_names_for_each_part: Dict[str, List[str]] = (
+            self._get_robocasa_joint_names_for_each_part()
+        )
+        self.robocasa_actuator_names_for_each_part: Dict[str, List[str]] = (
+            self._get_robotcasa_actuator_names_for_each_part()
+        )
+
+        # Store mappings directly as class attributes
+        self.gr00t_joint_name_to_index = self.gr00t_joint_info.actuated_joint_name_to_index
+        self.gr00t_body_joint_name_to_index = self.gr00t_joint_info.body_actuated_joint_to_index
+        self.gr00t_gripper_joint_name_to_index = {
+            "left": self.gr00t_joint_info.gripper_actuated_joint_to_index[0],
+            "right": self.gr00t_joint_info.gripper_actuated_joint_to_index[1],
+        }
+        self.gr00t_to_robocasa_actuator_indices = self._get_actuator_mapping()
+
+        if self.robot_key == "GR1_Fourier":
+            self.joint_multiplier = (
+                lambda x: np.array([-1, 1, 1, -1, -1, -1, -1, -1, -1, -1, -1]) * x
+            )
+            self.actuator_multiplier = (
+                lambda x: np.array([-1, -1, -1, -1, -1, -1, -1, -1, 1, 1, -1]) * x
+            )
+        else:
+            self.joint_multiplier = lambda x: x
+            self.actuator_multiplier = lambda x: x
+
+        # Store DOF counts directly
+        self.body_dof = len(self.gr00t_joint_info.body_actuated_joint_names)
+        self.gripper_dof = len(self.gr00t_joint_info.left_hand_actuated_joint_names) + len(
+            self.gr00t_joint_info.right_hand_actuated_joint_names
+        )
+        self.whole_dof = self.body_dof + self.gripper_dof
+        self.body_nu = len(self.gr00t_joint_info.body_actuated_joint_names)
+        self.gripper_nu = len(self.gr00t_joint_info.left_hand_actuated_joint_names) + len(
+            self.gr00t_joint_info.right_hand_actuated_joint_names
+        )
+        self.whole_nu = self.body_nu + self.gripper_nu
+
+    def _get_robocasa_joint_names_for_each_part(self) -> Dict[str, List[str]]:
+        part_names = self.robosuite_robot_model._ref_joints_indexes_dict.keys()
+        robocasa_joint_names_for_each_part = {}
+        for part_name in part_names:
+            joint_indices = self.robosuite_robot_model._ref_joints_indexes_dict[part_name]
+            joint_names = [
+                self.robosuite_robot_model.sim.model.joint_id2name(j) for j in joint_indices
+            ]
+            robocasa_joint_names_for_each_part[part_name] = joint_names
+        return robocasa_joint_names_for_each_part
+
+    def _get_robotcasa_actuator_names_for_each_part(self) -> Dict[str, List[str]]:
+        part_names = self.robosuite_robot_model._ref_actuators_indexes_dict.keys()
+        robocasa_actuator_names_for_each_part = {}
+        for part_name in part_names:
+            if part_name == "base":
+                continue
+            actuator_indices = self.robosuite_robot_model._ref_actuators_indexes_dict[part_name]
+            actuator_names = [
+                self.robosuite_robot_model.sim.model.actuator_id2name(j) for j in actuator_indices
+            ]
+            robocasa_actuator_names_for_each_part[part_name] = actuator_names
+        return robocasa_actuator_names_for_each_part
+
+    def _get_actuator_mapping(self) -> Dict[str, List[int]]:
+        """Get mapping from decoupled_wbc actuatored joint order to robocasa actuatored joint order for whole body."""
+        return {
+            part_name: [
+                self.gr00t_joint_info.actuated_joint_name_to_index[j]
+                for j in self.robocasa_actuator_names_for_each_part[part_name]
+            ]
+            for part_name in self.robocasa_actuator_names_for_each_part.keys()
+        }
+
+    def check_action_dim_match(self, vec_dim: int) -> bool:
+        """
+        Check if input vector dimension matches expected dimension.
+
+        Args:
+            vec_dim: Dimension of input vector
+
+        Returns:
+            bool: True if dimensions match
+        """
+        return vec_dim == self.whole_dof
+
+    def gr00t_to_robocasa_action_dict(self, action_vec: np.ndarray) -> Dict[str, Any]:
+        """
+        Convert gr00t flat action vector to robocasa dictionary mapping part names to actions.
+
+        Args:
+            robot: Robocasa robot model instance
+            action_vec: Full action vector array in gr00t actuated joint order
+
+        Returns:
+            dict: Mapping from part names to action vectors for robocasa
+        """
+        if not self.check_action_dim_match(len(action_vec)):
+            raise ValueError(
+                f"Action vector dimension mismatch: {len(action_vec)} != {self.whole_dof}"
+            )
+
+        action_dict = {}
+        cc = self.robosuite_robot_model.composite_controller
+
+        for part_name, controller in cc.part_controllers.items():
+            if "gripper" in part_name:
+                robocasa_action = action_vec[self.gr00t_to_robocasa_actuator_indices[part_name]]
+                if self.actuator_multiplier is not None:
+                    robocasa_action = self.actuator_multiplier(robocasa_action)
+                action_dict[part_name] = remove_mimic_joints(
+                    cc.grippers[part_name], robocasa_action
+                )
+            elif "base" in part_name:
+                assert (
+                    len(self.gr00t_to_robocasa_actuator_indices.get(part_name, [])) == 0
+                    or self.robosuite_robot_model.default_base == "FloatingLeggedBase"
+                )
+            else:
+                action_dict[part_name] = action_vec[
+                    self.gr00t_to_robocasa_actuator_indices[part_name]
+                ]
+
+        return action_dict
+
+    def robocasa_to_gr00t_actuated_order(
+        self, joint_names: List[str], q: np.ndarray, obs_type: str = "body"
+    ) -> np.ndarray:
+        """
+        Convert observation from robocasa joint order to gr00t actuated joint order.
+
+        Args:
+            joint_names: List of joint names in robocasa order (with prefixes)
+            q: Joint positions corresponding to joint_names
+            obs_type: Type of observation ("body", "left_gripper", "right_gripper", or "whole")
+
+        Returns:
+            Joint positions in gr00t actuated joint order
+        """
+        assert len(joint_names) == len(q), "Joint names and q must have the same length"
+
+        if obs_type == "body":
+            actuated_q = np.zeros(self.body_dof)
+            for i, jn in enumerate(joint_names):
+                actuated_q[self.gr00t_body_joint_name_to_index[jn]] = q[i]
+        elif obs_type == "left_gripper":
+            actuated_q = np.zeros(self.gripper_dof // 2)
+            for i, jn in enumerate(joint_names):
+                actuated_q[self.gr00t_gripper_joint_name_to_index["left"][jn]] = q[i]
+        elif obs_type == "right_gripper":
+            actuated_q = np.zeros(self.gripper_dof // 2)
+            for i, jn in enumerate(joint_names):
+                actuated_q[self.gr00t_gripper_joint_name_to_index["right"][jn]] = q[i]
+        elif obs_type == "whole":
+            actuated_q = np.zeros(self.whole_dof)
+            for i, jn in enumerate(joint_names):
+                actuated_q[self.gr00t_joint_name_to_index[jn]] = q[i]
+        else:
+            raise ValueError(f"Unknown observation type: {obs_type}")
+        return actuated_q
+
+    def gr00t_to_robocasa_joint_order(
+        self, joint_names: List[str], q_in_actuated_order: np.ndarray
+    ) -> np.ndarray:
+        """
+        Convert gr00t actuated joint order to robocasa joint order.
+
+        Args:
+            joint_names: List of joint names in robocasa order (with prefixes)
+            q_in_actuated_order: Joint positions corresponding to joint_names in gr00t actuated joint order
+
+        Returns:
+            Joint positions in robocasa joint order
+        """
+        q = np.zeros(len(joint_names))
+        for i, jn in enumerate(joint_names):
+            q[i] = q_in_actuated_order[self.gr00t_joint_name_to_index[jn]]
+        return q

GR00T-WholeBodyControl/decoupled_wbc/control/envs/robocasa/utils/sim_utils.py

@@ -0,0 +1,8 @@
+try:
+    import robosuite.macros_private as macros
+except ImportError:
+    import robosuite.macros as macros
+
+
+def change_simulation_timestep(timestep: float):
+    macros.SIMULATION_TIMESTEP = timestep

GR00T-WholeBodyControl/decoupled_wbc/control/main/teleop/configs/configs.py

@@ -0,0 +1,483 @@
+from dataclasses import dataclass
+import os
+from pathlib import Path
+from typing import Literal, Optional
+
+import yaml
+
+import decoupled_wbc
+from decoupled_wbc.control.main.config_template import ArgsConfig as ArgsConfigTemplate
+from decoupled_wbc.control.policy.wbc_policy_factory import WBC_VERSIONS
+from decoupled_wbc.control.utils.network_utils import resolve_interface
+
+
+def override_wbc_config(
+    wbc_config: dict, config: "BaseConfig", missed_keys_only: bool = False
+) -> dict:
+    """Override WBC YAML values with dataclass values.
+
+    Args:
+        wbc_config: The loaded WBC YAML configuration dictionary
+        config: The BaseConfig dataclass instance with override values
+        missed_keys_only: If True, only add keys that don't exist in wbc_config.
+                         If False, validate all keys exist and override all.
+
+    Returns:
+        Updated wbc_config dictionary with overridden values
+
+    Raises:
+        KeyError: If any required keys are missing from the WBC YAML configuration
+                  (only when missed_keys_only=False)
+    """
+    # Override yaml values with dataclass values
+    key_to_value = {
+        "INTERFACE": config.interface,
+        "ENV_TYPE": config.env_type,
+        "VERSION": config.wbc_version,
+        "SIMULATOR": config.simulator,
+        "SIMULATE_DT": 1 / float(config.sim_frequency),
+        "ENABLE_OFFSCREEN": config.enable_offscreen,
+        "ENABLE_ONSCREEN": config.enable_onscreen,
+        "model_path": config.wbc_model_path,
+        "enable_waist": config.enable_waist,
+        "with_hands": config.with_hands,
+        "verbose": config.verbose,
+        "verbose_timing": config.verbose_timing,
+        "upper_body_max_joint_speed": config.upper_body_joint_speed,
+        "keyboard_dispatcher_type": config.keyboard_dispatcher_type,
+        "enable_gravity_compensation": config.enable_gravity_compensation,
+        "gravity_compensation_joints": config.gravity_compensation_joints,
+        "high_elbow_pose": config.high_elbow_pose,
+    }
+
+    if missed_keys_only:
+        # Only add keys that don't exist in wbc_config
+        for key in key_to_value:
+            if key not in wbc_config:
+                wbc_config[key] = key_to_value[key]
+    else:
+        # Set all keys (overwrite existing)
+        for key in key_to_value:
+            wbc_config[key] = key_to_value[key]
+
+    # g1 kp, kd, sim2real gap
+    if config.env_type == "real":
+        # update waist pitch damping, index 14
+        wbc_config["MOTOR_KD"][14] = wbc_config["MOTOR_KD"][14] - 10
+
+    return wbc_config
+
+
+@dataclass
+class BaseConfig(ArgsConfigTemplate):
+    """Base config inherited by all G1 control loops"""
+
+    # WBC Configuration
+    wbc_version: Literal[tuple(WBC_VERSIONS)] = "gear_wbc"
+    """Version of the whole body controller."""
+
+    wbc_model_path: str = (
+        "policy/GR00T-WholeBodyControl-Balance.onnx," "policy/GR00T-WholeBodyControl-Walk.onnx"
+    )
+    """Path to WBC model file (relative to decoupled_wbc/sim2mujoco/resources/robots/g1)"""
+    """gear_wbc model path: policy/GR00T-WholeBodyControl-Balance.onnx,policy/GR00T-WholeBodyControl-Walk.onnx"""
+
+    wbc_policy_class: str = "G1DecoupledWholeBodyPolicy"
+    """Whole body policy class."""
+
+    # System Configuration
+    interface: str = "sim"
+    """Interface to use for the control loop. [sim, real, lo, enxe8ea6a9c4e09]"""
+
+    simulator: str = "mujoco"
+    """Simulator to use."""
+
+    sim_sync_mode: bool = False
+    """Whether to run the control loop in sync mode."""
+
+    control_frequency: int = 50
+    """Frequency of the control loop."""
+
+    sim_frequency: int = 200
+    """Frequency of the simulation loop."""
+
+    # Robot Configuration
+    enable_waist: bool = False
+    """Whether to include waist joints in IK."""
+
+    with_hands: bool = True
+    """Enable hand functionality. When False, robot operates without hands."""
+
+    high_elbow_pose: bool = False
+    """Enable high elbow pose configuration for default joint positions."""
+
+    verbose: bool = True
+    """Whether to print verbose output."""
+
+    # Additional common fields
+    enable_offscreen: bool = False
+    """Whether to enable offscreen rendering."""
+
+    enable_onscreen: bool = True
+    """Whether to enable onscreen rendering."""
+
+    upper_body_joint_speed: float = 1000
+    """Upper body joint speed."""
+
+    env_name: str = "default"
+    """Environment name."""
+
+    ik_indicator: bool = False
+    """Whether to draw IK indicators."""
+
+    verbose_timing: bool = False
+    """Enable verbose timing output every iteration."""
+
+    keyboard_dispatcher_type: str = "raw"
+    """Keyboard dispatcher to use. [raw, ros]"""
+
+    # Gravity Compensation Configuration
+    enable_gravity_compensation: bool = False
+    """Enable gravity compensation using pinocchio dynamics."""
+
+    gravity_compensation_joints: Optional[list[str]] = None
+    """Joint groups to apply gravity compensation to (e.g., ['arms', 'left_arm', 'right_arm'])."""
+    # Teleop/Device Configuration
+    body_control_device: str = "dummy"
+    """Device to use for body control. Options: dummy, vive, iphone, leapmotion, joycon."""
+
+    hand_control_device: Optional[str] = "dummy"
+    """Device to use for hand control. Options: None, manus, joycon, iphone."""
+
+    body_streamer_ip: str = "10.112.210.229"
+    """IP address for body streamer (vive only)."""
+
+    body_streamer_keyword: str = "knee"
+    """Body streamer keyword (vive only)."""
+
+    enable_visualization: bool = False
+    """Whether to enable visualization."""
+
+    enable_real_device: bool = True
+    """Whether to enable real device."""
+
+    teleop_frequency: int = 20
+    """Teleoperation frequency (Hz)."""
+
+    teleop_replay_path: Optional[str] = None
+    """Path to teleop replay data."""
+
+    # Deployment/Camera Configuration
+    robot_ip: str = "192.168.123.164"
+    """Robot IP address"""
+    # Data collection settings
+    data_collection: bool = True
+    """Enable data collection"""
+
+    data_collection_frequency: int = 20
+    """Data collection frequency (Hz)"""
+
+    root_output_dir: str = "outputs"
+    """Root output directory"""
+
+    # Policy settings
+    enable_upper_body_operation: bool = True
+    """Enable upper body operation"""
+
+    upper_body_operation_mode: Literal["teleop", "inference"] = "teleop"
+    """Upper body operation mode"""
+
+    def __post_init__(self):
+        # Resolve interface (handles sim/real shortcuts, platform differences, and error handling)
+        self.interface, self.env_type = resolve_interface(self.interface)
+
+    def load_wbc_yaml(self) -> dict:
+        """Load and merge wbc yaml with dataclass overrides"""
+        # Get the base path to decoupled_wbc and convert to Path object
+        package_path = Path(os.path.dirname(decoupled_wbc.__file__))
+
+        if self.wbc_version == "gear_wbc":
+            config_path = str(package_path / "control/main/teleop/configs/g1_29dof_gear_wbc.yaml")
+        else:
+            raise ValueError(
+                f"Invalid wbc_version: {self.wbc_version}, please use one of: " f"gear_wbc"
+            )
+
+        with open(config_path) as file:
+            wbc_config = yaml.load(file, Loader=yaml.FullLoader)
+
+        # Override yaml values with dataclass values
+        wbc_config = override_wbc_config(wbc_config, self)
+
+        return wbc_config
+
+
+@dataclass
+class ControlLoopConfig(BaseConfig):
+    """Config for running the G1 control loop."""
+
+    pass
+
+
+@dataclass
+class TeleopConfig(BaseConfig):
+    """Config for running the G1 teleop policy loop."""
+
+    robot: Literal["g1"] = "g1"
+    """Name of the robot to use, e.g., 'g1'."""
+
+    lerobot_replay_path: Optional[str] = None
+    """Path to lerobot replay data."""
+
+    # Override defaults for teleop-specific values
+    body_streamer_ip: str = "10.110.67.24"
+    """IP address for body streamer (vive only)."""
+
+    body_streamer_keyword: str = "foot"
+    """Keyword for body streamer (vive only)."""
+
+    teleop_frequency: float = 20  # Override to be float instead of int
+    """Frequency of the teleop loop."""
+
+    binary_hand_ik: bool = True
+    """Whether to use binary IK."""
+
+
+@dataclass
+class ComposedCameraClientConfig:
+    """Config for running the composed camera client."""
+
+    camera_port: int = 5555
+    """Port number"""
+
+    camera_host: str = "localhost"
+    """Host IP address"""
+
+    fps: float = 20.0
+    """FPS of the camera viewer"""
+
+
+@dataclass
+class DataExporterConfig(BaseConfig, ComposedCameraClientConfig):
+    """Config for running the G1 data exporter."""
+
+    dataset_name: Optional[str] = None
+    """Name of the dataset to save the data to. If the dataset already exists,
+    the new episodes will be appended to existing dataset. If the dataset does not exist,
+    episodes will be saved under root_output_dir/dataset_name.
+    """
+
+    task_prompt: str = "demo"
+    """Language Task prompt for the dataset."""
+
+    state_dim: int = 43
+    """Size of the state."""
+
+    action_dim: int = 43
+    """Size of the action."""
+
+    teleoperator_username: Optional[str] = None
+    """Teleoperator username."""
+
+    support_operator_username: Optional[str] = None
+    """Support operator username."""
+
+    robot_id: Optional[str] = None
+    """Robot ID."""
+
+    lower_body_policy: Optional[str] = None
+    """Lower body policy."""
+
+    img_stream_viewer: bool = False
+    """Whether to open a matplot lib window to view the camera images."""
+
+    text_to_speech: bool = True
+    """Whether to use text-to-speech for voice feedback."""
+
+    add_stereo_camera: bool = True
+    """Whether to add stereo camera for data collection. If False, only use a signle ego view camera."""
+
+
+@dataclass
+class SyncSimDataCollectionConfig(ControlLoopConfig, TeleopConfig):
+    """Args Config for running the data collection loop."""
+
+    robot: str = "G1"
+    """Name of the robot to collect data for (e.g., G1 variants)."""
+
+    task_name: str = "GroundOnly"
+    """Name of the task to collect data for. [PnPBottle, GroundOnly, ...]"""
+
+    body_control_device: str = "dummy"
+    """Device to use for body control. Options: dummy, vive, iphone, leapmotion, joycon."""
+
+    hand_control_device: Optional[str] = "dummy"
+    """Device to use for hand control. Options: None, manus, joycon, iphone."""
+
+    remove_existing_dir: bool = False
+    """Whether to remove existing output directory if it exists."""
+
+    hardcode_teleop_cmd: bool = False
+    """Whether to hardcode the teleop command for testing purposes."""
+
+    ik_indicator: bool = False
+    """Whether to draw IK indicators."""
+
+    enable_onscreen: bool = True
+    """Whether to show the onscreen rendering."""
+
+    save_img_obs: bool = False
+    """Whether to save image observations."""
+
+    success_hold_steps: int = 50
+    """Number of steps to collect after task completion before saving."""
+
+    renderer: Literal["mjviewer", "mujoco", "rerun"] = "mjviewer"
+    """Renderer to use for the environment. """
+
+    replay_data_path: str | None = None
+    """Path to the data (.pkl) to replay. If None, will not replay. Used for CI/CD."""
+
+    replay_speed: float = 2.5
+    """Speed multiplier for replay data. Higher values make replay slower (e.g., 2.5 for sync sim tests)."""
+
+    ci_test: bool = False
+    """Whether to run the CI test."""
+
+    ci_test_mode: Literal["unit", "pre_merge"] = "pre_merge"
+    """'unit' for fast 50-step tests, 'pre_merge' for 500-step test with tracking checks."""
+
+    manual_control: bool = False
+    """Enable manual control of data collection start/save. When True, use toggle_data_collection
+    to manually control episode states (idle -> recording -> need_to_save -> idle).
+    When False (default), automatically starts and stops data collection based on task completion."""
+
+
+@dataclass
+class SyncSimPlaybackConfig(SyncSimDataCollectionConfig):
+    """Configuration class for playback script arguments."""
+
+    enable_real_device: bool = False
+    """Whether to enable real device"""
+
+    dataset: str | None = None
+    """Path to the demonstration dataset, either an HDF5 file or a LeRobot folder path."""
+
+    use_actions: bool = False
+    """Whether to use actions for playback"""
+
+    use_wbc_goals: bool = False
+    """Whether to use WBC goals for control"""
+
+    use_teleop_cmd: bool = False
+    """Whether to use teleop IK for action generation"""
+
+    # Video recording arguments.
+    # Warning: enabling this key will leads to divergence between playback and recording.
+    save_video: bool = False
+    """Whether to save video of the playback"""
+
+    # Saving to LeRobot dataset.
+    # Warning: enabling this key will leads to divergence between playback and recording.
+    save_lerobot: bool = False
+    """Whether to save the playback as a new LeRobot dataset"""
+
+    video_path: str | None = None
+    """Path to save the output video. If not specified, 
+       will use the nearest folder to dataset and save as playback_video.mp4"""
+
+    num_episodes: int = 1
+    """Number of episodes to load and playback/record (loads only the first N episodes from dataset)"""
+
+    intervention: bool = False
+    """Whether to denote intervention timesteps with colored borders in video frames"""
+
+    ci_test: bool = False
+    """Whether this is a CI test run, which limits the number of steps for testing purposes"""
+
+    def validate_args(self):
+        # Validate argument combinations
+        if self.use_teleop_cmd and not self.use_actions:
+            raise ValueError("--use-teleop-cmd requires --use-actions to be set")
+
+        # Note: using teleop cmd has playback divergence unlike using wbc goals, as TeleopPolicy has a warmup loop
+        if self.use_teleop_cmd and self.use_wbc_goals:
+            raise ValueError("--use-teleop-cmd and --use-wbc-goals are mutually exclusive")
+
+        if (self.use_teleop_cmd or self.use_wbc_goals) and not self.use_actions:
+            raise ValueError(
+                "You are using --use-teleop-cmd or --use-wbc-goals but not --use-actions. "
+                "This will not play back actions whether via teleop or wbc goals. "
+                "Instead, it'll play back states only."
+            )
+
+        if self.save_img_obs and not self.save_lerobot:
+            raise ValueError("--save-img-obs is only supported with --save-lerobot")
+
+        if self.intervention and not self.save_video:
+            raise ValueError("--intervention requires --save-video to be enabled for visualization")
+
+
+@dataclass
+class WebcamRecorderConfig(BaseConfig):
+    """Config for running the webcam recorder."""
+
+    output_dir: str = "logs_experiment"
+    """Output directory for webcam recordings"""
+
+    device_id: int = 0
+    """Camera device ID"""
+
+    fps: int = 30
+    """Recording frame rate"""
+
+    duration: Optional[int] = None
+    """Recording duration in seconds (None for continuous)"""
+
+
+@dataclass
+class SimLoopConfig(BaseConfig):
+    """Config for running the simulation loop."""
+
+    mp_start_method: str = "spawn"
+    """Multiprocessing start method"""
+
+    enable_image_publish: bool = False
+    """Enable image publishing in simulation"""
+
+    camera_port: int = 5555
+    """Camera port for image publishing"""
+
+    verbose: bool = False
+    """Verbose output, override the base config verbose"""
+
+
+@dataclass
+class DeploymentConfig(BaseConfig, ComposedCameraClientConfig):
+    """G1 Robot Deployment Configuration
+
+    Simplified deployment config that inherits all common fields from G1BaseConfig.
+    All deployment settings are now available in the base config.
+    """
+
+    camera_publish_rate: float = 30.0
+    """Camera publish rate (Hz)"""
+
+    view_camera: bool = True
+    """Enable camera viewer"""
+    # Webcam recording settings
+    enable_webcam_recording: bool = True
+    """Enable webcam recording for real robot deployment monitoring"""
+
+    webcam_output_dir: str = "logs_experiment"
+    """Output directory for webcam recordings"""
+
+    skip_img_transform: bool = False
+    """Skip image transformation in the model (for faster internet)"""
+
+    sim_in_single_process: bool = False
+    """Run simulator in a separate process. When True, sets simulator to None in main control loop
+    and launches run_sim_loop.py separately."""
+
+    image_publish: bool = False
+    """Enable image publishing in simulation loop (passed to run_sim_loop.py)"""

GR00T-WholeBodyControl/decoupled_wbc/control/main/teleop/configs/g1_29dof_gear_wbc.yaml

@@ -0,0 +1,421 @@
+GEAR_WBC_CONFIG: "decoupled_wbc/sim2mujoco/resources/robots/g1/g1_gear_wbc.yaml"
+
+# copy from g1_43dof_hist.yaml
+ROBOT_TYPE: 'g1_29dof' # Robot name, "go2", "b2", "b2w", "h1", "go2w", "g1" 
+ROBOT_SCENE: "decoupled_wbc/control/robot_model/model_data/g1/scene_43dof.xml" # Robot scene, for Sim2Sim
+# ROBOT_SCENE: "decoupled_wbc/control/robot_model/model_data/g1/scene_29dof_activated3dex.xml"
+
+DOMAIN_ID: 0 # Domain id
+# Network Interface, "lo" for simulation and the one with "192.168.123.222" for real robot
+# INTERFACE: "enxe8ea6a9c4e09"
+# INTERFACE: "enxc8a3623c9cb7"
+INTERFACE: "lo"
+SIMULATOR: "mujoco"  # "robocasa"
+
+USE_JOYSTICK: 0 # Simulate Unitree WirelessController using a gamepad (0: disable, 1: enable)
+JOYSTICK_TYPE: "xbox" # support "xbox" and "switch" gamepad layout
+JOYSTICK_DEVICE: 0 # Joystick number
+
+FREE_BASE: False
+
+PRINT_SCENE_INFORMATION: True # Print link, joint and sensors information of robot
+ENABLE_ELASTIC_BAND: True # Virtual spring band, used for lifting h1
+
+SIMULATE_DT: 0.005  # Need to be larger than the runtime of viewer.sync()
+VIEWER_DT: 0.02  # Viewer update time
+REWARD_DT: 0.02
+USE_SENSOR: False
+USE_HISTORY: True
+USE_HISTORY_LOCO: True
+USE_HISTORY_MIMIC: True
+
+GAIT_PERIOD: 0.9 # 1.25
+
+MOTOR2JOINT: [0, 1, 2, 3, 4, 5,
+              6, 7, 8, 9, 10, 11,
+              12, 13, 14, 
+              15, 16, 17, 18, 19, 20, 21, 
+              22, 23, 24, 25, 26, 27, 28]
+            
+JOINT2MOTOR: [0, 1, 2, 3, 4, 5,
+              6, 7, 8, 9, 10, 11,
+              12, 13, 14, 
+              15, 16, 17, 18, 19, 20, 21, 
+              22, 23, 24, 25, 26, 27, 28]
+            
+
+UNITREE_LEGGED_CONST:
+  HIGHLEVEL: 0xEE
+  LOWLEVEL: 0xFF
+  TRIGERLEVEL: 0xF0
+  PosStopF: 2146000000.0
+  VelStopF: 16000.0
+  MODE_MACHINE: 5
+  MODE_PR:  0
+
+JOINT_KP: [
+    100, 100, 100, 200, 20, 20,
+    100, 100, 100, 200, 20, 20,
+    400, 400, 400,
+    90, 60, 20, 60, 4, 4, 4,
+    90, 60, 20, 60, 4, 4, 4
+]
+
+
+JOINT_KD: [
+   2.5, 2.5, 2.5, 5, 0.2, 0.1,
+   2.5, 2.5, 2.5, 5, 0.2, 0.1,
+   5.0, 5.0, 5.0,
+   2.0, 1.0, 0.4, 1.0, 0.2, 0.2, 0.2,
+    2.0, 1.0, 0.4, 1.0, 0.2, 0.2, 0.2
+]
+
+# arm kp
+# soft kp, safe, test it first
+# 50, 50, 20, 20, 10, 10, 10
+# hard kp, use only if policy is safe
+# 200, 200, 80, 80, 50, 50, 50,
+
+# MOTOR_KP: [
+#     100, 100, 100, 200, 20, 20,
+#     100, 100, 100, 200, 20, 20,
+#     400, 400, 400,
+#     50, 50, 20, 20, 10, 10, 10,
+#     50, 50, 20, 20, 10, 10, 10
+# ]
+
+MOTOR_KP: [
+    150, 150, 150, 200, 40, 40,
+    150, 150, 150, 200, 40, 40, 
+    250, 250, 250,
+    100, 100, 40, 40, 20, 20, 20,
+    100, 100, 40, 40, 20, 20, 20
+]
+
+MOTOR_KD: [
+    2, 2, 2, 4, 2, 2,
+    2, 2, 2, 4, 2, 2,
+    5, 5, 5,
+    5, 5, 2, 2, 2, 2, 2,
+    5, 5, 2, 2, 2, 2, 2
+]
+
+# MOTOR_KP: [
+#     100, 100, 100, 200, 20, 20,
+#     100, 100, 100, 200, 20, 20,
+#     400, 400, 400,
+#     90, 60, 20, 60, 4, 4, 4,
+#     90, 60, 20, 60, 4, 4, 4
+# ]
+
+
+# MOTOR_KD: [
+#    2.5, 2.5, 2.5, 5, 0.2, 0.1,
+#    2.5, 2.5, 2.5, 5, 0.2, 0.1,
+#    5.0, 5.0, 5.0,
+#    2.0, 1.0, 0.4, 1.0, 0.2, 0.2, 0.2,
+#     2.0, 1.0, 0.4, 1.0, 0.2, 0.2, 0.2
+# ]
+
+
+WeakMotorJointIndex:
+  left_hip_yaw_joint: 0
+  left_hip_roll_joint: 1
+  left_hip_pitch_joint: 2
+  left_knee_joint: 3
+  left_ankle_pitch_joint: 4
+  left_ankle_roll_joint: 5
+  right_hip_yaw_joint: 6
+  right_hip_roll_joint: 7
+  right_hip_pitch_joint: 8
+  right_knee_joint: 9
+  right_ankle_pitch_joint: 10
+  right_ankle_roll_joint: 11
+  waist_yaw_joint : 12
+  waist_roll_joint : 13
+  waist_pitch_joint : 14
+  left_shoulder_pitch_joint: 15
+  left_shoulder_roll_joint: 16
+  left_shoulder_yaw_joint: 17
+  left_elbow_joint: 18
+  left_wrist_roll_joint: 19
+  left_wrist_pitch_joint: 20
+  left_wrist_yaw_joint: 21
+  right_shoulder_pitch_joint: 22
+  right_shoulder_roll_joint: 23
+  right_shoulder_yaw_joint: 24
+  right_elbow_joint: 25
+  right_wrist_roll_joint: 26
+  right_wrist_pitch_joint: 27
+  right_wrist_yaw_joint: 28
+
+NUM_MOTORS: 29
+NUM_JOINTS: 29
+NUM_HAND_MOTORS: 7
+NUM_HAND_JOINTS: 7
+NUM_UPPER_BODY_JOINTS: 17
+
+DEFAULT_DOF_ANGLES: [
+    -0.1,  # left_hip_pitch_joint 
+    0.0,  # left_hip_roll_joint
+    0.0,  # left_hip_yaw_joint
+    0.3,  # left_knee_joint
+    -0.2, # left_ankle_pitch_joint
+    0.0,  # left_ankle_roll_joint
+    -0.1, # right_hip_pitch_joint
+    0.0,  # right_hip_roll_joint
+    0.0,  # right_hip_yaw_joint
+    0.3,  # right_knee_joint
+    -0.2, # right_ankle_pitch_joint
+    0.0,  # right_ankle_roll_joint
+    0.0,  # waist_yaw_joint
+    0.0,  # waist_roll_joint
+    0.0,  # waist_pitch_joint
+    0.0,  # left_shoulder_pitch_joint
+    0.0,  # left_shoulder_roll_joint
+    0.0,  # left_shoulder_yaw_joint
+    0.0,  # left_elbow_joint
+    0.0,  # left_wrist_roll_joint
+    0.0,  # left_wrist_pitch_joint
+    0.0,  # left_wrist_yaw_joint
+    0.0,  # right_shoulder_pitch_joint
+    0.0,  # right_shoulder_roll_joint
+    0.0,  # right_shoulder_yaw_joint
+    0.0,  # right_elbow_joint
+    0.0,  # right_wrist_roll_joint
+    0.0,  # right_wrist_pitch_joint
+    0.0   # right_wrist_yaw_joint
+]
+
+DEFAULT_MOTOR_ANGLES: [
+    -0.1,  # left_hip_pitch_joint 
+    0.0,  # left_hip_roll_joint
+    0.0,  # left_hip_yaw_joint
+    0.3,  # left_knee_joint
+    -0.2, # left_ankle_pitch_joint
+    0.0,  # left_ankle_roll_joint
+    -0.1, # right_hip_pitch_joint
+    0.0,  # right_hip_roll_joint
+    0.0,  # right_hip_yaw_joint
+    0.3,  # right_knee_joint
+    -0.2, # right_ankle_pitch_joint
+    0.0,  # right_ankle_roll_joint
+    0.0,  # waist_yaw_joint
+    0.0,  # waist_roll_joint
+    0.0,  # waist_pitch_joint
+    0.0,  # left_shoulder_pitch_joint
+    0.0,  # left_shoulder_roll_joint
+    0.0,  # left_shoulder_yaw_joint
+    0.0,  # left_elbow_joint
+    0.0,  # left_wrist_roll_joint
+    0.0,  # left_wrist_pitch_joint
+    0.0,  # left_wrist_yaw_joint
+    0.0,  # right_shoulder_pitch_joint
+    0.0,  # right_shoulder_roll_joint
+    0.0,  # right_shoulder_yaw_joint
+    0.0,  # right_elbow_joint
+    0.0,  # right_wrist_roll_joint
+    0.0,  # right_wrist_pitch_joint
+    0.0   # right_wrist_yaw_joint
+]
+
+motor_pos_lower_limit_list: [-2.5307, -0.5236, -2.7576, -0.087267, -0.87267, -0.2618, 
+                             -2.5307, -2.9671, -2.7576, -0.087267, -0.87267, -0.2618, 
+                             -2.618, -0.52, -0.52,
+                             -3.0892, -1.5882, -2.618, -1.0472, 
+                             -1.972222054, -1.61443, -1.61443,
+                             -3.0892, -2.2515, -2.618, -1.0472, 
+                             -1.972222054, -1.61443, -1.61443]
+motor_pos_upper_limit_list: [2.8798, 2.9671, 2.7576, 2.8798, 0.5236, 0.2618, 
+                            2.8798, 0.5236, 2.7576, 2.8798, 0.5236, 0.2618, 
+                            2.618, 0.52, 0.52,
+                            2.6704, 2.2515, 2.618, 2.0944, 
+                            1.972222054, 1.61443, 1.61443,
+                            2.6704, 1.5882, 2.618, 2.0944, 
+                            1.972222054, 1.61443, 1.61443]
+motor_vel_limit_list: [32.0, 32.0, 32.0, 20.0, 37.0, 37.0, 
+                      32.0, 32.0, 32.0, 20.0, 37.0, 37.0, 
+                      32.0, 37.0, 37.0, 
+                      37.0, 37.0, 37.0, 37.0, 
+                      37.0, 22.0, 22.0,
+                      37.0, 37.0, 37.0, 37.0, 
+                      37.0, 22.0, 22.0]
+motor_effort_limit_list: [88.0, 88.0, 88.0, 139.0, 50.0, 50.0, 
+                        88.0, 88.0, 88.0, 139.0, 50.0, 50.0, 
+                        88.0, 50.0, 50.0,
+                        25.0, 25.0, 25.0, 25.0, 
+                        25.0, 5.0, 5.0,
+                        2.45, 0.7, 0.7, 0.7, 0.7, 0.7, 0.7,
+                        25.0, 25.0, 25.0, 25.0, 
+                        25.0, 5.0, 5.0,
+                        2.45, 0.7, 0.7, 0.7, 0.7, 0.7, 0.7]
+history_config: {
+                  base_ang_vel: 4,
+                  projected_gravity: 4,
+                  command_lin_vel: 4,
+                  command_ang_vel: 4,
+                  command_base_height: 4,
+                  command_stand: 4,
+                  ref_upper_dof_pos: 4,
+                  dof_pos: 4,
+                  dof_vel: 4,
+                  actions: 4,
+                  # phase_time: 4,
+                  ref_motion_phase: 4,
+                  sin_phase: 4,
+                  cos_phase: 4
+                }
+history_loco_config: {
+                  base_ang_vel: 4,
+                  projected_gravity: 4,
+                  command_lin_vel: 4,
+                  command_ang_vel: 4,
+                  # command_base_height: 4,
+                  command_stand: 4,
+                  ref_upper_dof_pos: 4,
+                  dof_pos: 4,
+                  dof_vel: 4,
+                  actions: 4,
+                  # phase_time: 4,
+                  sin_phase: 4,
+                  cos_phase: 4
+                }
+history_loco_height_config: {
+                  base_ang_vel: 4,
+                  projected_gravity: 4,
+                  command_lin_vel: 4,
+                  command_ang_vel: 4,
+                  command_base_height: 4,
+                  command_stand: 4,
+                  ref_upper_dof_pos: 4,
+                  dof_pos: 4,
+                  dof_vel: 4,
+                  actions: 4,
+                  # phase_time: 4,
+                  sin_phase: 4,
+                  cos_phase: 4
+                }
+history_mimic_config: {
+                  base_ang_vel: 4,
+                  projected_gravity: 4,
+                  dof_pos: 4,
+                  dof_vel: 4,
+                  actions: 4,
+                  ref_motion_phase: 4,
+                }
+obs_dims: {
+            base_lin_vel: 3,
+            base_ang_vel: 3,
+            projected_gravity: 3,
+            command_lin_vel: 2,
+            command_ang_vel: 1,
+            command_stand: 1,
+            command_base_height: 1,
+            ref_upper_dof_pos: 17, # upper body actions
+            dof_pos: 29,
+            dof_vel: 29,
+            # actions: 12, # lower body actions
+            actions: 29, # full body actions
+            phase_time: 1,
+            ref_motion_phase: 1, # mimic motion phase
+            sin_phase: 1,
+            cos_phase: 1,
+          }
+obs_loco_dims: {
+            base_lin_vel: 3,
+            base_ang_vel: 3,
+            projected_gravity: 3,
+            command_lin_vel: 2,
+            command_ang_vel: 1,
+            command_stand: 1,
+            command_base_height: 1,
+            ref_upper_dof_pos: 17, # upper body actions
+            dof_pos: 29,
+            dof_vel: 29,
+            actions: 12, # lower body actions
+            phase_time: 1,
+            sin_phase: 1,
+            cos_phase: 1,
+          }
+obs_mimic_dims: {
+            base_lin_vel: 3,
+            base_ang_vel: 3,
+            projected_gravity: 3,
+            dof_pos: 29,
+            dof_vel: 29,
+            actions: 29, # full body actions
+            ref_motion_phase: 1, # mimic motion phase
+          }
+obs_scales: {
+    base_lin_vel: 2.0,
+    base_ang_vel: 0.25,
+    projected_gravity: 1.0,
+    command_lin_vel: 1,
+    command_ang_vel: 1,
+    command_stand: 1,
+    command_base_height: 2, # Yuanhang: it's 2, not 1!
+    ref_upper_dof_pos: 1.0,
+    dof_pos: 1.0,
+    dof_vel: 0.05,
+    history: 1.0,
+    history_loco: 1.0,
+    history_mimic: 1.0,
+    actions: 1.0,
+    phase_time: 1.0,
+    ref_motion_phase: 1.0,
+    sin_phase: 1.0,
+    cos_phase: 1.0
+  }
+
+loco_upper_body_dof_pos: [
+  0.0, 0.0, 0.0, # waist
+  0.0, 0.3, 0.0, 1.0, # left shoulder and elbow
+  0.0, 0.0, 0.0, # left wrist
+  0.0, -0.3, 0.0, 1.0, # right shoulder and elbow
+  0.0, 0.0, 0.0 # right wrist
+]
+
+robot_dofs: {
+  "g1_29dof": [1, 1, 1, 1, 1, 1, 
+               1, 1, 1, 1, 1, 1,
+               1, 1, 1,
+               1, 1, 1, 1, 1, 1, 1,
+               1, 1, 1, 1, 1, 1, 1],
+  "g1_29dof_anneal_23dof": [1, 1, 1, 1, 1, 1, 
+                            1, 1, 1, 1, 1, 1,
+                            1, 1, 1,
+                            1, 1, 1, 1, 0, 0, 0,
+                            1, 1, 1, 1, 0, 0, 0],
+}
+
+mimic_robot_types: {
+
+  "APT_level1": "g1_29dof_anneal_23dof",
+}
+
+
+
+
+# 01281657
+mimic_models: {
+  "APT_level1": "20250116_225127-TairanTestbed_G129dofANNEAL23dof_dm_APT_video_APT_level1_MinimalFriction-0.3_RfiTrue_Far0.325_RESUME_LARGENOISE-motion_tracking-g1_29dof_anneal_23dof/exported/model_176500.onnx",
+  
+}
+
+
+
+start_upper_body_dof_pos: {
+
+  "APT_level1":
+  [0.19964170455932617, 0.07710712403059006, -0.2882401943206787, 
+  0.21672365069389343, 0.15629297494888306, -0.5167576670646667, 0.5782126784324646,
+  0.0, 0.0, 0.0, 
+  0.25740593671798706, -0.2504104673862457, 0.22500675916671753, 0.5127624273300171,
+  0.0, 0.0, 0.0],
+
+}
+
+motion_length_s: {
+  "APT_level1": 7.66,
+
+}

GR00T-WholeBodyControl/decoupled_wbc/control/main/teleop/configs/g1_gear_wbc.yaml

@@ -0,0 +1,45 @@
+
+# Simulation parameters
+simulation_duration: 60.0
+simulation_dt: 0.002
+control_decimation: 10
+
+# PD gains
+kps: [
+    150, 150, 150, 300, 40, 40,
+    150, 150, 150, 300, 40, 40, 
+    250, 250, 250,
+    100, 100, 40, 40, 20, 20, 20,
+    100, 100, 40, 40, 20, 20, 20
+]
+kds: [
+    2, 2, 2, 4, 2, 2,
+    2, 2, 2, 4, 2, 2,
+    5, 5, 5,
+    5, 5, 2, 2, 2, 2, 2,
+    5, 5, 2, 2, 2, 2, 2
+]
+
+# Default joint angles for legs
+default_angles: [-0.1, 0.0, 0.0, 0.3, -0.2, 0.0,
+                 -0.1, 0.0, 0.0, 0.3, -0.2, 0.0,
+                 0.0, 0.0, 0.0]
+# Scaling factors
+ang_vel_scale: 0.25
+dof_pos_scale: 1.0
+dof_vel_scale: 0.05
+action_scale: 0.25
+cmd_scale: [2.0, 2.0, 0.25]
+
+# Number of actions and observations
+num_actions: 15
+num_obs: 570  # 76 * 6 (observation dimension * history length)
+obs_history_len: 6
+
+# Initial commands
+cmd_init: [0.0, 0.0, 0.0]
+height_cmd: 0.74
+freq_cmd: 1.50
+roll_cmd: 0.0
+pitch_cmd: 0.0
+yaw_cmd: 0.0

GR00T-WholeBodyControl/decoupled_wbc/control/main/teleop/configs/identifiers.py

@@ -0,0 +1,14 @@
+"""
+Constants required during data collection, such as operator usernames and robot IDs.
+Additional constants needed for data collection can be added to this file.
+"""
+
+OPERATOR_USERNAMES = [
+    "NEW_USER",
+]
+
+G1_ROBOT_IDS = [
+    "sim",
+    "0001",
+    "0002",
+]

GR00T-WholeBodyControl/decoupled_wbc/control/main/teleop/playback_sync_sim_data.py

@@ -0,0 +1,627 @@
+"""
+A convenience script to playback random demonstrations using the decoupled_wbc controller from
+a set of demonstrations stored in a hdf5 file.
+
+Arguments:
+    --dataset (str): Path to demonstrations
+    --use-actions (optional): If this flag is provided, the actions are played back
+        through the MuJoCo simulator, instead of loading the simulator states
+        one by one.
+    --use-wbc-goals (optional): If set, will use the stored WBC goals to control the robot,
+        otherwise will use the actions directly. Only relevant if --use-actions is set.
+    --use-teleop-cmd (optional): If set, will use teleop IK directly with WBC timing
+        for action generation. Only relevant if --use-actions is set.
+    --visualize-gripper (optional): If set, will visualize the gripper site
+    --save-video (optional): If set, will save video of the playback using offscreen rendering
+    --video-path (optional): Path to save the output video. If not specified, will use the nearest
+        folder to dataset and save as playback_video.mp4
+    --num-episodes (optional): Number of episodes to playback/record (if None, plays random episodes)
+
+Example:
+    $ python decoupled_wbc/control/main/teleop/playback_sync_sim_data.py --dataset output/robocasa_datasets/
+        --use-actions --use-wbc-goals
+
+    $ python decoupled_wbc/control/main/teleop/playback_sync_sim_data.py --dataset output/robocasa_datasets/
+        --use-actions --use-teleop-cmd
+
+    # Record video of the first 5 episodes using WBC goals
+    $ python decoupled_wbc/control/main/teleop/playback_sync_sim_data.py --dataset output/robocasa_datasets/
+        --use-actions --use-wbc-goals --save-video --num-episodes 5
+"""
+
+import json
+import os
+from pathlib import Path
+import time
+from typing import Optional
+
+import cv2
+import numpy as np
+import rclpy
+from robosuite.environments.robot_env import RobotEnv
+from tqdm import tqdm
+import tyro
+
+from decoupled_wbc.control.main.teleop.configs.configs import SyncSimPlaybackConfig
+from decoupled_wbc.control.robot_model.instantiation import get_robot_type_and_model
+from decoupled_wbc.control.utils.sync_sim_utils import (
+    generate_frame,
+    get_data_exporter,
+    get_env,
+    get_policies,
+)
+from decoupled_wbc.data.constants import RS_VIEW_CAMERA_HEIGHT, RS_VIEW_CAMERA_WIDTH
+from decoupled_wbc.data.exporter import TypedLeRobotDataset
+
+CONTROL_NODE_NAME = "playback_node"
+GREEN_BOLD = "\033[1;32m"
+RED_BOLD = "\033[1;31m"
+RESET = "\033[0m"
+
+
+def load_lerobot_dataset(root_path, max_episodes=None):
+    task_name = None
+    episodes = []
+    start_index = 0
+    with open(Path(root_path) / "meta/episodes.jsonl", "r") as f:
+        for line in f:
+            episode = json.loads(line)
+            episode["start_index"] = start_index
+            start_index += episode["length"]
+            assert (
+                task_name is None or task_name == episode["tasks"][0]
+            ), "All episodes should have the same task name"
+            task_name = episode["tasks"][0]
+            episodes.append(episode)
+
+    dataset = TypedLeRobotDataset(
+        repo_id="tmp/test",
+        root=root_path,
+        load_video=False,
+    )
+
+    script_config = dataset.meta.info["script_config"]
+
+    assert len(dataset) == start_index, "Dataset length does not match expected length"
+
+    # Limit episodes if specified
+    if max_episodes is not None:
+        episodes = episodes[:max_episodes]
+        print(
+            f"Loading only first {len(episodes)} episodes (limited by max_episodes={max_episodes})"
+        )
+
+    f = {}
+    seeds = []
+    for ep in tqdm(range(len(episodes))):
+        seed = None
+        f[f"data/demo_{ep + 1}/states"] = []
+        f[f"data/demo_{ep + 1}/actions"] = []
+        f[f"data/demo_{ep + 1}/teleop_cmd"] = []
+        f[f"data/demo_{ep + 1}/wbc_goal"] = []
+        start_index = episodes[ep]["start_index"]
+        end_index = start_index + episodes[ep]["length"]
+        for i in tqdm(range(start_index, end_index)):
+            frame = dataset[i]
+            # load the seed
+            assert (
+                seed is None or seed == np.array(frame["observation.sim.seed"]).item()
+            ), "All observations in an episode should have the same seed"
+            seed = np.array(frame["observation.sim.seed"]).item()
+            # load the state
+            mujoco_state_len = frame["observation.sim.mujoco_state_len"]
+            mujoco_state = frame["observation.sim.mujoco_state"]
+            f[f"data/demo_{ep + 1}/states"].append(np.array(mujoco_state[:mujoco_state_len]))
+            # load the action
+            action = frame["action"]
+            f[f"data/demo_{ep + 1}/actions"].append(np.array(action))
+
+            # load the teleop command
+            teleop_cmd = {
+                "left_wrist": np.array(frame["observation.sim.left_wrist"].reshape(4, 4)),
+                "right_wrist": np.array(frame["observation.sim.right_wrist"].reshape(4, 4)),
+                "left_fingers": {
+                    "position": np.array(frame["observation.sim.left_fingers"].reshape(25, 4, 4)),
+                },
+                "right_fingers": {
+                    "position": np.array(frame["observation.sim.right_fingers"].reshape(25, 4, 4)),
+                },
+                "target_upper_body_pose": np.array(frame["observation.sim.target_upper_body_pose"]),
+                "base_height_command": np.array(frame["teleop.base_height_command"]),
+                "navigate_cmd": np.array(frame["teleop.navigate_command"]),
+            }
+            f[f"data/demo_{ep + 1}/teleop_cmd"].append(teleop_cmd)
+            # load the WBC goal
+            wbc_goal = {
+                "wrist_pose": np.array(frame["action.eef"]),
+                "target_upper_body_pose": np.array(frame["observation.sim.target_upper_body_pose"]),
+                "navigate_cmd": np.array(frame["teleop.navigate_command"]),
+                "base_height_command": np.array(frame["teleop.base_height_command"]),
+            }
+            f[f"data/demo_{ep + 1}/wbc_goal"].append(wbc_goal)
+
+        seeds.append(seed)
+
+    return seeds, f, script_config
+
+
+def validate_state(recorded_state, playback_state, ep, step, tolerance=1e-5):
+    """Validate that playback state matches recorded state within tolerance."""
+    if not np.allclose(recorded_state, playback_state, atol=tolerance):
+        err = np.linalg.norm(recorded_state - playback_state)
+        print(f"[warning] state diverged by {err:.12f} for ep {ep} at step {step}")
+        return False
+    return True
+
+
+def generate_and_save_frame(
+    config, sync_env, obs, wbc_action, seed, teleop_cmd, wbc_goal, gr00t_exporter
+):
+    """Generate and save a frame to LeRobot dataset if enabled."""
+    if config.save_lerobot:
+        max_mujoco_state_len, mujoco_state_len, mujoco_state = sync_env.get_mujoco_state_info()
+        frame = generate_frame(
+            obs,
+            wbc_action,
+            seed,
+            mujoco_state,
+            mujoco_state_len,
+            max_mujoco_state_len,
+            teleop_cmd,
+            wbc_goal,
+            config.save_img_obs,
+        )
+        gr00t_exporter.add_frame(frame)
+
+
+def playback_wbc_goals(
+    sync_env,
+    wbc_policy,
+    wbc_goals,
+    teleop_cmds,
+    states,
+    env,
+    onscreen,
+    config,
+    video_writer,
+    ep,
+    seed,
+    gr00t_exporter,
+    end_steps,
+):
+    """Playback using WBC goals to control the robot."""
+    ret = True
+    num_wbc_goals = len(wbc_goals) if end_steps == -1 else min(end_steps, len(wbc_goals))
+
+    for jj in range(num_wbc_goals):
+        wbc_goal = wbc_goals[jj]
+        obs = sync_env.observe()
+        wbc_policy.set_observation(obs)
+        wbc_policy.set_goal(wbc_goal)
+        wbc_action = wbc_policy.get_action()
+        sync_env.queue_action(wbc_action)
+
+        # Save frame if needed
+        if config.save_lerobot:
+            teleop_cmd = teleop_cmds[jj]
+            generate_and_save_frame(
+                config, sync_env, obs, wbc_action, seed, teleop_cmd, wbc_goal, gr00t_exporter
+            )
+
+        capture_or_render_frame(env, onscreen, config, video_writer)
+
+        if jj < len(states) - 1:
+            state_playback = env.sim.get_state().flatten()
+            if not validate_state(states[jj + 1], state_playback, ep, jj):
+                ret = False
+
+    return ret
+
+
+def playback_teleop_cmd(
+    sync_env,
+    wbc_policy,
+    teleop_policy,
+    wbc_goals,
+    teleop_cmds,
+    states,
+    env,
+    onscreen,
+    config,
+    video_writer,
+    ep,
+    seed,
+    gr00t_exporter,
+    end_steps,
+):
+    """Playback using teleop commands to control the robot."""
+    ret = True
+    num_steps = len(wbc_goals) if end_steps == -1 else min(end_steps, len(wbc_goals))
+
+    for jj in range(num_steps):
+        wbc_goal = wbc_goals[jj]
+        teleop_cmd = teleop_cmds[jj]
+
+        # Set IK goal from teleop command
+        ik_data = {
+            "body_data": {
+                teleop_policy.retargeting_ik.body.supplemental_info.hand_frame_names[
+                    "left"
+                ]: teleop_cmd["left_wrist"],
+                teleop_policy.retargeting_ik.body.supplemental_info.hand_frame_names[
+                    "right"
+                ]: teleop_cmd["right_wrist"],
+            },
+            "left_hand_data": teleop_cmd["left_fingers"],
+            "right_hand_data": teleop_cmd["right_fingers"],
+        }
+        teleop_policy.retargeting_ik.set_goal(ik_data)
+
+        # Store original and get new upper body pose
+        target_upper_body_pose = wbc_goal["target_upper_body_pose"].copy()
+        wbc_goal["target_upper_body_pose"] = teleop_policy.retargeting_ik.get_action()
+
+        # Execute WBC policy
+        obs = sync_env.observe()
+        wbc_policy.set_observation(obs)
+        wbc_policy.set_goal(wbc_goal)
+        wbc_action = wbc_policy.get_action()
+        sync_env.queue_action(wbc_action)
+
+        # Save frame if needed
+        generate_and_save_frame(
+            config, sync_env, obs, wbc_action, seed, teleop_cmd, wbc_goal, gr00t_exporter
+        )
+
+        # Render or capture frame
+        capture_or_render_frame(env, onscreen, config, video_writer)
+
+        # Validate states
+        if jj < len(states) - 1:
+            if not np.allclose(
+                target_upper_body_pose, wbc_goal["target_upper_body_pose"], atol=1e-5
+            ):
+                err = np.linalg.norm(target_upper_body_pose - wbc_goal["target_upper_body_pose"])
+                print(
+                    f"[warning] target_upper_body_pose diverged by {err:.12f} for ep {ep} at step {jj}"
+                )
+                ret = False
+
+            state_playback = env.sim.get_state().flatten()
+            if not validate_state(states[jj + 1], state_playback, ep, jj):
+                ret = False
+
+    return ret
+
+
+def playback_actions(
+    sync_env,
+    actions,
+    teleop_cmds,
+    wbc_goals,
+    states,
+    env,
+    onscreen,
+    config,
+    video_writer,
+    ep,
+    seed,
+    gr00t_exporter,
+    end_steps,
+):
+    """Playback using actions directly."""
+    ret = True
+    num_actions = len(actions) if end_steps == -1 else min(end_steps, len(actions))
+
+    for j in range(num_actions):
+        sync_env.queue_action({"q": actions[j]})
+
+        # Save frame if needed
+        if config.save_lerobot:
+            obs = sync_env.observe()
+            teleop_cmd = teleop_cmds[j]
+            wbc_goal = wbc_goals[j]
+            wbc_action = {"q": actions[j]}
+            generate_and_save_frame(
+                config, sync_env, obs, wbc_action, seed, teleop_cmd, wbc_goal, gr00t_exporter
+            )
+
+        capture_or_render_frame(env, onscreen, config, video_writer)
+
+        if j < len(states) - 1:
+            state_playback = env.sim.get_state().flatten()
+            if not validate_state(states[j + 1], state_playback, ep, j):
+                ret = False
+
+    return ret
+
+
+def playback_states(
+    sync_env,
+    states,
+    actions,
+    teleop_cmds,
+    wbc_goals,
+    env,
+    onscreen,
+    config,
+    video_writer,
+    seed,
+    gr00t_exporter,
+    end_steps,
+    ep,
+):
+    """Playback by forcing mujoco states directly."""
+    ret = True
+    num_states = len(states) if end_steps == -1 else min(end_steps, len(states))
+
+    for i in range(num_states):
+        sync_env.reset_to({"states": states[i]})
+        sync_env.render()
+
+        # Validate that the state was set correctly
+        if i < len(states):
+            state_playback = env.sim.get_state().flatten()
+            if not validate_state(states[i], state_playback, ep, i):
+                ret = False
+
+        # Save frame if needed
+        if config.save_lerobot:
+            obs = sync_env.observe()
+            teleop_cmd = teleop_cmds[i]
+            wbc_goal = wbc_goals[i]
+            wbc_action = {"q": actions[i]}
+            generate_and_save_frame(
+                config, sync_env, obs, wbc_action, seed, teleop_cmd, wbc_goal, gr00t_exporter
+            )
+
+        capture_or_render_frame(env, onscreen, config, video_writer)
+
+    return ret
+
+
+def main(config: SyncSimPlaybackConfig):
+    ret = True
+    start_time = time.time()
+
+    np.set_printoptions(precision=5, suppress=True, linewidth=120)
+
+    assert config.dataset is not None, "Folder must be specified for playback"
+
+    seeds, f, script_config = load_lerobot_dataset(config.dataset)
+
+    config.update(
+        script_config,
+        allowed_keys=[
+            "wbc_version",
+            "wbc_model_path",
+            "wbc_policy_class",
+            "control_frequency",
+            "enable_waist",
+            "with_hands",
+            "env_name",
+            "robot",
+            "task_name",
+            "teleop_frequency",
+            "data_collection_frequency",
+            "enable_gravity_compensation",
+            "gravity_compensation_joints",
+        ],
+    )
+    config.validate_args()
+
+    robot_type, robot_model = get_robot_type_and_model(config.robot, config.enable_waist)
+
+    # Setup rendering
+    if config.save_video or config.save_img_obs:
+        onscreen = False
+        offscreen = True
+    else:
+        onscreen = True
+        offscreen = False
+
+    # Set default video path if not specified
+    if config.save_video and config.video_path is None:
+        if os.path.isfile(config.dataset):
+            video_folder = Path(config.dataset).parent
+        else:
+            video_folder = Path(config.dataset)
+        video_folder.mkdir(parents=True, exist_ok=True)
+        config.video_path = str(video_folder / "playback_video.mp4")
+        print(f"Video recording enabled. Output: {config.video_path}")
+
+    sync_env = get_env(config, onscreen=onscreen, offscreen=offscreen)
+
+    gr00t_exporter = None
+    if config.save_lerobot:
+        obs = sync_env.observe()
+        gr00t_exporter = get_data_exporter(config, obs, robot_model)
+
+    # Initialize policies
+    wbc_policy, teleop_policy = get_policies(
+        config, robot_type, robot_model, activate_keyboard_listener=False
+    )
+
+    # List of all demonstrations episodes
+    demos = [f"demo_{i + 1}" for i in range(len(seeds))]
+    print(f"Loaded and will playback {len(demos)} episodes")
+    env = sync_env.base_env
+
+    # Setup video writer
+    video_writer = None
+    fourcc = None
+    if config.save_video:
+        fourcc = cv2.VideoWriter_fourcc(*"mp4v")
+        video_writer = cv2.VideoWriter(
+            config.video_path, fourcc, 20, (RS_VIEW_CAMERA_WIDTH, RS_VIEW_CAMERA_HEIGHT)
+        )
+
+    print("Loaded {} episodes from {}".format(len(demos), config.dataset))
+    print("seeds:", seeds)
+    print("demos:", demos, "\n\n")
+
+    # Handle episode selection - either limited number or infinite random
+    max_episodes = len(demos)
+    episode_count = 0
+    while True:
+        if episode_count >= max_episodes:
+            break
+        ep = demos[episode_count]
+        print(f"Playing back episode: {ep}")
+        episode_count += 1
+
+        # read the model xml, using the metadata stored in the attribute for this episode
+        seed = seeds[int(ep.split("_")[-1]) - 1]
+        sync_env.reset(seed=seed)
+
+        # load the actions and states
+        states = f["data/{}/states".format(ep)]
+        actions = f["data/{}/actions".format(ep)]
+        teleop_cmds = f["data/{}/teleop_cmd".format(ep)]
+        wbc_goals = f["data/{}/wbc_goal".format(ep)]
+
+        # reset the policies
+        wbc_policy, teleop_policy, _ = get_policies(
+            config, robot_type, robot_model, activate_keyboard_listener=False
+        )
+        end_steps = 20 if config.ci_test else -1
+
+        if config.use_actions:
+            # load the initial state
+            sync_env.reset_to({"states": states[0]})
+            # load the actions and play them back open-loop
+            if config.use_wbc_goals:
+                # use the wbc_goals to control the robot
+                episode_ret = playback_wbc_goals(
+                    sync_env,
+                    wbc_policy,
+                    wbc_goals,
+                    teleop_cmds,
+                    states,
+                    env,
+                    onscreen,
+                    config,
+                    video_writer,
+                    ep,
+                    seed,
+                    gr00t_exporter,
+                    end_steps,
+                )
+                ret = ret and episode_ret
+            elif config.use_teleop_cmd:
+                # use the teleop commands to control the robot
+                episode_ret = playback_teleop_cmd(
+                    sync_env,
+                    wbc_policy,
+                    teleop_policy,
+                    wbc_goals,
+                    teleop_cmds,
+                    states,
+                    env,
+                    onscreen,
+                    config,
+                    video_writer,
+                    ep,
+                    seed,
+                    gr00t_exporter,
+                    end_steps,
+                )
+                ret = ret and episode_ret
+            else:
+                episode_ret = playback_actions(
+                    sync_env,
+                    actions,
+                    teleop_cmds,
+                    wbc_goals,
+                    states,
+                    env,
+                    onscreen,
+                    config,
+                    video_writer,
+                    ep,
+                    seed,
+                    gr00t_exporter,
+                    end_steps,
+                )
+                ret = ret and episode_ret
+        else:
+            # force the sequence of internal mujoco states one by one
+            episode_ret = playback_states(
+                sync_env,
+                states,
+                actions,
+                teleop_cmds,
+                wbc_goals,
+                env,
+                onscreen,
+                config,
+                video_writer,
+                seed,
+                gr00t_exporter,
+                end_steps,
+                ep,
+            )
+            ret = ret and episode_ret
+
+        if config.save_lerobot:
+            gr00t_exporter.save_episode()
+
+        print(f"Episode {ep} playback finished.\n\n")
+
+    # close the env
+    sync_env.close()
+
+    # Cleanup
+    if video_writer is not None:
+        video_writer.release()
+        print(f"Video saved to: {config.video_path}")
+
+    end_time = time.time()
+    elapsed_time = end_time - start_time
+
+    if config.save_lerobot:
+        print(f"LeRobot dataset saved to: {gr00t_exporter.root}")
+
+    print(
+        f"{GREEN_BOLD}Playback with WBC version: {config.wbc_version}, {config.wbc_model_path}, "
+        f"{config.wbc_policy_class}, use_actions: {config.use_actions}, use_wbc_goals: {config.use_wbc_goals}, "
+        f"use_teleop_cmd: {config.use_teleop_cmd}{RESET}"
+    )
+    if ret:
+        print(f"{GREEN_BOLD}Playback completed successfully in {elapsed_time:.2f} seconds!{RESET}")
+    else:
+        print(f"{RED_BOLD}Playback encountered an error in {elapsed_time:.2f} seconds!{RESET}")
+
+    return ret
+
+
+def capture_or_render_frame(
+    env: RobotEnv,
+    onscreen: bool,
+    config: SyncSimPlaybackConfig,
+    video_writer: Optional[cv2.VideoWriter],
+):
+    """Capture frame for video recording if enabled, or render the environment."""
+    if config.save_video:
+        if hasattr(env, "sim") and hasattr(env.sim, "render"):
+            img = env.sim.render(
+                width=RS_VIEW_CAMERA_WIDTH,
+                height=RS_VIEW_CAMERA_HEIGHT,
+                camera_name=env.render_camera[0],
+            )
+            img_bgr = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
+            img_bgr = np.flipud(img_bgr)
+            video_writer.write(img_bgr)
+    elif onscreen:
+        env.render()
+
+
+if __name__ == "__main__":
+    config = tyro.cli(SyncSimPlaybackConfig)
+
+    rclpy.init(args=None)
+    node = rclpy.create_node("playback_decoupled_wbc_control")
+
+    main(config)
+
+    rclpy.shutdown()

GR00T-WholeBodyControl/decoupled_wbc/control/main/teleop/run_camera_viewer.py

@@ -0,0 +1,249 @@
+"""
+Camera viewer with manual recording support.
+
+This script provides a camera viewer that can display multiple camera streams
+and record them to video files with manual start/stop controls.
+
+Features:
+- Onscreen mode: Display camera feeds with optional recording
+- Offscreen mode: No display, recording only when triggered
+- Manual recording control with keyboard (R key to start/stop)
+
+Usage Examples:
+
+1. Basic onscreen viewing (with recording capability):
+   python run_camera_viewer.py --camera-host localhost --camera-port 5555
+
+2. Offscreen mode (no display, recording only):
+   python run_camera_viewer.py --offscreen --camera-host localhost --camera-port 5555
+
+3. Custom output directory:
+   python run_camera_viewer.py --output-path ./my_recordings --camera-host localhost
+
+Controls:
+- R key: Start/Stop recording
+- Q key: Quit application
+
+Output Structure:
+camera_output_20241211_143052/
+├── rec_143205/
+│   ├── ego_view_color_image.mp4
+│   ├── head_left_color_image.mp4
+│   └── head_right_color_image.mp4
+└── rec_143410/
+    ├── ego_view_color_image.mp4
+    └── head_left_color_image.mp4
+"""
+
+from dataclasses import dataclass
+from pathlib import Path
+import threading
+import time
+from typing import Any, Optional
+
+import cv2
+import rclpy
+from sshkeyboard import listen_keyboard, stop_listening
+import tyro
+
+from decoupled_wbc.control.main.teleop.configs.configs import ComposedCameraClientConfig
+from decoupled_wbc.control.sensor.composed_camera import ComposedCameraClientSensor
+from decoupled_wbc.control.utils.img_viewer import ImageViewer
+
+
+@dataclass
+class CameraViewerConfig(ComposedCameraClientConfig):
+    """Config for running the camera viewer with recording support."""
+
+    offscreen: bool = False
+    """Run in offscreen mode (no display, manual recording with R key)."""
+
+    output_path: Optional[str] = None
+    """Output path for saving videos. If None, auto-generates path."""
+
+    codec: str = "mp4v"
+    """Video codec to use for saving (e.g., 'mp4v', 'XVID')."""
+
+
+ArgsConfig = CameraViewerConfig
+
+
+def _get_camera_titles(image_data: dict[str, Any]) -> list[str]:
+    """
+    Detect all the individual camera streams from the image data.
+
+    schema format:
+    {
+        "timestamps": {"ego_view": 123.45, "ego_view_left_mono": 123.46},
+        "images": {"ego_view": np.ndarray, "ego_view_left_mono": np.ndarray}
+    }
+
+    Returns list of camera keys (e.g., ["ego_view", "ego_view_left_mono", "ego_view_right_mono"])
+    """
+    # Extract all camera keys from the images dictionary
+    camera_titles = list(image_data.get("images", {}).keys())
+    return camera_titles
+
+
+def main(config: ArgsConfig):
+    """Main function to run the camera viewer."""
+    # Initialize ROS
+    rclpy.init(args=None)
+    node = rclpy.create_node("camera_viewer")
+
+    # Start ROS spin in a separate thread
+    thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True)
+    thread.start()
+
+    image_sub = ComposedCameraClientSensor(server_ip=config.camera_host, port=config.camera_port)
+
+    # pre-fetch a sample image to get the number of camera angles
+    retry_count = 0
+    while True:
+        _sample_image = image_sub.read()
+        if _sample_image:
+            break
+        retry_count += 1
+        time.sleep(0.1)
+        if retry_count > 10:
+            raise Exception("Failed to get sample image")
+
+    camera_titles = _get_camera_titles(_sample_image)
+
+    # Setup output directory
+    if config.output_path is None:
+        output_dir = Path("camera_recordings")
+    else:
+        output_dir = Path(config.output_path)
+
+    # Recording state
+    is_recording = False
+    video_writers = {}
+    frame_count = 0
+    recording_start_time = None
+    should_quit = False
+
+    def on_press(key):
+        nonlocal is_recording, video_writers, frame_count, recording_start_time, should_quit
+
+        if key == "r":
+            if not is_recording:
+                # Start recording
+                recording_dir = output_dir / f"rec_{time.strftime('%Y%m%d_%H%M%S')}"
+                recording_dir.mkdir(parents=True, exist_ok=True)
+
+                # Create video writers
+                fourcc = cv2.VideoWriter_fourcc(*config.codec)
+                video_writers = {}
+
+                for title in camera_titles:
+                    img = _sample_image["images"].get(title)
+                    if img is not None:
+                        height, width = img.shape[:2]
+                        video_path = recording_dir / f"{title}.mp4"
+                        writer = cv2.VideoWriter(
+                            str(video_path), fourcc, config.fps, (width, height)
+                        )
+                        video_writers[title] = writer
+
+                is_recording = True
+                recording_start_time = time.time()
+                frame_count = 0
+                print(f"🔴 Recording started: {recording_dir}")
+            else:
+                # Stop recording
+                is_recording = False
+                for title, writer in video_writers.items():
+                    writer.release()
+                video_writers = {}
+
+                duration = time.time() - recording_start_time if recording_start_time else 0
+                print(f"⏹️  Recording stopped - {duration:.1f}s, {frame_count} frames")
+        elif key == "q":
+            should_quit = True
+            stop_listening()
+
+    # Setup keyboard listener in a separate thread
+    keyboard_thread = threading.Thread(
+        target=lambda: listen_keyboard(on_press=on_press), daemon=True
+    )
+    keyboard_thread.start()
+
+    # Setup viewer for onscreen mode
+    viewer = None
+    if not config.offscreen:
+        viewer = ImageViewer(
+            title="Camera Viewer",
+            figsize=(10, 8),
+            num_images=len(camera_titles),
+            image_titles=camera_titles,
+        )
+
+    # Print instructions
+    mode = "Offscreen" if config.offscreen else "Onscreen"
+    print(f"{mode} mode - Target FPS: {config.fps}")
+    print(f"Videos will be saved to: {output_dir}")
+    print("Controls: R key to start/stop recording, Q key to quit, Ctrl+C to exit")
+
+    # Create ROS rate controller
+    rate = node.create_rate(config.fps)
+
+    try:
+        while rclpy.ok() and not should_quit:
+            # Get images from all subscribers
+            images = []
+            image_data = image_sub.read()
+            if image_data:
+                for title in camera_titles:
+                    img = image_data["images"].get(title)
+                    images.append(img)
+
+                    # Save frame if recording
+                    if is_recording and img is not None and title in video_writers:
+                        # Convert from RGB to BGR for OpenCV
+                        if len(img.shape) == 3 and img.shape[2] == 3:
+                            img_bgr = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
+                        else:
+                            img_bgr = img
+                        video_writers[title].write(img_bgr)
+
+            # Display images if not offscreen
+            if not config.offscreen and viewer and any(img is not None for img in images):
+                status = "🔴 REC" if is_recording else "⏸️ Ready"
+                viewer._fig.suptitle(f"Camera Viewer - {status}")
+                viewer.show_multiple(images)
+
+            # Progress feedback
+            if is_recording:
+                frame_count += 1
+                if frame_count % 100 == 0:
+                    duration = time.time() - recording_start_time
+                    print(f"Recording: {frame_count} frames ({duration:.1f}s)")
+
+            rate.sleep()
+
+    except KeyboardInterrupt:
+        print("\nExiting...")
+    finally:
+        # Cleanup
+        try:
+            stop_listening()
+        except Exception:
+            pass
+
+        if video_writers:
+            for title, writer in video_writers.items():
+                writer.release()
+            if is_recording:
+                duration = time.time() - recording_start_time
+                print(f"Final: {duration:.1f}s, {frame_count} frames")
+
+        if viewer:
+            viewer.close()
+
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    config = tyro.cli(ArgsConfig)
+    main(config)

GR00T-WholeBodyControl/decoupled_wbc/control/main/teleop/run_g1_control_loop.py

@@ -0,0 +1,236 @@
+from copy import deepcopy
+import time
+
+import tyro
+
+from decoupled_wbc.control.envs.g1.g1_env import G1Env
+from decoupled_wbc.control.main.constants import (
+    CONTROL_GOAL_TOPIC,
+    DEFAULT_BASE_HEIGHT,
+    DEFAULT_NAV_CMD,
+    DEFAULT_WRIST_POSE,
+    JOINT_SAFETY_STATUS_TOPIC,
+    LOWER_BODY_POLICY_STATUS_TOPIC,
+    ROBOT_CONFIG_TOPIC,
+    STATE_TOPIC_NAME,
+)
+from decoupled_wbc.control.main.teleop.configs.configs import ControlLoopConfig
+from decoupled_wbc.control.policy.wbc_policy_factory import get_wbc_policy
+from decoupled_wbc.control.robot_model.instantiation.g1 import (
+    instantiate_g1_robot_model,
+)
+from decoupled_wbc.control.utils.keyboard_dispatcher import (
+    KeyboardDispatcher,
+    KeyboardEStop,
+    KeyboardListenerPublisher,
+    ROSKeyboardDispatcher,
+)
+from decoupled_wbc.control.utils.ros_utils import (
+    ROSManager,
+    ROSMsgPublisher,
+    ROSMsgSubscriber,
+    ROSServiceServer,
+)
+from decoupled_wbc.control.utils.telemetry import Telemetry
+
+CONTROL_NODE_NAME = "ControlPolicy"
+
+
+def main(config: ControlLoopConfig):
+    ros_manager = ROSManager(node_name=CONTROL_NODE_NAME)
+    node = ros_manager.node
+
+    # start the robot config server
+    ROSServiceServer(ROBOT_CONFIG_TOPIC, config.to_dict())
+
+    wbc_config = config.load_wbc_yaml()
+
+    data_exp_pub = ROSMsgPublisher(STATE_TOPIC_NAME)
+    lower_body_policy_status_pub = ROSMsgPublisher(LOWER_BODY_POLICY_STATUS_TOPIC)
+    joint_safety_status_pub = ROSMsgPublisher(JOINT_SAFETY_STATUS_TOPIC)
+
+    # Initialize telemetry
+    telemetry = Telemetry(window_size=100)
+
+    waist_location = "lower_and_upper_body" if config.enable_waist else "lower_body"
+    robot_model = instantiate_g1_robot_model(
+        waist_location=waist_location, high_elbow_pose=config.high_elbow_pose
+    )
+
+    env = G1Env(
+        env_name=config.env_name,
+        robot_model=robot_model,
+        config=wbc_config,
+        wbc_version=config.wbc_version,
+    )
+    if env.sim and not config.sim_sync_mode:
+        env.start_simulator()
+
+    wbc_policy = get_wbc_policy("g1", robot_model, wbc_config, config.upper_body_joint_speed)
+
+    keyboard_listener_pub = KeyboardListenerPublisher()
+    keyboard_estop = KeyboardEStop()
+    if config.keyboard_dispatcher_type == "raw":
+        dispatcher = KeyboardDispatcher()
+    elif config.keyboard_dispatcher_type == "ros":
+        dispatcher = ROSKeyboardDispatcher()
+    else:
+        raise ValueError(
+            f"Invalid keyboard dispatcher: {config.keyboard_dispatcher_type}, please use 'raw' or 'ros'"
+        )
+    dispatcher.register(env)
+    dispatcher.register(wbc_policy)
+    dispatcher.register(keyboard_listener_pub)
+    dispatcher.register(keyboard_estop)
+    dispatcher.start()
+
+    rate = node.create_rate(config.control_frequency)
+
+    upper_body_policy_subscriber = ROSMsgSubscriber(CONTROL_GOAL_TOPIC)
+
+    last_teleop_cmd = None
+    try:
+        while ros_manager.ok():
+            t_start = time.monotonic()
+            with telemetry.timer("total_loop"):
+                # Step simulator if in sync mode
+                with telemetry.timer("step_simulator"):
+                    if env.sim and config.sim_sync_mode:
+                        env.step_simulator()
+
+                # Measure observation time
+                with telemetry.timer("observe"):
+                    obs = env.observe()
+                    wbc_policy.set_observation(obs)
+
+                # Measure policy setup time
+                with telemetry.timer("policy_setup"):
+                    upper_body_cmd = upper_body_policy_subscriber.get_msg()
+
+                    t_now = time.monotonic()
+
+                    wbc_goal = {}
+                    if upper_body_cmd:
+                        wbc_goal = upper_body_cmd.copy()
+                        last_teleop_cmd = upper_body_cmd.copy()
+                        if config.ik_indicator:
+                            env.set_ik_indicator(upper_body_cmd)
+                    # Send goal to policy
+                    if wbc_goal:
+                        wbc_goal["interpolation_garbage_collection_time"] = t_now - 2 * (
+                            1 / config.control_frequency
+                        )
+                        wbc_policy.set_goal(wbc_goal)
+
+                # Measure policy action calculation time
+                with telemetry.timer("policy_action"):
+                    wbc_action = wbc_policy.get_action(time=t_now)
+
+                # Measure action queue time
+                with telemetry.timer("queue_action"):
+                    env.queue_action(wbc_action)
+
+                # Publish status information for InteractiveModeController
+                with telemetry.timer("publish_status"):
+                    # Get policy status - check if the lower body policy has use_policy_action enabled
+                    policy_use_action = False
+                    try:
+                        # Access the lower body policy through the decoupled whole body policy
+                        if hasattr(wbc_policy, "lower_body_policy"):
+                            policy_use_action = getattr(
+                                wbc_policy.lower_body_policy, "use_policy_action", False
+                            )
+                    except (AttributeError, TypeError):
+                        policy_use_action = False
+
+                    policy_status_msg = {"use_policy_action": policy_use_action, "timestamp": t_now}
+                    lower_body_policy_status_pub.publish(policy_status_msg)
+
+                    # Get joint safety status from G1Env (which already runs the safety monitor)
+                    joint_safety_ok = env.get_joint_safety_status()
+
+                    joint_safety_status_msg = {
+                        "joint_safety_ok": joint_safety_ok,
+                        "timestamp": t_now,
+                    }
+                    joint_safety_status_pub.publish(joint_safety_status_msg)
+
+                # Start or Stop data collection
+                if wbc_goal.get("toggle_data_collection", False):
+                    dispatcher.handle_key("c")
+
+                # Abort the current episode
+                if wbc_goal.get("toggle_data_abort", False):
+                    dispatcher.handle_key("x")
+
+                if env.use_sim and wbc_goal.get("reset_env_and_policy", False):
+                    print("Resetting sim environment and policy")
+                    # Reset teleop policy & sim env
+                    dispatcher.handle_key("k")
+
+                    # Clear upper body commands
+                    upper_body_policy_subscriber._msg = None
+                    upper_body_cmd = {
+                        "target_upper_body_pose": obs["q"][
+                            robot_model.get_joint_group_indices("upper_body")
+                        ],
+                        "wrist_pose": DEFAULT_WRIST_POSE,
+                        "base_height_command": DEFAULT_BASE_HEIGHT,
+                        "navigate_cmd": DEFAULT_NAV_CMD,
+                    }
+                    last_teleop_cmd = upper_body_cmd.copy()
+
+                    time.sleep(0.5)
+
+                msg = deepcopy(obs)
+                for key in obs.keys():
+                    if key.endswith("_image"):
+                        del msg[key]
+
+                # exporting data
+                if last_teleop_cmd:
+                    msg.update(
+                        {
+                            "action": wbc_action["q"],
+                            "action.eef": last_teleop_cmd.get("wrist_pose", DEFAULT_WRIST_POSE),
+                            "base_height_command": last_teleop_cmd.get(
+                                "base_height_command", DEFAULT_BASE_HEIGHT
+                            ),
+                            "navigate_command": last_teleop_cmd.get(
+                                "navigate_cmd", DEFAULT_NAV_CMD
+                            ),
+                            "timestamps": {
+                                "main_loop": time.time(),
+                                "proprio": time.time(),
+                            },
+                        }
+                    )
+                data_exp_pub.publish(msg)
+                end_time = time.monotonic()
+
+            if env.sim and (not env.sim.sim_thread or not env.sim.sim_thread.is_alive()):
+                raise RuntimeError("Simulator thread is not alive")
+
+            rate.sleep()
+
+            # Log timing information every 100 iterations (roughly every 2 seconds at 50Hz)
+            if config.verbose_timing:
+                # When verbose timing is enabled, always show timing
+                telemetry.log_timing_info(context="G1 Control Loop", threshold=0.0)
+            elif (end_time - t_start) > (1 / config.control_frequency) and not config.sim_sync_mode:
+                # Only show timing when loop is slow and verbose_timing is disabled
+                telemetry.log_timing_info(context="G1 Control Loop Missed", threshold=0.001)
+
+    except ros_manager.exceptions() as e:
+        print(f"ROSManager interrupted by user: {e}")
+    finally:
+        print("Cleaning up...")
+        # the order of the following is important
+        dispatcher.stop()
+        ros_manager.shutdown()
+        env.close()
+
+
+if __name__ == "__main__":
+    config = tyro.cli(ControlLoopConfig)
+    main(config)

GR00T-WholeBodyControl/decoupled_wbc/control/main/teleop/run_g1_data_exporter.py

@@ -0,0 +1,364 @@
+from collections import deque
+from datetime import datetime
+import threading
+import time
+
+import numpy as np
+import rclpy
+import tyro
+
+from decoupled_wbc.control.main.constants import ROBOT_CONFIG_TOPIC, STATE_TOPIC_NAME
+from decoupled_wbc.control.main.teleop.configs.configs import DataExporterConfig
+from decoupled_wbc.control.robot_model.instantiation import g1
+from decoupled_wbc.control.sensor.composed_camera import ComposedCameraClientSensor
+from decoupled_wbc.control.utils.episode_state import EpisodeState
+from decoupled_wbc.control.utils.keyboard_dispatcher import KeyboardListenerSubscriber
+from decoupled_wbc.control.utils.ros_utils import ROSMsgSubscriber, ROSServiceClient
+from decoupled_wbc.control.utils.telemetry import Telemetry
+from decoupled_wbc.control.utils.text_to_speech import TextToSpeech
+from decoupled_wbc.data.constants import BUCKET_BASE_PATH
+from decoupled_wbc.data.exporter import DataCollectionInfo, Gr00tDataExporter
+from decoupled_wbc.data.utils import get_dataset_features, get_modality_config
+
+
+class TimeDeltaException(Exception):
+    def __init__(self, failure_count: int, reset_timeout_sec: float):
+        """
+        Exception raised when the time delta between two messages exceeds
+        a threshold for a consecutive number of times
+        """
+        self.failure_count = failure_count
+        self.reset_timeout_sec = reset_timeout_sec
+        self.message = f"{self.failure_count} failures in {self.reset_timeout_sec} seconds"
+        super().__init__(self.message)
+
+
+class TimingThresholdMonitor:
+    def __init__(self, max_failures=3, reset_timeout_sec=5, time_delta=0.2, raise_exception=False):
+        """
+        Monitor the time diff (between two messages) and optionally raise an exception
+        if there is a consistent violations
+        """
+        self.max_failures = max_failures
+        self.reset_timeout_sec = reset_timeout_sec
+        self.failure_count = 0
+        self.last_failure_time = 0
+        self.time_delta = time_delta
+        self.raise_exception = raise_exception
+
+    def reset(self):
+        self.failure_count = 0
+        self.last_failure_time = 0
+
+    def log_time_delta(self, time_delta_sec: float):
+        time_delta = abs(time_delta_sec)
+        if time_delta > self.time_delta:
+            self.failure_count += 1
+            self.last_failure_time = time.monotonic()
+
+        if self.is_threshold_exceeded():
+            print(
+                f"Time delta exception: {self.failure_count} failures in {self.reset_timeout_sec} seconds"
+                f", time delta: {time_delta}"
+            )
+            if self.raise_exception:
+                raise TimeDeltaException(self.failure_count, self.reset_timeout_sec)
+
+    def is_threshold_exceeded(self):
+        if self.failure_count >= self.max_failures:
+            return True
+        if time.monotonic() - self.last_failure_time > self.reset_timeout_sec:
+            self.reset()
+        return False
+
+
+class Gr00tDataCollector:
+    def __init__(
+        self,
+        node,
+        camera_host: str,
+        camera_port: int,
+        state_topic_name: str,
+        data_exporter: Gr00tDataExporter,
+        text_to_speech=None,
+        frequency=20,
+        state_act_msg_frequency=50,
+    ):
+
+        self.text_to_speech = text_to_speech
+        self.frequency = frequency
+        self.data_exporter = data_exporter
+
+        self.node = node
+
+        thread = threading.Thread(target=rclpy.spin, args=(self.node,), daemon=True)
+        thread.start()
+        time.sleep(0.5)
+
+        self._episode_state = EpisodeState()
+        self._keyboard_listener = KeyboardListenerSubscriber()
+        self._state_subscriber = ROSMsgSubscriber(state_topic_name)
+        self._image_subscriber = ComposedCameraClientSensor(server_ip=camera_host, port=camera_port)
+        self.rate = self.node.create_rate(self.frequency)
+
+        self.obs_act_buffer = deque(maxlen=100)
+        self.latest_image_msg = None
+        self.latest_proprio_msg = None
+
+        self.state_polling_rate = 1 / state_act_msg_frequency
+        self.last_state_poll_time = time.monotonic()
+
+        self.telemetry = Telemetry(window_size=100)
+        self.timing_threshold_monitor = TimingThresholdMonitor()
+
+        print(f"Recording to {self.data_exporter.meta.root}")
+
+    @property
+    def current_episode_index(self):
+        return self.data_exporter.episode_buffer["episode_index"]
+
+    def _print_and_say(self, message: str, say: bool = True):
+        """Helper to use TextToSpeech print_and_say or fallback to print."""
+        if self.text_to_speech is not None:
+            self.text_to_speech.print_and_say(message, say)
+        else:
+            print(message)
+
+    def _check_keyboard_input(self):
+        key = self._keyboard_listener.read_msg()
+        if key == "c":
+            self._episode_state.change_state()
+            if self._episode_state.get_state() == self._episode_state.RECORDING:
+                self._print_and_say(f"Started recording {self.current_episode_index}")
+            elif self._episode_state.get_state() == self._episode_state.NEED_TO_SAVE:
+                self._print_and_say("Stopping recording, preparing to save")
+            elif self._episode_state.get_state() == self._episode_state.IDLE:
+                self._print_and_say("Saved episode and back to idle state")
+        elif key == "x":
+            if self._episode_state.get_state() == self._episode_state.RECORDING:
+                self.data_exporter.save_episode_as_discarded()
+                self._episode_state.reset_state()
+                self._print_and_say("Discarded episode")
+
+    def _add_data_frame(self):
+        t_start = time.monotonic()
+
+        if self.latest_proprio_msg is None or self.latest_image_msg is None:
+            self._print_and_say(
+                f"Waiting for message. "
+                f"Avail msg: proprio {self.latest_proprio_msg is not None} | "
+                f"image {self.latest_image_msg is not None}",
+                say=False,
+            )
+            return False
+
+        if self._episode_state.get_state() == self._episode_state.RECORDING:
+
+            # Calculate max time delta between images and proprio
+            max_time_delta = 0
+            for _, image_time in self.latest_image_msg["timestamps"].items():
+                time_delta = abs(image_time - self.latest_proprio_msg["timestamps"]["proprio"])
+                max_time_delta = max(max_time_delta, time_delta)
+
+            self.timing_threshold_monitor.log_time_delta(max_time_delta)
+            if (self.timing_threshold_monitor.failure_count + 1) % 100 == 0:
+                self._print_and_say("Image state delta too high, please discard data")
+
+            frame_data = {
+                "observation.state": self.latest_proprio_msg["q"],
+                "observation.eef_state": self.latest_proprio_msg["wrist_pose"],
+                "action": self.latest_proprio_msg["action"],
+                "action.eef": self.latest_proprio_msg["action.eef"],
+                "observation.img_state_delta": (
+                    np.array(
+                        [max_time_delta],
+                        dtype=np.float32,
+                    )
+                ),  # lerobot only supports adding numpy arrays
+                "teleop.navigate_command": np.array(
+                    self.latest_proprio_msg["navigate_command"], dtype=np.float64
+                ),
+                "teleop.base_height_command": np.array(
+                    [self.latest_proprio_msg["base_height_command"]], dtype=np.float64
+                ),
+            }
+
+            # Add images based on dataset features
+            images = self.latest_image_msg["images"]
+            for feature_name, feature_info in self.data_exporter.features.items():
+                if feature_info.get("dtype") in ["image", "video"]:
+                    # Extract image key from feature name (e.g., "observation.images.ego_view" -> "ego_view")
+                    image_key = feature_name.split(".")[-1]
+
+                    if image_key not in images:
+                        raise ValueError(
+                            f"Required image '{image_key}' for feature '{feature_name}' "
+                            f"not found in image message. Available images: {list(images.keys())}"
+                        )
+                    frame_data[feature_name] = images[image_key]
+
+            self.data_exporter.add_frame(frame_data)
+
+        t_end = time.monotonic()
+        if t_end - t_start > (1 / self.frequency):
+            print(f"DataExporter Missed: {t_end - t_start} sec")
+
+        if self._episode_state.get_state() == self._episode_state.NEED_TO_SAVE:
+            self.data_exporter.save_episode()
+            self.timing_threshold_monitor.reset()
+            self._print_and_say("Finished saving episode")
+            self._episode_state.change_state()
+
+        return True
+
+    def save_and_cleanup(self):
+        try:
+            self._print_and_say("saving episode done")
+            # save on going episode if any
+            buffer_size = self.data_exporter.episode_buffer.get("size", 0)
+            if buffer_size > 0:
+                self.data_exporter.save_episode()
+            self._print_and_say(f"Recording complete: {self.data_exporter.meta.root}", say=False)
+        except Exception as e:
+            self._print_and_say(f"Error saving episode: {e}")
+
+        self.node.destroy_node()
+        rclpy.shutdown()
+        self._print_and_say("Shutting down data exporter...", say=False)
+
+    def run(self):
+        try:
+            while rclpy.ok():
+                t_start = time.monotonic()
+                with self.telemetry.timer("total_loop"):
+                    # 1. poll proprio msg
+                    with self.telemetry.timer("poll_state"):
+                        msg = self._state_subscriber.get_msg()
+                        if msg is not None:
+                            self.latest_proprio_msg = msg
+
+                    # 2. poll image msg
+                    with self.telemetry.timer("poll_image"):
+                        msg = self._image_subscriber.read()
+                        if msg is not None:
+                            self.latest_image_msg = msg
+
+                    # 3. check keyboard input
+                    with self.telemetry.timer("check_keyboard"):
+                        self._check_keyboard_input()
+
+                    # 4. add frame
+                    with self.telemetry.timer("add_frame"):
+                        self._add_data_frame()
+
+                    end_time = time.monotonic()
+
+                self.rate.sleep()
+
+                # Log timing information if we missed our target frequency
+                if (end_time - t_start) > (1 / self.frequency):
+                    self.telemetry.log_timing_info(
+                        context="Data Exporter Loop Missed", threshold=0.001
+                    )
+
+        except KeyboardInterrupt:
+            print("Data exporter terminated by user")
+            # The user will trigger a keyboard interrupt if there's something wrong,
+            # so we flag the ongoing episode as discarded
+            buffer_size = self.data_exporter.episode_buffer.get("size", 0)
+            if buffer_size > 0:
+                self.data_exporter.save_episode_as_discarded()
+
+        finally:
+            self.save_and_cleanup()
+
+
+def main(config: DataExporterConfig):
+
+    rclpy.init(args=None)
+    node = rclpy.create_node("data_exporter")
+
+    waist_location = "lower_and_upper_body" if config.enable_waist else "lower_body"
+    g1_rm = g1.instantiate_g1_robot_model(
+        waist_location=waist_location, high_elbow_pose=config.high_elbow_pose
+    )
+
+    dataset_features = get_dataset_features(g1_rm, config.add_stereo_camera)
+    modality_config = get_modality_config(g1_rm, config.add_stereo_camera)
+
+    text_to_speech = TextToSpeech() if config.text_to_speech else None
+
+    # Only set DataCollectionInfo if we're creating a new dataset
+    # When adding to existing dataset, DataCollectionInfo will be ignored
+    if config.robot_id is not None:
+        data_collection_info = DataCollectionInfo(
+            teleoperator_username=config.teleoperator_username,
+            support_operator_username=config.support_operator_username,
+            robot_type="g1",
+            robot_id=config.robot_id,
+            lower_body_policy=config.lower_body_policy,
+            wbc_model_path=config.wbc_model_path,
+        )
+    else:
+        # Use default DataCollectionInfo when adding to existing dataset
+        # This will be ignored if the dataset already exists
+        data_collection_info = DataCollectionInfo()
+
+    robot_config_client = ROSServiceClient(ROBOT_CONFIG_TOPIC)
+    robot_config = robot_config_client.get_config()
+
+    data_exporter = Gr00tDataExporter.create(
+        save_root=f"{config.root_output_dir}/{config.dataset_name}",
+        fps=config.data_collection_frequency,
+        features=dataset_features,
+        modality_config=modality_config,
+        task=config.task_prompt,
+        upload_bucket_path=BUCKET_BASE_PATH,
+        data_collection_info=data_collection_info,
+        script_config=robot_config,
+    )
+
+    data_collector = Gr00tDataCollector(
+        node=node,
+        frequency=config.data_collection_frequency,
+        data_exporter=data_exporter,
+        state_topic_name=STATE_TOPIC_NAME,
+        camera_host=config.camera_host,
+        camera_port=config.camera_port,
+        text_to_speech=text_to_speech,
+    )
+    data_collector.run()
+
+
+if __name__ == "__main__":
+    config = tyro.cli(DataExporterConfig)
+    config.task_prompt = input("Enter the task prompt: ").strip().lower()
+    add_to_existing_dataset = input("Add to existing dataset? (y/n): ").strip().lower()
+
+    if add_to_existing_dataset == "y":
+        config.dataset_name = input("Enter the dataset name: ").strip().lower()
+        # When adding to existing dataset, we don't need robot_id or operator usernames
+        # as they should already be set in the existing dataset
+    elif add_to_existing_dataset == "n":
+        # robot_id = input("Enter the robot ID: ").strip().lower()
+        # if robot_id not in G1_ROBOT_IDS:
+        #     raise ValueError(f"Invalid robot ID: {robot_id}. Available robot IDs: {G1_ROBOT_IDS}")
+        config.robot_id = "sim"
+        config.dataset_name = f"{datetime.now().strftime('%Y-%m-%d-%H-%M-%S')}-G1-{config.robot_id}"
+
+        # Only ask for operator usernames when creating a new dataset
+        # print("Available teleoperator usernames:")
+        # for i, username in enumerate(OPERATOR_USERNAMES):
+        #     print(f"{i}: {username}")
+        # teleop_idx = int(input("Select teleoperator username index: "))
+        # config.teleoperator_username = OPERATOR_USERNAMES[teleop_idx]
+        config.teleoperator_username = "NEW_USER"
+
+        # print("\nAvailable support operator usernames:")
+        # for i, username in enumerate(OPERATOR_USERNAMES):
+        #     print(f"{i}: {username}")
+        # support_idx = int(input("Select support operator username index: "))
+        # config.support_operator_username = OPERATOR_USERNAMES[support_idx]
+        config.support_operator_username = "NEW_USER"
+
+    main(config)

GR00T-WholeBodyControl/decoupled_wbc/control/main/teleop/run_navigation_policy_loop.py

@@ -0,0 +1,68 @@
+import threading
+import time
+
+import rclpy
+
+from decoupled_wbc.control.main.constants import NAV_CMD_TOPIC
+from decoupled_wbc.control.policy.keyboard_navigation_policy import KeyboardNavigationPolicy
+from decoupled_wbc.control.utils.keyboard_dispatcher import KeyboardListenerSubscriber
+from decoupled_wbc.control.utils.ros_utils import ROSMsgPublisher
+
+FREQUENCY = 10
+NAV_NODE_NAME = "NavigationPolicy"
+
+
+def main():
+    rclpy.init(args=None)
+    node = rclpy.create_node(NAV_NODE_NAME)
+
+    # Start ROS spin in a separate thread
+    thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True)
+    thread.start()
+    time.sleep(0.5)
+
+    dict_publisher = ROSMsgPublisher(NAV_CMD_TOPIC)
+    keyboard_listener = KeyboardListenerSubscriber()
+
+    # Initialize navigation policy
+    navigation_policy = KeyboardNavigationPolicy()
+
+    # Create rate controller
+    rate = node.create_rate(FREQUENCY)
+
+    try:
+        while rclpy.ok():
+            t_now = time.monotonic()
+            # get keyboard input
+
+            navigation_policy.handle_keyboard_button(keyboard_listener.read_msg())
+            # Get action from navigation policy
+            action = navigation_policy.get_action(time=t_now)
+
+            # Add timestamp to the data
+            action["timestamp"] = t_now
+
+            # Create and publish ByteMultiArray message
+            dict_publisher.publish(action)
+
+            # Print status periodically (optional)
+            if int(t_now * 10) % 10 == 0:
+                nav_cmd = action["navigate_cmd"]
+                node.get_logger().info(
+                    f"Nav cmd: linear=({nav_cmd[0]:.2f}, {nav_cmd[1]:.2f}), "
+                    f"angular={nav_cmd[2]:.2f}"
+                )
+
+            rate.sleep()
+
+    except KeyboardInterrupt:
+        print("Navigation control loop terminated by user")
+
+    finally:
+        # Clean shutdown
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

GR00T-WholeBodyControl/decoupled_wbc/control/main/teleop/run_sim_loop.py

@@ -0,0 +1,61 @@
+from typing import Dict
+
+import tyro
+
+from decoupled_wbc.control.envs.g1.sim.simulator_factory import SimulatorFactory, init_channel
+from decoupled_wbc.control.main.teleop.configs.configs import SimLoopConfig
+from decoupled_wbc.control.robot_model.instantiation.g1 import (
+    instantiate_g1_robot_model,
+)
+from decoupled_wbc.control.robot_model.robot_model import RobotModel
+
+ArgsConfig = SimLoopConfig
+
+
+class SimWrapper:
+    def __init__(self, robot_model: RobotModel, env_name: str, config: Dict[str, any], **kwargs):
+        self.robot_model = robot_model
+        self.config = config
+
+        init_channel(config=self.config)
+
+        # Create simulator using factory
+        self.sim = SimulatorFactory.create_simulator(
+            config=self.config,
+            env_name=env_name,
+            **kwargs,
+        )
+
+
+def main(config: ArgsConfig):
+    wbc_config = config.load_wbc_yaml()
+    # NOTE: we will override the interface to local if it is not specified
+    wbc_config["ENV_NAME"] = config.env_name
+
+    if config.enable_image_publish:
+        assert (
+            config.enable_offscreen
+        ), "enable_offscreen must be True when enable_image_publish is True"
+
+    robot_model = instantiate_g1_robot_model()
+
+    sim_wrapper = SimWrapper(
+        robot_model=robot_model,
+        env_name=config.env_name,
+        config=wbc_config,
+        onscreen=wbc_config.get("ENABLE_ONSCREEN", True),
+        offscreen=wbc_config.get("ENABLE_OFFSCREEN", False),
+    )
+    # Start simulator as independent process
+    SimulatorFactory.start_simulator(
+        sim_wrapper.sim,
+        as_thread=False,
+        enable_image_publish=config.enable_image_publish,
+        mp_start_method=config.mp_start_method,
+        camera_port=config.camera_port,
+    )
+
+
+if __name__ == "__main__":
+    config = tyro.cli(ArgsConfig)
+    main(config)

GR00T-WholeBodyControl/decoupled_wbc/control/main/teleop/run_sync_sim_data_collection.py

@@ -0,0 +1,213 @@
+from pathlib import Path
+import time
+
+import tyro
+
+from decoupled_wbc.control.main.teleop.configs.configs import SyncSimDataCollectionConfig
+from decoupled_wbc.control.robot_model.instantiation import get_robot_type_and_model
+from decoupled_wbc.control.utils.keyboard_dispatcher import (
+    KeyboardDispatcher,
+    KeyboardListener,
+)
+from decoupled_wbc.control.utils.ros_utils import ROSManager
+from decoupled_wbc.control.utils.sync_sim_utils import (
+    COLLECTION_KEY,
+    SKIP_KEY,
+    CITestManager,
+    EpisodeManager,
+    generate_frame,
+    get_data_exporter,
+    get_env,
+    get_policies,
+)
+from decoupled_wbc.control.utils.telemetry import Telemetry
+
+CONTROL_NODE_NAME = "ControlPolicy"
+
+
+CONTROL_CMD_TOPIC = CONTROL_NODE_NAME + "/q_target"
+ENV_NODE_NAME = "SyncEnv"
+ENV_OBS_TOPIC = ENV_NODE_NAME + "/obs"
+
+
+def display_controls(config: SyncSimDataCollectionConfig):
+    """
+    Method to pretty print controls.
+    """
+
+    def print_command(char, info):
+        char += " " * (30 - len(char))
+        print("{}\t{}".format(char, info))
+
+    print("")
+    print_command("Keys", "Command")
+    if config.manual_control:
+        print_command(COLLECTION_KEY, "start/stop data collection")
+    print_command(SKIP_KEY, "skip and collect new episodes")
+    print_command("w-s-a-d", "move horizontally in x-y plane (press '=' first to enable)")
+    print_command("q", "rotate (counter-clockwise)")
+    print_command("e", "rotate (clockwise)")
+    print_command("space", "reset all velocity to zero")
+    print("")
+
+
+def main(config: SyncSimDataCollectionConfig):
+    ros_manager = ROSManager(node_name=CONTROL_NODE_NAME)
+    node = ros_manager.node
+
+    # Initialize telemetry
+    telemetry = Telemetry(window_size=100)
+
+    # Initialize robot model
+    robot_type, robot_model = get_robot_type_and_model(
+        config.robot,
+        enable_waist_ik=config.enable_waist,
+    )
+
+    # Initialize sim env
+    env = get_env(config, onscreen=config.enable_onscreen, offscreen=config.save_img_obs)
+    seed = int(time.time())
+    env.reset(seed)
+    env.render()
+    obs = env.observe()
+    robot_model.set_initial_body_pose(obs["q"])
+
+    # Initialize data exporter
+    exporter = get_data_exporter(
+        config,
+        obs,
+        robot_model,
+        save_path=Path("./outputs/ci_test/") if config.ci_test else None,
+    )
+
+    # Display control signals
+    display_controls(config)
+
+    # Initialize policies
+    wbc_policy, teleop_policy = get_policies(config, robot_type, robot_model)
+
+    dispatcher = KeyboardDispatcher()
+    keyboard_listener = KeyboardListener()  # for data collection keys
+    dispatcher.register(keyboard_listener)
+    dispatcher.register(wbc_policy)
+    dispatcher.register(teleop_policy)
+
+    dispatcher.start()
+
+    rate = node.create_rate(config.control_frequency)
+
+    # Initialize episode manager to handle state transitions and data collection
+    episode_manager = EpisodeManager(config)
+
+    # Initialize CI test manager
+    ci_test_manager = CITestManager(config) if config.ci_test else None
+
+    try:
+        while ros_manager.ok():
+
+            need_reset = False
+            keyboard_input = keyboard_listener.pop_key()
+
+            with telemetry.timer("total_loop"):
+                max_mujoco_state_len, mujoco_state_len, mujoco_state = env.get_mujoco_state_info()
+
+                # Measure observation time
+                with telemetry.timer("observe"):
+                    obs = env.observe()
+                    wbc_policy.set_observation(obs)
+
+                # Measure policy setup time
+                with telemetry.timer("policy_setup"):
+                    teleop_cmd = teleop_policy.get_action()
+
+                    wbc_goal = {}
+
+                    # Note that wbc_goal["navigation_cmd'] could be overwritten by teleop_cmd
+                    if teleop_cmd:
+                        for key, value in teleop_cmd.items():
+                            wbc_goal[key] = value
+                        # Draw IK indicators
+                        if config.ik_indicator:
+                            env.set_ik_indicator(teleop_cmd)
+                    if wbc_goal:
+                        wbc_policy.set_goal(wbc_goal)
+
+                # Measure policy action calculation time
+                with telemetry.timer("policy_action"):
+                    wbc_action = wbc_policy.get_action()
+
+                if config.ci_test:
+                    ci_test_manager.check_upper_body_motion(robot_model, wbc_action, config)
+
+                # Measure action queue time
+                with telemetry.timer("step"):
+                    obs, _, _, _, step_info = env.step(wbc_action)
+                    env.render()
+                    episode_manager.increment_step()
+
+                if config.ci_test and config.ci_test_mode == "pre_merge":
+                    ci_test_manager.check_end_effector_tracking(
+                        teleop_cmd, obs, config, episode_manager.get_step_count()
+                    )
+
+                # Handle data collection trigger
+                episode_manager.handle_collection_trigger(wbc_goal, keyboard_input, step_info)
+
+                # Collect data frame
+                if episode_manager.should_collect_data():
+                    frame = generate_frame(
+                        obs,
+                        wbc_action,
+                        seed,
+                        mujoco_state,
+                        mujoco_state_len,
+                        max_mujoco_state_len,
+                        teleop_cmd,
+                        wbc_goal,
+                        config.save_img_obs,
+                    )
+                    # exporting data
+                    exporter.add_frame(frame)
+
+                    # if done and task_completion_hold_count is 0, save the episode
+                    need_reset = episode_manager.check_export_and_completion(exporter)
+
+                # check data abort
+                if episode_manager.handle_skip(wbc_goal, keyboard_input, exporter):
+                    need_reset = True
+
+                if need_reset:
+                    if config.ci_test:
+                        print("CI test: Completed...")
+                        raise KeyboardInterrupt
+
+                    seed = int(time.time())
+                    env.reset(seed)
+                    env.render()
+
+                    print("Sleeping for 3 seconds before resetting teleop policy...")
+                    for j in range(3, 0, -1):
+                        print(f"Starting in {j}...")
+                        time.sleep(1)
+
+                    wbc_policy, teleop_policy = get_policies(config, robot_type, robot_model)
+                    episode_manager.reset_step_count()
+
+            rate.sleep()
+
+    except ros_manager.exceptions() as e:
+        print(f"ROSManager interrupted by user: {e}")
+    finally:
+        # Cleanup resources
+        teleop_policy.close()
+        dispatcher.stop()
+        ros_manager.shutdown()
+        env.close()
+        print("Sync sim data collection loop terminated.")
+
+    return True
+
+
+if __name__ == "__main__":
+    config = tyro.cli(SyncSimDataCollectionConfig)
+    main(config)

GR00T-WholeBodyControl/decoupled_wbc/control/main/teleop/run_teleop_policy_loop.py

@@ -0,0 +1,110 @@
+import time
+
+import rclpy
+import tyro
+
+from decoupled_wbc.control.main.constants import CONTROL_GOAL_TOPIC
+from decoupled_wbc.control.main.teleop.configs.configs import TeleopConfig
+from decoupled_wbc.control.policy.lerobot_replay_policy import LerobotReplayPolicy
+from decoupled_wbc.control.policy.teleop_policy import TeleopPolicy
+from decoupled_wbc.control.robot_model.instantiation.g1 import instantiate_g1_robot_model
+from decoupled_wbc.control.teleop.solver.hand.instantiation.g1_hand_ik_instantiation import (
+    instantiate_g1_hand_ik_solver,
+)
+from decoupled_wbc.control.teleop.teleop_retargeting_ik import TeleopRetargetingIK
+from decoupled_wbc.control.utils.ros_utils import ROSManager, ROSMsgPublisher
+from decoupled_wbc.control.utils.telemetry import Telemetry
+
+TELEOP_NODE_NAME = "TeleopPolicy"
+
+
+def main(config: TeleopConfig):
+    ros_manager = ROSManager(node_name=TELEOP_NODE_NAME)
+    node = ros_manager.node
+
+    if config.robot == "g1":
+        waist_location = "lower_and_upper_body" if config.enable_waist else "lower_body"
+        robot_model = instantiate_g1_robot_model(
+            waist_location=waist_location, high_elbow_pose=config.high_elbow_pose
+        )
+        left_hand_ik_solver, right_hand_ik_solver = instantiate_g1_hand_ik_solver()
+    else:
+        raise ValueError(f"Unsupported robot name: {config.robot}")
+
+    if config.lerobot_replay_path:
+        teleop_policy = LerobotReplayPolicy(
+            robot_model=robot_model, parquet_path=config.lerobot_replay_path
+        )
+    else:
+        print("running teleop policy, waiting teleop policy to be initialized...")
+        retargeting_ik = TeleopRetargetingIK(
+            robot_model=robot_model,
+            left_hand_ik_solver=left_hand_ik_solver,
+            right_hand_ik_solver=right_hand_ik_solver,
+            enable_visualization=config.enable_visualization,
+            body_active_joint_groups=["upper_body"],
+        )
+        teleop_policy = TeleopPolicy(
+            robot_model=robot_model,
+            retargeting_ik=retargeting_ik,
+            body_control_device=config.body_control_device,
+            hand_control_device=config.hand_control_device,
+            body_streamer_ip=config.body_streamer_ip,  # vive tracker, leap motion does not require
+            body_streamer_keyword=config.body_streamer_keyword,
+            enable_real_device=config.enable_real_device,
+            replay_data_path=config.teleop_replay_path,
+        )
+
+    # Create a publisher for the navigation commands
+    control_publisher = ROSMsgPublisher(CONTROL_GOAL_TOPIC)
+
+    # Create rate controller
+    rate = node.create_rate(config.teleop_frequency)
+    iteration = 0
+    time_to_get_to_initial_pose = 2  # seconds
+
+    telemetry = Telemetry(window_size=100)
+
+    try:
+        while rclpy.ok():
+            with telemetry.timer("total_loop"):
+                t_start = time.monotonic()
+                # Get the current teleop action
+                with telemetry.timer("get_action"):
+                    data = teleop_policy.get_action()
+
+                # Add timing information to the message
+                t_now = time.monotonic()
+                data["timestamp"] = t_now
+
+                # Set target completion time - longer for initial pose, then match control frequency
+                if iteration == 0:
+                    data["target_time"] = t_now + time_to_get_to_initial_pose
+                else:
+                    data["target_time"] = t_now + (1 / config.teleop_frequency)
+
+                # Publish the teleop command
+                with telemetry.timer("publish_teleop_command"):
+                    control_publisher.publish(data)
+
+                # For the initial pose, wait the full duration before continuing
+                if iteration == 0:
+                    print(f"Moving to initial pose for {time_to_get_to_initial_pose} seconds")
+                    time.sleep(time_to_get_to_initial_pose)
+                iteration += 1
+            end_time = time.monotonic()
+            if (end_time - t_start) > (1 / config.teleop_frequency):
+                telemetry.log_timing_info(context="Teleop Policy Loop Missed", threshold=0.001)
+            rate.sleep()
+
+    except ros_manager.exceptions() as e:
+        print(f"ROSManager interrupted by user: {e}")
+
+    finally:
+        print("Cleaning up...")
+        ros_manager.shutdown()
+
+
+if __name__ == "__main__":
+    config = tyro.cli(TeleopConfig)
+    main(config)

GR00T-WholeBodyControl/decoupled_wbc/control/robot_model/instantiation/__init__.py

@@ -0,0 +1,15 @@
+from .g1 import instantiate_g1_robot_model
+
+
+def get_robot_type_and_model(robot: str, enable_waist_ik: bool = False):
+    """Get the robot type from the robot name."""
+    if robot.lower().startswith("g1"):
+        if "FixedLowerBody" in robot or "FloatingBody" in robot:
+            waist_location = "upper_body"
+        elif enable_waist_ik:
+            waist_location = "lower_and_upper_body"
+        else:
+            waist_location = "lower_body"
+        return "g1", instantiate_g1_robot_model(waist_location=waist_location)
+    else:
+        raise ValueError(f"Invalid robot name: {robot}")

GR00T-WholeBodyControl/decoupled_wbc/control/robot_model/instantiation/g1.py

@@ -0,0 +1,62 @@
+import os
+from pathlib import Path
+from typing import Literal
+
+from decoupled_wbc.control.robot_model.robot_model import RobotModel
+from decoupled_wbc.control.robot_model.supplemental_info.g1.g1_supplemental_info import (
+    ElbowPose,
+    G1SupplementalInfo,
+    WaistLocation,
+)
+
+
+def instantiate_g1_robot_model(
+    waist_location: Literal["lower_body", "upper_body", "lower_and_upper_body"] = "lower_body",
+    high_elbow_pose: bool = False,
+):
+    """
+    Instantiate a G1 robot model with configurable waist location and pose.
+
+    Args:
+        waist_location: Whether to put waist in "lower_body" (default G1 behavior),
+                        "upper_body" (waist controlled with arms/manipulation via IK),
+                        or "lower_and_upper_body" (waist reference from arms/manipulation
+                        via IK then passed to lower body policy)
+        high_elbow_pose: Whether to use high elbow pose configuration for default joint positions
+
+    Returns:
+        RobotModel: Configured G1 robot model
+    """
+    project_root = Path(__file__).resolve().parent.parent.parent.parent.parent
+    robot_model_config = {
+        "asset_path": os.path.join(project_root, "decoupled_wbc/control/robot_model/model_data/g1"),
+        "urdf_path": os.path.join(
+            project_root, "decoupled_wbc/control/robot_model/model_data/g1/g1_29dof_with_hand.urdf"
+        ),
+    }
+    assert waist_location in [
+        "lower_body",
+        "upper_body",
+        "lower_and_upper_body",
+    ], f"Invalid waist_location: {waist_location}. Must be 'lower_body' or 'upper_body' or 'lower_and_upper_body'"
+
+    # Map string values to enums
+    waist_location_enum = {
+        "lower_body": WaistLocation.LOWER_BODY,
+        "upper_body": WaistLocation.UPPER_BODY,
+        "lower_and_upper_body": WaistLocation.LOWER_AND_UPPER_BODY,
+    }[waist_location]
+
+    elbow_pose_enum = ElbowPose.HIGH if high_elbow_pose else ElbowPose.LOW
+
+    # Create single configurable supplemental info instance
+    robot_model_supplemental_info = G1SupplementalInfo(
+        waist_location=waist_location_enum, elbow_pose=elbow_pose_enum
+    )
+
+    robot_model = RobotModel(
+        robot_model_config["urdf_path"],
+        robot_model_config["asset_path"],
+        supplemental_info=robot_model_supplemental_info,
+    )
+    return robot_model

GR00T-WholeBodyControl/decoupled_wbc/control/robot_model/model_data/g1/g1_29dof.urdf

@@ -0,0 +1,1091 @@
+<robot name="g1_29dof">
+  <mujoco>
+    <compiler meshdir="meshes" discardvisual="false"/>
+  </mujoco>
+
+  <!-- [CAUTION] uncomment when convert to mujoco -->
+  <!-- <link name="world"></link>
+  <joint name="floating_base_joint" type="floating">
+    <parent link="world"/>
+    <child link="pelvis"/>
+  </joint> -->
+
+  <link name="pelvis">
+    <inertial>
+      <origin xyz="0 0 -0.07605" rpy="0 0 0"/>
+      <mass value="3.813"/>
+      <inertia ixx="0.010549" ixy="0" ixz="2.1E-06" iyy="0.0093089" iyz="0" izz="0.0079184"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/pelvis.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="pelvis_contour_link">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="0.001"/>
+      <inertia ixx="1e-7" ixy="0" ixz="0" iyy="1e-7" iyz="0" izz="1e-7"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/pelvis_contour_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/pelvis_contour_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="pelvis_contour_joint" type="fixed">
+    <parent link="pelvis"/>
+    <child link="pelvis_contour_link"/>
+  </joint>
+
+  <!-- Legs -->
+  <link name="left_hip_pitch_link">
+    <inertial>
+      <origin xyz="0.002741 0.047791 -0.02606" rpy="0 0 0"/>
+      <mass value="1.35"/>
+      <inertia ixx="0.001811" ixy="3.68E-05" ixz="-3.44E-05" iyy="0.0014193" iyz="0.000171" izz="0.0012812"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_hip_pitch_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_hip_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_hip_pitch_joint" type="revolute">
+    <origin xyz="0 0.064452 -0.1027" rpy="0 0 0"/>
+    <parent link="pelvis"/>
+    <child link="left_hip_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-2.5307" upper="2.8798" effort="88" velocity="32"/>
+  </joint>
+  <link name="left_hip_roll_link">
+    <inertial>
+      <origin xyz="0.029812 -0.001045 -0.087934" rpy="0 0 0"/>
+      <mass value="1.52"/>
+      <inertia ixx="0.0023773" ixy="-3.8E-06" ixz="-0.0003908" iyy="0.0024123" iyz="1.84E-05" izz="0.0016595"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_hip_roll_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_hip_roll_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_hip_roll_joint" type="revolute">
+    <origin xyz="0 0.052 -0.030465" rpy="0 -0.1749 0"/>
+    <parent link="left_hip_pitch_link"/>
+    <child link="left_hip_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-0.5236" upper="2.9671" effort="88" velocity="32"/>
+  </joint>
+  <link name="left_hip_yaw_link">
+    <inertial>
+      <origin xyz="-0.057709 -0.010981 -0.15078" rpy="0 0 0"/>
+      <mass value="1.702"/>
+      <inertia ixx="0.0057774" ixy="-0.0005411" ixz="-0.0023948" iyy="0.0076124" iyz="-0.0007072" izz="0.003149"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_hip_yaw_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_hip_yaw_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_hip_yaw_joint" type="revolute">
+    <origin xyz="0.025001 0 -0.12412" rpy="0 0 0"/>
+    <parent link="left_hip_roll_link"/>
+    <child link="left_hip_yaw_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-2.7576" upper="2.7576" effort="88" velocity="32"/>
+  </joint>
+  <link name="left_knee_link">
+    <inertial>
+      <origin xyz="0.005457 0.003964 -0.12074" rpy="0 0 0"/>
+      <mass value="1.932"/>
+      <inertia ixx="0.011329" ixy="4.82E-05" ixz="-4.49E-05" iyy="0.011277" iyz="-0.0007146" izz="0.0015168"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_knee_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_knee_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_knee_joint" type="revolute">
+    <origin xyz="-0.078273 0.0021489 -0.17734" rpy="0 0.1749 0"/>
+    <parent link="left_hip_yaw_link"/>
+    <child link="left_knee_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.087267" upper="2.8798" effort="139" velocity="20"/>
+  </joint>
+  <link name="left_ankle_pitch_link">
+    <inertial>
+      <origin xyz="-0.007269 0 0.011137" rpy="0 0 0"/>
+      <mass value="0.074"/>
+      <inertia ixx="8.4E-06" ixy="0" ixz="-2.9E-06" iyy="1.89E-05" iyz="0" izz="1.26E-05"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_ankle_pitch_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_ankle_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_ankle_pitch_joint" type="revolute">
+    <origin xyz="0 -9.4445E-05 -0.30001" rpy="0 0 0"/>
+    <parent link="left_knee_link"/>
+    <child link="left_ankle_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.87267" upper="0.5236" effort="50" velocity="37"/>
+  </joint>
+  <link name="left_ankle_roll_link">
+    <inertial>
+      <origin xyz="0.026505 0 -0.016425" rpy="0 0 0"/>
+      <mass value="0.608"/>
+      <inertia ixx="0.0002231" ixy="2E-07" ixz="8.91E-05" iyy="0.0016161" iyz="-1E-07" izz="0.0016667"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_ankle_roll_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="-0.05 0.025 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.005"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin xyz="-0.05 -0.025 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.005"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin xyz="0.12 0.03 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.005"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin xyz="0.12 -0.03 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.005"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_ankle_roll_joint" type="revolute">
+    <origin xyz="0 0 -0.017558" rpy="0 0 0"/>
+    <parent link="left_ankle_pitch_link"/>
+    <child link="left_ankle_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-0.2618" upper="0.2618" effort="50" velocity="37"/>
+  </joint>
+  <link name="right_hip_pitch_link">
+    <inertial>
+      <origin xyz="0.002741 -0.047791 -0.02606" rpy="0 0 0"/>
+      <mass value="1.35"/>
+      <inertia ixx="0.001811" ixy="-3.68E-05" ixz="-3.44E-05" iyy="0.0014193" iyz="-0.000171" izz="0.0012812"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_hip_pitch_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_hip_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_hip_pitch_joint" type="revolute">
+    <origin xyz="0 -0.064452 -0.1027" rpy="0 0 0"/>
+    <parent link="pelvis"/>
+    <child link="right_hip_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-2.5307" upper="2.8798" effort="88" velocity="32"/>
+  </joint>
+  <link name="right_hip_roll_link">
+    <inertial>
+      <origin xyz="0.029812 0.001045 -0.087934" rpy="0 0 0"/>
+      <mass value="1.52"/>
+      <inertia ixx="0.0023773" ixy="3.8E-06" ixz="-0.0003908" iyy="0.0024123" iyz="-1.84E-05" izz="0.0016595"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_hip_roll_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_hip_roll_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_hip_roll_joint" type="revolute">
+    <origin xyz="0 -0.052 -0.030465" rpy="0 -0.1749 0"/>
+    <parent link="right_hip_pitch_link"/>
+    <child link="right_hip_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-2.9671" upper="0.5236" effort="88" velocity="32"/>
+  </joint>
+  <link name="right_hip_yaw_link">
+    <inertial>
+      <origin xyz="-0.057709 0.010981 -0.15078" rpy="0 0 0"/>
+      <mass value="1.702"/>
+      <inertia ixx="0.0057774" ixy="0.0005411" ixz="-0.0023948" iyy="0.0076124" iyz="0.0007072" izz="0.003149"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_hip_yaw_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_hip_yaw_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_hip_yaw_joint" type="revolute">
+    <origin xyz="0.025001 0 -0.12412" rpy="0 0 0"/>
+    <parent link="right_hip_roll_link"/>
+    <child link="right_hip_yaw_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-2.7576" upper="2.7576" effort="88" velocity="32"/>
+  </joint>
+  <link name="right_knee_link">
+    <inertial>
+      <origin xyz="0.005457 -0.003964 -0.12074" rpy="0 0 0"/>
+      <mass value="1.932"/>
+      <inertia ixx="0.011329" ixy="-4.82E-05" ixz="4.49E-05" iyy="0.011277" iyz="0.0007146" izz="0.0015168"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_knee_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_knee_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_knee_joint" type="revolute">
+    <origin xyz="-0.078273 -0.0021489 -0.17734" rpy="0 0.1749 0"/>
+    <parent link="right_hip_yaw_link"/>
+    <child link="right_knee_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.087267" upper="2.8798" effort="139" velocity="20"/>
+  </joint>
+  <link name="right_ankle_pitch_link">
+    <inertial>
+      <origin xyz="-0.007269 0 0.011137" rpy="0 0 0"/>
+      <mass value="0.074"/>
+      <inertia ixx="8.4E-06" ixy="0" ixz="-2.9E-06" iyy="1.89E-05" iyz="0" izz="1.26E-05"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_ankle_pitch_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_ankle_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_ankle_pitch_joint" type="revolute">
+    <origin xyz="0 9.4445E-05 -0.30001" rpy="0 0 0"/>
+    <parent link="right_knee_link"/>
+    <child link="right_ankle_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.87267" upper="0.5236" effort="50" velocity="37"/>
+  </joint>
+  <link name="right_ankle_roll_link">
+    <inertial>
+      <origin xyz="0.026505 0 -0.016425" rpy="0 0 0"/>
+      <mass value="0.608"/>
+      <inertia ixx="0.0002231" ixy="-2E-07" ixz="8.91E-05" iyy="0.0016161" iyz="1E-07" izz="0.0016667"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_ankle_roll_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="-0.05 0.025 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.005"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin xyz="-0.05 -0.025 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.005"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin xyz="0.12 0.03 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.005"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin xyz="0.12 -0.03 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.005"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_ankle_roll_joint" type="revolute">
+    <origin xyz="0 0 -0.017558" rpy="0 0 0"/>
+    <parent link="right_ankle_pitch_link"/>
+    <child link="right_ankle_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-0.2618" upper="0.2618" effort="50" velocity="37"/>
+  </joint>
+
+  <!-- Torso -->
+  <link name="waist_yaw_link">
+    <inertial>
+      <origin xyz="0.003964 0 0.018769" rpy="0 0 0"/>
+      <mass value="0.244"/>
+      <inertia ixx="9.9587E-05" ixy="-1.833E-06" ixz="-1.2617E-05" iyy="0.00012411" iyz="-1.18E-07" izz="0.00015586"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/waist_yaw_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="waist_yaw_joint" type="revolute">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="pelvis"/>
+    <child link="waist_yaw_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-2.618" upper="2.618" effort="88" velocity="32"/>
+  </joint>
+  <link name="waist_roll_link">
+    <inertial>
+      <origin xyz="0 -0.000236 0.010111" rpy="0 0 0"/>
+      <mass value="0.047"/>
+      <inertia ixx="7.515E-06" ixy="0" ixz="0" iyy="6.398E-06" iyz="9.9E-08" izz="3.988E-06"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/waist_roll_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="waist_roll_joint" type="revolute">
+    <origin xyz="-0.0039635 0 0.035" rpy="0 0 0"/>
+    <parent link="waist_yaw_link"/>
+    <child link="waist_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-0.52" upper="0.52" effort="50" velocity="37"/>
+  </joint>
+  <link name="torso_link">
+    <inertial>
+      <origin xyz="0.002601 0.000257 0.153719" rpy="0 0 0"/>
+      <mass value="8.562"/>
+      <inertia ixx="0.065674966" ixy="-8.597E-05" ixz="-0.001737252" iyy="0.053535188" iyz="8.6899E-05" izz="0.030808125"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/torso_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/torso_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="waist_pitch_joint" type="revolute">
+    <origin xyz="0 0 0.019" rpy="0 0 0"/>
+    <parent link="waist_roll_link"/>
+    <child link="torso_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.52" upper="0.52" effort="50" velocity="37"/>
+  </joint>
+
+  <!-- LOGO -->
+  <joint name="logo_joint" type="fixed">
+    <origin xyz="0.0039635 0 -0.054" rpy="0 0 0"/>
+    <parent link="torso_link"/>
+    <child link="logo_link"/>
+  </joint>
+  <link name="logo_link">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="0.001"/>
+      <inertia ixx="1e-7" ixy="0" ixz="0" iyy="1e-7" iyz="0" izz="1e-7"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/logo_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/logo_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- Head -->
+  <link name="head_link">
+    <inertial>
+      <origin xyz="0.005267 0.000299 0.449869" rpy="0 0 0"/>
+      <mass value="1.036"/>
+      <inertia ixx="0.004085051" ixy="-2.543E-06" ixz="-6.9455E-05" iyy="0.004185212" iyz="-3.726E-06" izz="0.001807911"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/head_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/head_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="head_joint" type="fixed">
+    <origin xyz="0.0039635 0 -0.054" rpy="0 0 0"/>
+    <parent link="torso_link"/>
+    <child link="head_link"/>
+  </joint>
+
+  <!-- Waist Support -->
+  <link name="waist_support_link">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="0.001"/>
+      <inertia ixx="1e-7" ixy="0" ixz="0" iyy="1e-7" iyz="0" izz="1e-7"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/waist_support_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/waist_support_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="waist_support_joint" type="fixed">
+    <origin xyz="0.0039635 0 -0.054" rpy="0 0 0"/>
+    <parent link="torso_link"/>
+    <child link="waist_support_link"/>
+  </joint>
+
+  <!-- IMU -->
+  <link name="imu_link"></link>
+  <joint name="imu_joint" type="fixed">
+    <origin xyz="-0.03959 -0.00224 0.13792" rpy="0 0 0"/>
+    <parent link="torso_link"/>
+    <child link="imu_link"/>
+  </joint>
+
+  <!-- d435 -->
+  <link name="d435_link"></link>
+  <joint name="d435_joint" type="fixed">
+    <origin xyz="0.0576235 0.01753 0.41987" rpy="0 0.8307767239493009 0"/>
+    <parent link="torso_link"/>
+    <child link="d435_link"/>
+  </joint>
+
+  <!-- mid360 -->
+  <link name="mid360_link"></link>
+  <joint name="mid360_joint" type="fixed">
+    <origin xyz="0.0002835 0.00003 0.40618" rpy="0 0.04014257279586953 0"/>
+    <parent link="torso_link"/>
+    <child link="mid360_link"/>
+  </joint>
+
+  <!-- Arm -->
+  <link name="left_shoulder_pitch_link">
+    <inertial>
+      <origin xyz="0 0.035892 -0.011628" rpy="0 0 0"/>
+      <mass value="0.718"/>
+      <inertia ixx="0.0004291" ixy="-9.2E-06" ixz="6.4E-06" iyy="0.000453" iyz="2.26E-05" izz="0.000423"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_shoulder_pitch_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0.04 -0.01" rpy="0 1.5707963267948966 0"/>
+      <geometry>
+        <cylinder radius="0.03" length="0.05"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_shoulder_pitch_joint" type="revolute">
+    <origin xyz="0.0039563 0.10022 0.23778" rpy="0.27931 5.4949E-05 -0.00019159"/>
+    <parent link="torso_link"/>
+    <child link="left_shoulder_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-3.0892" upper="2.6704" effort="25" velocity="37"/>
+  </joint>
+  <link name="left_shoulder_roll_link">
+    <inertial>
+      <origin xyz="-0.000227 0.00727 -0.063243" rpy="0 0 0"/>
+      <mass value="0.643"/>
+      <inertia ixx="0.0006177" ixy="-1E-06" ixz="8.7E-06" iyy="0.0006912" iyz="-5.3E-06" izz="0.0003894"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_shoulder_roll_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="-0.004 0.006 -0.053" rpy="0 0 0"/>
+      <geometry>
+        <cylinder radius="0.03" length="0.03"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_shoulder_roll_joint" type="revolute">
+    <origin xyz="0 0.038 -0.013831" rpy="-0.27925 0 0"/>
+    <parent link="left_shoulder_pitch_link"/>
+    <child link="left_shoulder_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-1.5882" upper="2.2515" effort="25" velocity="37"/>
+  </joint>
+  <link name="left_shoulder_yaw_link">
+    <inertial>
+      <origin xyz="0.010773 -0.002949 -0.072009" rpy="0 0 0"/>
+      <mass value="0.734"/>
+      <inertia ixx="0.0009988" ixy="7.9E-06" ixz="0.0001412" iyy="0.0010605" iyz="-2.86E-05" izz="0.0004354"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_shoulder_yaw_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_shoulder_yaw_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_shoulder_yaw_joint" type="revolute">
+    <origin xyz="0 0.00624 -0.1032" rpy="0 0 0"/>
+    <parent link="left_shoulder_roll_link"/>
+    <child link="left_shoulder_yaw_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-2.618" upper="2.618" effort="25" velocity="37"/>
+  </joint>
+  <link name="left_elbow_link">
+    <inertial>
+      <origin xyz="0.064956 0.004454 -0.010062" rpy="0 0 0"/>
+      <mass value="0.6"/>
+      <inertia ixx="0.0002891" ixy="6.53E-05" ixz="1.72E-05" iyy="0.0004152" iyz="-5.6E-06" izz="0.0004197"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_elbow_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_elbow_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_elbow_joint" type="revolute">
+    <origin xyz="0.015783 0 -0.080518" rpy="0 0 0"/>
+    <parent link="left_shoulder_yaw_link"/>
+    <child link="left_elbow_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-1.0472" upper="2.0944" effort="25" velocity="37"/>
+  </joint>
+  <joint name="left_wrist_roll_joint" type="revolute">
+    <origin xyz="0.100 0.00188791 -0.010" rpy="0 0 0"/>
+    <axis xyz="1 0 0"/>
+    <parent link="left_elbow_link"/>
+    <child link="left_wrist_roll_link"/>
+    <limit effort="25" velocity="37" lower="-1.972222054" upper="1.972222054"/>
+  </joint>
+  <link name="left_wrist_roll_link">
+    <inertial>
+      <origin xyz="0.01713944778 0.00053759094 0.00000048864" rpy="0 0 0"/>
+      <mass value="0.08544498"/>
+      <inertia ixx="0.00004821544023" ixy="-0.00000424511021" ixz="0.00000000510599" iyy="0.00003722899093" iyz="-0.00000000123525" izz="0.00005482106541"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_wrist_roll_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_wrist_roll_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_wrist_pitch_joint" type="revolute">
+    <origin xyz="0.038 0 0" rpy="0 0 0"/>
+    <axis xyz="0 1 0"/>
+    <parent link="left_wrist_roll_link"/>
+    <child link="left_wrist_pitch_link"/>
+    <limit effort="5" velocity="22" lower="-1.614429558" upper="1.614429558"/>
+  </joint>
+  <link name="left_wrist_pitch_link">
+    <inertial>
+      <origin xyz="0.02299989837 -0.00111685314 -0.00111658096" rpy="0 0 0"/>
+      <mass value="0.48404956"/>
+      <inertia ixx="0.00016579646273" ixy="-0.00001231206746" ixz="0.00001231699194" iyy="0.00042954057410" iyz="0.00000081417712" izz="0.00042953697654"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_wrist_pitch_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_wrist_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_wrist_yaw_joint" type="revolute">
+    <origin xyz="0.046 0 0" rpy="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <parent link="left_wrist_pitch_link"/>
+    <child link="left_wrist_yaw_link"/>
+    <limit effort="5" velocity="22" lower="-1.614429558" upper="1.614429558"/>
+  </joint>
+  <link name="left_wrist_yaw_link">
+    <inertial>
+      <origin xyz="0.02200381568 0.00049485096 0.00053861123" rpy="0 0 0"/>
+      <mass value="0.08457647"/>
+      <inertia ixx="0.00004929128828" ixy="-0.00000045735494" ixz="0.00000445867591" iyy="0.00005973338134" iyz="0.00000043217198" izz="0.00003928083826"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_wrist_yaw_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_wrist_yaw_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_hand_palm_joint" type="fixed">
+    <origin xyz="0.0415 0.003 0" rpy="0 0 0"/>
+    <parent link="left_wrist_yaw_link"/>
+    <child link="left_rubber_hand"/>
+  </joint>
+  <link name="left_rubber_hand">
+    <inertial>
+      <origin xyz="0.05361310808 -0.00295905240 0.00215413091" rpy="0 0 0"/>
+      <mass value="0.170"/>
+      <inertia ixx="0.00010099485234748" ixy="0.00003618590790516" ixz="-0.00000074301518642" iyy="0.00028135871571621" iyz="0.00000330189743286" izz="0.00021894770413514"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_rubber_hand.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_rubber_hand.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="right_shoulder_pitch_link">
+    <inertial>
+      <origin xyz="0 -0.035892 -0.011628" rpy="0 0 0"/>
+      <mass value="0.718"/>
+      <inertia ixx="0.0004291" ixy="9.2E-06" ixz="6.4E-06" iyy="0.000453" iyz="-2.26E-05" izz="0.000423"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_shoulder_pitch_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 -0.04 -0.01" rpy="0 1.5707963267948966 0"/>
+      <geometry>
+        <cylinder radius="0.03" length="0.05"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_shoulder_pitch_joint" type="revolute">
+    <origin xyz="0.0039563 -0.10021 0.23778" rpy="-0.27931 5.4949E-05 0.00019159"/>
+    <parent link="torso_link"/>
+    <child link="right_shoulder_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-3.0892" upper="2.6704" effort="25" velocity="37"/>
+  </joint>
+  <link name="right_shoulder_roll_link">
+    <inertial>
+      <origin xyz="-0.000227 -0.00727 -0.063243" rpy="0 0 0"/>
+      <mass value="0.643"/>
+      <inertia ixx="0.0006177" ixy="1E-06" ixz="8.7E-06" iyy="0.0006912" iyz="5.3E-06" izz="0.0003894"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_shoulder_roll_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="-0.004 -0.006 -0.053" rpy="0 0 0"/>
+      <geometry>
+        <cylinder radius="0.03" length="0.03"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_shoulder_roll_joint" type="revolute">
+    <origin xyz="0 -0.038 -0.013831" rpy="0.27925 0 0"/>
+    <parent link="right_shoulder_pitch_link"/>
+    <child link="right_shoulder_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-2.2515" upper="1.5882" effort="25" velocity="37"/>
+  </joint>
+  <link name="right_shoulder_yaw_link">
+    <inertial>
+      <origin xyz="0.010773 0.002949 -0.072009" rpy="0 0 0"/>
+      <mass value="0.734"/>
+      <inertia ixx="0.0009988" ixy="-7.9E-06" ixz="0.0001412" iyy="0.0010605" iyz="2.86E-05" izz="0.0004354"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_shoulder_yaw_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_shoulder_yaw_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_shoulder_yaw_joint" type="revolute">
+    <origin xyz="0 -0.00624 -0.1032" rpy="0 0 0"/>
+    <parent link="right_shoulder_roll_link"/>
+    <child link="right_shoulder_yaw_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-2.618" upper="2.618" effort="25" velocity="37"/>
+  </joint>
+  <link name="right_elbow_link">
+    <inertial>
+      <origin xyz="0.064956 -0.004454 -0.010062" rpy="0 0 0"/>
+      <mass value="0.6"/>
+      <inertia ixx="0.0002891" ixy="-6.53E-05" ixz="1.72E-05" iyy="0.0004152" iyz="5.6E-06" izz="0.0004197"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_elbow_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_elbow_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_elbow_joint" type="revolute">
+    <origin xyz="0.015783 0 -0.080518" rpy="0 0 0"/>
+    <parent link="right_shoulder_yaw_link"/>
+    <child link="right_elbow_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-1.0472" upper="2.0944" effort="25" velocity="37"/>
+  </joint>
+  <joint name="right_wrist_roll_joint" type="revolute">
+    <origin xyz="0.100 -0.00188791 -0.010" rpy="0 0 0"/>
+    <axis xyz="1 0 0"/>
+    <parent link="right_elbow_link"/>
+    <child link="right_wrist_roll_link"/>
+    <limit effort="25" velocity="37" lower="-1.972222054" upper="1.972222054"/>
+  </joint>
+  <link name="right_wrist_roll_link">
+    <inertial>
+      <origin xyz="0.01713944778 -0.00053759094 0.00000048864" rpy="0 0 0"/>
+      <mass value="0.08544498"/>
+      <inertia ixx="0.00004821544023" ixy="0.00000424511021" ixz="0.00000000510599" iyy="0.00003722899093" iyz="0.00000000123525" izz="0.00005482106541"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_wrist_roll_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_wrist_roll_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_wrist_pitch_joint" type="revolute">
+    <origin xyz="0.038 0 0" rpy="0 0 0"/>
+    <axis xyz="0 1 0"/>
+    <parent link="right_wrist_roll_link"/>
+    <child link="right_wrist_pitch_link"/>
+    <limit effort="5" velocity="22" lower="-1.614429558" upper="1.614429558"/>
+  </joint>
+  <link name="right_wrist_pitch_link">
+    <inertial>
+      <origin xyz="0.02299989837 0.00111685314 -0.00111658096" rpy="0 0 0"/>
+      <mass value="0.48404956"/>
+      <inertia ixx="0.00016579646273" ixy="0.00001231206746" ixz="0.00001231699194" iyy="0.00042954057410" iyz="-0.00000081417712" izz="0.00042953697654"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_wrist_pitch_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_wrist_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_wrist_yaw_joint" type="revolute">
+    <origin xyz="0.046 0 0" rpy="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <parent link="right_wrist_pitch_link"/>
+    <child link="right_wrist_yaw_link"/>
+    <limit effort="5" velocity="22" lower="-1.614429558" upper="1.614429558"/>
+  </joint>
+  <link name="right_wrist_yaw_link">
+    <inertial>
+      <origin xyz="0.02200381568 -0.00049485096 0.00053861123" rpy="0 0 0"/>
+      <mass value="0.08457647"/>
+      <inertia ixx="0.00004929128828" ixy="0.00000045735494" ixz="0.00000445867591" iyy="0.00005973338134" iyz="-0.00000043217198" izz="0.00003928083826"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_wrist_yaw_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_wrist_yaw_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_hand_palm_joint" type="fixed">
+    <origin xyz="0.0415 -0.003 0" rpy="0 0 0"/>
+    <parent link="right_wrist_yaw_link"/>
+    <child link="right_rubber_hand"/>
+  </joint>
+  <link name="right_rubber_hand">
+    <inertial>
+      <origin xyz="0.05361310808 0.00295905240 0.00215413091" rpy="0 0 0"/>
+      <mass value="0.170"/>
+      <inertia ixx="0.00010099485234748" ixy="-0.00003618590790516" ixz="-0.00000074301518642" iyy="0.00028135871571621" iyz="-0.00000330189743286" izz="0.00021894770413514"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_rubber_hand.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_rubber_hand.STL"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>

GR00T-WholeBodyControl/decoupled_wbc/control/robot_model/model_data/g1/g1_29dof_old.xml

@@ -0,0 +1,568 @@
+<mujoco model="g1_29dof">
+  <compiler angle="radian" meshdir="meshes" />
+
+  <default>
+    <default class="torso_motor">
+      <joint damping="0.05" armature="0.01" frictionloss="0.2"/>
+    </default>
+    <default class="leg_motor">
+      <joint damping="0.05" armature="0.01" frictionloss="0.2"/>
+    </default>
+    <default class="ankle_motor">
+      <joint damping="0.05" armature="0.01" frictionloss="0.2"/>
+    </default>
+    <default class="arm_motor">
+      <joint damping="0.05" armature="0.01" frictionloss="0.2"/>
+    </default>
+    <default class="wrist_motor">
+      <joint damping="0.05" armature="0.01" frictionloss="0.1"/>
+    </default>
+    
+  </default>
+
+  <asset>
+    <mesh name="pelvis" file="pelvis.STL" />
+    <mesh name="pelvis_contour_link" file="pelvis_contour_link.STL" />
+    <mesh name="left_hip_pitch_link" file="left_hip_pitch_link.STL" />
+    <mesh name="left_hip_roll_link" file="left_hip_roll_link.STL" />
+    <mesh name="left_hip_yaw_link" file="left_hip_yaw_link.STL" />
+    <mesh name="left_knee_link" file="left_knee_link.STL" />
+    <mesh name="left_ankle_pitch_link" file="left_ankle_pitch_link.STL" />
+    <mesh name="left_ankle_roll_link" file="left_ankle_roll_link.STL" />
+    <mesh name="right_hip_pitch_link" file="right_hip_pitch_link.STL" />
+    <mesh name="right_hip_roll_link" file="right_hip_roll_link.STL" />
+    <mesh name="right_hip_yaw_link" file="right_hip_yaw_link.STL" />
+    <mesh name="right_knee_link" file="right_knee_link.STL" />
+    <mesh name="right_ankle_pitch_link" file="right_ankle_pitch_link.STL" />
+    <mesh name="right_ankle_roll_link" file="right_ankle_roll_link.STL" />
+    <mesh name="waist_yaw_link" file="waist_yaw_link.STL" />
+    <mesh name="waist_roll_link" file="waist_roll_link.STL" />
+    <mesh name="torso_link" file="torso_link.STL" />
+    <mesh name="logo_link" file="logo_link.STL" />
+    <mesh name="head_link" file="head_link.STL" />
+    <mesh name="waist_support_link" file="waist_support_link.STL" />
+    <mesh name="left_shoulder_pitch_link" file="left_shoulder_pitch_link.STL" />
+    <mesh name="left_shoulder_roll_link" file="left_shoulder_roll_link.STL" />
+    <mesh name="left_shoulder_yaw_link" file="left_shoulder_yaw_link.STL" />
+    <mesh name="left_elbow_link" file="left_elbow_link.STL" />
+    <mesh name="left_wrist_roll_link" file="left_wrist_roll_link.STL" />
+    <mesh name="left_wrist_pitch_link" file="left_wrist_pitch_link.STL" />
+    <mesh name="left_wrist_yaw_link" file="left_wrist_yaw_link.STL" />
+    <mesh name="left_rubber_hand" file="left_rubber_hand.STL" />
+    <mesh name="right_shoulder_pitch_link" file="right_shoulder_pitch_link.STL" />
+    <mesh name="right_shoulder_roll_link" file="right_shoulder_roll_link.STL" />
+    <mesh name="right_shoulder_yaw_link" file="right_shoulder_yaw_link.STL" />
+    <mesh name="right_elbow_link" file="right_elbow_link.STL" />
+    <mesh name="right_wrist_roll_link" file="right_wrist_roll_link.STL" />
+    <mesh name="right_wrist_pitch_link" file="right_wrist_pitch_link.STL" />
+    <mesh name="right_wrist_yaw_link" file="right_wrist_yaw_link.STL" />
+    <mesh name="right_rubber_hand" file="right_rubber_hand.STL" />
+  </asset>
+
+  <worldbody>
+    <body name="pelvis" pos="0 0 0.793" quat="1 0 0 0">
+      <inertial pos="0 0 -0.07605" quat="1 0 -0.000399148 0" mass="3.813"
+        diaginertia="0.010549 0.0093089 0.0079184" />
+      <joint name="floating_base_joint" type="free" limited="false" actuatorfrclimited="false" />
+      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.2 0.2 0.2 1"
+        mesh="pelvis" />
+      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
+        mesh="pelvis_contour_link" />
+      <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="pelvis_contour_link" />
+      <body name="left_hip_pitch_link" pos="0 0.064452 -0.1027">
+        <inertial pos="0.002741 0.047791 -0.02606" quat="0.954862 0.293964 0.0302556 0.030122"
+          mass="1.35" diaginertia="0.00181517 0.00153422 0.00116212" />
+        <joint name="left_hip_pitch_joint" pos="0 0 0" axis="0 1 0" range="-2.5307 2.8798"
+          actuatorfrcrange="-88 88" class="leg_motor" />
+        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.2 0.2 0.2 1"
+          mesh="left_hip_pitch_link" />
+        <geom type="mesh" rgba="0.2 0.2 0.2 1" mesh="left_hip_pitch_link" />
+        <body name="left_hip_roll_link" pos="0 0.052 -0.030465" quat="0.996179 0 -0.0873386 0">
+          <inertial pos="0.029812 -0.001045 -0.087934"
+            quat="0.977808 -1.97119e-05 0.205576 -0.0403793" mass="1.52"
+            diaginertia="0.00254986 0.00241169 0.00148755" />
+          <joint name="left_hip_roll_joint" pos="0 0 0" axis="1 0 0" range="-0.5236 2.9671"
+            actuatorfrcrange="-88 88" class="leg_motor" />
+          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
+            mesh="left_hip_roll_link" />
+          <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_hip_roll_link" />
+          <body name="left_hip_yaw_link" pos="0.025001 0 -0.12412">
+            <inertial pos="-0.057709 -0.010981 -0.15078" quat="0.600598 0.15832 0.223482 0.751181"
+              mass="1.702" diaginertia="0.00776166 0.00717575 0.00160139" />
+            <joint name="left_hip_yaw_joint" pos="0 0 0" axis="0 0 1" range="-2.7576 2.7576"
+              actuatorfrcrange="-88 88" class="leg_motor" />
+            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
+              mesh="left_hip_yaw_link" />
+            <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_hip_yaw_link" />
+            <body name="left_knee_link" pos="-0.078273 0.0021489 -0.17734"
+              quat="0.996179 0 0.0873386 0">
+              <inertial pos="0.005457 0.003964 -0.12074"
+                quat="0.923418 -0.0327699 0.0158246 0.382067" mass="1.932"
+                diaginertia="0.0113804 0.0112778 0.00146458" />
+              <joint name="left_knee_joint" pos="0 0 0" axis="0 1 0" range="-0.087267 2.8798"
+                actuatorfrcrange="-139 139" class="leg_motor" />
+              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                rgba="0.7 0.7 0.7 1" mesh="left_knee_link" />
+              <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_knee_link" />
+              <body name="left_ankle_pitch_link" pos="0 -9.4445e-05 -0.30001">
+                <inertial pos="-0.007269 0 0.011137" quat="0.603053 0.369225 0.369225 0.603053"
+                  mass="0.074" diaginertia="1.89e-05 1.40805e-05 6.9195e-06" />
+                <joint name="left_ankle_pitch_joint" pos="0 0 0" axis="0 1 0"
+                  range="-0.87267 0.5236" actuatorfrcrange="-50 50"  class="ankle_motor"/>
+                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                  rgba="0.7 0.7 0.7 1" mesh="left_ankle_pitch_link" />
+                <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_ankle_pitch_link" />
+                <body name="left_ankle_roll_link" pos="0 0 -0.017558">
+                  <inertial pos="0.026505 0 -0.016425"
+                    quat="-0.000481092 0.728482 -0.000618967 0.685065" mass="0.608"
+                    diaginertia="0.00167218 0.0016161 0.000217621" />
+                  <joint name="left_ankle_roll_joint" pos="0 0 0" axis="1 0 0"
+                    range="-0.2618 0.2618" actuatorfrcrange="-50 50" class="ankle_motor"/>
+                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                    rgba="0.2 0.2 0.2 1" mesh="left_ankle_roll_link" />
+                  <body name="dummy_lf_1" >
+                      <inertial pos="0 0 0" mass="0.0001" diaginertia="0 0 0" />
+                      <geom size="0.005" pos="-0.05 0.025 -0.03" rgba="0.2 0.2 0.2 1" />
+                      <site name="left_foot_contact_1" pos="-0.05 0.025 -0.03" size="0.005" />
+                  </body>
+                  <body name="dummy_lf_2" >
+                      <inertial pos="0 0 0" mass="0.0001" diaginertia="0 0 0" />
+                      <geom size="0.005" pos="-0.05 -0.025 -0.03" rgba="0.2 0.2 0.2 1" />
+                      <site name="left_foot_contact_2" pos="-0.05 -0.025 -0.03" size="0.005" />
+                  </body>
+                  <body name="dummy_lf_3" >
+                      <inertial pos="0 0 0" mass="0.0001" diaginertia="0 0 0" />
+                      <geom size="0.005" pos="0.12 0.03 -0.03" rgba="0.2 0.2 0.2 1" />
+                      <site name="left_foot_contact_3" pos="0.12 0.03 -0.03" size="0.005" />
+                  </body>
+                  <body name="dummy_lf_4" >
+                      <inertial pos="0 0 0" mass="0.0001" diaginertia="0 0 0" />
+                      <geom size="0.005" pos="0.12 -0.03 -0.03" rgba="0.2 0.2 0.2 1" />
+                      <site name="left_foot_contact_4" pos="0.12 -0.03 -0.03" size="0.005" />
+                  </body>
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+      <body name="right_hip_pitch_link" pos="0 -0.064452 -0.1027">
+        <inertial pos="0.002741 -0.047791 -0.02606" quat="0.954862 -0.293964 0.0302556 -0.030122"
+          mass="1.35" diaginertia="0.00181517 0.00153422 0.00116212" />
+        <joint name="right_hip_pitch_joint" pos="0 0 0" axis="0 1 0" range="-2.5307 2.8798"
+          actuatorfrcrange="-88 88" class="leg_motor" />
+        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.2 0.2 0.2 1"
+          mesh="right_hip_pitch_link" />
+        <geom type="mesh" rgba="0.2 0.2 0.2 1" mesh="right_hip_pitch_link" />
+        <body name="right_hip_roll_link" pos="0 -0.052 -0.030465" quat="0.996179 0 -0.0873386 0">
+          <inertial pos="0.029812 0.001045 -0.087934" quat="0.977808 1.97119e-05 0.205576 0.0403793"
+            mass="1.52" diaginertia="0.00254986 0.00241169 0.00148755" />
+          <joint name="right_hip_roll_joint" pos="0 0 0" axis="1 0 0" range="-2.9671 0.5236"
+            actuatorfrcrange="-88 88" class="leg_motor" />
+          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
+            mesh="right_hip_roll_link" />
+          <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_hip_roll_link" />
+          <body name="right_hip_yaw_link" pos="0.025001 0 -0.12412">
+            <inertial pos="-0.057709 0.010981 -0.15078" quat="0.751181 0.223482 0.15832 0.600598"
+              mass="1.702" diaginertia="0.00776166 0.00717575 0.00160139" />
+            <joint name="right_hip_yaw_joint" pos="0 0 0" axis="0 0 1" range="-2.7576 2.7576"
+              actuatorfrcrange="-88 88" class="leg_motor" />
+            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
+              mesh="right_hip_yaw_link" />
+            <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_hip_yaw_link" />
+            <body name="right_knee_link" pos="-0.078273 -0.0021489 -0.17734"
+              quat="0.996179 0 0.0873386 0">
+              <inertial pos="0.005457 -0.003964 -0.12074"
+                quat="0.923439 0.0345276 0.0116333 -0.382012" mass="1.932"
+                diaginertia="0.011374 0.0112843 0.00146452" />
+              <joint name="right_knee_joint" pos="0 0 0" axis="0 1 0" range="-0.087267 2.8798"
+                actuatorfrcrange="-139 139" class="leg_motor" />
+              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                rgba="0.7 0.7 0.7 1" mesh="right_knee_link" />
+              <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_knee_link" />
+              <body name="right_ankle_pitch_link" pos="0 9.4445e-05 -0.30001">
+                <inertial pos="-0.007269 0 0.011137" quat="0.603053 0.369225 0.369225 0.603053"
+                  mass="0.074" diaginertia="1.89e-05 1.40805e-05 6.9195e-06" />
+                <joint name="right_ankle_pitch_joint" pos="0 0 0" axis="0 1 0"
+                  range="-0.87267 0.5236" actuatorfrcrange="-50 50" class="ankle_motor" />
+                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                  rgba="0.7 0.7 0.7 1" mesh="right_ankle_pitch_link" />
+                <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_ankle_pitch_link" />
+                <body name="right_ankle_roll_link" pos="0 0 -0.017558">
+                  <inertial pos="0.026505 0 -0.016425"
+                    quat="0.000481092 0.728482 0.000618967 0.685065" mass="0.608"
+                    diaginertia="0.00167218 0.0016161 0.000217621" />
+                  <joint name="right_ankle_roll_joint" pos="0 0 0" axis="1 0 0"
+                    range="-0.2618 0.2618" actuatorfrcrange="-50 50" class="ankle_motor" />
+                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                    rgba="0.2 0.2 0.2 1" mesh="right_ankle_roll_link" />
+                  <geom size="0.005" pos="-0.05 0.025 -0.03" rgba="0.2 0.2 0.2 1" />
+                  <geom size="0.005" pos="-0.05 -0.025 -0.03" rgba="0.2 0.2 0.2 1" />
+                  <geom size="0.005" pos="0.12 0.03 -0.03" rgba="0.2 0.2 0.2 1" />
+                  <geom size="0.005" pos="0.12 -0.03 -0.03" rgba="0.2 0.2 0.2 1" />
+                  <body name="dummy_rf_1" >
+                      <inertial pos="0 0 0" mass="0.0001" diaginertia="0 0 0" />
+                      <site name="right_foot_contact_1" pos="-0.05 0.025 -0.03" size="0.005" />
+                  </body>
+                  <body name="dummy_rf_2" >
+                      <inertial pos="0 0 0" mass="0.0001" diaginertia="0 0 0" />
+                      <site name="right_foot_contact_2" pos="-0.05 -0.025 -0.03" size="0.005" />
+                  </body>
+                  <body name="dummy_rf_3" >
+                      <inertial pos="0 0 0" mass="0.0001" diaginertia="0 0 0" />
+                      <site name="right_foot_contact_3" pos="0.12 0.03 -0.03" size="0.005" />
+                  </body>
+                  <body name="dummy_rf_4" >
+                      <inertial pos="0 0 0" mass="0.0001" diaginertia="0 0 0" />
+                      <site name="right_foot_contact_4" pos="0.12 -0.03 -0.03" size="0.005" />
+                  </body>
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+      <body name="waist_yaw_link">
+        <inertial pos="0.003964 0 0.018769" quat="-0.0178291 0.628464 0.0282471 0.777121"
+          mass="0.244" diaginertia="0.000158561 0.000124229 9.67669e-05" />
+        <joint name="waist_yaw_joint" pos="0 0 0" axis="0 0 1" range="-2.618 2.618"
+          actuatorfrcrange="-88 88" class="torso_motor"/>
+        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
+          mesh="waist_yaw_link" />
+        <body name="waist_roll_link" pos="-0.0039635 0 0.035">
+          <inertial pos="0 -0.000236 0.010111" quat="0.99979 0.020492 0 0" mass="0.047"
+            diaginertia="7.515e-06 6.40206e-06 3.98394e-06" />
+          <joint name="waist_roll_joint" pos="0 0 0" axis="1 0 0" range="-0.52 0.52"
+            actuatorfrcrange="-50 50" class="torso_motor"/>
+          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
+            mesh="waist_roll_link" />
+          <body name="torso_link" pos="0 0 0.019">
+            <camera name="head_camera" pos="0.06 0.0 0.45" euler="0 -0.8 -1.57"/>
+            <inertial pos="0.00331658 0.000261533 0.179856"
+              quat="0.999831 0.000376204 0.0179895 -0.00377704" mass="9.598"
+              diaginertia="0.12407 0.111951 0.0325382" />
+            <joint name="waist_pitch_joint" pos="0 0 0" axis="0 1 0" range="-0.52 0.52"
+              actuatorfrcrange="-50 50" class="torso_motor"/>
+            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
+              mesh="torso_link" />
+            <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="torso_link" />
+            <geom pos="0.0039635 0 -0.054" quat="1 0 0 0" type="mesh" contype="0" conaffinity="0"
+              group="1" density="0" rgba="0.2 0.2 0.2 1" mesh="logo_link" />
+            <geom pos="0.0039635 0 -0.054" quat="1 0 0 0" type="mesh" rgba="0.2 0.2 0.2 1"
+              mesh="logo_link" />
+            <geom pos="0.0039635 0 -0.054" type="mesh" contype="0" conaffinity="0" group="1"
+              density="0" rgba="0.2 0.2 0.2 1" mesh="head_link" />
+            <geom pos="0.0039635 0 -0.054" type="mesh" rgba="0.2 0.2 0.2 1" mesh="head_link" />
+            <geom pos="0.0039635 0 -0.054" quat="1 0 0 0" type="mesh" contype="0" conaffinity="0"
+              group="1" density="0" rgba="0.7 0.7 0.7 1" mesh="waist_support_link" />
+            <geom pos="0.0039635 0 -0.054" quat="1 0 0 0" type="mesh" rgba="0.7 0.7 0.7 1"
+              mesh="waist_support_link" />
+            <site name="imu" size="0.01" pos="-0.03959 -0.00224 0.13792" />
+            <body name="left_shoulder_pitch_link" pos="0.0039563 0.10022 0.23778"
+              quat="0.990264 0.139201 1.38722e-05 -9.86868e-05">
+              <inertial pos="0 0.035892 -0.011628" quat="0.654152 0.0130458 -0.326267 0.68225"
+                mass="0.718" diaginertia="0.000465864 0.000432842 0.000406394" />
+              <joint name="left_shoulder_pitch_joint" pos="0 0 0" axis="0 1 0"
+                range="-3.0892 2.6704" actuatorfrcrange="-25 25" class="arm_motor" />
+              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                rgba="0.7 0.7 0.7 1" mesh="left_shoulder_pitch_link" />
+              <geom size="0.03 0.025" pos="0 0.04 -0.01" quat="0.707107 0 0.707107 0"
+                type="cylinder" rgba="0.7 0.7 0.7 1" />
+              <body name="left_shoulder_roll_link" pos="0 0.038 -0.013831"
+                quat="0.990268 -0.139172 0 0">
+                <inertial pos="-0.000227 0.00727 -0.063243"
+                  quat="0.701256 -0.0196223 -0.00710317 0.712604" mass="0.643"
+                  diaginertia="0.000691311 0.000618011 0.000388977" />
+                <joint name="left_shoulder_roll_joint" pos="0 0 0" axis="1 0 0"
+                  range="-1.5882 2.2515" actuatorfrcrange="-25 25" class="arm_motor" />
+                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                  rgba="0.7 0.7 0.7 1" mesh="left_shoulder_roll_link" />
+                <geom size="0.03 0.015" pos="-0.004 0.006 -0.053" type="cylinder"
+                  rgba="0.7 0.7 0.7 1" />
+                <body name="left_shoulder_yaw_link" pos="0 0.00624 -0.1032">
+                  <inertial pos="0.010773 -0.002949 -0.072009"
+                    quat="0.716879 -0.0964829 -0.0679942 0.687134" mass="0.734"
+                    diaginertia="0.00106187 0.00103217 0.000400661" />
+                  <joint name="left_shoulder_yaw_joint" pos="0 0 0" axis="0 0 1"
+                    range="-2.618 2.618" actuatorfrcrange="-25 25" class="arm_motor" />
+                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                    rgba="0.7 0.7 0.7 1" mesh="left_shoulder_yaw_link" />
+                  <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_shoulder_yaw_link" />
+                  <body name="left_elbow_link" pos="0.015783 0 -0.080518">
+                    <inertial pos="0.064956 0.004454 -0.010062"
+                      quat="0.541765 0.636132 0.388821 0.388129" mass="0.6"
+                      diaginertia="0.000443035 0.000421612 0.000259353" />
+                    <joint name="left_elbow_joint" pos="0 0 0" axis="0 1 0" range="-1.0472 2.0944"
+                      actuatorfrcrange="-25 25" class="arm_motor" />
+                    <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                      rgba="0.7 0.7 0.7 1" mesh="left_elbow_link" />
+                    <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_elbow_link" />
+                    <body name="left_wrist_roll_link" pos="0.1 0.00188791 -0.01">
+                      <inertial pos="0.0171394 0.000537591 4.8864e-07"
+                        quat="0.575338 0.411667 -0.574906 0.411094" mass="0.085445"
+                        diaginertia="5.48211e-05 4.96646e-05 3.57798e-05" />
+                      <joint name="left_wrist_roll_joint" pos="0 0 0" axis="1 0 0"
+                        range="-1.97222 1.97222" actuatorfrcrange="-25 25" class="arm_motor" />
+                      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                        rgba="0.7 0.7 0.7 1" mesh="left_wrist_roll_link" />
+                      <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_wrist_roll_link" />
+                      <body name="left_wrist_pitch_link" pos="0.038 0 0">
+                        <inertial pos="0.0229999 -0.00111685 -0.00111658"
+                          quat="0.249998 0.661363 0.293036 0.643608" mass="0.48405"
+                          diaginertia="0.000430353 0.000429873 0.000164648" />
+                        <joint name="left_wrist_pitch_joint" pos="0 0 0" axis="0 1 0"
+                          range="-1.61443 1.61443" actuatorfrcrange="-5 5" class="wrist_motor" />
+                        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                          rgba="0.7 0.7 0.7 1" mesh="left_wrist_pitch_link" />
+                        <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_wrist_pitch_link" />
+                        <body name="left_wrist_yaw_link" pos="0.046 0 0">
+                          <inertial pos="0.0708244 0.000191745 0.00161742"
+                            quat="0.510571 0.526295 0.468078 0.493188" mass="0.254576"
+                            diaginertia="0.000646113 0.000559993 0.000147566" />
+                          <joint name="left_wrist_yaw_joint" pos="0 0 0" axis="0 0 1"
+                            range="-1.61443 1.61443" actuatorfrcrange="-5 5" class="wrist_motor" />
+                          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                            rgba="0.7 0.7 0.7 1" mesh="left_wrist_yaw_link" />
+                          <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_wrist_yaw_link" />
+                          <body name="left_rubber_hand" >
+                            <inertial pos="0.0001 0.0001 0.0001" mass="0.0001" diaginertia="0 0 0" />
+                            <geom pos="0.0415 0.003 0" quat="1 0 0 0" type="mesh"  density="0" rgba="0.7 0.7 0.7 1"
+                              mesh="left_rubber_hand" />
+                          </body>
+                        </body>
+                      </body>
+                    </body>
+                  </body>
+                </body>
+              </body>
+            </body>
+            <body name="right_shoulder_pitch_link" pos="0.0039563 -0.10021 0.23778"
+              quat="0.990264 -0.139201 1.38722e-05 9.86868e-05">
+              <inertial pos="0 -0.035892 -0.011628" quat="0.68225 -0.326267 0.0130458 0.654152"
+                mass="0.718" diaginertia="0.000465864 0.000432842 0.000406394" />
+              <joint name="right_shoulder_pitch_joint" pos="0 0 0" axis="0 1 0"
+                range="-3.0892 2.6704" actuatorfrcrange="-25 25" class="arm_motor" />
+              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                rgba="0.7 0.7 0.7 1" mesh="right_shoulder_pitch_link" />
+              <geom size="0.03 0.025" pos="0 -0.04 -0.01" quat="0.707107 0 0.707107 0"
+                type="cylinder" rgba="0.7 0.7 0.7 1" />
+              <body name="right_shoulder_roll_link" pos="0 -0.038 -0.013831"
+                quat="0.990268 0.139172 0 0">
+                <inertial pos="-0.000227 -0.00727 -0.063243"
+                  quat="0.712604 -0.00710317 -0.0196223 0.701256" mass="0.643"
+                  diaginertia="0.000691311 0.000618011 0.000388977" />
+                <joint name="right_shoulder_roll_joint" pos="0 0 0" axis="1 0 0"
+                  range="-2.2515 1.5882" actuatorfrcrange="-25 25" class="arm_motor" />
+                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                  rgba="0.7 0.7 0.7 1" mesh="right_shoulder_roll_link" />
+                <geom size="0.03 0.015" pos="-0.004 -0.006 -0.053" type="cylinder"
+                  rgba="0.7 0.7 0.7 1" />
+                <body name="right_shoulder_yaw_link" pos="0 -0.00624 -0.1032">
+                  <inertial pos="0.010773 0.002949 -0.072009"
+                    quat="0.687134 -0.0679942 -0.0964829 0.716879" mass="0.734"
+                    diaginertia="0.00106187 0.00103217 0.000400661" />
+                  <joint name="right_shoulder_yaw_joint" pos="0 0 0" axis="0 0 1"
+                    range="-2.618 2.618" actuatorfrcrange="-25 25" class="arm_motor" />
+                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                    rgba="0.7 0.7 0.7 1" mesh="right_shoulder_yaw_link" />
+                  <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_shoulder_yaw_link" />
+                  <body name="right_elbow_link" pos="0.015783 0 -0.080518">
+                    <inertial pos="0.064956 -0.004454 -0.010062"
+                      quat="0.388129 0.388821 0.636132 0.541765" mass="0.6"
+                      diaginertia="0.000443035 0.000421612 0.000259353" />
+                    <joint name="right_elbow_joint" pos="0 0 0" axis="0 1 0" range="-1.0472 2.0944"
+                      actuatorfrcrange="-25 25" class="arm_motor" />
+                    <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                      rgba="0.7 0.7 0.7 1" mesh="right_elbow_link" />
+                    <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_elbow_link" />
+                    <body name="right_wrist_roll_link" pos="0.1 -0.00188791 -0.01">
+                      <inertial pos="0.0171394 -0.000537591 4.8864e-07"
+                        quat="0.411667 0.575338 -0.411094 0.574906" mass="0.085445"
+                        diaginertia="5.48211e-05 4.96646e-05 3.57798e-05" />
+                      <joint name="right_wrist_roll_joint" pos="0 0 0" axis="1 0 0"
+                        range="-1.97222 1.97222" actuatorfrcrange="-25 25" class="arm_motor" />
+                      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                        rgba="0.7 0.7 0.7 1" mesh="right_wrist_roll_link" />
+                      <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_wrist_roll_link" />
+                      <body name="right_wrist_pitch_link" pos="0.038 0 0">
+                        <inertial pos="0.0229999 0.00111685 -0.00111658"
+                          quat="0.643608 0.293036 0.661363 0.249998" mass="0.48405"
+                          diaginertia="0.000430353 0.000429873 0.000164648" />
+                        <joint name="right_wrist_pitch_joint" pos="0 0 0" axis="0 1 0"
+                          range="-1.61443 1.61443" actuatorfrcrange="-5 5" class="wrist_motor" />
+                        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                          rgba="0.7 0.7 0.7 1" mesh="right_wrist_pitch_link" />
+                        <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_wrist_pitch_link" />
+                        <body name="right_wrist_yaw_link" pos="0.046 0 0">
+                          <inertial pos="0.0708244 -0.000191745 0.00161742"
+                            quat="0.493188 0.468078 0.526295 0.510571" mass="0.254576"
+                            diaginertia="0.000646113 0.000559993 0.000147566" />
+                          <joint name="right_wrist_yaw_joint" pos="0 0 0" axis="0 0 1"
+                            range="-1.61443 1.61443" actuatorfrcrange="-5 5" class="wrist_motor" />
+                          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                            rgba="0.7 0.7 0.7 1" mesh="right_wrist_yaw_link" />
+                          <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_wrist_yaw_link" />
+                          <body name="right_rubber_hand" >
+                            <inertial pos="0.0001 0.0001 0.0001" mass="0.0001" diaginertia="0 0 0" />
+                            <geom pos="0.0415 -0.003 0" quat="1 0 0 0" type="mesh"  density="0" rgba="0.7 0.7 0.7 1"
+                              mesh="right_rubber_hand" />
+                          </body>
+                        </body>
+                      </body>
+                    </body>
+                  </body>
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+    </body>
+  </worldbody>
+
+  <actuator>
+    <motor name="left_hip_pitch" joint="left_hip_pitch_joint" ctrlrange="-88 88" />
+    <motor name="left_hip_roll" joint="left_hip_roll_joint" ctrlrange="-88 88" />
+    <motor name="left_hip_yaw" joint="left_hip_yaw_joint" ctrlrange="-88 88" />
+    <motor name="left_knee" joint="left_knee_joint" ctrlrange="-139 139" />
+    <motor name="left_ankle_pitch" joint="left_ankle_pitch_joint" ctrlrange="-50 50" />
+    <motor name="left_ankle_roll" joint="left_ankle_roll_joint" ctrlrange="-50 50" />
+
+    <motor name="right_hip_pitch" joint="right_hip_pitch_joint" ctrlrange="-88 88" />
+    <motor name="right_hip_roll" joint="right_hip_roll_joint" ctrlrange="-88 88" />
+    <motor name="right_hip_yaw" joint="right_hip_yaw_joint" ctrlrange="-88 88" />
+    <motor name="right_knee" joint="right_knee_joint" ctrlrange="-139 139" />
+    <motor name="right_ankle_pitch" joint="right_ankle_pitch_joint" ctrlrange="-50 50" />
+    <motor name="right_ankle_roll" joint="right_ankle_roll_joint" ctrlrange="-50 50" />
+
+    <motor name="waist_yaw" joint="waist_yaw_joint" ctrlrange="-88 88" />
+    <motor name="waist_roll" joint="waist_roll_joint" ctrlrange="-50 50" />
+    <motor name="waist_pitch" joint="waist_pitch_joint" ctrlrange="-50 50" />
+
+    <motor name="left_shoulder_pitch" joint="left_shoulder_pitch_joint" ctrlrange="-25 25" />
+    <motor name="left_shoulder_roll" joint="left_shoulder_roll_joint" ctrlrange="-25 25" />
+    <motor name="left_shoulder_yaw" joint="left_shoulder_yaw_joint" ctrlrange="-25 25" />
+    <motor name="left_elbow" joint="left_elbow_joint" ctrlrange="-25 25" />
+    <motor name="left_wrist_roll" joint="left_wrist_roll_joint" ctrlrange="-25 25" />
+    <motor name="left_wrist_pitch" joint="left_wrist_pitch_joint" ctrlrange="-5 5" />
+    <motor name="left_wrist_yaw" joint="left_wrist_yaw_joint" ctrlrange="-5 5" />
+
+    <motor name="right_shoulder_pitch" joint="right_shoulder_pitch_joint" ctrlrange="-25 25" />
+    <motor name="right_shoulder_roll" joint="right_shoulder_roll_joint" ctrlrange="-25 25" />
+    <motor name="right_shoulder_yaw" joint="right_shoulder_yaw_joint" ctrlrange="-25 25" />
+    <motor name="right_elbow" joint="right_elbow_joint" ctrlrange="-25 25" />
+    <motor name="right_wrist_roll" joint="right_wrist_roll_joint" ctrlrange="-25 25" />
+    <motor name="right_wrist_pitch" joint="right_wrist_pitch_joint" ctrlrange="-5 5" />
+    <motor name="right_wrist_yaw" joint="right_wrist_yaw_joint" ctrlrange="-5 5" />
+  </actuator>
+
+  <sensor>
+    <jointpos name="left_hip_pitch_pos" joint="left_hip_pitch_joint" />
+    <jointpos name="left_hip_roll_pos" joint="left_hip_roll_joint" />
+    <jointpos name="left_hip_yaw_pos" joint="left_hip_yaw_joint" />
+    <jointpos name="left_knee_pos" joint="left_knee_joint" />
+    <jointpos name="left_ankle_pitch_pos" joint="left_ankle_pitch_joint" />
+    <jointpos name="left_ankle_roll_pos" joint="left_ankle_roll_joint" />
+    <jointpos name="right_hip_pitch_pos" joint="right_hip_pitch_joint" />
+    <jointpos name="right_hip_roll_pos" joint="right_hip_roll_joint" />
+    <jointpos name="right_hip_yaw_pos" joint="right_hip_yaw_joint" />
+    <jointpos name="right_knee_pos" joint="right_knee_joint" />
+    <jointpos name="right_ankle_pitch_pos" joint="right_ankle_pitch_joint" />
+    <jointpos name="right_ankle_roll_pos" joint="right_ankle_roll_joint" />
+    <jointpos name="waist_yaw_pos" joint="waist_yaw_joint" />
+    <jointpos name="waist_roll_pos" joint="waist_roll_joint" />
+    <jointpos name="waist_pitch_pos" joint="waist_pitch_joint" />
+    <jointpos name="left_shoulder_pitch_pos" joint="left_shoulder_pitch_joint" />
+    <jointpos name="left_shoulder_roll_pos" joint="left_shoulder_roll_joint" />
+    <jointpos name="left_shoulder_yaw_pos" joint="left_shoulder_yaw_joint" />
+    <jointpos name="left_elbow_pos" joint="left_elbow_joint" />
+    <jointpos name="left_wrist_roll_pos" joint="left_wrist_roll_joint" />
+    <jointpos name="left_wrist_pitch_pos" joint="left_wrist_pitch_joint" />
+    <jointpos name="left_wrist_yaw_pos" joint="left_wrist_yaw_joint" />
+    <jointpos name="right_shoulder_pitch_pos" joint="right_shoulder_pitch_joint" />
+    <jointpos name="right_shoulder_roll_pos" joint="right_shoulder_roll_joint" />
+    <jointpos name="right_shoulder_yaw_pos" joint="right_shoulder_yaw_joint" />
+    <jointpos name="right_elbow_pos" joint="right_elbow_joint" />
+    <jointpos name="right_wrist_roll_pos" joint="right_wrist_roll_joint" />
+    <jointpos name="right_wrist_pitch_pos" joint="right_wrist_pitch_joint" />
+    <jointpos name="right_wrist_yaw_pos" joint="right_wrist_yaw_joint" />
+
+    <jointvel name="left_hip_pitch_vel" joint="left_hip_pitch_joint" />
+    <jointvel name="left_hip_roll_vel" joint="left_hip_roll_joint" />
+    <jointvel name="left_hip_yaw_vel" joint="left_hip_yaw_joint" />
+    <jointvel name="left_knee_vel" joint="left_knee_joint" />
+    <jointvel name="left_ankle_pitch_vel" joint="left_ankle_pitch_joint" />
+    <jointvel name="left_ankle_roll_vel" joint="left_ankle_roll_joint" />
+    <jointvel name="right_hip_pitch_vel" joint="right_hip_pitch_joint" />
+    <jointvel name="right_hip_roll_vel" joint="right_hip_roll_joint" />
+    <jointvel name="right_hip_yaw_vel" joint="right_hip_yaw_joint" />
+    <jointvel name="right_knee_vel" joint="right_knee_joint" />
+    <jointvel name="right_ankle_pitch_vel" joint="right_ankle_pitch_joint" />
+    <jointvel name="right_ankle_roll_vel" joint="right_ankle_roll_joint" />
+    <jointvel name="waist_yaw_vel" joint="waist_yaw_joint" />
+    <jointvel name="waist_roll_vel" joint="waist_roll_joint" />
+    <jointvel name="waist_pitch_vel" joint="waist_pitch_joint" />
+    <jointvel name="left_shoulder_pitch_vel" joint="left_shoulder_pitch_joint" />
+    <jointvel name="left_shoulder_roll_vel" joint="left_shoulder_roll_joint" />
+    <jointvel name="left_shoulder_yaw_vel" joint="left_shoulder_yaw_joint" />
+    <jointvel name="left_elbow_vel" joint="left_elbow_joint" />
+    <jointvel name="left_wrist_roll_vel" joint="left_wrist_roll_joint" />
+    <jointvel name="left_wrist_pitch_vel" joint="left_wrist_pitch_joint" />
+    <jointvel name="left_wrist_yaw_vel" joint="left_wrist_yaw_joint" />
+    <jointvel name="right_shoulder_pitch_vel" joint="right_shoulder_pitch_joint" />
+    <jointvel name="right_shoulder_roll_vel" joint="right_shoulder_roll_joint" />
+    <jointvel name="right_shoulder_yaw_vel" joint="right_shoulder_yaw_joint" />
+    <jointvel name="right_elbow_vel" joint="right_elbow_joint" />
+    <jointvel name="right_wrist_roll_vel" joint="right_wrist_roll_joint" />
+    <jointvel name="right_wrist_pitch_vel" joint="right_wrist_pitch_joint" />
+    <jointvel name="right_wrist_yaw_vel" joint="right_wrist_yaw_joint" />
+
+    <jointactuatorfrc name="left_hip_pitch_torque" joint="left_hip_pitch_joint" />
+    <jointactuatorfrc name="left_hip_roll_torque" joint="left_hip_roll_joint" />
+    <jointactuatorfrc name="left_hip_yaw_torque" joint="left_hip_yaw_joint" />
+    <jointactuatorfrc name="left_knee_torque" joint="left_knee_joint" />
+    <jointactuatorfrc name="left_ankle_pitch_torque" joint="left_ankle_pitch_joint" />
+    <jointactuatorfrc name="left_ankle_roll_torque" joint="left_ankle_roll_joint" />
+    <jointactuatorfrc name="right_hip_pitch_torque" joint="right_hip_pitch_joint" />
+    <jointactuatorfrc name="right_hip_roll_torque" joint="right_hip_roll_joint" />
+    <jointactuatorfrc name="right_hip_yaw_torque" joint="right_hip_yaw_joint" />
+    <jointactuatorfrc name="right_knee_torque" joint="right_knee_joint" />
+    <jointactuatorfrc name="right_ankle_pitch_torque" joint="right_ankle_pitch_joint" />
+    <jointactuatorfrc name="right_ankle_roll_torque" joint="right_ankle_roll_joint" />
+    <jointactuatorfrc name="waist_yaw_torque" joint="waist_yaw_joint" />
+    <jointactuatorfrc name="waist_roll_torque" joint="waist_roll_joint" />
+    <jointactuatorfrc name="waist_pitch_torque" joint="waist_pitch_joint" />
+    <jointactuatorfrc name="left_shoulder_pitch_torque" joint="left_shoulder_pitch_joint" />
+    <jointactuatorfrc name="left_shoulder_roll_torque" joint="left_shoulder_roll_joint" />
+    <jointactuatorfrc name="left_shoulder_yaw_torque" joint="left_shoulder_yaw_joint" />
+    <jointactuatorfrc name="left_elbow_torque" joint="left_elbow_joint" />
+    <jointactuatorfrc name="left_wrist_roll_torque" joint="left_wrist_roll_joint" />
+    <jointactuatorfrc name="left_wrist_pitch_torque" joint="left_wrist_pitch_joint" />
+    <jointactuatorfrc name="left_wrist_yaw_torque" joint="left_wrist_yaw_joint" />
+    <jointactuatorfrc name="right_shoulder_pitch_torque" joint="right_shoulder_pitch_joint" />
+    <jointactuatorfrc name="right_shoulder_roll_torque" joint="right_shoulder_roll_joint" />
+    <jointactuatorfrc name="right_shoulder_yaw_torque" joint="right_shoulder_yaw_joint" />
+    <jointactuatorfrc name="right_elbow_torque" joint="right_elbow_joint" />
+    <jointactuatorfrc name="right_wrist_roll_torque" joint="right_wrist_roll_joint" />
+    <jointactuatorfrc name="right_wrist_pitch_torque" joint="right_wrist_pitch_joint" />
+    <jointactuatorfrc name="right_wrist_yaw_torque" joint="right_wrist_yaw_joint" />
+
+    <framequat name="imu_quat" objtype="site" objname="imu" />
+    <gyro name="imu_gyro" site="imu" />
+    <accelerometer name="imu_acc" site="imu" />
+
+    <framepos name="frame_pos" objtype="site" objname="imu" />
+    <framelinvel name="frame_vel" objtype="site" objname="imu" />
+  </sensor>
+  <sensor>
+      <force site="left_foot_contact_1" />
+      <force site="left_foot_contact_2" />
+      <force site="left_foot_contact_3" />
+      <force site="left_foot_contact_4" />
+      <force site="right_foot_contact_1" />
+      <force site="right_foot_contact_2" />
+      <force site="right_foot_contact_3" />
+      <force site="right_foot_contact_4" />
+  </sensor>
+
+</mujoco>

GR00T-WholeBodyControl/decoupled_wbc/control/robot_model/model_data/g1/g1_29dof_with_hand.urdf

@@ -0,0 +1,1497 @@
+<robot name="g1_29dof_with_hand">
+  <mujoco>
+    <compiler meshdir="meshes" discardvisual="false"/>
+  </mujoco>
+
+  <!-- [CAUTION] uncomment when convert to mujoco -->
+  <!-- <link name="world"></link>
+  <joint name="floating_base_joint" type="floating">
+    <parent link="world"/>
+    <child link="pelvis"/>
+  </joint> -->
+
+  <link name="pelvis">
+    <inertial>
+      <origin xyz="0 0 -0.07605" rpy="0 0 0"/>
+      <mass value="3.813"/>
+      <inertia ixx="0.010549" ixy="0" ixz="2.1E-06" iyy="0.0093089" iyz="0" izz="0.0079184"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/pelvis.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="pelvis_contour_link">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="0.001"/>
+      <inertia ixx="1e-7" ixy="0" ixz="0" iyy="1e-7" iyz="0" izz="1e-7"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/pelvis_contour_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/pelvis_contour_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="pelvis_contour_joint" type="fixed">
+    <parent link="pelvis"/>
+    <child link="pelvis_contour_link"/>
+  </joint>
+
+  <!-- Legs -->
+  <link name="left_hip_pitch_link">
+    <inertial>
+      <origin xyz="0.002741 0.047791 -0.02606" rpy="0 0 0"/>
+      <mass value="1.35"/>
+      <inertia ixx="0.001811" ixy="3.68E-05" ixz="-3.44E-05" iyy="0.0014193" iyz="0.000171" izz="0.0012812"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_hip_pitch_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_hip_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_hip_pitch_joint" type="revolute">
+    <origin xyz="0 0.064452 -0.1027" rpy="0 0 0"/>
+    <parent link="pelvis"/>
+    <child link="left_hip_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-2.5307" upper="2.8798" effort="88" velocity="32"/>
+  </joint>
+  <link name="left_hip_roll_link">
+    <inertial>
+      <origin xyz="0.029812 -0.001045 -0.087934" rpy="0 0 0"/>
+      <mass value="1.52"/>
+      <inertia ixx="0.0023773" ixy="-3.8E-06" ixz="-0.0003908" iyy="0.0024123" iyz="1.84E-05" izz="0.0016595"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_hip_roll_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_hip_roll_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_hip_roll_joint" type="revolute">
+    <origin xyz="0 0.052 -0.030465" rpy="0 -0.1749 0"/>
+    <parent link="left_hip_pitch_link"/>
+    <child link="left_hip_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-0.5236" upper="2.9671" effort="88" velocity="32"/>
+  </joint>
+  <link name="left_hip_yaw_link">
+    <inertial>
+      <origin xyz="-0.057709 -0.010981 -0.15078" rpy="0 0 0"/>
+      <mass value="1.702"/>
+      <inertia ixx="0.0057774" ixy="-0.0005411" ixz="-0.0023948" iyy="0.0076124" iyz="-0.0007072" izz="0.003149"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_hip_yaw_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_hip_yaw_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_hip_yaw_joint" type="revolute">
+    <origin xyz="0.025001 0 -0.12412" rpy="0 0 0"/>
+    <parent link="left_hip_roll_link"/>
+    <child link="left_hip_yaw_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-2.7576" upper="2.7576" effort="88" velocity="32"/>
+  </joint>
+  <link name="left_knee_link">
+    <inertial>
+      <origin xyz="0.005457 0.003964 -0.12074" rpy="0 0 0"/>
+      <mass value="1.932"/>
+      <inertia ixx="0.011329" ixy="4.82E-05" ixz="-4.49E-05" iyy="0.011277" iyz="-0.0007146" izz="0.0015168"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_knee_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_knee_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_knee_joint" type="revolute">
+    <origin xyz="-0.078273 0.0021489 -0.17734" rpy="0 0.1749 0"/>
+    <parent link="left_hip_yaw_link"/>
+    <child link="left_knee_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.087267" upper="2.8798" effort="139" velocity="20"/>
+  </joint>
+  <link name="left_ankle_pitch_link">
+    <inertial>
+      <origin xyz="-0.007269 0 0.011137" rpy="0 0 0"/>
+      <mass value="0.074"/>
+      <inertia ixx="8.4E-06" ixy="0" ixz="-2.9E-06" iyy="1.89E-05" iyz="0" izz="1.26E-05"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_ankle_pitch_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_ankle_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_ankle_pitch_joint" type="revolute">
+    <origin xyz="0 -9.4445E-05 -0.30001" rpy="0 0 0"/>
+    <parent link="left_knee_link"/>
+    <child link="left_ankle_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.87267" upper="0.5236" effort="50" velocity="37"/>
+  </joint>
+  <link name="left_ankle_roll_link">
+    <inertial>
+      <origin xyz="0.026505 0 -0.016425" rpy="0 0 0"/>
+      <mass value="0.608"/>
+      <inertia ixx="0.0002231" ixy="2E-07" ixz="8.91E-05" iyy="0.0016161" iyz="-1E-07" izz="0.0016667"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_ankle_roll_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="-0.05 0.025 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.005"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin xyz="-0.05 -0.025 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.005"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin xyz="0.12 0.03 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.005"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin xyz="0.12 -0.03 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.005"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_ankle_roll_joint" type="revolute">
+    <origin xyz="0 0 -0.017558" rpy="0 0 0"/>
+    <parent link="left_ankle_pitch_link"/>
+    <child link="left_ankle_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-0.2618" upper="0.2618" effort="50" velocity="37"/>
+  </joint>
+  <link name="right_hip_pitch_link">
+    <inertial>
+      <origin xyz="0.002741 -0.047791 -0.02606" rpy="0 0 0"/>
+      <mass value="1.35"/>
+      <inertia ixx="0.001811" ixy="-3.68E-05" ixz="-3.44E-05" iyy="0.0014193" iyz="-0.000171" izz="0.0012812"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_hip_pitch_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_hip_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_hip_pitch_joint" type="revolute">
+    <origin xyz="0 -0.064452 -0.1027" rpy="0 0 0"/>
+    <parent link="pelvis"/>
+    <child link="right_hip_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-2.5307" upper="2.8798" effort="88" velocity="32"/>
+  </joint>
+  <link name="right_hip_roll_link">
+    <inertial>
+      <origin xyz="0.029812 0.001045 -0.087934" rpy="0 0 0"/>
+      <mass value="1.52"/>
+      <inertia ixx="0.0023773" ixy="3.8E-06" ixz="-0.0003908" iyy="0.0024123" iyz="-1.84E-05" izz="0.0016595"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_hip_roll_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_hip_roll_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_hip_roll_joint" type="revolute">
+    <origin xyz="0 -0.052 -0.030465" rpy="0 -0.1749 0"/>
+    <parent link="right_hip_pitch_link"/>
+    <child link="right_hip_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-2.9671" upper="0.5236" effort="88" velocity="32"/>
+  </joint>
+  <link name="right_hip_yaw_link">
+    <inertial>
+      <origin xyz="-0.057709 0.010981 -0.15078" rpy="0 0 0"/>
+      <mass value="1.702"/>
+      <inertia ixx="0.0057774" ixy="0.0005411" ixz="-0.0023948" iyy="0.0076124" iyz="0.0007072" izz="0.003149"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_hip_yaw_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_hip_yaw_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_hip_yaw_joint" type="revolute">
+    <origin xyz="0.025001 0 -0.12412" rpy="0 0 0"/>
+    <parent link="right_hip_roll_link"/>
+    <child link="right_hip_yaw_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-2.7576" upper="2.7576" effort="88" velocity="32"/>
+  </joint>
+  <link name="right_knee_link">
+    <inertial>
+      <origin xyz="0.005457 -0.003964 -0.12074" rpy="0 0 0"/>
+      <mass value="1.932"/>
+      <inertia ixx="0.011329" ixy="-4.82E-05" ixz="4.49E-05" iyy="0.011277" iyz="0.0007146" izz="0.0015168"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_knee_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_knee_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_knee_joint" type="revolute">
+    <origin xyz="-0.078273 -0.0021489 -0.17734" rpy="0 0.1749 0"/>
+    <parent link="right_hip_yaw_link"/>
+    <child link="right_knee_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.087267" upper="2.8798" effort="139" velocity="20"/>
+  </joint>
+  <link name="right_ankle_pitch_link">
+    <inertial>
+      <origin xyz="-0.007269 0 0.011137" rpy="0 0 0"/>
+      <mass value="0.074"/>
+      <inertia ixx="8.4E-06" ixy="0" ixz="-2.9E-06" iyy="1.89E-05" iyz="0" izz="1.26E-05"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_ankle_pitch_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_ankle_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_ankle_pitch_joint" type="revolute">
+    <origin xyz="0 9.4445E-05 -0.30001" rpy="0 0 0"/>
+    <parent link="right_knee_link"/>
+    <child link="right_ankle_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.87267" upper="0.5236" effort="50" velocity="37"/>
+  </joint>
+  <link name="right_ankle_roll_link">
+    <inertial>
+      <origin xyz="0.026505 0 -0.016425" rpy="0 0 0"/>
+      <mass value="0.608"/>
+      <inertia ixx="0.0002231" ixy="-2E-07" ixz="8.91E-05" iyy="0.0016161" iyz="1E-07" izz="0.0016667"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_ankle_roll_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="-0.05 0.025 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.005"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin xyz="-0.05 -0.025 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.005"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin xyz="0.12 0.03 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.005"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin xyz="0.12 -0.03 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.005"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_ankle_roll_joint" type="revolute">
+    <origin xyz="0 0 -0.017558" rpy="0 0 0"/>
+    <parent link="right_ankle_pitch_link"/>
+    <child link="right_ankle_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-0.2618" upper="0.2618" effort="50" velocity="37"/>
+  </joint>
+
+  <!-- Torso -->
+  <link name="waist_yaw_link">
+    <inertial>
+      <origin xyz="0.003964 0 0.018769" rpy="0 0 0"/>
+      <mass value="0.244"/>
+      <inertia ixx="9.9587E-05" ixy="-1.833E-06" ixz="-1.2617E-05" iyy="0.00012411" iyz="-1.18E-07" izz="0.00015586"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/waist_yaw_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="waist_yaw_joint" type="revolute">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="pelvis"/>
+    <child link="waist_yaw_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-2.618" upper="2.618" effort="88" velocity="32"/>
+  </joint>
+  <link name="waist_roll_link">
+    <inertial>
+      <origin xyz="0 -0.000236 0.010111" rpy="0 0 0"/>
+      <mass value="0.047"/>
+      <inertia ixx="7.515E-06" ixy="0" ixz="0" iyy="6.398E-06" iyz="9.9E-08" izz="3.988E-06"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/waist_roll_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="waist_roll_joint" type="revolute">
+    <origin xyz="-0.0039635 0 0.035" rpy="0 0 0"/>
+    <parent link="waist_yaw_link"/>
+    <child link="waist_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-0.52" upper="0.52" effort="50" velocity="37"/>
+  </joint>
+  <link name="torso_link">
+    <inertial>
+      <origin xyz="0.002601 0.000257 0.153719" rpy="0 0 0"/>
+      <mass value="8.562"/>
+      <inertia ixx="0.065674966" ixy="-8.597E-05" ixz="-0.001737252" iyy="0.053535188" iyz="8.6899E-05" izz="0.030808125"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/torso_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/torso_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="waist_pitch_joint" type="revolute">
+    <origin xyz="0 0 0.019" rpy="0 0 0"/>
+    <parent link="waist_roll_link"/>
+    <child link="torso_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.52" upper="0.52" effort="50" velocity="37"/>
+  </joint>
+
+  <!-- LOGO -->
+  <joint name="logo_joint" type="fixed">
+    <origin xyz="0.0039635 0 -0.054" rpy="0 0 0"/>
+    <parent link="torso_link"/>
+    <child link="logo_link"/>
+  </joint>
+  <link name="logo_link">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="0.001"/>
+      <inertia ixx="1e-7" ixy="0" ixz="0" iyy="1e-7" iyz="0" izz="1e-7"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/logo_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/logo_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- Head -->
+  <link name="head_link">
+    <inertial>
+      <origin xyz="0.005267 0.000299 0.449869" rpy="0 0 0"/>
+      <mass value="1.036"/>
+      <inertia ixx="0.004085051" ixy="-2.543E-06" ixz="-6.9455E-05" iyy="0.004185212" iyz="-3.726E-06" izz="0.001807911"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/head_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/head_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="head_joint" type="fixed">
+    <origin xyz="0.0039635 0 -0.054" rpy="0 0 0"/>
+    <parent link="torso_link"/>
+    <child link="head_link"/>
+  </joint>
+
+  <!-- Waist Support -->
+  <link name="waist_support_link">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="0.001"/>
+      <inertia ixx="1e-7" ixy="0" ixz="0" iyy="1e-7" iyz="0" izz="1e-7"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/waist_support_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/waist_support_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="waist_support_joint" type="fixed">
+    <origin xyz="0.0039635 0 -0.054" rpy="0 0 0"/>
+    <parent link="torso_link"/>
+    <child link="waist_support_link"/>
+  </joint>
+
+  <!-- IMU -->
+  <link name="imu_link"></link>
+  <joint name="imu_joint" type="fixed">
+    <origin xyz="-0.03959 -0.00224 0.13792" rpy="0 0 0"/>
+    <parent link="torso_link"/>
+    <child link="imu_link"/>
+  </joint>
+
+  <!-- d435 -->
+  <link name="d435_link"></link>
+  <joint name="d435_joint" type="fixed">
+    <origin xyz="0.0576235 0.01753 0.41987" rpy="0 0.8307767239493009 0"/>
+    <parent link="torso_link"/>
+    <child link="d435_link"/>
+  </joint>
+
+  <!-- mid360 -->
+  <link name="mid360_link"></link>
+  <joint name="mid360_joint" type="fixed">
+    <origin xyz="0.0002835 0.00003 0.40618" rpy="0 0.04014257279586953 0"/>
+    <parent link="torso_link"/>
+    <child link="mid360_link"/>
+  </joint>
+
+  <!-- Arm -->
+  <link name="left_shoulder_pitch_link">
+    <inertial>
+      <origin xyz="0 0.035892 -0.011628" rpy="0 0 0"/>
+      <mass value="0.718"/>
+      <inertia ixx="0.0004291" ixy="-9.2E-06" ixz="6.4E-06" iyy="0.000453" iyz="2.26E-05" izz="0.000423"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_shoulder_pitch_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0.04 -0.01" rpy="0 1.5707963267948966 0"/>
+      <geometry>
+        <cylinder radius="0.03" length="0.05"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_shoulder_pitch_joint" type="revolute">
+    <origin xyz="0.0039563 0.10022 0.23778" rpy="0.27931 5.4949E-05 -0.00019159"/>
+    <parent link="torso_link"/>
+    <child link="left_shoulder_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-3.0892" upper="2.6704" effort="25" velocity="37"/>
+  </joint>
+  <link name="left_shoulder_roll_link">
+    <inertial>
+      <origin xyz="-0.000227 0.00727 -0.063243" rpy="0 0 0"/>
+      <mass value="0.643"/>
+      <inertia ixx="0.0006177" ixy="-1E-06" ixz="8.7E-06" iyy="0.0006912" iyz="-5.3E-06" izz="0.0003894"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_shoulder_roll_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="-0.004 0.006 -0.053" rpy="0 0 0"/>
+      <geometry>
+        <cylinder radius="0.03" length="0.03"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_shoulder_roll_joint" type="revolute">
+    <origin xyz="0 0.038 -0.013831" rpy="-0.27925 0 0"/>
+    <parent link="left_shoulder_pitch_link"/>
+    <child link="left_shoulder_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-1.5882" upper="2.2515" effort="25" velocity="37"/>
+  </joint>
+  <link name="left_shoulder_yaw_link">
+    <inertial>
+      <origin xyz="0.010773 -0.002949 -0.072009" rpy="0 0 0"/>
+      <mass value="0.734"/>
+      <inertia ixx="0.0009988" ixy="7.9E-06" ixz="0.0001412" iyy="0.0010605" iyz="-2.86E-05" izz="0.0004354"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_shoulder_yaw_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_shoulder_yaw_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_shoulder_yaw_joint" type="revolute">
+    <origin xyz="0 0.00624 -0.1032" rpy="0 0 0"/>
+    <parent link="left_shoulder_roll_link"/>
+    <child link="left_shoulder_yaw_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-2.618" upper="2.618" effort="25" velocity="37"/>
+  </joint>
+  <link name="left_elbow_link">
+    <inertial>
+      <origin xyz="0.064956 0.004454 -0.010062" rpy="0 0 0"/>
+      <mass value="0.6"/>
+      <inertia ixx="0.0002891" ixy="6.53E-05" ixz="1.72E-05" iyy="0.0004152" iyz="-5.6E-06" izz="0.0004197"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_elbow_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_elbow_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_elbow_joint" type="revolute">
+    <origin xyz="0.015783 0 -0.080518" rpy="0 0 0"/>
+    <parent link="left_shoulder_yaw_link"/>
+    <child link="left_elbow_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-1.0472" upper="2.0944" effort="25" velocity="37"/>
+  </joint>
+  <joint name="left_wrist_roll_joint" type="revolute">
+    <origin xyz="0.100 0.00188791 -0.010" rpy="0 0 0"/>
+    <axis xyz="1 0 0"/>
+    <parent link="left_elbow_link"/>
+    <child link="left_wrist_roll_link"/>
+    <limit effort="25" velocity="37" lower="-1.972222054" upper="1.972222054"/>
+  </joint>
+  <link name="left_wrist_roll_link">
+    <inertial>
+      <origin xyz="0.01713944778 0.00053759094 0.00000048864" rpy="0 0 0"/>
+      <mass value="0.08544498"/>
+      <inertia ixx="0.00004821544023" ixy="-0.00000424511021" ixz="0.00000000510599" iyy="0.00003722899093" iyz="-0.00000000123525" izz="0.00005482106541"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_wrist_roll_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_wrist_roll_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_wrist_pitch_joint" type="revolute">
+    <origin xyz="0.038 0 0" rpy="0 0 0"/>
+    <axis xyz="0 1 0"/>
+    <parent link="left_wrist_roll_link"/>
+    <child link="left_wrist_pitch_link"/>
+    <limit effort="5" velocity="22" lower="-1.614429558" upper="1.614429558"/>
+  </joint>
+  <link name="left_wrist_pitch_link">
+    <inertial>
+      <origin xyz="0.02299989837 -0.00111685314 -0.00111658096" rpy="0 0 0"/>
+      <mass value="0.48404956"/>
+      <inertia ixx="0.00016579646273" ixy="-0.00001231206746" ixz="0.00001231699194" iyy="0.00042954057410" iyz="0.00000081417712" izz="0.00042953697654"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_wrist_pitch_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_wrist_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_wrist_yaw_joint" type="revolute">
+    <origin xyz="0.046 0 0" rpy="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <parent link="left_wrist_pitch_link"/>
+    <child link="left_wrist_yaw_link"/>
+    <limit effort="5" velocity="22" lower="-1.614429558" upper="1.614429558"/>
+  </joint>
+  <link name="left_wrist_yaw_link">
+    <inertial>
+      <origin xyz="0.02200381568 0.00049485096 0.00053861123" rpy="0 0 0"/>
+      <mass value="0.08457647"/>
+      <inertia ixx="0.00004929128828" ixy="-0.00000045735494" ixz="0.00000445867591" iyy="0.00005973338134" iyz="0.00000043217198" izz="0.00003928083826"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_wrist_yaw_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_wrist_yaw_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_hand_palm_joint" type="fixed">
+    <origin xyz="0.0415 0.003 0" rpy="0 0 0"/>
+    <parent link="left_wrist_yaw_link"/>
+    <child link="left_hand_palm_link"/>
+  </joint>
+  <link name="left_hand_palm_link">
+    <inertial>
+      <origin xyz="0.06214634836 -0.00050869656 -0.00058171093" rpy="0 0 0"/>
+      <mass value="0.37283854"/>
+      <inertia ixx="0.00027535181027" ixy="-0.00001595519465" ixz="-0.00000242161890" iyy="0.00053951827219" iyz="-0.00000042279435" izz="0.00039623390907"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_hand_palm_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_hand_palm_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_hand_thumb_0_joint" type="revolute">
+    <origin xyz="0.0255 0 0" rpy="0 0 0"/>
+    <axis xyz="0 1 0"/>
+    <parent link="left_hand_palm_link"/>
+    <child link="left_hand_thumb_0_link"/>
+    <limit effort="2.45" velocity="6.857" lower="-1.04719755" upper="1.04719755"/>
+  </joint>
+  <link name="left_hand_thumb_0_link">
+    <inertial>
+      <origin xyz="-0.00088424580 -0.00863407079 0.00094429336" rpy="0 0 0"/>
+      <mass value="0.08623657"/>
+      <inertia ixx="0.00001602919238" ixy="0.00000010683177" ixz="0.00000016728875" iyy="0.00001451795012" iyz="-0.00000051094752" izz="0.00001637877663"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_hand_thumb_0_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_hand_thumb_0_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_hand_thumb_1_joint" type="revolute">
+    <origin xyz="-0.0025 -0.0193 0" rpy="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <parent link="left_hand_thumb_0_link"/>
+    <child link="left_hand_thumb_1_link"/>
+    <limit effort="1.4" velocity="12" lower="-0.72431163" upper="1.04719755"/>
+  </joint>
+  <link name="left_hand_thumb_1_link">
+    <inertial>
+      <origin xyz="-0.00082788768 -0.03547435774 -0.00038089960" rpy="0 0 0"/>
+      <mass value="0.05885070"/>
+      <inertia ixx="0.00001274699945" ixy="-0.00000050770784" ixz="0.00000016088850" iyy="0.00000601573947" iyz="-0.00000027839003" izz="0.00001234543582"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_hand_thumb_1_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="-0.001 -0.032 0" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.02 0.03 0.02"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_hand_thumb_2_joint" type="revolute">
+    <origin xyz="0 -0.0458 0" rpy="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <parent link="left_hand_thumb_1_link"/>
+    <child link="left_hand_thumb_2_link"/>
+    <limit effort="1.4" velocity="12" lower="0" upper="1.74532925"/>
+  </joint>
+  <link name="left_hand_thumb_2_link">
+    <inertial>
+      <origin xyz="-0.00171735242 -0.02628192939 0.00010778879" rpy="0 0 0"/>
+      <mass value="0.02030626"/>
+      <inertia ixx="0.00000461267817" ixy="-0.00000003422130" ixz="-0.00000000823881" iyy="0.00000153561368" iyz="-0.00000002549885" izz="0.00000386625776"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_hand_thumb_2_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_hand_thumb_2_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_hand_middle_0_joint" type="revolute">
+    <origin xyz="0.0777 0.0016 -0.0285" rpy="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <parent link="left_hand_palm_link"/>
+    <child link="left_hand_middle_0_link"/>
+    <limit effort="1.4" velocity="12" lower="-1.57079632" upper="0"/>
+  </joint>
+  <link name="left_hand_middle_0_link">
+    <inertial>
+      <origin xyz="0.03547435774 0.00082788768 0.00038089960" rpy="0 0 0"/>
+      <mass value="0.05885070"/>
+      <inertia ixx="0.00000601573947" ixy="-0.00000050770784" ixz="-0.00000027839003" iyy="0.00001274699945" iyz="0.00000016088850" izz="0.00001234543582"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_hand_middle_0_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_hand_middle_0_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_hand_middle_1_joint" type="revolute">
+    <origin xyz="0.0458 0 0" rpy="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <parent link="left_hand_middle_0_link"/>
+    <child link="left_hand_middle_1_link"/>
+    <limit effort="1.4" velocity="12" lower="-1.74532925" upper="0"/>
+  </joint>
+  <link name="left_hand_middle_1_link">
+    <inertial>
+      <origin xyz="0.02628192939 0.00171735242 -0.00010778879" rpy="0 0 0"/>
+      <mass value="0.02030626"/>
+      <inertia ixx="0.00000153561368" ixy="-0.00000003422130" ixz="-0.00000002549885" iyy="0.00000461267817" iyz="-0.00000000823881" izz="0.00000386625776"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_hand_middle_1_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_hand_middle_1_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_hand_index_0_joint" type="revolute">
+    <origin xyz="0.0777 0.0016 0.0285" rpy="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <parent link="left_hand_palm_link"/>
+    <child link="left_hand_index_0_link"/>
+    <limit effort="1.4" velocity="12" lower="-1.57079632" upper="0"/>
+  </joint>
+  <link name="left_hand_index_0_link">
+    <inertial>
+      <origin xyz="0.03547435774 0.00082788768 0.00038089960" rpy="0 0 0"/>
+      <mass value="0.05885070"/>
+      <inertia ixx="0.00000601573947" ixy="-0.00000050770784" ixz="-0.00000027839003" iyy="0.00001274699945" iyz="0.00000016088850" izz="0.00001234543582"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_hand_index_0_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_hand_index_0_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_hand_index_1_joint" type="revolute">
+    <origin xyz="0.0458 0 0" rpy="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <parent link="left_hand_index_0_link"/>
+    <child link="left_hand_index_1_link"/>
+    <limit effort="1.4" velocity="12" lower="-1.74532925" upper="0"/>
+  </joint>
+  <link name="left_hand_index_1_link">
+    <inertial>
+      <origin xyz="0.02628192939 0.00171735242 -0.00010778879" rpy="0 0 0"/>
+      <mass value="0.02030626"/>
+      <inertia ixx="0.00000153561368" ixy="-0.00000003422130" ixz="-0.00000002549885" iyy="0.00000461267817" iyz="-0.00000000823881" izz="0.00000386625776"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_hand_index_1_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_hand_index_1_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="right_shoulder_pitch_link">
+    <inertial>
+      <origin xyz="0 -0.035892 -0.011628" rpy="0 0 0"/>
+      <mass value="0.718"/>
+      <inertia ixx="0.0004291" ixy="9.2E-06" ixz="6.4E-06" iyy="0.000453" iyz="-2.26E-05" izz="0.000423"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_shoulder_pitch_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 -0.04 -0.01" rpy="0 1.5707963267948966 0"/>
+      <geometry>
+        <cylinder radius="0.03" length="0.05"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_shoulder_pitch_joint" type="revolute">
+    <origin xyz="0.0039563 -0.10021 0.23778" rpy="-0.27931 5.4949E-05 0.00019159"/>
+    <parent link="torso_link"/>
+    <child link="right_shoulder_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-3.0892" upper="2.6704" effort="25" velocity="37"/>
+  </joint>
+  <link name="right_shoulder_roll_link">
+    <inertial>
+      <origin xyz="-0.000227 -0.00727 -0.063243" rpy="0 0 0"/>
+      <mass value="0.643"/>
+      <inertia ixx="0.0006177" ixy="1E-06" ixz="8.7E-06" iyy="0.0006912" iyz="5.3E-06" izz="0.0003894"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_shoulder_roll_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="-0.004 -0.006 -0.053" rpy="0 0 0"/>
+      <geometry>
+        <cylinder radius="0.03" length="0.03"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_shoulder_roll_joint" type="revolute">
+    <origin xyz="0 -0.038 -0.013831" rpy="0.27925 0 0"/>
+    <parent link="right_shoulder_pitch_link"/>
+    <child link="right_shoulder_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-2.2515" upper="1.5882" effort="25" velocity="37"/>
+  </joint>
+  <link name="right_shoulder_yaw_link">
+    <inertial>
+      <origin xyz="0.010773 0.002949 -0.072009" rpy="0 0 0"/>
+      <mass value="0.734"/>
+      <inertia ixx="0.0009988" ixy="-7.9E-06" ixz="0.0001412" iyy="0.0010605" iyz="2.86E-05" izz="0.0004354"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_shoulder_yaw_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_shoulder_yaw_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_shoulder_yaw_joint" type="revolute">
+    <origin xyz="0 -0.00624 -0.1032" rpy="0 0 0"/>
+    <parent link="right_shoulder_roll_link"/>
+    <child link="right_shoulder_yaw_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-2.618" upper="2.618" effort="25" velocity="37"/>
+  </joint>
+  <link name="right_elbow_link">
+    <inertial>
+      <origin xyz="0.064956 -0.004454 -0.010062" rpy="0 0 0"/>
+      <mass value="0.6"/>
+      <inertia ixx="0.0002891" ixy="-6.53E-05" ixz="1.72E-05" iyy="0.0004152" iyz="5.6E-06" izz="0.0004197"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_elbow_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_elbow_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_elbow_joint" type="revolute">
+    <origin xyz="0.015783 0 -0.080518" rpy="0 0 0"/>
+    <parent link="right_shoulder_yaw_link"/>
+    <child link="right_elbow_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-1.0472" upper="2.0944" effort="25" velocity="37"/>
+  </joint>
+  <joint name="right_wrist_roll_joint" type="revolute">
+    <origin xyz="0.100 -0.00188791 -0.010" rpy="0 0 0"/>
+    <axis xyz="1 0 0"/>
+    <parent link="right_elbow_link"/>
+    <child link="right_wrist_roll_link"/>
+    <limit effort="25" velocity="37" lower="-1.972222054" upper="1.972222054"/>
+  </joint>
+  <link name="right_wrist_roll_link">
+    <inertial>
+      <origin xyz="0.01713944778 -0.00053759094 0.00000048864" rpy="0 0 0"/>
+      <mass value="0.08544498"/>
+      <inertia ixx="0.00004821544023" ixy="0.00000424511021" ixz="0.00000000510599" iyy="0.00003722899093" iyz="0.00000000123525" izz="0.00005482106541"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_wrist_roll_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_wrist_roll_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_wrist_pitch_joint" type="revolute">
+    <origin xyz="0.038 0 0" rpy="0 0 0"/>
+    <axis xyz="0 1 0"/>
+    <parent link="right_wrist_roll_link"/>
+    <child link="right_wrist_pitch_link"/>
+    <limit effort="5" velocity="22" lower="-1.614429558" upper="1.614429558"/>
+  </joint>
+  <link name="right_wrist_pitch_link">
+    <inertial>
+      <origin xyz="0.02299989837 0.00111685314 -0.00111658096" rpy="0 0 0"/>
+      <mass value="0.48404956"/>
+      <inertia ixx="0.00016579646273" ixy="0.00001231206746" ixz="0.00001231699194" iyy="0.00042954057410" iyz="-0.00000081417712" izz="0.00042953697654"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_wrist_pitch_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_wrist_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_wrist_yaw_joint" type="revolute">
+    <origin xyz="0.046 0 0" rpy="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <parent link="right_wrist_pitch_link"/>
+    <child link="right_wrist_yaw_link"/>
+    <limit effort="5" velocity="22" lower="-1.614429558" upper="1.614429558"/>
+  </joint>
+  <link name="right_wrist_yaw_link">
+    <inertial>
+      <origin xyz="0.02200381568 -0.00049485096 0.00053861123" rpy="0 0 0"/>
+      <mass value="0.08457647"/>
+      <inertia ixx="0.00004929128828" ixy="0.00000045735494" ixz="0.00000445867591" iyy="0.00005973338134" iyz="-0.00000043217198" izz="0.00003928083826"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_wrist_yaw_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_wrist_yaw_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_hand_palm_joint" type="fixed">
+    <origin xyz="0.0415 -0.003 0" rpy="0 0 0"/>
+    <parent link="right_wrist_yaw_link"/>
+    <child link="right_hand_palm_link"/>
+  </joint>
+  <link name="right_hand_palm_link">
+    <inertial>
+      <origin xyz="0.06214634836 0.00050869656 -0.00058171093" rpy="0 0 0"/>
+      <mass value="0.37283854"/>
+      <inertia ixx="0.00027535181027" ixy="0.00001595519465" ixz="-0.00000242161890" iyy="0.00053951827219" iyz="0.00000042279435" izz="0.00039623390907"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_hand_palm_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_hand_palm_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_hand_thumb_0_joint" type="revolute">
+    <origin xyz="0.0255 0 0" rpy="0 0 0"/>
+    <axis xyz="0 1 0"/>
+    <parent link="right_hand_palm_link"/>
+    <child link="right_hand_thumb_0_link"/>
+    <limit effort="2.45" velocity="6.857" lower="-1.04719755" upper="1.04719755"/>
+  </joint>
+  <link name="right_hand_thumb_0_link">
+    <inertial>
+      <origin xyz="-0.00088424580 0.00863407079 0.00094429336" rpy="0 0 0"/>
+      <mass value="0.08623657"/>
+      <inertia ixx="0.00001602919238" ixy="-0.00000010683177" ixz="0.00000016728875" iyy="0.00001451795012" iyz="0.00000051094752" izz="0.00001637877663"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_hand_thumb_0_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_hand_thumb_0_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_hand_thumb_1_joint" type="revolute">
+    <origin xyz="-0.0025 0.0193 0" rpy="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <parent link="right_hand_thumb_0_link"/>
+    <child link="right_hand_thumb_1_link"/>
+    <limit effort="1.4" velocity="12" lower="-1.04719755" upper="0.72431163"/>
+  </joint>
+  <link name="right_hand_thumb_1_link">
+    <inertial>
+      <origin xyz="-0.00082788768 0.03547435774 -0.00038089960" rpy="0 0 0"/>
+      <mass value="0.05885070"/>
+      <inertia ixx="0.00001274699945" ixy="0.00000050770784" ixz="0.00000016088850" iyy="0.00000601573947" iyz="0.00000027839003" izz="0.00001234543582"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_hand_thumb_1_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="-0.001 0.032 0" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.02 0.03 0.02"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_hand_thumb_2_joint" type="revolute">
+    <origin xyz="0 0.0458 0" rpy="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <parent link="right_hand_thumb_1_link"/>
+    <child link="right_hand_thumb_2_link"/>
+    <limit effort="1.4" velocity="12" lower="-1.74532925" upper="0"/>
+  </joint>
+  <link name="right_hand_thumb_2_link">
+    <inertial>
+      <origin xyz="-0.00171735242 0.02628192939 0.00010778879" rpy="0 0 0"/>
+      <mass value="0.02030626"/>
+      <inertia ixx="0.00000461267817" ixy="0.00000003422130" ixz="-0.00000000823881" iyy="0.00000153561368" iyz="0.00000002549885" izz="0.00000386625776"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_hand_thumb_2_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_hand_thumb_2_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_hand_middle_0_joint" type="revolute">
+    <origin xyz="0.0777 -0.0016 -0.0285" rpy="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <parent link="right_hand_palm_link"/>
+    <child link="right_hand_middle_0_link"/>
+    <limit effort="1.4" velocity="12" lower="0" upper="1.57079632"/>
+  </joint>
+  <link name="right_hand_middle_0_link">
+    <inertial>
+      <origin xyz="0.03547435774 -0.00082788768 0.00038089960" rpy="0 0 0"/>
+      <mass value="0.05885070"/>
+      <inertia ixx="0.00000601573947" ixy="0.00000050770784" ixz="-0.00000027839003" iyy="0.00001274699945" iyz="-0.00000016088850" izz="0.00001234543582"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_hand_middle_0_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_hand_middle_0_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_hand_middle_1_joint" type="revolute">
+    <origin xyz="0.0458 0 0" rpy="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <parent link="right_hand_middle_0_link"/>
+    <child link="right_hand_middle_1_link"/>
+    <limit effort="1.4" velocity="12" lower="0" upper="1.74532925"/>
+  </joint>
+  <link name="right_hand_middle_1_link">
+    <inertial>
+      <origin xyz="0.02628192939 -0.00171735242 -0.00010778879" rpy="0 0 0"/>
+      <mass value="0.02030626"/>
+      <inertia ixx="0.00000153561368" ixy="0.00000003422130" ixz="-0.00000002549885" iyy="0.00000461267817" iyz="0.00000000823881" izz="0.00000386625776"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_hand_middle_1_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_hand_middle_1_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_hand_index_0_joint" type="revolute">
+    <origin xyz="0.0777 -0.0016 0.0285" rpy="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <parent link="right_hand_palm_link"/>
+    <child link="right_hand_index_0_link"/>
+    <limit effort="1.4" velocity="12" lower="0" upper="1.57079632"/>
+  </joint>
+  <link name="right_hand_index_0_link">
+    <inertial>
+      <origin xyz="0.03547435774 -0.00082788768 0.00038089960" rpy="0 0 0"/>
+      <mass value="0.05885070"/>
+      <inertia ixx="0.00000601573947" ixy="0.00000050770784" ixz="-0.00000027839003" iyy="0.00001274699945" iyz="-0.00000016088850" izz="0.00001234543582"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_hand_index_0_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_hand_index_0_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_hand_index_1_joint" type="revolute">
+    <origin xyz="0.0458 0 0" rpy="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <parent link="right_hand_index_0_link"/>
+    <child link="right_hand_index_1_link"/>
+    <limit effort="1.4" velocity="12" lower="0" upper="1.74532925"/>
+  </joint>
+  <link name="right_hand_index_1_link">
+    <inertial>
+      <origin xyz="0.02628192939 -0.00171735242 -0.00010778879" rpy="0 0 0"/>
+      <mass value="0.02030626"/>
+      <inertia ixx="0.00000153561368" ixy="0.00000003422130" ixz="-0.00000002549885" iyy="0.00000461267817" iyz="0.00000000823881" izz="0.00000386625776"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_hand_index_1_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_hand_index_1_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>

GR00T-WholeBodyControl/decoupled_wbc/control/robot_model/model_data/g1/g1_29dof_with_hand.xml

@@ -0,0 +1,748 @@
+<mujoco model="g1_29dof_with_hand">
+  <compiler angle="radian" meshdir="meshes" />
+
+  <default>
+    <default class="torso_motor">
+      <joint damping="0.05" armature="0.01" frictionloss="0.2"/>
+    </default>
+    <default class="leg_motor">
+      <joint damping="0.05" armature="0.01" frictionloss="0.2"/>
+    </default>
+    <default class="ankle_motor">
+      <joint damping="0.05" armature="0.01" frictionloss="0.2"/>
+    </default>
+    <default class="arm_motor">
+      <joint damping="0.05" armature="0.01" frictionloss="0.2"/>
+    </default>
+    <default class="wrist_motor">
+      <joint damping="0.05" armature="0.01" frictionloss="0.1"/>
+    </default>
+    <default class="finger_motor">
+      <joint damping="0.05" armature="0.01" frictionloss="0.1"/>
+    </default>
+  </default>
+  <asset>
+    <mesh name="pelvis" file="pelvis.STL" />
+    <mesh name="pelvis_contour_link" file="pelvis_contour_link.STL" />
+    <mesh name="left_hip_pitch_link" file="left_hip_pitch_link.STL" />
+    <mesh name="left_hip_roll_link" file="left_hip_roll_link.STL" />
+    <mesh name="left_hip_yaw_link" file="left_hip_yaw_link.STL" />
+    <mesh name="left_knee_link" file="left_knee_link.STL" />
+    <mesh name="left_ankle_pitch_link" file="left_ankle_pitch_link.STL" />
+    <mesh name="left_ankle_roll_link" file="left_ankle_roll_link.STL" />
+    <mesh name="right_hip_pitch_link" file="right_hip_pitch_link.STL" />
+    <mesh name="right_hip_roll_link" file="right_hip_roll_link.STL" />
+    <mesh name="right_hip_yaw_link" file="right_hip_yaw_link.STL" />
+    <mesh name="right_knee_link" file="right_knee_link.STL" />
+    <mesh name="right_ankle_pitch_link" file="right_ankle_pitch_link.STL" />
+    <mesh name="right_ankle_roll_link" file="right_ankle_roll_link.STL" />
+    <mesh name="waist_yaw_link" file="waist_yaw_link.STL" />
+    <mesh name="waist_roll_link" file="waist_roll_link.STL" />
+    <mesh name="torso_link" file="torso_link.STL" />
+    <mesh name="logo_link" file="logo_link.STL" />
+    <mesh name="head_link" file="head_link.STL" />
+    <mesh name="waist_support_link" file="waist_support_link.STL" />
+    <mesh name="left_shoulder_pitch_link" file="left_shoulder_pitch_link.STL" />
+    <mesh name="left_shoulder_roll_link" file="left_shoulder_roll_link.STL" />
+    <mesh name="left_shoulder_yaw_link" file="left_shoulder_yaw_link.STL" />
+    <mesh name="left_elbow_link" file="left_elbow_link.STL" />
+    <mesh name="left_wrist_roll_link" file="left_wrist_roll_link.STL" />
+    <mesh name="left_wrist_pitch_link" file="left_wrist_pitch_link.STL" />
+    <mesh name="left_wrist_yaw_link" file="left_wrist_yaw_link.STL" />
+    <mesh name="left_hand_palm_link" file="left_hand_palm_link.STL" />
+    <mesh name="left_hand_thumb_0_link" file="left_hand_thumb_0_link.STL" />
+    <mesh name="left_hand_thumb_1_link" file="left_hand_thumb_1_link.STL" />
+    <mesh name="left_hand_thumb_2_link" file="left_hand_thumb_2_link.STL" />
+    <mesh name="left_hand_middle_0_link" file="left_hand_middle_0_link.STL" />
+    <mesh name="left_hand_middle_1_link" file="left_hand_middle_1_link.STL" />
+    <mesh name="left_hand_index_0_link" file="left_hand_index_0_link.STL" />
+    <mesh name="left_hand_index_1_link" file="left_hand_index_1_link.STL" />
+    <mesh name="right_shoulder_pitch_link" file="right_shoulder_pitch_link.STL" />
+    <mesh name="right_shoulder_roll_link" file="right_shoulder_roll_link.STL" />
+    <mesh name="right_shoulder_yaw_link" file="right_shoulder_yaw_link.STL" />
+    <mesh name="right_elbow_link" file="right_elbow_link.STL" />
+    <mesh name="right_wrist_roll_link" file="right_wrist_roll_link.STL" />
+    <mesh name="right_wrist_pitch_link" file="right_wrist_pitch_link.STL" />
+    <mesh name="right_wrist_yaw_link" file="right_wrist_yaw_link.STL" />
+    <mesh name="right_hand_palm_link" file="right_hand_palm_link.STL" />
+    <mesh name="right_hand_thumb_0_link" file="right_hand_thumb_0_link.STL" />
+    <mesh name="right_hand_thumb_1_link" file="right_hand_thumb_1_link.STL" />
+    <mesh name="right_hand_thumb_2_link" file="right_hand_thumb_2_link.STL" />
+    <mesh name="right_hand_middle_0_link" file="right_hand_middle_0_link.STL" />
+    <mesh name="right_hand_middle_1_link" file="right_hand_middle_1_link.STL" />
+    <mesh name="right_hand_index_0_link" file="right_hand_index_0_link.STL" />
+    <mesh name="right_hand_index_1_link" file="right_hand_index_1_link.STL" />
+  </asset>
+
+  <worldbody>
+    <body name="pelvis" pos="0 0 0.793">
+      <inertial pos="0 0 -0.07605" quat="1 0 -0.000399148 0" mass="3.813"
+        diaginertia="0.010549 0.0093089 0.0079184" />
+      <joint name="floating_base_joint" type="free" limited="false" actuatorfrclimited="false" />
+      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.2 0.2 0.2 1"
+        mesh="pelvis" />
+      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
+        mesh="pelvis_contour_link" />
+      <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="pelvis_contour_link" />
+      <body name="left_hip_pitch_link" pos="0 0.064452 -0.1027">
+        <inertial pos="0.002741 0.047791 -0.02606" quat="0.954862 0.293964 0.0302556 0.030122"
+          mass="1.35" diaginertia="0.00181517 0.00153422 0.00116212" />
+        <joint name="left_hip_pitch_joint" pos="0 0 0" axis="0 1 0" range="-2.5307 2.8798"
+          actuatorfrcrange="-88 88" class="leg_motor" />
+        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.2 0.2 0.2 1"
+          mesh="left_hip_pitch_link" />
+        <geom type="mesh" rgba="0.2 0.2 0.2 1" mesh="left_hip_pitch_link" />
+        <body name="left_hip_roll_link" pos="0 0.052 -0.030465" quat="0.996179 0 -0.0873386 0">
+          <inertial pos="0.029812 -0.001045 -0.087934"
+            quat="0.977808 -1.97119e-05 0.205576 -0.0403793" mass="1.52"
+            diaginertia="0.00254986 0.00241169 0.00148755" />
+          <joint name="left_hip_roll_joint" pos="0 0 0" axis="1 0 0" range="-0.5236 2.9671"
+            actuatorfrcrange="-88 88" class="leg_motor" />
+          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
+            mesh="left_hip_roll_link" />
+          <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_hip_roll_link" />
+          <body name="left_hip_yaw_link" pos="0.025001 0 -0.12412">
+            <inertial pos="-0.057709 -0.010981 -0.15078" quat="0.600598 0.15832 0.223482 0.751181"
+              mass="1.702" diaginertia="0.00776166 0.00717575 0.00160139" />
+            <joint name="left_hip_yaw_joint" pos="0 0 0" axis="0 0 1" range="-2.7576 2.7576"
+              actuatorfrcrange="-88 88" class="leg_motor" />
+            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
+              mesh="left_hip_yaw_link" />
+            <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_hip_yaw_link" />
+            <body name="left_knee_link" pos="-0.078273 0.0021489 -0.17734"
+              quat="0.996179 0 0.0873386 0">
+              <inertial pos="0.005457 0.003964 -0.12074"
+                quat="0.923418 -0.0327699 0.0158246 0.382067" mass="1.932"
+                diaginertia="0.0113804 0.0112778 0.00146458" />
+              <joint name="left_knee_joint" pos="0 0 0" axis="0 1 0" range="-0.087267 2.8798"
+                actuatorfrcrange="-139 139" class="leg_motor" />
+              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                rgba="0.7 0.7 0.7 1" mesh="left_knee_link" />
+              <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_knee_link" />
+              <body name="left_ankle_pitch_link" pos="0 -9.4445e-05 -0.30001">
+                <inertial pos="-0.007269 0 0.011137" quat="0.603053 0.369225 0.369225 0.603053"
+                  mass="0.074" diaginertia="1.89e-05 1.40805e-05 6.9195e-06" />
+                <joint name="left_ankle_pitch_joint" pos="0 0 0" axis="0 1 0"
+                  range="-0.87267 0.5236" actuatorfrcrange="-50 50" class="ankle_motor" />
+                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                  rgba="0.7 0.7 0.7 1" mesh="left_ankle_pitch_link" />
+                <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_ankle_pitch_link" />
+                <body name="left_ankle_roll_link" pos="0 0 -0.017558">
+                  <inertial pos="0.026505 0 -0.016425"
+                    quat="-0.000481092 0.728482 -0.000618967 0.685065" mass="0.608"
+                    diaginertia="0.00167218 0.0016161 0.000217621" />
+                  <joint name="left_ankle_roll_joint" pos="0 0 0" axis="1 0 0"
+                    range="-0.2618 0.2618" actuatorfrcrange="-50 50" class="ankle_motor" />
+                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                    rgba="0.2 0.2 0.2 1" mesh="left_ankle_roll_link" />
+                  <geom size="0.005" pos="-0.05 0.025 -0.03" rgba="0.2 0.2 0.2 1" />
+                  <geom size="0.005" pos="-0.05 -0.025 -0.03" rgba="0.2 0.2 0.2 1" />
+                  <geom size="0.005" pos="0.12 0.03 -0.03" rgba="0.2 0.2 0.2 1" />
+                  <geom size="0.005" pos="0.12 -0.03 -0.03" rgba="0.2 0.2 0.2 1" />
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+      <body name="right_hip_pitch_link" pos="0 -0.064452 -0.1027">
+        <inertial pos="0.002741 -0.047791 -0.02606" quat="0.954862 -0.293964 0.0302556 -0.030122"
+          mass="1.35" diaginertia="0.00181517 0.00153422 0.00116212" />
+        <joint name="right_hip_pitch_joint" pos="0 0 0" axis="0 1 0" range="-2.5307 2.8798"
+          actuatorfrcrange="-88 88" class="leg_motor" />
+        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.2 0.2 0.2 1"
+          mesh="right_hip_pitch_link" />
+        <geom type="mesh" rgba="0.2 0.2 0.2 1" mesh="right_hip_pitch_link" />
+        <body name="right_hip_roll_link" pos="0 -0.052 -0.030465" quat="0.996179 0 -0.0873386 0">
+          <inertial pos="0.029812 0.001045 -0.087934" quat="0.977808 1.97119e-05 0.205576 0.0403793"
+            mass="1.52" diaginertia="0.00254986 0.00241169 0.00148755" />
+          <joint name="right_hip_roll_joint" pos="0 0 0" axis="1 0 0" range="-2.9671 0.5236"
+            actuatorfrcrange="-88 88" class="leg_motor" />
+          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
+            mesh="right_hip_roll_link" />
+          <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_hip_roll_link" />
+          <body name="right_hip_yaw_link" pos="0.025001 0 -0.12412">
+            <inertial pos="-0.057709 0.010981 -0.15078" quat="0.751181 0.223482 0.15832 0.600598"
+              mass="1.702" diaginertia="0.00776166 0.00717575 0.00160139" />
+            <joint name="right_hip_yaw_joint" pos="0 0 0" axis="0 0 1" range="-2.7576 2.7576"
+              actuatorfrcrange="-88 88" class="leg_motor" />
+            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
+              mesh="right_hip_yaw_link" />
+            <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_hip_yaw_link" />
+            <body name="right_knee_link" pos="-0.078273 -0.0021489 -0.17734"
+              quat="0.996179 0 0.0873386 0">
+              <inertial pos="0.005457 -0.003964 -0.12074"
+                quat="0.923439 0.0345276 0.0116333 -0.382012" mass="1.932"
+                diaginertia="0.011374 0.0112843 0.00146452" />
+              <joint name="right_knee_joint" pos="0 0 0" axis="0 1 0" range="-0.087267 2.8798"
+                actuatorfrcrange="-139 139" class="leg_motor" />
+              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                rgba="0.7 0.7 0.7 1" mesh="right_knee_link" />
+              <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_knee_link" />
+              <body name="right_ankle_pitch_link" pos="0 9.4445e-05 -0.30001">
+                <inertial pos="-0.007269 0 0.011137" quat="0.603053 0.369225 0.369225 0.603053"
+                  mass="0.074" diaginertia="1.89e-05 1.40805e-05 6.9195e-06" />
+                <joint name="right_ankle_pitch_joint" pos="0 0 0" axis="0 1 0"
+                  range="-0.87267 0.5236" actuatorfrcrange="-50 50" class="ankle_motor" />
+                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                  rgba="0.7 0.7 0.7 1" mesh="right_ankle_pitch_link" />
+                <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_ankle_pitch_link" />
+                <body name="right_ankle_roll_link" pos="0 0 -0.017558">
+                  <inertial pos="0.026505 0 -0.016425"
+                    quat="0.000481092 0.728482 0.000618967 0.685065" mass="0.608"
+                    diaginertia="0.00167218 0.0016161 0.000217621" />
+                  <joint name="right_ankle_roll_joint" pos="0 0 0" axis="1 0 0"
+                    range="-0.2618 0.2618" actuatorfrcrange="-50 50" class="ankle_motor" />
+                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                    rgba="0.2 0.2 0.2 1" mesh="right_ankle_roll_link" />
+                  <geom size="0.005" pos="-0.05 0.025 -0.03" rgba="0.2 0.2 0.2 1" />
+                  <geom size="0.005" pos="-0.05 -0.025 -0.03" rgba="0.2 0.2 0.2 1" />
+                  <geom size="0.005" pos="0.12 0.03 -0.03" rgba="0.2 0.2 0.2 1" />
+                  <geom size="0.005" pos="0.12 -0.03 -0.03" rgba="0.2 0.2 0.2 1" />
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+      <body name="waist_yaw_link">
+        <inertial pos="0.003964 0 0.018769" quat="-0.0178291 0.628464 0.0282471 0.777121"
+          mass="0.244" diaginertia="0.000158561 0.000124229 9.67669e-05" />
+        <joint name="waist_yaw_joint" pos="0 0 0" axis="0 0 1" range="-2.618 2.618"
+          actuatorfrcrange="-88 88" class="torso_motor" />
+        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
+          mesh="waist_yaw_link" />
+        <body name="waist_roll_link" pos="-0.0039635 0 0.035">
+          <inertial pos="0 -0.000236 0.010111" quat="0.99979 0.020492 0 0" mass="0.047"
+            diaginertia="7.515e-06 6.40206e-06 3.98394e-06" />
+          <joint name="waist_roll_joint" pos="0 0 0" axis="1 0 0" range="-0.52 0.52"
+            actuatorfrcrange="-50 50" class="torso_motor"/>
+          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
+            mesh="waist_roll_link" />
+          <body name="torso_link" pos="0 0 0.019">
+            <inertial pos="0.00331658 0.000261533 0.179856"
+              quat="0.999831 0.000376204 0.0179895 -0.00377704" mass="9.598"
+              diaginertia="0.12407 0.111951 0.0325382" />
+            <joint name="waist_pitch_joint" pos="0 0 0" axis="0 1 0" range="-0.52 0.52"
+              actuatorfrcrange="-50 50" class="torso_motor"/>
+            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
+              mesh="torso_link" />
+            <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="torso_link" />
+            <geom pos="0.0039635 0 -0.054" quat="1 0 0 0" type="mesh" contype="0" conaffinity="0"
+              group="1" density="0" rgba="0.2 0.2 0.2 1" mesh="logo_link" />
+            <geom pos="0.0039635 0 -0.054" quat="1 0 0 0" type="mesh" rgba="0.2 0.2 0.2 1"
+              mesh="logo_link" />
+            <geom pos="0.0039635 0 -0.054" type="mesh" contype="0" conaffinity="0" group="1"
+              density="0" rgba="0.2 0.2 0.2 1" mesh="head_link" />
+            <geom pos="0.0039635 0 -0.054" type="mesh" rgba="0.2 0.2 0.2 1" mesh="head_link" />
+            <geom pos="0.0039635 0 -0.054" quat="1 0 0 0" type="mesh" contype="0" conaffinity="0"
+              group="1" density="0" rgba="0.7 0.7 0.7 1" mesh="waist_support_link" />
+            <geom pos="0.0039635 0 -0.054" quat="1 0 0 0" type="mesh" rgba="0.7 0.7 0.7 1"
+              mesh="waist_support_link" />
+            <site name="imu" size="0.01" pos="-0.03959 -0.00224 0.13792" />
+            <body name="left_shoulder_pitch_link" pos="0.0039563 0.10022 0.23778"
+              quat="0.990264 0.139201 1.38722e-05 -9.86868e-05">
+              <inertial pos="0 0.035892 -0.011628" quat="0.654152 0.0130458 -0.326267 0.68225"
+                mass="0.718" diaginertia="0.000465864 0.000432842 0.000406394" />
+              <joint name="left_shoulder_pitch_joint" pos="0 0 0" axis="0 1 0"
+                range="-3.0892 2.6704" actuatorfrcrange="-25 25" class="arm_motor" />
+              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                rgba="0.7 0.7 0.7 1" mesh="left_shoulder_pitch_link" />
+              <geom size="0.03 0.025" pos="0 0.04 -0.01" quat="0.707107 0 0.707107 0"
+                type="cylinder" rgba="0.7 0.7 0.7 1" />
+              <body name="left_shoulder_roll_link" pos="0 0.038 -0.013831"
+                quat="0.990268 -0.139172 0 0">
+                <inertial pos="-0.000227 0.00727 -0.063243"
+                  quat="0.701256 -0.0196223 -0.00710317 0.712604" mass="0.643"
+                  diaginertia="0.000691311 0.000618011 0.000388977" />
+                <joint name="left_shoulder_roll_joint" pos="0 0 0" axis="1 0 0"
+                  range="-1.5882 2.2515" actuatorfrcrange="-25 25" class="arm_motor" />
+                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                  rgba="0.7 0.7 0.7 1" mesh="left_shoulder_roll_link" />
+                <geom size="0.03 0.015" pos="-0.004 0.006 -0.053" type="cylinder"
+                  rgba="0.7 0.7 0.7 1" />
+                <body name="left_shoulder_yaw_link" pos="0 0.00624 -0.1032">
+                  <inertial pos="0.010773 -0.002949 -0.072009"
+                    quat="0.716879 -0.0964829 -0.0679942 0.687134" mass="0.734"
+                    diaginertia="0.00106187 0.00103217 0.000400661" />
+                  <joint name="left_shoulder_yaw_joint" pos="0 0 0" axis="0 0 1"
+                    range="-2.618 2.618" actuatorfrcrange="-25 25" class="arm_motor" />
+                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                    rgba="0.7 0.7 0.7 1" mesh="left_shoulder_yaw_link" />
+                  <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_shoulder_yaw_link" />
+                  <body name="left_elbow_link" pos="0.015783 0 -0.080518">
+                    <inertial pos="0.064956 0.004454 -0.010062"
+                      quat="0.541765 0.636132 0.388821 0.388129" mass="0.6"
+                      diaginertia="0.000443035 0.000421612 0.000259353" />
+                    <joint name="left_elbow_joint" pos="0 0 0" axis="0 1 0" range="-1.0472 2.0944"
+                      actuatorfrcrange="-25 25" class="arm_motor" />
+                    <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                      rgba="0.7 0.7 0.7 1" mesh="left_elbow_link" />
+                    <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_elbow_link" />
+                    <body name="left_wrist_roll_link" pos="0.1 0.00188791 -0.01">
+                      <inertial pos="0.0171394 0.000537591 4.8864e-07"
+                        quat="0.575338 0.411667 -0.574906 0.411094" mass="0.085445"
+                        diaginertia="5.48211e-05 4.96646e-05 3.57798e-05" />
+                      <joint name="left_wrist_roll_joint" pos="0 0 0" axis="1 0 0"
+                        range="-1.97222 1.97222" actuatorfrcrange="-25 25" class="arm_motor" />
+                      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                        rgba="0.7 0.7 0.7 1" mesh="left_wrist_roll_link" />
+                      <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_wrist_roll_link" />
+                      <body name="left_wrist_pitch_link" pos="0.038 0 0">
+                        <inertial pos="0.0229999 -0.00111685 -0.00111658"
+                          quat="0.249998 0.661363 0.293036 0.643608" mass="0.48405"
+                          diaginertia="0.000430353 0.000429873 0.000164648" />
+                        <joint name="left_wrist_pitch_joint" pos="0 0 0" axis="0 1 0"
+                          range="-1.61443 1.61443" actuatorfrcrange="-5 5" class="wrist_motor" />
+                        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                          rgba="0.7 0.7 0.7 1" mesh="left_wrist_pitch_link" />
+                        <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_wrist_pitch_link" />
+                        <body name="left_wrist_yaw_link" pos="0.046 0 0">
+                          <inertial pos="0.0885506 0.00212216 -0.000374562"
+                            quat="0.487149 0.493844 0.513241 0.505358" mass="0.457415"
+                            diaginertia="0.00105989 0.000895419 0.000323842" />
+                          <joint name="left_wrist_yaw_joint" pos="0 0 0" axis="0 0 1"
+                            range="-1.61443 1.61443" actuatorfrcrange="-5 5" class="wrist_motor" />
+                          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                            rgba="0.7 0.7 0.7 1" mesh="left_wrist_yaw_link" />
+                          <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_wrist_yaw_link" />
+                          <geom pos="0.0415 0.003 0" quat="1 0 0 0" type="mesh" contype="0"
+                            conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
+                            mesh="left_hand_palm_link" />
+                          <geom pos="0.0415 0.003 0" quat="1 0 0 0" type="mesh" rgba="0.7 0.7 0.7 1"
+                            mesh="left_hand_palm_link" />
+                          <body name="left_hand_thumb_0_link" pos="0.067 0.003 0">
+                            <inertial pos="-0.000884246 -0.00863407 0.000944293"
+                              quat="0.462991 0.643965 -0.460173 0.398986" mass="0.0862366"
+                              diaginertia="1.6546e-05 1.60058e-05 1.43741e-05" />
+                            <joint name="left_hand_thumb_0_joint" pos="0 0 0" axis="0 1 0"
+                              range="-1.0472 1.0472" actuatorfrcrange="-2.45 2.45"
+                              class="finger_motor" />
+                            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                              rgba="0.7 0.7 0.7 1" mesh="left_hand_thumb_0_link" />
+                            <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_hand_thumb_0_link" />
+                            <body name="left_hand_thumb_1_link" pos="-0.0025 -0.0193 0">
+                              <inertial pos="-0.000827888 -0.0354744 -0.0003809"
+                                quat="0.685598 0.705471 -0.15207 0.0956069" mass="0.0588507"
+                                diaginertia="1.28514e-05 1.22902e-05 5.9666e-06" />
+                              <joint name="left_hand_thumb_1_joint" pos="0 0 0" axis="0 0 1"
+                                range="-0.724312 1.0472" actuatorfrcrange="-1.4 1.4"
+                                class="finger_motor" />
+                              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                                rgba="0.7 0.7 0.7 1" mesh="left_hand_thumb_1_link" />
+                              <geom size="0.01 0.015 0.01" pos="-0.001 -0.032 0" type="box"
+                                rgba="0.7 0.7 0.7 1" />
+                              <body name="left_hand_thumb_2_link" pos="0 -0.0458 0">
+                                <inertial pos="-0.00171735 -0.0262819 0.000107789"
+                                  quat="0.703174 0.710977 -0.00017564 -0.00766553" mass="0.0203063"
+                                  diaginertia="4.61314e-06 3.86645e-06 1.53495e-06" />
+                                <joint name="left_hand_thumb_2_joint" pos="0 0 0" axis="0 0 1"
+                                  range="0 1.74533" actuatorfrcrange="-1.4 1.4" class="finger_motor" />
+                                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                                  rgba="0.7 0.7 0.7 1" mesh="left_hand_thumb_2_link" />
+                                <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_hand_thumb_2_link" />
+                              </body>
+                            </body>
+                          </body>
+                          <body name="left_hand_middle_0_link" pos="0.1192 0.0046 -0.0285">
+                            <inertial pos="0.0354744 0.000827888 0.0003809"
+                              quat="0.391313 0.552395 0.417187 0.606373" mass="0.0588507"
+                              diaginertia="1.28514e-05 1.22902e-05 5.9666e-06" />
+                            <joint name="left_hand_middle_0_joint" pos="0 0 0" axis="0 0 1"
+                              range="-1.5708 0" actuatorfrcrange="-1.4 1.4" class="finger_motor" />
+                            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                              rgba="0.7 0.7 0.7 1" mesh="left_hand_middle_0_link" />
+                            <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_hand_middle_0_link" />
+                            <body name="left_hand_middle_1_link" pos="0.0458 0 0">
+                              <inertial pos="0.0262819 0.00171735 -0.000107789"
+                                quat="0.502612 0.491799 0.502639 0.502861" mass="0.0203063"
+                                diaginertia="4.61314e-06 3.86645e-06 1.53495e-06" />
+                              <joint name="left_hand_middle_1_joint" pos="0 0 0" axis="0 0 1"
+                                range="-1.74533 0" actuatorfrcrange="-1.4 1.4" class="finger_motor" />
+                              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                                rgba="0.7 0.7 0.7 1" mesh="left_hand_middle_1_link" />
+                              <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_hand_middle_1_link" />
+                            </body>
+                          </body>
+                          <body name="left_hand_index_0_link" pos="0.1192 0.0046 0.0285">
+                            <inertial pos="0.0354744 0.000827888 0.0003809"
+                              quat="0.391313 0.552395 0.417187 0.606373" mass="0.0588507"
+                              diaginertia="1.28514e-05 1.22902e-05 5.9666e-06" />
+                            <joint name="left_hand_index_0_joint" pos="0 0 0" axis="0 0 1"
+                              range="-1.5708 0" actuatorfrcrange="-1.4 1.4" class="finger_motor" />
+                            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                              rgba="0.7 0.7 0.7 1" mesh="left_hand_index_0_link" />
+                            <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_hand_index_0_link" />
+                            <body name="left_hand_index_1_link" pos="0.0458 0 0">
+                              <inertial pos="0.0262819 0.00171735 -0.000107789"
+                                quat="0.502612 0.491799 0.502639 0.502861" mass="0.0203063"
+                                diaginertia="4.61314e-06 3.86645e-06 1.53495e-06" />
+                              <joint name="left_hand_index_1_joint" pos="0 0 0" axis="0 0 1"
+                                range="-1.74533 0" actuatorfrcrange="-1.4 1.4" class="finger_motor" />
+                              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                                rgba="0.7 0.7 0.7 1" mesh="left_hand_index_1_link" />
+                              <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_hand_index_1_link" />
+                            </body>
+                          </body>
+                        </body>
+                      </body>
+                    </body>
+                  </body>
+                </body>
+              </body>
+            </body>
+            <body name="right_shoulder_pitch_link" pos="0.0039563 -0.10021 0.23778"
+              quat="0.990264 -0.139201 1.38722e-05 9.86868e-05">
+              <inertial pos="0 -0.035892 -0.011628" quat="0.68225 -0.326267 0.0130458 0.654152"
+                mass="0.718" diaginertia="0.000465864 0.000432842 0.000406394" />
+              <joint name="right_shoulder_pitch_joint" pos="0 0 0" axis="0 1 0"
+                range="-3.0892 2.6704" actuatorfrcrange="-25 25" class="arm_motor" />
+              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                rgba="0.7 0.7 0.7 1" mesh="right_shoulder_pitch_link" />
+              <geom size="0.03 0.025" pos="0 -0.04 -0.01" quat="0.707107 0 0.707107 0"
+                type="cylinder" rgba="0.7 0.7 0.7 1" />
+              <body name="right_shoulder_roll_link" pos="0 -0.038 -0.013831"
+                quat="0.990268 0.139172 0 0">
+                <inertial pos="-0.000227 -0.00727 -0.063243"
+                  quat="0.712604 -0.00710317 -0.0196223 0.701256" mass="0.643"
+                  diaginertia="0.000691311 0.000618011 0.000388977" />
+                <joint name="right_shoulder_roll_joint" pos="0 0 0" axis="1 0 0"
+                  range="-2.2515 1.5882" actuatorfrcrange="-25 25" class="arm_motor" />
+                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                  rgba="0.7 0.7 0.7 1" mesh="right_shoulder_roll_link" />
+                <geom size="0.03 0.015" pos="-0.004 -0.006 -0.053" type="cylinder"
+                  rgba="0.7 0.7 0.7 1" />
+                <body name="right_shoulder_yaw_link" pos="0 -0.00624 -0.1032">
+                  <inertial pos="0.010773 0.002949 -0.072009"
+                    quat="0.687134 -0.0679942 -0.0964829 0.716879" mass="0.734"
+                    diaginertia="0.00106187 0.00103217 0.000400661" />
+                  <joint name="right_shoulder_yaw_joint" pos="0 0 0" axis="0 0 1"
+                    range="-2.618 2.618" actuatorfrcrange="-25 25" class="arm_motor" />
+                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                    rgba="0.7 0.7 0.7 1" mesh="right_shoulder_yaw_link" />
+                  <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_shoulder_yaw_link" />
+                  <body name="right_elbow_link" pos="0.015783 0 -0.080518">
+                    <inertial pos="0.064956 -0.004454 -0.010062"
+                      quat="0.388129 0.388821 0.636132 0.541765" mass="0.6"
+                      diaginertia="0.000443035 0.000421612 0.000259353" />
+                    <joint name="right_elbow_joint" pos="0 0 0" axis="0 1 0" range="-1.0472 2.0944"
+                      actuatorfrcrange="-25 25" class="arm_motor" />
+                    <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                      rgba="0.7 0.7 0.7 1" mesh="right_elbow_link" />
+                    <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_elbow_link" />
+                    <body name="right_wrist_roll_link" pos="0.1 -0.00188791 -0.01">
+                      <inertial pos="0.0171394 -0.000537591 4.8864e-07"
+                        quat="0.411667 0.575338 -0.411094 0.574906" mass="0.085445"
+                        diaginertia="5.48211e-05 4.96646e-05 3.57798e-05" />
+                      <joint name="right_wrist_roll_joint" pos="0 0 0" axis="1 0 0"
+                        range="-1.97222 1.97222" actuatorfrcrange="-25 25" class="arm_motor" />
+                      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                        rgba="0.7 0.7 0.7 1" mesh="right_wrist_roll_link" />
+                      <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_wrist_roll_link" />
+                      <body name="right_wrist_pitch_link" pos="0.038 0 0">
+                        <inertial pos="0.0229999 0.00111685 -0.00111658"
+                          quat="0.643608 0.293036 0.661363 0.249998" mass="0.48405"
+                          diaginertia="0.000430353 0.000429873 0.000164648" />
+                        <joint name="right_wrist_pitch_joint" pos="0 0 0" axis="0 1 0"
+                          range="-1.61443 1.61443" actuatorfrcrange="-5 5" class="wrist_motor" />
+                        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                          rgba="0.7 0.7 0.7 1" mesh="right_wrist_pitch_link" />
+                        <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_wrist_pitch_link" />
+                        <body name="right_wrist_yaw_link" pos="0.046 0 0">
+                          <inertial pos="0.0885506 -0.00212216 0.000573742"
+                            quat="0.507224 0.511377 0.494292 0.486717" mass="0.457415"
+                            diaginertia="0.0010598 0.000895373 0.0003238" />
+                          <joint name="right_wrist_yaw_joint" pos="0 0 0" axis="0 0 1"
+                            range="-1.61443 1.61443" actuatorfrcrange="-5 5" class="wrist_motor" />
+                          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                            rgba="0.7 0.7 0.7 1" mesh="right_wrist_yaw_link" />
+                          <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_wrist_yaw_link" />
+                          <geom pos="0.0415 -0.003 0" quat="1 0 0 0" type="mesh" contype="0"
+                            conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
+                            mesh="right_hand_palm_link" />
+                          <geom pos="0.0415 -0.003 0" quat="1 0 0 0" type="mesh"
+                            rgba="0.7 0.7 0.7 1" mesh="right_hand_palm_link" />
+                          <body name="right_hand_thumb_0_link" pos="0.067 -0.003 0">
+                              <inertial pos="-0.000884246 0.00863407 0.000944293"
+                                quat="0.643965 0.462991 -0.398986 0.460173" mass="0.0862366"
+                                diaginertia="1.6546e-05 1.60058e-05 1.43741e-05" />
+                              <joint name="right_hand_thumb_0_joint" pos="0 0 0" axis="0 1 0"
+                                range="-1.0472 1.0472" actuatorfrcrange="-2.45 2.45"
+                                class="finger_motor" />
+                              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                                rgba="0.7 0.7 0.7 1" mesh="right_hand_thumb_0_link" />
+                              <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_hand_thumb_0_link" />
+                              <body name="right_hand_thumb_1_link" pos="-0.0025 0.0193 0">
+                                <inertial pos="-0.000827888 0.0354744 -0.0003809"
+                                  quat="0.705471 0.685598 -0.0956069 0.15207" mass="0.0588507"
+                                  diaginertia="1.28514e-05 1.22902e-05 5.9666e-06" />
+                                <joint name="right_hand_thumb_1_joint" pos="0 0 0" axis="0 0 1"
+                                  range="-1.0472 0.724312" actuatorfrcrange="-1.4 1.4"
+                                  class="finger_motor" />
+                                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                                  rgba="0.7 0.7 0.7 1" mesh="right_hand_thumb_1_link" />
+                                <geom size="0.01 0.015 0.01" pos="-0.001 0.032 0" type="box"
+                                  rgba="0.7 0.7 0.7 1" />
+                                <body name="right_hand_thumb_2_link" pos="0 0.0458 0">
+                                  <inertial pos="-0.00171735 0.0262819 0.000107789"
+                                    quat="0.710977 0.703174 0.00766553 0.00017564" mass="0.0203063"
+                                    diaginertia="4.61314e-06 3.86645e-06 1.53495e-06" />
+                                  <joint name="right_hand_thumb_2_joint" pos="0 0 0" axis="0 0 1"
+                                    range="-1.74533 0" actuatorfrcrange="-1.4 1.4"
+                                    class="finger_motor" />
+                                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                                    rgba="0.7 0.7 0.7 1" mesh="right_hand_thumb_2_link" />
+                                  <geom type="mesh" rgba="0.7 0.7 0.7 1"
+                                    mesh="right_hand_thumb_2_link" />
+                                </body>
+                              </body>
+                          </body>
+                          <body name="right_hand_middle_0_link" pos="0.1192 -0.0046 -0.0285">
+                            <inertial pos="0.0354744 -0.000827888 0.0003809"
+                              quat="0.606373 0.417187 0.552395 0.391313" mass="0.0588507"
+                              diaginertia="1.28514e-05 1.22902e-05 5.9666e-06" />
+                            <joint name="right_hand_middle_0_joint" pos="0 0 0" axis="0 0 1"
+                              range="0 1.5708" actuatorfrcrange="-1.4 1.4" class="finger_motor" />
+                            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                              rgba="0.7 0.7 0.7 1" mesh="right_hand_middle_0_link" />
+                            <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_hand_middle_0_link" />
+                            <body name="right_hand_middle_1_link" pos="0.0458 0 0">
+                              <inertial pos="0.0262819 -0.00171735 -0.000107789"
+                                quat="0.502861 0.502639 0.491799 0.502612" mass="0.0203063"
+                                diaginertia="4.61314e-06 3.86645e-06 1.53495e-06" />
+                              <joint name="right_hand_middle_1_joint" pos="0 0 0" axis="0 0 1"
+                                range="0 1.74533" actuatorfrcrange="-1.4 1.4" class="finger_motor" />
+                              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                                rgba="0.7 0.7 0.7 1" mesh="right_hand_middle_1_link" />
+                              <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_hand_middle_1_link" />
+                            </body>
+                          </body>
+                          <body name="right_hand_index_0_link" pos="0.1192 -0.0046 0.0285">
+                            <inertial pos="0.0354744 -0.000827888 0.0003809"
+                              quat="0.606373 0.417187 0.552395 0.391313" mass="0.0588507"
+                              diaginertia="1.28514e-05 1.22902e-05 5.9666e-06" />
+                            <joint name="right_hand_index_0_joint" pos="0 0 0" axis="0 0 1"
+                              range="0 1.5708" actuatorfrcrange="-1.4 1.4" class="finger_motor" />
+                            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                              rgba="0.7 0.7 0.7 1" mesh="right_hand_index_0_link" />
+                            <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_hand_index_0_link" />
+                            <body name="right_hand_index_1_link" pos="0.0458 0 0">
+                              <inertial pos="0.0262819 -0.00171735 -0.000107789"
+                                quat="0.502861 0.502639 0.491799 0.502612" mass="0.0203063"
+                                diaginertia="4.61314e-06 3.86645e-06 1.53495e-06" />
+                              <joint name="right_hand_index_1_joint" pos="0 0 0" axis="0 0 1"
+                                range="0 1.74533" actuatorfrcrange="-1.4 1.4" class="finger_motor" />
+                              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                                rgba="0.7 0.7 0.7 1" mesh="right_hand_index_1_link" />
+                              <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_hand_index_1_link" />
+                            </body>
+                          </body>
+                        </body>
+                      </body>
+                    </body>
+                  </body>
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+    </body>
+  </worldbody>
+
+  <actuator>
+    <motor name="left_hip_pitch" joint="left_hip_pitch_joint" ctrlrange="-88 88" />
+    <motor name="left_hip_roll" joint="left_hip_roll_joint" ctrlrange="-88 88" />
+    <motor name="left_hip_yaw" joint="left_hip_yaw_joint" ctrlrange="-88 88" />
+    <motor name="left_knee" joint="left_knee_joint" ctrlrange="-139 139" />
+    <motor name="left_ankle_pitch" joint="left_ankle_pitch_joint" ctrlrange="-50 50" />
+    <motor name="left_ankle_roll" joint="left_ankle_roll_joint" ctrlrange="-50 50" />
+
+    <motor name="right_hip_pitch" joint="right_hip_pitch_joint" ctrlrange="-88 88" />
+    <motor name="right_hip_roll" joint="right_hip_roll_joint" ctrlrange="-88 88" />
+    <motor name="right_hip_yaw" joint="right_hip_yaw_joint" ctrlrange="-88 88" />
+    <motor name="right_knee" joint="right_knee_joint" ctrlrange="-139 139" />
+    <motor name="right_ankle_pitch" joint="right_ankle_pitch_joint" ctrlrange="-50 50" />
+    <motor name="right_ankle_roll" joint="right_ankle_roll_joint" ctrlrange="-50 50" />
+
+    <motor name="waist_yaw" joint="waist_yaw_joint" ctrlrange="-88 88" />
+    <motor name="waist_roll" joint="waist_roll_joint" ctrlrange="-50 50" />
+    <motor name="waist_pitch" joint="waist_pitch_joint" ctrlrange="-50 50" />
+
+    <motor name="left_shoulder_pitch" joint="left_shoulder_pitch_joint" ctrlrange="-25 25" />
+    <motor name="left_shoulder_roll" joint="left_shoulder_roll_joint" ctrlrange="-25 25" />
+    <motor name="left_shoulder_yaw" joint="left_shoulder_yaw_joint" ctrlrange="-25 25" />
+    <motor name="left_elbow" joint="left_elbow_joint" ctrlrange="-25 25" />
+    <motor name="left_wrist_roll" joint="left_wrist_roll_joint" ctrlrange="-25 25" />
+    <motor name="left_wrist_pitch" joint="left_wrist_pitch_joint" ctrlrange="-5 5" />
+    <motor name="left_wrist_yaw" joint="left_wrist_yaw_joint" ctrlrange="-5 5" />
+
+    <motor name="left_hand_thumb_0" joint="left_hand_thumb_0_joint" ctrlrange="-2.45 2.45" />
+    <motor name="left_hand_thumb_1" joint="left_hand_thumb_1_joint" ctrlrange="-1.4 1.4" />
+    <motor name="left_hand_thumb_2" joint="left_hand_thumb_2_joint" ctrlrange="-1.4 1.4" />
+    <motor name="left_hand_middle_0" joint="left_hand_middle_0_joint" ctrlrange="-1.4 1.4" />
+    <motor name="left_hand_middle_1" joint="left_hand_middle_1_joint" ctrlrange="-1.4 1.4" />
+    <motor name="left_hand_index_0" joint="left_hand_index_0_joint" ctrlrange="-1.4 1.4" />
+    <motor name="left_hand_index_1" joint="left_hand_index_1_joint" ctrlrange="-1.4 1.4" />
+
+    <motor name="right_shoulder_pitch" joint="right_shoulder_pitch_joint" ctrlrange="-25 25" />
+    <motor name="right_shoulder_roll" joint="right_shoulder_roll_joint" ctrlrange="-25 25" />
+    <motor name="right_shoulder_yaw" joint="right_shoulder_yaw_joint" ctrlrange="-25 25" />
+    <motor name="right_elbow" joint="right_elbow_joint" ctrlrange="-25 25" />
+    <motor name="right_wrist_roll" joint="right_wrist_roll_joint" ctrlrange="-25 25" />
+    <motor name="right_wrist_pitch" joint="right_wrist_pitch_joint" ctrlrange="-5 5" />
+    <motor name="right_wrist_yaw" joint="right_wrist_yaw_joint" ctrlrange="-5 5" />
+
+    <motor name="right_hand_thumb_0" joint="right_hand_thumb_0_joint" ctrlrange="-2.45 2.45" />
+    <motor name="right_hand_thumb_1" joint="right_hand_thumb_1_joint" ctrlrange="-1.4 1.4" />
+    <motor name="right_hand_thumb_2" joint="right_hand_thumb_2_joint" ctrlrange="-1.4 1.4" />
+    <motor name="right_hand_middle_0" joint="right_hand_middle_0_joint" ctrlrange="-1.4 1.4" />
+    <motor name="right_hand_middle_1" joint="right_hand_middle_1_joint" ctrlrange="-1.4 1.4" />
+    <motor name="right_hand_index_0" joint="right_hand_index_0_joint" ctrlrange="-1.4 1.4" />
+    <motor name="right_hand_index_1" joint="right_hand_index_1_joint" ctrlrange="-1.4 1.4" />
+  </actuator>
+
+  <sensor>
+    <jointpos name="left_hip_pitch_pos" joint="left_hip_pitch_joint" />
+    <jointpos name="left_hip_roll_pos" joint="left_hip_roll_joint" />
+    <jointpos name="left_hip_yaw_pos" joint="left_hip_yaw_joint" />
+    <jointpos name="left_knee_pos" joint="left_knee_joint" />
+    <jointpos name="left_ankle_pitch_pos" joint="left_ankle_pitch_joint" />
+    <jointpos name="left_ankle_roll_pos" joint="left_ankle_roll_joint" />
+    <jointpos name="right_hip_pitch_pos" joint="right_hip_pitch_joint" />
+    <jointpos name="right_hip_roll_pos" joint="right_hip_roll_joint" />
+    <jointpos name="right_hip_yaw_pos" joint="right_hip_yaw_joint" />
+    <jointpos name="right_knee_pos" joint="right_knee_joint" />
+    <jointpos name="right_ankle_pitch_pos" joint="right_ankle_pitch_joint" />
+    <jointpos name="right_ankle_roll_pos" joint="right_ankle_roll_joint" />
+    <jointpos name="waist_yaw_pos" joint="waist_yaw_joint" />
+    <jointpos name="waist_roll_pos" joint="waist_roll_joint" />
+    <jointpos name="waist_pitch_pos" joint="waist_pitch_joint" />
+    <jointpos name="left_shoulder_pitch_pos" joint="left_shoulder_pitch_joint" />
+    <jointpos name="left_shoulder_roll_pos" joint="left_shoulder_roll_joint" />
+    <jointpos name="left_shoulder_yaw_pos" joint="left_shoulder_yaw_joint" />
+    <jointpos name="left_elbow_pos" joint="left_elbow_joint" />
+    <jointpos name="left_wrist_roll_pos" joint="left_wrist_roll_joint" />
+    <jointpos name="left_wrist_pitch_pos" joint="left_wrist_pitch_joint" />
+    <jointpos name="left_wrist_yaw_pos" joint="left_wrist_yaw_joint" />
+    <jointpos name="right_shoulder_pitch_pos" joint="right_shoulder_pitch_joint" />
+    <jointpos name="right_shoulder_roll_pos" joint="right_shoulder_roll_joint" />
+    <jointpos name="right_shoulder_yaw_pos" joint="right_shoulder_yaw_joint" />
+    <jointpos name="right_elbow_pos" joint="right_elbow_joint" />
+    <jointpos name="right_wrist_roll_pos" joint="right_wrist_roll_joint" />
+    <jointpos name="right_wrist_pitch_pos" joint="right_wrist_pitch_joint" />
+    <jointpos name="right_wrist_yaw_pos" joint="right_wrist_yaw_joint" />
+
+    <jointpos name="left_hand_thumb_0_pos" joint="left_hand_thumb_0_joint" />
+    <jointpos name="left_hand_thumb_1_pos" joint="left_hand_thumb_1_joint" />
+    <jointpos name="left_hand_thumb_2_pos" joint="left_hand_thumb_2_joint" />
+    <jointpos name="left_hand_middle_0_pos" joint="left_hand_middle_0_joint" />
+    <jointpos name="left_hand_middle_1_pos" joint="left_hand_middle_1_joint" />
+    <jointpos name="left_hand_index_0_pos" joint="left_hand_index_0_joint" />
+    <jointpos name="left_hand_index_1_pos" joint="left_hand_index_1_joint" />
+    <jointpos name="right_hand_thumb_0_pos" joint="right_hand_thumb_0_joint" />
+    <jointpos name="right_hand_thumb_1_pos" joint="right_hand_thumb_1_joint" />
+    <jointpos name="right_hand_thumb_2_pos" joint="right_hand_thumb_2_joint" />
+    <jointpos name="right_hand_middle_0_pos" joint="right_hand_middle_0_joint" />
+    <jointpos name="right_hand_middle_1_pos" joint="right_hand_middle_1_joint" />
+    <jointpos name="right_hand_index_0_pos" joint="right_hand_index_0_joint" />
+    <jointpos name="right_hand_index_1_pos" joint="right_hand_index_1_joint" />
+
+    <jointvel name="left_hip_pitch_vel" joint="left_hip_pitch_joint" />
+    <jointvel name="left_hip_roll_vel" joint="left_hip_roll_joint" />
+    <jointvel name="left_hip_yaw_vel" joint="left_hip_yaw_joint" />
+    <jointvel name="left_knee_vel" joint="left_knee_joint" />
+    <jointvel name="left_ankle_pitch_vel" joint="left_ankle_pitch_joint" />
+    <jointvel name="left_ankle_roll_vel" joint="left_ankle_roll_joint" />
+    <jointvel name="right_hip_pitch_vel" joint="right_hip_pitch_joint" />
+    <jointvel name="right_hip_roll_vel" joint="right_hip_roll_joint" />
+    <jointvel name="right_hip_yaw_vel" joint="right_hip_yaw_joint" />
+    <jointvel name="right_knee_vel" joint="right_knee_joint" />
+    <jointvel name="right_ankle_pitch_vel" joint="right_ankle_pitch_joint" />
+    <jointvel name="right_ankle_roll_vel" joint="right_ankle_roll_joint" />
+    <jointvel name="waist_yaw_vel" joint="waist_yaw_joint" />
+    <jointvel name="waist_roll_vel" joint="waist_roll_joint" />
+    <jointvel name="waist_pitch_vel" joint="waist_pitch_joint" />
+    <jointvel name="left_shoulder_pitch_vel" joint="left_shoulder_pitch_joint" />
+    <jointvel name="left_shoulder_roll_vel" joint="left_shoulder_roll_joint" />
+    <jointvel name="left_shoulder_yaw_vel" joint="left_shoulder_yaw_joint" />
+    <jointvel name="left_elbow_vel" joint="left_elbow_joint" />
+    <jointvel name="left_wrist_roll_vel" joint="left_wrist_roll_joint" />
+    <jointvel name="left_wrist_pitch_vel" joint="left_wrist_pitch_joint" />
+    <jointvel name="left_wrist_yaw_vel" joint="left_wrist_yaw_joint" />
+    <jointvel name="right_shoulder_pitch_vel" joint="right_shoulder_pitch_joint" />
+    <jointvel name="right_shoulder_roll_vel" joint="right_shoulder_roll_joint" />
+    <jointvel name="right_shoulder_yaw_vel" joint="right_shoulder_yaw_joint" />
+    <jointvel name="right_elbow_vel" joint="right_elbow_joint" />
+    <jointvel name="right_wrist_roll_vel" joint="right_wrist_roll_joint" />
+    <jointvel name="right_wrist_pitch_vel" joint="right_wrist_pitch_joint" />
+    <jointvel name="right_wrist_yaw_vel" joint="right_wrist_yaw_joint" />
+
+    <jointvel name="left_hand_thumb_0_vel" joint="left_hand_thumb_0_joint" />
+    <jointvel name="left_hand_thumb_1_vel" joint="left_hand_thumb_1_joint" />
+    <jointvel name="left_hand_thumb_2_vel" joint="left_hand_thumb_2_joint" />
+    <jointvel name="left_hand_middle_0_vel" joint="left_hand_middle_0_joint" />
+    <jointvel name="left_hand_middle_1_vel" joint="left_hand_middle_1_joint" />
+    <jointvel name="left_hand_index_0_vel" joint="left_hand_index_0_joint" />
+    <jointvel name="left_hand_index_1_vel" joint="left_hand_index_1_joint" />
+    <jointvel name="right_hand_thumb_0_vel" joint="right_hand_thumb_0_joint" />
+    <jointvel name="right_hand_thumb_1_vel" joint="right_hand_thumb_1_joint" />
+    <jointvel name="right_hand_thumb_2_vel" joint="right_hand_thumb_2_joint" />
+    <jointvel name="right_hand_middle_0_vel" joint="right_hand_middle_0_joint" />
+    <jointvel name="right_hand_middle_1_vel" joint="right_hand_middle_1_joint" />
+    <jointvel name="right_hand_index_0_vel" joint="right_hand_index_0_joint" />
+    <jointvel name="right_hand_index_1_vel" joint="right_hand_index_1_joint" />
+
+    <jointactuatorfrc name="left_hip_pitch_torque" joint="left_hip_pitch_joint" />
+    <jointactuatorfrc name="left_hip_roll_torque" joint="left_hip_roll_joint" />
+    <jointactuatorfrc name="left_hip_yaw_torque" joint="left_hip_yaw_joint" />
+    <jointactuatorfrc name="left_knee_torque" joint="left_knee_joint" />
+    <jointactuatorfrc name="left_ankle_pitch_torque" joint="left_ankle_pitch_joint" />
+    <jointactuatorfrc name="left_ankle_roll_torque" joint="left_ankle_roll_joint" />
+    <jointactuatorfrc name="right_hip_pitch_torque" joint="right_hip_pitch_joint" />
+    <jointactuatorfrc name="right_hip_roll_torque" joint="right_hip_roll_joint" />
+    <jointactuatorfrc name="right_hip_yaw_torque" joint="right_hip_yaw_joint" />
+    <jointactuatorfrc name="right_knee_torque" joint="right_knee_joint" />
+    <jointactuatorfrc name="right_ankle_pitch_torque" joint="right_ankle_pitch_joint" />
+    <jointactuatorfrc name="right_ankle_roll_torque" joint="right_ankle_roll_joint" />
+    <jointactuatorfrc name="waist_yaw_torque" joint="waist_yaw_joint" />
+    <jointactuatorfrc name="waist_roll_torque" joint="waist_roll_joint" />
+    <jointactuatorfrc name="waist_pitch_torque" joint="waist_pitch_joint" />
+    <jointactuatorfrc name="left_shoulder_pitch_torque" joint="left_shoulder_pitch_joint" />
+    <jointactuatorfrc name="left_shoulder_roll_torque" joint="left_shoulder_roll_joint" />
+    <jointactuatorfrc name="left_shoulder_yaw_torque" joint="left_shoulder_yaw_joint" />
+    <jointactuatorfrc name="left_elbow_torque" joint="left_elbow_joint" />
+    <jointactuatorfrc name="left_wrist_roll_torque" joint="left_wrist_roll_joint" />
+    <jointactuatorfrc name="left_wrist_pitch_torque" joint="left_wrist_pitch_joint" />
+    <jointactuatorfrc name="left_wrist_yaw_torque" joint="left_wrist_yaw_joint" />
+    <jointactuatorfrc name="right_shoulder_pitch_torque" joint="right_shoulder_pitch_joint" />
+    <jointactuatorfrc name="right_shoulder_roll_torque" joint="right_shoulder_roll_joint" />
+    <jointactuatorfrc name="right_shoulder_yaw_torque" joint="right_shoulder_yaw_joint" />
+    <jointactuatorfrc name="right_elbow_torque" joint="right_elbow_joint" />
+    <jointactuatorfrc name="right_wrist_roll_torque" joint="right_wrist_roll_joint" />
+    <jointactuatorfrc name="right_wrist_pitch_torque" joint="right_wrist_pitch_joint" />
+    <jointactuatorfrc name="right_wrist_yaw_torque" joint="right_wrist_yaw_joint" />
+
+    <jointactuatorfrc name="left_hand_thumb_0_torque" joint="left_hand_thumb_0_joint" />
+    <jointactuatorfrc name="left_hand_thumb_1_torque" joint="left_hand_thumb_1_joint" />
+    <jointactuatorfrc name="left_hand_thumb_2_torque" joint="left_hand_thumb_2_joint" />
+    <jointactuatorfrc name="left_hand_middle_0_torque" joint="left_hand_middle_0_joint" />
+    <jointactuatorfrc name="left_hand_middle_1_torque" joint="left_hand_middle_1_joint" />
+    <jointactuatorfrc name="left_hand_index_0_torque" joint="left_hand_index_0_joint" />
+    <jointactuatorfrc name="left_hand_index_1_torque" joint="left_hand_index_1_joint" />
+    <jointactuatorfrc name="right_hand_thumb_0_torque" joint="right_hand_thumb_0_joint" />
+    <jointactuatorfrc name="right_hand_thumb_1_torque" joint="right_hand_thumb_1_joint" />
+    <jointactuatorfrc name="right_hand_thumb_2_torque" joint="right_hand_thumb_2_joint" />
+    <jointactuatorfrc name="right_hand_middle_0_torque" joint="right_hand_middle_0_joint" />
+    <jointactuatorfrc name="right_hand_middle_1_torque" joint="right_hand_middle_1_joint" />
+    <jointactuatorfrc name="right_hand_index_0_torque" joint="right_hand_index_0_joint" />
+    <jointactuatorfrc name="right_hand_index_1_torque" joint="right_hand_index_1_joint" />
+
+    <framequat name="imu_quat" objtype="site" objname="imu" />
+    <gyro name="imu_gyro" site="imu" />
+    <accelerometer name="imu_acc" site="imu" />
+
+    <framepos name="frame_pos" objtype="site" objname="imu" />
+    <framelinvel name="frame_vel" objtype="site" objname="imu" />
+  </sensor>
+</mujoco>

GR00T-WholeBodyControl/decoupled_wbc/control/robot_model/model_data/g1/g1_29dof_with_hand_rev_1_0_activatedfinger.xml

@@ -0,0 +1,669 @@
+<mujoco model="g1_29dof_with_hand_rev_1_0">
+  <compiler angle="radian" meshdir="meshes" />
+  <default>
+    <default class="torso_motor">
+      <joint damping="0.05" armature="0.01" frictionloss="0.2" />
+    </default>
+    <default class="leg_motor">
+      <joint damping="0.05" armature="0.01" frictionloss="0.2" />
+    </default>
+    <default class="ankle_motor">
+      <joint damping="0.05" armature="0.01" frictionloss="0.2" />
+    </default>
+    <default class="arm_motor">
+      <joint damping="0.05" armature="0.01" frictionloss="0.2" />
+    </default>
+    <default class="wrist_motor">
+      <joint damping="0.05" armature="0.01" frictionloss="0.1" />
+    </default>
+    <default class="finger_motor">
+      <joint damping="0.05" armature="0.01" frictionloss="0.1" />
+    </default>
+
+  </default>
+
+  <asset>
+    <mesh name="pelvis" file="pelvis.STL" />
+    <mesh name="pelvis_contour_link" file="pelvis_contour_link.STL" />
+    <mesh name="left_hip_pitch_link" file="left_hip_pitch_link.STL" />
+    <mesh name="left_hip_roll_link" file="left_hip_roll_link.STL" />
+    <mesh name="left_hip_yaw_link" file="left_hip_yaw_link.STL" />
+    <mesh name="left_knee_link" file="left_knee_link.STL" />
+    <mesh name="left_ankle_pitch_link" file="left_ankle_pitch_link.STL" />
+    <mesh name="left_ankle_roll_link" file="left_ankle_roll_link.STL" />
+    <mesh name="right_hip_pitch_link" file="right_hip_pitch_link.STL" />
+    <mesh name="right_hip_roll_link" file="right_hip_roll_link.STL" />
+    <mesh name="right_hip_yaw_link" file="right_hip_yaw_link.STL" />
+    <mesh name="right_knee_link" file="right_knee_link.STL" />
+    <mesh name="right_ankle_pitch_link" file="right_ankle_pitch_link.STL" />
+    <mesh name="right_ankle_roll_link" file="right_ankle_roll_link.STL" />
+    <mesh name="waist_yaw_link" file="waist_yaw_link_rev_1_0.STL" />
+    <mesh name="waist_roll_link" file="waist_roll_link_rev_1_0.STL" />
+    <mesh name="torso_link" file="torso_link_rev_1_0.STL" />
+    <mesh name="logo_link" file="logo_link.STL" />
+    <mesh name="head_link" file="head_link.STL" />
+    <mesh name="left_shoulder_pitch_link" file="left_shoulder_pitch_link.STL" />
+    <mesh name="left_shoulder_roll_link" file="left_shoulder_roll_link.STL" />
+    <mesh name="left_shoulder_yaw_link" file="left_shoulder_yaw_link.STL" />
+    <mesh name="left_elbow_link" file="left_elbow_link.STL" />
+    <mesh name="left_wrist_roll_link" file="left_wrist_roll_link.STL" />
+    <mesh name="left_wrist_pitch_link" file="left_wrist_pitch_link.STL" />
+    <mesh name="left_wrist_yaw_link" file="left_wrist_yaw_link.STL" />
+    <mesh name="left_hand_palm_link" file="left_hand_palm_link.STL" />
+    <mesh name="left_hand_thumb_0_link" file="left_hand_thumb_0_link.STL" />
+    <mesh name="left_hand_thumb_1_link" file="left_hand_thumb_1_link.STL" />
+    <mesh name="left_hand_thumb_2_link" file="left_hand_thumb_2_link.STL" />
+    <mesh name="left_hand_middle_0_link" file="left_hand_middle_0_link.STL" />
+    <mesh name="left_hand_middle_1_link" file="left_hand_middle_1_link.STL" />
+    <mesh name="left_hand_index_0_link" file="left_hand_index_0_link.STL" />
+    <mesh name="left_hand_index_1_link" file="left_hand_index_1_link.STL" />
+    <mesh name="right_shoulder_pitch_link" file="right_shoulder_pitch_link.STL" />
+    <mesh name="right_shoulder_roll_link" file="right_shoulder_roll_link.STL" />
+    <mesh name="right_shoulder_yaw_link" file="right_shoulder_yaw_link.STL" />
+    <mesh name="right_elbow_link" file="right_elbow_link.STL" />
+    <mesh name="right_wrist_roll_link" file="right_wrist_roll_link.STL" />
+    <mesh name="right_wrist_pitch_link" file="right_wrist_pitch_link.STL" />
+    <mesh name="right_wrist_yaw_link" file="right_wrist_yaw_link.STL" />
+    <mesh name="right_hand_palm_link" file="right_hand_palm_link.STL" />
+    <mesh name="right_hand_thumb_0_link" file="right_hand_thumb_0_link.STL" />
+    <mesh name="right_hand_thumb_1_link" file="right_hand_thumb_1_link.STL" />
+    <mesh name="right_hand_thumb_2_link" file="right_hand_thumb_2_link.STL" />
+    <mesh name="right_hand_middle_0_link" file="right_hand_middle_0_link.STL" />
+    <mesh name="right_hand_middle_1_link" file="right_hand_middle_1_link.STL" />
+    <mesh name="right_hand_index_0_link" file="right_hand_index_0_link.STL" />
+    <mesh name="right_hand_index_1_link" file="right_hand_index_1_link.STL" />
+  </asset>
+
+  <worldbody>
+    <body name="pelvis" pos="0 0 0.793">
+      <inertial pos="0 0 -0.07605" quat="1 0 -0.000399148 0" mass="3.813"
+        diaginertia="0.010549 0.0093089 0.0079184" />
+      <joint name="floating_base_joint" type="free" limited="false" actuatorfrclimited="false" />
+      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.2 0.2 0.2 1"
+        mesh="pelvis" />
+      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
+        mesh="pelvis_contour_link" />
+      <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="pelvis_contour_link" />
+      <site name="imu_in_pelvis" size="0.01" pos="0.04525 0 -0.08339" />
+      <body name="left_hip_pitch_link" pos="0 0.064452 -0.1027">
+        <inertial pos="0.002741 0.047791 -0.02606" quat="0.954862 0.293964 0.0302556 0.030122"
+          mass="1.35" diaginertia="0.00181517 0.00153422 0.00116212" />
+        <joint name="left_hip_pitch_joint" pos="0 0 0" axis="0 1 0" range="-2.5307 2.8798"
+          actuatorfrcrange="-88 88" class="leg_motor" />
+        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.2 0.2 0.2 1"
+          mesh="left_hip_pitch_link" />
+        <geom type="mesh" rgba="0.2 0.2 0.2 1" mesh="left_hip_pitch_link" />
+        <body name="left_hip_roll_link" pos="0 0.052 -0.030465" quat="0.996179 0 -0.0873386 0">
+          <inertial pos="0.029812 -0.001045 -0.087934"
+            quat="0.977808 -1.97119e-05 0.205576 -0.0403793" mass="1.52"
+            diaginertia="0.00254986 0.00241169 0.00148755" />
+          <joint name="left_hip_roll_joint" pos="0 0 0" axis="1 0 0" range="-0.5236 2.9671"
+            actuatorfrcrange="-139 139" class="leg_motor" />
+          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
+            mesh="left_hip_roll_link" />
+          <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_hip_roll_link" />
+          <body name="left_hip_yaw_link" pos="0.025001 0 -0.12412">
+            <inertial pos="-0.057709 -0.010981 -0.15078" quat="0.600598 0.15832 0.223482 0.751181"
+              mass="1.702" diaginertia="0.00776166 0.00717575 0.00160139" />
+            <joint name="left_hip_yaw_joint" pos="0 0 0" axis="0 0 1" range="-2.7576 2.7576"
+              actuatorfrcrange="-88 88" class="leg_motor" />
+            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
+              mesh="left_hip_yaw_link" />
+            <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_hip_yaw_link" />
+            <body name="left_knee_link" pos="-0.078273 0.0021489 -0.17734"
+              quat="0.996179 0 0.0873386 0">
+              <inertial pos="0.005457 0.003964 -0.12074"
+                quat="0.923418 -0.0327699 0.0158246 0.382067" mass="1.932"
+                diaginertia="0.0113804 0.0112778 0.00146458" />
+              <joint name="left_knee_joint" pos="0 0 0" axis="0 1 0" range="-0.087267 2.8798"
+                actuatorfrcrange="-139 139" class="leg_motor" />
+              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                rgba="0.7 0.7 0.7 1" mesh="left_knee_link" />
+              <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_knee_link" />
+              <body name="left_ankle_pitch_link" pos="0 -9.4445e-05 -0.30001">
+                <inertial pos="-0.007269 0 0.011137" quat="0.603053 0.369225 0.369225 0.603053"
+                  mass="0.074" diaginertia="1.89e-05 1.40805e-05 6.9195e-06" />
+                <joint name="left_ankle_pitch_joint" pos="0 0 0" axis="0 1 0"
+                  range="-0.87267 0.5236" actuatorfrcrange="-50 50" class="ankle_motor" />
+                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                  rgba="0.7 0.7 0.7 1" mesh="left_ankle_pitch_link" />
+                <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_ankle_pitch_link" />
+                <body name="left_ankle_roll_link" pos="0 0 -0.017558">
+                  <inertial pos="0.026505 0 -0.016425"
+                    quat="-0.000481092 0.728482 -0.000618967 0.685065" mass="0.608"
+                    diaginertia="0.00167218 0.0016161 0.000217621" />
+                  <joint name="left_ankle_roll_joint" pos="0 0 0" axis="1 0 0"
+                    range="-0.2618 0.2618" actuatorfrcrange="-50 50" class="ankle_motor" />
+                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                    rgba="0.2 0.2 0.2 1" mesh="left_ankle_roll_link" />
+                  <geom size="0.005" pos="-0.05 0.025 -0.03" rgba="0.2 0.2 0.2 1" />
+                  <geom size="0.005" pos="-0.05 -0.025 -0.03" rgba="0.2 0.2 0.2 1" />
+                  <geom size="0.005" pos="0.12 0.03 -0.03" rgba="0.2 0.2 0.2 1" />
+                  <geom size="0.005" pos="0.12 -0.03 -0.03" rgba="0.2 0.2 0.2 1" />
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+      <body name="right_hip_pitch_link" pos="0 -0.064452 -0.1027">
+        <inertial pos="0.002741 -0.047791 -0.02606" quat="0.954862 -0.293964 0.0302556 -0.030122"
+          mass="1.35" diaginertia="0.00181517 0.00153422 0.00116212" />
+        <joint name="right_hip_pitch_joint" pos="0 0 0" axis="0 1 0" range="-2.5307 2.8798"
+          actuatorfrcrange="-88 88" class="leg_motor" />
+        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.2 0.2 0.2 1"
+          mesh="right_hip_pitch_link" />
+        <geom type="mesh" rgba="0.2 0.2 0.2 1" mesh="right_hip_pitch_link" />
+        <body name="right_hip_roll_link" pos="0 -0.052 -0.030465" quat="0.996179 0 -0.0873386 0">
+          <inertial pos="0.029812 0.001045 -0.087934" quat="0.977808 1.97119e-05 0.205576 0.0403793"
+            mass="1.52" diaginertia="0.00254986 0.00241169 0.00148755" />
+          <joint name="right_hip_roll_joint" pos="0 0 0" axis="1 0 0" range="-2.9671 0.5236"
+            actuatorfrcrange="-139 139" class="leg_motor" />
+          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
+            mesh="right_hip_roll_link" />
+          <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_hip_roll_link" />
+          <body name="right_hip_yaw_link" pos="0.025001 0 -0.12412">
+            <inertial pos="-0.057709 0.010981 -0.15078" quat="0.751181 0.223482 0.15832 0.600598"
+              mass="1.702" diaginertia="0.00776166 0.00717575 0.00160139" />
+            <joint name="right_hip_yaw_joint" pos="0 0 0" axis="0 0 1" range="-2.7576 2.7576"
+              actuatorfrcrange="-88 88" class="leg_motor" />
+            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
+              mesh="right_hip_yaw_link" />
+            <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_hip_yaw_link" />
+            <body name="right_knee_link" pos="-0.078273 -0.0021489 -0.17734"
+              quat="0.996179 0 0.0873386 0">
+              <inertial pos="0.005457 -0.003964 -0.12074"
+                quat="0.923439 0.0345276 0.0116333 -0.382012" mass="1.932"
+                diaginertia="0.011374 0.0112843 0.00146452" />
+              <joint name="right_knee_joint" pos="0 0 0" axis="0 1 0" range="-0.087267 2.8798"
+                actuatorfrcrange="-139 139" class="leg_motor" />
+              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                rgba="0.7 0.7 0.7 1" mesh="right_knee_link" />
+              <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_knee_link" />
+              <body name="right_ankle_pitch_link" pos="0 9.4445e-05 -0.30001">
+                <inertial pos="-0.007269 0 0.011137" quat="0.603053 0.369225 0.369225 0.603053"
+                  mass="0.074" diaginertia="1.89e-05 1.40805e-05 6.9195e-06" />
+                <joint name="right_ankle_pitch_joint" pos="0 0 0" axis="0 1 0"
+                  range="-0.87267 0.5236" actuatorfrcrange="-50 50" class="ankle_motor" />
+                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                  rgba="0.7 0.7 0.7 1" mesh="right_ankle_pitch_link" />
+                <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_ankle_pitch_link" />
+                <body name="right_ankle_roll_link" pos="0 0 -0.017558">
+                  <inertial pos="0.026505 0 -0.016425"
+                    quat="0.000481092 0.728482 0.000618967 0.685065" mass="0.608"
+                    diaginertia="0.00167218 0.0016161 0.000217621" />
+                  <joint name="right_ankle_roll_joint" pos="0 0 0" axis="1 0 0"
+                    range="-0.2618 0.2618" actuatorfrcrange="-50 50" class="ankle_motor" />
+                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                    rgba="0.2 0.2 0.2 1" mesh="right_ankle_roll_link" />
+                  <geom size="0.005" pos="-0.05 0.025 -0.03" rgba="0.2 0.2 0.2 1" />
+                  <geom size="0.005" pos="-0.05 -0.025 -0.03" rgba="0.2 0.2 0.2 1" />
+                  <geom size="0.005" pos="0.12 0.03 -0.03" rgba="0.2 0.2 0.2 1" />
+                  <geom size="0.005" pos="0.12 -0.03 -0.03" rgba="0.2 0.2 0.2 1" />
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+      <body name="waist_yaw_link">
+        <inertial pos="0.003494 0.000233 0.018034" quat="0.289697 0.591001 -0.337795 0.672821"
+          mass="0.214" diaginertia="0.000163531 0.000107714 0.000102205" />
+        <joint name="waist_yaw_joint" pos="0 0 0" axis="0 0 1" range="-2.618 2.618"
+          actuatorfrcrange="-88 88" class="torso_motor" />
+        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
+          mesh="waist_yaw_link" />
+        <body name="waist_roll_link" pos="-0.0039635 0 0.044">
+          <inertial pos="0 2.3e-05 0" quat="0.5 0.5 -0.5 0.5" mass="0.086"
+            diaginertia="8.245e-06 7.079e-06 6.339e-06" />
+          <joint name="waist_roll_joint" pos="0 0 0" axis="1 0 0" range="-0.52 0.52"
+            actuatorfrcrange="-50 50" class="torso_motor" />
+          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
+            mesh="waist_roll_link" />
+          <body name="torso_link">
+            <inertial pos="0.00203158 0.000339683 0.184568"
+              quat="0.999803 -6.03319e-05 0.0198256 0.00131986" mass="7.818"
+              diaginertia="0.121847 0.109825 0.0273735" />
+            <joint name="waist_pitch_joint" pos="0 0 0" axis="0 1 0" range="-0.52 0.52"
+              actuatorfrcrange="-50 50" class="torso_motor" />
+            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
+              mesh="torso_link" />
+            <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="torso_link" />
+            <geom pos="0.0039635 0 -0.044" quat="1 0 0 0" type="mesh" contype="0" conaffinity="0"
+              group="1" density="0" rgba="0.2 0.2 0.2 1" mesh="logo_link" />
+            <geom pos="0.0039635 0 -0.044" quat="1 0 0 0" type="mesh" rgba="0.2 0.2 0.2 1"
+              mesh="logo_link" />
+            <geom pos="0.0039635 0 -0.044" type="mesh" contype="0" conaffinity="0" group="1"
+              density="0" rgba="0.2 0.2 0.2 1" mesh="head_link" />
+            <geom pos="0.0039635 0 -0.044" type="mesh" rgba="0.2 0.2 0.2 1" mesh="head_link" />
+            <site name="imu_in_torso" size="0.01" pos="-0.03959 -0.00224 0.14792" />
+            <body name="left_shoulder_pitch_link" pos="0.0039563 0.10022 0.24778"
+              quat="0.990264 0.139201 1.38722e-05 -9.86868e-05">
+              <inertial pos="0 0.035892 -0.011628" quat="0.654152 0.0130458 -0.326267 0.68225"
+                mass="0.718" diaginertia="0.000465864 0.000432842 0.000406394" />
+              <joint name="left_shoulder_pitch_joint" pos="0 0 0" axis="0 1 0"
+                range="-3.0892 2.6704" actuatorfrcrange="-25 25" class="arm_motor" />
+              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                rgba="0.7 0.7 0.7 1" mesh="left_shoulder_pitch_link" />
+              <geom size="0.03 0.025" pos="0 0.04 -0.01" quat="0.707107 0 0.707107 0"
+                type="cylinder" rgba="0.7 0.7 0.7 1" />
+              <body name="left_shoulder_roll_link" pos="0 0.038 -0.013831"
+                quat="0.990268 -0.139172 0 0">
+                <inertial pos="-0.000227 0.00727 -0.063243"
+                  quat="0.701256 -0.0196223 -0.00710317 0.712604" mass="0.643"
+                  diaginertia="0.000691311 0.000618011 0.000388977" />
+                <joint name="left_shoulder_roll_joint" pos="0 0 0" axis="1 0 0"
+                  range="-1.5882 2.2515" actuatorfrcrange="-25 25" class="arm_motor" />
+                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                  rgba="0.7 0.7 0.7 1" mesh="left_shoulder_roll_link" />
+                <geom size="0.03 0.015" pos="-0.004 0.006 -0.053" type="cylinder"
+                  rgba="0.7 0.7 0.7 1" />
+                <body name="left_shoulder_yaw_link" pos="0 0.00624 -0.1032">
+                  <inertial pos="0.010773 -0.002949 -0.072009"
+                    quat="0.716879 -0.0964829 -0.0679942 0.687134" mass="0.734"
+                    diaginertia="0.00106187 0.00103217 0.000400661" />
+                  <joint name="left_shoulder_yaw_joint" pos="0 0 0" axis="0 0 1"
+                    range="-2.618 2.618" actuatorfrcrange="-25 25" class="arm_motor" />
+                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                    rgba="0.7 0.7 0.7 1" mesh="left_shoulder_yaw_link" />
+                  <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_shoulder_yaw_link" />
+                  <body name="left_elbow_link" pos="0.015783 0 -0.080518">
+                    <inertial pos="0.064956 0.004454 -0.010062"
+                      quat="0.541765 0.636132 0.388821 0.388129" mass="0.6"
+                      diaginertia="0.000443035 0.000421612 0.000259353" />
+                    <joint name="left_elbow_joint" pos="0 0 0" axis="0 1 0" range="-1.0472 2.0944"
+                      actuatorfrcrange="-25 25" class="arm_motor" />
+                    <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                      rgba="0.7 0.7 0.7 1" mesh="left_elbow_link" />
+                    <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_elbow_link" />
+                    <body name="left_wrist_roll_link" pos="0.1 0.00188791 -0.01">
+                      <inertial pos="0.0171394 0.000537591 4.8864e-07"
+                        quat="0.575338 0.411667 -0.574906 0.411094" mass="0.085445"
+                        diaginertia="5.48211e-05 4.96646e-05 3.57798e-05" />
+                      <joint name="left_wrist_roll_joint" pos="0 0 0" axis="1 0 0"
+                        range="-1.97222 1.97222" actuatorfrcrange="-25 25" class="wrist_motor" />
+                      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                        rgba="0.7 0.7 0.7 1" mesh="left_wrist_roll_link" />
+                      <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_wrist_roll_link" />
+                      <body name="left_wrist_pitch_link" pos="0.038 0 0">
+                        <inertial pos="0.0229999 -0.00111685 -0.00111658"
+                          quat="0.249998 0.661363 0.293036 0.643608" mass="0.48405"
+                          diaginertia="0.000430353 0.000429873 0.000164648" />
+                        <joint name="left_wrist_pitch_joint" pos="0 0 0" axis="0 1 0"
+                          range="-1.61443 1.61443" actuatorfrcrange="-5 5" class="wrist_motor" />
+                        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                          rgba="0.7 0.7 0.7 1" mesh="left_wrist_pitch_link" />
+                        <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_wrist_pitch_link" />
+                        <body name="left_wrist_yaw_link" pos="0.046 0 0">
+                          <inertial pos="0.0885506 0.00212216 -0.000374562"
+                            quat="0.487149 0.493844 0.513241 0.505358" mass="0.457415"
+                            diaginertia="0.00105989 0.000895419 0.000323842" />
+                          <joint name="left_wrist_yaw_joint" pos="0 0 0" axis="0 0 1"
+                            range="-1.61443 1.61443" actuatorfrcrange="-5 5" class="wrist_motor" />
+                          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                            rgba="0.7 0.7 0.7 1" mesh="left_wrist_yaw_link" />
+                          <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_wrist_yaw_link" />
+                          <geom pos="0.0415 0.003 0" quat="1 0 0 0" type="mesh" contype="0"
+                            conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
+                            mesh="left_hand_palm_link" />
+                          <geom pos="0.0415 0.003 0" quat="1 0 0 0" type="mesh" rgba="0.7 0.7 0.7 1"
+                            mesh="left_hand_palm_link" />
+                          <body name="left_hand_thumb_0_link" pos="0.067 0.003 0">
+                            <inertial pos="-0.000884246 -0.00863407 0.000944293"
+                              quat="0.462991 0.643965 -0.460173 0.398986" mass="0.0862366"
+                              diaginertia="1.6546e-05 1.60058e-05 1.43741e-05" />
+                            <joint name="left_hand_thumb_0_joint" pos="0 0 0" axis="0 1 0"
+                              range="-1.0472 1.0472" actuatorfrcrange="-2.45 2.45"
+                              class="finger_motor" />
+                            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                              rgba="0.7 0.7 0.7 1" mesh="left_hand_thumb_0_link" />
+                            <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_hand_thumb_0_link" />
+                            <body name="left_hand_thumb_1_link" pos="-0.0025 -0.0193 0">
+                              <inertial pos="-0.000827888 -0.0354744 -0.0003809"
+                                quat="0.685598 0.705471 -0.15207 0.0956069" mass="0.0588507"
+                                diaginertia="1.28514e-05 1.22902e-05 5.9666e-06" />
+                              <joint name="left_hand_thumb_1_joint" pos="0 0 0" axis="0 0 1"
+                                range="-0.724312 1.0472" actuatorfrcrange="-1.4 1.4"
+                                class="finger_motor" />
+                              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                                rgba="0.7 0.7 0.7 1" mesh="left_hand_thumb_1_link" />
+                              <geom size="0.01 0.015 0.01" pos="-0.001 -0.032 0" type="box"
+                                rgba="0.7 0.7 0.7 1" />
+                              <body name="left_hand_thumb_2_link" pos="0 -0.0458 0">
+                                <inertial pos="-0.00171735 -0.0262819 0.000107789"
+                                  quat="0.703174 0.710977 -0.00017564 -0.00766553" mass="0.0203063"
+                                  diaginertia="4.61314e-06 3.86645e-06 1.53495e-06" />
+                                <joint name="left_hand_thumb_2_joint" pos="0 0 0" axis="0 0 1"
+                                  range="0 1.74533" actuatorfrcrange="-1.4 1.4" class="finger_motor" />
+                                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                                  rgba="0.7 0.7 0.7 1" mesh="left_hand_thumb_2_link" />
+                                <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_hand_thumb_2_link" />
+                              </body>
+                            </body>
+                          </body>
+                          <body name="left_hand_middle_0_link" pos="0.1192 0.0046 -0.0285">
+                            <inertial pos="0.0354744 0.000827888 0.0003809"
+                              quat="0.391313 0.552395 0.417187 0.606373" mass="0.0588507"
+                              diaginertia="1.28514e-05 1.22902e-05 5.9666e-06" />
+                            <joint name="left_hand_middle_0_joint" pos="0 0 0" axis="0 0 1"
+                              range="-1.5708 0" actuatorfrcrange="-1.4 1.4" class="finger_motor" />
+                            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                              rgba="0.7 0.7 0.7 1" mesh="left_hand_middle_0_link" />
+                            <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_hand_middle_0_link" />
+                            <body name="left_hand_middle_1_link" pos="0.0458 0 0">
+                              <inertial pos="0.0262819 0.00171735 -0.000107789"
+                                quat="0.502612 0.491799 0.502639 0.502861" mass="0.0203063"
+                                diaginertia="4.61314e-06 3.86645e-06 1.53495e-06" />
+                              <joint name="left_hand_middle_1_joint" pos="0 0 0" axis="0 0 1"
+                                range="-1.74533 0" actuatorfrcrange="-1.4 1.4" class="finger_motor" />
+                              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                                rgba="0.7 0.7 0.7 1" mesh="left_hand_middle_1_link" />
+                              <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_hand_middle_1_link" />
+                            </body>
+                          </body>
+                          <body name="left_hand_index_0_link" pos="0.1192 0.0046 0.0285">
+                            <inertial pos="0.0354744 0.000827888 0.0003809"
+                              quat="0.391313 0.552395 0.417187 0.606373" mass="0.0588507"
+                              diaginertia="1.28514e-05 1.22902e-05 5.9666e-06" />
+                            <joint name="left_hand_index_0_joint" pos="0 0 0" axis="0 0 1"
+                              range="-1.5708 0" actuatorfrcrange="-1.4 1.4" class="finger_motor" />
+                            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                              rgba="0.7 0.7 0.7 1" mesh="left_hand_index_0_link" />
+                            <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_hand_index_0_link" />
+                            <body name="left_hand_index_1_link" pos="0.0458 0 0">
+                              <inertial pos="0.0262819 0.00171735 -0.000107789"
+                                quat="0.502612 0.491799 0.502639 0.502861" mass="0.0203063"
+                                diaginertia="4.61314e-06 3.86645e-06 1.53495e-06" />
+                              <joint name="left_hand_index_1_joint" pos="0 0 0" axis="0 0 1"
+                                range="-1.74533 0" actuatorfrcrange="-1.4 1.4" class="finger_motor" />
+                              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                                rgba="0.7 0.7 0.7 1" mesh="left_hand_index_1_link" />
+                              <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_hand_index_1_link" />
+                            </body>
+                          </body>
+                        </body>
+                      </body>
+                    </body>
+                  </body>
+                </body>
+              </body>
+            </body>
+            <body name="right_shoulder_pitch_link" pos="0.0039563 -0.10021 0.24778"
+              quat="0.990264 -0.139201 1.38722e-05 9.86868e-05">
+              <inertial pos="0 -0.035892 -0.011628" quat="0.68225 -0.326267 0.0130458 0.654152"
+                mass="0.718" diaginertia="0.000465864 0.000432842 0.000406394" />
+              <joint name="right_shoulder_pitch_joint" pos="0 0 0" axis="0 1 0"
+                range="-3.0892 2.6704" actuatorfrcrange="-25 25" class="arm_motor" />
+              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                rgba="0.7 0.7 0.7 1" mesh="right_shoulder_pitch_link" />
+              <geom size="0.03 0.025" pos="0 -0.04 -0.01" quat="0.707107 0 0.707107 0"
+                type="cylinder" rgba="0.7 0.7 0.7 1" />
+              <body name="right_shoulder_roll_link" pos="0 -0.038 -0.013831"
+                quat="0.990268 0.139172 0 0">
+                <inertial pos="-0.000227 -0.00727 -0.063243"
+                  quat="0.712604 -0.00710317 -0.0196223 0.701256" mass="0.643"
+                  diaginertia="0.000691311 0.000618011 0.000388977" />
+                <joint name="right_shoulder_roll_joint" pos="0 0 0" axis="1 0 0"
+                  range="-2.2515 1.5882" actuatorfrcrange="-25 25" class="arm_motor" />
+                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                  rgba="0.7 0.7 0.7 1" mesh="right_shoulder_roll_link" />
+                <geom size="0.03 0.015" pos="-0.004 -0.006 -0.053" type="cylinder"
+                  rgba="0.7 0.7 0.7 1" />
+                <body name="right_shoulder_yaw_link" pos="0 -0.00624 -0.1032">
+                  <inertial pos="0.010773 0.002949 -0.072009"
+                    quat="0.687134 -0.0679942 -0.0964829 0.716879" mass="0.734"
+                    diaginertia="0.00106187 0.00103217 0.000400661" />
+                  <joint name="right_shoulder_yaw_joint" pos="0 0 0" axis="0 0 1"
+                    range="-2.618 2.618" actuatorfrcrange="-25 25" class="arm_motor" />
+                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                    rgba="0.7 0.7 0.7 1" mesh="right_shoulder_yaw_link" />
+                  <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_shoulder_yaw_link" />
+                  <body name="right_elbow_link" pos="0.015783 0 -0.080518">
+                    <inertial pos="0.064956 -0.004454 -0.010062"
+                      quat="0.388129 0.388821 0.636132 0.541765" mass="0.6"
+                      diaginertia="0.000443035 0.000421612 0.000259353" />
+                    <joint name="right_elbow_joint" pos="0 0 0" axis="0 1 0" range="-1.0472 2.0944"
+                      actuatorfrcrange="-25 25" class="arm_motor" />
+                    <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                      rgba="0.7 0.7 0.7 1" mesh="right_elbow_link" />
+                    <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_elbow_link" />
+                    <body name="right_wrist_roll_link" pos="0.1 -0.00188791 -0.01">
+                      <inertial pos="0.0171394 -0.000537591 4.8864e-07"
+                        quat="0.411667 0.575338 -0.411094 0.574906" mass="0.085445"
+                        diaginertia="5.48211e-05 4.96646e-05 3.57798e-05" />
+                      <joint name="right_wrist_roll_joint" pos="0 0 0" axis="1 0 0"
+                        range="-1.97222 1.97222" actuatorfrcrange="-25 25" class="wrist_motor" />
+                      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                        rgba="0.7 0.7 0.7 1" mesh="right_wrist_roll_link" />
+                      <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_wrist_roll_link" />
+                      <body name="right_wrist_pitch_link" pos="0.038 0 0">
+                        <inertial pos="0.0229999 0.00111685 -0.00111658"
+                          quat="0.643608 0.293036 0.661363 0.249998" mass="0.48405"
+                          diaginertia="0.000430353 0.000429873 0.000164648" />
+                        <joint name="right_wrist_pitch_joint" pos="0 0 0" axis="0 1 0"
+                          range="-1.61443 1.61443" actuatorfrcrange="-5 5" class="wrist_motor" />
+                        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                          rgba="0.7 0.7 0.7 1" mesh="right_wrist_pitch_link" />
+                        <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_wrist_pitch_link" />
+                        <body name="right_wrist_yaw_link" pos="0.046 0 0">
+                          <inertial pos="0.0885506 -0.00212216 -0.000374562"
+                            quat="0.505358 0.513241 0.493844 0.487149" mass="0.457415"
+                            diaginertia="0.00105989 0.000895419 0.000323842" />
+                          <joint name="right_wrist_yaw_joint" pos="0 0 0" axis="0 0 1"
+                            range="-1.61443 1.61443" actuatorfrcrange="-5 5" class="wrist_motor" />
+                          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                            rgba="0.7 0.7 0.7 1" mesh="right_wrist_yaw_link" />
+                          <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_wrist_yaw_link" />
+                          <geom pos="0.0415 -0.003 0" quat="1 0 0 0" type="mesh" contype="0"
+                            conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
+                            mesh="right_hand_palm_link" />
+                          <geom pos="0.0415 -0.003 0" quat="1 0 0 0" type="mesh"
+                            rgba="0.7 0.7 0.7 1" mesh="right_hand_palm_link" />
+                          <body name="right_hand_thumb_0_link" pos="0.067 -0.003 0">
+                            <inertial pos="-0.000884246 0.00863407 0.000944293"
+                              quat="0.643965 0.462991 -0.398986 0.460173" mass="0.0862366"
+                              diaginertia="1.6546e-05 1.60058e-05 1.43741e-05" />
+                            <joint name="right_hand_thumb_0_joint" pos="0 0 0" axis="0 1 0"
+                              range="-1.0472 1.0472" actuatorfrcrange="-2.45 2.45"
+                              class="finger_motor" />
+                            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                              rgba="0.7 0.7 0.7 1" mesh="right_hand_thumb_0_link" />
+                            <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_hand_thumb_0_link" />
+                            <body name="right_hand_thumb_1_link" pos="-0.0025 0.0193 0">
+                              <inertial pos="-0.000827888 0.0354744 -0.0003809"
+                                quat="0.705471 0.685598 -0.0956069 0.15207" mass="0.0588507"
+                                diaginertia="1.28514e-05 1.22902e-05 5.9666e-06" />
+                              <joint name="right_hand_thumb_1_joint" pos="0 0 0" axis="0 0 1"
+                                range="-1.0472 0.724312" actuatorfrcrange="-1.4 1.4"
+                                class="finger_motor" />
+                              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                                rgba="0.7 0.7 0.7 1" mesh="right_hand_thumb_1_link" />
+                              <geom size="0.01 0.015 0.01" pos="-0.001 0.032 0" type="box"
+                                rgba="0.7 0.7 0.7 1" />
+                              <body name="right_hand_thumb_2_link" pos="0 0.0458 0">
+                                <inertial pos="-0.00171735 0.0262819 0.000107789"
+                                  quat="0.710977 0.703174 0.00766553 0.00017564" mass="0.0203063"
+                                  diaginertia="4.61314e-06 3.86645e-06 1.53495e-06" />
+                                <joint name="right_hand_thumb_2_joint" pos="0 0 0" axis="0 0 1"
+                                  range="-1.74533 0" actuatorfrcrange="-1.4 1.4"
+                                  class="finger_motor" />
+                                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                                  rgba="0.7 0.7 0.7 1" mesh="right_hand_thumb_2_link" />
+                                <geom type="mesh" rgba="0.7 0.7 0.7 1"
+                                  mesh="right_hand_thumb_2_link" />
+                              </body>
+                            </body>
+                          </body>
+                          <body name="right_hand_middle_0_link" pos="0.1192 -0.0046 -0.0285">
+                            <inertial pos="0.0354744 -0.000827888 0.0003809"
+                              quat="0.606373 0.417187 0.552395 0.391313" mass="0.0588507"
+                              diaginertia="1.28514e-05 1.22902e-05 5.9666e-06" />
+                            <joint name="right_hand_middle_0_joint" pos="0 0 0" axis="0 0 1"
+                              range="0 1.5708" actuatorfrcrange="-1.4 1.4" class="finger_motor" />
+                            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                              rgba="0.7 0.7 0.7 1" mesh="right_hand_middle_0_link" />
+                            <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_hand_middle_0_link" />
+                            <body name="right_hand_middle_1_link" pos="0.0458 0 0">
+                              <inertial pos="0.0262819 -0.00171735 -0.000107789"
+                                quat="0.502861 0.502639 0.491799 0.502612" mass="0.0203063"
+                                diaginertia="4.61314e-06 3.86645e-06 1.53495e-06" />
+                              <joint name="right_hand_middle_1_joint" pos="0 0 0" axis="0 0 1"
+                                range="0 1.74533" actuatorfrcrange="-1.4 1.4" class="finger_motor" />
+                              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                                rgba="0.7 0.7 0.7 1" mesh="right_hand_middle_1_link" />
+                              <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_hand_middle_1_link" />
+                            </body>
+                          </body>
+                          <body name="right_hand_index_0_link" pos="0.1192 -0.0046 0.0285">
+                            <inertial pos="0.0354744 -0.000827888 0.0003809"
+                              quat="0.606373 0.417187 0.552395 0.391313" mass="0.0588507"
+                              diaginertia="1.28514e-05 1.22902e-05 5.9666e-06" />
+                            <joint name="right_hand_index_0_joint" pos="0 0 0" axis="0 0 1"
+                              range="0 1.5708" actuatorfrcrange="-1.4 1.4" class="finger_motor" />
+                            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                              rgba="0.7 0.7 0.7 1" mesh="right_hand_index_0_link" />
+                            <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_hand_index_0_link" />
+                            <body name="right_hand_index_1_link" pos="0.0458 0 0">
+                              <inertial pos="0.0262819 -0.00171735 -0.000107789"
+                                quat="0.502861 0.502639 0.491799 0.502612" mass="0.0203063"
+                                diaginertia="4.61314e-06 3.86645e-06 1.53495e-06" />
+                              <joint name="right_hand_index_1_joint" pos="0 0 0" axis="0 0 1"
+                                range="0 1.74533" actuatorfrcrange="-1.4 1.4" class="finger_motor" />
+                              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                                rgba="0.7 0.7 0.7 1" mesh="right_hand_index_1_link" />
+                              <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_hand_index_1_link" />
+                            </body>
+                          </body>
+                        </body>
+                      </body>
+                    </body>
+                  </body>
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+    </body>
+  </worldbody>
+
+  <actuator>
+    <!-- <motor name="fx" joint="floating_base_joint" gear="1 0 0 0 0 0" />
+      <motor name="fy" joint="floating_base_joint" gear="0 1 0 0 0 0" />
+      <motor name="fz" joint="floating_base_joint" gear="0 0 1 0 0 0" />
+      <motor name="tx" joint="floating_base_joint" gear="0 0 0 1 0 0" />
+      <motor name="ty" joint="floating_base_joint" gear="0 0 0 0 1 0" />
+      <motor name="tz" joint="floating_base_joint" gear="0 0 0 0 0 1" /> -->
+
+      <motor name="left_hip_pitch_joint" joint="left_hip_pitch_joint" />
+      <motor
+      name="left_hip_roll_joint" joint="left_hip_roll_joint" />
+      <motor name="left_hip_yaw_joint"
+      joint="left_hip_yaw_joint" />
+      <motor name="left_knee_joint" joint="left_knee_joint" />
+      <motor
+      name="left_ankle_pitch_joint" joint="left_ankle_pitch_joint" />
+      <motor
+      name="left_ankle_roll_joint" joint="left_ankle_roll_joint" />
+      <motor
+      name="right_hip_pitch_joint" joint="right_hip_pitch_joint" />
+      <motor
+      name="right_hip_roll_joint" joint="right_hip_roll_joint" />
+      <motor name="right_hip_yaw_joint"
+      joint="right_hip_yaw_joint" />
+      <motor name="right_knee_joint" joint="right_knee_joint" />
+      <motor
+      name="right_ankle_pitch_joint" joint="right_ankle_pitch_joint" />
+      <motor
+      name="right_ankle_roll_joint" joint="right_ankle_roll_joint" />
+      <motor name="waist_yaw_joint"
+      joint="waist_yaw_joint" />
+      <motor name="waist_roll_joint" joint="waist_roll_joint" />
+      <motor
+      name="waist_pitch_joint" joint="waist_pitch_joint" />
+      <motor name="left_shoulder_pitch_joint"
+      joint="left_shoulder_pitch_joint" />
+      <motor name="left_shoulder_roll_joint"
+      joint="left_shoulder_roll_joint" />
+      <motor name="left_shoulder_yaw_joint"
+      joint="left_shoulder_yaw_joint" />
+      <motor name="left_elbow_joint" joint="left_elbow_joint" />
+      <motor
+      name="left_wrist_roll_joint" joint="left_wrist_roll_joint" />
+      <motor
+      name="left_wrist_pitch_joint" joint="left_wrist_pitch_joint" />
+      <motor
+      name="left_wrist_yaw_joint" joint="left_wrist_yaw_joint" />
+      <motor
+      name="left_hand_thumb_0_joint" joint="left_hand_thumb_0_joint" />
+      <motor
+      name="left_hand_thumb_1_joint" joint="left_hand_thumb_1_joint" />
+      <motor
+      name="left_hand_thumb_2_joint" joint="left_hand_thumb_2_joint" />
+      <motor
+      name="left_hand_middle_0_joint" joint="left_hand_middle_0_joint" />
+      <motor
+      name="left_hand_middle_1_joint" joint="left_hand_middle_1_joint" />
+      <motor
+      name="left_hand_index_0_joint" joint="left_hand_index_0_joint" />
+      <motor
+      name="left_hand_index_1_joint" joint="left_hand_index_1_joint" />
+      <motor
+      name="right_shoulder_pitch_joint" joint="right_shoulder_pitch_joint" />
+      <motor
+      name="right_shoulder_roll_joint" joint="right_shoulder_roll_joint" />
+      <motor
+      name="right_shoulder_yaw_joint" joint="right_shoulder_yaw_joint" />
+      <motor
+      name="right_elbow_joint" joint="right_elbow_joint" />
+      <motor name="right_wrist_roll_joint"
+      joint="right_wrist_roll_joint" />
+      <motor name="right_wrist_pitch_joint"
+      joint="right_wrist_pitch_joint" />
+      <motor name="right_wrist_yaw_joint"
+      joint="right_wrist_yaw_joint" />
+      <motor name="right_hand_thumb_0_joint"
+      joint="right_hand_thumb_0_joint" />
+      <motor name="right_hand_thumb_1_joint"
+      joint="right_hand_thumb_1_joint" />
+      <motor name="right_hand_thumb_2_joint"
+      joint="right_hand_thumb_2_joint" />
+      <motor name="right_hand_index_0_joint"
+      joint="right_hand_index_0_joint" />
+      <motor name="right_hand_index_1_joint"
+      joint="right_hand_index_1_joint" />
+      <motor name="right_hand_middle_0_joint"
+      joint="right_hand_middle_0_joint" />
+      <motor name="right_hand_middle_1_joint"
+      joint="right_hand_middle_1_joint" /> --> </actuator>
+
+  <sensor>
+    <gyro name="imu-torso-angular-velocity" site="imu_in_torso" noise="5e-4" cutoff="34.9" />
+    <accelerometer name="imu-torso-linear-acceleration" site="imu_in_torso" noise="1e-2"
+      cutoff="157" />
+    <gyro name="imu-pelvis-angular-velocity" site="imu_in_pelvis" noise="5e-4" cutoff="34.9" />
+    <accelerometer name="imu-pelvis-linear-acceleration" site="imu_in_pelvis" noise="1e-2"
+      cutoff="157" />
+  </sensor>
+
+
+  <!-- setup scene -->
+  <!-- <statistic center="1.0 0.7 1.0" extent="0.8"/>
+    <visual>
+      <headlight diffuse="0.6 0.6 0.6" ambient="0.1 0.1 0.1" specular="0.9 0.9 0.9"/>
+      <rgba haze="0.15 0.25 0.35 1"/>
+      <global azimuth="-140" elevation="-20"/>
+    </visual>
+    <asset>
+      <texture type="skybox" builtin="flat" rgb1="0 0 0" rgb2="0 0 0" width="512" height="3072"/>
+      <texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1
+  0.2 0.3" markrgb="0.8 0.8 0.8" width="300" height="300"/>
+      <material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5"
+  reflectance="0.2"/>
+    </asset>
+    <worldbody>
+      <light pos="1 0 3.5" dir="0 0 -1" directional="true"/>
+      <geom name="floor" size="0 0 0.05" type="plane" material="groundplane"/>
+    </worldbody> -->
+</mujoco>
+

GR00T-WholeBodyControl/decoupled_wbc/control/robot_model/model_data/g1/lift_box_43dof.xml

@@ -0,0 +1,49 @@
+<mujoco model="g1_43dof_lift_box scene">
+  <include file="g1_29dof_with_hand_rev_1_0_activatedfinger.xml" />
+  <!-- Note that the hand model in g1_29dof_with_hand.xml is not stable for simulation. -->
+
+  <statistic center="0 0 0.5" extent="2.0" />
+
+  <visual>
+    <headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0" />
+    <rgba haze="0.15 0.25 0.35 1" />
+    <global azimuth="-130" elevation="-20" />
+  </visual>
+
+  <asset>
+    <texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512"
+      height="3072" />
+    <texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4"
+      rgb2="0.1 0.2 0.3"
+      markrgb="0.8 0.8 0.8" width="300" height="300" />
+    <texture type="2d" name="table" builtin="flat" rgb1="0.7 0.6 0.5" width="512" height="512" />
+    <material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5"
+      reflectance="0.2" />
+    <material name="table" texture="table" texuniform="true" reflectance="0.1" />
+    <texture type="2d" name="cube" builtin="flat" rgb1="0.3 0.7 0.8" width="512" height="512" />
+    <material name="cube" texture="cube" texuniform="true" reflectance="0.1" />
+  </asset>
+
+  <worldbody>
+    <light pos="0 0 1.5" dir="0 0 -1" directional="true" />
+    <geom name="floor" size="0 0 0.05" type="plane" material="groundplane" />
+    <!-- Site -->
+    <site name="com_marker" pos="0.1 0 0" size="0.05" rgba="1 0 0 1" type="sphere" />
+
+    <body name="table_body" pos="0.6 0 0.4">
+      <geom name="table_top" pos="0 0 0.3" size="0.5 1.0 0.05" type="box" material="table"/>
+      <geom name="table_leg1" pos="0.0 0.9 -0.05" size="0.5 0.05 0.4" type="box" material="table"/>
+      <geom name="table_leg2" pos="0.0 -0.9 -0.05" size="0.5 0.05 0.4" type="box" material="table"/>
+    </body>
+    <body name="box_body" pos="0.35 0 0.9">
+      <joint type="free" damping="0.0005" name="box_joint" />
+      <geom name="box" size="0.1 0.1 0.1" type="box" material="cube" solimp="0.998 0.998 0.001"
+        solref="0.001 2" density="100" friction="0.95 0.3 0.1" />
+    </body>
+
+  </worldbody>
+
+  <default>
+    <geom friction="1.0" />
+  </default>
+</mujoco>