diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..7606081
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,5 @@
+__pycache__
+logs
+
+.idea
+.vscode
\ No newline at end of file
diff --git a/README.md b/README.md
index c58594e..3e5a781 100644
--- a/README.md
+++ b/README.md
@@ -1 +1,64 @@
-详细使用说明，请参考 https://support.unitree.com/home/zh/developer/rl_example
+# Unitree RL GYM
+
+TODO: 简介
+
+### Installation
+
+1. Create a new python virtual env with python 3.6, 3.7 or 3.8 (3.8 recommended)
+2. Install pytorch 1.10 with cuda-11.3:
+
+   ```
+   pip3 install torch==1.10.0+cu113 torchvision==0.11.1+cu113 torchaudio==0.10.0+cu113 -f https://download.pytorch.org/whl/cu113/torch_stable.html
+
+   ```
+3. Install Isaac Gym
+
+   - Download and install Isaac Gym Preview 4 from [https://developer.nvidia.com/isaac-gym](https://developer.nvidia.com/isaac-gym)
+   - `cd isaacgym/python && pip install -e .`
+   - Try running an example `cd examples && python 1080_balls_of_solitude.py`
+   - For troubleshooting check docs isaacgym/docs/index.html
+4. Install rsl_rl (PPO implementation)
+
+   - Clone [https://github.com/leggedrobotics/rsl_rl](https://github.com/leggedrobotics/rsl_rl)
+   - `cd rsl_rl && git checkout v1.0.2 && pip install -e .`
+
+### Usage
+
+1. Train:
+   `python legged_gym/scripts/train.py --task=go2`
+
+   * To run on CPU add following arguments: `--sim_device=cpu`, `--rl_device=cpu` (sim on CPU and rl on GPU is possible).
+   * To run headless (no rendering) add `--headless`.
+   * **Important** : To improve performance, once the training starts press `v` to stop the rendering. You can then enable it later to check the progress.
+   * The trained policy is saved in `logs/<experiment_name>/<date_time>_<run_name>/model_<iteration>.pt`. Where `<experiment_name>` and `<run_name>` are defined in the train config.
+   * The following command line arguments override the values set in the config files:
+   * --task TASK: Task name.
+   * --resume: Resume training from a checkpoint
+   * --experiment_name EXPERIMENT_NAME: Name of the experiment to run or load.
+   * --run_name RUN_NAME: Name of the run.
+   * --load_run LOAD_RUN: Name of the run to load when resume=True. If -1: will load the last run.
+   * --checkpoint CHECKPOINT: Saved model checkpoint number. If -1: will load the last checkpoint.
+   * --num_envs NUM_ENVS: Number of environments to create.
+   * --seed SEED: Random seed.
+   * --max_iterations MAX_ITERATIONS: Maximum number of training iterations.
+2. Play:`python legged_gym/scripts/play.py --task=go2`
+    * By default, the loaded policy is the last model of the last run of the experiment folder.
+
+    * Other runs/model iteration can be selected by setting `load_run` and `checkpoint` in the train config.
+
+### Robots Demo
+
+1. Go2
+
+
+
+2. H1
+
+
+
+3. H1-2
+
+https://github.com/user-attachments/assets/a937e9c4-fe91-4240-88ea-d83b0160cad5
+
+4. G1
+
diff --git a/legged_gym/envs/__init__.py b/legged_gym/envs/__init__.py
index f87d9b2..fcad6bf 100644
--- a/legged_gym/envs/__init__.py
+++ b/legged_gym/envs/__init__.py
@@ -1,12 +1,14 @@
-
 from legged_gym import LEGGED_GYM_ROOT_DIR, LEGGED_GYM_ENVS_DIR
 
 from legged_gym.envs.go2.go2_config import GO2RoughCfg, GO2RoughCfgPPO
 from legged_gym.envs.h1.h1_config import H1RoughCfg, H1RoughCfgPPO
+from legged_gym.envs.h1_2.h1_2_config import H1_2RoughCfg, H1_2RoughCfgPPO
+from legged_gym.envs.g1.g1_config import G1RoughCfg, G1RoughCfgPPO
 from .base.legged_robot import LeggedRobot
 
 from legged_gym.utils.task_registry import task_registry
+
 task_registry.register( "go2", LeggedRobot, GO2RoughCfg(), GO2RoughCfgPPO())
 task_registry.register( "h1", LeggedRobot, H1RoughCfg(), H1RoughCfgPPO())
-
-
+task_registry.register( "h1_2", LeggedRobot, H1_2RoughCfg(), H1_2RoughCfgPPO())
+task_registry.register( "g1", LeggedRobot, G1RoughCfg(), G1RoughCfgPPO())
diff --git a/legged_gym/envs/g1/g1_config.py b/legged_gym/envs/g1/g1_config.py
new file mode 100644
index 0000000..e21f852
--- /dev/null
+++ b/legged_gym/envs/g1/g1_config.py
@@ -0,0 +1,83 @@
+from legged_gym.envs.base.legged_robot_config import LeggedRobotCfg, LeggedRobotCfgPPO
+
+class G1RoughCfg( LeggedRobotCfg ):
+    class init_state( LeggedRobotCfg.init_state ):
+        pos = [0.0, 0.0, 0.8] # x,y,z [m]
+        default_joint_angles = { # = target angles [rad] when action = 0.0
+           'left_hip_yaw_joint' : 0. ,   
+           'left_hip_roll_joint' : 0,               
+           'left_hip_pitch_joint' : -0.1,         
+           'left_knee_joint' : 0.3,       
+           'left_ankle_pitch_joint' : -0.2,     
+           'left_ankle_roll_joint' : 0,     
+           'right_hip_yaw_joint' : 0., 
+           'right_hip_roll_joint' : 0, 
+           'right_hip_pitch_joint' : -0.1,                                       
+           'right_knee_joint' : 0.3,                                             
+           'right_ankle_pitch_joint': -0.2,                              
+           'right_ankle_roll_joint' : 0,       
+           'torso_joint' : 0.
+        }
+    
+    class env(LeggedRobotCfg.env):
+        num_observations = 48
+        num_actions = 12
+      
+
+    class control( LeggedRobotCfg.control ):
+        # PD Drive parameters:
+        control_type = 'P'
+          # PD Drive parameters:
+        stiffness = {'hip_yaw': 150,
+                     'hip_roll': 150,
+                     'hip_pitch': 150,
+                     'knee': 300,
+                     'ankle': 40,
+                     }  # [N*m/rad]
+        damping = {  'hip_yaw': 2,
+                     'hip_roll': 2,
+                     'hip_pitch': 2,
+                     'knee': 4,
+                     'ankle': 2,
+                     }  # [N*m/rad]  # [N*m*s/rad]
+        # action scale: target angle = actionScale * action + defaultAngle
+        action_scale = 0.25
+        # decimation: Number of control action updates @ sim DT per policy DT
+        decimation = 4
+
+    class asset( LeggedRobotCfg.asset ):
+        file = '{LEGGED_GYM_ROOT_DIR}/resources/robots/g1/urdf/g1.urdf'
+        name = "g1"
+        foot_name = "ankle_roll"
+        penalize_contacts_on = ["hip", "knee"]
+        terminate_after_contacts_on = ["torso"]
+        self_collisions = 1 # 1 to disable, 0 to enable...bitwise filter
+        flip_visual_attachments = False
+  
+    class rewards( LeggedRobotCfg.rewards ):
+        soft_dof_pos_limit = 0.9
+        base_height_target = 0.728
+        class scales( LeggedRobotCfg.rewards.scales ):
+            tracking_lin_vel = 1.0
+            tracking_ang_vel = 0.5
+            lin_vel_z = -2.0
+            ang_vel_xy = -0.05
+            orientation = -1.0
+            base_height = -10.0
+            dof_acc = -2.5e-8
+            feet_air_time = 1.0
+            collision = 0.0
+            action_rate = -0.01
+            # torques = -0.0001
+            dof_pos_limits = -5.0
+
+class G1RoughCfgPPO( LeggedRobotCfgPPO ):
+    class policy:
+        init_noise_std = 0.8
+    class algorithm( LeggedRobotCfgPPO.algorithm ):
+        entropy_coef = 0.01
+    class runner( LeggedRobotCfgPPO.runner ):
+        run_name = ''
+        experiment_name = 'g1'
+
+  
diff --git a/legged_gym/envs/h1/__pycache__/h1_config.cpython-38.pyc b/legged_gym/envs/h1/__pycache__/h1_config.cpython-38.pyc
deleted file mode 100644
index a8ed8ab..0000000
Binary files a/legged_gym/envs/h1/__pycache__/h1_config.cpython-38.pyc and /dev/null differ
diff --git a/legged_gym/envs/h1_2/README.md b/legged_gym/envs/h1_2/README.md
new file mode 100644
index 0000000..4c491ff
--- /dev/null
+++ b/legged_gym/envs/h1_2/README.md
@@ -0,0 +1,25 @@
+# H1_2 RL Example (Preview)
+
+## Simplified URDF
+
+This task utilizes a simplified version of URDF. We fix some joints and ignore most of the collisions.
+
+### Fixed Joints
+
+We fix all the joints in the hands, wrists and the elbow roll joints, since those joints have very limited effect on the whole body dynamics and are commonly controlled by other controllers.
+
+### Collisions
+
+We only keep the collision of foot roll links, knee links and the base. Early termination is majorly checked by the angular position of the base.
+
+## Dynamics
+
+Free "light" end effectors can lead to unstable simulation. Thus please be carefull with the control parameters for the joints that may affect such end effectors.
+
+## Results
+
+https://github.com/user-attachments/assets/a937e9c4-fe91-4240-88ea-d83b0160cad5
+
+## Preview Stage
+
+**The reward functions are not well tuned and cannot produce satisfactory results at the current stage. A feasible version is comming soon.**
diff --git a/legged_gym/envs/h1_2/h1_2_config.py b/legged_gym/envs/h1_2/h1_2_config.py
new file mode 100644
index 0000000..0e498d5
--- /dev/null
+++ b/legged_gym/envs/h1_2/h1_2_config.py
@@ -0,0 +1,121 @@
+from legged_gym.envs.base.legged_robot_config import LeggedRobotCfg, LeggedRobotCfgPPO
+
+
+class H1_2RoughCfg(LeggedRobotCfg):
+
+    class init_state(LeggedRobotCfg.init_state):
+        pos = [0.0, 0.0, 1.0]  # x,y,z [m]
+        default_joint_angles = { # = target angles [rad] when action = 0.0
+            'left_hip_yaw_joint': 0,
+            'left_hip_roll_joint': 0,
+            'left_hip_pitch_joint': -0.6,
+            'left_knee_joint': 1.2,
+            'left_ankle_pitch_joint': -0.6,
+            'left_ankle_roll_joint': 0.0,
+
+            'right_hip_yaw_joint': 0,
+            'right_hip_roll_joint': 0,
+            'right_hip_pitch_joint': -0.6,
+            'right_knee_joint': 1.2,
+            'right_ankle_pitch_joint': -0.6,
+            'right_ankle_roll_joint': 0.0,
+
+            'torso_joint': 0,
+
+            'left_shoulder_pitch_joint': 0.4,
+            'left_shoulder_roll_joint': 0,
+            'left_shoulder_yaw_joint': 0,
+            'left_elbow_pitch_joint': 0.3,
+
+            'right_shoulder_pitch_joint': 0.4,
+            'right_shoulder_roll_joint': 0,
+            'right_shoulder_yaw_joint': 0,
+            'right_elbow_pitch_joint': 0.3,
+        }
+
+    class env(LeggedRobotCfg.env):
+        num_actions = 21
+        num_observations = 12 + num_actions * 3
+        num_envs = 8192
+
+    class control(LeggedRobotCfg.control):
+        # PD Drive parameters:
+        control_type = 'P'
+        # PD Drive parameters:
+        stiffness = {
+            'hip_yaw_joint': 200.,
+            'hip_roll_joint': 200.,
+            'hip_pitch_joint': 200.,
+            'knee_joint': 300.,
+            'ankle_pitch_joint': 60.,
+            'ankle_roll_joint': 40.,
+            'torso_joint': 600.,
+            'shoulder_pitch_joint': 80.,
+            'shoulder_roll_joint': 80.,
+            'shoulder_yaw_joint': 40.,
+            'elbow_pitch_joint': 60.,
+        }  # [N*m/rad]
+        damping = {
+            'hip_yaw_joint': 5.0,
+            'hip_roll_joint': 5.0,
+            'hip_pitch_joint': 5.0,
+            'knee_joint': 7.5,
+            'ankle_pitch_joint': 1.0,
+            'ankle_roll_joint': 0.3,
+            'torso_joint': 15.0,
+            'shoulder_pitch_joint': 2.0,
+            'shoulder_roll_joint': 2.0,
+            'shoulder_yaw_joint': 1.0,
+            'elbow_pitch_joint': 1.0,
+        }  # [N*m/rad]  # [N*m*s/rad]
+        # action scale: target angle = actionScale * action + defaultAngle
+        action_scale = 0.25
+        # decimation: Number of control action updates @ sim DT per policy DT
+        decimation = 10
+
+    class asset(LeggedRobotCfg.asset):
+        file = '{LEGGED_GYM_ROOT_DIR}/resources/robots/h1_2/h1_2_simplified.urdf'
+        name = "h1_2"
+        foot_name = "ankle_roll"
+        penalize_contacts_on = []
+        terminate_after_contacts_on = []
+        self_collisions = 1  # 1 to disable, 0 to enable...bitwise filter
+        flip_visual_attachments = False
+        armature = 6e-4  # stablize semi-euler integration for end effectors
+
+    class sim(LeggedRobotCfg.sim):
+        dt = 0.002  # stablize semi-euler integration for end effectors
+
+    class rewards(LeggedRobotCfg.rewards):
+        soft_dof_pos_limit = 0.9
+        base_height_target = 0.98
+
+        class scales(LeggedRobotCfg.rewards.scales):
+            tracking_lin_vel = 1.0
+            tracking_ang_vel = 0.5
+            lin_vel_z = -0.2
+            ang_vel_xy = -0.1
+            orientation = -0.1
+            base_height = -10.0
+            dof_acc = -3.5e-9
+            feet_air_time = 1.0
+            collision = 0.0
+            action_rate = -0.001
+            torques = 0.0
+            dof_pos_limits = -1.0
+        
+        only_positive_rewards = False # if true negative total rewards are clipped at zero (avoids early termination problems)
+    
+    class normalization(LeggedRobotCfg.normalization):
+        clip_actions = 10.0
+
+
+class H1_2RoughCfgPPO(LeggedRobotCfgPPO):
+
+    class policy(LeggedRobotCfgPPO.policy):
+        init_noise_std = 0.3
+        activation = 'tanh'
+
+    class runner(LeggedRobotCfgPPO.runner):
+        run_name = ''
+        experiment_name = 'h1_2'
diff --git a/legged_gym/scripts/play.py b/legged_gym/scripts/play.py
index 28aec6b..03ec2d7 100644
--- a/legged_gym/scripts/play.py
+++ b/legged_gym/scripts/play.py
@@ -1,7 +1,8 @@
-import sys
-sys.path.append("/home/unitree/unitree_rl_gym")
-from legged_gym import LEGGED_GYM_ROOT_DIR
 import os
+unitree_rl_gym_path = os.path.abspath(__file__ + "../../../../")
+import sys
+sys.path.append(unitree_rl_gym_path)
+from legged_gym import LEGGED_GYM_ROOT_DIR
 
 import isaacgym
 from legged_gym.envs import *
diff --git a/legged_gym/scripts/train.py b/legged_gym/scripts/train.py
index aa904e8..5d84855 100644
--- a/legged_gym/scripts/train.py
+++ b/legged_gym/scripts/train.py
@@ -1,8 +1,9 @@
-import numpy as np
 import os
+unitree_rl_gym_path = os.path.abspath(__file__ + "../../../../")
+import numpy as np
 from datetime import datetime
 import sys
-sys.path.append("/home/unitree/unitree_rl_gym/")
+sys.path.append(unitree_rl_gym_path)
 
 import isaacgym
 from legged_gym.envs import *
diff --git a/resources/robots/g1/meshes/head_link.STL b/resources/robots/g1/meshes/head_link.STL
new file mode 100644
index 0000000..455274f
Binary files /dev/null and b/resources/robots/g1/meshes/head_link.STL differ
diff --git a/resources/robots/g1/meshes/left_ankle_pitch_link.STL b/resources/robots/g1/meshes/left_ankle_pitch_link.STL
new file mode 100644
index 0000000..c291f60
Binary files /dev/null and b/resources/robots/g1/meshes/left_ankle_pitch_link.STL differ
diff --git a/resources/robots/g1/meshes/left_ankle_roll_link.STL b/resources/robots/g1/meshes/left_ankle_roll_link.STL
new file mode 100644
index 0000000..f2b6ae7
Binary files /dev/null and b/resources/robots/g1/meshes/left_ankle_roll_link.STL differ
diff --git a/resources/robots/g1/meshes/left_elbow_pitch_link.STL b/resources/robots/g1/meshes/left_elbow_pitch_link.STL
new file mode 100644
index 0000000..5ba2fde
Binary files /dev/null and b/resources/robots/g1/meshes/left_elbow_pitch_link.STL differ
diff --git a/resources/robots/g1/meshes/left_elbow_roll_link.STL b/resources/robots/g1/meshes/left_elbow_roll_link.STL
new file mode 100644
index 0000000..9741ac0
Binary files /dev/null and b/resources/robots/g1/meshes/left_elbow_roll_link.STL differ
diff --git a/resources/robots/g1/meshes/left_five_link.STL b/resources/robots/g1/meshes/left_five_link.STL
new file mode 100644
index 0000000..cc35d51
Binary files /dev/null and b/resources/robots/g1/meshes/left_five_link.STL differ
diff --git a/resources/robots/g1/meshes/left_four_link.STL b/resources/robots/g1/meshes/left_four_link.STL
new file mode 100644
index 0000000..6392be7
Binary files /dev/null and b/resources/robots/g1/meshes/left_four_link.STL differ
diff --git a/resources/robots/g1/meshes/left_hip_pitch_link.STL b/resources/robots/g1/meshes/left_hip_pitch_link.STL
new file mode 100644
index 0000000..1e56afc
Binary files /dev/null and b/resources/robots/g1/meshes/left_hip_pitch_link.STL differ
diff --git a/resources/robots/g1/meshes/left_hip_roll_link.STL b/resources/robots/g1/meshes/left_hip_roll_link.STL
new file mode 100644
index 0000000..d1df2f4
Binary files /dev/null and b/resources/robots/g1/meshes/left_hip_roll_link.STL differ
diff --git a/resources/robots/g1/meshes/left_hip_yaw_link.STL b/resources/robots/g1/meshes/left_hip_yaw_link.STL
new file mode 100644
index 0000000..deadd3c
Binary files /dev/null and b/resources/robots/g1/meshes/left_hip_yaw_link.STL differ
diff --git a/resources/robots/g1/meshes/left_knee_link.STL b/resources/robots/g1/meshes/left_knee_link.STL
new file mode 100644
index 0000000..e0095a4
Binary files /dev/null and b/resources/robots/g1/meshes/left_knee_link.STL differ
diff --git a/resources/robots/g1/meshes/left_one_link.STL b/resources/robots/g1/meshes/left_one_link.STL
new file mode 100644
index 0000000..6ee97bd
Binary files /dev/null and b/resources/robots/g1/meshes/left_one_link.STL differ
diff --git a/resources/robots/g1/meshes/left_palm_link.STL b/resources/robots/g1/meshes/left_palm_link.STL
new file mode 100644
index 0000000..524a0e2
Binary files /dev/null and b/resources/robots/g1/meshes/left_palm_link.STL differ
diff --git a/resources/robots/g1/meshes/left_shoulder_pitch_link.STL b/resources/robots/g1/meshes/left_shoulder_pitch_link.STL
new file mode 100644
index 0000000..e54d656
Binary files /dev/null and b/resources/robots/g1/meshes/left_shoulder_pitch_link.STL differ
diff --git a/resources/robots/g1/meshes/left_shoulder_roll_link.STL b/resources/robots/g1/meshes/left_shoulder_roll_link.STL
new file mode 100644
index 0000000..75dc809
Binary files /dev/null and b/resources/robots/g1/meshes/left_shoulder_roll_link.STL differ
diff --git a/resources/robots/g1/meshes/left_shoulder_yaw_link.STL b/resources/robots/g1/meshes/left_shoulder_yaw_link.STL
new file mode 100644
index 0000000..4e5a7c9
Binary files /dev/null and b/resources/robots/g1/meshes/left_shoulder_yaw_link.STL differ
diff --git a/resources/robots/g1/meshes/left_six_link.STL b/resources/robots/g1/meshes/left_six_link.STL
new file mode 100644
index 0000000..2739846
Binary files /dev/null and b/resources/robots/g1/meshes/left_six_link.STL differ
diff --git a/resources/robots/g1/meshes/left_three_link.STL b/resources/robots/g1/meshes/left_three_link.STL
new file mode 100644
index 0000000..cc35d51
Binary files /dev/null and b/resources/robots/g1/meshes/left_three_link.STL differ
diff --git a/resources/robots/g1/meshes/left_two_link.STL b/resources/robots/g1/meshes/left_two_link.STL
new file mode 100644
index 0000000..b85b645
Binary files /dev/null and b/resources/robots/g1/meshes/left_two_link.STL differ
diff --git a/resources/robots/g1/meshes/left_zero_link.STL b/resources/robots/g1/meshes/left_zero_link.STL
new file mode 100644
index 0000000..f50147a
Binary files /dev/null and b/resources/robots/g1/meshes/left_zero_link.STL differ
diff --git a/resources/robots/g1/meshes/logo_link.STL b/resources/robots/g1/meshes/logo_link.STL
new file mode 100644
index 0000000..20f7bba
Binary files /dev/null and b/resources/robots/g1/meshes/logo_link.STL differ
diff --git a/resources/robots/g1/meshes/pelvis.STL b/resources/robots/g1/meshes/pelvis.STL
new file mode 100644
index 0000000..5877dee
Binary files /dev/null and b/resources/robots/g1/meshes/pelvis.STL differ
diff --git a/resources/robots/g1/meshes/pelvis_contour_link.STL b/resources/robots/g1/meshes/pelvis_contour_link.STL
new file mode 100644
index 0000000..72fa4ac
Binary files /dev/null and b/resources/robots/g1/meshes/pelvis_contour_link.STL differ
diff --git a/resources/robots/g1/meshes/right_ankle_pitch_link.STL b/resources/robots/g1/meshes/right_ankle_pitch_link.STL
new file mode 100644
index 0000000..36e3a64
Binary files /dev/null and b/resources/robots/g1/meshes/right_ankle_pitch_link.STL differ
diff --git a/resources/robots/g1/meshes/right_ankle_roll_link.STL b/resources/robots/g1/meshes/right_ankle_roll_link.STL
new file mode 100644
index 0000000..d0a2657
Binary files /dev/null and b/resources/robots/g1/meshes/right_ankle_roll_link.STL differ
diff --git a/resources/robots/g1/meshes/right_elbow_pitch_link.STL b/resources/robots/g1/meshes/right_elbow_pitch_link.STL
new file mode 100644
index 0000000..a62a422
Binary files /dev/null and b/resources/robots/g1/meshes/right_elbow_pitch_link.STL differ
diff --git a/resources/robots/g1/meshes/right_elbow_roll_link.STL b/resources/robots/g1/meshes/right_elbow_roll_link.STL
new file mode 100644
index 0000000..6010e8b
Binary files /dev/null and b/resources/robots/g1/meshes/right_elbow_roll_link.STL differ
diff --git a/resources/robots/g1/meshes/right_five_link.STL b/resources/robots/g1/meshes/right_five_link.STL
new file mode 100644
index 0000000..37f9dc2
Binary files /dev/null and b/resources/robots/g1/meshes/right_five_link.STL differ
diff --git a/resources/robots/g1/meshes/right_four_link.STL b/resources/robots/g1/meshes/right_four_link.STL
new file mode 100644
index 0000000..be81a79
Binary files /dev/null and b/resources/robots/g1/meshes/right_four_link.STL differ
diff --git a/resources/robots/g1/meshes/right_hip_pitch_link.STL b/resources/robots/g1/meshes/right_hip_pitch_link.STL
new file mode 100644
index 0000000..66b47ea
Binary files /dev/null and b/resources/robots/g1/meshes/right_hip_pitch_link.STL differ
diff --git a/resources/robots/g1/meshes/right_hip_roll_link.STL b/resources/robots/g1/meshes/right_hip_roll_link.STL
new file mode 100644
index 0000000..021dd7f
Binary files /dev/null and b/resources/robots/g1/meshes/right_hip_roll_link.STL differ
diff --git a/resources/robots/g1/meshes/right_hip_yaw_link.STL b/resources/robots/g1/meshes/right_hip_yaw_link.STL
new file mode 100644
index 0000000..330e55a
Binary files /dev/null and b/resources/robots/g1/meshes/right_hip_yaw_link.STL differ
diff --git a/resources/robots/g1/meshes/right_knee_link.STL b/resources/robots/g1/meshes/right_knee_link.STL
new file mode 100644
index 0000000..7be6860
Binary files /dev/null and b/resources/robots/g1/meshes/right_knee_link.STL differ
diff --git a/resources/robots/g1/meshes/right_one_link.STL b/resources/robots/g1/meshes/right_one_link.STL
new file mode 100644
index 0000000..93e103a
Binary files /dev/null and b/resources/robots/g1/meshes/right_one_link.STL differ
diff --git a/resources/robots/g1/meshes/right_palm_link.STL b/resources/robots/g1/meshes/right_palm_link.STL
new file mode 100644
index 0000000..7cd39cd
Binary files /dev/null and b/resources/robots/g1/meshes/right_palm_link.STL differ
diff --git a/resources/robots/g1/meshes/right_shoulder_pitch_link.STL b/resources/robots/g1/meshes/right_shoulder_pitch_link.STL
new file mode 100644
index 0000000..7e1d01d
Binary files /dev/null and b/resources/robots/g1/meshes/right_shoulder_pitch_link.STL differ
diff --git a/resources/robots/g1/meshes/right_shoulder_roll_link.STL b/resources/robots/g1/meshes/right_shoulder_roll_link.STL
new file mode 100644
index 0000000..f653871
Binary files /dev/null and b/resources/robots/g1/meshes/right_shoulder_roll_link.STL differ
diff --git a/resources/robots/g1/meshes/right_shoulder_yaw_link.STL b/resources/robots/g1/meshes/right_shoulder_yaw_link.STL
new file mode 100644
index 0000000..01aa95a
Binary files /dev/null and b/resources/robots/g1/meshes/right_shoulder_yaw_link.STL differ
diff --git a/resources/robots/g1/meshes/right_six_link.STL b/resources/robots/g1/meshes/right_six_link.STL
new file mode 100644
index 0000000..be81a79
Binary files /dev/null and b/resources/robots/g1/meshes/right_six_link.STL differ
diff --git a/resources/robots/g1/meshes/right_three_link.STL b/resources/robots/g1/meshes/right_three_link.STL
new file mode 100644
index 0000000..37f9dc2
Binary files /dev/null and b/resources/robots/g1/meshes/right_three_link.STL differ
diff --git a/resources/robots/g1/meshes/right_two_link.STL b/resources/robots/g1/meshes/right_two_link.STL
new file mode 100644
index 0000000..1af670c
Binary files /dev/null and b/resources/robots/g1/meshes/right_two_link.STL differ
diff --git a/resources/robots/g1/meshes/right_zero_link.STL b/resources/robots/g1/meshes/right_zero_link.STL
new file mode 100644
index 0000000..4fd999b
Binary files /dev/null and b/resources/robots/g1/meshes/right_zero_link.STL differ
diff --git a/resources/robots/g1/meshes/torso_link.STL b/resources/robots/g1/meshes/torso_link.STL
new file mode 100644
index 0000000..b6a6d12
Binary files /dev/null and b/resources/robots/g1/meshes/torso_link.STL differ
diff --git a/resources/robots/g1/urdf/g1.urdf b/resources/robots/g1/urdf/g1.urdf
new file mode 100644
index 0000000..ce3a36a
--- /dev/null
+++ b/resources/robots/g1/urdf/g1.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="../meshes/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="../meshes/pelvis.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="pelvis_contour_link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/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="../meshes/left_hip_pitch_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/left_hip_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_hip_pitch_joint" type="revolute">
+    <origin xyz="0 0.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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/left_ankle_roll_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/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="../meshes/right_hip_pitch_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/right_hip_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_hip_pitch_joint" type="revolute">
+    <origin xyz="0 -0.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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/right_ankle_roll_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/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="../meshes/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="../meshes/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="../meshes/head_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/head_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="head_joint" type="fixed">
+    <origin xyz="0 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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/logo_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/logo_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- 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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/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="../meshes/right_six_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>
\ No newline at end of file
diff --git a/resources/robots/h1_2/README.md b/resources/robots/h1_2/README.md
new file mode 100644
index 0000000..b9c139b
--- /dev/null
+++ b/resources/robots/h1_2/README.md
@@ -0,0 +1,79 @@
+# Unitree H1 Description (URDF & MJCF)
+
+## Overview
+
+This package includes a streamlined robot description (URDF & MJCF) for the [Unitree H1](https://www.unitree.com/h1/), developed by [Unitree Robotics](https://www.unitree.com/).
+
+<p align="center">
+  <img src="h1_5.png" width="500"/>
+</p>
+
+Unitree H1 have 51 DOFs:
+
+```text
+root [⚓] => /pelvis/
+    left_hip_yaw_joint [⚙+Z] => /left_hip_yaw_link/
+        left_hip_pitch_joint [⚙+Y] => /left_hip_pitch_link/
+            left_hip_roll_joint [⚙+X] => /left_hip_roll_link/
+                left_knee_joint [⚙+Y] => /left_knee_link/
+                    left_ankle_pitch_joint [⚙+Y] => /left_ankle_pitch_link/
+                        left_ankle_roll_joint [⚙+X] => /left_ankle_roll_link/
+    right_hip_yaw_joint [⚙+Z] => /right_hip_yaw_link/
+        right_hip_pitch_joint [⚙+Y] => /right_hip_pitch_link/
+            right_hip_roll_joint [⚙+X] => /right_hip_roll_link/
+                right_knee_joint [⚙+Y] => /right_knee_link/
+                    right_ankle_pitch_joint [⚙+Y] => /right_ankle_pitch_link/
+                        right_ankle_roll_joint [⚙+X] => /right_ankle_roll_link/
+    torso_joint [⚙+Z] => /torso_link/
+        left_shoulder_pitch_joint [⚙+Y] => /left_shoulder_pitch_link/
+            left_shoulder_roll_joint [⚙+X] => /left_shoulder_roll_link/
+                left_shoulder_yaw_joint [⚙+Z] => /left_shoulder_yaw_link/
+                    left_elbow_pitch_joint [⚙+Y] => /left_elbow_pitch_link/
+                        left_elbow_roll_joint [⚙+X] => /left_elbow_roll_link/
+                            left_wrist_pitch_joint [⚙+Y] => /left_wrist_pitch_link/
+                                left_wrist_yaw_joint [⚙+Z] => /left_wrist_yaw_link/
+                                    L_base_link_joint [⚓] => /L_hand_base_link/
+                                        L_thumb_proximal_yaw_joint [⚙+Z] => /L_thumb_proximal_base/
+                                            L_thumb_proximal_pitch_joint [⚙-Z] => /L_thumb_proximal/
+                                                L_thumb_intermediate_joint [⚙-Z] => /L_thumb_intermediate/
+                                                    L_thumb_distal_joint [⚙-Z] => /L_thumb_distal/
+                                        L_index_proximal_joint [⚙-Z] => /L_index_proximal/
+                                            L_index_intermediate_joint [⚙-Z] => /L_index_intermediate/
+                                        L_middle_proximal_joint [⚙-Z] => /L_middle_proximal/
+                                            L_middle_intermediate_joint [⚙-Z] => /L_middle_intermediate/
+                                        L_ring_proximal_joint [⚙-Z] => /L_ring_proximal/
+                                            L_ring_intermediate_joint [⚙-Z] => /L_ring_intermediate/
+                                        L_pinky_proximal_joint [⚙-Z] => /L_pinky_proximal/
+                                            L_pinky_intermediate_joint [⚙-Z] => /L_pinky_intermediate/
+        right_shoulder_pitch_joint [⚙+Y] => /right_shoulder_pitch_link/
+            right_shoulder_roll_joint [⚙+X] => /right_shoulder_roll_link/
+                right_shoulder_yaw_joint [⚙+Z] => /right_shoulder_yaw_link/
+                    right_elbow_pitch_joint [⚙+Y] => /right_elbow_pitch_link/
+                        right_elbow_roll_joint [⚙+X] => /right_elbow_roll_link/
+                            right_wrist_pitch_joint [⚙+Y] => /right_wrist_pitch_link/
+                                right_wrist_yaw_joint [⚙+Z] => /right_wrist_yaw_link/
+                                    R_base_link_joint [⚓] => /R_hand_base_link/
+                                        R_thumb_proximal_yaw_joint [⚙-Z] => /R_thumb_proximal_base/
+                                            R_thumb_proximal_pitch_joint [⚙+Z] => /R_thumb_proximal/
+                                                R_thumb_intermediate_joint [⚙+Z] => /R_thumb_intermediate/
+                                                    R_thumb_distal_joint [⚙+Z] => /R_thumb_distal/
+                                        R_index_proximal_joint [⚙+Z] => /R_index_proximal/
+                                            R_index_intermediate_joint [⚙+Z] => /R_index_intermediate/
+                                        R_middle_proximal_joint [⚙+Z] => /R_middle_proximal/
+                                            R_middle_intermediate_joint [⚙+Z] => /R_middle_intermediate/
+                                        R_ring_proximal_joint [⚙+Z] => /R_ring_proximal/
+                                            R_ring_intermediate_joint [⚙+Z] => /R_ring_intermediate/
+                                        R_pinky_proximal_joint [⚙+Z] => /R_pinky_proximal/
+                                            R_pinky_intermediate_joint [⚙+Z] => /R_pinky_intermediate/
+```
+
+## Visulization with [MuJoCo](https://github.com/google-deepmind/mujoco)
+
+1. Open MuJoCo Viewer
+
+   ```bash
+   pip install mujoco
+   python -m mujoco.viewer
+   ```
+
+2. Drag and drop the MJCF model file (`scene.xml`) to the MuJoCo Viewer.
diff --git a/resources/robots/h1_2/h1_2.png b/resources/robots/h1_2/h1_2.png
new file mode 100644
index 0000000..89efec8
Binary files /dev/null and b/resources/robots/h1_2/h1_2.png differ
diff --git a/resources/robots/h1_2/h1_2.urdf b/resources/robots/h1_2/h1_2.urdf
new file mode 100644
index 0000000..1b5a418
--- /dev/null
+++ b/resources/robots/h1_2/h1_2.urdf
@@ -0,0 +1,1623 @@
+<robot name="h1_5">
+
+  <mujoco>
+    <compiler meshdir="meshes" discardvisual="false"/>
+  </mujoco>
+
+  <!-- [CAUTION] uncomment when convert to mujoco -->
+  <!-- <link name="world"></link>
+  <joint name="floating_base_joint" type="floating">
+    <parent link="world"/>
+    <child link="pelvis"/>
+  </joint> -->
+
+  <link name="pelvis">
+    <inertial>
+      <origin xyz="-0.0004 3.7E-05 -0.046864" rpy="0 0 0"/>
+      <mass value="5.983"/>
+      <inertia ixx="49168.411E-06" ixy="-19.869E-06" ixz="-48.460E-06" iyy="9025.844E-06" iyz="3.431E-06" izz="53155.891E-06"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/pelvis.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.05"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- L Leg -->
+  <link name="left_hip_yaw_link">
+    <inertial>
+      <origin xyz="0 -0.026197 0.006647" rpy="0 0 0"/>
+      <mass value="2.829"/>
+      <inertia ixx="4553.605E-06" ixy="0.005E-06" ixz="0.817E-06" iyy="5688.730E-06" iyz="-345.157E-06" izz="3548.907E-06"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_hip_yaw_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0.02 0 0" rpy="0 1.5707963267948966192313216916398 0"/>
+      <geometry>
+        <cylinder radius="0.01" length="0.02"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_hip_yaw_joint" type="revolute">
+    <origin xyz="0 0.0875 -0.1632" rpy="0 0 0"/>
+    <parent link="pelvis"/>
+    <child link="left_hip_yaw_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-0.43" upper="0.43" effort="200" velocity="23"/>
+  </joint>
+  <link name="left_hip_pitch_link">
+    <inertial>
+      <origin xyz="-0.00781 -0.004724 -0.000063" rpy="0 0 0"/>
+      <mass value="2.920"/>
+      <inertia ixx="5594.7E-06" ixy="143.308E-06" ixz="-11.847E-06" iyy="3862.581E-06" iyz="-9.607E-06" izz="4450.563E-06"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_hip_pitch_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 -0.2" rpy="0 0 0"/>
+      <geometry>
+        <cylinder radius="0.02" length="0.2"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_hip_pitch_joint" type="revolute">
+    <origin xyz="0 0.0755 0" rpy="0 0 0"/>
+    <parent link="left_hip_yaw_link"/>
+    <child link="left_hip_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-3.14" upper="2.5" effort="200" velocity="23"/>
+  </joint>
+  <link name="left_hip_roll_link">
+    <inertial>
+      <origin xyz="0.004171 -0.008576 -0.194509" rpy="0 0 0"/>
+      <mass value="4.962"/>
+      <inertia ixx="46305.869E-06" ixy="-322.626E-06" ixz="-1199.703E-06" iyy="47955.402E-06" iyz="288.147E-06" izz="8914.518E-06"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_hip_roll_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0.06 0" rpy="1.5707963267948966192313216916398 0 0"/>
+      <geometry>
+        <cylinder radius="0.02" length="0.01"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_hip_roll_joint" type="revolute">
+    <origin xyz="0 0 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.43" upper="3.14" effort="200" velocity="23"/>
+  </joint>
+  <link name="left_knee_link">
+    <inertial>
+      <origin xyz="0.000179 0.000121 -0.168936" rpy="0 0 0"/>
+      <mass value="3.839"/>
+      <inertia ixx="39002.363e-6" ixy="-72.364e-6" ixz="-794.506e-6" iyy="39042.376e-6" iyz="-18.663e-6" izz="5029.829e-6"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_knee_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 -0.2" rpy="0 0 0"/>
+      <geometry>
+        <cylinder radius="0.02" length="0.2"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_knee_joint" type="revolute">
+    <origin xyz="0 0 -0.4" rpy="0 0 0"/>
+    <parent link="left_hip_roll_link"/>
+    <child link="left_knee_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.26" upper="2.05" effort="300" velocity="14"/>
+  </joint>
+  <link name="left_ankle_pitch_link">
+    <inertial>
+      <origin xyz="-0.000294 0 -0.010794" rpy="0 0 0"/>
+      <mass value="0.102"/>
+      <inertia ixx="0.000023944" ixy="0" ixz="0.000000121" iyy="0.000021837" iyz="0" izz="0.000013414"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_ankle_pitch_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_ankle_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_ankle_pitch_joint" type="revolute">
+    <origin xyz="0 0 -0.4" rpy="0 0 0"/>
+    <parent link="left_knee_link"/>
+    <child link="left_ankle_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.897334" upper="0.523598" effort="60" velocity="9"/>
+  </joint>
+  <link name="left_ankle_roll_link">
+    <inertial>
+      <origin xyz="0.029589 0 -0.015973" rpy="0 0 0"/>
+      <mass value="0.747"/>
+      <inertia ixx="648.830e-6" ixy="0" ixz="158.380e-6" iyy="3435.339e-6" iyz="0" izz="3583.259e-6"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_ankle_roll_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_ankle_roll_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_ankle_roll_joint" type="revolute">
+    <origin xyz="0 0 -0.02" rpy="0 0 0"/>
+    <parent link="left_ankle_pitch_link"/>
+    <child link="left_ankle_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-0.261799" upper="0.261799" effort="40" velocity="9"/>
+  </joint>
+
+  <!-- R Leg -->
+  <link name="right_hip_yaw_link">
+    <inertial>
+      <origin xyz="0 0.026197 0.006647" rpy="0 0 0"/>
+      <mass value="2.829"/>
+      <inertia ixx="4553.605E-06" ixy="-0.005E-06" ixz="0.817E-06" iyy="5688.730E-06" iyz="345.157E-06" izz="3548.907E-06"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_hip_yaw_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0.02 0 0" rpy="0 1.5707963267948966192313216916398 0"/>
+      <geometry>
+        <cylinder radius="0.01" length="0.02"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_hip_yaw_joint" type="revolute">
+    <origin xyz="0 -0.0875 -0.1632" rpy="0 0 0"/>
+    <parent link="pelvis"/>
+    <child link="right_hip_yaw_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-0.43" upper="0.43" effort="200" velocity="23"/>
+  </joint>
+  <link name="right_hip_pitch_link">
+    <inertial>
+      <origin xyz="-0.00781 0.004724 -0.000063" rpy="0 0 0"/>
+      <mass value="2.920"/>
+      <inertia ixx="5594.7E-06" ixy="-143.308E-06" ixz="-11.847E-06" iyy="3862.581E-06" iyz="9.607E-06" izz="4450.563E-06"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_hip_pitch_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 -0.2" rpy="0 0 0"/>
+      <geometry>
+        <cylinder radius="0.02" length="0.2"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_hip_pitch_joint" type="revolute">
+    <origin xyz="0 -0.0755 0" rpy="0 0 0"/>
+    <parent link="right_hip_yaw_link"/>
+    <child link="right_hip_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-3.14" upper="2.5" effort="200" velocity="23"/>
+  </joint>
+  <link name="right_hip_roll_link">
+    <inertial>
+      <origin xyz="0.004171 0.008576 -0.194509" rpy="0 0 0"/>
+      <mass value="4.962"/>
+      <inertia ixx="46305.869E-06" ixy="322.626E-06" ixz="-1199.703E-06" iyy="47955.402E-06" iyz="-288.147E-06" izz="8914.518E-06"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_hip_roll_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 -0.06 0" rpy="1.5707963267948966192313216916398 0 0"/>
+      <geometry>
+        <cylinder radius="0.02" length="0.01"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_hip_roll_joint" type="revolute">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="right_hip_pitch_link"/>
+    <child link="right_hip_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-3.14" upper="0.43" effort="200" velocity="23"/>
+  </joint>
+  <link name="right_knee_link">
+    <inertial>
+      <origin xyz="0.000179 -0.000121 -0.168936" rpy="0 0 0"/>
+      <mass value="3.839"/>
+      <inertia ixx="39002.363e-6" ixy="72.364e-6" ixz="-794.506e-6" iyy="39042.376e-6" iyz="18.663e-6" izz="5029.829e-6"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_knee_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 -0.2" rpy="0 0 0"/>
+      <geometry>
+        <cylinder radius="0.02" length="0.2"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_knee_joint" type="revolute">
+    <origin xyz="0 0 -0.4" rpy="0 0 0"/>
+    <parent link="right_hip_roll_link"/>
+    <child link="right_knee_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.26" upper="2.05" effort="300" velocity="14"/>
+  </joint>
+  <link name="right_ankle_pitch_link">
+    <inertial>
+      <origin xyz="-0.000294 0 -0.010794" rpy="0 0 0"/>
+      <mass value="0.102"/>
+      <inertia ixx="0.000023944" ixy="0" ixz="0.000000121" iyy="0.000021837" iyz="0" izz="0.000013414"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_ankle_pitch_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_ankle_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_ankle_pitch_joint" type="revolute">
+    <origin xyz="0 0 -0.4" rpy="0 0 0"/>
+    <parent link="right_knee_link"/>
+    <child link="right_ankle_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.897334" upper="0.523598" effort="60" velocity="9"/>
+  </joint>
+  <link name="right_ankle_roll_link">
+    <inertial>
+      <origin xyz="0.029589 0 -0.015973" rpy="0 0 0"/>
+      <mass value="0.747"/>
+      <inertia ixx="648.830e-6" ixy="0" ixz="158.380e-6" iyy="3435.339e-6" iyz="0" izz="3583.259e-6"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_ankle_roll_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_ankle_roll_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_ankle_roll_joint" type="revolute">
+    <origin xyz="0 0 -0.02" rpy="0 0 0"/>
+    <parent link="right_ankle_pitch_link"/>
+    <child link="right_ankle_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-0.261799" upper="0.261799" effort="40" velocity="9"/>
+  </joint>
+
+  <!-- Torso -->
+  <link name="torso_link">
+    <inertial>
+      <origin xyz="0.000489 0.002797 0.20484" rpy="0 0 0"/>
+      <mass value="17.789"/>
+      <inertia ixx="0.4873" ixy="-0.00053763" ixz="0.0020276" iyy="0.40963" iyz="-0.00074582" izz="0.12785"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/torso_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0.15" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.08 0.16 0.1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="torso_joint" type="revolute">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="pelvis"/>
+    <child link="torso_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-2.35" upper="2.35" effort="200" velocity="23"/>
+  </joint>
+
+  <!-- L arm -->
+  <link name="left_shoulder_pitch_link">
+    <inertial>
+      <origin xyz="0.003053 0.06042 -0.0059" rpy="0 0 0"/>
+      <mass value="1.327"/>
+      <inertia ixx="0.00058661" ixy="-2.0157E-05" ixz="2.985E-06" iyy="0.0003989" iyz="2.238E-05" izz="0.00052936"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_shoulder_pitch_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </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 0.14806 0.42333" rpy="0.2618 0 0"/>
+    <parent link="torso_link"/>
+    <child link="left_shoulder_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-3.14" upper="1.57" effort="40" velocity="9"/>
+  </joint>
+  <link name="left_shoulder_roll_link">
+    <inertial>
+      <origin xyz="-0.030932 -1E-06 -0.10609" rpy="0 0 0"/>
+      <mass value="1.393"/>
+      <inertia ixx="0.0018408" ixy="3.98E-07" ixz="-0.00048324" iyy="0.001934638" iyz="1.61E-07" izz="0.000617733"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_shoulder_roll_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </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.0342 0.061999 -0.0060011" rpy="-0.2618 0 0"/>
+    <parent link="left_shoulder_pitch_link"/>
+    <child link="left_shoulder_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-0.38" upper="3.4" effort="40" velocity="9"/>
+  </joint>
+  <link name="left_shoulder_yaw_link">
+    <inertial>
+      <origin xyz="0.004583 0.001128 -0.001128" rpy="0 0 0"/>
+      <mass value="1.505"/>
+      <inertia ixx="0.004199" ixy="-1.7786E-05" ixz="0.00018372" iyy="0.0043139" iyz="9.278E-05" izz="0.00065755"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_shoulder_yaw_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_shoulder_yaw_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_shoulder_yaw_joint" type="revolute">
+    <origin xyz="-0.0342 0 -0.1456" rpy="0 0 0"/>
+    <parent link="left_shoulder_roll_link"/>
+    <child link="left_shoulder_yaw_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-3.01" upper="2.66" effort="18" velocity="20"/>
+  </joint>
+  <link name="left_elbow_pitch_link">
+    <inertial>
+      <origin xyz="0.077092 -0.028751 -0.009714 " rpy="0 0 0"/>
+      <mass value="0.691"/>
+      <inertia ixx="0.000318491" ixy="0.000224554" ixz="6.62076E-05" iyy="0.000861224" iyz="-2.3329E-05" izz="0.000897899"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_elbow_pitch_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_elbow_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_elbow_pitch_joint" type="revolute">
+    <origin xyz="0.006 0.0329 -0.182" rpy="0 0 0"/>
+    <parent link="left_shoulder_yaw_link"/>
+    <child link="left_elbow_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-2.53" upper="1.6" effort="18" velocity="20"/>
+  </joint>
+
+  <link name="left_elbow_roll_link">
+    <inertial>
+      <origin xyz="0.035281 -0.002320 0.000337" rpy="0 0 0"/>
+      <mass value="0.683"/>
+      <inertia ixx="0.000313275" ixy="0.000016521" ixz="-0.000002501" iyy="0.000310412" iyz="0.000005435" izz="0.000345998"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_elbow_roll_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/left_elbow_roll_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_elbow_roll_joint" type="revolute">
+    <origin xyz="0.121 -0.0329 -0.011" rpy="0 0 0"/>
+    <parent link="left_elbow_pitch_link"/>
+    <child link="left_elbow_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-2.967" upper="2.967" effort="19" velocity="31.4"/>
+  </joint>
+
+  <link name="left_wrist_pitch_link">
+    <inertial>
+      <origin xyz="0.020395 0.000036 -0.002973" rpy="0 0 0"/>
+      <mass value="0.484"/>
+      <inertia ixx="0.000071364" ixy="0.000000308" ixz="0.000001485" iyy="0.000069915" iyz="-0.000000088" izz="0.000070702"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_wrist_pitch_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/left_wrist_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_wrist_pitch_joint" type="revolute">
+    <origin xyz="0.085 0 0" rpy="0 0 0"/>
+    <parent link="left_elbow_roll_link"/>
+    <child link="left_wrist_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.471" upper="0.349" effort="19" velocity="31.4"/>
+  </joint>
+  <link name="left_wrist_yaw_link">
+    <inertial>
+      <origin xyz="0.027967 0.000096 0.000739" rpy="0 0 0"/>
+      <mass value="0.124"/>
+      <inertia ixx="137.3e-6" ixy="-0.171e-6" ixz="2.942e-6" iyy="169.998e-6" iyz="-0.025e-6" izz="84.808e-6"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/wrist_yaw_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <!-- <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/wrist_yaw_link.STL"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="left_wrist_yaw_joint" type="revolute">
+    <origin xyz="0.020 0 0" rpy="0 0 0"/>
+    <parent link="left_wrist_pitch_link"/>
+    <child link="left_wrist_yaw_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-1.012" upper="1.012" effort="19" velocity="31.4"/>
+  </joint>
+
+  <!-- R arm -->
+  <link name="right_shoulder_pitch_link">
+    <inertial>
+      <origin xyz="0.003053 -0.06042 -0.0059" rpy="0 0 0"/>
+      <mass value="1.327"/>
+      <inertia ixx="0.00058661" ixy="2.0157E-05" ixz="2.985E-06" iyy="0.0003989" iyz="-2.238E-05" izz="0.00052936"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_shoulder_pitch_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </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 -0.14806 0.42333" rpy="-0.2618 0 0"/>
+    <parent link="torso_link"/>
+    <child link="right_shoulder_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-1.57" upper="3.14" effort="40" velocity="9"/>
+  </joint>
+  <link name="right_shoulder_roll_link">
+    <inertial>
+      <origin xyz="-0.030932 1E-06 -0.10609" rpy="0 0 0"/>
+      <mass value="1.393"/>
+      <inertia ixx="0.0018408" ixy="-3.98E-07" ixz="-0.00048324" iyy="0.001934638" iyz="-1.61E-07" izz="0.000617733"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_shoulder_roll_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </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.0342 -0.061999 -0.0060011" rpy="0.2618 0 0"/>
+    <parent link="right_shoulder_pitch_link"/>
+    <child link="right_shoulder_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-3.4" upper="0.38" effort="40" velocity="9"/>
+  </joint>
+  <link name="right_shoulder_yaw_link">
+    <inertial>
+      <origin xyz="0.004583 -0.001128 -0.001128" rpy="0 0 0"/>
+      <mass value="1.505"/>
+      <inertia ixx="0.004199" ixy="1.7786E-05" ixz="0.00018372" iyy="0.0043139" iyz="-9.278E-05" izz="0.00065755"/>
+
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_shoulder_yaw_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_shoulder_yaw_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_shoulder_yaw_joint" type="revolute">
+    <origin xyz="-0.0342 0 -0.1456" rpy="0 0 0"/>
+    <parent link="right_shoulder_roll_link"/>
+    <child link="right_shoulder_yaw_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-2.66" upper="3.01" effort="18" velocity="20"/>
+  </joint>
+  <link name="right_elbow_pitch_link">
+    <inertial>
+      <origin xyz="0.077092 0.028751 -0.009714 " rpy="0 0 0"/>
+      <mass value="0.691"/>
+      <inertia ixx="0.000318491" ixy="-0.000224554" ixz="6.62076E-05" iyy="0.000861224" iyz="2.3329E-05" izz="0.000897899"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_elbow_pitch_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_elbow_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_elbow_pitch_joint" type="revolute">
+    <origin xyz="0.006 -0.0329 -0.182" rpy="0 0 0"/>
+    <parent link="right_shoulder_yaw_link"/>
+    <child link="right_elbow_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-1.6" upper="2.53" effort="18" velocity="20"/>
+  </joint>
+
+  <link name="right_elbow_roll_link">
+    <inertial>
+      <origin xyz="0.035281 -0.002320 0.000337" rpy="0 0 0"/>
+      <mass value="0.683"/>
+      <inertia ixx="0.000313275" ixy="0.000016521" ixz="-0.000002501" iyy="0.000310412" iyz="0.000005435" izz="0.000345998"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_elbow_roll_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/right_elbow_roll_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_elbow_roll_joint" type="revolute">
+    <origin xyz="0.121 0.0329 -0.011" rpy="0 0 0"/>
+    <parent link="right_elbow_pitch_link"/>
+    <child link="right_elbow_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-2.967" upper="2.967" effort="19" velocity="31.4"/>
+  </joint>
+
+  <link name="right_wrist_pitch_link">
+    <inertial>
+      <origin xyz="0.020395 0.000036 -0.002973" rpy="0 0 0"/>
+      <mass value="0.484"/>
+      <inertia ixx="0.000071364" ixy="0.000000308" ixz="0.000001485" iyy="0.000069915" iyz="-0.000000088" izz="0.000070702"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_wrist_pitch_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/right_wrist_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_wrist_pitch_joint" type="revolute">
+    <origin xyz="0.085 0 0" rpy="0 0 0"/>
+    <parent link="right_elbow_roll_link"/>
+    <child link="right_wrist_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.471" upper="0.349" effort="19" velocity="31.4"/>
+  </joint>
+  <link name="right_wrist_yaw_link">
+    <inertial>
+      <origin xyz="0.027967 -0.000096 0.000739" rpy="0 0 0"/>
+      <mass value="0.124"/>
+      <inertia ixx="137.3e-6" ixy="0.171e-6" ixz="2.942e-6" iyy="169.998e-6" iyz="0.025e-6" izz="84.808e-6"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/wrist_yaw_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <!-- <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/wrist_yaw_link.STL"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="right_wrist_yaw_joint" type="revolute">
+    <origin xyz="0.020 0 0" rpy="0 0 0"/>
+    <parent link="right_wrist_pitch_link"/>
+    <child link="right_wrist_yaw_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-1.012" upper="1.012" effort="19" velocity="31.4"/>
+  </joint>
+
+  <!-- LOGO -->
+  <link name="logo_link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/logo_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="logo_joint" type="fixed">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="torso_link"/>
+    <child link="logo_link"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+
+  <!-- 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>
+
+  <!-- L Gripper -->
+  <joint name="L_base_link_joint" type="fixed">
+    <origin xyz="0.054 0 0" rpy="0 0 1.5707963267948966192313216916398"/>
+    <parent link="left_wrist_yaw_link"/>
+    <child link="L_hand_base_link"/>
+  </joint>
+  <link name="L_hand_base_link">
+    <inertial>
+      <origin xyz="-0.002551 -0.066047 -0.0019357" rpy="0 0 0"/>
+      <mass value="0.14143"/>
+      <inertia ixx="0.0001234" ixy="2.1995E-06" ixz="-1.7694E-06" iyy="8.3835E-05" iyz="1.5968E-06" izz="7.7231E-05"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/L_hand_base_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/L_hand_base_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="L_thumb_proximal_base">
+    <inertial>
+      <origin xyz="0.0048817 0.00038782 -0.00722" rpy="0 0 0"/>
+      <mass value="0.0018869"/>
+      <inertia ixx="5.5158E-08" ixy="-1.1803E-08" ixz="-4.6743E-09" iyy="8.2164E-08" iyz="-1.3521E-09" izz="6.7434E-08"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link11_L.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link11_L.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="L_thumb_proximal_yaw_joint" type="revolute">
+    <origin xyz="-0.01696 -0.0691 0.02045" rpy="1.5708 -1.5708 0"/>
+    <parent link="L_hand_base_link"/>
+    <child link="L_thumb_proximal_base"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-0.1" upper="1.3" effort="1" velocity="0.5"/>
+  </joint>
+  <link name="L_thumb_proximal">
+    <inertial>
+      <origin xyz="0.021936 -0.01279 -0.0080386" rpy="0 0 0"/>
+      <mass value="0.0066101"/>
+      <inertia ixx="1.5693E-06" ixy="7.8339E-07" ixz="8.5959E-10" iyy="1.7356E-06" iyz="1.0378E-09" izz="2.787E-06"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link12_L.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link12_L.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="L_thumb_proximal_pitch_joint" type="revolute">
+    <origin xyz="0.0099867 0.0098242 -0.0089" rpy="-1.5708 0 0.16939"/>
+    <parent link="L_thumb_proximal_base"/>
+    <child link="L_thumb_proximal"/>
+    <axis xyz="0 0 -1"/>
+    <limit lower="-0.1" upper="0.6" effort="1" velocity="0.5"/>
+  </joint>
+  <link name="L_thumb_intermediate">
+    <inertial>
+      <origin xyz="0.0095531 0.0016282 -0.0072002" rpy="0 0 0"/>
+      <mass value="0.0037844"/>
+      <inertia ixx="3.6981E-07" ixy="9.8603E-08" ixz="-2.8173E-12" iyy="3.2395E-07" iyz="-2.8028E-12" izz="4.6532E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link13_L.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link13_L.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="L_thumb_intermediate_joint" type="revolute">
+    <origin xyz="0.04407 -0.034553 -0.0008" rpy="0 0 0"/>
+    <parent link="L_thumb_proximal"/>
+    <child link="L_thumb_intermediate"/>
+    <axis xyz="0 0 -1"/>
+    <limit lower="0" upper="0.8" effort="1" velocity="0.5"/>
+    <mimic joint="L_thumb_proximal_pitch_joint" multiplier="1.6" offset="0"/>
+  </joint>
+  <link name="L_thumb_distal">
+    <inertial>
+      <origin xyz="0.0092888 -0.004953 -0.0060033" rpy="0 0 0"/>
+      <mass value="0.003344"/>
+      <inertia ixx="1.3632E-07" ixy="5.6787E-08" ixz="-9.1939E-11" iyy="1.4052E-07" iyz="1.2145E-10" izz="2.0026E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link14_L.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link14_L.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="L_thumb_distal_joint" type="revolute">
+    <origin xyz="0.020248 -0.010156 -0.0012" rpy="0 0 0"/>
+    <parent link="L_thumb_intermediate"/>
+    <child link="L_thumb_distal"/>
+    <axis xyz="0 0 -1"/>
+    <limit lower="0" upper="1.2" effort="1" velocity="0.5"/>
+    <mimic joint="L_thumb_proximal_pitch_joint" multiplier="2.4" offset="0"/>
+  </joint>
+  <link name="L_index_proximal">
+    <inertial>
+      <origin xyz="0.0012971 -0.011934 -0.0059998" rpy="0 0 0"/>
+      <mass value="0.0042405"/>
+      <inertia ixx="6.6215E-07" ixy="1.8442E-08" ixz="1.3746E-12" iyy="2.1167E-07" iyz="-1.4773E-11" izz="6.9402E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link15_L.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link15_L.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="L_index_proximal_joint" type="revolute">
+    <origin xyz="0.00028533 -0.13653 0.032268" rpy="-0.034907 0 0"/>
+    <parent link="L_hand_base_link"/>
+    <child link="L_index_proximal"/>
+    <axis xyz="0 0 -1"/>
+    <limit lower="0" upper="1.7" effort="1" velocity="0.5"/>
+  </joint>
+  <link name="L_index_intermediate">
+    <inertial>
+      <origin xyz="0.0021753 -0.019567 -0.005" rpy="0 0 0"/>
+      <mass value="0.0045682"/>
+      <inertia ixx="7.6284E-07" ixy="-8.063E-08" ixz="3.6797E-13" iyy="9.4308E-08" iyz="1.5743E-13" izz="7.8176E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link16_L.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link16_L.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="L_index_intermediate_joint" type="revolute">
+    <origin xyz="-0.0024229 -0.032041 -0.001" rpy="0 0 0"/>
+    <parent link="L_index_proximal"/>
+    <child link="L_index_intermediate"/>
+    <axis xyz="0 0 -1"/>
+    <limit lower="0" upper="1.7" effort="1" velocity="0.5"/>
+    <mimic joint="L_index_proximal_joint" multiplier="1" offset="0"/>
+  </joint>
+  <link name="L_middle_proximal">
+    <inertial>
+      <origin xyz="0.0012971 -0.011934 -0.0059999" rpy="0 0 0"/>
+      <mass value="0.0042405"/>
+      <inertia ixx="6.6215E-07" ixy="1.8442E-08" ixz="1.2299E-12" iyy="2.1167E-07" iyz="-1.4484E-11" izz="6.9402E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link17_L.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link17_L.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="L_middle_proximal_joint" type="revolute">
+    <origin xyz="0.00028533 -0.1371 0.01295" rpy="0 0 0"/>
+    <parent link="L_hand_base_link"/>
+    <child link="L_middle_proximal"/>
+    <axis xyz="0 0 -1"/>
+    <limit lower="0" upper="1.7" effort="1" velocity="0.5"/>
+  </joint>
+  <link name="L_middle_intermediate">
+    <inertial>
+      <origin xyz="0.001921 -0.020796 -0.0049999" rpy="0 0 0"/>
+      <mass value="0.0050397"/>
+      <inertia ixx="9.5823E-07" ixy="-1.1425E-07" ixz="-2.4186E-12" iyy="1.0646E-07" iyz="3.6974E-12" izz="9.8385E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link18_L.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link18_L.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="L_middle_intermediate_joint" type="revolute">
+    <origin xyz="-0.0024229 -0.032041 -0.001" rpy="0 0 0"/>
+    <parent link="L_middle_proximal"/>
+    <child link="L_middle_intermediate"/>
+    <axis xyz="0 0 -1"/>
+    <limit lower="0" upper="1.7" effort="1" velocity="0.5"/>
+    <mimic joint="L_middle_proximal_joint" multiplier="1" offset="0"/>
+  </joint>
+  <link name="L_ring_proximal">
+    <inertial>
+      <origin xyz="0.0012971 -0.011934 -0.0059999" rpy="0 0 0"/>
+      <mass value="0.0042405"/>
+      <inertia ixx="6.6215E-07" ixy="1.8442E-08" ixz="9.6052E-13" iyy="2.1167E-07" iyz="-1.4124E-11" izz="6.9402E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link19_L.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link19_L.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="L_ring_proximal_joint" type="revolute">
+    <origin xyz="0.00028533 -0.13691 -0.0062872" rpy="0.05236 0 0"/>
+    <parent link="L_hand_base_link"/>
+    <child link="L_ring_proximal"/>
+    <axis xyz="0 0 -1"/>
+    <limit lower="0" upper="1.7" effort="1" velocity="0.5"/>
+  </joint>
+  <link name="L_ring_intermediate">
+    <inertial>
+      <origin xyz="0.0021753 -0.019567 -0.005" rpy="0 0 0"/>
+      <mass value="0.0045682"/>
+      <inertia ixx="7.6285E-07" ixy="-8.0631E-08" ixz="3.3472E-14" iyy="9.4308E-08" iyz="-4.4773E-13" izz="7.8176E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link20_L.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link20_L.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="L_ring_intermediate_joint" type="revolute">
+    <origin xyz="-0.0024229 -0.032041 -0.001" rpy="0 0 0"/>
+    <parent link="L_ring_proximal"/>
+    <child link="L_ring_intermediate"/>
+    <axis xyz="0 0 -1"/>
+    <limit lower="0" upper="1.7" effort="1" velocity="0.5"/>
+    <mimic joint="L_ring_proximal_joint" multiplier="1" offset="0"/>
+  </joint>
+  <link name="L_pinky_proximal">
+    <inertial>
+      <origin xyz="0.0012971 -0.011934 -0.0059999" rpy="0 0 0"/>
+      <mass value="0.0042405"/>
+      <inertia ixx="6.6215E-07" ixy="1.8442E-08" ixz="1.0279E-12" iyy="2.1167E-07" iyz="-1.4277E-11" izz="6.9402E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link21_L.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link21_L.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="L_pinky_proximal_joint" type="revolute">
+    <origin xyz="0.00028533 -0.13571 -0.025488" rpy="0.10472 0 0"/>
+    <parent link="L_hand_base_link"/>
+    <child link="L_pinky_proximal"/>
+    <axis xyz="0 0 -1"/>
+    <limit lower="0" upper="1.7" effort="1" velocity="0.5"/>
+  </joint>
+  <link name="L_pinky_intermediate">
+    <inertial>
+      <origin xyz="0.0024788 -0.016208 -0.0050001" rpy="0 0 0"/>
+      <mass value="0.0036036"/>
+      <inertia ixx="4.3923E-07" ixy="-4.1355E-08" ixz="1.2263E-12" iyy="7.0315E-08" iyz="3.1311E-12" izz="4.4881E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link22_L.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link22_L.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="L_pinky_intermediate_joint" type="revolute">
+    <origin xyz="-0.0024229 -0.032041 -0.001" rpy="0 0 0"/>
+    <parent link="L_pinky_proximal"/>
+    <child link="L_pinky_intermediate"/>
+    <axis xyz="0 0 -1"/>
+    <limit lower="0" upper="1.7" effort="1" velocity="0.5"/>
+    <mimic joint="L_pinky_proximal_joint" multiplier="1" offset="0"/>
+  </joint>
+
+  <!-- R Gripper -->
+  <joint name="R_base_link_joint" type="fixed">
+    <origin xyz="0.054 0 0" rpy="3.1415926535897932384626433832795 0 -1.5707963267948966192313216916398"/>
+    <parent link="right_wrist_yaw_link"/>
+    <child link="R_hand_base_link"/>
+  </joint>
+  <link name="R_hand_base_link">
+    <inertial>
+      <origin xyz="-0.0025264 -0.066047 0.0019598" rpy="0 0 0"/>
+      <mass value="0.14143"/>
+      <inertia ixx="0.00012281" ixy="2.1711E-06" ixz="1.7709E-06" iyy="8.3832E-05" iyz="-1.6551E-06" izz="7.6663E-05"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/R_hand_base_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/R_hand_base_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="R_thumb_proximal_base">
+    <inertial>
+      <origin xyz="-0.0048064 0.0009382 -0.00757" rpy="0 0 0"/>
+      <mass value="0.0018869"/>
+      <inertia ixx="5.816E-08" ixy="1.4539E-08" ixz="4.491E-09" iyy="7.9161E-08" iyz="-1.8727E-09" izz="6.7433E-08"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link11_R.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link11_R.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="R_thumb_proximal_yaw_joint" type="revolute">
+    <origin xyz="-0.01696 -0.0691 -0.02045" rpy="1.5708 -1.5708 0"/>
+    <parent link="R_hand_base_link"/>
+    <child link="R_thumb_proximal_base"/>
+    <axis xyz="0 0 -1"/>
+    <limit lower="-0.1" upper="1.3" effort="1" velocity="0.5"/>
+  </joint>
+  <link name="R_thumb_proximal">
+    <inertial>
+      <origin xyz="0.021932 0.012785 -0.0080386" rpy="0 0 0"/>
+      <mass value="0.0066075"/>
+      <inertia ixx="1.5686E-06" ixy="-7.8296E-07" ixz="8.9143E-10" iyy="1.7353E-06" iyz="-1.0191E-09" izz="2.786E-06"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link12_R.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link12_R.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="R_thumb_proximal_pitch_joint" type="revolute">
+    <origin xyz="-0.0088099 0.010892 -0.00925" rpy="1.5708 0 2.8587"/>
+    <parent link="R_thumb_proximal_base"/>
+    <child link="R_thumb_proximal"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-0.1" upper="0.6" effort="1" velocity="0.5"/>
+  </joint>
+  <link name="R_thumb_intermediate">
+    <inertial>
+      <origin xyz="0.0095544 -0.0016282 -0.0071997" rpy="0 0 0"/>
+      <mass value="0.0037847"/>
+      <inertia ixx="3.6981E-07" ixy="-9.8581E-08" ixz="-4.7469E-12" iyy="3.2394E-07" iyz="1.0939E-12" izz="4.6531E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link13_R.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link13_R.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="R_thumb_intermediate_joint" type="revolute">
+    <origin xyz="0.04407 0.034553 -0.0008" rpy="0 0 0"/>
+    <parent link="R_thumb_proximal"/>
+    <child link="R_thumb_intermediate"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="0" upper="0.8" effort="1" velocity="0.5"/>
+    <mimic joint="R_thumb_proximal_pitch_joint" multiplier="1.6" offset="0"/>
+  </joint>
+  <link name="R_thumb_distal">
+    <inertial>
+      <origin xyz="0.0092888 0.0049529 -0.0060033" rpy="0 0 0"/>
+      <mass value="0.0033441"/>
+      <inertia ixx="1.3632E-07" ixy="-5.6788E-08" ixz="-9.2764E-11" iyy="1.4052E-07" iyz="-1.2283E-10" izz="2.0026E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link14_R.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link14_R.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="R_thumb_distal_joint" type="revolute">
+    <origin xyz="0.020248 0.010156 -0.0012" rpy="0 0 0"/>
+    <parent link="R_thumb_intermediate"/>
+    <child link="R_thumb_distal"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="0" upper="1.2" effort="1" velocity="0.5"/>
+    <mimic joint="R_thumb_proximal_pitch_joint" multiplier="2.4" offset="0"/>
+  </joint>
+  <link name="R_index_proximal">
+    <inertial>
+      <origin xyz="0.0012259 0.011942 -0.0060001" rpy="0 0 0"/>
+      <mass value="0.0042403"/>
+      <inertia ixx="6.6232E-07" ixy="-1.5775E-08" ixz="1.8515E-12" iyy="2.1146E-07" iyz="-5.0828E-12" izz="6.9398E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link15_R.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link15_R.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="R_index_proximal_joint" type="revolute">
+    <origin xyz="0.00028533 -0.13653 -0.032268" rpy="-3.1067 0 0"/>
+    <parent link="R_hand_base_link"/>
+    <child link="R_index_proximal"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="0" upper="1.7" effort="1" velocity="0.5"/>
+  </joint>
+  <link name="R_index_intermediate">
+    <inertial>
+      <origin xyz="0.0019697 0.019589 -0.005" rpy="0 0 0"/>
+      <mass value="0.0045683"/>
+      <inertia ixx="7.6111E-07" ixy="8.7637E-08" ixz="-3.7751E-13" iyy="9.6076E-08" iyz="9.9444E-13" izz="7.8179E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link16_R.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link16_R.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="R_index_intermediate_joint" type="revolute">
+    <origin xyz="-0.0026138 0.032026 -0.001" rpy="0 0 0"/>
+    <parent link="R_index_proximal"/>
+    <child link="R_index_intermediate"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="0" upper="1.7" effort="1" velocity="0.5"/>
+    <mimic joint="R_index_proximal_joint" multiplier="1" offset="0"/>
+  </joint>
+  <link name="R_middle_proximal">
+    <inertial>
+      <origin xyz="0.001297 0.011934 -0.0060001" rpy="0 0 0"/>
+      <mass value="0.0042403"/>
+      <inertia ixx="6.6211E-07" ixy="-1.8461E-08" ixz="1.8002E-12" iyy="2.1167E-07" iyz="-6.6808E-12" izz="6.9397E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link17_R.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link17_R.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="R_middle_proximal_joint" type="revolute">
+    <origin xyz="0.00028533 -0.1371 -0.01295" rpy="-3.1416 0 0"/>
+    <parent link="R_hand_base_link"/>
+    <child link="R_middle_proximal"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="0" upper="1.7" effort="1" velocity="0.5"/>
+  </joint>
+  <link name="R_middle_intermediate">
+    <inertial>
+      <origin xyz="0.001921 0.020796 -0.005" rpy="0 0 0"/>
+      <mass value="0.0050396"/>
+      <inertia ixx="9.5822E-07" ixy="1.1425E-07" ixz="-2.4791E-12" iyy="1.0646E-07" iyz="5.9173E-12" izz="9.8384E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link18_R.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link18_R.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="R_middle_intermediate_joint" type="revolute">
+    <origin xyz="-0.0024229 0.032041 -0.001" rpy="0 0 0"/>
+    <parent link="R_middle_proximal"/>
+    <child link="R_middle_intermediate"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="0" upper="1.7" effort="1" velocity="0.5"/>
+    <mimic joint="R_middle_proximal_joint" multiplier="1" offset="0"/>
+  </joint>
+  <link name="R_ring_proximal">
+    <inertial>
+      <origin xyz="0.001297 0.011934 -0.0060002" rpy="0 0 0"/>
+      <mass value="0.0042403"/>
+      <inertia ixx="6.6211E-07" ixy="-1.8461E-08" ixz="1.5793E-12" iyy="2.1167E-07" iyz="-6.6868E-12" izz="6.9397E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link19_R.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link19_R.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="R_ring_proximal_joint" type="revolute">
+    <origin xyz="0.00028533 -0.13691 0.0062872" rpy="3.0892 0 0"/>
+    <parent link="R_hand_base_link"/>
+    <child link="R_ring_proximal"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="0" upper="1.7" effort="1" velocity="0.5"/>
+  </joint>
+  <link name="R_ring_intermediate">
+    <inertial>
+      <origin xyz="0.0021753 0.019567 -0.005" rpy="0 0 0"/>
+      <mass value="0.0045683"/>
+      <inertia ixx="7.6286E-07" ixy="8.0635E-08" ixz="-6.1562E-13" iyy="9.431E-08" iyz="5.8619E-13" izz="7.8177E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link20_R.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link20_R.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="R_ring_intermediate_joint" type="revolute">
+    <origin xyz="-0.0024229 0.032041 -0.001" rpy="0 0 0"/>
+    <parent link="R_ring_proximal"/>
+    <child link="R_ring_intermediate"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="0" upper="1.7" effort="1" velocity="0.5"/>
+    <mimic joint="R_ring_proximal_joint" multiplier="1" offset="0"/>
+  </joint>
+  <link name="R_pinky_proximal">
+    <inertial>
+      <origin xyz="0.001297 0.011934 -0.0060001" rpy="0 0 0"/>
+      <mass value="0.0042403"/>
+      <inertia ixx="6.6211E-07" ixy="-1.8461E-08" ixz="1.6907E-12" iyy="2.1167E-07" iyz="-6.9334E-12" izz="6.9397E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link21_R.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link21_R.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="R_pinky_proximal_joint" type="revolute">
+    <origin xyz="0.00028533 -0.13571 0.025488" rpy="3.0369 0 0"/>
+    <parent link="R_hand_base_link"/>
+    <child link="R_pinky_proximal"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="0" upper="1.7" effort="1" velocity="0.5"/>
+  </joint>
+  <link name="R_pinky_intermediate">
+    <inertial>
+      <origin xyz="0.0024748 0.016203 -0.0050031" rpy="0 0 0"/>
+      <mass value="0.0035996"/>
+      <inertia ixx="4.3913E-07" ixy="4.1418E-08" ixz="3.7168E-11" iyy="7.0247E-08" iyz="5.8613E-11" izz="4.4867E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link22_R.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link22_R.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="R_pinky_intermediate_joint" type="revolute">
+    <origin xyz="-0.0024229 0.032041 -0.001" rpy="0 0 0"/>
+    <parent link="R_pinky_proximal"/>
+    <child link="R_pinky_intermediate"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="0" upper="1.7" effort="1" velocity="0.5"/>
+    <mimic joint="R_pinky_proximal_joint" multiplier="1" offset="0"/>
+  </joint>
+</robot>
\ No newline at end of file
diff --git a/resources/robots/h1_2/h1_2.xml b/resources/robots/h1_2/h1_2.xml
new file mode 100644
index 0000000..d946402
--- /dev/null
+++ b/resources/robots/h1_2/h1_2.xml
@@ -0,0 +1,438 @@
+<mujoco model="h1_5">
+  <compiler angle="radian" meshdir="meshes/" autolimits="true"/>
+  <statistic meansize="0.112107" extent="1.95557" center="0.0256948 1.86841e-05 -0.178443"/>
+  <asset>
+    <mesh name="pelvis" file="pelvis.STL"/>
+    <mesh name="left_hip_yaw_link" file="left_hip_yaw_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_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_yaw_link" file="right_hip_yaw_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_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="torso_link" file="torso_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_pitch_link" file="left_elbow_pitch_link.STL"/>
+    <mesh name="left_elbow_roll_link" file="left_elbow_roll_link.STL"/>
+    <mesh name="left_wrist_pitch_link" file="left_wrist_pitch_link.STL"/>
+    <mesh name="wrist_yaw_link" file="wrist_yaw_link.STL"/>
+    <mesh name="right_shoulder_pitch_link" file="right_shoulder_pitch_link.STL"/>
+    <mesh name="right_shoulder_roll_link" file="right_shoulder_roll_link.STL"/>
+    <mesh name="right_shoulder_yaw_link" file="right_shoulder_yaw_link.STL"/>
+    <mesh name="right_elbow_pitch_link" file="right_elbow_pitch_link.STL"/>
+    <mesh name="right_elbow_roll_link" file="right_elbow_roll_link.STL"/>
+    <mesh name="right_wrist_pitch_link" file="right_wrist_pitch_link.STL"/>
+    <mesh name="logo_link" file="logo_link.STL"/>
+    <mesh name="L_hand_base_link" file="L_hand_base_link.STL"/>
+    <mesh name="link11_L" file="link11_L.STL"/>
+    <mesh name="link12_L" file="link12_L.STL"/>
+    <mesh name="link13_L" file="link13_L.STL"/>
+    <mesh name="link14_L" file="link14_L.STL"/>
+    <mesh name="link15_L" file="link15_L.STL"/>
+    <mesh name="link16_L" file="link16_L.STL"/>
+    <mesh name="link17_L" file="link17_L.STL"/>
+    <mesh name="link18_L" file="link18_L.STL"/>
+    <mesh name="link19_L" file="link19_L.STL"/>
+    <mesh name="link20_L" file="link20_L.STL"/>
+    <mesh name="link21_L" file="link21_L.STL"/>
+    <mesh name="link22_L" file="link22_L.STL"/>
+    <mesh name="R_hand_base_link" file="R_hand_base_link.STL"/>
+    <mesh name="link11_R" file="link11_R.STL"/>
+    <mesh name="link12_R" file="link12_R.STL"/>
+    <mesh name="link13_R" file="link13_R.STL"/>
+    <mesh name="link14_R" file="link14_R.STL"/>
+    <mesh name="link15_R" file="link15_R.STL"/>
+    <mesh name="link16_R" file="link16_R.STL"/>
+    <mesh name="link17_R" file="link17_R.STL"/>
+    <mesh name="link18_R" file="link18_R.STL"/>
+    <mesh name="link19_R" file="link19_R.STL"/>
+    <mesh name="link20_R" file="link20_R.STL"/>
+    <mesh name="link21_R" file="link21_R.STL"/>
+    <mesh name="link22_R" file="link22_R.STL"/>
+  </asset>
+  <worldbody>
+    <body name="pelvis" pos="0 0 1.03">
+      <inertial pos="-0.0004 3.7e-05 -0.046864" quat="0.497097 0.496809 -0.503132 0.502925" mass="5.983" diaginertia="0.0531565 0.0491678 0.00902583"/>
+      <joint name="floating_base_joint" type="free"/>
+      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="pelvis"/>
+      <geom size="0.05" rgba="0.1 0.1 0.1 1"/>
+      <body name="left_hip_yaw_link" pos="0 0.0875 -0.1632">
+        <inertial pos="0 -0.026197 0.006647" quat="0.704899 -0.0553755 0.0548434 0.705013" mass="2.829" diaginertia="0.00574303 0.00455361 0.00349461"/>
+        <joint name="left_hip_yaw_joint" pos="0 0 0" axis="0 0 1" range="-0.43 0.43"/>
+        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="left_hip_yaw_link"/>
+        <geom size="0.01 0.01" pos="0.02 0 0" quat="0.707107 0 0.707107 0" type="cylinder" rgba="0.1 0.1 0.1 1"/>
+        <body name="left_hip_pitch_link" pos="0 0.0755 0">
+          <inertial pos="-0.00781 -0.004724 -6.3e-05" quat="0.701575 0.711394 0.0330266 0.0249149" mass="2.92" diaginertia="0.00560661 0.00445055 0.00385068"/>
+          <joint name="left_hip_pitch_joint" pos="0 0 0" axis="0 1 0" range="-3.14 2.5"/>
+          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="left_hip_pitch_link"/>
+          <geom size="0.02 0.1" pos="0 0 -0.2" type="cylinder" rgba="0.1 0.1 0.1 1"/>
+          <body name="left_hip_roll_link">
+            <inertial pos="0.004171 -0.008576 -0.194509" quat="0.634842 0.0146079 0.0074063 0.772469" mass="4.962" diaginertia="0.0480229 0.0462788 0.00887409"/>
+            <joint name="left_hip_roll_joint" pos="0 0 0" axis="1 0 0" range="-0.43 3.14"/>
+            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="left_hip_roll_link"/>
+            <geom size="0.02 0.005" pos="0 0.06 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.1 0.1 0.1 1"/>
+            <body name="left_knee_link" pos="0 0 -0.4">
+              <inertial pos="0.000179 0.000121 -0.168936" quat="0.416585 0.0104983 0.00514003 0.909021" mass="3.839" diaginertia="0.0391044 0.038959 0.00501125"/>
+              <joint name="left_knee_joint" pos="0 0 0" axis="0 1 0" range="-0.26 2.05"/>
+              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="left_knee_link"/>
+              <geom size="0.02 0.1" pos="0 0 -0.2" type="cylinder" rgba="0.1 0.1 0.1 1"/>
+              <body name="left_ankle_pitch_link" pos="0 0 -0.4">
+                <inertial pos="-0.000294 0 -0.010794" quat="0.999984 0 -0.00574445 0" mass="0.102" diaginertia="2.39454e-05 2.1837e-05 1.34126e-05"/>
+                <joint name="left_ankle_pitch_joint" pos="0 0 0" axis="0 1 0" range="-0.897334 0.523598"/>
+                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="left_ankle_pitch_link"/>
+                <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="left_ankle_pitch_link"/>
+                <body name="left_ankle_roll_link" pos="0 0 -0.02">
+                  <inertial pos="0.029589 0 -0.015973" quat="0 0.725858 0 0.687845" mass="0.747" diaginertia="0.00359178 0.00343534 0.000640307"/>
+                  <joint name="left_ankle_roll_joint" pos="0 0 0" axis="1 0 0" range="-0.261799 0.261799"/>
+                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="left_ankle_roll_link"/>
+                  <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="left_ankle_roll_link"/>
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+      <body name="right_hip_yaw_link" pos="0 -0.0875 -0.1632">
+        <inertial pos="0 0.026197 0.006647" quat="0.705013 0.0548434 -0.0553755 0.704899" mass="2.829" diaginertia="0.00574303 0.00455361 0.00349461"/>
+        <joint name="right_hip_yaw_joint" pos="0 0 0" axis="0 0 1" range="-0.43 0.43"/>
+        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="right_hip_yaw_link"/>
+        <geom size="0.01 0.01" pos="0.02 0 0" quat="0.707107 0 0.707107 0" type="cylinder" rgba="0.1 0.1 0.1 1"/>
+        <body name="right_hip_pitch_link" pos="0 -0.0755 0">
+          <inertial pos="-0.00781 0.004724 -6.3e-05" quat="0.711394 0.701575 -0.0249149 -0.0330266" mass="2.92" diaginertia="0.00560661 0.00445055 0.00385068"/>
+          <joint name="right_hip_pitch_joint" pos="0 0 0" axis="0 1 0" range="-3.14 2.5"/>
+          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="right_hip_pitch_link"/>
+          <geom size="0.02 0.1" pos="0 0 -0.2" type="cylinder" rgba="0.1 0.1 0.1 1"/>
+          <body name="right_hip_roll_link">
+            <inertial pos="0.004171 0.008576 -0.194509" quat="0.772469 0.0074063 0.0146079 0.634842" mass="4.962" diaginertia="0.0480229 0.0462788 0.00887409"/>
+            <joint name="right_hip_roll_joint" pos="0 0 0" axis="1 0 0" range="-3.14 0.43"/>
+            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="right_hip_roll_link"/>
+            <geom size="0.02 0.005" pos="0 -0.06 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.1 0.1 0.1 1"/>
+            <body name="right_knee_link" pos="0 0 -0.4">
+              <inertial pos="0.000179 -0.000121 -0.168936" quat="0.909021 0.00514003 0.0104983 0.416585" mass="3.839" diaginertia="0.0391044 0.038959 0.00501125"/>
+              <joint name="right_knee_joint" pos="0 0 0" axis="0 1 0" range="-0.26 2.05"/>
+              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="right_knee_link"/>
+              <geom size="0.02 0.1" pos="0 0 -0.2" type="cylinder" rgba="0.1 0.1 0.1 1"/>
+              <body name="right_ankle_pitch_link" pos="0 0 -0.4">
+                <inertial pos="-0.000294 0 -0.010794" quat="0.999984 0 -0.00574445 0" mass="0.102" diaginertia="2.39454e-05 2.1837e-05 1.34126e-05"/>
+                <joint name="right_ankle_pitch_joint" pos="0 0 0" axis="0 1 0" range="-0.897334 0.523598"/>
+                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="right_ankle_pitch_link"/>
+                <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="right_ankle_pitch_link"/>
+                <body name="right_ankle_roll_link" pos="0 0 -0.02">
+                  <inertial pos="0.029589 0 -0.015973" quat="0 0.725858 0 0.687845" mass="0.747" diaginertia="0.00359178 0.00343534 0.000640307"/>
+                  <joint name="right_ankle_roll_joint" pos="0 0 0" axis="1 0 0" range="-0.261799 0.261799"/>
+                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="right_ankle_roll_link"/>
+                  <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="right_ankle_roll_link"/>
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+      <body name="torso_link">
+        <inertial pos="0.000489 0.002797 0.20484" quat="0.999989 -0.00130808 -0.00282289 -0.00349105" mass="17.789" diaginertia="0.487315 0.409628 0.127837"/>
+        <joint name="torso_joint" pos="0 0 0" axis="0 0 1" range="-2.35 2.35"/>
+        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="torso_link"/>
+        <geom size="0.04 0.08 0.05" pos="0 0 0.15" type="box" rgba="0.1 0.1 0.1 1"/>
+        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="1 1 1 1" mesh="logo_link"/>
+        <site name="imu" size="0.01" pos="-0.04452 -0.01891 0.27756"/>
+        <body name="left_shoulder_pitch_link" pos="0 0.14806 0.42333" quat="0.991445 0.130526 0 0">
+          <inertial pos="0.003053 0.06042 -0.0059" quat="0.761799 0.645681 -0.0378496 -0.0363943" mass="1.327" diaginertia="0.000588757 0.00053309 0.000393023"/>
+          <joint name="left_shoulder_pitch_joint" pos="0 0 0" axis="0 1 0" range="-3.14 1.57"/>
+          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="left_shoulder_pitch_link"/>
+          <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="left_shoulder_pitch_link"/>
+          <body name="left_shoulder_roll_link" pos="0.0342 0.061999 -0.0060011" quat="0.991445 -0.130526 0 0">
+            <inertial pos="-0.030932 -1e-06 -0.10609" quat="0.986055 0.000456937 0.166408 0.00213553" mass="1.393" diaginertia="0.00200869 0.00193464 0.000449847"/>
+            <joint name="left_shoulder_roll_joint" pos="0 0 0" axis="1 0 0" range="-0.38 3.4"/>
+            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="left_shoulder_roll_link"/>
+            <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="left_shoulder_roll_link"/>
+            <body name="left_shoulder_yaw_link" pos="-0.0342 0 -0.1456">
+              <inertial pos="0.004583 0.001128 -0.001128" quat="0.663644 -0.0108866 -0.0267235 0.747492" mass="1.505" diaginertia="0.00431782 0.00420697 0.000645658"/>
+              <joint name="left_shoulder_yaw_joint" pos="0 0 0" axis="0 0 1" range="-3.01 2.66"/>
+              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="left_shoulder_yaw_link"/>
+              <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="left_shoulder_yaw_link"/>
+              <body name="left_elbow_pitch_link" pos="0.006 0.0329 -0.182">
+                <inertial pos="0.077092 -0.028751 -0.009714" quat="0.544921 0.610781 0.423352 0.388305" mass="0.691" diaginertia="0.000942091 0.000905273 0.00023025"/>
+                <joint name="left_elbow_pitch_joint" pos="0 0 0" axis="0 1 0" range="-2.53 1.6"/>
+                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="left_elbow_pitch_link"/>
+                <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="left_elbow_pitch_link"/>
+                <body name="left_elbow_roll_link" pos="0.121 -0.0329 -0.011">
+                  <inertial pos="0.035281 -0.00232 0.000337" quat="0.334998 0.622198 -0.240131 0.66557" mass="0.683" diaginertia="0.00034681 0.000328248 0.000294628"/>
+                  <joint name="left_elbow_roll_joint" pos="0 0 0" axis="1 0 0" range="-2.967 2.967"/>
+                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="left_elbow_roll_link"/>
+                  <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="left_elbow_roll_link"/>
+                  <body name="left_wrist_pitch_link" pos="0.085 0 0">
+                    <inertial pos="0.020395 3.6e-05 -0.002973" quat="0.915893 -0.228405 -0.327262 -0.0432527" mass="0.484" diaginertia="7.25675e-05 7.00325e-05 6.9381e-05"/>
+                    <joint name="left_wrist_pitch_joint" pos="0 0 0" axis="0 1 0" range="-0.471 0.349"/>
+                    <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="left_wrist_pitch_link"/>
+                    <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="left_wrist_pitch_link"/>
+                    <body name="left_wrist_yaw_link" pos="0.02 0 0">
+                      <inertial pos="0.0770303 -0.00131441 -0.00068617" quat="0.499919 0.510625 0.506813 0.482165" mass="0.26543" diaginertia="0.000854397 0.000723298 0.00022115"/>
+                      <joint name="left_wrist_yaw_joint" pos="0 0 0" axis="0 0 1" range="-1.012 1.012"/>
+                      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="wrist_yaw_link"/>
+                      <geom pos="0.054 0 0" quat="0.707107 0 0 0.707107" type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="L_hand_base_link"/>
+                      <geom pos="0.054 0 0" quat="0.707107 0 0 0.707107" type="mesh" rgba="0.1 0.1 0.1 1" mesh="L_hand_base_link"/>
+                      <body name="L_thumb_proximal_base" pos="0.1231 -0.01696 0.02045" quat="-2.59735e-06 0.707107 0 0.707107">
+                        <inertial pos="0.0048817 0.00038782 -0.00722" quat="0.445981 0.352284 0.495833 0.656617" mass="0.0018869" diaginertia="8.66031e-08 6.87331e-08 4.94199e-08"/>
+                        <joint name="L_thumb_proximal_yaw_joint" pos="0 0 0" axis="0 0 1" range="-0.1 1.3"/>
+                        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link11_L"/>
+                        <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link11_L"/>
+                        <body name="L_thumb_proximal" pos="0.0099867 0.0098242 -0.0089" quat="0.704571 -0.704573 -0.0598169 0.0598167">
+                          <inertial pos="0.021936 -0.01279 -0.0080386" quat="0.25452 0.660687 -0.251949 0.659723" mass="0.0066101" diaginertia="2.78701e-06 2.44024e-06 8.6466e-07"/>
+                          <joint name="L_thumb_proximal_pitch_joint" pos="0 0 0" axis="0 0 -1" range="-0.1 0.6"/>
+                          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link12_L"/>
+                          <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link12_L"/>
+                          <body name="L_thumb_intermediate" pos="0.04407 -0.034553 -0.0008">
+                            <inertial pos="0.0095531 0.0016282 -0.0072002" quat="0.30738 0.636732 -0.307526 0.636803" mass="0.0037844" diaginertia="4.6532e-07 4.48114e-07 2.45646e-07"/>
+                            <joint name="L_thumb_intermediate_joint" pos="0 0 0" axis="0 0 -1" range="0 0.8"/>
+                            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link13_L"/>
+                            <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link13_L"/>
+                            <body name="L_thumb_distal" pos="0.020248 -0.010156 -0.0012">
+                              <inertial pos="0.0092888 -0.004953 -0.0060033" quat="0.266264 0.65596 -0.262836 0.655544" mass="0.003344" diaginertia="2.0026e-07 1.95246e-07 8.1594e-08"/>
+                              <joint name="L_thumb_distal_joint" pos="0 0 0" axis="0 0 -1" range="0 1.2"/>
+                              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link14_L"/>
+                              <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link14_L"/>
+                            </body>
+                          </body>
+                        </body>
+                      </body>
+                      <body name="L_index_proximal" pos="0.19053 0.00028533 0.032268" quat="0.706999 -0.0123409 -0.0123409 0.706999">
+                        <inertial pos="0.0012971 -0.011934 -0.0059998" quat="0.489677 0.510115 -0.489692 0.510099" mass="0.0042405" diaginertia="6.9402e-07 6.62904e-07 2.10916e-07"/>
+                        <joint name="L_index_proximal_joint" pos="0 0 0" axis="0 0 -1" range="0 1.7"/>
+                        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link15_L"/>
+                        <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link15_L"/>
+                        <body name="L_index_intermediate" pos="-0.0024229 -0.032041 -0.001">
+                          <inertial pos="0.0021753 -0.019567 -0.005" quat="0.528694 0.469555 -0.528694 0.469555" mass="0.0045682" diaginertia="7.8176e-07 7.72427e-07 8.47209e-08"/>
+                          <joint name="L_index_intermediate_joint" pos="0 0 0" axis="0 0 -1" range="0 1.7"/>
+                          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link16_L"/>
+                          <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link16_L"/>
+                        </body>
+                      </body>
+                      <body name="L_middle_proximal" pos="0.1911 0.00028533 0.01295" quat="0.707107 0 0 0.707107">
+                        <inertial pos="0.0012971 -0.011934 -0.0059999" quat="0.489677 0.510115 -0.489692 0.510099" mass="0.0042405" diaginertia="6.9402e-07 6.62904e-07 2.10916e-07"/>
+                        <joint name="L_middle_proximal_joint" pos="0 0 0" axis="0 0 -1" range="0 1.7"/>
+                        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link17_L"/>
+                        <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link17_L"/>
+                        <body name="L_middle_intermediate" pos="-0.0024229 -0.032041 -0.001">
+                          <inertial pos="0.001921 -0.020796 -0.0049999" quat="0.531603 0.466115 -0.531728 0.466262" mass="0.0050397" diaginertia="9.8385e-07 9.73288e-07 9.14016e-08"/>
+                          <joint name="L_middle_intermediate_joint" pos="0 0 0" axis="0 0 -1" range="0 1.7"/>
+                          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link18_L"/>
+                          <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link18_L"/>
+                        </body>
+                      </body>
+                      <body name="L_ring_proximal" pos="0.19091 0.00028533 -0.0062872" quat="0.706864 0.0185099 0.0185099 0.706864">
+                        <inertial pos="0.0012971 -0.011934 -0.0059999" quat="0.489677 0.510114 -0.489692 0.510099" mass="0.0042405" diaginertia="6.9402e-07 6.62904e-07 2.10916e-07"/>
+                        <joint name="L_ring_proximal_joint" pos="0 0 0" axis="0 0 -1" range="0 1.7"/>
+                        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link19_L"/>
+                        <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link19_L"/>
+                        <body name="L_ring_intermediate" pos="-0.0024229 -0.032041 -0.001">
+                          <inertial pos="0.0021753 -0.019567 -0.005" quat="0.528694 0.469556 -0.528694 0.469556" mass="0.0045682" diaginertia="7.8176e-07 7.72437e-07 8.47208e-08"/>
+                          <joint name="L_ring_intermediate_joint" pos="0 0 0" axis="0 0 -1" range="0 1.7"/>
+                          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link20_L"/>
+                          <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link20_L"/>
+                        </body>
+                      </body>
+                      <body name="L_pinky_proximal" pos="0.18971 0.00028533 -0.025488" quat="0.706138 0.0370072 0.0370072 0.706138">
+                        <inertial pos="0.0012971 -0.011934 -0.0059999" quat="0.489677 0.510114 -0.489692 0.510099" mass="0.0042405" diaginertia="6.9402e-07 6.62904e-07 2.10916e-07"/>
+                        <joint name="L_pinky_proximal_joint" pos="0 0 0" axis="0 0 -1" range="0 1.7"/>
+                        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link21_L"/>
+                        <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link21_L"/>
+                        <body name="L_pinky_intermediate" pos="-0.0024229 -0.032041 -0.001">
+                          <inertial pos="0.0024788 -0.016208 -0.0050001" quat="0.526797 0.471683 -0.526793 0.471687" mass="0.0036036" diaginertia="4.4881e-07 4.43809e-07 6.5736e-08"/>
+                          <joint name="L_pinky_intermediate_joint" pos="0 0 0" axis="0 0 -1" range="0 1.7"/>
+                          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link22_L"/>
+                          <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link22_L"/>
+                        </body>
+                      </body>
+                    </body>
+                  </body>
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+        <body name="right_shoulder_pitch_link" pos="0 -0.14806 0.42333" quat="0.991445 -0.130526 0 0">
+          <inertial pos="0.003053 -0.06042 -0.0059" quat="0.645681 0.761799 0.0363943 0.0378496" mass="1.327" diaginertia="0.000588757 0.00053309 0.000393023"/>
+          <joint name="right_shoulder_pitch_joint" pos="0 0 0" axis="0 1 0" range="-1.57 3.14"/>
+          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="right_shoulder_pitch_link"/>
+          <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="right_shoulder_pitch_link"/>
+          <body name="right_shoulder_roll_link" pos="0.0342 -0.061999 -0.0060011" quat="0.991445 0.130526 0 0">
+            <inertial pos="-0.030932 1e-06 -0.10609" quat="0.986055 -0.000456937 0.166408 -0.00213553" mass="1.393" diaginertia="0.00200869 0.00193464 0.000449847"/>
+            <joint name="right_shoulder_roll_joint" pos="0 0 0" axis="1 0 0" range="-3.4 0.38"/>
+            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="right_shoulder_roll_link"/>
+            <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="right_shoulder_roll_link"/>
+            <body name="right_shoulder_yaw_link" pos="-0.0342 0 -0.1456">
+              <inertial pos="0.004583 -0.001128 -0.001128" quat="0.747492 -0.0267235 -0.0108866 0.663644" mass="1.505" diaginertia="0.00431782 0.00420697 0.000645658"/>
+              <joint name="right_shoulder_yaw_joint" pos="0 0 0" axis="0 0 1" range="-2.66 3.01"/>
+              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="right_shoulder_yaw_link"/>
+              <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="right_shoulder_yaw_link"/>
+              <body name="right_elbow_pitch_link" pos="0.006 -0.0329 -0.182">
+                <inertial pos="0.077092 0.028751 -0.009714" quat="0.388305 0.423352 0.610781 0.544921" mass="0.691" diaginertia="0.000942091 0.000905273 0.00023025"/>
+                <joint name="right_elbow_pitch_joint" pos="0 0 0" axis="0 1 0" range="-1.6 2.53"/>
+                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="right_elbow_pitch_link"/>
+                <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="right_elbow_pitch_link"/>
+                <body name="right_elbow_roll_link" pos="0.121 0.0329 -0.011">
+                  <inertial pos="0.035281 -0.00232 0.000337" quat="0.334998 0.622198 -0.240131 0.66557" mass="0.683" diaginertia="0.00034681 0.000328248 0.000294628"/>
+                  <joint name="right_elbow_roll_joint" pos="0 0 0" axis="1 0 0" range="-2.967 2.967"/>
+                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="right_elbow_roll_link"/>
+                  <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="right_elbow_roll_link"/>
+                  <body name="right_wrist_pitch_link" pos="0.085 0 0">
+                    <inertial pos="0.020395 3.6e-05 -0.002973" quat="0.915893 -0.228405 -0.327262 -0.0432527" mass="0.484" diaginertia="7.25675e-05 7.00325e-05 6.9381e-05"/>
+                    <joint name="right_wrist_pitch_joint" pos="0 0 0" axis="0 1 0" range="-0.471 0.349"/>
+                    <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="right_wrist_pitch_link"/>
+                    <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="right_wrist_pitch_link"/>
+                    <body name="right_wrist_yaw_link" pos="0.02 0 0">
+                      <inertial pos="0.0770303 0.0013013 -0.000699011" quat="0.482149 0.506915 0.510629 0.499827" mass="0.26543" diaginertia="0.00085381 0.000722728 0.000221145"/>
+                      <joint name="right_wrist_yaw_joint" pos="0 0 0" axis="0 0 1" range="-1.012 1.012"/>
+                      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="wrist_yaw_link"/>
+                      <geom pos="0.054 0 0" quat="0 0.707107 -0.707107 0" type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="R_hand_base_link"/>
+                      <geom pos="0.054 0 0" quat="0 0.707107 -0.707107 0" type="mesh" rgba="0.1 0.1 0.1 1" mesh="R_hand_base_link"/>
+                      <body name="R_thumb_proximal_base" pos="0.1231 0.01696 0.02045" quat="-0.707107 -2.59735e-06 -0.707107 0">
+                        <inertial pos="-0.0048064 0.0009382 -0.00757" quat="0.515015 0.680854 0.408023 0.323596" mass="0.0018869" diaginertia="8.66026e-08 6.8732e-08 4.94194e-08"/>
+                        <joint name="R_thumb_proximal_yaw_joint" pos="0 0 0" axis="0 0 -1" range="-0.1 1.3"/>
+                        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link11_R"/>
+                        <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link11_R"/>
+                        <body name="R_thumb_proximal" pos="-0.0088099 0.010892 -0.00925" quat="0.0996843 0.0996847 0.700046 0.700044">
+                          <inertial pos="0.021932 0.012785 -0.0080386" quat="-0.254474 0.660716 0.251893 0.659733" mass="0.0066075" diaginertia="2.78601e-06 2.43933e-06 8.64566e-07"/>
+                          <joint name="R_thumb_proximal_pitch_joint" pos="0 0 0" axis="0 0 1" range="-0.1 0.6"/>
+                          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link12_R"/>
+                          <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link12_R"/>
+                          <body name="R_thumb_intermediate" pos="0.04407 0.034553 -0.0008">
+                            <inertial pos="0.0095544 -0.0016282 -0.0071997" quat="0.636718 0.307389 -0.636802 0.307548" mass="0.0037847" diaginertia="4.6531e-07 4.48089e-07 2.45661e-07"/>
+                            <joint name="R_thumb_intermediate_joint" pos="0 0 0" axis="0 0 1" range="0 0.8"/>
+                            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link13_R"/>
+                            <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link13_R"/>
+                            <body name="R_thumb_distal" pos="0.020248 0.010156 -0.0012">
+                              <inertial pos="0.0092888 0.0049529 -0.0060033" quat="-0.266294 0.655967 0.262806 0.655537" mass="0.0033441" diaginertia="2.0026e-07 1.95247e-07 8.1593e-08"/>
+                              <joint name="R_thumb_distal_joint" pos="0 0 0" axis="0 0 1" range="0 1.2"/>
+                              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link14_R"/>
+                              <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link14_R"/>
+                            </body>
+                          </body>
+                        </body>
+                      </body>
+                      <body name="R_index_proximal" pos="0.19053 -0.00028533 0.032268" quat="0.706999 0.0123358 -0.0123358 -0.706999">
+                        <inertial pos="0.0012259 0.011942 -0.0060001" quat="0.50867 0.49121 -0.508643 0.491172" mass="0.0042403" diaginertia="6.9398e-07 6.62871e-07 2.10909e-07"/>
+                        <joint name="R_index_proximal_joint" pos="0 0 0" axis="0 0 1" range="0 1.7"/>
+                        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link15_R"/>
+                        <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link15_R"/>
+                        <body name="R_index_intermediate" pos="-0.0026138 0.032026 -0.001">
+                          <inertial pos="0.0019697 0.019589 -0.005" quat="0.466773 0.531152 -0.466773 0.531153" mass="0.0045683" diaginertia="7.8179e-07 7.72465e-07 8.47212e-08"/>
+                          <joint name="R_index_intermediate_joint" pos="0 0 0" axis="0 0 1" range="0 1.7"/>
+                          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link16_R"/>
+                          <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link16_R"/>
+                        </body>
+                      </body>
+                      <body name="R_middle_proximal" pos="0.1911 -0.00028533 0.01295" quat="0.707107 -2.59735e-06 2.59735e-06 -0.707107">
+                        <inertial pos="0.001297 0.011934 -0.0060001" quat="0.510131 0.489693 -0.510105 0.489653" mass="0.0042403" diaginertia="6.9397e-07 6.62865e-07 2.10915e-07"/>
+                        <joint name="R_middle_proximal_joint" pos="0 0 0" axis="0 0 1" range="0 1.7"/>
+                        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link17_R"/>
+                        <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link17_R"/>
+                        <body name="R_middle_intermediate" pos="-0.0024229 0.032041 -0.001">
+                          <inertial pos="0.001921 0.020796 -0.005" quat="0.466148 0.531627 -0.466229 0.531705" mass="0.0050396" diaginertia="9.8384e-07 9.73279e-07 9.14014e-08"/>
+                          <joint name="R_middle_intermediate_joint" pos="0 0 0" axis="0 0 1" range="0 1.7"/>
+                          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link18_R"/>
+                          <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link18_R"/>
+                        </body>
+                      </body>
+                      <body name="R_ring_proximal" pos="0.19091 -0.00028533 -0.0062872" quat="-0.706864 0.0185215 -0.0185215 0.706864">
+                        <inertial pos="0.001297 0.011934 -0.0060002" quat="0.510129 0.489691 -0.510107 0.489654" mass="0.0042403" diaginertia="6.9397e-07 6.62865e-07 2.10915e-07"/>
+                        <joint name="R_ring_proximal_joint" pos="0 0 0" axis="0 0 1" range="0 1.7"/>
+                        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link19_R"/>
+                        <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link19_R"/>
+                        <body name="R_ring_intermediate" pos="-0.0024229 0.032041 -0.001">
+                          <inertial pos="0.0021753 0.019567 -0.005" quat="0.469554 0.528695 -0.469554 0.528695" mass="0.0045683" diaginertia="7.8177e-07 7.72448e-07 8.4722e-08"/>
+                          <joint name="R_ring_intermediate_joint" pos="0 0 0" axis="0 0 1" range="0 1.7"/>
+                          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link20_R"/>
+                          <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link20_R"/>
+                        </body>
+                      </body>
+                      <body name="R_pinky_proximal" pos="0.18971 -0.00028533 -0.025488" quat="-0.706138 0.0369975 -0.0369975 0.706138">
+                        <inertial pos="0.001297 0.011934 -0.0060001" quat="0.51013 0.489693 -0.510106 0.489653" mass="0.0042403" diaginertia="6.9397e-07 6.62865e-07 2.10915e-07"/>
+                        <joint name="R_pinky_proximal_joint" pos="0 0 0" axis="0 0 1" range="0 1.7"/>
+                        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link21_R"/>
+                        <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link21_R"/>
+                        <body name="R_pinky_intermediate" pos="-0.0024229 0.032041 -0.001">
+                          <inertial pos="0.0024748 0.016203 -0.0050031" quat="0.47398 0.528862 -0.469291 0.524799" mass="0.0035996" diaginertia="4.4867e-07 4.43723e-07 6.56538e-08"/>
+                          <joint name="R_pinky_intermediate_joint" pos="0 0 0" axis="0 0 1" range="0 1.7"/>
+                          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link22_R"/>
+                          <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link22_R"/>
+                        </body>
+                      </body>
+                    </body>
+                  </body>
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+    </body>
+  </worldbody>
+
+
+  <actuator>
+    <motor name="left_hip_yaw_joint"      joint="left_hip_yaw_joint"      ctrlrange="-200 200"/>
+    <motor name="left_hip_pitch_joint"    joint="left_hip_pitch_joint"    ctrlrange="-200 200"/>
+    <motor name="left_hip_roll_joint"     joint="left_hip_roll_joint"     ctrlrange="-200 200"/>
+    <motor name="left_knee_joint"         joint="left_knee_joint"         ctrlrange="-300 300"/>
+    <motor name="left_ankle_pitch_joint"  joint="left_ankle_pitch_joint"  ctrlrange="-60 60"/>
+    <motor name="left_ankle_roll_joint"   joint="left_ankle_roll_joint"   ctrlrange="-40 40"/>
+    <motor name="right_hip_yaw_joint"     joint="right_hip_yaw_joint"     ctrlrange="-200 200"/>
+    <motor name="right_hip_pitch_joint"   joint="right_hip_pitch_joint"   ctrlrange="-200 200"/>
+    <motor name="right_hip_roll_joint"    joint="right_hip_roll_joint"    ctrlrange="-200 200"/>
+    <motor name="right_knee_joint"        joint="right_knee_joint"        ctrlrange="-300 300"/>
+    <motor name="right_ankle_pitch_joint" joint="right_ankle_pitch_joint" ctrlrange="-60 60"/>
+    <motor name="right_ankle_roll_joint"  joint="right_ankle_roll_joint"  ctrlrange="-40 40"/>
+    <motor name="torso_joint" joint="torso_joint" ctrlrange="-200 200"/>
+    <motor name="left_shoulder_pitch_joint"  joint="left_shoulder_pitch_joint"  ctrlrange="-40 40"/>
+    <motor name="left_shoulder_roll_joint"   joint="left_shoulder_roll_joint"   ctrlrange="-40 40"/>
+    <motor name="left_shoulder_yaw_joint"    joint="left_shoulder_yaw_joint"    ctrlrange="-18 18"/>
+    <motor name="left_elbow_pitch_joint"     joint="left_elbow_pitch_joint"     ctrlrange="-18 18"/>
+    <motor name="left_elbow_roll_joint"      joint="left_elbow_roll_joint"      ctrlrange="-19 19"/>
+    <motor name="left_wrist_pitch_joint"     joint="left_wrist_pitch_joint"     ctrlrange="-19 19"/>
+    <motor name="left_wrist_yaw_joint"       joint="left_wrist_yaw_joint"       ctrlrange="-19 19"/>
+    <motor name="right_shoulder_pitch_joint" joint="right_shoulder_pitch_joint" ctrlrange="-40 40"/>
+    <motor name="right_shoulder_roll_joint"  joint="right_shoulder_roll_joint"  ctrlrange="-40 40"/>
+    <motor name="right_shoulder_yaw_joint"   joint="right_shoulder_yaw_joint"   ctrlrange="-18 18"/>
+    <motor name="right_elbow_pitch_joint"    joint="right_elbow_pitch_joint"    ctrlrange="-18 18"/>
+    <motor name="right_elbow_roll_joint"     joint="right_elbow_roll_joint"     ctrlrange="-19 19"/>
+    <motor name="right_wrist_pitch_joint"    joint="right_wrist_pitch_joint"    ctrlrange="-19 19"/>
+    <motor name="right_wrist_yaw_joint"      joint="right_wrist_yaw_joint"      ctrlrange="-19 19"/>
+    <motor name="L_index_proximal_joint"       joint="L_index_proximal_joint"       ctrlrange="-1 1"/>
+    <motor name="L_index_intermediate_joint"   joint="L_index_intermediate_joint"   ctrlrange="-1 1"/>
+    <motor name="L_middle_proximal_joint"      joint="L_middle_proximal_joint"      ctrlrange="-1 1"/>
+    <motor name="L_middle_intermediate_joint"  joint="L_middle_intermediate_joint"  ctrlrange="-1 1"/>
+    <motor name="L_ring_proximal_joint"        joint="L_ring_proximal_joint"        ctrlrange="-1 1"/>
+    <motor name="L_ring_intermediate_joint"    joint="L_ring_intermediate_joint"    ctrlrange="-1 1"/>
+    <motor name="L_pinky_proximal_joint"       joint="L_pinky_proximal_joint"       ctrlrange="-1 1"/>
+    <motor name="L_pinky_intermediate_joint"   joint="L_pinky_intermediate_joint"   ctrlrange="-1 1"/>
+    <motor name="L_thumb_proximal_yaw_joint"   joint="L_thumb_proximal_yaw_joint"   ctrlrange="-1 1"/>
+    <motor name="L_thumb_proximal_pitch_joint" joint="L_thumb_proximal_pitch_joint" ctrlrange="-1 1"/>
+    <motor name="L_thumb_intermediate_joint"   joint="L_thumb_intermediate_joint"   ctrlrange="-1 1"/>
+    <motor name="L_thumb_distal_joint"         joint="L_thumb_distal_joint"         ctrlrange="-1 1"/>
+    <motor name="R_index_proximal_joint"       joint="R_index_proximal_joint"       ctrlrange="-1 1"/>
+    <motor name="R_index_intermediate_joint"   joint="R_index_intermediate_joint"   ctrlrange="-1 1"/>
+    <motor name="R_middle_proximal_joint"      joint="R_middle_proximal_joint"      ctrlrange="-1 1"/>
+    <motor name="R_middle_intermediate_joint"  joint="R_middle_intermediate_joint"  ctrlrange="-1 1"/>
+    <motor name="R_ring_proximal_joint"        joint="R_ring_proximal_joint"        ctrlrange="-1 1"/>
+    <motor name="R_ring_intermediate_joint"    joint="R_ring_intermediate_joint"    ctrlrange="-1 1"/>
+    <motor name="R_pinky_proximal_joint"       joint="R_pinky_proximal_joint"       ctrlrange="-1 1"/>
+    <motor name="R_pinky_intermediate_joint"   joint="R_pinky_intermediate_joint"   ctrlrange="-1 1"/>
+    <motor name="R_thumb_proximal_yaw_joint"   joint="R_thumb_proximal_yaw_joint"   ctrlrange="-1 1"/>
+    <motor name="R_thumb_proximal_pitch_joint" joint="R_thumb_proximal_pitch_joint" ctrlrange="-1 1"/>
+    <motor name="R_thumb_intermediate_joint"   joint="R_thumb_intermediate_joint"   ctrlrange="-1 1"/>
+    <motor name="R_thumb_distal_joint"         joint="R_thumb_distal_joint"         ctrlrange="-1 1"/>
+  </actuator>
+
+  <sensor>
+    <gyro name="imu-angular-velocity" site="imu" noise="5e-4" cutoff="34.9"/>
+    <accelerometer name="imu-linear-acceleration" site="imu" noise="1e-2" cutoff="157"/>
+  </sensor>
+</mujoco>
\ No newline at end of file
diff --git a/resources/robots/h1_2/h1_2_simplified.urdf b/resources/robots/h1_2/h1_2_simplified.urdf
new file mode 100644
index 0000000..c474fc8
--- /dev/null
+++ b/resources/robots/h1_2/h1_2_simplified.urdf
@@ -0,0 +1,1333 @@
+<robot name="h1_5">
+
+  <mujoco>
+    <compiler meshdir="meshes" discardvisual="false"/>
+  </mujoco>
+
+  <!-- [CAUTION] uncomment when convert to mujoco -->
+  <!-- <link name="world"></link>
+  <joint name="floating_base_joint" type="floating">
+    <parent link="world"/>
+    <child link="pelvis"/>
+  </joint> -->
+
+  <link name="pelvis">
+    <inertial>
+      <origin xyz="-0.0004 3.7E-05 -0.046864" rpy="0 0 0"/>
+      <mass value="5.983"/>
+      <inertia ixx="49168.411E-06" ixy="-19.869E-06" ixz="-48.460E-06" iyy="9025.844E-06" iyz="3.431E-06" izz="53155.891E-06"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/pelvis.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+
+  <!-- L Leg -->
+  <link name="left_hip_yaw_link">
+    <inertial>
+      <origin xyz="0 -0.026197 0.006647" rpy="0 0 0"/>
+      <mass value="2.829"/>
+      <inertia ixx="4553.605E-06" ixy="0.005E-06" ixz="0.817E-06" iyy="5688.730E-06" iyz="-345.157E-06" izz="3548.907E-06"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_hip_yaw_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="left_hip_yaw_joint" type="revolute">
+    <origin xyz="0 0.0875 -0.1632" rpy="0 0 0"/>
+    <parent link="pelvis"/>
+    <child link="left_hip_yaw_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-0.43" upper="0.43" effort="200" velocity="23"/>
+  </joint>
+  <link name="left_hip_pitch_link">
+    <inertial>
+      <origin xyz="-0.00781 -0.004724 -0.000063" rpy="0 0 0"/>
+      <mass value="2.920"/>
+      <inertia ixx="5594.7E-06" ixy="143.308E-06" ixz="-11.847E-06" iyy="3862.581E-06" iyz="-9.607E-06" izz="4450.563E-06"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_hip_pitch_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="left_hip_pitch_joint" type="revolute">
+    <origin xyz="0 0.0755 0" rpy="0 0 0"/>
+    <parent link="left_hip_yaw_link"/>
+    <child link="left_hip_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-3.14" upper="2.5" effort="200" velocity="23"/>
+  </joint>
+  <link name="left_hip_roll_link">
+    <inertial>
+      <origin xyz="0.004171 -0.008576 -0.194509" rpy="0 0 0"/>
+      <mass value="4.962"/>
+      <inertia ixx="46305.869E-06" ixy="-322.626E-06" ixz="-1199.703E-06" iyy="47955.402E-06" iyz="288.147E-06" izz="8914.518E-06"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_hip_roll_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="left_hip_roll_joint" type="revolute">
+    <origin xyz="0 0 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.43" upper="3.14" effort="200" velocity="23"/>
+  </joint>
+  <link name="left_knee_link">
+    <inertial>
+      <origin xyz="0.000179 0.000121 -0.168936" rpy="0 0 0"/>
+      <mass value="3.839"/>
+      <inertia ixx="39002.363e-6" ixy="-72.364e-6" ixz="-794.506e-6" iyy="39042.376e-6" iyz="-18.663e-6" izz="5029.829e-6"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_knee_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 -0.2" rpy="0 0 0"/>
+      <geometry>
+        <cylinder radius="0.02" length="0.2"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_knee_joint" type="revolute">
+    <origin xyz="0 0 -0.4" rpy="0 0 0"/>
+    <parent link="left_hip_roll_link"/>
+    <child link="left_knee_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.26" upper="2.05" effort="300" velocity="14"/>
+  </joint>
+  <link name="left_ankle_pitch_link">
+    <inertial>
+      <origin xyz="-0.000294 0 -0.010794" rpy="0 0 0"/>
+      <mass value="0.102"/>
+      <inertia ixx="0.000023944" ixy="0" ixz="0.000000121" iyy="0.000021837" iyz="0" izz="0.000013414"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_ankle_pitch_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="left_ankle_pitch_joint" type="revolute">
+    <origin xyz="0 0 -0.4" rpy="0 0 0"/>
+    <parent link="left_knee_link"/>
+    <child link="left_ankle_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.897334" upper="0.523598" effort="60" velocity="9"/>
+  </joint>
+  <link name="left_ankle_roll_link">
+    <inertial>
+      <origin xyz="0.029589 0 -0.015973" rpy="0 0 0"/>
+      <mass value="0.747"/>
+      <inertia ixx="648.830e-6" ixy="0" ixz="158.380e-6" iyy="3435.339e-6" iyz="0" izz="3583.259e-6"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_ankle_roll_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_ankle_roll_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_ankle_roll_joint" type="revolute">
+    <origin xyz="0 0 -0.02" rpy="0 0 0"/>
+    <parent link="left_ankle_pitch_link"/>
+    <child link="left_ankle_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-0.261799" upper="0.261799" effort="40" velocity="9"/>
+  </joint>
+
+  <!-- R Leg -->
+  <link name="right_hip_yaw_link">
+    <inertial>
+      <origin xyz="0 0.026197 0.006647" rpy="0 0 0"/>
+      <mass value="2.829"/>
+      <inertia ixx="4553.605E-06" ixy="-0.005E-06" ixz="0.817E-06" iyy="5688.730E-06" iyz="345.157E-06" izz="3548.907E-06"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_hip_yaw_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="right_hip_yaw_joint" type="revolute">
+    <origin xyz="0 -0.0875 -0.1632" rpy="0 0 0"/>
+    <parent link="pelvis"/>
+    <child link="right_hip_yaw_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-0.43" upper="0.43" effort="200" velocity="23"/>
+  </joint>
+  <link name="right_hip_pitch_link">
+    <inertial>
+      <origin xyz="-0.00781 0.004724 -0.000063" rpy="0 0 0"/>
+      <mass value="2.920"/>
+      <inertia ixx="5594.7E-06" ixy="-143.308E-06" ixz="-11.847E-06" iyy="3862.581E-06" iyz="9.607E-06" izz="4450.563E-06"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_hip_pitch_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="right_hip_pitch_joint" type="revolute">
+    <origin xyz="0 -0.0755 0" rpy="0 0 0"/>
+    <parent link="right_hip_yaw_link"/>
+    <child link="right_hip_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-3.14" upper="2.5" effort="200" velocity="23"/>
+  </joint>
+  <link name="right_hip_roll_link">
+    <inertial>
+      <origin xyz="0.004171 0.008576 -0.194509" rpy="0 0 0"/>
+      <mass value="4.962"/>
+      <inertia ixx="46305.869E-06" ixy="322.626E-06" ixz="-1199.703E-06" iyy="47955.402E-06" iyz="-288.147E-06" izz="8914.518E-06"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_hip_roll_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="right_hip_roll_joint" type="revolute">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="right_hip_pitch_link"/>
+    <child link="right_hip_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-3.14" upper="0.43" effort="200" velocity="23"/>
+  </joint>
+  <link name="right_knee_link">
+    <inertial>
+      <origin xyz="0.000179 -0.000121 -0.168936" rpy="0 0 0"/>
+      <mass value="3.839"/>
+      <inertia ixx="39002.363e-6" ixy="72.364e-6" ixz="-794.506e-6" iyy="39042.376e-6" iyz="18.663e-6" izz="5029.829e-6"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_knee_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 -0.2" rpy="0 0 0"/>
+      <geometry>
+        <cylinder radius="0.02" length="0.2"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_knee_joint" type="revolute">
+    <origin xyz="0 0 -0.4" rpy="0 0 0"/>
+    <parent link="right_hip_roll_link"/>
+    <child link="right_knee_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.26" upper="2.05" effort="300" velocity="14"/>
+  </joint>
+  <link name="right_ankle_pitch_link">
+    <inertial>
+      <origin xyz="-0.000294 0 -0.010794" rpy="0 0 0"/>
+      <mass value="0.102"/>
+      <inertia ixx="0.000023944" ixy="0" ixz="0.000000121" iyy="0.000021837" iyz="0" izz="0.000013414"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_ankle_pitch_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="right_ankle_pitch_joint" type="revolute">
+    <origin xyz="0 0 -0.4" rpy="0 0 0"/>
+    <parent link="right_knee_link"/>
+    <child link="right_ankle_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.897334" upper="0.523598" effort="60" velocity="9"/>
+  </joint>
+  <link name="right_ankle_roll_link">
+    <inertial>
+      <origin xyz="0.029589 0 -0.015973" rpy="0 0 0"/>
+      <mass value="0.747"/>
+      <inertia ixx="648.830e-6" ixy="0" ixz="158.380e-6" iyy="3435.339e-6" iyz="0" izz="3583.259e-6"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_ankle_roll_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_ankle_roll_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_ankle_roll_joint" type="revolute">
+    <origin xyz="0 0 -0.02" rpy="0 0 0"/>
+    <parent link="right_ankle_pitch_link"/>
+    <child link="right_ankle_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-0.261799" upper="0.261799" effort="40" velocity="9"/>
+  </joint>
+
+  <!-- Torso -->
+  <link name="torso_link">
+    <inertial>
+      <origin xyz="0.000489 0.002797 0.20484" rpy="0 0 0"/>
+      <mass value="17.789"/>
+      <inertia ixx="0.4873" ixy="-0.00053763" ixz="0.0020276" iyy="0.40963" iyz="-0.00074582" izz="0.12785"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/torso_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0.15" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.08 0.16 0.1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="torso_joint" type="revolute">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="pelvis"/>
+    <child link="torso_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-2.35" upper="2.35" effort="200" velocity="23"/>
+  </joint>
+
+  <!-- L arm -->
+  <link name="left_shoulder_pitch_link">
+    <inertial>
+      <origin xyz="0.003053 0.06042 -0.0059" rpy="0 0 0"/>
+      <mass value="1.327"/>
+      <inertia ixx="0.00058661" ixy="-2.0157E-05" ixz="2.985E-06" iyy="0.0003989" iyz="2.238E-05" izz="0.00052936"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_shoulder_pitch_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="left_shoulder_pitch_joint" type="revolute">
+    <origin xyz="0 0.14806 0.42333" rpy="0.2618 0 0"/>
+    <parent link="torso_link"/>
+    <child link="left_shoulder_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-3.14" upper="1.57" effort="40" velocity="9"/>
+  </joint>
+  <link name="left_shoulder_roll_link">
+    <inertial>
+      <origin xyz="-0.030932 -1E-06 -0.10609" rpy="0 0 0"/>
+      <mass value="1.393"/>
+      <inertia ixx="0.0018408" ixy="3.98E-07" ixz="-0.00048324" iyy="0.001934638" iyz="1.61E-07" izz="0.000617733"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_shoulder_roll_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="left_shoulder_roll_joint" type="revolute">
+    <origin xyz="0.0342 0.061999 -0.0060011" rpy="-0.2618 0 0"/>
+    <parent link="left_shoulder_pitch_link"/>
+    <child link="left_shoulder_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-0.38" upper="3.4" effort="40" velocity="9"/>
+  </joint>
+  <link name="left_shoulder_yaw_link">
+    <inertial>
+      <origin xyz="0.004583 0.001128 -0.001128" rpy="0 0 0"/>
+      <mass value="1.505"/>
+      <inertia ixx="0.004199" ixy="-1.7786E-05" ixz="0.00018372" iyy="0.0043139" iyz="9.278E-05" izz="0.00065755"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_shoulder_yaw_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="left_shoulder_yaw_joint" type="revolute">
+    <origin xyz="-0.0342 0 -0.1456" rpy="0 0 0"/>
+    <parent link="left_shoulder_roll_link"/>
+    <child link="left_shoulder_yaw_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-3.01" upper="2.66" effort="18" velocity="20"/>
+  </joint>
+  <link name="left_elbow_pitch_link">
+    <inertial>
+      <origin xyz="0.077092 -0.028751 -0.009714 " rpy="0 0 0"/>
+      <mass value="0.691"/>
+      <inertia ixx="0.000318491" ixy="0.000224554" ixz="6.62076E-05" iyy="0.000861224" iyz="-2.3329E-05" izz="0.000897899"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_elbow_pitch_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="left_elbow_pitch_joint" type="revolute">
+    <origin xyz="0.006 0.0329 -0.182" rpy="0 0 0"/>
+    <parent link="left_shoulder_yaw_link"/>
+    <child link="left_elbow_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-2.53" upper="1.6" effort="18" velocity="20"/>
+  </joint>
+
+  <link name="left_elbow_roll_link">
+    <inertial>
+      <origin xyz="0.035281 -0.002320 0.000337" rpy="0 0 0"/>
+      <mass value="0.683"/>
+      <inertia ixx="0.000313275" ixy="0.000016521" ixz="-0.000002501" iyy="0.000310412" iyz="0.000005435" izz="0.000345998"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_elbow_roll_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="left_elbow_roll_joint" type="fixed">
+    <origin xyz="0.121 -0.0329 -0.011" rpy="0 0 0"/>
+    <parent link="left_elbow_pitch_link"/>
+    <child link="left_elbow_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-2.967" upper="2.967" effort="19" velocity="31.4"/>
+  </joint>
+
+  <link name="left_wrist_pitch_link">
+    <inertial>
+      <origin xyz="0.020395 0.000036 -0.002973" rpy="0 0 0"/>
+      <mass value="0.484"/>
+      <inertia ixx="0.000071364" ixy="0.000000308" ixz="0.000001485" iyy="0.000069915" iyz="-0.000000088" izz="0.000070702"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/left_wrist_pitch_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="left_wrist_pitch_joint" type="fixed">
+    <origin xyz="0.085 0 0" rpy="0 0 0"/>
+    <parent link="left_elbow_roll_link"/>
+    <child link="left_wrist_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.471" upper="0.349" effort="19" velocity="31.4"/>
+  </joint>
+  <link name="left_wrist_yaw_link">
+    <inertial>
+      <origin xyz="0.027967 0.000096 0.000739" rpy="0 0 0"/>
+      <mass value="0.124"/>
+      <inertia ixx="137.3e-6" ixy="-0.171e-6" ixz="2.942e-6" iyy="169.998e-6" iyz="-0.025e-6" izz="84.808e-6"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/wrist_yaw_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="left_wrist_yaw_joint" type="fixed">
+    <origin xyz="0.020 0 0" rpy="0 0 0"/>
+    <parent link="left_wrist_pitch_link"/>
+    <child link="left_wrist_yaw_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-1.012" upper="1.012" effort="19" velocity="31.4"/>
+  </joint>
+
+  <!-- R arm -->
+  <link name="right_shoulder_pitch_link">
+    <inertial>
+      <origin xyz="0.003053 -0.06042 -0.0059" rpy="0 0 0"/>
+      <mass value="1.327"/>
+      <inertia ixx="0.00058661" ixy="2.0157E-05" ixz="2.985E-06" iyy="0.0003989" iyz="-2.238E-05" izz="0.00052936"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_shoulder_pitch_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="right_shoulder_pitch_joint" type="revolute">
+    <origin xyz="0 -0.14806 0.42333" rpy="-0.2618 0 0"/>
+    <parent link="torso_link"/>
+    <child link="right_shoulder_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-1.57" upper="3.14" effort="40" velocity="9"/>
+  </joint>
+  <link name="right_shoulder_roll_link">
+    <inertial>
+      <origin xyz="-0.030932 1E-06 -0.10609" rpy="0 0 0"/>
+      <mass value="1.393"/>
+      <inertia ixx="0.0018408" ixy="-3.98E-07" ixz="-0.00048324" iyy="0.001934638" iyz="-1.61E-07" izz="0.000617733"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_shoulder_roll_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="right_shoulder_roll_joint" type="revolute">
+    <origin xyz="0.0342 -0.061999 -0.0060011" rpy="0.2618 0 0"/>
+    <parent link="right_shoulder_pitch_link"/>
+    <child link="right_shoulder_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-3.4" upper="0.38" effort="40" velocity="9"/>
+  </joint>
+  <link name="right_shoulder_yaw_link">
+    <inertial>
+      <origin xyz="0.004583 -0.001128 -0.001128" rpy="0 0 0"/>
+      <mass value="1.505"/>
+      <inertia ixx="0.004199" ixy="1.7786E-05" ixz="0.00018372" iyy="0.0043139" iyz="-9.278E-05" izz="0.00065755"/>
+
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_shoulder_yaw_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="right_shoulder_yaw_joint" type="revolute">
+    <origin xyz="-0.0342 0 -0.1456" rpy="0 0 0"/>
+    <parent link="right_shoulder_roll_link"/>
+    <child link="right_shoulder_yaw_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-2.66" upper="3.01" effort="18" velocity="20"/>
+  </joint>
+  <link name="right_elbow_pitch_link">
+    <inertial>
+      <origin xyz="0.077092 0.028751 -0.009714 " rpy="0 0 0"/>
+      <mass value="0.691"/>
+      <inertia ixx="0.000318491" ixy="-0.000224554" ixz="6.62076E-05" iyy="0.000861224" iyz="2.3329E-05" izz="0.000897899"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_elbow_pitch_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="right_elbow_pitch_joint" type="revolute">
+    <origin xyz="0.006 -0.0329 -0.182" rpy="0 0 0"/>
+    <parent link="right_shoulder_yaw_link"/>
+    <child link="right_elbow_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-1.6" upper="2.53" effort="18" velocity="20"/>
+  </joint>
+
+  <link name="right_elbow_roll_link">
+    <inertial>
+      <origin xyz="0.035281 -0.002320 0.000337" rpy="0 0 0"/>
+      <mass value="0.683"/>
+      <inertia ixx="0.000313275" ixy="0.000016521" ixz="-0.000002501" iyy="0.000310412" iyz="0.000005435" izz="0.000345998"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_elbow_roll_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="right_elbow_roll_joint" type="fixed">
+    <origin xyz="0.121 0.0329 -0.011" rpy="0 0 0"/>
+    <parent link="right_elbow_pitch_link"/>
+    <child link="right_elbow_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-2.967" upper="2.967" effort="19" velocity="31.4"/>
+  </joint>
+
+  <link name="right_wrist_pitch_link">
+    <inertial>
+      <origin xyz="0.020395 0.000036 -0.002973" rpy="0 0 0"/>
+      <mass value="0.484"/>
+      <inertia ixx="0.000071364" ixy="0.000000308" ixz="0.000001485" iyy="0.000069915" iyz="-0.000000088" izz="0.000070702"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/right_wrist_pitch_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="right_wrist_pitch_joint" type="fixed">
+    <origin xyz="0.085 0 0" rpy="0 0 0"/>
+    <parent link="right_elbow_roll_link"/>
+    <child link="right_wrist_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.471" upper="0.349" effort="19" velocity="31.4"/>
+  </joint>
+  <link name="right_wrist_yaw_link">
+    <inertial>
+      <origin xyz="0.027967 -0.000096 0.000739" rpy="0 0 0"/>
+      <mass value="0.124"/>
+      <inertia ixx="137.3e-6" ixy="0.171e-6" ixz="2.942e-6" iyy="169.998e-6" iyz="0.025e-6" izz="84.808e-6"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/wrist_yaw_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="right_wrist_yaw_joint" type="fixed">
+    <origin xyz="0.020 0 0" rpy="0 0 0"/>
+    <parent link="right_wrist_pitch_link"/>
+    <child link="right_wrist_yaw_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-1.012" upper="1.012" effort="19" velocity="31.4"/>
+  </joint>
+
+  <!-- LOGO -->
+  <link name="logo_link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/logo_link.STL"/>
+      </geometry>
+      <material name="white">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="logo_joint" type="fixed">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="torso_link"/>
+    <child link="logo_link"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+
+  <!-- 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>
+
+  <!-- L Gripper -->
+  <joint name="L_base_link_joint" type="fixed">
+    <origin xyz="0.054 0 0" rpy="0 0 1.5707963267948966192313216916398"/>
+    <parent link="left_wrist_yaw_link"/>
+    <child link="L_hand_base_link"/>
+  </joint>
+  <link name="L_hand_base_link">
+    <inertial>
+      <origin xyz="-0.002551 -0.066047 -0.0019357" rpy="0 0 0"/>
+      <mass value="0.14143"/>
+      <inertia ixx="0.0001234" ixy="2.1995E-06" ixz="-1.7694E-06" iyy="8.3835E-05" iyz="1.5968E-06" izz="7.7231E-05"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/L_hand_base_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="L_thumb_proximal_base">
+    <inertial>
+      <origin xyz="0.0048817 0.00038782 -0.00722" rpy="0 0 0"/>
+      <mass value="0.0018869"/>
+      <inertia ixx="5.5158E-08" ixy="-1.1803E-08" ixz="-4.6743E-09" iyy="8.2164E-08" iyz="-1.3521E-09" izz="6.7434E-08"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link11_L.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="L_thumb_proximal_yaw_joint" type="fixed">
+    <origin xyz="-0.01696 -0.0691 0.02045" rpy="1.5708 -1.5708 0"/>
+    <parent link="L_hand_base_link"/>
+    <child link="L_thumb_proximal_base"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-0.1" upper="1.3" effort="1" velocity="0.5"/>
+  </joint>
+  <link name="L_thumb_proximal">
+    <inertial>
+      <origin xyz="0.021936 -0.01279 -0.0080386" rpy="0 0 0"/>
+      <mass value="0.0066101"/>
+      <inertia ixx="1.5693E-06" ixy="7.8339E-07" ixz="8.5959E-10" iyy="1.7356E-06" iyz="1.0378E-09" izz="2.787E-06"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link12_L.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="L_thumb_proximal_pitch_joint" type="fixed">
+    <origin xyz="0.0099867 0.0098242 -0.0089" rpy="-1.5708 0 0.16939"/>
+    <parent link="L_thumb_proximal_base"/>
+    <child link="L_thumb_proximal"/>
+    <axis xyz="0 0 -1"/>
+    <limit lower="-0.1" upper="0.6" effort="1" velocity="0.5"/>
+  </joint>
+  <link name="L_thumb_intermediate">
+    <inertial>
+      <origin xyz="0.0095531 0.0016282 -0.0072002" rpy="0 0 0"/>
+      <mass value="0.0037844"/>
+      <inertia ixx="3.6981E-07" ixy="9.8603E-08" ixz="-2.8173E-12" iyy="3.2395E-07" iyz="-2.8028E-12" izz="4.6532E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link13_L.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="L_thumb_intermediate_joint" type="fixed">
+    <origin xyz="0.04407 -0.034553 -0.0008" rpy="0 0 0"/>
+    <parent link="L_thumb_proximal"/>
+    <child link="L_thumb_intermediate"/>
+    <axis xyz="0 0 -1"/>
+    <limit lower="0" upper="0.8" effort="1" velocity="0.5"/>
+    <mimic joint="L_thumb_proximal_pitch_joint" multiplier="1.6" offset="0"/>
+  </joint>
+  <link name="L_thumb_distal">
+    <inertial>
+      <origin xyz="0.0092888 -0.004953 -0.0060033" rpy="0 0 0"/>
+      <mass value="0.003344"/>
+      <inertia ixx="1.3632E-07" ixy="5.6787E-08" ixz="-9.1939E-11" iyy="1.4052E-07" iyz="1.2145E-10" izz="2.0026E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link14_L.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="L_thumb_distal_joint" type="fixed">
+    <origin xyz="0.020248 -0.010156 -0.0012" rpy="0 0 0"/>
+    <parent link="L_thumb_intermediate"/>
+    <child link="L_thumb_distal"/>
+    <axis xyz="0 0 -1"/>
+    <limit lower="0" upper="1.2" effort="1" velocity="0.5"/>
+    <mimic joint="L_thumb_proximal_pitch_joint" multiplier="2.4" offset="0"/>
+  </joint>
+  <link name="L_index_proximal">
+    <inertial>
+      <origin xyz="0.0012971 -0.011934 -0.0059998" rpy="0 0 0"/>
+      <mass value="0.0042405"/>
+      <inertia ixx="6.6215E-07" ixy="1.8442E-08" ixz="1.3746E-12" iyy="2.1167E-07" iyz="-1.4773E-11" izz="6.9402E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link15_L.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="L_index_proximal_joint" type="fixed">
+    <origin xyz="0.00028533 -0.13653 0.032268" rpy="-0.034907 0 0"/>
+    <parent link="L_hand_base_link"/>
+    <child link="L_index_proximal"/>
+    <axis xyz="0 0 -1"/>
+    <limit lower="0" upper="1.7" effort="1" velocity="0.5"/>
+  </joint>
+  <link name="L_index_intermediate">
+    <inertial>
+      <origin xyz="0.0021753 -0.019567 -0.005" rpy="0 0 0"/>
+      <mass value="0.0045682"/>
+      <inertia ixx="7.6284E-07" ixy="-8.063E-08" ixz="3.6797E-13" iyy="9.4308E-08" iyz="1.5743E-13" izz="7.8176E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link16_L.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="L_index_intermediate_joint" type="fixed">
+    <origin xyz="-0.0024229 -0.032041 -0.001" rpy="0 0 0"/>
+    <parent link="L_index_proximal"/>
+    <child link="L_index_intermediate"/>
+    <axis xyz="0 0 -1"/>
+    <limit lower="0" upper="1.7" effort="1" velocity="0.5"/>
+    <mimic joint="L_index_proximal_joint" multiplier="1" offset="0"/>
+  </joint>
+  <link name="L_middle_proximal">
+    <inertial>
+      <origin xyz="0.0012971 -0.011934 -0.0059999" rpy="0 0 0"/>
+      <mass value="0.0042405"/>
+      <inertia ixx="6.6215E-07" ixy="1.8442E-08" ixz="1.2299E-12" iyy="2.1167E-07" iyz="-1.4484E-11" izz="6.9402E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link17_L.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="L_middle_proximal_joint" type="fixed">
+    <origin xyz="0.00028533 -0.1371 0.01295" rpy="0 0 0"/>
+    <parent link="L_hand_base_link"/>
+    <child link="L_middle_proximal"/>
+    <axis xyz="0 0 -1"/>
+    <limit lower="0" upper="1.7" effort="1" velocity="0.5"/>
+  </joint>
+  <link name="L_middle_intermediate">
+    <inertial>
+      <origin xyz="0.001921 -0.020796 -0.0049999" rpy="0 0 0"/>
+      <mass value="0.0050397"/>
+      <inertia ixx="9.5823E-07" ixy="-1.1425E-07" ixz="-2.4186E-12" iyy="1.0646E-07" iyz="3.6974E-12" izz="9.8385E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link18_L.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="L_middle_intermediate_joint" type="fixed">
+    <origin xyz="-0.0024229 -0.032041 -0.001" rpy="0 0 0"/>
+    <parent link="L_middle_proximal"/>
+    <child link="L_middle_intermediate"/>
+    <axis xyz="0 0 -1"/>
+    <limit lower="0" upper="1.7" effort="1" velocity="0.5"/>
+    <mimic joint="L_middle_proximal_joint" multiplier="1" offset="0"/>
+  </joint>
+  <link name="L_ring_proximal">
+    <inertial>
+      <origin xyz="0.0012971 -0.011934 -0.0059999" rpy="0 0 0"/>
+      <mass value="0.0042405"/>
+      <inertia ixx="6.6215E-07" ixy="1.8442E-08" ixz="9.6052E-13" iyy="2.1167E-07" iyz="-1.4124E-11" izz="6.9402E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link19_L.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="L_ring_proximal_joint" type="fixed">
+    <origin xyz="0.00028533 -0.13691 -0.0062872" rpy="0.05236 0 0"/>
+    <parent link="L_hand_base_link"/>
+    <child link="L_ring_proximal"/>
+    <axis xyz="0 0 -1"/>
+    <limit lower="0" upper="1.7" effort="1" velocity="0.5"/>
+  </joint>
+  <link name="L_ring_intermediate">
+    <inertial>
+      <origin xyz="0.0021753 -0.019567 -0.005" rpy="0 0 0"/>
+      <mass value="0.0045682"/>
+      <inertia ixx="7.6285E-07" ixy="-8.0631E-08" ixz="3.3472E-14" iyy="9.4308E-08" iyz="-4.4773E-13" izz="7.8176E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link20_L.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="L_ring_intermediate_joint" type="fixed">
+    <origin xyz="-0.0024229 -0.032041 -0.001" rpy="0 0 0"/>
+    <parent link="L_ring_proximal"/>
+    <child link="L_ring_intermediate"/>
+    <axis xyz="0 0 -1"/>
+    <limit lower="0" upper="1.7" effort="1" velocity="0.5"/>
+    <mimic joint="L_ring_proximal_joint" multiplier="1" offset="0"/>
+  </joint>
+  <link name="L_pinky_proximal">
+    <inertial>
+      <origin xyz="0.0012971 -0.011934 -0.0059999" rpy="0 0 0"/>
+      <mass value="0.0042405"/>
+      <inertia ixx="6.6215E-07" ixy="1.8442E-08" ixz="1.0279E-12" iyy="2.1167E-07" iyz="-1.4277E-11" izz="6.9402E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link21_L.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="L_pinky_proximal_joint" type="fixed">
+    <origin xyz="0.00028533 -0.13571 -0.025488" rpy="0.10472 0 0"/>
+    <parent link="L_hand_base_link"/>
+    <child link="L_pinky_proximal"/>
+    <axis xyz="0 0 -1"/>
+    <limit lower="0" upper="1.7" effort="1" velocity="0.5"/>
+  </joint>
+  <link name="L_pinky_intermediate">
+    <inertial>
+      <origin xyz="0.0024788 -0.016208 -0.0050001" rpy="0 0 0"/>
+      <mass value="0.0036036"/>
+      <inertia ixx="4.3923E-07" ixy="-4.1355E-08" ixz="1.2263E-12" iyy="7.0315E-08" iyz="3.1311E-12" izz="4.4881E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link22_L.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="L_pinky_intermediate_joint" type="fixed">
+    <origin xyz="-0.0024229 -0.032041 -0.001" rpy="0 0 0"/>
+    <parent link="L_pinky_proximal"/>
+    <child link="L_pinky_intermediate"/>
+    <axis xyz="0 0 -1"/>
+    <limit lower="0" upper="1.7" effort="1" velocity="0.5"/>
+    <mimic joint="L_pinky_proximal_joint" multiplier="1" offset="0"/>
+  </joint>
+
+  <!-- R Gripper -->
+  <joint name="R_base_link_joint" type="fixed">
+    <origin xyz="0.054 0 0" rpy="3.1415926535897932384626433832795 0 -1.5707963267948966192313216916398"/>
+    <parent link="right_wrist_yaw_link"/>
+    <child link="R_hand_base_link"/>
+  </joint>
+  <link name="R_hand_base_link">
+    <inertial>
+      <origin xyz="-0.0025264 -0.066047 0.0019598" rpy="0 0 0"/>
+      <mass value="0.14143"/>
+      <inertia ixx="0.00012281" ixy="2.1711E-06" ixz="1.7709E-06" iyy="8.3832E-05" iyz="-1.6551E-06" izz="7.6663E-05"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/R_hand_base_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="R_thumb_proximal_base">
+    <inertial>
+      <origin xyz="-0.0048064 0.0009382 -0.00757" rpy="0 0 0"/>
+      <mass value="0.0018869"/>
+      <inertia ixx="5.816E-08" ixy="1.4539E-08" ixz="4.491E-09" iyy="7.9161E-08" iyz="-1.8727E-09" izz="6.7433E-08"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link11_R.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="R_thumb_proximal_yaw_joint" type="fixed">
+    <origin xyz="-0.01696 -0.0691 -0.02045" rpy="1.5708 -1.5708 0"/>
+    <parent link="R_hand_base_link"/>
+    <child link="R_thumb_proximal_base"/>
+    <axis xyz="0 0 -1"/>
+    <limit lower="-0.1" upper="1.3" effort="1" velocity="0.5"/>
+  </joint>
+  <link name="R_thumb_proximal">
+    <inertial>
+      <origin xyz="0.021932 0.012785 -0.0080386" rpy="0 0 0"/>
+      <mass value="0.0066075"/>
+      <inertia ixx="1.5686E-06" ixy="-7.8296E-07" ixz="8.9143E-10" iyy="1.7353E-06" iyz="-1.0191E-09" izz="2.786E-06"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link12_R.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="R_thumb_proximal_pitch_joint" type="fixed">
+    <origin xyz="-0.0088099 0.010892 -0.00925" rpy="1.5708 0 2.8587"/>
+    <parent link="R_thumb_proximal_base"/>
+    <child link="R_thumb_proximal"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-0.1" upper="0.6" effort="1" velocity="0.5"/>
+  </joint>
+  <link name="R_thumb_intermediate">
+    <inertial>
+      <origin xyz="0.0095544 -0.0016282 -0.0071997" rpy="0 0 0"/>
+      <mass value="0.0037847"/>
+      <inertia ixx="3.6981E-07" ixy="-9.8581E-08" ixz="-4.7469E-12" iyy="3.2394E-07" iyz="1.0939E-12" izz="4.6531E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link13_R.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="R_thumb_intermediate_joint" type="fixed">
+    <origin xyz="0.04407 0.034553 -0.0008" rpy="0 0 0"/>
+    <parent link="R_thumb_proximal"/>
+    <child link="R_thumb_intermediate"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="0" upper="0.8" effort="1" velocity="0.5"/>
+    <mimic joint="R_thumb_proximal_pitch_joint" multiplier="1.6" offset="0"/>
+  </joint>
+  <link name="R_thumb_distal">
+    <inertial>
+      <origin xyz="0.0092888 0.0049529 -0.0060033" rpy="0 0 0"/>
+      <mass value="0.0033441"/>
+      <inertia ixx="1.3632E-07" ixy="-5.6788E-08" ixz="-9.2764E-11" iyy="1.4052E-07" iyz="-1.2283E-10" izz="2.0026E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link14_R.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="R_thumb_distal_joint" type="fixed">
+    <origin xyz="0.020248 0.010156 -0.0012" rpy="0 0 0"/>
+    <parent link="R_thumb_intermediate"/>
+    <child link="R_thumb_distal"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="0" upper="1.2" effort="1" velocity="0.5"/>
+    <mimic joint="R_thumb_proximal_pitch_joint" multiplier="2.4" offset="0"/>
+  </joint>
+  <link name="R_index_proximal">
+    <inertial>
+      <origin xyz="0.0012259 0.011942 -0.0060001" rpy="0 0 0"/>
+      <mass value="0.0042403"/>
+      <inertia ixx="6.6232E-07" ixy="-1.5775E-08" ixz="1.8515E-12" iyy="2.1146E-07" iyz="-5.0828E-12" izz="6.9398E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link15_R.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="R_index_proximal_joint" type="fixed">
+    <origin xyz="0.00028533 -0.13653 -0.032268" rpy="-3.1067 0 0"/>
+    <parent link="R_hand_base_link"/>
+    <child link="R_index_proximal"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="0" upper="1.7" effort="1" velocity="0.5"/>
+  </joint>
+  <link name="R_index_intermediate">
+    <inertial>
+      <origin xyz="0.0019697 0.019589 -0.005" rpy="0 0 0"/>
+      <mass value="0.0045683"/>
+      <inertia ixx="7.6111E-07" ixy="8.7637E-08" ixz="-3.7751E-13" iyy="9.6076E-08" iyz="9.9444E-13" izz="7.8179E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link16_R.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="R_index_intermediate_joint" type="fixed">
+    <origin xyz="-0.0026138 0.032026 -0.001" rpy="0 0 0"/>
+    <parent link="R_index_proximal"/>
+    <child link="R_index_intermediate"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="0" upper="1.7" effort="1" velocity="0.5"/>
+    <mimic joint="R_index_proximal_joint" multiplier="1" offset="0"/>
+  </joint>
+  <link name="R_middle_proximal">
+    <inertial>
+      <origin xyz="0.001297 0.011934 -0.0060001" rpy="0 0 0"/>
+      <mass value="0.0042403"/>
+      <inertia ixx="6.6211E-07" ixy="-1.8461E-08" ixz="1.8002E-12" iyy="2.1167E-07" iyz="-6.6808E-12" izz="6.9397E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link17_R.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="R_middle_proximal_joint" type="fixed">
+    <origin xyz="0.00028533 -0.1371 -0.01295" rpy="-3.1416 0 0"/>
+    <parent link="R_hand_base_link"/>
+    <child link="R_middle_proximal"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="0" upper="1.7" effort="1" velocity="0.5"/>
+  </joint>
+  <link name="R_middle_intermediate">
+    <inertial>
+      <origin xyz="0.001921 0.020796 -0.005" rpy="0 0 0"/>
+      <mass value="0.0050396"/>
+      <inertia ixx="9.5822E-07" ixy="1.1425E-07" ixz="-2.4791E-12" iyy="1.0646E-07" iyz="5.9173E-12" izz="9.8384E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link18_R.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="R_middle_intermediate_joint" type="fixed">
+    <origin xyz="-0.0024229 0.032041 -0.001" rpy="0 0 0"/>
+    <parent link="R_middle_proximal"/>
+    <child link="R_middle_intermediate"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="0" upper="1.7" effort="1" velocity="0.5"/>
+    <mimic joint="R_middle_proximal_joint" multiplier="1" offset="0"/>
+  </joint>
+  <link name="R_ring_proximal">
+    <inertial>
+      <origin xyz="0.001297 0.011934 -0.0060002" rpy="0 0 0"/>
+      <mass value="0.0042403"/>
+      <inertia ixx="6.6211E-07" ixy="-1.8461E-08" ixz="1.5793E-12" iyy="2.1167E-07" iyz="-6.6868E-12" izz="6.9397E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link19_R.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="R_ring_proximal_joint" type="fixed">
+    <origin xyz="0.00028533 -0.13691 0.0062872" rpy="3.0892 0 0"/>
+    <parent link="R_hand_base_link"/>
+    <child link="R_ring_proximal"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="0" upper="1.7" effort="1" velocity="0.5"/>
+  </joint>
+  <link name="R_ring_intermediate">
+    <inertial>
+      <origin xyz="0.0021753 0.019567 -0.005" rpy="0 0 0"/>
+      <mass value="0.0045683"/>
+      <inertia ixx="7.6286E-07" ixy="8.0635E-08" ixz="-6.1562E-13" iyy="9.431E-08" iyz="5.8619E-13" izz="7.8177E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link20_R.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="R_ring_intermediate_joint" type="fixed">
+    <origin xyz="-0.0024229 0.032041 -0.001" rpy="0 0 0"/>
+    <parent link="R_ring_proximal"/>
+    <child link="R_ring_intermediate"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="0" upper="1.7" effort="1" velocity="0.5"/>
+    <mimic joint="R_ring_proximal_joint" multiplier="1" offset="0"/>
+  </joint>
+  <link name="R_pinky_proximal">
+    <inertial>
+      <origin xyz="0.001297 0.011934 -0.0060001" rpy="0 0 0"/>
+      <mass value="0.0042403"/>
+      <inertia ixx="6.6211E-07" ixy="-1.8461E-08" ixz="1.6907E-12" iyy="2.1167E-07" iyz="-6.9334E-12" izz="6.9397E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link21_R.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="R_pinky_proximal_joint" type="fixed">
+    <origin xyz="0.00028533 -0.13571 0.025488" rpy="3.0369 0 0"/>
+    <parent link="R_hand_base_link"/>
+    <child link="R_pinky_proximal"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="0" upper="1.7" effort="1" velocity="0.5"/>
+  </joint>
+  <link name="R_pinky_intermediate">
+    <inertial>
+      <origin xyz="0.0024748 0.016203 -0.0050031" rpy="0 0 0"/>
+      <mass value="0.0035996"/>
+      <inertia ixx="4.3913E-07" ixy="4.1418E-08" ixz="3.7168E-11" iyy="7.0247E-08" iyz="5.8613E-11" izz="4.4867E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link22_R.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="R_pinky_intermediate_joint" type="fixed">
+    <origin xyz="-0.0024229 0.032041 -0.001" rpy="0 0 0"/>
+    <parent link="R_pinky_proximal"/>
+    <child link="R_pinky_intermediate"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="0" upper="1.7" effort="1" velocity="0.5"/>
+    <mimic joint="R_pinky_proximal_joint" multiplier="1" offset="0"/>
+  </joint>
+</robot>
\ No newline at end of file
diff --git a/resources/robots/h1_2/meshes/L_hand_base_link.STL b/resources/robots/h1_2/meshes/L_hand_base_link.STL
new file mode 100644
index 0000000..a2f67ee
Binary files /dev/null and b/resources/robots/h1_2/meshes/L_hand_base_link.STL differ
diff --git a/resources/robots/h1_2/meshes/L_index_intermediate.STL b/resources/robots/h1_2/meshes/L_index_intermediate.STL
new file mode 100644
index 0000000..24a5a83
Binary files /dev/null and b/resources/robots/h1_2/meshes/L_index_intermediate.STL differ
diff --git a/resources/robots/h1_2/meshes/L_index_proximal.STL b/resources/robots/h1_2/meshes/L_index_proximal.STL
new file mode 100644
index 0000000..f899431
Binary files /dev/null and b/resources/robots/h1_2/meshes/L_index_proximal.STL differ
diff --git a/resources/robots/h1_2/meshes/L_middle_intermediate.STL b/resources/robots/h1_2/meshes/L_middle_intermediate.STL
new file mode 100644
index 0000000..c498cd7
Binary files /dev/null and b/resources/robots/h1_2/meshes/L_middle_intermediate.STL differ
diff --git a/resources/robots/h1_2/meshes/L_middle_proximal.STL b/resources/robots/h1_2/meshes/L_middle_proximal.STL
new file mode 100644
index 0000000..8f0aca6
Binary files /dev/null and b/resources/robots/h1_2/meshes/L_middle_proximal.STL differ
diff --git a/resources/robots/h1_2/meshes/L_pinky_intermediate.STL b/resources/robots/h1_2/meshes/L_pinky_intermediate.STL
new file mode 100644
index 0000000..6b9cdad
Binary files /dev/null and b/resources/robots/h1_2/meshes/L_pinky_intermediate.STL differ
diff --git a/resources/robots/h1_2/meshes/L_pinky_proximal.STL b/resources/robots/h1_2/meshes/L_pinky_proximal.STL
new file mode 100644
index 0000000..ccb63bf
Binary files /dev/null and b/resources/robots/h1_2/meshes/L_pinky_proximal.STL differ
diff --git a/resources/robots/h1_2/meshes/L_ring_intermediate.STL b/resources/robots/h1_2/meshes/L_ring_intermediate.STL
new file mode 100644
index 0000000..23c4efa
Binary files /dev/null and b/resources/robots/h1_2/meshes/L_ring_intermediate.STL differ
diff --git a/resources/robots/h1_2/meshes/L_ring_proximal.STL b/resources/robots/h1_2/meshes/L_ring_proximal.STL
new file mode 100644
index 0000000..c3cabb9
Binary files /dev/null and b/resources/robots/h1_2/meshes/L_ring_proximal.STL differ
diff --git a/resources/robots/h1_2/meshes/L_thumb_distal.STL b/resources/robots/h1_2/meshes/L_thumb_distal.STL
new file mode 100644
index 0000000..291a14f
Binary files /dev/null and b/resources/robots/h1_2/meshes/L_thumb_distal.STL differ
diff --git a/resources/robots/h1_2/meshes/L_thumb_intermediate.STL b/resources/robots/h1_2/meshes/L_thumb_intermediate.STL
new file mode 100644
index 0000000..03a3425
Binary files /dev/null and b/resources/robots/h1_2/meshes/L_thumb_intermediate.STL differ
diff --git a/resources/robots/h1_2/meshes/L_thumb_proximal.STL b/resources/robots/h1_2/meshes/L_thumb_proximal.STL
new file mode 100644
index 0000000..08071d3
Binary files /dev/null and b/resources/robots/h1_2/meshes/L_thumb_proximal.STL differ
diff --git a/resources/robots/h1_2/meshes/L_thumb_proximal_base.STL b/resources/robots/h1_2/meshes/L_thumb_proximal_base.STL
new file mode 100644
index 0000000..9933a6c
Binary files /dev/null and b/resources/robots/h1_2/meshes/L_thumb_proximal_base.STL differ
diff --git a/resources/robots/h1_2/meshes/R_hand_base_link.STL b/resources/robots/h1_2/meshes/R_hand_base_link.STL
new file mode 100644
index 0000000..3478a98
Binary files /dev/null and b/resources/robots/h1_2/meshes/R_hand_base_link.STL differ
diff --git a/resources/robots/h1_2/meshes/R_index_intermediate.STL b/resources/robots/h1_2/meshes/R_index_intermediate.STL
new file mode 100644
index 0000000..0cf0f8d
Binary files /dev/null and b/resources/robots/h1_2/meshes/R_index_intermediate.STL differ
diff --git a/resources/robots/h1_2/meshes/R_index_proximal.STL b/resources/robots/h1_2/meshes/R_index_proximal.STL
new file mode 100644
index 0000000..cbfc711
Binary files /dev/null and b/resources/robots/h1_2/meshes/R_index_proximal.STL differ
diff --git a/resources/robots/h1_2/meshes/R_middle_intermediate.STL b/resources/robots/h1_2/meshes/R_middle_intermediate.STL
new file mode 100644
index 0000000..ebc2616
Binary files /dev/null and b/resources/robots/h1_2/meshes/R_middle_intermediate.STL differ
diff --git a/resources/robots/h1_2/meshes/R_middle_proximal.STL b/resources/robots/h1_2/meshes/R_middle_proximal.STL
new file mode 100644
index 0000000..3f630a3
Binary files /dev/null and b/resources/robots/h1_2/meshes/R_middle_proximal.STL differ
diff --git a/resources/robots/h1_2/meshes/R_pinky_intermediate.STL b/resources/robots/h1_2/meshes/R_pinky_intermediate.STL
new file mode 100644
index 0000000..c2cf58b
Binary files /dev/null and b/resources/robots/h1_2/meshes/R_pinky_intermediate.STL differ
diff --git a/resources/robots/h1_2/meshes/R_pinky_proximal.STL b/resources/robots/h1_2/meshes/R_pinky_proximal.STL
new file mode 100644
index 0000000..d4710f1
Binary files /dev/null and b/resources/robots/h1_2/meshes/R_pinky_proximal.STL differ
diff --git a/resources/robots/h1_2/meshes/R_ring_intermediate.STL b/resources/robots/h1_2/meshes/R_ring_intermediate.STL
new file mode 100644
index 0000000..5fed852
Binary files /dev/null and b/resources/robots/h1_2/meshes/R_ring_intermediate.STL differ
diff --git a/resources/robots/h1_2/meshes/R_ring_proximal.STL b/resources/robots/h1_2/meshes/R_ring_proximal.STL
new file mode 100644
index 0000000..5c3703e
Binary files /dev/null and b/resources/robots/h1_2/meshes/R_ring_proximal.STL differ
diff --git a/resources/robots/h1_2/meshes/R_thumb_distal.STL b/resources/robots/h1_2/meshes/R_thumb_distal.STL
new file mode 100644
index 0000000..13f98f8
Binary files /dev/null and b/resources/robots/h1_2/meshes/R_thumb_distal.STL differ
diff --git a/resources/robots/h1_2/meshes/R_thumb_intermediate.STL b/resources/robots/h1_2/meshes/R_thumb_intermediate.STL
new file mode 100644
index 0000000..a42829f
Binary files /dev/null and b/resources/robots/h1_2/meshes/R_thumb_intermediate.STL differ
diff --git a/resources/robots/h1_2/meshes/R_thumb_proximal.STL b/resources/robots/h1_2/meshes/R_thumb_proximal.STL
new file mode 100644
index 0000000..137dbde
Binary files /dev/null and b/resources/robots/h1_2/meshes/R_thumb_proximal.STL differ
diff --git a/resources/robots/h1_2/meshes/R_thumb_proximal_base.STL b/resources/robots/h1_2/meshes/R_thumb_proximal_base.STL
new file mode 100644
index 0000000..30817a8
Binary files /dev/null and b/resources/robots/h1_2/meshes/R_thumb_proximal_base.STL differ
diff --git a/resources/robots/h1_2/meshes/left_ankle_A_link.STL b/resources/robots/h1_2/meshes/left_ankle_A_link.STL
new file mode 100644
index 0000000..541cb71
Binary files /dev/null and b/resources/robots/h1_2/meshes/left_ankle_A_link.STL differ
diff --git a/resources/robots/h1_2/meshes/left_ankle_A_rod_link.STL b/resources/robots/h1_2/meshes/left_ankle_A_rod_link.STL
new file mode 100644
index 0000000..281dc11
Binary files /dev/null and b/resources/robots/h1_2/meshes/left_ankle_A_rod_link.STL differ
diff --git a/resources/robots/h1_2/meshes/left_ankle_B_link.STL b/resources/robots/h1_2/meshes/left_ankle_B_link.STL
new file mode 100644
index 0000000..cd54990
Binary files /dev/null and b/resources/robots/h1_2/meshes/left_ankle_B_link.STL differ
diff --git a/resources/robots/h1_2/meshes/left_ankle_B_rod_link.STL b/resources/robots/h1_2/meshes/left_ankle_B_rod_link.STL
new file mode 100644
index 0000000..8d80e0a
Binary files /dev/null and b/resources/robots/h1_2/meshes/left_ankle_B_rod_link.STL differ
diff --git a/resources/robots/h1_2/meshes/left_ankle_pitch_link.STL b/resources/robots/h1_2/meshes/left_ankle_pitch_link.STL
new file mode 100644
index 0000000..827fb2c
Binary files /dev/null and b/resources/robots/h1_2/meshes/left_ankle_pitch_link.STL differ
diff --git a/resources/robots/h1_2/meshes/left_ankle_roll_link.STL b/resources/robots/h1_2/meshes/left_ankle_roll_link.STL
new file mode 100644
index 0000000..5fe6027
Binary files /dev/null and b/resources/robots/h1_2/meshes/left_ankle_roll_link.STL differ
diff --git a/resources/robots/h1_2/meshes/left_elbow_pitch_link.STL b/resources/robots/h1_2/meshes/left_elbow_pitch_link.STL
new file mode 100644
index 0000000..55ace3f
Binary files /dev/null and b/resources/robots/h1_2/meshes/left_elbow_pitch_link.STL differ
diff --git a/resources/robots/h1_2/meshes/left_elbow_roll_link.STL b/resources/robots/h1_2/meshes/left_elbow_roll_link.STL
new file mode 100644
index 0000000..112437b
Binary files /dev/null and b/resources/robots/h1_2/meshes/left_elbow_roll_link.STL differ
diff --git a/resources/robots/h1_2/meshes/left_hand_link.STL b/resources/robots/h1_2/meshes/left_hand_link.STL
new file mode 100644
index 0000000..6ce2dcf
Binary files /dev/null and b/resources/robots/h1_2/meshes/left_hand_link.STL differ
diff --git a/resources/robots/h1_2/meshes/left_hip_pitch_link.STL b/resources/robots/h1_2/meshes/left_hip_pitch_link.STL
new file mode 100644
index 0000000..924c0e5
Binary files /dev/null and b/resources/robots/h1_2/meshes/left_hip_pitch_link.STL differ
diff --git a/resources/robots/h1_2/meshes/left_hip_roll_link.STL b/resources/robots/h1_2/meshes/left_hip_roll_link.STL
new file mode 100644
index 0000000..a9353e9
Binary files /dev/null and b/resources/robots/h1_2/meshes/left_hip_roll_link.STL differ
diff --git a/resources/robots/h1_2/meshes/left_hip_yaw_link.STL b/resources/robots/h1_2/meshes/left_hip_yaw_link.STL
new file mode 100644
index 0000000..2b97fd7
Binary files /dev/null and b/resources/robots/h1_2/meshes/left_hip_yaw_link.STL differ
diff --git a/resources/robots/h1_2/meshes/left_knee_link.STL b/resources/robots/h1_2/meshes/left_knee_link.STL
new file mode 100644
index 0000000..fe89e75
Binary files /dev/null and b/resources/robots/h1_2/meshes/left_knee_link.STL differ
diff --git a/resources/robots/h1_2/meshes/left_shoulder_pitch_link.STL b/resources/robots/h1_2/meshes/left_shoulder_pitch_link.STL
new file mode 100644
index 0000000..23289ce
Binary files /dev/null and b/resources/robots/h1_2/meshes/left_shoulder_pitch_link.STL differ
diff --git a/resources/robots/h1_2/meshes/left_shoulder_roll_link.STL b/resources/robots/h1_2/meshes/left_shoulder_roll_link.STL
new file mode 100644
index 0000000..a28d854
Binary files /dev/null and b/resources/robots/h1_2/meshes/left_shoulder_roll_link.STL differ
diff --git a/resources/robots/h1_2/meshes/left_shoulder_yaw_link.STL b/resources/robots/h1_2/meshes/left_shoulder_yaw_link.STL
new file mode 100644
index 0000000..87cbbbc
Binary files /dev/null and b/resources/robots/h1_2/meshes/left_shoulder_yaw_link.STL differ
diff --git a/resources/robots/h1_2/meshes/left_wrist_pitch_link.STL b/resources/robots/h1_2/meshes/left_wrist_pitch_link.STL
new file mode 100644
index 0000000..83d6d87
Binary files /dev/null and b/resources/robots/h1_2/meshes/left_wrist_pitch_link.STL differ
diff --git a/resources/robots/h1_2/meshes/link11_L.STL b/resources/robots/h1_2/meshes/link11_L.STL
new file mode 100644
index 0000000..9933a6c
Binary files /dev/null and b/resources/robots/h1_2/meshes/link11_L.STL differ
diff --git a/resources/robots/h1_2/meshes/link11_R.STL b/resources/robots/h1_2/meshes/link11_R.STL
new file mode 100644
index 0000000..30817a8
Binary files /dev/null and b/resources/robots/h1_2/meshes/link11_R.STL differ
diff --git a/resources/robots/h1_2/meshes/link12_L.STL b/resources/robots/h1_2/meshes/link12_L.STL
new file mode 100644
index 0000000..08071d3
Binary files /dev/null and b/resources/robots/h1_2/meshes/link12_L.STL differ
diff --git a/resources/robots/h1_2/meshes/link12_R.STL b/resources/robots/h1_2/meshes/link12_R.STL
new file mode 100644
index 0000000..137dbde
Binary files /dev/null and b/resources/robots/h1_2/meshes/link12_R.STL differ
diff --git a/resources/robots/h1_2/meshes/link13_L.STL b/resources/robots/h1_2/meshes/link13_L.STL
new file mode 100644
index 0000000..03a3425
Binary files /dev/null and b/resources/robots/h1_2/meshes/link13_L.STL differ
diff --git a/resources/robots/h1_2/meshes/link13_R.STL b/resources/robots/h1_2/meshes/link13_R.STL
new file mode 100644
index 0000000..a42829f
Binary files /dev/null and b/resources/robots/h1_2/meshes/link13_R.STL differ
diff --git a/resources/robots/h1_2/meshes/link14_L.STL b/resources/robots/h1_2/meshes/link14_L.STL
new file mode 100644
index 0000000..291a14f
Binary files /dev/null and b/resources/robots/h1_2/meshes/link14_L.STL differ
diff --git a/resources/robots/h1_2/meshes/link14_R.STL b/resources/robots/h1_2/meshes/link14_R.STL
new file mode 100644
index 0000000..13f98f8
Binary files /dev/null and b/resources/robots/h1_2/meshes/link14_R.STL differ
diff --git a/resources/robots/h1_2/meshes/link15_L.STL b/resources/robots/h1_2/meshes/link15_L.STL
new file mode 100644
index 0000000..f899431
Binary files /dev/null and b/resources/robots/h1_2/meshes/link15_L.STL differ
diff --git a/resources/robots/h1_2/meshes/link15_R.STL b/resources/robots/h1_2/meshes/link15_R.STL
new file mode 100644
index 0000000..cbfc711
Binary files /dev/null and b/resources/robots/h1_2/meshes/link15_R.STL differ
diff --git a/resources/robots/h1_2/meshes/link16_L.STL b/resources/robots/h1_2/meshes/link16_L.STL
new file mode 100644
index 0000000..24a5a83
Binary files /dev/null and b/resources/robots/h1_2/meshes/link16_L.STL differ
diff --git a/resources/robots/h1_2/meshes/link16_R.STL b/resources/robots/h1_2/meshes/link16_R.STL
new file mode 100644
index 0000000..0cf0f8d
Binary files /dev/null and b/resources/robots/h1_2/meshes/link16_R.STL differ
diff --git a/resources/robots/h1_2/meshes/link17_L.STL b/resources/robots/h1_2/meshes/link17_L.STL
new file mode 100644
index 0000000..8f0aca6
Binary files /dev/null and b/resources/robots/h1_2/meshes/link17_L.STL differ
diff --git a/resources/robots/h1_2/meshes/link17_R.STL b/resources/robots/h1_2/meshes/link17_R.STL
new file mode 100644
index 0000000..3f630a3
Binary files /dev/null and b/resources/robots/h1_2/meshes/link17_R.STL differ
diff --git a/resources/robots/h1_2/meshes/link18_L.STL b/resources/robots/h1_2/meshes/link18_L.STL
new file mode 100644
index 0000000..c498cd7
Binary files /dev/null and b/resources/robots/h1_2/meshes/link18_L.STL differ
diff --git a/resources/robots/h1_2/meshes/link18_R.STL b/resources/robots/h1_2/meshes/link18_R.STL
new file mode 100644
index 0000000..ebc2616
Binary files /dev/null and b/resources/robots/h1_2/meshes/link18_R.STL differ
diff --git a/resources/robots/h1_2/meshes/link19_L.STL b/resources/robots/h1_2/meshes/link19_L.STL
new file mode 100644
index 0000000..c3cabb9
Binary files /dev/null and b/resources/robots/h1_2/meshes/link19_L.STL differ
diff --git a/resources/robots/h1_2/meshes/link19_R.STL b/resources/robots/h1_2/meshes/link19_R.STL
new file mode 100644
index 0000000..5c3703e
Binary files /dev/null and b/resources/robots/h1_2/meshes/link19_R.STL differ
diff --git a/resources/robots/h1_2/meshes/link20_L.STL b/resources/robots/h1_2/meshes/link20_L.STL
new file mode 100644
index 0000000..23c4efa
Binary files /dev/null and b/resources/robots/h1_2/meshes/link20_L.STL differ
diff --git a/resources/robots/h1_2/meshes/link20_R.STL b/resources/robots/h1_2/meshes/link20_R.STL
new file mode 100644
index 0000000..5fed852
Binary files /dev/null and b/resources/robots/h1_2/meshes/link20_R.STL differ
diff --git a/resources/robots/h1_2/meshes/link21_L.STL b/resources/robots/h1_2/meshes/link21_L.STL
new file mode 100644
index 0000000..ccb63bf
Binary files /dev/null and b/resources/robots/h1_2/meshes/link21_L.STL differ
diff --git a/resources/robots/h1_2/meshes/link21_R.STL b/resources/robots/h1_2/meshes/link21_R.STL
new file mode 100644
index 0000000..d4710f1
Binary files /dev/null and b/resources/robots/h1_2/meshes/link21_R.STL differ
diff --git a/resources/robots/h1_2/meshes/link22_L.STL b/resources/robots/h1_2/meshes/link22_L.STL
new file mode 100644
index 0000000..6b9cdad
Binary files /dev/null and b/resources/robots/h1_2/meshes/link22_L.STL differ
diff --git a/resources/robots/h1_2/meshes/link22_R.STL b/resources/robots/h1_2/meshes/link22_R.STL
new file mode 100644
index 0000000..c2cf58b
Binary files /dev/null and b/resources/robots/h1_2/meshes/link22_R.STL differ
diff --git a/resources/robots/h1_2/meshes/logo_link.STL b/resources/robots/h1_2/meshes/logo_link.STL
new file mode 100644
index 0000000..c396d51
Binary files /dev/null and b/resources/robots/h1_2/meshes/logo_link.STL differ
diff --git a/resources/robots/h1_2/meshes/pelvis.STL b/resources/robots/h1_2/meshes/pelvis.STL
new file mode 100644
index 0000000..05e5512
Binary files /dev/null and b/resources/robots/h1_2/meshes/pelvis.STL differ
diff --git a/resources/robots/h1_2/meshes/right_ankle_A_link.STL b/resources/robots/h1_2/meshes/right_ankle_A_link.STL
new file mode 100644
index 0000000..2ed1eaf
Binary files /dev/null and b/resources/robots/h1_2/meshes/right_ankle_A_link.STL differ
diff --git a/resources/robots/h1_2/meshes/right_ankle_A_rod_link.STL b/resources/robots/h1_2/meshes/right_ankle_A_rod_link.STL
new file mode 100644
index 0000000..1fc9acc
Binary files /dev/null and b/resources/robots/h1_2/meshes/right_ankle_A_rod_link.STL differ
diff --git a/resources/robots/h1_2/meshes/right_ankle_B_link.STL b/resources/robots/h1_2/meshes/right_ankle_B_link.STL
new file mode 100644
index 0000000..6563110
Binary files /dev/null and b/resources/robots/h1_2/meshes/right_ankle_B_link.STL differ
diff --git a/resources/robots/h1_2/meshes/right_ankle_B_rod_link.STL b/resources/robots/h1_2/meshes/right_ankle_B_rod_link.STL
new file mode 100644
index 0000000..bdaa919
Binary files /dev/null and b/resources/robots/h1_2/meshes/right_ankle_B_rod_link.STL differ
diff --git a/resources/robots/h1_2/meshes/right_ankle_link.STL b/resources/robots/h1_2/meshes/right_ankle_link.STL
new file mode 100644
index 0000000..7045cea
Binary files /dev/null and b/resources/robots/h1_2/meshes/right_ankle_link.STL differ
diff --git a/resources/robots/h1_2/meshes/right_ankle_pitch_link.STL b/resources/robots/h1_2/meshes/right_ankle_pitch_link.STL
new file mode 100644
index 0000000..827fb2c
Binary files /dev/null and b/resources/robots/h1_2/meshes/right_ankle_pitch_link.STL differ
diff --git a/resources/robots/h1_2/meshes/right_ankle_roll_link.STL b/resources/robots/h1_2/meshes/right_ankle_roll_link.STL
new file mode 100644
index 0000000..5fe6027
Binary files /dev/null and b/resources/robots/h1_2/meshes/right_ankle_roll_link.STL differ
diff --git a/resources/robots/h1_2/meshes/right_elbow_pitch_link.STL b/resources/robots/h1_2/meshes/right_elbow_pitch_link.STL
new file mode 100644
index 0000000..70bf14d
Binary files /dev/null and b/resources/robots/h1_2/meshes/right_elbow_pitch_link.STL differ
diff --git a/resources/robots/h1_2/meshes/right_elbow_roll_link.STL b/resources/robots/h1_2/meshes/right_elbow_roll_link.STL
new file mode 100644
index 0000000..5f7f228
Binary files /dev/null and b/resources/robots/h1_2/meshes/right_elbow_roll_link.STL differ
diff --git a/resources/robots/h1_2/meshes/right_hand_link.STL b/resources/robots/h1_2/meshes/right_hand_link.STL
new file mode 100644
index 0000000..6ce2dcf
Binary files /dev/null and b/resources/robots/h1_2/meshes/right_hand_link.STL differ
diff --git a/resources/robots/h1_2/meshes/right_hip_pitch_link.STL b/resources/robots/h1_2/meshes/right_hip_pitch_link.STL
new file mode 100644
index 0000000..aa0b4c2
Binary files /dev/null and b/resources/robots/h1_2/meshes/right_hip_pitch_link.STL differ
diff --git a/resources/robots/h1_2/meshes/right_hip_roll_link.STL b/resources/robots/h1_2/meshes/right_hip_roll_link.STL
new file mode 100644
index 0000000..6032b95
Binary files /dev/null and b/resources/robots/h1_2/meshes/right_hip_roll_link.STL differ
diff --git a/resources/robots/h1_2/meshes/right_hip_yaw_link.STL b/resources/robots/h1_2/meshes/right_hip_yaw_link.STL
new file mode 100644
index 0000000..5039005
Binary files /dev/null and b/resources/robots/h1_2/meshes/right_hip_yaw_link.STL differ
diff --git a/resources/robots/h1_2/meshes/right_knee_link.STL b/resources/robots/h1_2/meshes/right_knee_link.STL
new file mode 100644
index 0000000..fe89e75
Binary files /dev/null and b/resources/robots/h1_2/meshes/right_knee_link.STL differ
diff --git a/resources/robots/h1_2/meshes/right_pitch_link.STL b/resources/robots/h1_2/meshes/right_pitch_link.STL
new file mode 100644
index 0000000..f5276e9
Binary files /dev/null and b/resources/robots/h1_2/meshes/right_pitch_link.STL differ
diff --git a/resources/robots/h1_2/meshes/right_shoulder_pitch_link.STL b/resources/robots/h1_2/meshes/right_shoulder_pitch_link.STL
new file mode 100644
index 0000000..87974a3
Binary files /dev/null and b/resources/robots/h1_2/meshes/right_shoulder_pitch_link.STL differ
diff --git a/resources/robots/h1_2/meshes/right_shoulder_roll_link.STL b/resources/robots/h1_2/meshes/right_shoulder_roll_link.STL
new file mode 100644
index 0000000..cf9bfb5
Binary files /dev/null and b/resources/robots/h1_2/meshes/right_shoulder_roll_link.STL differ
diff --git a/resources/robots/h1_2/meshes/right_shoulder_yaw_link.STL b/resources/robots/h1_2/meshes/right_shoulder_yaw_link.STL
new file mode 100644
index 0000000..701f379
Binary files /dev/null and b/resources/robots/h1_2/meshes/right_shoulder_yaw_link.STL differ
diff --git a/resources/robots/h1_2/meshes/right_wrist_pitch_link.STL b/resources/robots/h1_2/meshes/right_wrist_pitch_link.STL
new file mode 100644
index 0000000..f21d1f0
Binary files /dev/null and b/resources/robots/h1_2/meshes/right_wrist_pitch_link.STL differ
diff --git a/resources/robots/h1_2/meshes/torso_link.STL b/resources/robots/h1_2/meshes/torso_link.STL
new file mode 100644
index 0000000..37e1f60
Binary files /dev/null and b/resources/robots/h1_2/meshes/torso_link.STL differ
diff --git a/resources/robots/h1_2/meshes/wrist_yaw_link.STL b/resources/robots/h1_2/meshes/wrist_yaw_link.STL
new file mode 100644
index 0000000..90643cf
Binary files /dev/null and b/resources/robots/h1_2/meshes/wrist_yaw_link.STL differ
diff --git a/resources/robots/h1_2/scene.xml b/resources/robots/h1_2/scene.xml
new file mode 100644
index 0000000..fad346e
--- /dev/null
+++ b/resources/robots/h1_2/scene.xml
@@ -0,0 +1,22 @@
+<mujoco model="h1 scene">
+  <include file="h1_5.xml"/>
+
+  <statistic center="1.0 0.7 1.5" 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>
\ No newline at end of file