Add files using upload-large-folder tool

GR00T-WholeBodyControl/gear_sonic/camera/drivers/dummy.py

@@ -0,0 +1,86 @@
+"""Dummy / replay sensor for testing without real camera hardware.
+
+``DummySensor`` generates random images.
+``ReplayDummySensor`` loops frames from a video file.
+"""
+
+import time
+from typing import Any
+
+import cv2
+import numpy as np
+
+try:
+    import gymnasium as gym
+except ImportError:
+    gym = None  # type: ignore[assignment]
+
+from gear_sonic.camera.sensor import Sensor
+from gear_sonic.camera.sensor_server import ImageMessageSchema
+
+
+class DummySensor(Sensor):
+    """Produces random 640x480 images at each read() call."""
+
+    def __init__(self):
+        pass
+
+    def read(self) -> dict[str, Any] | None:
+        return {
+            "color_image": np.random.randint(0, 255, (480, 640, 3), dtype=np.uint8),
+            "timestamp": time.time(),
+        }
+
+    def serialize(self, data: dict[str, Any]) -> dict[str, Any]:
+        raise NotImplementedError("DummySensor does not support serialize()")
+
+    def close(self):
+        pass
+
+    def observation_space(self):
+        if gym is None:
+            return None
+        return gym.spaces.Dict(
+            {
+                "color_image": gym.spaces.Box(
+                    low=0, high=255, shape=(480, 640, 3), dtype=np.uint8
+                ),
+            }
+        )
+
+
+class ReplayDummySensor(DummySensor):
+    """Loops frames from a video file, useful for offline testing."""
+
+    def __init__(self, video_path: str):
+        self.video_path = video_path
+        self.image_ctr = 0
+        self.video_reader = cv2.VideoCapture(video_path)
+        self.frames = []
+        while self.video_reader.isOpened():
+            ret, frame = self.video_reader.read()
+            if not ret:
+                break
+            self.frames.append(cv2.cvtColor(frame, cv2.COLOR_BGR2RGB))
+
+    def read(self) -> dict[str, Any] | None:
+        self.image_ctr += 1
+        if self.image_ctr >= len(self.frames):
+            self.image_ctr = 0
+
+        img = self.frames[self.image_ctr]
+        img = cv2.resize(img, (640, 480))
+        return {
+            "color_image": img,
+            "timestamp": {"color_image": time.time()},
+        }
+
+    def serialize(self, data: dict[str, Any]) -> dict[str, Any]:
+        serialized_msg = ImageMessageSchema(
+            timestamps=data["timestamp"] if isinstance(data["timestamp"], dict) else {"color_image": data["timestamp"]},
+            images={"color_image": data["color_image"]},
+        )
+        return serialized_msg.serialize()
+
+    def close(self):
+        self.video_reader.release()

GR00T-WholeBodyControl/gear_sonic/camera/drivers/oak.py

@@ -0,0 +1,409 @@
+"""OAK (DepthAI) camera driver.
+
+Requires the ``depthai`` SDK — install with::
+
+    pip install depthai
+
+See https://docs.luxonis.com/ for hardware-specific instructions.
+"""
+
+import time
+from typing import Any
+
+import cv2
+import numpy as np
+
+try:
+    import gymnasium as gym
+except ImportError:
+    gym = None  # type: ignore[assignment]
+
+import depthai as dai
+
+from gear_sonic.camera.sensor import Sensor
+from gear_sonic.camera.sensor_server import (
+    CameraMountPosition,
+    ImageMessageSchema,
+    SensorServer,
+)
+
+
+class OAKConfig:
+    """Configuration for the OAK camera."""
+
+    color_image_dim: tuple[int, int] = (640, 480)
+    monochrome_image_dim: tuple[int, int] = (640, 480)
+    fps: int = 30
+    enable_color: bool = True
+    enable_mono_cameras: bool = False
+    mount_position: str = CameraMountPosition.EGO_VIEW.value
+    autofocus: bool = False
+    manual_focus: int = 130
+    use_mjpeg: bool = False
+    mjpeg_quality: int = 80
+
+
+class OAKSensor(Sensor, SensorServer):
+    """Sensor for the OAK camera family (OAK-D, OAK-1, etc.)."""
+
+    def __init__(
+        self,
+        run_as_server: bool = False,
+        port: int = 5555,
+        config: OAKConfig = OAKConfig(),
+        device_id: str | None = None,
+        mount_position: str = CameraMountPosition.EGO_VIEW.value,
+    ):
+        self.config = config
+        self.mount_position = mount_position
+        self._run_as_server = run_as_server
+
+        device_infos = dai.Device.getAllAvailableDevices()
+        assert len(device_infos) > 0, f"No OAK devices found for {mount_position}"
+        print(f"Device infos: {device_infos}")
+        if device_id is not None:
+            device_found = False
+            for device_info in device_infos:
+                if device_info.getDeviceId() == device_id:
+                    self.device = dai.Device(device_info, maxUsbSpeed=dai.UsbSpeed.SUPER_PLUS)
+                    device_found = True
+                    break
+            if not device_found:
+                raise ValueError(f"Device with ID {device_id} not found")
+        else:
+            self.device = dai.Device()
+
+        print(f"Connected to OAK device: {self.device.getDeviceName(), self.device.getDeviceId()}")
+        print(f"Device ID: {self.device.getDeviceId()}")
+
+        sockets: list[dai.CameraBoardSocket] = self.device.getConnectedCameras()
+        print(f"Available cameras: {[str(s) for s in sockets]}")
+
+        self.pipeline = dai.Pipeline(self.device)
+        self.output_queues = {}
+        self._use_mjpeg = config.use_mjpeg
+
+        # RGB camera (CAM_A)
+        if config.enable_color and dai.CameraBoardSocket.CAM_A in sockets:
+            self.cam_rgb = self.pipeline.create(dai.node.Camera)
+            cam_socket = dai.CameraBoardSocket.CAM_A
+            self.cam_rgb = self.cam_rgb.build(cam_socket)
+
+            if config.use_mjpeg:
+                cam_out = self.cam_rgb.requestOutput(
+                    config.color_image_dim,
+                    dai.ImgFrame.Type.NV12,
+                    fps=config.fps,
+                )
+                encoder = self.pipeline.create(dai.node.VideoEncoder)
+                encoder.setDefaultProfilePreset(
+                    config.fps, dai.VideoEncoderProperties.Profile.MJPEG
+                )
+                encoder.setQuality(config.mjpeg_quality)
+                cam_out.link(encoder.input)
+                self.output_queues["color"] = encoder.out.createOutputQueue(
+                    maxSize=3, blocking=False
+                )
+            else:
+                self.output_queues["color"] = self.cam_rgb.requestOutput(
+                    config.color_image_dim,
+                    fps=config.fps,
+                ).createOutputQueue(maxSize=3, blocking=False)
+            print(f"Enabled CAM_A (RGB){' with MJPEG encoding' if config.use_mjpeg else ''}")
+
+            if not config.autofocus:
+                ctrl_in = self.cam_rgb.inputControl.createInputQueue()
+                ctrl = dai.CameraControl()
+                ctrl.setAutoFocusMode(dai.CameraControl.AutoFocusMode.OFF)
+                ctrl.setManualFocus(config.manual_focus)
+                ctrl_in.send(ctrl)
+                print(f"Autofocus disabled, manual focus set to {config.manual_focus}")
+
+        # Monochrome cameras (CAM_B / CAM_C)
+        if config.enable_mono_cameras:
+            if dai.CameraBoardSocket.CAM_B in sockets:
+                self.cam_mono_left = self.pipeline.create(dai.node.Camera)
+                cam_socket = dai.CameraBoardSocket.CAM_B
+                self.cam_mono_left = self.cam_mono_left.build(cam_socket)
+
+                if config.use_mjpeg:
+                    cam_out = self.cam_mono_left.requestOutput(
+                        config.monochrome_image_dim,
+                        dai.ImgFrame.Type.NV12,
+                        fps=config.fps,
+                    )
+                    encoder = self.pipeline.create(dai.node.VideoEncoder)
+                    encoder.setDefaultProfilePreset(
+                        config.fps, dai.VideoEncoderProperties.Profile.MJPEG
+                    )
+                    encoder.setQuality(config.mjpeg_quality)
+                    cam_out.link(encoder.input)
+                    self.output_queues["mono_left"] = encoder.out.createOutputQueue(
+                        maxSize=3, blocking=False
+                    )
+                else:
+                    self.output_queues["mono_left"] = self.cam_mono_left.requestOutput(
+                        config.monochrome_image_dim,
+                        fps=config.fps,
+                    ).createOutputQueue(maxSize=3, blocking=False)
+                print("Enabled CAM_B (Monochrome Left)")
+
+            if dai.CameraBoardSocket.CAM_C in sockets:
+                self.cam_mono_right = self.pipeline.create(dai.node.Camera)
+                cam_socket = dai.CameraBoardSocket.CAM_C
+                self.cam_mono_right = self.cam_mono_right.build(cam_socket)
+
+                if config.use_mjpeg:
+                    cam_out = self.cam_mono_right.requestOutput(
+                        config.monochrome_image_dim,
+                        dai.ImgFrame.Type.NV12,
+                        fps=config.fps,
+                    )
+                    encoder = self.pipeline.create(dai.node.VideoEncoder)
+                    encoder.setDefaultProfilePreset(
+                        config.fps, dai.VideoEncoderProperties.Profile.MJPEG
+                    )
+                    encoder.setQuality(config.mjpeg_quality)
+                    cam_out.link(encoder.input)
+                    self.output_queues["mono_right"] = encoder.out.createOutputQueue(
+                        maxSize=3, blocking=False
+                    )
+                else:
+                    self.output_queues["mono_right"] = self.cam_mono_right.requestOutput(
+                        config.monochrome_image_dim,
+                        fps=config.fps,
+                    ).createOutputQueue(maxSize=3, blocking=False)
+                print("Enabled CAM_C (Monochrome Right)")
+
+        assert len(self.output_queues) > 0, "No output queues enabled"
+
+        self.pipeline.start()
+
+        print(f"[{mount_position}] Pipeline started, waiting for stabilization...")
+        time.sleep(2.0)
+
+        for _ in range(10):
+            test_frame = None
+            for queue_name, q in self.output_queues.items():
+                test_frame = q.tryGet()
+                if test_frame:
+                    print(f"[{mount_position}] First frame received from {queue_name}")
+                    break
+            if test_frame:
+                break
+            time.sleep(0.3)
+        else:
+            print(f"[{mount_position}] Warning: No frames received during init verification")
+
+        if run_as_server:
+            self.start_server(port)
+
+    def read(self) -> dict[str, Any] | None:
+        if not self.pipeline.isRunning():
+            print(f"[ERROR] OAK pipeline stopped for {self.mount_position}")
+            return None
+        if not self.device.isPipelineRunning():
+            print(f"[ERROR] OAK device disconnected for {self.mount_position}")
+            return None
+
+        timestamps = {}
+        images = {}
+        rgb_frame_time = None
+
+        def drain_queue_get_latest(queue):
+            latest_frame = None
+            while True:
+                frame = queue.tryGet()
+                if frame is None:
+                    break
+                latest_frame = frame
+            return latest_frame
+
+        expected_cameras = set(self.output_queues.keys())
+        received_cameras = set()
+
+        if "color" in self.output_queues:
+            try:
+                rgb_frame = drain_queue_get_latest(self.output_queues["color"])
+                if rgb_frame is None:
+                    return None
+                rgb_frame_time = rgb_frame.getTimestamp()
+                read_time = time.time()
+                frame_age = (dai.Clock.now() - rgb_frame_time).total_seconds()
+                capture_time = read_time - frame_age
+
+                if self._use_mjpeg:
+                    images[self.mount_position] = bytes(rgb_frame.getData())
+                else:
+                    images[self.mount_position] = rgb_frame.getCvFrame()[..., ::-1]
+                timestamps[self.mount_position] = capture_time
+                received_cameras.add("color")
+            except Exception as e:
+                print(f"[ERROR] Failed to read color frame from {self.mount_position}: {e}")
+                return None
+
+        if "mono_left" in self.output_queues:
+            try:
+                mono_left_frame = drain_queue_get_latest(self.output_queues["mono_left"])
+                if mono_left_frame is None:
+                    return None
+                mono_left_frame_time = mono_left_frame.getTimestamp()
+                read_time = time.time()
+                frame_age = (dai.Clock.now() - mono_left_frame_time).total_seconds()
+                capture_time = read_time - frame_age
+
+                key = f"{self.mount_position}_left_mono"
+                if self._use_mjpeg:
+                    images[key] = bytes(mono_left_frame.getData())
+                else:
+                    images[key] = mono_left_frame.getCvFrame()
+                timestamps[key] = capture_time
+                received_cameras.add("mono_left")
+            except Exception as e:
+                print(f"[ERROR] Failed to read mono_left frame from {self.mount_position}: {e}")
+                return None
+
+        if "mono_right" in self.output_queues:
+            try:
+                mono_right_frame = drain_queue_get_latest(self.output_queues["mono_right"])
+                if mono_right_frame is None:
+                    return None
+                mono_right_frame_time = mono_right_frame.getTimestamp()
+                read_time = time.time()
+                frame_age = (dai.Clock.now() - mono_right_frame_time).total_seconds()
+                capture_time = read_time - frame_age
+
+                key = f"{self.mount_position}_right_mono"
+                if self._use_mjpeg:
+                    images[key] = bytes(mono_right_frame.getData())
+                else:
+                    images[key] = mono_right_frame.getCvFrame()
+                timestamps[key] = capture_time
+                received_cameras.add("mono_right")
+            except Exception as e:
+                print(f"[ERROR] Failed to read mono_right frame from {self.mount_position}: {e}")
+                return None
+
+        if received_cameras != expected_cameras:
+            missing = expected_cameras - received_cameras
+            print(f"[ERROR] Missing frames from cameras: {missing} for {self.mount_position}")
+            return None
+
+        if rgb_frame_time is not None:
+            frame_age = (dai.Clock.now() - rgb_frame_time).total_seconds()
+            if frame_age > 0.1:
+                print(
+                    f"[{self.mount_position}] OAK frame age too large: {frame_age * 1000:.1f}ms"
+                )
+
+        return {"timestamps": timestamps, "images": images}
+
+    def serialize(self, data: dict[str, Any]) -> dict[str, Any]:
+        serialized_msg = ImageMessageSchema(timestamps=data["timestamps"], images=data["images"])
+        return serialized_msg.serialize()
+
+    def observation_space(self):
+        if gym is None:
+            return None
+        spaces = {}
+        if self.config.enable_color:
+            spaces["color_image"] = gym.spaces.Box(
+                low=0,
+                high=255,
+                shape=(self.config.color_image_dim[1], self.config.color_image_dim[0], 3),
+                dtype=np.uint8,
+            )
+        if self.config.enable_mono_cameras:
+            spaces["mono_left_image"] = gym.spaces.Box(
+                low=0,
+                high=255,
+                shape=(self.config.monochrome_image_dim[1], self.config.monochrome_image_dim[0]),
+                dtype=np.uint8,
+            )
+            spaces["mono_right_image"] = gym.spaces.Box(
+                low=0,
+                high=255,
+                shape=(self.config.monochrome_image_dim[1], self.config.monochrome_image_dim[0]),
+                dtype=np.uint8,
+            )
+        return gym.spaces.Dict(spaces)
+
+    def close(self):
+        if self._run_as_server:
+            self.stop_server()
+        if hasattr(self, "pipeline") and self.pipeline.isRunning():
+            self.pipeline.stop()
+        self.device.close()
+
+    def run_server(self):
+        if not self._run_as_server:
+            raise ValueError("run_as_server must be True to call run_server()")
+        while True:
+            frame = self.read()
+            if frame is None:
+                continue
+            msg = self.serialize(frame)
+            self.send_message({self.mount_position: msg})
+
+    def __del__(self):
+        self.close()
+
+
+if __name__ == "__main__":
+    import argparse
+
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--server", action="store_true", help="Run as server")
+    parser.add_argument("--host", type=str, default="localhost", help="Server IP address")
+    parser.add_argument("--port", type=int, default=5555, help="Port number")
+    parser.add_argument("--device-id", type=str, default=None, help="Specific device ID")
+    parser.add_argument(
+        "--enable-mono", action="store_true", help="Enable monochrome cameras (CAM_B & CAM_C)"
+    )
+    parser.add_argument("--mount-position", type=str, default="ego_view", help="Mount position")
+    parser.add_argument("--show-image", action="store_true", help="Display images")
+    parser.add_argument("--use-mjpeg", action="store_true", help="Use MJPEG encoding on-device")
+    parser.add_argument(
+        "--mjpeg-quality", type=int, default=80, help="MJPEG quality 1-100 (default: 80)"
+    )
+    args = parser.parse_args()
+
+    oak_config = OAKConfig()
+    if args.enable_mono:
+        oak_config.enable_mono_cameras = True
+    if args.use_mjpeg:
+        oak_config.use_mjpeg = True
+        oak_config.mjpeg_quality = args.mjpeg_quality
+
+    if args.server:
+        oak = OAKSensor(
+            run_as_server=True,
+            port=args.port,
+            config=oak_config,
+            device_id=args.device_id,
+            mount_position=args.mount_position,
+        )
+        print(f"Starting OAK server on port {args.port}")
+        oak.run_server()
+    else:
+        oak = OAKSensor(run_as_server=False, config=oak_config, device_id=args.device_id)
+        print("Running OAK camera in standalone mode")
+
+        while True:
+            frame = oak.read()
+            if frame is None:
+                print("Waiting for frame...")
+                time.sleep(0.5)
+                continue
+
+            if args.show_image:
+                for key, img in frame.get("images", {}).items():
+                    if isinstance(img, np.ndarray):
+                        cv2.imshow(key, img[..., ::-1] if img.ndim == 3 and img.shape[2] == 3 else img)
+                if cv2.waitKey(1) == ord("q"):
+                    break
+
+            time.sleep(0.01)
+
+        cv2.destroyAllWindows()
+        oak.close()

GR00T-WholeBodyControl/gear_sonic/camera/drivers/realsense.py

@@ -0,0 +1,173 @@
+"""Intel RealSense camera driver.
+
+Requires the ``pyrealsense2`` SDK — install with::
+
+    pip install pyrealsense2
+
+See https://github.com/IntelRealSense/librealsense for hardware-specific instructions.
+"""
+
+import time
+from typing import Any
+
+import numpy as np
+
+try:
+    import gymnasium as gym
+except ImportError:
+    gym = None  # type: ignore[assignment]
+
+import pyrealsense2 as rs
+
+from gear_sonic.camera.sensor import Sensor
+from gear_sonic.camera.sensor_server import (
+    CameraMountPosition,
+    ImageMessageSchema,
+    SensorServer,
+)
+
+
+class RealSenseConfig:
+    """Configuration for the RealSense camera."""
+
+    depth_image_dim: tuple[int, int] = (640, 480)
+    color_image_dim: tuple[int, int] = (640, 480)
+    fps: int = 30
+    mount_position: str = CameraMountPosition.EGO_VIEW.value
+
+
+class RealSenseSensor(Sensor, SensorServer):
+    """Sensor for Intel RealSense depth cameras."""
+
+    def __init__(
+        self,
+        run_as_server: bool = False,
+        port: int = 5555,
+        config: RealSenseConfig = RealSenseConfig(),
+        id: int = 0,
+        mount_position: str = CameraMountPosition.EGO_VIEW.value,
+    ):
+        devices = rs.context().query_devices()
+        if len(devices) == 0:
+            raise RuntimeError("No RealSense devices found")
+
+        for device in devices:
+            print(f"Device: {device.get_info(rs.camera_info.name)}")
+            print(f"    Serial number: {device.get_info(rs.camera_info.serial_number)}")
+            print(f"    Firmware version: {device.get_info(rs.camera_info.firmware_version)}")
+
+        self.pipeline = rs.pipeline()
+        self.config = rs.config()
+        devices = sorted(devices, key=lambda x: x.get_info(rs.camera_info.serial_number))
+        self.config.enable_device(devices[id].get_info(rs.camera_info.serial_number))
+
+        try:
+            self.config.enable_stream(
+                rs.stream.color,
+                config.color_image_dim[0],
+                config.color_image_dim[1],
+                rs.format.rgb8,
+                config.fps,
+            )
+            self.config.enable_stream(
+                rs.stream.depth,
+                config.depth_image_dim[0],
+                config.depth_image_dim[1],
+                rs.format.z16,
+                config.fps,
+            )
+            self.pipeline.start(self.config)
+        except Exception as e:
+            raise RuntimeError(f"Failed to start RealSense pipeline: {e}")
+
+        self._realsense_config = config
+        self._run_as_server = run_as_server
+        self.mount_position = mount_position
+        if self._run_as_server:
+            self.start_server(port)
+        print(
+            f"Done initializing RealSense sensor: "
+            f"{devices[id].get_info(rs.camera_info.serial_number)}"
+        )
+
+    def read(self) -> dict[str, Any] | None:
+        try:
+            frames = self.pipeline.wait_for_frames()
+        except Exception as e:
+            print(f"ERROR! Failed to wait for frames: {e}")
+            return None
+
+        color_frame = frames.get_color_frame()
+        depth_frame = frames.get_depth_frame()
+
+        if not color_frame or not depth_frame:
+            print("WARNING! No color or depth frame")
+            return None
+
+        try:
+            color_image = np.asanyarray(color_frame.get_data())
+            depth_image = np.asanyarray(depth_frame.get_data())
+        except Exception as e:
+            print(f"ERROR! Failed to convert frames to numpy arrays: {e}")
+            return None
+
+        if color_image.size == 0 or depth_image.size == 0:
+            print("WARNING! Empty color or depth image")
+            return None
+
+        current_time = time.time()
+        timestamps = {
+            self.mount_position: current_time,
+            f"{self.mount_position}_depth": current_time,
+        }
+        images = {
+            self.mount_position: color_image,
+            f"{self.mount_position}_depth": depth_image,
+        }
+        return {"timestamps": timestamps, "images": images}
+
+    def serialize(self, data: dict[str, Any]) -> dict[str, Any]:
+        serialized_msg = ImageMessageSchema(timestamps=data["timestamps"], images=data["images"])
+        return serialized_msg.serialize()
+
+    def observation_space(self):
+        if gym is None:
+            return None
+        return gym.spaces.Dict(
+            {
+                "color_image": gym.spaces.Box(
+                    low=0,
+                    high=255,
+                    shape=(
+                        self._realsense_config.color_image_dim[1],
+                        self._realsense_config.color_image_dim[0],
+                        3,
+                    ),
+                    dtype=np.uint8,
+                ),
+                "depth_image": gym.spaces.Box(
+                    low=0,
+                    high=255,
+                    shape=(
+                        self._realsense_config.depth_image_dim[1],
+                        self._realsense_config.depth_image_dim[0],
+                        1,
+                    ),
+                    dtype=np.uint16,
+                ),
+            }
+        )
+
+    def close(self):
+        if self._run_as_server:
+            self.stop_server()
+        self.pipeline.stop()
+
+    def run_server(self):
+        if not self._run_as_server:
+            raise ValueError("run_as_server must be True to call run_server()")
+        while True:
+            read_result = self.read()
+            if read_result is None:
+                continue
+            self.send_message({self.mount_position: self.serialize(read_result)})

GR00T-WholeBodyControl/gear_sonic/camera/drivers/usb_camera.py

@@ -0,0 +1,100 @@
+"""Generic USB webcam driver using OpenCV.
+
+No hardware SDK needed — works with any UVC-compatible camera visible as
+``/dev/video*``.  Only requires ``opencv-python``.
+"""
+
+import time
+from typing import Any
+
+import cv2
+import numpy as np
+
+try:
+    import gymnasium as gym
+except ImportError:
+    gym = None  # type: ignore[assignment]
+
+from gear_sonic.camera.sensor import Sensor
+from gear_sonic.camera.sensor_server import CameraMountPosition
+
+
+class USBCameraConfig:
+    """Configuration for generic USB camera."""
+
+    image_dim: tuple = (640, 480)
+    fps: int = 30
+    device_index: int = 0
+
+
+class USBCameraSensor(Sensor):
+    """Sensor for generic USB cameras using OpenCV VideoCapture."""
+
+    def __init__(
+        self,
+        config: USBCameraConfig = USBCameraConfig(),
+        mount_position: str = CameraMountPosition.EGO_VIEW.value,
+        device_index: int | None = None,
+    ):
+        self.config = config
+        self.mount_position = mount_position
+
+        idx = device_index if device_index is not None else config.device_index
+
+        self.cap = cv2.VideoCapture(idx)
+        if not self.cap.isOpened():
+            raise RuntimeError(f"Failed to open USB camera at index {idx}")
+
+        self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, config.image_dim[0])
+        self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, config.image_dim[1])
+        self.cap.set(cv2.CAP_PROP_FPS, config.fps)
+        self.cap.set(cv2.CAP_PROP_BUFFERSIZE, 1)
+
+        print(f"[{mount_position}] Warming up USB camera...")
+        for _ in range(10):
+            ret, _ = self.cap.read()
+            if ret:
+                break
+            time.sleep(0.1)
+
+        print(f"[{mount_position}] USB camera opened at index {idx}")
+        width = int(self.cap.get(cv2.CAP_PROP_FRAME_WIDTH))
+        height = int(self.cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
+        print(f"  Resolution: {width}x{height}")
+        print(f"  FPS: {self.cap.get(cv2.CAP_PROP_FPS)}")
+
+    def read(self) -> dict[str, Any] | None:
+        ret, frame = self.cap.read()
+        if not ret or frame is None:
+            print(f"[{self.mount_position}] USB camera read failed: ret={ret}")
+            return None
+
+        frame_rgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
+        return {
+            "timestamps": {self.mount_position: time.time()},
+            "images": {self.mount_position: frame_rgb},
+        }
+
+    def serialize(self, data: dict[str, Any]) -> dict[str, Any]:
+        from gear_sonic.camera.sensor_server import ImageMessageSchema
+
+        serialized_msg = ImageMessageSchema(timestamps=data["timestamps"], images=data["images"])
+        return serialized_msg.serialize()
+
+    def observation_space(self):
+        if gym is None:
+            return None
+        return gym.spaces.Dict(
+            {
+                "color_image": gym.spaces.Box(
+                    low=0,
+                    high=255,
+                    shape=(self.config.image_dim[1], self.config.image_dim[0], 3),
+                    dtype=np.uint8,
+                ),
+            }
+        )
+
+    def close(self):
+        if self.cap is not None:
+            self.cap.release()

GR00T-WholeBodyControl/gear_sonic/data/assets/robot_description/mjcf/g1_29dof_rev_1_0.xml

@@ -0,0 +1,472 @@
+<mujoco model="g1_29dof_rev_1_0">
+    <compiler angle="radian" meshdir="../meshes/g1/" />
+
+    <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_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">
+            <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" />
+                <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" />
+                    <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" />
+                        <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" />
+                            <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" />
+                                <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" />
+                                    <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" />
+                <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" />
+                    <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" />
+                        <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" />
+                            <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" />
+                                <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" />
+                                    <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" />
+                <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" />
+                    <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" />
+                        <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" />
+                            <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" />
+                                <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" />
+                                    <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" />
+                                        <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" />
+                                            <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" />
+                                                <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" />
+                                                    <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_rubber_hand" />
+                                                </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" />
+                            <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" />
+                                <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" />
+                                    <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" />
+                                        <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" />
+                                            <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" />
+                                                <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" />
+                                                    <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_rubber_hand" />
+                                                </body>
+                                            </body>
+                                        </body>
+                                    </body>
+                                </body>
+                            </body>
+                        </body>
+                    </body>
+                </body>
+            </body>
+        </body>
+    </worldbody>
+
+    <actuator>
+        <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="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" />
+    </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/gear_sonic/data/assets/robot_description/mjcf/h2.xml

@@ -0,0 +1,270 @@
+<mujoco model="H1031">
+  <compiler angle="radian" meshdir="meshes/"/>
+
+  <asset>
+    <mesh name="pelvis" content_type="model/stl" file="pelvis.stl"/>
+    <mesh name="pelvis_collision_link" content_type="model/stl" file="pelvis_collision.stl"/>
+    <mesh name="left_hip_pitch_link" content_type="model/stl" file="left_hip_pitch_link.stl"/>
+    <mesh name="left_hip_roll_link" content_type="model/stl" file="left_hip_roll_link.stl"/>
+    <mesh name="left_hip_yaw_link" content_type="model/stl" file="left_hip_yaw_link.stl"/>
+    <mesh name="left_knee_link" content_type="model/stl" file="left_knee_link.stl"/>
+    <mesh name="left_ankle_roll_link" content_type="model/stl" file="left_ankle_roll_link.stl"/>
+    <mesh name="left_ankle_pitch_link" content_type="model/stl" file="left_ankle_pitch_link.stl"/>
+    <mesh name="right_hip_pitch_link" content_type="model/stl" file="right_hip_pitch_link.stl"/>
+    <mesh name="right_hip_roll_link" content_type="model/stl" file="right_hip_roll_link.stl"/>
+    <mesh name="right_hip_yaw_link" content_type="model/stl" file="right_hip_yaw_link.stl"/>
+    <mesh name="right_knee_link" content_type="model/stl" file="right_knee_link.stl"/>
+    <mesh name="right_ankle_roll_link" content_type="model/stl" file="right_ankle_roll_link.stl"/>
+    <mesh name="right_ankle_pitch_link" content_type="model/stl" file="right_ankle_pitch_link.stl"/>
+    <mesh name="waist_yaw_link" content_type="model/stl" file="waist_yaw_link.stl"/>
+    <mesh name="waist_roll_link" content_type="model/stl" file="waist_roll_link.stl"/>
+    <mesh name="torso_link" content_type="model/stl" file="torso_link.stl"/>
+    <mesh name="torso_collision" content_type="model/stl" file="torso_collision.stl"/>
+    <mesh name="head_pitch_link" content_type="model/stl" file="head_pitch_link.stl"/>
+    <mesh name="head_yaw_link" content_type="model/stl" file="head_yaw_link.stl"/>
+    <mesh name="left_shoulder_pitch_link" content_type="model/stl" file="left_shoulder_pitch_link.stl"/>
+    <mesh name="left_shoulder_roll_link" content_type="model/stl" file="left_shoulder_roll_link.stl"/>
+    <mesh name="left_shoulder_yaw_link" content_type="model/stl" file="left_shoulder_yaw_link.stl"/>
+    <mesh name="left_elbow_link" content_type="model/stl" file="left_elbow_link.stl"/>
+    <mesh name="left_wrist_roll_link" content_type="model/stl" file="left_wrist_roll_link.stl"/>
+    <mesh name="left_wrist_pitch_link" content_type="model/stl" file="left_wrist_pitch_link.stl"/>
+    <mesh name="left_rubber_hand" content_type="model/stl" file="left_rubber_hand.stl"/>
+    <mesh name="right_shoulder_pitch_link" content_type="model/stl" file="right_shoulder_pitch_link.stl"/>
+    <mesh name="right_shoulder_roll_link" content_type="model/stl" file="right_shoulder_roll_link.stl"/>
+    <mesh name="right_shoulder_yaw_link" content_type="model/stl" file="right_shoulder_yaw_link.stl"/>
+    <mesh name="right_elbow_link" content_type="model/stl" file="right_elbow_link.stl"/>
+    <mesh name="right_wrist_roll_link" content_type="model/stl" file="right_wrist_roll_link.stl"/>
+    <mesh name="right_wrist_pitch_link" content_type="model/stl" file="right_wrist_pitch_link.stl"/>
+    <mesh name="right_rubber_hand" content_type="model/stl" file="right_rubber_hand.stl"/>
+  </asset>
+
+  <worldbody>
+    <body name="pelvis" pos="0 0 1.04">
+      <site name="imu" size="0.01" pos="0.0 0.0 0.0"/>
+      <inertial pos="0.00046244 -0.00004214 -0.01473435" quat="1 0 0 0" mass="7.60468095" diaginertia="0.03548205 0.02290677 0.02753519"/>
+      <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_collision_link"/>
+      <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="pelvis_collision_link"/>
+      <body name="left_hip_pitch_link" pos="0 0.083873 -0.048424" quat="0.965926 -0.25882 0 0">
+        <inertial pos="0.00300595 0.0593147 -0.0170915" quat="0.899485 0.435934 -0.0222136 -0.0198741" mass="2.79753" diaginertia="0.00510297 0.00410085 0.0034593"/>
+        <joint name="left_hip_pitch_joint" pos="0 0 0" axis="0 1 0" range="-2.4526 2.77542" actuatorfrcrange="-360 360"/>
+        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1" mesh="left_hip_pitch_link"/>
+        <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_hip_pitch_link"/>
+        <body name="left_hip_roll_link" pos="0.041851 0.0635 -0.019" quat="0.965926 0.25882 0 0">
+          <inertial pos="-0.0302619 -0.0046893 -0.0444985" quat="-0.0935812 0.633472 -0.0477755 0.766598" mass="0.627146" diaginertia="0.00200252 0.00167509 0.00128023"/>
+          <joint name="left_hip_roll_joint" pos="0 0 0" axis="1 0 0" range="-0.467441 2.16886" actuatorfrcrange="-360 360"/>
+          <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.028187 -0.0095 -0.073">
+            <inertial pos="0.00069408 -0.0108659 -0.113015" quat="0.621993 -0.0405952 0.0198637 0.781717" mass="7.08752" diaginertia="0.0514012 0.050596 0.0130067"/>
+            <joint name="left_hip_yaw_joint" pos="0 0 0" axis="0 0 1" range="-2.827 2.827" actuatorfrcrange="-360 360"/>
+            <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.010341 0.018489 -0.3191">
+              <inertial pos="0.00461331 -0.0245322 -0.167155" quat="0.689662 0.00325283 0.00290593 0.724118" mass="3.5534" diaginertia="0.0512388 0.0510884 0.00333998"/>
+              <joint name="left_knee_joint" pos="0 0 0" axis="0 1 0" range="-0.08725 2.53025" actuatorfrcrange="-360 360"/>
+              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1" mesh="left_knee_link"/>
+              <geom size="0.04 0.175" pos="0.01 -0.025 -0.25" type="cylinder" rgba="0.7 0.7 0.7 1"/>
+              <body name="left_ankle_roll_link" pos="0.03272 -0.023002 -0.49662">
+                <inertial pos="-0.0174063 6.424e-05 -0.0002146" quat="0.499157 0.502573 -0.497809 0.500449" mass="0.123182" diaginertia="4.00501e-05 3.43207e-05 1.44993e-05"/>
+                <joint name="left_ankle_roll_joint" pos="0 0 0" axis="1 0 0" range="-0.349066 0.296706" actuatorfrcrange="-19 19"/>
+                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1" mesh="left_ankle_roll_link"/>
+                <body name="left_ankle_pitch_link" pos="-0.0225 -0.0373 0">
+                  <inertial pos="0.032059 0.0370053 -0.0288282" quat="6.19452e-05 0.72345 -3.0004e-05 0.690377" mass="0.844967" diaginertia="0.00438317 0.00410964 0.000599978"/>
+                  <joint name="left_ankle_pitch_joint" pos="0 0 0" axis="0 1 0" range="-1.13446 0.610865" actuatorfrcrange="-66.88 66.88"/>
+                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.4 0.4 0.4 1" mesh="left_ankle_pitch_link"/>
+                  <geom type="mesh" rgba="0.4 0.4 0.4 1" mesh="left_ankle_pitch_link"/>
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+      <body name="right_hip_pitch_link" pos="0 -0.083873 -0.048424" quat="0.965926 0.25882 0 0">
+        <inertial pos="0.00300595 -0.0593147 -0.0170915" quat="0.435934 0.899485 0.0198741 0.0222136" mass="2.79753" diaginertia="0.00510297 0.00410085 0.0034593"/>
+        <joint name="right_hip_pitch_joint" pos="0 0 0" axis="0 1 0" range="-2.4526 2.77542" actuatorfrcrange="-360 360"/>
+        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1" mesh="right_hip_pitch_link"/>
+        <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_hip_pitch_link"/>
+        <body name="right_hip_roll_link" pos="0.041851 -0.0635 -0.019" quat="0.965926 -0.25882 0 0">
+          <inertial pos="-0.0302619 0.0046893 -0.0444985" quat="0.0935812 0.633472 0.0477755 0.766598" mass="0.627146" diaginertia="0.00200252 0.00167509 0.00128023"/>
+          <joint name="right_hip_roll_joint" pos="0 0 0" axis="1 0 0" range="-2.16886 0.467441" actuatorfrcrange="-360 360"/>
+          <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.028187 0.0095 -0.073">
+            <inertial pos="0.00069408 0.0108659 -0.113015" quat="0.781717 0.0198637 -0.0405952 0.621993" mass="7.08752" diaginertia="0.0514012 0.050596 0.0130067"/>
+            <joint name="right_hip_yaw_joint" pos="0 0 0" axis="0 0 1" range="-2.827 2.827" actuatorfrcrange="-360 360"/>
+            <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.010341 -0.018489 -0.3191">
+              <inertial pos="0.00461331 0.0245322 -0.167155" quat="0.724118 0.00290593 0.00325283 0.689662" mass="3.5534" diaginertia="0.0512388 0.0510884 0.00333998"/>
+              <joint name="right_knee_joint" pos="0 0 0" axis="0 1 0" range="-0.08725 2.53025" actuatorfrcrange="-360 360"/>
+              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1" mesh="right_knee_link"/>
+              <geom size="0.04 0.175" pos="0.01 0.025 -0.25" type="cylinder" rgba="0.7 0.7 0.7 1"/>
+              <body name="right_ankle_roll_link" pos="0.03272 0.023002 -0.49662">
+                <inertial pos="-0.0174063 -6.424e-05 -0.0002146" quat="0.502573 0.499157 -0.500449 0.497809" mass="0.123182" diaginertia="4.00501e-05 3.43207e-05 1.44993e-05"/>
+                <joint name="right_ankle_roll_joint" pos="0 0 0" axis="1 0 0" range="-0.296706 0.349066" actuatorfrcrange="-19 19"/>
+                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1" mesh="right_ankle_roll_link"/>
+                <body name="right_ankle_pitch_link" pos="-0.0225 0.0373 0">
+                  <inertial pos="0.032059 -0.0370053 -0.0288282" quat="-6.19452e-05 0.72345 3.0004e-05 0.690377" mass="0.844967" diaginertia="0.00438317 0.00410964 0.000599978"/>
+                  <joint name="right_ankle_pitch_joint" pos="0 0 0" axis="0 1 0" range="-1.13446 0.610865" actuatorfrcrange="-66.88 66.88"/>
+                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.4 0.4 0.4 1" mesh="right_ankle_pitch_link"/>
+                  <geom type="mesh" rgba="0.4 0.4 0.4 1" mesh="right_ankle_pitch_link"/>
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+      <body name="waist_yaw_link" pos="0 0 0.089837">
+        <inertial pos="0.00626994 0 0.0169696" quat="0 0.717728 0 0.696323" mass="0.279762" diaginertia="0.000311422 0.0002473 0.000123348"/>
+        <joint name="waist_yaw_joint" pos="0 0 0" axis="0 0 1" range="-1.7453 1.7453" actuatorfrcrange="-120 120"/>
+        <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">
+          <inertial pos="0 0.00128474 0" quat="0 0.707107 0 0.707107" mass="0.0658504" diaginertia="1.58e-05 1.041e-05 8.09e-06"/>
+          <joint name="waist_roll_joint" pos="0 0 0" axis="1 0 0" range="-0.5236 0.5236" actuatorfrcrange="-180 180"/>
+          <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.0134247 0.0054719 0.210501" quat="0.999399 -0.0341887 0.00399831 0.00403179" mass="15.8703" diaginertia="0.273064 0.244383 0.0840858"/>
+            <joint name="waist_pitch_joint" pos="0 0 0" axis="0 1 0" range="-0.43633 0.5236" actuatorfrcrange="-180 180"/>
+            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.4 0.4 0.4 1" mesh="torso_link"/>
+            <geom type="mesh" rgba="0.4 0.4 0.4 1" mesh="torso_collision"/>
+            <body name="head_pitch_link" pos="-0.0002283 2.7112e-05 0.42181">
+              <inertial pos="5.279e-08 -0.00035318 0.045125" quat="0.999996 -0.000955249 -0.00262397 -0.000580497" mass="1.82" diaginertia="0.00538697 0.00532442 0.00140982"/>
+              <joint name="head_pitch_joint" pos="0 0 0" axis="0 1 0" range="-0.5236 0.83775" actuatorfrcrange="-50 50"/>
+              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1" mesh="head_pitch_link"/>
+              <body name="head_yaw_link" pos="0 0.0003 0.1325">
+                <inertial pos="0.0219 0 0.0187" quat="0.667311 -0.227767 -0.231751 0.670156" mass="1.3" diaginertia="0.00535274 0.00472336 0.00378091"/>
+                <joint name="head_yaw_joint" pos="0 0 0" axis="0 0 1" range="-1.7453 1.7453" actuatorfrcrange="-50 50"/>
+                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1" mesh="head_yaw_link"/>
+                <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="head_yaw_link"/>
+              </body>
+            </body>
+            <body name="left_shoulder_pitch_link" pos="0.0015 0.11254 0.35812" quat="0.990959 0.134164 0 0">
+              <inertial pos="0.00296153 0.0520268 -0.025045" quat="0.955852 0.29138 -0.0379121 0.00288293" mass="1.06056" diaginertia="0.00122192 0.00110581 0.000458834"/>
+              <joint name="left_shoulder_pitch_joint" pos="0 0 0" axis="0 1 0" range="-2.61799 1.8326" actuatorfrcrange="-120 120"/>
+              <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 type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_shoulder_pitch_link"/>
+              <body name="left_shoulder_roll_link" pos="0.027682 0.061638 -0.039909" quat="0.990959 -0.134164 0 0">
+                <inertial pos="-0.02061 0.00415443 -0.0629422" quat="0.678366 0.110828 0.0778018 0.722138" mass="1.34511" diaginertia="0.00210225 0.00198513 0.00105666"/>
+                <joint name="left_shoulder_roll_joint" pos="0 0 0" axis="1 0 0" range="-0.516617 2.63545" actuatorfrcrange="-54 54"/>
+                <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 type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_shoulder_roll_link"/>
+                <body name="left_shoulder_yaw_link" pos="-0.030632 0.0037809 -0.10422">
+                  <inertial pos="0.00364187 0.0186515 -0.0558555" quat="0.995617 -0.0729116 -0.0584518 -0.00371547" mass="0.915717" diaginertia="0.00306267 0.00257349 0.00143042"/>
+                  <joint name="left_shoulder_yaw_joint" pos="0 0 0" axis="0 0 1" range="-2.61799 2.61799" actuatorfrcrange="-54 54"/>
+                  <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.016955 0.031006 -0.12719">
+                    <inertial pos="0.0459969 -0.0283296 -0.00661325" quat="0.520177 0.507229 0.507638 0.463074" mass="1.69432" diaginertia="0.00548751 0.00533836 0.000695052"/>
+                    <joint name="left_elbow_joint" pos="0 0 0" axis="0 1 0" range="-0.986111 3.07178" actuatorfrcrange="-54 54"/>
+                    <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.12826 -0.030815 -0.013299">
+                      <inertial pos="0.0337195 0.00237482 0.00031544" quat="-0.242945 0.638599 0.286806 0.671499" mass="0.969837" diaginertia="0.000926999 0.000903832 0.000852829"/>
+                      <joint name="left_wrist_roll_joint" pos="0 0 0" axis="1 0 0" range="-2.61799 2.61799" actuatorfrcrange="-54 54"/>
+                      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1" mesh="left_wrist_roll_link"/>
+                      <body name="left_wrist_pitch_link" pos="0.082003 -0.031003 0.00020786">
+                        <inertial pos="0.01821 0.0314035 0.0115687" quat="0.977698 -0.037912 -0.203807 0.033629" mass="0.946953" diaginertia="0.000918192 0.00082104 0.000648419"/>
+                        <joint name="left_wrist_pitch_joint" pos="0 0 0" axis="0 1 0" range="-0.436332 0.436332" actuatorfrcrange="-25 25"/>
+                        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1" mesh="left_wrist_pitch_link"/>
+                        <body name="left_wrist_yaw_link" pos="0.026025 0.030996 0.026981">
+                          <inertial pos="0.00932726 0.00247855 -0.0196166" quat="0.528225 0.503579 0.545204 0.41248" mass="0.775752" diaginertia="0.0021247 0.00196098 0.000336621"/>
+                          <joint name="left_wrist_yaw_joint" pos="0 0 0" axis="0 0 1" range="-1.22173 1.22173" actuatorfrcrange="-25 25"/>
+                          <geom size="0.028 0.03" pos="0 0 -0.029" type="cylinder" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"/>
+                          <geom size="0.04" pos="0 0 -0.029" rgba="0.7 0.7 0.7 1"/>
+                          <geom pos="0.025 0 -0.03" quat="1 0 0 0" type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.4 0.4 0.4 1" mesh="left_rubber_hand"/>
+                        </body>
+                      </body>
+                    </body>
+                  </body>
+                </body>
+              </body>
+            </body>
+            <body name="right_shoulder_pitch_link" pos="0.0015 -0.1125 0.35812" quat="0.990959 -0.134164 0 0">
+              <inertial pos="0.00296153 -0.0520268 -0.025045" quat="0.955852 -0.291379 -0.0379149 -0.00289698" mass="1.06056" diaginertia="0.00122192 0.00110581 0.000458835"/>
+              <joint name="right_shoulder_pitch_joint" pos="0 0 0" axis="0 1 0" range="-2.61799 1.8326" actuatorfrcrange="-120 120"/>
+              <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 type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_shoulder_pitch_link"/>
+              <body name="right_shoulder_roll_link" pos="0.027682 -0.061638 -0.039909" quat="0.990959 0.134164 0 0">
+                <inertial pos="-0.02061 -0.00415443 -0.0629422" quat="0.722138 0.0778018 0.110828 0.678366" mass="1.34511" diaginertia="0.00210225 0.00198513 0.00105666"/>
+                <joint name="right_shoulder_roll_joint" pos="0 0 0" axis="1 0 0" range="-2.63545 0.516617" actuatorfrcrange="-54 54"/>
+                <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 type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_shoulder_roll_link"/>
+                <body name="right_shoulder_yaw_link" pos="-0.030632 -0.0037809 -0.10422">
+                  <inertial pos="0.00364187 -0.0186515 -0.0558555" quat="0.995617 0.0729116 -0.0584518 0.00371547" mass="0.915717" diaginertia="0.00306267 0.00257349 0.00143042"/>
+                  <joint name="right_shoulder_yaw_joint" pos="0 0 0" axis="0 0 1" range="-2.61799 2.61799" actuatorfrcrange="-54 54"/>
+                  <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.016955 -0.031006 -0.12719">
+                    <inertial pos="0.0459969 0.0283296 -0.00661325" quat="0.463074 0.507638 0.507229 0.520177" mass="1.69432" diaginertia="0.00548751 0.00533836 0.000695052"/>
+                    <joint name="right_elbow_joint" pos="0 0 0" axis="0 1 0" range="-0.986111 3.07178" actuatorfrcrange="-54 54"/>
+                    <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.12826 0.030815 -0.013299">
+                      <inertial pos="0.0337195 -0.00237482 0.00031544" quat="0.242945 0.638599 -0.286806 0.671499" mass="0.969837" diaginertia="0.000926999 0.000903832 0.000852829"/>
+                      <joint name="right_wrist_roll_joint" pos="0 0 0" axis="1 0 0" range="-2.61799 2.61799" actuatorfrcrange="-54 54"/>
+                      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1" mesh="right_wrist_roll_link"/>
+                      <body name="right_wrist_pitch_link" pos="0.082003 0.031003 0.00020786">
+                        <inertial pos="0.01821 -0.0314035 0.0115687" quat="0.977698 0.037912 -0.203807 -0.033629" mass="0.946953" diaginertia="0.000918192 0.00082104 0.000648419"/>
+                        <joint name="right_wrist_pitch_joint" pos="0 0 0" axis="0 1 0" range="-0.436332 0.436332" actuatorfrcrange="-25 25"/>
+                        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1" mesh="right_wrist_pitch_link"/>
+                        <body name="right_wrist_yaw_link" pos="0.026025 -0.030996 0.026981">
+                          <inertial pos="0.00932726 -0.00247855 -0.0196166" quat="0.41248 0.545204 0.503579 0.528225" mass="0.775752" diaginertia="0.0021247 0.00196098 0.000336621"/>
+                          <joint name="right_wrist_yaw_joint" pos="0 0 0" axis="0 0 1" range="-1.22173 1.22173" actuatorfrcrange="-25 25"/>
+                          <geom size="0.028 0.03" pos="0 0 -0.029" type="cylinder" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"/>
+                          <geom size="0.04" pos="0 0 -0.029" rgba="0.7 0.7 0.7 1"/>
+                          <geom pos="0.025 0 -0.03" quat="1 0 0 0" type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.4 0.4 0.4 1" mesh="right_rubber_hand"/>
+                        </body>
+                      </body>
+                    </body>
+                  </body>
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+    </body>
+  </worldbody>
+
+
+  <actuator>
+    <motor name="left_hip_pitch" joint="left_hip_pitch_joint" gear="1"/>
+    <motor name="left_hip_roll" joint="left_hip_roll_joint" gear="1"/>
+    <motor name="left_hip_yaw" joint="left_hip_yaw_joint" gear="1"/>
+    <motor name="left_knee" joint="left_knee_joint" gear="1"/>
+    <motor name="left_ankle_roll" joint="left_ankle_roll_joint" gear="1"/>
+    <motor name="left_ankle_pitch" joint="left_ankle_pitch_joint" gear="1"/>
+
+    <motor name="right_hip_pitch" joint="right_hip_pitch_joint" gear="1"/>
+    <motor name="right_hip_roll" joint="right_hip_roll_joint" gear="1"/>
+    <motor name="right_hip_yaw" joint="right_hip_yaw_joint" gear="1"/>
+    <motor name="right_knee" joint="right_knee_joint" gear="1"/>
+    <motor name="right_ankle_roll" joint="right_ankle_roll_joint" gear="1"/>
+    <motor name="right_ankle_pitch" joint="right_ankle_pitch_joint" gear="1"/>
+
+    <motor name="waist_yaw" joint="waist_yaw_joint" gear="1"/>
+    <motor name="waist_roll" joint="waist_roll_joint" gear="1"/>
+    <motor name="waist_pitch" joint="waist_pitch_joint" gear="1"/>
+
+    <motor name="head_pitch" joint="head_pitch_joint" gear="1"/>
+    <motor name="head_yaw" joint="head_yaw_joint" gear="1"/>
+
+    <motor name="left_shoulder_pitch" joint="left_shoulder_pitch_joint" gear="1"/>
+    <motor name="left_shoulder_roll" joint="left_shoulder_roll_joint" gear="1"/>
+    <motor name="left_shoulder_yaw" joint="left_shoulder_yaw_joint" gear="1"/>
+    <motor name="left_elbow" joint="left_elbow_joint" gear="1"/>
+    <motor name="left_wrist_roll" joint="left_wrist_roll_joint" gear="1"/>
+    <motor name="left_wrist_pitch" joint="left_wrist_pitch_joint" gear="1"/>
+    <motor name="left_wrist_yaw" joint="left_wrist_yaw_joint" gear="1"/>
+
+    <motor name="right_shoulder_pitch" joint="right_shoulder_pitch_joint" gear="1"/>
+    <motor name="right_shoulder_roll" joint="right_shoulder_roll_joint" gear="1"/>
+    <motor name="right_shoulder_yaw" joint="right_shoulder_yaw_joint" gear="1"/>
+    <motor name="right_elbow" joint="right_elbow_joint" gear="1"/>
+    <motor name="right_wrist_roll" joint="right_wrist_roll_joint" gear="1"/>
+    <motor name="right_wrist_pitch" joint="right_wrist_pitch_joint" gear="1"/>
+    <motor name="right_wrist_yaw" joint="right_wrist_yaw_joint" gear="1"/>
+  </actuator>
+</mujoco>

GR00T-WholeBodyControl/gear_sonic/data/assets/robot_description/urdf/g1/main.urdf

@@ -0,0 +1,1498 @@
+<?xml version="1.0"?>
+
+<!-- This URDF is adapted from the BeyondMimic project.
+     See: https://github.com/HybridRobotics/whole_body_tracking -->
+
+<robot name="g1">
+
+    <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="package://robot_description/meshes/g1/pelvis.STL" />
+            </geometry>
+            <material name="black">
+                <color rgba="0.05 0.05 0.05 1" />
+            </material>
+        </visual>
+        <collision>
+            <origin xyz="0 0 -0.08" />
+            <geometry>
+                <sphere radius="0.07" />
+            </geometry>
+        </collision>
+    </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="package://robot_description/meshes/g1/pelvis_contour_link.STL" />
+            </geometry>
+            <material name="black">
+                <color rgba="0.05 0.05 0.05 1" />
+            </material>
+        </visual>
+    </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="package://robot_description/meshes/g1/left_hip_pitch_link.STL" />
+            </geometry>
+            <material name="dark">
+                <color rgba="0.2 0.2 0.2 1" />
+            </material>
+        </visual>
+    </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="package://robot_description/meshes/g1/left_hip_roll_link.STL" />
+            </geometry>
+            <material name="white">
+                <color rgba="0.7 0.7 0.7 1" />
+            </material>
+        </visual>
+        <collision>
+            <origin xyz="0.02 0 -0.1" />
+            <geometry>
+                <cylinder radius="0.05" length="0.2" />
+            </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="139" velocity="20" />
+    </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="package://robot_description/meshes/g1/left_hip_yaw_link.STL" />
+            </geometry>
+            <material name="white">
+                <color rgba="0.7 0.7 0.7 1" />
+            </material>
+        </visual>
+    </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="package://robot_description/meshes/g1/left_knee_link.STL" />
+            </geometry>
+            <material name="white">
+                <color rgba="0.7 0.7 0.7 1" />
+            </material>
+        </visual>
+        <collision>
+            <origin xyz="0.02 0 -0.1" />
+            <geometry>
+                <cylinder radius="0.05" length="0.2" />
+            </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="package://robot_description/meshes/g1/left_ankle_pitch_link.STL" />
+            </geometry>
+            <material name="black">
+                <color rgba="0.05 0.05 0.05 1" />
+            </material>
+        </visual>
+    </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="package://robot_description/meshes/g1/left_ankle_roll_link.STL" />
+            </geometry>
+            <material name="black">
+                <color rgba="0.05 0.05 0.05 1" />
+            </material>
+        </visual>
+        <collision>
+            <origin xyz="0.075 -0.026 -0.025" rpy="0 1.5708 0" />
+            <geometry>
+                <cylinder radius="0.01" length="0.05" />
+            </geometry>
+        </collision>
+        <collision>
+            <origin xyz="0.0395 -0.018 -0.025" rpy="0 -1.5708 0" />
+            <geometry>
+                <cylinder radius="0.008" length="0.167" />
+            </geometry>
+        </collision>
+        <collision>
+            <origin xyz="0.039 -0.01 -0.025" rpy="0 -1.5708 0" />
+            <geometry>
+                <cylinder radius="0.01" length="0.182" />
+            </geometry>
+        </collision>
+        <collision>
+            <origin xyz="0.039 0 -0.025" rpy="0 -1.5708 0" />
+            <geometry>
+                <cylinder radius="0.01" length="0.186" />
+            </geometry>
+        </collision>
+        <collision>
+            <origin xyz="0.039 0.01 -0.025" rpy="0 -1.5708 0" />
+            <geometry>
+                <cylinder radius="0.01" length="0.182" />
+            </geometry>
+        </collision>
+        <collision>
+            <origin xyz="0.0395 0.018 -0.025" rpy="0 -1.5708 0" />
+            <geometry>
+                <cylinder radius="0.008" length="0.167" />
+            </geometry>
+        </collision>
+        <collision>
+            <origin xyz="0.075 0.026 -0.025" rpy="0 1.5708 0" />
+            <geometry>
+                <cylinder radius="0.01" length="0.05" />
+            </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="package://robot_description/meshes/g1/right_hip_pitch_link.STL" />
+            </geometry>
+            <material name="dark">
+                <color rgba="0.2 0.2 0.2 1" />
+            </material>
+        </visual>
+    </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="package://robot_description/meshes/g1/right_hip_roll_link.STL" />
+            </geometry>
+            <material name="white">
+                <color rgba="0.7 0.7 0.7 1" />
+            </material>
+        </visual>
+        <collision>
+            <origin xyz="0.02 0 -0.1" />
+            <geometry>
+                <cylinder radius="0.05" length="0.2" />
+            </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="139" velocity="20" />
+    </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="package://robot_description/meshes/g1/right_hip_yaw_link.STL" />
+            </geometry>
+            <material name="white">
+                <color rgba="0.7 0.7 0.7 1" />
+            </material>
+        </visual>
+    </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="package://robot_description/meshes/g1/right_knee_link.STL" />
+            </geometry>
+            <material name="white">
+                <color rgba="0.7 0.7 0.7 1" />
+            </material>
+        </visual>
+        <collision>
+            <origin xyz="0.02 0 -0.1" />
+            <geometry>
+                <cylinder radius="0.05" length="0.2" />
+            </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="package://robot_description/meshes/g1/right_ankle_pitch_link.STL" />
+            </geometry>
+            <material name="black">
+                <color rgba="0.05 0.05 0.05 1" />
+            </material>
+        </visual>
+    </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="package://robot_description/meshes/g1/right_ankle_roll_link.STL" />
+            </geometry>
+            <material name="black">
+                <color rgba="0.05 0.05 0.05 1" />
+            </material>
+        </visual>
+        <collision>
+            <origin xyz="0.075 -0.026 -0.025" rpy="0 1.5708 0" />
+            <geometry>
+                <cylinder radius="0.01" length="0.05" />
+            </geometry>
+        </collision>
+        <collision>
+            <origin xyz="0.0395 -0.018 -0.025" rpy="0 -1.5708 0" />
+            <geometry>
+                <cylinder radius="0.008" length="0.167" />
+            </geometry>
+        </collision>
+        <collision>
+            <origin xyz="0.039 -0.01 -0.025" rpy="0 -1.5708 0" />
+            <geometry>
+                <cylinder radius="0.01" length="0.182" />
+            </geometry>
+        </collision>
+        <collision>
+            <origin xyz="0.039 0 -0.025" rpy="0 -1.5708 0" />
+            <geometry>
+                <cylinder radius="0.01" length="0.186" />
+            </geometry>
+        </collision>
+        <collision>
+            <origin xyz="0.039 0.01 -0.025" rpy="0 -1.5708 0" />
+            <geometry>
+                <cylinder radius="0.01" length="0.182" />
+            </geometry>
+        </collision>
+        <collision>
+            <origin xyz="0.0395 0.018 -0.025" rpy="0 -1.5708 0" />
+            <geometry>
+                <cylinder radius="0.008" length="0.167" />
+            </geometry>
+        </collision>
+        <collision>
+            <origin xyz="0.075 0.026 -0.025" rpy="0 1.5708 0" />
+            <geometry>
+                <cylinder radius="0.01" length="0.05" />
+            </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.003494 0.000233 0.018034" rpy="0 0 0" />
+            <mass value="0.214" />
+            <inertia ixx="0.00010673" ixy="2.703E-06" ixz="-7.631E-06" iyy="0.00010422"
+                iyz="-2.01E-07"
+                izz="0.0001625" />
+        </inertial>
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://robot_description/meshes/g1/waist_yaw_link_rev_1_0.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 2.3E-05 0" rpy="0 0 0" />
+            <mass value="0.086" />
+            <inertia ixx="7.079E-06" ixy="0" ixz="0" iyy="6.339E-06" iyz="0" izz="8.245E-06" />
+        </inertial>
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://robot_description/meshes/g1/waist_roll_link_rev_1_0.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.044" 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.000931 0.000346 0.15082" rpy="0 0 0" />
+            <mass value="6.78" />
+            <inertia ixx="0.05905" ixy="3.3302E-05" ixz="-0.0017715" iyy="0.047014"
+                iyz="-2.2399E-05" izz="0.025652" />
+        </inertial>
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://robot_description/meshes/g1/torso_link_rev_1_0.STL" />
+            </geometry>
+            <material name="white">
+                <color rgba="0.7 0.7 0.7 1" />
+            </material>
+        </visual>
+        <collision>
+            <origin xyz="0.005 0 0.22" rpy="1.5708 0 0" />
+            <geometry>
+                <cylinder radius="0.073" length="0.064" />
+            </geometry>
+        </collision>
+        <collision>
+            <origin xyz="0.005 0 0.13" rpy="1.5708 0 0" />
+            <geometry>
+                <cylinder radius="0.07" length="0.056" />
+            </geometry>
+        </collision>
+        <collision>
+            <origin xyz="0.005 0 0.06" rpy="1.5708 0 0" />
+            <geometry>
+                <cylinder radius="0.065" length="0.04" />
+            </geometry>
+        </collision>
+        <collision>
+            <origin xyz="0.01 0 0.415" rpy="3.1416 0 0" />
+            <geometry>
+                <cylinder radius="0.068" length="0.01" />
+            </geometry>
+        </collision>
+    </link>
+    <joint name="waist_pitch_joint" type="revolute">
+        <origin xyz="0 0 0" 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.044" 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="package://robot_description/meshes/g1/logo_link.STL" />
+            </geometry>
+            <material name="dark">
+                <color rgba="0.2 0.2 0.2 1" />
+            </material>
+        </visual>
+    </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="package://robot_description/meshes/g1/head_link.STL" />
+            </geometry>
+            <material name="dark">
+                <color rgba="0.2 0.2 0.2 1" />
+            </material>
+        </visual>
+    </link>
+    <joint name="head_joint" type="fixed">
+        <origin xyz="0.0039635 0 -0.044" rpy="0 0 0" />
+        <parent link="torso_link" />
+        <child link="head_link" />
+    </joint>
+
+
+    <!-- IMU -->
+    <link name="imu_in_torso"></link>
+    <joint name="imu_in_torso_joint" type="fixed">
+        <origin xyz="-0.03959 -0.00224 0.14792" rpy="0 0 0" />
+        <parent link="torso_link" />
+        <child link="imu_in_torso" />
+    </joint>
+
+    <link name="imu_in_pelvis"></link>
+    <joint name="imu_in_pelvis_joint" type="fixed">
+        <origin xyz="0.04525 0 -0.08339" rpy="0 0 0" />
+        <parent link="pelvis" />
+        <child link="imu_in_pelvis" />
+    </joint>
+
+    <!-- mid360 -->
+    <link name="mid360_link"></link>
+    <joint name="mid360_joint" type="fixed">
+        <origin xyz="0.0002835 0.00003 0.41618" rpy="0 3.101 3.1415" />
+        <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="package://robot_description/meshes/g1/left_shoulder_pitch_link.STL" />
+            </geometry>
+            <material name="white">
+                <color rgba="0.7 0.7 0.7 1" />
+            </material>
+        </visual>
+    </link>
+    <joint name="left_shoulder_pitch_joint" type="revolute">
+        <origin xyz="0.0039563 0.10022 0.24778" 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="package://robot_description/meshes/g1/left_shoulder_roll_link.STL" />
+            </geometry>
+            <material name="white">
+                <color rgba="0.7 0.7 0.7 1" />
+            </material>
+        </visual>
+    </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="package://robot_description/meshes/g1/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.015" rpy="3.1416 0 0" />
+            <geometry>
+                <cylinder radius="0.035" length="0.13" />
+            </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="package://robot_description/meshes/g1/left_elbow_link.STL" />
+            </geometry>
+            <material name="white">
+                <color rgba="0.7 0.7 0.7 1" />
+            </material>
+        </visual>
+        <collision>
+            <origin xyz="0.055 0 -0.01" rpy="0 -1.5708 0" />
+            <geometry>
+                <cylinder radius="0.035" length="0.13" />
+            </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="package://robot_description/meshes/g1/left_wrist_roll_link.STL" />
+            </geometry>
+            <material name="black">
+                <color rgba="0.05 0.05 0.05 1" />
+            </material>
+        </visual>
+    </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="package://robot_description/meshes/g1/left_wrist_pitch_link.STL" />
+            </geometry>
+            <material name="black">
+                <color rgba="0.05 0.05 0.05 1" />
+            </material>
+        </visual>
+    </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="package://robot_description/meshes/g1/left_wrist_yaw_link.STL" />
+            </geometry>
+            <material name="black">
+                <color rgba="0.05 0.05 0.05 1" />
+            </material>
+        </visual>
+        <collision>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://robot_description/meshes/g1/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>
+        <collision>
+            <geometry>
+                <mesh filename="package://robot_description/meshes/g1/left_hand_palm_link.STL" />
+            </geometry>
+        </collision>
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://robot_description/meshes/g1/left_hand_palm_link.STL" />
+            </geometry>
+            <material name="black">
+                <color rgba="0.05 0.05 0.05 1" />
+            </material>
+        </visual>
+
+    </link>
+    <joint name="left_hand_thumb_0_joint" type="fixed">
+        <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="package://robot_description/meshes/g1/left_hand_thumb_0_link.STL" />
+            </geometry>
+            <material name="black">
+                <color rgba="0.05 0.05 0.05 1" />
+            </material>
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://robot_description/meshes/g1/left_hand_thumb_0_link.STL" />
+            </geometry>
+        </collision>
+
+    </link>
+    <joint name="left_hand_thumb_1_joint" type="fixed">
+        <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="package://robot_description/meshes/g1/left_hand_thumb_1_link.STL" />
+            </geometry>
+            <material name="black">
+                <color rgba="0.05 0.05 0.05 1" />
+            </material>
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://robot_description/meshes/g1/left_hand_thumb_1_link.STL" />
+            </geometry>
+        </collision>
+    </link>
+    <joint name="left_hand_thumb_2_joint" type="fixed">
+        <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="package://robot_description/meshes/g1/left_hand_thumb_2_link.STL" />
+            </geometry>
+            <material name="black">
+                <color rgba="0.05 0.05 0.05 1" />
+            </material>
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://robot_description/meshes/g1/left_hand_thumb_2_link.STL" />
+            </geometry>
+        </collision>
+
+    </link>
+    <joint name="left_hand_middle_0_joint" type="fixed">
+        <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="package://robot_description/meshes/g1/left_hand_middle_0_link.STL" />
+            </geometry>
+            <material name="black">
+                <color rgba="0.05 0.05 0.05 1" />
+            </material>
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://robot_description/meshes/g1/left_hand_middle_0_link.STL" />
+            </geometry>
+        </collision>
+
+    </link>
+    <joint name="left_hand_middle_1_joint" type="fixed">
+        <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="package://robot_description/meshes/g1/left_hand_middle_1_link.STL" />
+            </geometry>
+            <material name="black">
+                <color rgba="0.05 0.05 0.05 1" />
+            </material>
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://robot_description/meshes/g1/left_hand_middle_1_link.STL" />
+            </geometry>
+        </collision>
+    </link>
+    <joint name="left_hand_index_0_joint" type="fixed">
+        <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="package://robot_description/meshes/g1/left_hand_index_0_link.STL" />
+            </geometry>
+            <material name="black">
+                <color rgba="0.05 0.05 0.05 1" />
+            </material>
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://robot_description/meshes/g1/left_hand_index_0_link.STL" />
+            </geometry>
+        </collision>
+    </link>
+    <joint name="left_hand_index_1_joint" type="fixed">
+        <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="package://robot_description/meshes/g1/left_hand_index_1_link.STL" />
+            </geometry>
+            <material name="black">
+                <color rgba="0.05 0.05 0.05 1" />
+            </material>
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://robot_description/meshes/g1/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="package://robot_description/meshes/g1/right_shoulder_pitch_link.STL" />
+            </geometry>
+            <material name="white">
+                <color rgba="0.7 0.7 0.7 1" />
+            </material>
+        </visual>
+    </link>
+    <joint name="right_shoulder_pitch_joint" type="revolute">
+        <origin xyz="0.0039563 -0.10021 0.24778" 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="package://robot_description/meshes/g1/right_shoulder_roll_link.STL" />
+            </geometry>
+            <material name="white">
+                <color rgba="0.7 0.7 0.7 1" />
+            </material>
+        </visual>
+    </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="package://robot_description/meshes/g1/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.015" rpy="3.1416 0 0" />
+            <geometry>
+                <cylinder radius="0.035" length="0.13" />
+            </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="package://robot_description/meshes/g1/right_elbow_link.STL" />
+            </geometry>
+            <material name="white">
+                <color rgba="0.7 0.7 0.7 1" />
+            </material>
+        </visual>
+        <collision>
+            <origin xyz="0.055 0 -0.01" rpy="0 -1.5708 0" />
+            <geometry>
+                <cylinder radius="0.035" length="0.13" />
+            </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="package://robot_description/meshes/g1/right_wrist_roll_link.STL" />
+            </geometry>
+            <material name="black">
+                <color rgba="0.05 0.05 0.05 1" />
+            </material>
+        </visual>
+    </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="package://robot_description/meshes/g1/right_wrist_pitch_link.STL" />
+            </geometry>
+            <material name="black">
+                <color rgba="0.05 0.05 0.05 1" />
+            </material>
+        </visual>
+    </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="package://robot_description/meshes/g1/right_wrist_yaw_link.STL" />
+            </geometry>
+            <material name="black">
+                <color rgba="0.05 0.05 0.05 1" />
+            </material>
+        </visual>
+        <collision>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://robot_description/meshes/g1/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="package://robot_description/meshes/g1/right_hand_palm_link.STL" />
+            </geometry>
+            <material name="black">
+                <color rgba="0.05 0.05 0.05 1" />
+            </material>
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://robot_description/meshes/g1/right_hand_palm_link.STL" />
+            </geometry>
+        </collision>
+
+    </link>
+    <joint name="right_hand_thumb_0_joint" type="fixed">
+        <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="package://robot_description/meshes/g1/right_hand_thumb_0_link.STL" />
+            </geometry>
+            <material name="black">
+                <color rgba="0.05 0.05 0.05 1" />
+            </material>
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://robot_description/meshes/g1/right_hand_thumb_0_link.STL" />
+            </geometry>
+        </collision>
+    </link>
+    <joint name="right_hand_thumb_1_joint" type="fixed">
+        <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="package://robot_description/meshes/g1/right_hand_thumb_1_link.STL" />
+            </geometry>
+            <material name="black">
+                <color rgba="0.05 0.05 0.05 1" />
+            </material>
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://robot_description/meshes/g1/right_hand_thumb_1_link.STL" />
+            </geometry>
+        </collision>
+    </link>
+    <joint name="right_hand_thumb_2_joint" type="fixed">
+        <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="package://robot_description/meshes/g1/right_hand_thumb_2_link.STL" />
+            </geometry>
+            <material name="black">
+                <color rgba="0.05 0.05 0.05 1" />
+            </material>
+        </visual>
+        <collision>
+            <geometry>
+                <mesh
+                    filename="package://robot_description/meshes/g1/right_hand_thumb_2_link.STL" />
+            </geometry>
+        </collision>
+    </link>
+    <joint name="right_hand_middle_0_joint" type="fixed">
+        <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="package://robot_description/meshes/g1/right_hand_middle_0_link.STL" />
+            </geometry>
+            <material name="black">
+                <color rgba="0.05 0.05 0.05 1" />
+            </material>
+        </visual>
+        <collision>
+            <geometry>
+                <mesh
+                    filename="package://robot_description/meshes/g1/right_hand_middle_0_link.STL" />
+            </geometry>
+        </collision>
+    </link>
+    <joint name="right_hand_middle_1_joint" type="fixed">
+        <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="package://robot_description/meshes/g1/right_hand_middle_1_link.STL" />
+            </geometry>
+            <material name="black">
+                <color rgba="0.05 0.05 0.05 1" />
+            </material>
+        </visual>
+        <collision>
+            <geometry>
+                <mesh
+                    filename="package://robot_description/meshes/g1/right_hand_middle_1_link.STL" />
+            </geometry>
+        </collision>
+
+    </link>
+    <joint name="right_hand_index_0_joint" type="fixed">
+        <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="package://robot_description/meshes/g1/right_hand_index_0_link.STL" />
+            </geometry>
+            <material name="black">
+                <color rgba="0.05 0.05 0.05 1" />
+            </material>
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://robot_description/meshes/g1/right_hand_index_0_link.STL" />
+            </geometry>
+        </collision>
+    </link>
+    <joint name="right_hand_index_1_joint" type="fixed">
+        <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="package://robot_description/meshes/g1/right_hand_index_1_link.STL" />
+            </geometry>
+            <material name="black">
+                <color rgba="0.05 0.05 0.05 1" />
+            </material>
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="package://robot_description/meshes/g1/right_hand_index_1_link.STL" />
+            </geometry>
+        </collision>
+    </link>
+
+
+    <link name="LL_FOOT">
+        <inertial>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <mass value="1e-9" />
+            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
+        </inertial>
+    </link>
+    <joint name="LL_FOOT_frame" type="fixed">
+        <origin xyz="0.04 0 -0.037" rpy="0 0 0" />
+        <parent link="left_ankle_roll_link" />
+        <child link="LL_FOOT" />
+    </joint>
+
+    <link name="LR_FOOT">
+        <inertial>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <mass value="1e-9" />
+            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
+        </inertial>
+    </link>
+    <joint name="LR_FOOT_frame" type="fixed">
+        <origin xyz="0.04 0 -0.037" rpy="0 0 0" />
+        <parent link="right_ankle_roll_link" />
+        <child link="LR_FOOT" />
+    </joint>
+</robot>

GR00T-WholeBodyControl/gear_sonic/data/assets/robot_description/urdf/h2/h2.urdf

@@ -0,0 +1,885 @@
+<robot name="H1031">
+  <material name="dark">
+    <color rgba="0.4 0.4 0.4 1"/>
+  </material>
+  <material name="white">
+    <color rgba="0.7 0.7 0.7 1"/>
+  </material>
+
+  <mujoco>
+    <compiler meshdir="meshes" discardvisual="false"/>
+  </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.00046244 -0.00004214 -0.01473435" rpy="0 0 0"/>
+      <mass value="7.60468095"/>
+      <inertia ixx="0.03548205" ixy="-0.00060277" ixz="0.00014978" iyy="0.02290677" iyz="-0.00000038" izz="0.02753519"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/pelvis.stl"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/pelvis_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="left_hip_pitch_link">
+    <inertial>
+      <origin xyz="0.00300595 0.05931469 -0.01709150" rpy="0 0 0"/>
+      <mass value="2.79752530"/>
+      <inertia ixx="0.00509731" ixy="-0.00008393" ixz="0.00004539" iyy="0.00371022" iyz="0.00030980" izz="0.00385560"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_hip_pitch_link.stl"/>
+      </geometry>
+      <material name="white"/>
+    </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.083873 -0.048424" rpy="-0.5236 0 0"/>
+    <parent link="pelvis"/>
+    <child link="left_hip_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-2.4525975" upper="2.7754225" effort="360" velocity="20"/>
+  </joint>
+  <link name="left_hip_roll_link">
+    <inertial>
+      <origin xyz="-0.03026193 -0.00468930 -0.04449852" rpy="0 0 0"/>
+      <mass value="0.62714607"/>
+      <inertia ixx="0.00130633" ixy="-0.00000552" ixz="-0.00013133" iyy="0.00168791" iyz="-0.00006773" izz="0.00196361"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_hip_roll_link.stl"/>
+      </geometry>
+      <material name="white"/>
+    </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.041851 0.0635 -0.019" rpy="0.5236 0 0"/>
+    <parent link="left_hip_pitch_link"/>
+    <child link="left_hip_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-0.467441" upper="2.1688605" effort="360" velocity="20"/>
+  </joint>
+  <link name="left_hip_yaw_link">
+    <inertial>
+      <origin xyz="0.00069408 -0.01086587 -0.11301456" rpy="0 0 0"/>
+      <mass value="7.08752046"/>
+      <inertia ixx="0.05057956" ixy="-0.00005547" ixz="0.00146675" iyy="0.05110490" iyz="-0.00312202" izz="0.01331946"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_hip_yaw_link.stl"/>
+      </geometry>
+      <material name="white"/>
+    </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.028187 -0.0095 -0.073" rpy="0 0 0"/>
+    <parent link="left_hip_roll_link"/>
+    <child link="left_hip_yaw_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-2.827" upper="2.827" effort="360" velocity="20"/>
+  </joint>
+  <link name="left_knee_link">
+    <inertial>
+      <origin xyz="0.00461331 -0.02453217 -0.16715459" rpy="0 0 0"/>
+      <mass value="3.55340244"/>
+      <inertia ixx="0.05108514" ixy="-0.00000720" ixz="-0.00041631" iyy="0.05123842" iyz="0.00001339" izz="0.00334361"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_knee_link.stl"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+    <collision>
+      <origin xyz="0.01 -0.025 -0.25" rpy="0 0 0"/>
+      <geometry>
+        <cylinder radius="0.04" length="0.35"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_knee_joint" type="revolute">
+    <origin xyz="-0.010341 0.018489 -0.3191" rpy="0 0 0"/>
+    <parent link="left_hip_yaw_link"/>
+    <child link="left_knee_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.08725" upper="2.53025" effort="360" velocity="20"/>
+  </joint>
+  <link name="left_ankle_roll_link">
+    <inertial>
+      <origin xyz="-0.01740632 0.00006424 -0.00021460" rpy="0 0 0"/>
+      <mass value="0.12318157"/>
+      <inertia ixx="0.00003432" ixy="0.00000012" ixz="0.00000002" iyy="0.00001450" iyz="-0.00000002" izz="0.00004005"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_ankle_roll_link.stl"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+  </link>
+  <joint name="left_ankle_roll_joint" type="revolute">
+    <origin xyz="0.03272 -0.023002 -0.49662" rpy="0 0 0"/>
+    <parent link="left_knee_link"/>
+    <child link="left_ankle_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-0.3490658503" upper="0.296705972783" effort="19" velocity="100.7"/>
+  </joint>
+  <link name="left_ankle_pitch_link">
+    <inertial>
+      <origin xyz="0.03205900 0.03700531 -0.02882818" rpy="0 0 0"/>
+      <mass value="0.84496747"/>
+      <inertia ixx="0.00060825" ixy="0.00000046" ixz="0.00017671" iyy="0.00410964" iyz="-0.00000001" izz="0.00437490"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_ankle_pitch_link.stl"/>
+      </geometry>
+      <material name="dark"/>
+    </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.0225 -0.0373 0" rpy="0 0 0"/>
+    <parent link="left_ankle_roll_link"/>
+    <child link="left_ankle_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-1.13446401358" upper="0.61086523808" effort="66.88" velocity="28.61"/>
+  </joint>
+  <link name="right_hip_pitch_link">
+    <inertial>
+      <origin xyz="0.00300595 -0.05931469 -0.01709150" rpy="0 0 0"/>
+      <mass value="2.79752530"/>
+      <inertia ixx="0.00509731" ixy="0.00008393" ixz="0.00004539" iyy="0.00371022" iyz="-0.00030980" izz="0.00385560"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_hip_pitch_link.stl"/>
+      </geometry>
+      <material name="white"/>
+    </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.083873 -0.048424" rpy="0.5236 0 0"/>
+    <parent link="pelvis"/>
+    <child link="right_hip_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-2.4525975" upper="2.7754225" effort="360" velocity="20"/>
+  </joint>
+  <link name="right_hip_roll_link">
+    <inertial>
+      <origin xyz="-0.03026193 0.00468930 -0.04449852" rpy="0 0 0"/>
+      <mass value="0.62714607"/>
+      <inertia ixx="0.00130633" ixy="0.00000552" ixz="-0.00013133" iyy="0.00168791" iyz="0.00006773" izz="0.00196361"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_hip_roll_link.stl"/>
+      </geometry>
+      <material name="white"/>
+    </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.041851 -0.0635 -0.019" rpy="-0.5236 0 0"/>
+    <parent link="right_hip_pitch_link"/>
+    <child link="right_hip_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-2.1688605" upper="0.467441" effort="360" velocity="20"/>
+  </joint>
+  <link name="right_hip_yaw_link">
+    <inertial>
+      <origin xyz="0.00069408 0.01086587 -0.11301456" rpy="0 0 0"/>
+      <mass value="7.08752046"/>
+      <inertia ixx="0.05057956" ixy="0.00005547" ixz="0.00146675" iyy="0.05110490" iyz="0.00312202" izz="0.01331946"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_hip_yaw_link.stl"/>
+      </geometry>
+      <material name="white"/>
+    </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.028187 0.0095 -0.073" rpy="0 0 0"/>
+    <parent link="right_hip_roll_link"/>
+    <child link="right_hip_yaw_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-2.827" upper="2.827" effort="360" velocity="20"/>
+  </joint>
+  <link name="right_knee_link">
+    <inertial>
+      <origin xyz="0.00461331 0.02453217 -0.16715459" rpy="0 0 0"/>
+      <mass value="3.55340244"/>
+      <inertia ixx="0.05108514" ixy="0.00000720" ixz="-0.00041631" iyy="0.05123842" iyz="-0.00001339" izz="0.00334361"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_knee_link.stl"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+    <collision>
+      <origin xyz="0.01 0.025 -0.25" rpy="0 0 0"/>
+      <geometry>
+        <cylinder radius="0.04" length="0.35"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_knee_joint" type="revolute">
+    <origin xyz="-0.010341 -0.018489 -0.3191" rpy="0 0 0"/>
+    <parent link="right_hip_yaw_link"/>
+    <child link="right_knee_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.08725" upper="2.53025" effort="360" velocity="20"/>
+  </joint>
+  <link name="right_ankle_roll_link">
+    <inertial>
+      <origin xyz="-0.01740632 -0.00006424 -0.00021460" rpy="0 0 0"/>
+      <mass value="0.12318157"/>
+      <inertia ixx="0.00003432" ixy="-0.00000012" ixz="0.00000002" iyy="0.00001450" iyz="0.00000002" izz="0.00004005"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_ankle_roll_link.stl"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+  </link>
+  <joint name="right_ankle_roll_joint" type="revolute">
+    <origin xyz="0.03272 0.023002 -0.49662" rpy="0 0 0"/>
+    <parent link="right_knee_link"/>
+    <child link="right_ankle_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-0.296705972783" upper="0.3490658503" effort="19" velocity="100.7"/>
+  </joint>
+  <link name="right_ankle_pitch_link">
+    <inertial>
+      <origin xyz="0.03205900 -0.03700531 -0.02882818" rpy="0 0 0"/>
+      <mass value="0.84496747"/>
+      <inertia ixx="0.00060825" ixy="-0.00000046" ixz="0.00017671" iyy="0.00410964" iyz="0.00000001" izz="0.00437490"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_ankle_pitch_link.stl"/>
+      </geometry>
+      <material name="dark"/>
+    </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.0225 0.0373 0" rpy="0 0 0"/>
+    <parent link="right_ankle_roll_link"/>
+    <child link="right_ankle_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-1.13446401358" upper="0.61086523808" effort="66.88" velocity="28.61"/>
+  </joint>
+  <link name="waist_yaw_link">
+    <inertial>
+      <origin xyz="0.00626994 0 0.01696958" rpy="0 0 0"/>
+      <mass value="0.27976220"/>
+      <inertia ixx="0.00012352" ixy="0.00000000" ixz="0.00000569" iyy="0.00024730" iyz="0.00000000" izz="0.00031125"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/waist_yaw_link.stl"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+  </link>
+  <joint name="waist_yaw_joint" type="revolute">
+    <origin xyz="0 0 0.089837" rpy="0 0 0"/>
+    <parent link="pelvis"/>
+    <child link="waist_yaw_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-1.7453" upper="1.7453" effort="120" velocity="28.375"/>
+  </joint>
+  <link name="waist_roll_link">
+    <inertial>
+      <origin xyz="0.00000000 0.00128474 0.00000000" rpy="0 0 0"/>
+      <mass value="0.06585040"/>
+      <inertia ixx="0.00000809" ixy="0.00000000" ixz="0.00000000" iyy="0.00001041" iyz="0.00000000" izz="0.00001580"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/waist_roll_link.stl"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+  </link>
+  <joint name="waist_roll_joint" type="revolute">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="waist_yaw_link"/>
+    <child link="waist_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-0.5236" upper="0.5236" effort="180" velocity="28.375"/>
+  </joint>
+  <link name="torso_link">
+    <inertial>
+      <origin xyz="0.01342470 0.00547190 0.21050133" rpy="0 0 0"/>
+      <mass value="15.87026294"/>
+      <inertia ixx="0.27305029" ixy="0.00013871" ixz="-0.00147103" iyy="0.24363561" iyz="-0.01093518" izz="0.08484661"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/torso_link.stl"/>
+      </geometry>
+      <material name="dark"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/torso_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="waist_pitch_joint" type="revolute">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="waist_roll_link"/>
+    <child link="torso_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.43633" upper="0.5236" effort="180" velocity="28.375"/>
+  </joint>
+
+  <!-- head -->
+
+  <link name="head_pitch_link">
+    <inertial>
+      <origin xyz="5.279E-08 -0.00035318 0.045125" rpy="0 0 0"/>
+      <mass value="1.82"/>
+      <inertia ixx="0.005386858" ixy="-0.000000033" ixz="0.000020867" iyy="0.005324406" iyz="-0.000007491" izz="0.001409942"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/head_pitch_link.stl"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+    <!-- <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/head_pitch_link.stl"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="head_pitch_joint" type="revolute">
+    <origin xyz="-0.0002283 2.7112E-05 0.42181" rpy="0 0 0"/>
+    <parent link="torso_link"/>
+    <child link="head_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.5236" upper="0.83775" effort="50" velocity="10"/>
+  </joint>
+
+  <link name="head_yaw_link">
+    <inertial>
+      <origin xyz="0.0219 0 0.0187" rpy="0 0 0"/>
+      <mass value="1.3"/>
+      <inertia ixx="0.004367405" ixy="-0.00000739" ixz="0.000456883" iyy="0.005352667" iyz="0.000007463" izz="0.004136928"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/head_yaw_link.stl"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/head_yaw_link.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="head_yaw_joint" type="revolute">
+    <origin xyz="0 0.0003 0.1325" rpy="0 0 0"/>
+    <parent link="head_pitch_link"/>
+    <child link="head_yaw_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-1.7453" upper="1.7453" effort="50" velocity="10"/>
+  </joint>
+
+  <!-- arm -->
+
+  <link name="left_shoulder_pitch_link">
+    <inertial>
+      <origin xyz="0.00296153 0.05202683 -0.02504502" rpy="0 0 0"/>
+      <mass value="1.06055553"/>
+      <inertia ixx="0.00121801" ixy="-0.000027444" ixz="0.00004648" iyy="0.00090494" iyz="0.00029813" izz="0.00066362"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_shoulder_pitch_link.stl"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_shoulder_pitch_link.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_shoulder_pitch_joint" type="revolute">
+    <origin xyz="0.0015 0.11254 0.35812" rpy="0.26914 0 0"/>
+    <parent link="torso_link"/>
+    <child link="left_shoulder_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-2.6179939" upper="1.8325957" effort="120" velocity="28.375"/>
+  </joint>
+  <link name="left_shoulder_roll_link">
+    <inertial>
+      <origin xyz="-0.02061003 0.00415443 -0.06294221" rpy="0 0 0"/>
+      <mass value="1.34511054"/>
+      <inertia ixx="0.00191998" ixy="0.00000294" ixz="-0.00023793" iyy="0.00210021" iyz="0.00004035" izz="0.00112386"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_shoulder_roll_link.stl"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_shoulder_roll_link.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_shoulder_roll_joint" type="revolute">
+    <origin xyz="0.027682 0.061638 -0.039909" rpy="-0.26914 0 0"/>
+    <parent link="left_shoulder_pitch_link"/>
+    <child link="left_shoulder_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-0.5166175" upper="2.6354472" effort="54" velocity="34.3"/>
+  </joint>
+  <link name="left_shoulder_yaw_link">
+    <inertial>
+      <origin xyz="0.00364187 0.01865147 -0.05585548" rpy="0 0 0"/>
+      <mass value="0.91571732"/>
+      <inertia ixx="0.00304064" ixy="0.00001983" ixz="0.00018692" iyy="0.00254925" iyz="-0.00016348" izz="0.00147669"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_shoulder_yaw_link.stl"/>
+      </geometry>
+      <material name="white"/>
+    </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.030632 0.0037809 -0.10422" rpy="0 0 0"/>
+    <parent link="left_shoulder_roll_link"/>
+    <child link="left_shoulder_yaw_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-2.6179939" upper="2.6179939" effort="54" velocity="34.3"/>
+  </joint>
+  <link name="left_elbow_link">
+    <inertial>
+      <origin xyz="0.04599687 -0.02832961 -0.00661325" rpy="0 0 0"/>
+      <mass value="1.69431869"/>
+      <inertia ixx="0.00071506" ixy="0.00027494" ixz="0.00013832" iyy="0.00547116" iyz="-0.00001668" izz="0.00533470"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_elbow_link.stl"/>
+      </geometry>
+      <material name="white"/>
+    </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.016955 0.031006 -0.12719" rpy="0 0 0"/>
+    <parent link="left_shoulder_yaw_link"/>
+    <child link="left_elbow_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.986111" upper="3.0717795" effort="54" velocity="34.3"/>
+  </joint>
+  <link name="left_wrist_roll_link">
+    <inertial>
+      <origin xyz="0.03371947 0.00237482 0.00031544" rpy="0 0 0"/>
+      <mass value="0.96983674"/>
+      <inertia ixx="0.00087762" ixy="-0.00002554" ixz="-0.00000226" iyy="0.00087920" iyz="0.00000022" izz="0.00092684"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_wrist_roll_link.stl"/>
+      </geometry>
+      <material name="white"/>
+    </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_roll_joint" type="revolute">
+    <origin xyz="0.12826 -0.030815 -0.013299" rpy="0 0 0"/>
+    <parent link="left_elbow_link"/>
+    <child link="left_wrist_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-2.6179939" upper="2.6179939" effort="54" velocity="34.3"/>
+  </joint>
+  <link name="left_wrist_pitch_link">
+    <inertial>
+      <origin xyz="0.01820997 0.03140354 0.01156873" rpy="0 0 0"/>
+      <mass value="0.94695328"/>
+      <inertia ixx="0.00087455" ixy="0.00001140" ixz="0.00009847" iyy="0.00082105" iyz="-0.00000641" izz="0.00069205"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_wrist_pitch_link.stl"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+  </link>
+  <joint name="left_wrist_pitch_joint" type="revolute">
+    <origin xyz="0.082003 -0.031003 0.00020786" rpy="0 0 0"/>
+    <parent link="left_wrist_roll_link"/>
+    <child link="left_wrist_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.4363323" upper="0.4363323" effort="25" velocity="50.125"/>
+  </joint>
+  <link name="left_wrist_yaw_link">
+    <inertial>
+      <origin xyz="-0.01011731 0.00400457 -0.01730713" rpy="0 0 0"/>
+      <mass value="0.60575230"/>
+      <inertia ixx="0.00024292" ixy="0.00002474" ixz="0.00003864" iyy="0.00076758" iyz="-0.00003303" izz="0.00067790"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 -0.029" rpy="0 0 0"/>
+      <geometry>
+        <cylinder radius="0.028" length="0.06"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 -0.029" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.04"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_wrist_yaw_joint" type="revolute">
+    <origin xyz="0.026025 0.030996 0.026981" rpy="0 0 0"/>
+    <parent link="left_wrist_pitch_link"/>
+    <child link="left_wrist_yaw_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-1.2217305" upper="1.2217305" effort="25" velocity="50.125"/>
+  </joint>
+  <joint name="left_hand_palm_joint" type="fixed">
+    <origin xyz="0.025 0 -0.03" 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="dark"/>
+    </visual>
+  </link>
+  <link name="right_shoulder_pitch_link">
+    <inertial>
+      <origin xyz="0.00296153 -0.05202683 -0.02504502" rpy="0 0 0"/>
+      <mass value="1.06055553"/>
+      <inertia ixx="0.00121801" ixy="0.00002744" ixz="0.00004648" iyy="0.00090494" iyz="-0.00029813" izz="0.00066362"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_shoulder_pitch_link.stl"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_shoulder_pitch_link.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_shoulder_pitch_joint" type="revolute">
+    <origin xyz="0.0015 -0.1125 0.35812" rpy="-0.26914 0 0"/>
+    <parent link="torso_link"/>
+    <child link="right_shoulder_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-2.6179939" upper="1.8325957" effort="120" velocity="28.375"/>
+  </joint>
+  <link name="right_shoulder_roll_link">
+    <inertial>
+      <origin xyz="-0.02061003 -0.00415443 -0.06294221" rpy="0 0 0"/>
+      <mass value="1.34511054"/>
+      <inertia ixx="0.00191998" ixy="-0.00000294" ixz="-0.00023793" iyy="0.00210021" iyz="-0.00004035" izz="0.00112386"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_shoulder_roll_link.stl"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_shoulder_roll_link.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_shoulder_roll_joint" type="revolute">
+    <origin xyz="0.027682 -0.061638 -0.039909" rpy="0.26914 0 0"/>
+    <parent link="right_shoulder_pitch_link"/>
+    <child link="right_shoulder_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-2.6354472" upper="0.5166175" effort="54" velocity="34.3"/>
+  </joint>
+  <link name="right_shoulder_yaw_link">
+    <inertial>
+      <origin xyz="0.00364187 -0.01865147 -0.05585548" rpy="0 0 0"/>
+      <mass value="0.91571732"/>
+      <inertia ixx="0.00304064" ixy="-0.00001983" ixz="0.00018692" iyy="0.00254925" iyz="0.00016348" izz="0.00147669"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_shoulder_yaw_link.stl"/>
+      </geometry>
+      <material name="white"/>
+    </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.030632 -0.0037809 -0.10422" rpy="0 0 0"/>
+    <parent link="right_shoulder_roll_link"/>
+    <child link="right_shoulder_yaw_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-2.6179939" upper="2.6179939" effort="54" velocity="34.3"/>
+  </joint>
+  <link name="right_elbow_link">
+    <inertial>
+      <origin xyz="0.04599687 0.02832961 -0.00661325" rpy="0 0 0"/>
+      <mass value="1.69431869"/>
+      <inertia ixx="0.00071506" ixy="-0.00027494" ixz="0.00013832" iyy="0.00547116" iyz="0.00001668" izz="0.00533470"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_elbow_link.stl"/>
+      </geometry>
+      <material name="white"/>
+    </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.016955 -0.031006 -0.12719" rpy="0 0 0"/>
+    <parent link="right_shoulder_yaw_link"/>
+    <child link="right_elbow_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.986111" upper="3.0717795" effort="54" velocity="34.3"/>
+  </joint>
+  <link name="right_wrist_roll_link">
+    <inertial>
+      <origin xyz="0.03371947 -0.00237482 0.00031544" rpy="0 0 0"/>
+      <mass value="0.96983674"/>
+      <inertia ixx="0.00087762" ixy="0.00002554" ixz="-0.00000226" iyy="0.00087920" iyz="-0.00000022" izz="0.00092684"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_wrist_roll_link.stl"/>
+      </geometry>
+      <material name="white"/>
+    </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_roll_joint" type="revolute">
+    <origin xyz="0.12826 0.030815 -0.013299" rpy="0 0 0"/>
+    <parent link="right_elbow_link"/>
+    <child link="right_wrist_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-2.6179939" upper="2.6179939" effort="54" velocity="34.3"/>
+  </joint>
+  <link name="right_wrist_pitch_link">
+    <inertial>
+      <origin xyz="0.01820997 -0.03140354 0.01156873" rpy="0 0 0"/>
+      <mass value="0.94695328"/>
+      <inertia ixx="0.00087455" ixy="-0.00001140" ixz="0.00009847" iyy="0.00082105" iyz="0.00000641" izz="0.00069205"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_wrist_pitch_link.stl"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+  </link>
+  <joint name="right_wrist_pitch_joint" type="revolute">
+    <origin xyz="0.082003 0.031003 0.00020786" rpy="0 0 0"/>
+    <parent link="right_wrist_roll_link"/>
+    <child link="right_wrist_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.4363323" upper="0.4363323" effort="25" velocity="50.125"/>
+  </joint>
+  <link name="right_wrist_yaw_link">
+    <inertial>
+      <origin xyz="-0.01011731 -0.00400457 -0.01730713" rpy="0 0 0"/>
+      <mass value="0.60575230"/>
+      <inertia ixx="0.00024292" ixy="-0.00002474" ixz="0.00003864" iyy="0.00076758" iyz="0.00003303" izz="0.00067790"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 -0.029" rpy="0 0 0"/>
+      <geometry>
+        <cylinder radius="0.028" length="0.06"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 -0.029" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.04"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_wrist_yaw_joint" type="revolute">
+    <origin xyz="0.026025 -0.030996 0.026981" rpy="0 0 0"/>
+    <parent link="right_wrist_pitch_link"/>
+    <child link="right_wrist_yaw_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-1.2217305" upper="1.2217305" effort="25" velocity="50.125"/>
+  </joint>
+  <joint name="right_hand_palm_joint" type="fixed">
+    <origin xyz="0.025 0 -0.03" 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="dark"/>
+    </visual>
+  </link>
+</robot>

GR00T-WholeBodyControl/gear_sonic/data/robots/g1/meshes/old/g1_10dof_sausage.urdf

@@ -0,0 +1,1299 @@
+<robot name="g1">
+
+  <link name="pelvis">
+    <inertial>
+      <origin xyz="0 0 -0.07605" rpy="0 0 0"/>
+      <mass value="2.86"/>
+      <inertia ixx="0.0079143" ixy="0" ixz="1.6E-06" iyy="0.0069837" iyz="0" izz="0.0059404"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/pelvis.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/pelvis.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="pelvis_contour_link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/pelvis_contour_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="pelvis_contour_joint" type="fixed">
+    <parent link="pelvis"/>
+    <child link="pelvis_contour_link"/>
+  </joint>
+
+  <!-- Leg -->
+  <link name="left_hip_pitch_link">
+    <inertial>
+      <origin xyz="0.001962 0.049392 -0.000941" rpy="0 0 0"/>
+      <mass value="1.299"/>
+      <inertia ixx="0.0013873" ixy="-1.63E-05" ixz="-1E-06" iyy="0.0009059" iyz="-4.24E-05" izz="0.0009196"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_hip_pitch_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0.01 0.05 0" rpy="0 1.57 0"/>
+      <geometry>
+        <cylinder radius="0.03" length="0.05"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_hip_pitch_joint" type="revolute">
+    <origin xyz="0 0.06445 -0.1027" rpy="0 -0.34907 0"/>
+    <parent link="pelvis"/>
+    <child link="left_hip_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-2.35" upper="3.05" effort="88" velocity="32"/>
+  </joint>
+  <link name="left_hip_roll_link">
+    <inertial>
+      <origin xyz="0.024757 -0.001036 -0.086323" rpy="0 0 0"/>
+      <mass value="1.446"/>
+      <inertia ixx="0.0022702" ixy="-3.7E-06" ixz="-0.0003789" iyy="0.002304" iyz="-1.8E-05" izz="0.0016001"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_hip_roll_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0.02 0 -0.1" rpy="0 0. 0"/>
+      <geometry>
+        <cylinder radius="0.04" length="0.05"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_hip_roll_joint" type="revolute">
+    <origin xyz="0 0.0523 0" rpy="0 0 0"/>
+    <parent link="left_hip_pitch_link"/>
+    <child link="left_hip_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-0.26" upper="2.53" effort="88" velocity="32"/>
+  </joint>
+  <link name="left_hip_yaw_link">
+    <inertial>
+      <origin xyz="-0.053554 -0.011477 -0.14067" rpy="0 0 0"/>
+      <mass value="2.052"/>
+      <inertia ixx="0.0087264" ixy="-0.0004402" ixz="-0.0036676" iyy="0.011374" iyz="-0.0006654" izz="0.004279"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_hip_yaw_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="-0.05 0 -0.14" rpy="0 0.5 0"/>
+      <geometry>
+        <cylinder radius="0.03" length="0.12"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_hip_yaw_joint" type="revolute">
+    <origin xyz="0.01966 -0.0012139 -0.1241" rpy="0 0 0"/>
+    <parent link="left_hip_roll_link"/>
+    <child link="left_hip_yaw_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-2.75" upper="2.75" effort="88" velocity="32"/>
+  </joint>
+  <link name="left_knee_link">
+    <inertial>
+      <origin xyz="0.005505 0.006534 -0.116629" rpy="0 0 0"/>
+      <mass value="2.252"/>
+      <inertia ixx="0.012443837" ixy="0.000053496" ixz="-0.000437641" iyy="0.012674902" iyz="-0.000682499" izz="0.001986501"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_knee_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0.007 0.005 -0.15" rpy="0 0 0"/>
+      <geometry>
+        <cylinder radius="0.03" length="0.13"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_knee_joint" type="revolute">
+    <origin xyz="-0.078292 -0.0017335 -0.177225" rpy="0 0.5096 0"/>
+    <parent link="left_hip_yaw_link"/>
+    <child link="left_knee_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.33489" upper="2.5449" 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="../mesh/G1/left_ankle_pitch_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0.02" rpy="0 0 0"/>
+      <geometry>
+        <cylinder radius="0.01" length="0.01"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_ankle_pitch_joint" type="revolute">
+    <origin xyz="0 0.0040687 -0.30007" rpy="0 -0.16053 0"/>
+    <parent link="left_knee_link"/>
+    <child link="left_ankle_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.68" upper="0.73" effort="40" velocity="53"/>
+  </joint>
+  <link name="left_ankle_roll_link">
+    <inertial>
+      <origin xyz="0.024762 2E-05 -0.012526" rpy="0 0 0"/>
+      <mass value="0.391"/>
+      <inertia ixx="0.0001552" ixy="-1E-07" ixz="7.51E-05" iyy="0.0010657" iyz="1E-07" izz="0.001098"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_ankle_roll_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0.035 0 -0.015" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.19 0.04 0.03"/>
+      </geometry>
+    </collision>
+    <!-- <collision>
+      <origin xyz="-0.06 0.02 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.001"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin xyz="-0.06 -0.02 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.001"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin xyz="0.13 0.02 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.001"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin xyz="0.13 -0.02 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.001"/>
+      </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="40" velocity="53"/>
+  </joint>
+  <link name="right_hip_pitch_link">
+    <inertial>
+      <origin xyz="0.001962 -0.049392 -0.000941" rpy="0 0 0"/>
+      <mass value="1.299"/>
+      <inertia ixx="0.0013873" ixy="1.63E-05" ixz="-1E-06" iyy="0.0009059" iyz="4.24E-05" izz="0.0009196"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_hip_pitch_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0.01 -0.05 0" rpy="0 1.57 0"/>
+      <geometry>
+        <cylinder radius="0.03" length="0.05"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_hip_pitch_joint" type="revolute">
+    <origin xyz="0 -0.06445 -0.1027" rpy="0 -0.34907 0"/>
+    <parent link="pelvis"/>
+    <child link="right_hip_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-2.35" upper="3.05" effort="88" velocity="32"/>
+  </joint>
+  <link name="right_hip_roll_link">
+    <inertial>
+      <origin xyz="0.024757 0.001036 -0.086323" rpy="0 0 0"/>
+      <mass value="1.446"/>
+      <inertia ixx="0.0022702" ixy="3.7E-06" ixz="-0.0003789" iyy="0.002304" iyz="1.8E-05" izz="0.0016001"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_hip_roll_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0.02 0 -0.1" rpy="0 0. 0"/>
+      <geometry>
+        <cylinder radius="0.04" length="0.05"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_hip_roll_joint" type="revolute">
+    <origin xyz="0 -0.0523 0" rpy="0 0 0"/>
+    <parent link="right_hip_pitch_link"/>
+    <child link="right_hip_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-2.53" upper="0.26" effort="88" velocity="32"/>
+  </joint>
+  <link name="right_hip_yaw_link">
+    <inertial>
+      <origin xyz="-0.053554 0.011477 -0.14067" rpy="0 0 0"/>
+      <mass value="2.052"/>
+      <inertia ixx="0.0087264" ixy="0.0004402" ixz="-0.0036676" iyy="0.011374" iyz="0.0006654" izz="0.004279"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_hip_yaw_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="-0.05 0 -0.14" rpy="0 0.5 0"/>
+      <geometry>
+        <cylinder radius="0.03" length="0.12"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_hip_yaw_joint" type="revolute">
+    <origin xyz="0.01966 0.0012139 -0.1241" rpy="0 0 0"/>
+    <parent link="right_hip_roll_link"/>
+    <child link="right_hip_yaw_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-2.75" upper="2.75" effort="88" velocity="32"/>
+  </joint>
+  <link name="right_knee_link">
+    <inertial>
+      <origin xyz="0.005505 -0.006534 -0.116629" rpy="0 0 0"/>
+      <mass value="2.252"/>
+      <inertia ixx="0.012443837" ixy="-0.000053496" ixz="-0.000437641" iyy="0.012674902" iyz="0.000682499" izz="0.001986501"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_knee_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0.007 -0.005 -0.15" rpy="0 0 0"/>
+      <geometry>
+        <cylinder radius="0.03" length="0.13"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_knee_joint" type="revolute">
+    <origin xyz="-0.078292 0.0017335 -0.177225" rpy="0 0.5096 0"/>
+    <parent link="right_hip_yaw_link"/>
+    <child link="right_knee_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.33489" upper="2.5449" 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="../mesh/G1/right_ankle_pitch_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0.02" rpy="0 0 0"/>
+      <geometry>
+        <cylinder radius="0.01" length="0.01"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_ankle_pitch_joint" type="revolute">
+    <origin xyz="0 -0.0040687 -0.30007" rpy="0 -0.16053 0"/>
+    <parent link="right_knee_link"/>
+    <child link="right_ankle_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.68" upper="0.73" effort="40" velocity="53"/>
+  </joint>
+  <link name="right_ankle_roll_link">
+    <inertial>
+      <origin xyz="0.024762 -2E-05 -0.012526" rpy="0 0 0"/>
+      <mass value="0.391"/>
+      <inertia ixx="0.0001552" ixy="1E-07" ixz="7.51E-05" iyy="0.0010657" iyz="-1E-07" izz="0.001098"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_ankle_roll_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0.035 0 -0.015" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.19 0.04 0.03"/>
+      </geometry>
+    </collision>
+      <!-- <geometry>
+        <mesh filename="../mesh/G1/right_ankle_roll_link.STL"/>
+      </geometry>
+    </collision> -->
+    <!-- <collision>
+      <origin xyz="-0.06 0.02 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.001"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin xyz="-0.06 -0.02 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.001"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin xyz="0.13 0.02 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.001"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin xyz="0.13 -0.02 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.001"/>
+      </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="40" velocity="53"/>
+  </joint>
+
+  <!-- Torso -->
+  <link name="torso_link">
+    <inertial>
+      <origin xyz="0.00197122283 0.00266902037 0.16936510960" rpy="0 0 0"/>
+      <mass value="6.33959811"/>
+      <inertia ixx="0.06037082109027" ixy="-0.00010168407055" ixz="0.00069845018092" iyy="0.04355153796551" iyz="0.00034366422728" izz="0.03339981866010"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/torso_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/torso_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="torso_joint" type="fixed">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="pelvis"/>
+    <child link="torso_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-2.618" upper="2.618" effort="88" velocity="32"/>
+  </joint>
+
+  <!-- Head -->
+  <link name="head_link">
+    <inertial>
+      <origin xyz="0.00138066852 0.00028430950 0.42034187824" rpy="0 0 0"/>
+      <mass value="1.17976522"/>
+      <inertia ixx="0.00543236042361" ixy="0.00000140137425" ixz="0.00034554752228" iyy="0.00552885306699" iyz="0.00001501216392" izz="0.00165378108136"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/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="../mesh/G1/head_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="head_joint" type="fixed">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="torso_link"/>
+    <child link="head_link"/>
+  </joint>
+
+  <!-- L ARM -->
+  <link name="left_shoulder_pitch_link">
+    <inertial>
+      <origin xyz="-0.001431 0.048811 0.001304" rpy="0 0 0"/>
+      <mass value="0.713"/>
+      <inertia ixx="0.0004614" ixy="-9.3E-06" ixz="1E-05" iyy="0.0004146" iyz="5.5E-06" izz="0.0004416"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_shoulder_pitch_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_shoulder_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_shoulder_pitch_joint" type="fixed">
+    <origin xyz="-0.0025 0.10396 0.25928" rpy="0.27925 0 0"/>
+    <parent link="torso_link"/>
+    <child link="left_shoulder_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-2.9671" upper="2.7925" effort="21" velocity="53"/>
+  </joint>
+  <link name="left_shoulder_roll_link">
+    <inertial>
+      <origin xyz="-0.003415 0.006955 -0.064598" rpy="0 0 0"/>
+      <mass value="0.642"/>
+      <inertia ixx="0.0006159" ixy="0" ixz="-5.6E-06" iyy="0.0006835" iyz="2.1E-06" izz="0.000373"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_shoulder_roll_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <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="fixed">
+    <origin xyz="0 0.052 0" 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="21" velocity="53"/>
+  </joint>
+  <link name="left_shoulder_yaw_link">
+    <inertial>
+      <origin xyz="0.000375 -0.00444 -0.072374" rpy="0 0 0"/>
+      <mass value="0.713"/>
+      <inertia ixx="0.0009699" ixy="7.1E-06" ixz="7.8E-06" iyy="0.0009691" iyz="-4.49E-05" izz="0.0003826"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_shoulder_yaw_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_shoulder_yaw_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_shoulder_yaw_joint" type="fixed">
+    <origin xyz="-0.00354 0.0062424 -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="21" velocity="53"/>
+  </joint>
+  <link name="left_elbow_pitch_link">
+    <inertial>
+      <origin xyz="0.064497 0.002873 0" rpy="0 0 0"/>
+      <mass value="0.601"/>
+      <inertia ixx="0.0002845" ixy="8.06E-05" ixz="9E-09" iyy="0.0004647" iyz="5E-09" izz="0.0004712"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_elbow_pitch_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_elbow_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_elbow_pitch_joint" type="fixed">
+    <origin xyz="0 0.00189 -0.0855" rpy="0 0 0"/>
+    <parent link="left_shoulder_yaw_link"/>
+    <child link="left_elbow_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.2268" upper="3.4208" effort="21" velocity="53"/>
+  </joint>
+  <link name="left_elbow_roll_link">
+    <inertial>
+      <origin xyz="0.081811 0.001454 0.001" rpy="0 0 0"/>
+      <mass value="0.227"/>
+      <inertia ixx="0.0001093" ixy="-1.55E-05" ixz="-2.91E-05" iyy="0.000977" iyz="1E-06" izz="0.0009676"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_elbow_roll_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_elbow_roll_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_elbow_roll_joint" type="fixed">
+    <origin xyz="0.1 0 0" rpy="0 0 0"/>
+    <parent link="left_elbow_pitch_link"/>
+    <child link="left_elbow_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-2.0943" upper="2.0943" effort="21" velocity="53"/>
+  </joint>
+
+  <!-- R ARM -->
+  <link name="right_shoulder_pitch_link">
+    <inertial>
+      <origin xyz="-0.001431 -0.048811 0.001304" rpy="0 0 0"/>
+      <mass value="0.713"/>
+      <inertia ixx="0.0004614" ixy="9.3E-06" ixz="1E-05" iyy="0.0004146" iyz="-5.5E-06" izz="0.0004416"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_shoulder_pitch_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_shoulder_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_shoulder_pitch_joint" type="fixed">
+    <origin xyz="-0.0025 -0.10396 0.25928" rpy="-0.27925 0 0"/>
+    <parent link="torso_link"/>
+    <child link="right_shoulder_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-2.9671" upper="2.7925" effort="21" velocity="53"/>
+  </joint>
+  <link name="right_shoulder_roll_link">
+    <inertial>
+      <origin xyz="-0.003415 -0.006955 -0.064598" rpy="0 0 0"/>
+      <mass value="0.642"/>
+      <inertia ixx="0.0006159" ixy="0" ixz="-5.6E-06" iyy="0.0006835" iyz="-2.1E-06" izz="0.000373"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_shoulder_roll_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <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="fixed">
+    <origin xyz="0 -0.052 0" 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="21" velocity="53"/>
+  </joint>
+  <link name="right_shoulder_yaw_link">
+    <inertial>
+      <origin xyz="0.000375 0.00444 -0.072374" rpy="0 0 0"/>
+      <mass value="0.713"/>
+      <inertia ixx="0.0009699" ixy="-7.1E-06" ixz="7.8E-06" iyy="0.0009691" iyz="4.49E-05" izz="0.0003826"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_shoulder_yaw_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_shoulder_yaw_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_shoulder_yaw_joint" type="fixed">
+    <origin xyz="-0.00354 -0.0062424 -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="21" velocity="53"/>
+  </joint>
+  <link name="right_elbow_pitch_link">
+    <inertial>
+      <origin xyz="0.064497 -0.002873 0" rpy="0 0 0"/>
+      <mass value="0.601"/>
+      <inertia ixx="0.0002845" ixy="-8.06E-05" ixz="9E-09" iyy="0.0004647" iyz="-5E-09" izz="0.0004712"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_elbow_pitch_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_elbow_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_elbow_pitch_joint" type="fixed">
+    <origin xyz="0 -0.00189 -0.0855" rpy="0 0 0"/>
+    <parent link="right_shoulder_yaw_link"/>
+    <child link="right_elbow_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.2268" upper="3.4208" effort="21" velocity="53"/>
+  </joint>
+  <link name="right_elbow_roll_link">
+    <inertial>
+      <origin xyz="0.081811 -0.001454 0.001" rpy="0 0 0"/>
+      <mass value="0.227"/>
+      <inertia ixx="0.0001093" ixy="1.55E-05" ixz="-2.91E-05" iyy="0.000977" iyz="-1E-06" izz="0.0009676"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_elbow_roll_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_elbow_roll_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_elbow_roll_joint" type="fixed">
+    <origin xyz="0.1 0 0" rpy="0 0 0"/>
+    <parent link="right_elbow_pitch_link"/>
+    <child link="right_elbow_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-2.0943" upper="2.0943" effort="21" velocity="53"/>
+  </joint>
+
+  <!-- LOGO -->
+  <joint name="logo_joint" type="fixed">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="torso_link"/>
+    <child link="logo_link"/>
+  </joint>
+  <link name="logo_link">
+    <inertial>
+      <origin xyz="-0.00306556489 0.00005625032 0.24526459523" rpy="0 0 0"/>
+      <mass value="0.001"/>
+      <inertia ixx="0.00000089650105" ixy="0.00000007028325" ixz="0.00000002735427" iyy="0.00000644846110" iyz="0.00000000029738" izz="0.00000688611155"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/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="../mesh/G1/logo_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- IMU -->
+  <link name="imu_link"></link>
+  <joint name="imu_joint" type="fixed">
+    <origin xyz="-0.04233868314 0.00166 0.152067" rpy="0 0 0"/>
+    <parent link="torso_link"/>
+    <child link="imu_link"/>
+  </joint>
+
+  <!-- Gripper -->
+  <joint name="left_palm_joint" type="fixed">
+    <origin xyz="0.120 0 0" rpy="0 0 0"/>
+    <parent link="left_elbow_roll_link"/>
+    <child link="left_palm_link"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+  <link name="left_palm_link">
+    <inertial>
+      <origin xyz="0.05578422502 0.00148509387 -0.00032670143" rpy="0 0 0"/>
+      <mass value="0.28126043"/>
+      <inertia ixx="0.00017638101047" ixy="-0.00000652171140" ixz="0.00000021301775" iyy="0.00031084250366" iyz="0.00000000708018" izz="0.00018939350451"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_palm_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_palm_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_zero_joint" type="fixed">
+    <origin xyz="0.050 0 0" rpy="0 0 0"/>
+    <parent link="left_palm_link"/>
+    <child link="left_zero_link"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="0.7" velocity="23" lower="-0.523598" upper="0.523598"/>
+  </joint>
+  <link name="left_zero_link">
+    <inertial>
+      <origin xyz="-0.02604664462 -0.00877511180 0.00001086048" rpy="0 0 0"/>
+      <mass value="0.05098927"/>
+      <inertia ixx="0.00000679821860" ixy="-0.00000098281498" ixz="-0.00000000406407" iyy="0.00000501631802" iyz="0.00000002833450" izz="0.00000868017156"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_zero_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <!-- <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_zero_link.STL"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="left_one_joint" type="fixed">
+    <origin xyz="-0.02652502957 -0.0188 -0.00005" rpy="0 0 0"/>
+    <parent link="left_zero_link"/>
+    <child link="left_one_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="-1" upper="1.2"/>
+  </joint>
+  <link name="left_one_link">
+    <inertial>
+      <origin xyz="-0.00181538145 -0.03270571529 -0.00018587117" rpy="0 0 0"/>
+      <mass value="0.04776204"/>
+      <inertia ixx="0.00000800634866" ixy="-0.00000020811350" ixz="-0.00000004687654" iyy="0.00000366333777" iyz="0.00000005133915" izz="0.00000785913677"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_one_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <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_two_joint" type="fixed">
+    <origin xyz="0 -0.0431 0" rpy="0 0 0"/>
+    <parent link="left_one_link"/>
+    <child link="left_two_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="0" upper="1.84"/>
+  </joint>
+  <link name="left_two_link">
+    <inertial>
+      <origin xyz="-0.00135257289 -0.02375910756 -0.00011966865" rpy="0 0 0"/>
+      <mass value="0.01385835"/>
+      <inertia ixx="0.00000254698095" ixy="-0.00000010086310" ixz="-0.00000000325710" iyy="0.00000094744556" iyz="0.00000003605469" izz="0.00000204288618"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_two_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_two_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_three_joint" type="fixed">
+    <origin xyz="0.085 0.004 0.02395" rpy="0 0 0"/>
+    <parent link="left_palm_link"/>
+    <child link="left_three_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="-1.84" upper="0.3"/>
+  </joint>
+  <link name="left_three_link">
+    <inertial>
+      <origin xyz="0.03625612321 -0.00157250459 0.00026989918" rpy="0 0 0"/>
+      <mass value="0.04884689"/>
+      <inertia ixx="0.00000383218469" ixy="0.00000003239984" ixz="0.00000001923377" iyy="0.00000890974599" iyz="-0.00000004429800" izz="0.00000865612355"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_three_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_three_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_four_joint" type="fixed">
+    <origin xyz="0.0471 -0.0036 0" rpy="0 0 0"/>
+    <parent link="left_three_link"/>
+    <child link="left_four_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="-1.84" upper="0"/>
+  </joint>
+  <link name="left_four_link">
+    <inertial>
+      <origin xyz="0.02375910756 0.00135257289 0.00011966865" rpy="0 0 0"/>
+      <mass value="0.01385835"/>
+      <inertia ixx="0.00000094744556" ixy="-0.00000010086310" ixz="0.00000003605469" iyy="0.00000254698095" iyz="-0.00000000325710" izz="0.00000204288618"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_four_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_four_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_five_joint" type="fixed">
+    <origin xyz="0.085 0.004 -0.02395" rpy="0 0 0"/>
+    <parent link="left_palm_link"/>
+    <child link="left_five_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="-1.84" upper="0.3"/>
+  </joint>
+  <link name="left_five_link">
+    <inertial>
+      <origin xyz="0.03625612321 -0.00157250459 0.00026989918" rpy="0 0 0"/>
+      <mass value="0.04884689"/>
+      <inertia ixx="0.00000383218469" ixy="0.00000003239984" ixz="0.00000001923377" iyy="0.00000890974599" iyz="-0.00000004429800" izz="0.00000865612355"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_five_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_five_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_six_joint" type="fixed">
+    <origin xyz="0.0471 -0.0036 0" rpy="0 0 0"/>
+    <parent link="left_five_link"/>
+    <child link="left_six_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="-1.84" upper="0"/>
+  </joint>
+  <link name="left_six_link">
+    <inertial>
+      <origin xyz="0.02375910756 0.00135257289 0.00011966865" rpy="0 0 0"/>
+      <mass value="0.01385835"/>
+      <inertia ixx="0.00000094744556" ixy="-0.00000010086310" ixz="0.00000003605469" iyy="0.00000254698095" iyz="-0.00000000325710" izz="0.00000204288618"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_six_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_six_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+
+
+  <joint name="right_palm_joint" type="fixed">
+    <origin xyz="0.120 0 0" rpy="0 0 0"/>
+    <parent link="right_elbow_roll_link"/>
+    <child link="right_palm_link"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+  <link name="right_palm_link">
+    <inertial>
+      <origin xyz="0.05578422502 -0.00148509387 -0.00032670143" rpy="0 0 0"/>
+      <mass value="0.28126043"/>
+      <inertia ixx="0.00017638101047" ixy="0.00000652171140" ixz="0.00000021301775" iyy="0.00031084250366" iyz="-0.00000000708018" izz="0.00018939350451"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_palm_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_palm_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_zero_joint" type="fixed">
+    <origin xyz="0.050 0 0" rpy="0 0 0"/>
+    <parent link="right_palm_link"/>
+    <child link="right_zero_link"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="0.7" velocity="23" lower="-0.523598" upper="0.523598"/>
+  </joint>
+  <link name="right_zero_link">
+    <inertial>
+      <origin xyz="-0.02604664462 0.00877511180 0.00001086048" rpy="0 0 0"/>
+      <mass value="0.05098927"/>
+      <inertia ixx="0.00000679821860" ixy="0.00000098281498" ixz="-0.00000000406407" iyy="0.00000501631802" iyz="-0.00000002833450" izz="0.00000868017156"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_zero_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <!-- <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_zero_link.STL"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="right_one_joint" type="fixed">
+    <origin xyz="-0.02652502957 0.0188 -0.00005" rpy="0 0 0"/>
+    <parent link="right_zero_link"/>
+    <child link="right_one_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="-1.2" upper="1"/>
+  </joint>
+  <link name="right_one_link">
+    <inertial>
+      <origin xyz="-0.00181538145 0.03270571529 -0.00018587117" rpy="0 0 0"/>
+      <mass value="0.04776204"/>
+      <inertia ixx="0.00000800634866" ixy="0.00000020811350" ixz="-0.00000004687654" iyy="0.00000366333777" iyz="-0.00000005133915" izz="0.00000785913677"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_one_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <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_two_joint" type="fixed">
+    <origin xyz="0 0.0431 0" rpy="0 0 0"/>
+    <parent link="right_one_link"/>
+    <child link="right_two_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="-1.84" upper="0"/>
+  </joint>
+  <link name="right_two_link">
+    <inertial>
+      <origin xyz="-0.00135257289 0.02375910756 -0.00011966865" rpy="0 0 0"/>
+      <mass value="0.01385835"/>
+      <inertia ixx="0.00000254698095" ixy="0.00000010086310" ixz="-0.00000000325710" iyy="0.00000094744556" iyz="-0.00000003605469" izz="0.00000204288618"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_two_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_two_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_three_joint" type="fixed">
+    <origin xyz="0.085 -0.004 0.02395" rpy="0 0 0"/>
+    <parent link="right_palm_link"/>
+    <child link="right_three_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="-0.3" upper="1.84"/>
+  </joint>
+  <link name="right_three_link">
+    <inertial>
+      <origin xyz="0.03625612321 0.00157250459 0.00026989918" rpy="0 0 0"/>
+      <mass value="0.04884689"/>
+      <inertia ixx="0.00000383218469" ixy="-0.00000003239984" ixz="0.00000001923377" iyy="0.00000890974599" iyz="0.00000004429800" izz="0.00000865612355"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_three_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_three_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_four_joint" type="fixed">
+    <origin xyz="0.0471 0.0036 0" rpy="0 0 0"/>
+    <parent link="right_three_link"/>
+    <child link="right_four_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="0" upper="1.84"/>
+  </joint>
+  <link name="right_four_link">
+    <inertial>
+      <origin xyz="0.02375910756 -0.00135257289 0.00011966865" rpy="0 0 0"/>
+      <mass value="0.01385835"/>
+      <inertia ixx="0.00000094744556" ixy="0.00000010086310" ixz="0.00000003605469" iyy="0.00000254698095" iyz="0.00000000325710" izz="0.00000204288618"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_four_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_four_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_five_joint" type="fixed">
+    <origin xyz="0.085 -0.004 -0.02395" rpy="0 0 0"/>
+    <parent link="right_palm_link"/>
+    <child link="right_five_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="-0.3" upper="1.84"/>
+  </joint>
+  <link name="right_five_link">
+    <inertial>
+      <origin xyz="0.03625612321 0.00157250459 0.00026989918" rpy="0 0 0"/>
+      <mass value="0.04884689"/>
+      <inertia ixx="0.00000383218469" ixy="-0.00000003239984" ixz="0.00000001923377" iyy="0.00000890974599" iyz="0.00000004429800" izz="0.00000865612355"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_five_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_five_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_six_joint" type="fixed">
+    <origin xyz="0.0471 0.0036 0" rpy="0 0 0"/>
+    <parent link="right_five_link"/>
+    <child link="right_six_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="0" upper="1.84"/>
+  </joint>
+  <link name="right_six_link">
+    <inertial>
+      <origin xyz="0.02375910756 -0.00135257289 0.00011966865" rpy="0 0 0"/>
+      <mass value="0.01385835"/>
+      <inertia ixx="0.00000094744556" ixy="0.00000010086310" ixz="0.00000003605469" iyy="0.00000254698095" iyz="0.00000000325710" izz="0.00000204288618"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_six_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_six_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>

GR00T-WholeBodyControl/gear_sonic/data/robots/g1/meshes/old/g1_12dof.urdf

@@ -0,0 +1,1295 @@
+<robot name="g1">
+
+  <link name="pelvis">
+    <inertial>
+      <origin xyz="0 0 -0.07605" rpy="0 0 0"/>
+      <mass value="2.86"/>
+      <inertia ixx="0.0079143" ixy="0" ixz="1.6E-06" iyy="0.0069837" iyz="0" izz="0.0059404"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/pelvis.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/pelvis.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="pelvis_contour_link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/pelvis_contour_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="pelvis_contour_joint" type="fixed">
+    <parent link="pelvis"/>
+    <child link="pelvis_contour_link"/>
+  </joint>
+
+  <!-- Leg -->
+  <link name="left_hip_pitch_link">
+    <inertial>
+      <origin xyz="0.001962 0.049392 -0.000941" rpy="0 0 0"/>
+      <mass value="1.299"/>
+      <inertia ixx="0.0013873" ixy="-1.63E-05" ixz="-1E-06" iyy="0.0009059" iyz="-4.24E-05" izz="0.0009196"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/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="../mesh/G1/left_hip_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_hip_pitch_joint" type="revolute">
+    <origin xyz="0 0.06445 -0.1027" rpy="0 -0.34907 0"/>
+    <parent link="pelvis"/>
+    <child link="left_hip_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-2.35" upper="3.05" effort="88" velocity="32"/>
+  </joint>
+  <link name="left_hip_roll_link">
+    <inertial>
+      <origin xyz="0.024757 -0.001036 -0.086323" rpy="0 0 0"/>
+      <mass value="1.446"/>
+      <inertia ixx="0.0022702" ixy="-3.7E-06" ixz="-0.0003789" iyy="0.002304" iyz="-1.8E-05" izz="0.0016001"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_hip_roll_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_hip_roll_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_hip_roll_joint" type="revolute">
+    <origin xyz="0 0.0523 0" rpy="0 0 0"/>
+    <parent link="left_hip_pitch_link"/>
+    <child link="left_hip_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-0.26" upper="2.53" effort="88" velocity="32"/>
+  </joint>
+  <link name="left_hip_yaw_link">
+    <inertial>
+      <origin xyz="-0.053554 -0.011477 -0.14067" rpy="0 0 0"/>
+      <mass value="2.052"/>
+      <inertia ixx="0.0087264" ixy="-0.0004402" ixz="-0.0036676" iyy="0.011374" iyz="-0.0006654" izz="0.004279"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_hip_yaw_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_hip_yaw_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_hip_yaw_joint" type="revolute">
+    <origin xyz="0.01966 -0.0012139 -0.1241" rpy="0 0 0"/>
+    <parent link="left_hip_roll_link"/>
+    <child link="left_hip_yaw_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-2.75" upper="2.75" effort="88" velocity="32"/>
+  </joint>
+  <link name="left_knee_link">
+    <inertial>
+      <origin xyz="0.005505 0.006534 -0.116629" rpy="0 0 0"/>
+      <mass value="2.252"/>
+      <inertia ixx="0.012443837" ixy="0.000053496" ixz="-0.000437641" iyy="0.012674902" iyz="-0.000682499" izz="0.001986501"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_knee_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0.007 0.005 -0.15" rpy="0 0 0"/>
+      <geometry>
+        <cylinder radius="0.015" length="0.15"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_knee_joint" type="revolute">
+    <origin xyz="-0.078292 -0.0017335 -0.177225" rpy="0 0.5096 0"/>
+    <parent link="left_hip_yaw_link"/>
+    <child link="left_knee_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.33489" upper="2.5449" 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="../mesh/G1/left_ankle_pitch_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_ankle_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_ankle_pitch_joint" type="revolute">
+    <origin xyz="0 0.0040687 -0.30007" rpy="0 -0.16053 0"/>
+    <parent link="left_knee_link"/>
+    <child link="left_ankle_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.68" upper="0.73" effort="40" velocity="53"/>
+  </joint>
+  <link name="left_ankle_roll_link">
+    <inertial>
+      <origin xyz="0.024762 2E-05 -0.012526" rpy="0 0 0"/>
+      <mass value="0.391"/>
+      <inertia ixx="0.0001552" ixy="-1E-07" ixz="7.51E-05" iyy="0.0010657" iyz="1E-07" izz="0.001098"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_ankle_roll_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="../mesh/G1/left_ankle_roll_link.STL"/>
+      </geometry>
+    </collision>
+    <!-- <collision>
+      <origin xyz="-0.06 0.02 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.001"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin xyz="-0.06 -0.02 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.001"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin xyz="0.13 0.02 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.001"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin xyz="0.13 -0.02 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.001"/>
+      </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="40" velocity="53"/>
+  </joint>
+  <link name="right_hip_pitch_link">
+    <inertial>
+      <origin xyz="0.001962 -0.049392 -0.000941" rpy="0 0 0"/>
+      <mass value="1.299"/>
+      <inertia ixx="0.0013873" ixy="1.63E-05" ixz="-1E-06" iyy="0.0009059" iyz="4.24E-05" izz="0.0009196"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/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="../mesh/G1/right_hip_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_hip_pitch_joint" type="revolute">
+    <origin xyz="0 -0.06445 -0.1027" rpy="0 -0.34907 0"/>
+    <parent link="pelvis"/>
+    <child link="right_hip_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-2.35" upper="3.05" effort="88" velocity="32"/>
+  </joint>
+  <link name="right_hip_roll_link">
+    <inertial>
+      <origin xyz="0.024757 0.001036 -0.086323" rpy="0 0 0"/>
+      <mass value="1.446"/>
+      <inertia ixx="0.0022702" ixy="3.7E-06" ixz="-0.0003789" iyy="0.002304" iyz="1.8E-05" izz="0.0016001"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_hip_roll_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_hip_roll_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_hip_roll_joint" type="revolute">
+    <origin xyz="0 -0.0523 0" rpy="0 0 0"/>
+    <parent link="right_hip_pitch_link"/>
+    <child link="right_hip_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-2.53" upper="0.26" effort="88" velocity="32"/>
+  </joint>
+  <link name="right_hip_yaw_link">
+    <inertial>
+      <origin xyz="-0.053554 0.011477 -0.14067" rpy="0 0 0"/>
+      <mass value="2.052"/>
+      <inertia ixx="0.0087264" ixy="0.0004402" ixz="-0.0036676" iyy="0.011374" iyz="0.0006654" izz="0.004279"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_hip_yaw_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_hip_yaw_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_hip_yaw_joint" type="revolute">
+    <origin xyz="0.01966 0.0012139 -0.1241" rpy="0 0 0"/>
+    <parent link="right_hip_roll_link"/>
+    <child link="right_hip_yaw_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-2.75" upper="2.75" effort="88" velocity="32"/>
+  </joint>
+  <link name="right_knee_link">
+    <inertial>
+      <origin xyz="0.005505 -0.006534 -0.116629" rpy="0 0 0"/>
+      <mass value="2.252"/>
+      <inertia ixx="0.012443837" ixy="-0.000053496" ixz="-0.000437641" iyy="0.012674902" iyz="0.000682499" izz="0.001986501"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_knee_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0.007 -0.005 -0.15" rpy="0 0 0"/>
+      <geometry>
+        <cylinder radius="0.015" length="0.15"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_knee_joint" type="revolute">
+    <origin xyz="-0.078292 0.0017335 -0.177225" rpy="0 0.5096 0"/>
+    <parent link="right_hip_yaw_link"/>
+    <child link="right_knee_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.33489" upper="2.5449" 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="../mesh/G1/right_ankle_pitch_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_ankle_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_ankle_pitch_joint" type="revolute">
+    <origin xyz="0 -0.0040687 -0.30007" rpy="0 -0.16053 0"/>
+    <parent link="right_knee_link"/>
+    <child link="right_ankle_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.68" upper="0.73" effort="40" velocity="53"/>
+  </joint>
+  <link name="right_ankle_roll_link">
+    <inertial>
+      <origin xyz="0.024762 -2E-05 -0.012526" rpy="0 0 0"/>
+      <mass value="0.391"/>
+      <inertia ixx="0.0001552" ixy="1E-07" ixz="7.51E-05" iyy="0.0010657" iyz="-1E-07" izz="0.001098"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_ankle_roll_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="../mesh/G1/right_ankle_roll_link.STL"/>
+      </geometry>
+    </collision>
+    <!-- <collision>
+      <origin xyz="-0.06 0.02 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.001"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin xyz="-0.06 -0.02 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.001"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin xyz="0.13 0.02 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.001"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin xyz="0.13 -0.02 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.001"/>
+      </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="40" velocity="53"/>
+  </joint>
+
+  <!-- Torso -->
+  <link name="torso_link">
+    <inertial>
+      <origin xyz="0.00197122283 0.00266902037 0.16936510960" rpy="0 0 0"/>
+      <mass value="6.33959811"/>
+      <inertia ixx="0.06037082109027" ixy="-0.00010168407055" ixz="0.00069845018092" iyy="0.04355153796551" iyz="0.00034366422728" izz="0.03339981866010"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/torso_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/torso_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="torso_joint" type="fixed">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="pelvis"/>
+    <child link="torso_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-2.618" upper="2.618" effort="88" velocity="32"/>
+  </joint>
+
+  <!-- Head -->
+  <link name="head_link">
+    <inertial>
+      <origin xyz="0.00138066852 0.00028430950 0.42034187824" rpy="0 0 0"/>
+      <mass value="1.17976522"/>
+      <inertia ixx="0.00543236042361" ixy="0.00000140137425" ixz="0.00034554752228" iyy="0.00552885306699" iyz="0.00001501216392" izz="0.00165378108136"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/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="../mesh/G1/head_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="head_joint" type="fixed">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="torso_link"/>
+    <child link="head_link"/>
+  </joint>
+
+  <!-- L ARM -->
+  <link name="left_shoulder_pitch_link">
+    <inertial>
+      <origin xyz="-0.001431 0.048811 0.001304" rpy="0 0 0"/>
+      <mass value="0.713"/>
+      <inertia ixx="0.0004614" ixy="-9.3E-06" ixz="1E-05" iyy="0.0004146" iyz="5.5E-06" izz="0.0004416"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_shoulder_pitch_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_shoulder_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_shoulder_pitch_joint" type="fixed">
+    <origin xyz="-0.0025 0.10396 0.25928" rpy="0.27925 0 0"/>
+    <parent link="torso_link"/>
+    <child link="left_shoulder_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-2.9671" upper="2.7925" effort="21" velocity="53"/>
+  </joint>
+  <link name="left_shoulder_roll_link">
+    <inertial>
+      <origin xyz="-0.003415 0.006955 -0.064598" rpy="0 0 0"/>
+      <mass value="0.642"/>
+      <inertia ixx="0.0006159" ixy="0" ixz="-5.6E-06" iyy="0.0006835" iyz="2.1E-06" izz="0.000373"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_shoulder_roll_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <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="fixed">
+    <origin xyz="0 0.052 0" 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="21" velocity="53"/>
+  </joint>
+  <link name="left_shoulder_yaw_link">
+    <inertial>
+      <origin xyz="0.000375 -0.00444 -0.072374" rpy="0 0 0"/>
+      <mass value="0.713"/>
+      <inertia ixx="0.0009699" ixy="7.1E-06" ixz="7.8E-06" iyy="0.0009691" iyz="-4.49E-05" izz="0.0003826"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_shoulder_yaw_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_shoulder_yaw_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_shoulder_yaw_joint" type="fixed">
+    <origin xyz="-0.00354 0.0062424 -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="21" velocity="53"/>
+  </joint>
+  <link name="left_elbow_pitch_link">
+    <inertial>
+      <origin xyz="0.064497 0.002873 0" rpy="0 0 0"/>
+      <mass value="0.601"/>
+      <inertia ixx="0.0002845" ixy="8.06E-05" ixz="9E-09" iyy="0.0004647" iyz="5E-09" izz="0.0004712"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_elbow_pitch_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_elbow_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_elbow_pitch_joint" type="fixed">
+    <origin xyz="0 0.00189 -0.0855" rpy="0 0 0"/>
+    <parent link="left_shoulder_yaw_link"/>
+    <child link="left_elbow_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.2268" upper="3.4208" effort="21" velocity="53"/>
+  </joint>
+  <link name="left_elbow_roll_link">
+    <inertial>
+      <origin xyz="0.081811 0.001454 0.001" rpy="0 0 0"/>
+      <mass value="0.227"/>
+      <inertia ixx="0.0001093" ixy="-1.55E-05" ixz="-2.91E-05" iyy="0.000977" iyz="1E-06" izz="0.0009676"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_elbow_roll_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_elbow_roll_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_elbow_roll_joint" type="fixed">
+    <origin xyz="0.1 0 0" rpy="0 0 0"/>
+    <parent link="left_elbow_pitch_link"/>
+    <child link="left_elbow_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-2.0943" upper="2.0943" effort="21" velocity="53"/>
+  </joint>
+
+  <!-- R ARM -->
+  <link name="right_shoulder_pitch_link">
+    <inertial>
+      <origin xyz="-0.001431 -0.048811 0.001304" rpy="0 0 0"/>
+      <mass value="0.713"/>
+      <inertia ixx="0.0004614" ixy="9.3E-06" ixz="1E-05" iyy="0.0004146" iyz="-5.5E-06" izz="0.0004416"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_shoulder_pitch_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_shoulder_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_shoulder_pitch_joint" type="fixed">
+    <origin xyz="-0.0025 -0.10396 0.25928" rpy="-0.27925 0 0"/>
+    <parent link="torso_link"/>
+    <child link="right_shoulder_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-2.9671" upper="2.7925" effort="21" velocity="53"/>
+  </joint>
+  <link name="right_shoulder_roll_link">
+    <inertial>
+      <origin xyz="-0.003415 -0.006955 -0.064598" rpy="0 0 0"/>
+      <mass value="0.642"/>
+      <inertia ixx="0.0006159" ixy="0" ixz="-5.6E-06" iyy="0.0006835" iyz="-2.1E-06" izz="0.000373"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_shoulder_roll_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <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="fixed">
+    <origin xyz="0 -0.052 0" 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="21" velocity="53"/>
+  </joint>
+  <link name="right_shoulder_yaw_link">
+    <inertial>
+      <origin xyz="0.000375 0.00444 -0.072374" rpy="0 0 0"/>
+      <mass value="0.713"/>
+      <inertia ixx="0.0009699" ixy="-7.1E-06" ixz="7.8E-06" iyy="0.0009691" iyz="4.49E-05" izz="0.0003826"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_shoulder_yaw_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_shoulder_yaw_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_shoulder_yaw_joint" type="fixed">
+    <origin xyz="-0.00354 -0.0062424 -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="21" velocity="53"/>
+  </joint>
+  <link name="right_elbow_pitch_link">
+    <inertial>
+      <origin xyz="0.064497 -0.002873 0" rpy="0 0 0"/>
+      <mass value="0.601"/>
+      <inertia ixx="0.0002845" ixy="-8.06E-05" ixz="9E-09" iyy="0.0004647" iyz="-5E-09" izz="0.0004712"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_elbow_pitch_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_elbow_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_elbow_pitch_joint" type="fixed">
+    <origin xyz="0 -0.00189 -0.0855" rpy="0 0 0"/>
+    <parent link="right_shoulder_yaw_link"/>
+    <child link="right_elbow_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.2268" upper="3.4208" effort="21" velocity="53"/>
+  </joint>
+  <link name="right_elbow_roll_link">
+    <inertial>
+      <origin xyz="0.081811 -0.001454 0.001" rpy="0 0 0"/>
+      <mass value="0.227"/>
+      <inertia ixx="0.0001093" ixy="1.55E-05" ixz="-2.91E-05" iyy="0.000977" iyz="-1E-06" izz="0.0009676"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_elbow_roll_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_elbow_roll_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_elbow_roll_joint" type="fixed">
+    <origin xyz="0.1 0 0" rpy="0 0 0"/>
+    <parent link="right_elbow_pitch_link"/>
+    <child link="right_elbow_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-2.0943" upper="2.0943" effort="21" velocity="53"/>
+  </joint>
+
+  <!-- LOGO -->
+  <joint name="logo_joint" type="fixed">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="torso_link"/>
+    <child link="logo_link"/>
+  </joint>
+  <link name="logo_link">
+    <inertial>
+      <origin xyz="-0.00306556489 0.00005625032 0.24526459523" rpy="0 0 0"/>
+      <mass value="0.001"/>
+      <inertia ixx="0.00000089650105" ixy="0.00000007028325" ixz="0.00000002735427" iyy="0.00000644846110" iyz="0.00000000029738" izz="0.00000688611155"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/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="../mesh/G1/logo_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- IMU -->
+  <link name="imu_link"></link>
+  <joint name="imu_joint" type="fixed">
+    <origin xyz="-0.04233868314 0.00166 0.152067" rpy="0 0 0"/>
+    <parent link="torso_link"/>
+    <child link="imu_link"/>
+  </joint>
+
+  <!-- Gripper -->
+  <joint name="left_palm_joint" type="fixed">
+    <origin xyz="0.120 0 0" rpy="0 0 0"/>
+    <parent link="left_elbow_roll_link"/>
+    <child link="left_palm_link"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+  <link name="left_palm_link">
+    <inertial>
+      <origin xyz="0.05578422502 0.00148509387 -0.00032670143" rpy="0 0 0"/>
+      <mass value="0.28126043"/>
+      <inertia ixx="0.00017638101047" ixy="-0.00000652171140" ixz="0.00000021301775" iyy="0.00031084250366" iyz="0.00000000708018" izz="0.00018939350451"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_palm_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_palm_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_zero_joint" type="fixed">
+    <origin xyz="0.050 0 0" rpy="0 0 0"/>
+    <parent link="left_palm_link"/>
+    <child link="left_zero_link"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="0.7" velocity="23" lower="-0.523598" upper="0.523598"/>
+  </joint>
+  <link name="left_zero_link">
+    <inertial>
+      <origin xyz="-0.02604664462 -0.00877511180 0.00001086048" rpy="0 0 0"/>
+      <mass value="0.05098927"/>
+      <inertia ixx="0.00000679821860" ixy="-0.00000098281498" ixz="-0.00000000406407" iyy="0.00000501631802" iyz="0.00000002833450" izz="0.00000868017156"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_zero_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <!-- <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_zero_link.STL"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="left_one_joint" type="fixed">
+    <origin xyz="-0.02652502957 -0.0188 -0.00005" rpy="0 0 0"/>
+    <parent link="left_zero_link"/>
+    <child link="left_one_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="-1" upper="1.2"/>
+  </joint>
+  <link name="left_one_link">
+    <inertial>
+      <origin xyz="-0.00181538145 -0.03270571529 -0.00018587117" rpy="0 0 0"/>
+      <mass value="0.04776204"/>
+      <inertia ixx="0.00000800634866" ixy="-0.00000020811350" ixz="-0.00000004687654" iyy="0.00000366333777" iyz="0.00000005133915" izz="0.00000785913677"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_one_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <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_two_joint" type="fixed">
+    <origin xyz="0 -0.0431 0" rpy="0 0 0"/>
+    <parent link="left_one_link"/>
+    <child link="left_two_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="0" upper="1.84"/>
+  </joint>
+  <link name="left_two_link">
+    <inertial>
+      <origin xyz="-0.00135257289 -0.02375910756 -0.00011966865" rpy="0 0 0"/>
+      <mass value="0.01385835"/>
+      <inertia ixx="0.00000254698095" ixy="-0.00000010086310" ixz="-0.00000000325710" iyy="0.00000094744556" iyz="0.00000003605469" izz="0.00000204288618"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_two_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_two_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_three_joint" type="fixed">
+    <origin xyz="0.085 0.004 0.02395" rpy="0 0 0"/>
+    <parent link="left_palm_link"/>
+    <child link="left_three_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="-1.84" upper="0.3"/>
+  </joint>
+  <link name="left_three_link">
+    <inertial>
+      <origin xyz="0.03625612321 -0.00157250459 0.00026989918" rpy="0 0 0"/>
+      <mass value="0.04884689"/>
+      <inertia ixx="0.00000383218469" ixy="0.00000003239984" ixz="0.00000001923377" iyy="0.00000890974599" iyz="-0.00000004429800" izz="0.00000865612355"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_three_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_three_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_four_joint" type="fixed">
+    <origin xyz="0.0471 -0.0036 0" rpy="0 0 0"/>
+    <parent link="left_three_link"/>
+    <child link="left_four_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="-1.84" upper="0"/>
+  </joint>
+  <link name="left_four_link">
+    <inertial>
+      <origin xyz="0.02375910756 0.00135257289 0.00011966865" rpy="0 0 0"/>
+      <mass value="0.01385835"/>
+      <inertia ixx="0.00000094744556" ixy="-0.00000010086310" ixz="0.00000003605469" iyy="0.00000254698095" iyz="-0.00000000325710" izz="0.00000204288618"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_four_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_four_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_five_joint" type="fixed">
+    <origin xyz="0.085 0.004 -0.02395" rpy="0 0 0"/>
+    <parent link="left_palm_link"/>
+    <child link="left_five_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="-1.84" upper="0.3"/>
+  </joint>
+  <link name="left_five_link">
+    <inertial>
+      <origin xyz="0.03625612321 -0.00157250459 0.00026989918" rpy="0 0 0"/>
+      <mass value="0.04884689"/>
+      <inertia ixx="0.00000383218469" ixy="0.00000003239984" ixz="0.00000001923377" iyy="0.00000890974599" iyz="-0.00000004429800" izz="0.00000865612355"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_five_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_five_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_six_joint" type="fixed">
+    <origin xyz="0.0471 -0.0036 0" rpy="0 0 0"/>
+    <parent link="left_five_link"/>
+    <child link="left_six_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="-1.84" upper="0"/>
+  </joint>
+  <link name="left_six_link">
+    <inertial>
+      <origin xyz="0.02375910756 0.00135257289 0.00011966865" rpy="0 0 0"/>
+      <mass value="0.01385835"/>
+      <inertia ixx="0.00000094744556" ixy="-0.00000010086310" ixz="0.00000003605469" iyy="0.00000254698095" iyz="-0.00000000325710" izz="0.00000204288618"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_six_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/left_six_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+
+
+  <joint name="right_palm_joint" type="fixed">
+    <origin xyz="0.120 0 0" rpy="0 0 0"/>
+    <parent link="right_elbow_roll_link"/>
+    <child link="right_palm_link"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+  <link name="right_palm_link">
+    <inertial>
+      <origin xyz="0.05578422502 -0.00148509387 -0.00032670143" rpy="0 0 0"/>
+      <mass value="0.28126043"/>
+      <inertia ixx="0.00017638101047" ixy="0.00000652171140" ixz="0.00000021301775" iyy="0.00031084250366" iyz="-0.00000000708018" izz="0.00018939350451"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_palm_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_palm_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_zero_joint" type="fixed">
+    <origin xyz="0.050 0 0" rpy="0 0 0"/>
+    <parent link="right_palm_link"/>
+    <child link="right_zero_link"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="0.7" velocity="23" lower="-0.523598" upper="0.523598"/>
+  </joint>
+  <link name="right_zero_link">
+    <inertial>
+      <origin xyz="-0.02604664462 0.00877511180 0.00001086048" rpy="0 0 0"/>
+      <mass value="0.05098927"/>
+      <inertia ixx="0.00000679821860" ixy="0.00000098281498" ixz="-0.00000000406407" iyy="0.00000501631802" iyz="-0.00000002833450" izz="0.00000868017156"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_zero_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <!-- <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_zero_link.STL"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="right_one_joint" type="fixed">
+    <origin xyz="-0.02652502957 0.0188 -0.00005" rpy="0 0 0"/>
+    <parent link="right_zero_link"/>
+    <child link="right_one_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="-1.2" upper="1"/>
+  </joint>
+  <link name="right_one_link">
+    <inertial>
+      <origin xyz="-0.00181538145 0.03270571529 -0.00018587117" rpy="0 0 0"/>
+      <mass value="0.04776204"/>
+      <inertia ixx="0.00000800634866" ixy="0.00000020811350" ixz="-0.00000004687654" iyy="0.00000366333777" iyz="-0.00000005133915" izz="0.00000785913677"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_one_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <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_two_joint" type="fixed">
+    <origin xyz="0 0.0431 0" rpy="0 0 0"/>
+    <parent link="right_one_link"/>
+    <child link="right_two_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="-1.84" upper="0"/>
+  </joint>
+  <link name="right_two_link">
+    <inertial>
+      <origin xyz="-0.00135257289 0.02375910756 -0.00011966865" rpy="0 0 0"/>
+      <mass value="0.01385835"/>
+      <inertia ixx="0.00000254698095" ixy="0.00000010086310" ixz="-0.00000000325710" iyy="0.00000094744556" iyz="-0.00000003605469" izz="0.00000204288618"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_two_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_two_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_three_joint" type="fixed">
+    <origin xyz="0.085 -0.004 0.02395" rpy="0 0 0"/>
+    <parent link="right_palm_link"/>
+    <child link="right_three_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="-0.3" upper="1.84"/>
+  </joint>
+  <link name="right_three_link">
+    <inertial>
+      <origin xyz="0.03625612321 0.00157250459 0.00026989918" rpy="0 0 0"/>
+      <mass value="0.04884689"/>
+      <inertia ixx="0.00000383218469" ixy="-0.00000003239984" ixz="0.00000001923377" iyy="0.00000890974599" iyz="0.00000004429800" izz="0.00000865612355"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_three_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_three_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_four_joint" type="fixed">
+    <origin xyz="0.0471 0.0036 0" rpy="0 0 0"/>
+    <parent link="right_three_link"/>
+    <child link="right_four_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="0" upper="1.84"/>
+  </joint>
+  <link name="right_four_link">
+    <inertial>
+      <origin xyz="0.02375910756 -0.00135257289 0.00011966865" rpy="0 0 0"/>
+      <mass value="0.01385835"/>
+      <inertia ixx="0.00000094744556" ixy="0.00000010086310" ixz="0.00000003605469" iyy="0.00000254698095" iyz="0.00000000325710" izz="0.00000204288618"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_four_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_four_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_five_joint" type="fixed">
+    <origin xyz="0.085 -0.004 -0.02395" rpy="0 0 0"/>
+    <parent link="right_palm_link"/>
+    <child link="right_five_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="-0.3" upper="1.84"/>
+  </joint>
+  <link name="right_five_link">
+    <inertial>
+      <origin xyz="0.03625612321 0.00157250459 0.00026989918" rpy="0 0 0"/>
+      <mass value="0.04884689"/>
+      <inertia ixx="0.00000383218469" ixy="-0.00000003239984" ixz="0.00000001923377" iyy="0.00000890974599" iyz="0.00000004429800" izz="0.00000865612355"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_five_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_five_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_six_joint" type="fixed">
+    <origin xyz="0.0471 0.0036 0" rpy="0 0 0"/>
+    <parent link="right_five_link"/>
+    <child link="right_six_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="0" upper="1.84"/>
+  </joint>
+  <link name="right_six_link">
+    <inertial>
+      <origin xyz="0.02375910756 -0.00135257289 0.00011966865" rpy="0 0 0"/>
+      <mass value="0.01385835"/>
+      <inertia ixx="0.00000094744556" ixy="0.00000010086310" ixz="0.00000003605469" iyy="0.00000254698095" iyz="0.00000000325710" izz="0.00000204288618"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_six_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../mesh/G1/right_six_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>

GR00T-WholeBodyControl/gear_sonic/data/robots/g1/meshes/old/g1_23dof.urdf

@@ -0,0 +1,893 @@
+<robot name="g1_23dof">
+
+  <!-- [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="mesh/G1_23DoF/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="mesh/G1_23DoF/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="mesh/G1_23DoF/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="mesh/G1_23DoF/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="mesh/G1_23DoF/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="mesh/G1_23DoF/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="mesh/G1_23DoF/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="mesh/G1_23DoF/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="mesh/G1_23DoF/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="mesh/G1_23DoF/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="mesh/G1_23DoF/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="mesh/G1_23DoF/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="mesh/G1_23DoF/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="mesh/G1_23DoF/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="mesh/G1_23DoF/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="mesh/G1_23DoF/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="mesh/G1_23DoF/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="mesh/G1_23DoF/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="mesh/G1_23DoF/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="mesh/G1_23DoF/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="mesh/G1_23DoF/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="mesh/G1_23DoF/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="mesh/G1_23DoF/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="mesh/G1_23DoF/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="mesh/G1_23DoF/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_fixed_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="mesh/G1_23DoF/waist_yaw_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="waist_yaw_fixed_joint" type="fixed">
+    <origin xyz="0.0039635 0 -0.054" rpy="0 0 0"/>
+    <parent link="torso_link"/>
+    <child link="waist_yaw_fixed_link"/>
+  </joint>
+  <joint name="waist_yaw_joint" type="fixed">
+    <origin xyz="-0.0039635 0 0.054" rpy="0 0 0"/>
+    <parent link="pelvis"/>
+    <child link="torso_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-2.618" upper="2.618" effort="88" velocity="32"/>
+  </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="mesh/G1_23DoF/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="mesh/G1_23DoF/torso_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- 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="mesh/G1_23DoF/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="mesh/G1_23DoF/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="mesh/G1_23DoF/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="mesh/G1_23DoF/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="mesh/G1_23DoF/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="mesh/G1_23DoF/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="mesh/G1_23DoF/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="fixed">
+    <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="mesh/G1_23DoF/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="fixed">
+    <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="mesh/G1_23DoF/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="mesh/G1_23DoF/left_shoulder_yaw_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_shoulder_yaw_joint" type="fixed">
+    <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="mesh/G1_23DoF/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="mesh/G1_23DoF/left_elbow_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_elbow_joint" type="fixed">
+    <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="fixed">
+    <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_rubber_hand"/>
+    <limit effort="25" velocity="37" lower="-1.972222054" upper="1.972222054"/>
+  </joint>
+  <link name="left_wrist_roll_rubber_hand">
+    <inertial>
+      <origin xyz="0.10794656650 0.00163511945 0.00202244863" rpy="0 0 0"/>
+      <mass value="0.35692864"/>
+      <inertia ixx="0.00019613494735" ixy="-0.00000419816908" ixz="-0.00003950860580" iyy="0.00200280358206" iyz="0.00000249774203" izz="0.00194181412808"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1_23DoF/left_wrist_roll_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="mesh/G1_23DoF/left_wrist_roll_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="mesh/G1_23DoF/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="fixed">
+    <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="mesh/G1_23DoF/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="fixed">
+    <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="mesh/G1_23DoF/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="mesh/G1_23DoF/right_shoulder_yaw_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_shoulder_yaw_joint" type="fixed">
+    <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="mesh/G1_23DoF/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="mesh/G1_23DoF/right_elbow_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_elbow_joint" type="fixed">
+    <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="fixed">
+    <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_rubber_hand"/>
+    <limit effort="25" velocity="37" lower="-1.972222054" upper="1.972222054"/>
+  </joint>
+  <link name="right_wrist_roll_rubber_hand">
+    <inertial>
+      <origin xyz="0.10794656650 -0.00163511945 0.00202244863" rpy="0 0 0"/>
+      <mass value="0.35692864"/>
+      <inertia ixx="0.00019613494735" ixy="0.00000419816908" ixz="-0.00003950860580" iyy="0.00200280358206" iyz="-0.00000249774203" izz="0.00194181412808"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1_23DoF/right_wrist_roll_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="mesh/G1_23DoF/right_wrist_roll_rubber_hand.STL"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>

GR00T-WholeBodyControl/gear_sonic/data/robots/g1/meshes/old/g1_unitree.urdf

@@ -0,0 +1,1295 @@
+<robot name="g1">
+
+  <link name="pelvis">
+    <inertial>
+      <origin xyz="0 0 -0.07605" rpy="0 0 0"/>
+      <mass value="2.86"/>
+      <inertia ixx="0.0079143" ixy="0" ixz="1.6E-06" iyy="0.0069837" iyz="0" izz="0.0059404"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/pelvis.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/pelvis.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="pelvis_contour_link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/pelvis_contour_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="pelvis_contour_joint" type="fixed">
+    <parent link="pelvis"/>
+    <child link="pelvis_contour_link"/>
+  </joint>
+
+  <!-- Leg -->
+  <link name="left_hip_pitch_link">
+    <inertial>
+      <origin xyz="0.001962 0.049392 -0.000941" rpy="0 0 0"/>
+      <mass value="1.299"/>
+      <inertia ixx="0.0013873" ixy="-1.63E-05" ixz="-1E-06" iyy="0.0009059" iyz="-4.24E-05" izz="0.0009196"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/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="mesh/G1/left_hip_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_hip_pitch_joint" type="revolute">
+    <origin xyz="0 0.06445 -0.1027" rpy="0 -0.34907 0"/>
+    <parent link="pelvis"/>
+    <child link="left_hip_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-2.35" upper="3.05" effort="88" velocity="32"/>
+  </joint>
+  <link name="left_hip_roll_link">
+    <inertial>
+      <origin xyz="0.024757 -0.001036 -0.086323" rpy="0 0 0"/>
+      <mass value="1.446"/>
+      <inertia ixx="0.0022702" ixy="-3.7E-06" ixz="-0.0003789" iyy="0.002304" iyz="-1.8E-05" izz="0.0016001"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/left_hip_roll_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/left_hip_roll_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_hip_roll_joint" type="revolute">
+    <origin xyz="0 0.0523 0" rpy="0 0 0"/>
+    <parent link="left_hip_pitch_link"/>
+    <child link="left_hip_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-0.26" upper="2.53" effort="88" velocity="32"/>
+  </joint>
+  <link name="left_hip_yaw_link">
+    <inertial>
+      <origin xyz="-0.053554 -0.011477 -0.14067" rpy="0 0 0"/>
+      <mass value="2.052"/>
+      <inertia ixx="0.0087264" ixy="-0.0004402" ixz="-0.0036676" iyy="0.011374" iyz="-0.0006654" izz="0.004279"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/left_hip_yaw_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/left_hip_yaw_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_hip_yaw_joint" type="revolute">
+    <origin xyz="0.01966 -0.0012139 -0.1241" rpy="0 0 0"/>
+    <parent link="left_hip_roll_link"/>
+    <child link="left_hip_yaw_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-2.75" upper="2.75" effort="88" velocity="32"/>
+  </joint>
+  <link name="left_knee_link">
+    <inertial>
+      <origin xyz="0.005505 0.006534 -0.116629" rpy="0 0 0"/>
+      <mass value="2.252"/>
+      <inertia ixx="0.012443837" ixy="0.000053496" ixz="-0.000437641" iyy="0.012674902" iyz="-0.000682499" izz="0.001986501"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/left_knee_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0.007 0.005 -0.15" rpy="0 0 0"/>
+      <geometry>
+        <cylinder radius="0.015" length="0.15"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_knee_joint" type="revolute">
+    <origin xyz="-0.078292 -0.0017335 -0.177225" rpy="0 0.5096 0"/>
+    <parent link="left_hip_yaw_link"/>
+    <child link="left_knee_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.33489" upper="2.5449" 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="mesh/G1/left_ankle_pitch_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/left_ankle_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_ankle_pitch_joint" type="revolute">
+    <origin xyz="0 0.0040687 -0.30007" rpy="0 -0.16053 0"/>
+    <parent link="left_knee_link"/>
+    <child link="left_ankle_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.68" upper="0.73" effort="40" velocity="53"/>
+  </joint>
+  <link name="left_ankle_roll_link">
+    <inertial>
+      <origin xyz="0.024762 2E-05 -0.012526" rpy="0 0 0"/>
+      <mass value="0.391"/>
+      <inertia ixx="0.0001552" ixy="-1E-07" ixz="7.51E-05" iyy="0.0010657" iyz="1E-07" izz="0.001098"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/left_ankle_roll_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="mesh/G1/left_ankle_roll_link.STL"/>
+      </geometry>
+    </collision>
+    <!-- <collision>
+      <origin xyz="-0.06 0.02 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.001"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin xyz="-0.06 -0.02 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.001"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin xyz="0.13 0.02 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.001"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin xyz="0.13 -0.02 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.001"/>
+      </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="40" velocity="53"/>
+  </joint>
+  <link name="right_hip_pitch_link">
+    <inertial>
+      <origin xyz="0.001962 -0.049392 -0.000941" rpy="0 0 0"/>
+      <mass value="1.299"/>
+      <inertia ixx="0.0013873" ixy="1.63E-05" ixz="-1E-06" iyy="0.0009059" iyz="4.24E-05" izz="0.0009196"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/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="mesh/G1/right_hip_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_hip_pitch_joint" type="revolute">
+    <origin xyz="0 -0.06445 -0.1027" rpy="0 -0.34907 0"/>
+    <parent link="pelvis"/>
+    <child link="right_hip_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-2.35" upper="3.05" effort="88" velocity="32"/>
+  </joint>
+  <link name="right_hip_roll_link">
+    <inertial>
+      <origin xyz="0.024757 0.001036 -0.086323" rpy="0 0 0"/>
+      <mass value="1.446"/>
+      <inertia ixx="0.0022702" ixy="3.7E-06" ixz="-0.0003789" iyy="0.002304" iyz="1.8E-05" izz="0.0016001"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/right_hip_roll_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/right_hip_roll_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_hip_roll_joint" type="revolute">
+    <origin xyz="0 -0.0523 0" rpy="0 0 0"/>
+    <parent link="right_hip_pitch_link"/>
+    <child link="right_hip_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-2.53" upper="0.26" effort="88" velocity="32"/>
+  </joint>
+  <link name="right_hip_yaw_link">
+    <inertial>
+      <origin xyz="-0.053554 0.011477 -0.14067" rpy="0 0 0"/>
+      <mass value="2.052"/>
+      <inertia ixx="0.0087264" ixy="0.0004402" ixz="-0.0036676" iyy="0.011374" iyz="0.0006654" izz="0.004279"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/right_hip_yaw_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/right_hip_yaw_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_hip_yaw_joint" type="revolute">
+    <origin xyz="0.01966 0.0012139 -0.1241" rpy="0 0 0"/>
+    <parent link="right_hip_roll_link"/>
+    <child link="right_hip_yaw_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-2.75" upper="2.75" effort="88" velocity="32"/>
+  </joint>
+  <link name="right_knee_link">
+    <inertial>
+      <origin xyz="0.005505 -0.006534 -0.116629" rpy="0 0 0"/>
+      <mass value="2.252"/>
+      <inertia ixx="0.012443837" ixy="-0.000053496" ixz="-0.000437641" iyy="0.012674902" iyz="0.000682499" izz="0.001986501"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/right_knee_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0.007 -0.005 -0.15" rpy="0 0 0"/>
+      <geometry>
+        <cylinder radius="0.015" length="0.15"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_knee_joint" type="revolute">
+    <origin xyz="-0.078292 0.0017335 -0.177225" rpy="0 0.5096 0"/>
+    <parent link="right_hip_yaw_link"/>
+    <child link="right_knee_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.33489" upper="2.5449" 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="mesh/G1/right_ankle_pitch_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/right_ankle_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_ankle_pitch_joint" type="revolute">
+    <origin xyz="0 -0.0040687 -0.30007" rpy="0 -0.16053 0"/>
+    <parent link="right_knee_link"/>
+    <child link="right_ankle_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.68" upper="0.73" effort="40" velocity="53"/>
+  </joint>
+  <link name="right_ankle_roll_link">
+    <inertial>
+      <origin xyz="0.024762 -2E-05 -0.012526" rpy="0 0 0"/>
+      <mass value="0.391"/>
+      <inertia ixx="0.0001552" ixy="1E-07" ixz="7.51E-05" iyy="0.0010657" iyz="-1E-07" izz="0.001098"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/right_ankle_roll_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="mesh/G1/right_ankle_roll_link.STL"/>
+      </geometry>
+    </collision>
+    <!-- <collision>
+      <origin xyz="-0.06 0.02 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.001"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin xyz="-0.06 -0.02 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.001"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin xyz="0.13 0.02 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.001"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin xyz="0.13 -0.02 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.001"/>
+      </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="40" velocity="53"/>
+  </joint>
+
+  <!-- Torso -->
+  <link name="torso_link">
+    <inertial>
+      <origin xyz="0.00197122283 0.00266902037 0.16936510960" rpy="0 0 0"/>
+      <mass value="6.33959811"/>
+      <inertia ixx="0.06037082109027" ixy="-0.00010168407055" ixz="0.00069845018092" iyy="0.04355153796551" iyz="0.00034366422728" izz="0.03339981866010"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/torso_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/torso_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="torso_joint" type="fixed">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="pelvis"/>
+    <child link="torso_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-2.618" upper="2.618" effort="88" velocity="32"/>
+  </joint>
+
+  <!-- Head -->
+  <link name="head_link">
+    <inertial>
+      <origin xyz="0.00138066852 0.00028430950 0.42034187824" rpy="0 0 0"/>
+      <mass value="1.17976522"/>
+      <inertia ixx="0.00543236042361" ixy="0.00000140137425" ixz="0.00034554752228" iyy="0.00552885306699" iyz="0.00001501216392" izz="0.00165378108136"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/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="mesh/G1/head_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="head_joint" type="fixed">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="torso_link"/>
+    <child link="head_link"/>
+  </joint>
+
+  <!-- L ARM -->
+  <link name="left_shoulder_pitch_link">
+    <inertial>
+      <origin xyz="-0.001431 0.048811 0.001304" rpy="0 0 0"/>
+      <mass value="0.713"/>
+      <inertia ixx="0.0004614" ixy="-9.3E-06" ixz="1E-05" iyy="0.0004146" iyz="5.5E-06" izz="0.0004416"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/left_shoulder_pitch_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/left_shoulder_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_shoulder_pitch_joint" type="fixed">
+    <origin xyz="-0.0025 0.10396 0.25928" rpy="0.27925 0 0"/>
+    <parent link="torso_link"/>
+    <child link="left_shoulder_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-2.9671" upper="2.7925" effort="21" velocity="53"/>
+  </joint>
+  <link name="left_shoulder_roll_link">
+    <inertial>
+      <origin xyz="-0.003415 0.006955 -0.064598" rpy="0 0 0"/>
+      <mass value="0.642"/>
+      <inertia ixx="0.0006159" ixy="0" ixz="-5.6E-06" iyy="0.0006835" iyz="2.1E-06" izz="0.000373"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/left_shoulder_roll_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <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="fixed">
+    <origin xyz="0 0.052 0" 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="21" velocity="53"/>
+  </joint>
+  <link name="left_shoulder_yaw_link">
+    <inertial>
+      <origin xyz="0.000375 -0.00444 -0.072374" rpy="0 0 0"/>
+      <mass value="0.713"/>
+      <inertia ixx="0.0009699" ixy="7.1E-06" ixz="7.8E-06" iyy="0.0009691" iyz="-4.49E-05" izz="0.0003826"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/left_shoulder_yaw_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/left_shoulder_yaw_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_shoulder_yaw_joint" type="fixed">
+    <origin xyz="-0.00354 0.0062424 -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="21" velocity="53"/>
+  </joint>
+  <link name="left_elbow_pitch_link">
+    <inertial>
+      <origin xyz="0.064497 0.002873 0" rpy="0 0 0"/>
+      <mass value="0.601"/>
+      <inertia ixx="0.0002845" ixy="8.06E-05" ixz="9E-09" iyy="0.0004647" iyz="5E-09" izz="0.0004712"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/left_elbow_pitch_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/left_elbow_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_elbow_pitch_joint" type="fixed">
+    <origin xyz="0 0.00189 -0.0855" rpy="0 0 0"/>
+    <parent link="left_shoulder_yaw_link"/>
+    <child link="left_elbow_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.2268" upper="3.4208" effort="21" velocity="53"/>
+  </joint>
+  <link name="left_elbow_roll_link">
+    <inertial>
+      <origin xyz="0.081811 0.001454 0.001" rpy="0 0 0"/>
+      <mass value="0.227"/>
+      <inertia ixx="0.0001093" ixy="-1.55E-05" ixz="-2.91E-05" iyy="0.000977" iyz="1E-06" izz="0.0009676"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/left_elbow_roll_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/left_elbow_roll_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_elbow_roll_joint" type="fixed">
+    <origin xyz="0.1 0 0" rpy="0 0 0"/>
+    <parent link="left_elbow_pitch_link"/>
+    <child link="left_elbow_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-2.0943" upper="2.0943" effort="21" velocity="53"/>
+  </joint>
+
+  <!-- R ARM -->
+  <link name="right_shoulder_pitch_link">
+    <inertial>
+      <origin xyz="-0.001431 -0.048811 0.001304" rpy="0 0 0"/>
+      <mass value="0.713"/>
+      <inertia ixx="0.0004614" ixy="9.3E-06" ixz="1E-05" iyy="0.0004146" iyz="-5.5E-06" izz="0.0004416"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/right_shoulder_pitch_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/right_shoulder_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_shoulder_pitch_joint" type="fixed">
+    <origin xyz="-0.0025 -0.10396 0.25928" rpy="-0.27925 0 0"/>
+    <parent link="torso_link"/>
+    <child link="right_shoulder_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-2.9671" upper="2.7925" effort="21" velocity="53"/>
+  </joint>
+  <link name="right_shoulder_roll_link">
+    <inertial>
+      <origin xyz="-0.003415 -0.006955 -0.064598" rpy="0 0 0"/>
+      <mass value="0.642"/>
+      <inertia ixx="0.0006159" ixy="0" ixz="-5.6E-06" iyy="0.0006835" iyz="-2.1E-06" izz="0.000373"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/right_shoulder_roll_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <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="fixed">
+    <origin xyz="0 -0.052 0" 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="21" velocity="53"/>
+  </joint>
+  <link name="right_shoulder_yaw_link">
+    <inertial>
+      <origin xyz="0.000375 0.00444 -0.072374" rpy="0 0 0"/>
+      <mass value="0.713"/>
+      <inertia ixx="0.0009699" ixy="-7.1E-06" ixz="7.8E-06" iyy="0.0009691" iyz="4.49E-05" izz="0.0003826"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/right_shoulder_yaw_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/right_shoulder_yaw_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_shoulder_yaw_joint" type="fixed">
+    <origin xyz="-0.00354 -0.0062424 -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="21" velocity="53"/>
+  </joint>
+  <link name="right_elbow_pitch_link">
+    <inertial>
+      <origin xyz="0.064497 -0.002873 0" rpy="0 0 0"/>
+      <mass value="0.601"/>
+      <inertia ixx="0.0002845" ixy="-8.06E-05" ixz="9E-09" iyy="0.0004647" iyz="-5E-09" izz="0.0004712"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/right_elbow_pitch_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/right_elbow_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_elbow_pitch_joint" type="fixed">
+    <origin xyz="0 -0.00189 -0.0855" rpy="0 0 0"/>
+    <parent link="right_shoulder_yaw_link"/>
+    <child link="right_elbow_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.2268" upper="3.4208" effort="21" velocity="53"/>
+  </joint>
+  <link name="right_elbow_roll_link">
+    <inertial>
+      <origin xyz="0.081811 -0.001454 0.001" rpy="0 0 0"/>
+      <mass value="0.227"/>
+      <inertia ixx="0.0001093" ixy="1.55E-05" ixz="-2.91E-05" iyy="0.000977" iyz="-1E-06" izz="0.0009676"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/right_elbow_roll_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/right_elbow_roll_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_elbow_roll_joint" type="fixed">
+    <origin xyz="0.1 0 0" rpy="0 0 0"/>
+    <parent link="right_elbow_pitch_link"/>
+    <child link="right_elbow_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-2.0943" upper="2.0943" effort="21" velocity="53"/>
+  </joint>
+
+  <!-- LOGO -->
+  <joint name="logo_joint" type="fixed">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="torso_link"/>
+    <child link="logo_link"/>
+  </joint>
+  <link name="logo_link">
+    <inertial>
+      <origin xyz="-0.00306556489 0.00005625032 0.24526459523" rpy="0 0 0"/>
+      <mass value="0.001"/>
+      <inertia ixx="0.00000089650105" ixy="0.00000007028325" ixz="0.00000002735427" iyy="0.00000644846110" iyz="0.00000000029738" izz="0.00000688611155"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/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="mesh/G1/logo_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- IMU -->
+  <link name="imu_link"></link>
+  <joint name="imu_joint" type="fixed">
+    <origin xyz="-0.04233868314 0.00166 0.152067" rpy="0 0 0"/>
+    <parent link="torso_link"/>
+    <child link="imu_link"/>
+  </joint>
+
+  <!-- Gripper -->
+  <joint name="left_palm_joint" type="fixed">
+    <origin xyz="0.120 0 0" rpy="0 0 0"/>
+    <parent link="left_elbow_roll_link"/>
+    <child link="left_palm_link"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+  <link name="left_palm_link">
+    <inertial>
+      <origin xyz="0.05578422502 0.00148509387 -0.00032670143" rpy="0 0 0"/>
+      <mass value="0.28126043"/>
+      <inertia ixx="0.00017638101047" ixy="-0.00000652171140" ixz="0.00000021301775" iyy="0.00031084250366" iyz="0.00000000708018" izz="0.00018939350451"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/left_palm_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/left_palm_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_zero_joint" type="fixed">
+    <origin xyz="0.050 0 0" rpy="0 0 0"/>
+    <parent link="left_palm_link"/>
+    <child link="left_zero_link"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="0.7" velocity="23" lower="-0.523598" upper="0.523598"/>
+  </joint>
+  <link name="left_zero_link">
+    <inertial>
+      <origin xyz="-0.02604664462 -0.00877511180 0.00001086048" rpy="0 0 0"/>
+      <mass value="0.05098927"/>
+      <inertia ixx="0.00000679821860" ixy="-0.00000098281498" ixz="-0.00000000406407" iyy="0.00000501631802" iyz="0.00000002833450" izz="0.00000868017156"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/left_zero_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <!-- <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/left_zero_link.STL"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="left_one_joint" type="fixed">
+    <origin xyz="-0.02652502957 -0.0188 -0.00005" rpy="0 0 0"/>
+    <parent link="left_zero_link"/>
+    <child link="left_one_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="-1" upper="1.2"/>
+  </joint>
+  <link name="left_one_link">
+    <inertial>
+      <origin xyz="-0.00181538145 -0.03270571529 -0.00018587117" rpy="0 0 0"/>
+      <mass value="0.04776204"/>
+      <inertia ixx="0.00000800634866" ixy="-0.00000020811350" ixz="-0.00000004687654" iyy="0.00000366333777" iyz="0.00000005133915" izz="0.00000785913677"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/left_one_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <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_two_joint" type="fixed">
+    <origin xyz="0 -0.0431 0" rpy="0 0 0"/>
+    <parent link="left_one_link"/>
+    <child link="left_two_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="0" upper="1.84"/>
+  </joint>
+  <link name="left_two_link">
+    <inertial>
+      <origin xyz="-0.00135257289 -0.02375910756 -0.00011966865" rpy="0 0 0"/>
+      <mass value="0.01385835"/>
+      <inertia ixx="0.00000254698095" ixy="-0.00000010086310" ixz="-0.00000000325710" iyy="0.00000094744556" iyz="0.00000003605469" izz="0.00000204288618"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/left_two_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/left_two_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_three_joint" type="fixed">
+    <origin xyz="0.085 0.004 0.02395" rpy="0 0 0"/>
+    <parent link="left_palm_link"/>
+    <child link="left_three_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="-1.84" upper="0.3"/>
+  </joint>
+  <link name="left_three_link">
+    <inertial>
+      <origin xyz="0.03625612321 -0.00157250459 0.00026989918" rpy="0 0 0"/>
+      <mass value="0.04884689"/>
+      <inertia ixx="0.00000383218469" ixy="0.00000003239984" ixz="0.00000001923377" iyy="0.00000890974599" iyz="-0.00000004429800" izz="0.00000865612355"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/left_three_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/left_three_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_four_joint" type="fixed">
+    <origin xyz="0.0471 -0.0036 0" rpy="0 0 0"/>
+    <parent link="left_three_link"/>
+    <child link="left_four_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="-1.84" upper="0"/>
+  </joint>
+  <link name="left_four_link">
+    <inertial>
+      <origin xyz="0.02375910756 0.00135257289 0.00011966865" rpy="0 0 0"/>
+      <mass value="0.01385835"/>
+      <inertia ixx="0.00000094744556" ixy="-0.00000010086310" ixz="0.00000003605469" iyy="0.00000254698095" iyz="-0.00000000325710" izz="0.00000204288618"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/left_four_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/left_four_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_five_joint" type="fixed">
+    <origin xyz="0.085 0.004 -0.02395" rpy="0 0 0"/>
+    <parent link="left_palm_link"/>
+    <child link="left_five_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="-1.84" upper="0.3"/>
+  </joint>
+  <link name="left_five_link">
+    <inertial>
+      <origin xyz="0.03625612321 -0.00157250459 0.00026989918" rpy="0 0 0"/>
+      <mass value="0.04884689"/>
+      <inertia ixx="0.00000383218469" ixy="0.00000003239984" ixz="0.00000001923377" iyy="0.00000890974599" iyz="-0.00000004429800" izz="0.00000865612355"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/left_five_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/left_five_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_six_joint" type="fixed">
+    <origin xyz="0.0471 -0.0036 0" rpy="0 0 0"/>
+    <parent link="left_five_link"/>
+    <child link="left_six_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="-1.84" upper="0"/>
+  </joint>
+  <link name="left_six_link">
+    <inertial>
+      <origin xyz="0.02375910756 0.00135257289 0.00011966865" rpy="0 0 0"/>
+      <mass value="0.01385835"/>
+      <inertia ixx="0.00000094744556" ixy="-0.00000010086310" ixz="0.00000003605469" iyy="0.00000254698095" iyz="-0.00000000325710" izz="0.00000204288618"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/left_six_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/left_six_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+
+
+  <joint name="right_palm_joint" type="fixed">
+    <origin xyz="0.120 0 0" rpy="0 0 0"/>
+    <parent link="right_elbow_roll_link"/>
+    <child link="right_palm_link"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+  <link name="right_palm_link">
+    <inertial>
+      <origin xyz="0.05578422502 -0.00148509387 -0.00032670143" rpy="0 0 0"/>
+      <mass value="0.28126043"/>
+      <inertia ixx="0.00017638101047" ixy="0.00000652171140" ixz="0.00000021301775" iyy="0.00031084250366" iyz="-0.00000000708018" izz="0.00018939350451"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/right_palm_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/right_palm_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_zero_joint" type="fixed">
+    <origin xyz="0.050 0 0" rpy="0 0 0"/>
+    <parent link="right_palm_link"/>
+    <child link="right_zero_link"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="0.7" velocity="23" lower="-0.523598" upper="0.523598"/>
+  </joint>
+  <link name="right_zero_link">
+    <inertial>
+      <origin xyz="-0.02604664462 0.00877511180 0.00001086048" rpy="0 0 0"/>
+      <mass value="0.05098927"/>
+      <inertia ixx="0.00000679821860" ixy="0.00000098281498" ixz="-0.00000000406407" iyy="0.00000501631802" iyz="-0.00000002833450" izz="0.00000868017156"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/right_zero_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <!-- <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/right_zero_link.STL"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="right_one_joint" type="fixed">
+    <origin xyz="-0.02652502957 0.0188 -0.00005" rpy="0 0 0"/>
+    <parent link="right_zero_link"/>
+    <child link="right_one_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="-1.2" upper="1"/>
+  </joint>
+  <link name="right_one_link">
+    <inertial>
+      <origin xyz="-0.00181538145 0.03270571529 -0.00018587117" rpy="0 0 0"/>
+      <mass value="0.04776204"/>
+      <inertia ixx="0.00000800634866" ixy="0.00000020811350" ixz="-0.00000004687654" iyy="0.00000366333777" iyz="-0.00000005133915" izz="0.00000785913677"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/right_one_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <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_two_joint" type="fixed">
+    <origin xyz="0 0.0431 0" rpy="0 0 0"/>
+    <parent link="right_one_link"/>
+    <child link="right_two_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="-1.84" upper="0"/>
+  </joint>
+  <link name="right_two_link">
+    <inertial>
+      <origin xyz="-0.00135257289 0.02375910756 -0.00011966865" rpy="0 0 0"/>
+      <mass value="0.01385835"/>
+      <inertia ixx="0.00000254698095" ixy="0.00000010086310" ixz="-0.00000000325710" iyy="0.00000094744556" iyz="-0.00000003605469" izz="0.00000204288618"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/right_two_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/right_two_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_three_joint" type="fixed">
+    <origin xyz="0.085 -0.004 0.02395" rpy="0 0 0"/>
+    <parent link="right_palm_link"/>
+    <child link="right_three_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="-0.3" upper="1.84"/>
+  </joint>
+  <link name="right_three_link">
+    <inertial>
+      <origin xyz="0.03625612321 0.00157250459 0.00026989918" rpy="0 0 0"/>
+      <mass value="0.04884689"/>
+      <inertia ixx="0.00000383218469" ixy="-0.00000003239984" ixz="0.00000001923377" iyy="0.00000890974599" iyz="0.00000004429800" izz="0.00000865612355"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/right_three_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/right_three_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_four_joint" type="fixed">
+    <origin xyz="0.0471 0.0036 0" rpy="0 0 0"/>
+    <parent link="right_three_link"/>
+    <child link="right_four_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="0" upper="1.84"/>
+  </joint>
+  <link name="right_four_link">
+    <inertial>
+      <origin xyz="0.02375910756 -0.00135257289 0.00011966865" rpy="0 0 0"/>
+      <mass value="0.01385835"/>
+      <inertia ixx="0.00000094744556" ixy="0.00000010086310" ixz="0.00000003605469" iyy="0.00000254698095" iyz="0.00000000325710" izz="0.00000204288618"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/right_four_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/right_four_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_five_joint" type="fixed">
+    <origin xyz="0.085 -0.004 -0.02395" rpy="0 0 0"/>
+    <parent link="right_palm_link"/>
+    <child link="right_five_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="-0.3" upper="1.84"/>
+  </joint>
+  <link name="right_five_link">
+    <inertial>
+      <origin xyz="0.03625612321 0.00157250459 0.00026989918" rpy="0 0 0"/>
+      <mass value="0.04884689"/>
+      <inertia ixx="0.00000383218469" ixy="-0.00000003239984" ixz="0.00000001923377" iyy="0.00000890974599" iyz="0.00000004429800" izz="0.00000865612355"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/right_five_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/right_five_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_six_joint" type="fixed">
+    <origin xyz="0.0471 0.0036 0" rpy="0 0 0"/>
+    <parent link="right_five_link"/>
+    <child link="right_six_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="0" upper="1.84"/>
+  </joint>
+  <link name="right_six_link">
+    <inertial>
+      <origin xyz="0.02375910756 -0.00135257289 0.00011966865" rpy="0 0 0"/>
+      <mass value="0.01385835"/>
+      <inertia ixx="0.00000094744556" ixy="0.00000010086310" ixz="0.00000003605469" iyy="0.00000254698095" iyz="0.00000000325710" izz="0.00000204288618"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/right_six_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="mesh/G1/right_six_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>