diff --git a/unitree_g1/README.md b/unitree_g1/README.md
index 265c902d..fe246081 100644
--- a/unitree_g1/README.md
+++ b/unitree_g1/README.md
@@ -3,26 +3,33 @@
 > [!IMPORTANT]
 > Requires MuJoCo 2.3.4 or later.
 
+## Changelog
+
+- 10/12/2024: Use updated models from Unitree's official [repo](https://github.com/unitreerobotics/unitree_ros/blob/master/robots/g1_description) (sha: c20ca8f1fe5e519474c6c8d10b1ce5c719dd7a65).
+  - Model without hands: [g1_29dof_rev_1_0](https://github.com/unitreerobotics/unitree_ros/blob/master/robots/g1_description/g1_29dof_rev_1_0.xml)
+  - Model with hands: [g1_29dof_with_hand_rev_1_0](https://github.com/unitreerobotics/unitree_ros/blob/master/robots/g1_description/g1_29dof_with_hand_rev_1_0.xml)
+- 20/05/2024: Initial release.
+
 ## Overview
 
 This package contains a simplified robot description (MJCF) of the [G1 Humanoid
 Robot](https://www.unitree.com/g1/) developed by [Unitree
 Robotics](https://www.unitree.com/). It is derived from the [publicly available
 MJCF
-description](https://github.com/unitreerobotics/unitree_ros/tree/master/robots/g1_description).
+description](https://github.com/unitreerobotics/unitree_ros/blob/master/robots/g1_description/g1_29dof_rev_1_0.xml). Specifically, this model has the fully actuated waist but not the full hands.
 
 <p float="left">
   <img src="g1.png" width="400">
+  <img src="g1_with_hands.png" width="400">
 </p>
 
 ## MJCF derivation steps
 
-1. Copied the MJCF description from [g1_description](https://github.com/unitreerobotics/unitree_ros/tree/master/robots/g1_description).
+1. Copied the MJCF description from [g1_description](https://github.com/unitreerobotics/unitree_ros/blob/master/robots/g1_description/g1_29dof_rev_1_0.xml).
 2. Manually edited the MJCF to extract common properties into the `<default>` section.
-3. Added sites for the IMU, head and feet.
-4. Add IMU sensor (gyro, accelero, framequat).
-5. Added stand keyframe.
-6. Added spotlight and tracking light.
+3. Added stand keyframe.
+4. Added joint position actuators (needs tuning).
+5. Applied similar edits to `g1_with_hands.xml`.
 
 ## License
 
diff --git a/unitree_g1/assets/head_link.STL b/unitree_g1/assets/head_link.STL
index 455274f4..2ee5fba1 100644
Binary files a/unitree_g1/assets/head_link.STL and b/unitree_g1/assets/head_link.STL differ
diff --git a/unitree_g1/assets/left_ankle_pitch_link.STL b/unitree_g1/assets/left_ankle_pitch_link.STL
index c291f607..69de8490 100644
Binary files a/unitree_g1/assets/left_ankle_pitch_link.STL and b/unitree_g1/assets/left_ankle_pitch_link.STL differ
diff --git a/unitree_g1/assets/left_ankle_roll_link.STL b/unitree_g1/assets/left_ankle_roll_link.STL
index f2b6ae7f..8864e9f9 100644
Binary files a/unitree_g1/assets/left_ankle_roll_link.STL and b/unitree_g1/assets/left_ankle_roll_link.STL differ
diff --git a/unitree_g1/assets/left_elbow_link.STL b/unitree_g1/assets/left_elbow_link.STL
new file mode 100644
index 00000000..1a96d99b
Binary files /dev/null and b/unitree_g1/assets/left_elbow_link.STL differ
diff --git a/unitree_g1/assets/left_elbow_pitch_link.STL b/unitree_g1/assets/left_elbow_pitch_link.STL
deleted file mode 100644
index 5ba2fdeb..00000000
Binary files a/unitree_g1/assets/left_elbow_pitch_link.STL and /dev/null differ
diff --git a/unitree_g1/assets/left_elbow_roll_link.STL b/unitree_g1/assets/left_elbow_roll_link.STL
deleted file mode 100644
index 9741ac03..00000000
Binary files a/unitree_g1/assets/left_elbow_roll_link.STL and /dev/null differ
diff --git a/unitree_g1/assets/left_five_link.STL b/unitree_g1/assets/left_five_link.STL
deleted file mode 100644
index cc35d512..00000000
Binary files a/unitree_g1/assets/left_five_link.STL and /dev/null differ
diff --git a/unitree_g1/assets/left_four_link.STL b/unitree_g1/assets/left_four_link.STL
deleted file mode 100644
index 6392be77..00000000
Binary files a/unitree_g1/assets/left_four_link.STL and /dev/null differ
diff --git a/unitree_g1/assets/left_hand_index_0_link.STL b/unitree_g1/assets/left_hand_index_0_link.STL
new file mode 100644
index 00000000..8069369a
Binary files /dev/null and b/unitree_g1/assets/left_hand_index_0_link.STL differ
diff --git a/unitree_g1/assets/left_hand_index_1_link.STL b/unitree_g1/assets/left_hand_index_1_link.STL
new file mode 100644
index 00000000..89d231d7
Binary files /dev/null and b/unitree_g1/assets/left_hand_index_1_link.STL differ
diff --git a/unitree_g1/assets/left_hand_middle_0_link.STL b/unitree_g1/assets/left_hand_middle_0_link.STL
new file mode 100644
index 00000000..8069369a
Binary files /dev/null and b/unitree_g1/assets/left_hand_middle_0_link.STL differ
diff --git a/unitree_g1/assets/left_hand_middle_1_link.STL b/unitree_g1/assets/left_hand_middle_1_link.STL
new file mode 100644
index 00000000..89d231d7
Binary files /dev/null and b/unitree_g1/assets/left_hand_middle_1_link.STL differ
diff --git a/unitree_g1/assets/left_hand_palm_link.STL b/unitree_g1/assets/left_hand_palm_link.STL
new file mode 100644
index 00000000..7d595ed8
Binary files /dev/null and b/unitree_g1/assets/left_hand_palm_link.STL differ
diff --git a/unitree_g1/assets/left_hand_thumb_0_link.STL b/unitree_g1/assets/left_hand_thumb_0_link.STL
new file mode 100644
index 00000000..3028bb4d
Binary files /dev/null and b/unitree_g1/assets/left_hand_thumb_0_link.STL differ
diff --git a/unitree_g1/assets/left_hand_thumb_1_link.STL b/unitree_g1/assets/left_hand_thumb_1_link.STL
new file mode 100644
index 00000000..d1c080c8
Binary files /dev/null and b/unitree_g1/assets/left_hand_thumb_1_link.STL differ
diff --git a/unitree_g1/assets/left_hand_thumb_2_link.STL b/unitree_g1/assets/left_hand_thumb_2_link.STL
new file mode 100644
index 00000000..8b32e966
Binary files /dev/null and b/unitree_g1/assets/left_hand_thumb_2_link.STL differ
diff --git a/unitree_g1/assets/left_hip_pitch_link.STL b/unitree_g1/assets/left_hip_pitch_link.STL
index 1e56afc8..5b751c76 100644
Binary files a/unitree_g1/assets/left_hip_pitch_link.STL and b/unitree_g1/assets/left_hip_pitch_link.STL differ
diff --git a/unitree_g1/assets/left_hip_roll_link.STL b/unitree_g1/assets/left_hip_roll_link.STL
index d1df2f43..778437ff 100644
Binary files a/unitree_g1/assets/left_hip_roll_link.STL and b/unitree_g1/assets/left_hip_roll_link.STL differ
diff --git a/unitree_g1/assets/left_hip_yaw_link.STL b/unitree_g1/assets/left_hip_yaw_link.STL
index deadd3c9..383093ab 100644
Binary files a/unitree_g1/assets/left_hip_yaw_link.STL and b/unitree_g1/assets/left_hip_yaw_link.STL differ
diff --git a/unitree_g1/assets/left_knee_link.STL b/unitree_g1/assets/left_knee_link.STL
index e0095a47..f2e98e54 100644
Binary files a/unitree_g1/assets/left_knee_link.STL and b/unitree_g1/assets/left_knee_link.STL differ
diff --git a/unitree_g1/assets/left_one_link.STL b/unitree_g1/assets/left_one_link.STL
deleted file mode 100644
index 6ee97bd7..00000000
Binary files a/unitree_g1/assets/left_one_link.STL and /dev/null differ
diff --git a/unitree_g1/assets/left_palm_link.STL b/unitree_g1/assets/left_palm_link.STL
deleted file mode 100644
index 524a0e21..00000000
Binary files a/unitree_g1/assets/left_palm_link.STL and /dev/null differ
diff --git a/unitree_g1/assets/left_rubber_hand.STL b/unitree_g1/assets/left_rubber_hand.STL
new file mode 100644
index 00000000..c44830f1
Binary files /dev/null and b/unitree_g1/assets/left_rubber_hand.STL differ
diff --git a/unitree_g1/assets/left_shoulder_pitch_link.STL b/unitree_g1/assets/left_shoulder_pitch_link.STL
index e54d6567..e698311f 100644
Binary files a/unitree_g1/assets/left_shoulder_pitch_link.STL and b/unitree_g1/assets/left_shoulder_pitch_link.STL differ
diff --git a/unitree_g1/assets/left_shoulder_roll_link.STL b/unitree_g1/assets/left_shoulder_roll_link.STL
index 75dc809b..80bca84a 100644
Binary files a/unitree_g1/assets/left_shoulder_roll_link.STL and b/unitree_g1/assets/left_shoulder_roll_link.STL differ
diff --git a/unitree_g1/assets/left_shoulder_yaw_link.STL b/unitree_g1/assets/left_shoulder_yaw_link.STL
index 4e5a7c98..281e6990 100644
Binary files a/unitree_g1/assets/left_shoulder_yaw_link.STL and b/unitree_g1/assets/left_shoulder_yaw_link.STL differ
diff --git a/unitree_g1/assets/left_six_link.STL b/unitree_g1/assets/left_six_link.STL
deleted file mode 100644
index 2739846f..00000000
Binary files a/unitree_g1/assets/left_six_link.STL and /dev/null differ
diff --git a/unitree_g1/assets/left_three_link.STL b/unitree_g1/assets/left_three_link.STL
deleted file mode 100644
index cc35d512..00000000
Binary files a/unitree_g1/assets/left_three_link.STL and /dev/null differ
diff --git a/unitree_g1/assets/left_two_link.STL b/unitree_g1/assets/left_two_link.STL
deleted file mode 100644
index b85b6457..00000000
Binary files a/unitree_g1/assets/left_two_link.STL and /dev/null differ
diff --git a/unitree_g1/assets/left_wrist_pitch_link.STL b/unitree_g1/assets/left_wrist_pitch_link.STL
new file mode 100644
index 00000000..82cc224a
Binary files /dev/null and b/unitree_g1/assets/left_wrist_pitch_link.STL differ
diff --git a/unitree_g1/assets/left_wrist_roll_link.STL b/unitree_g1/assets/left_wrist_roll_link.STL
new file mode 100644
index 00000000..f3c263a7
Binary files /dev/null and b/unitree_g1/assets/left_wrist_roll_link.STL differ
diff --git a/unitree_g1/assets/left_wrist_yaw_link.STL b/unitree_g1/assets/left_wrist_yaw_link.STL
new file mode 100644
index 00000000..31be4fd4
Binary files /dev/null and b/unitree_g1/assets/left_wrist_yaw_link.STL differ
diff --git a/unitree_g1/assets/left_zero_link.STL b/unitree_g1/assets/left_zero_link.STL
deleted file mode 100644
index f50147ac..00000000
Binary files a/unitree_g1/assets/left_zero_link.STL and /dev/null differ
diff --git a/unitree_g1/assets/logo_link.STL b/unitree_g1/assets/logo_link.STL
index 20f7bba0..e9792098 100644
Binary files a/unitree_g1/assets/logo_link.STL and b/unitree_g1/assets/logo_link.STL differ
diff --git a/unitree_g1/assets/pelvis.STL b/unitree_g1/assets/pelvis.STL
index 5877dee1..691a779b 100644
Binary files a/unitree_g1/assets/pelvis.STL and b/unitree_g1/assets/pelvis.STL differ
diff --git a/unitree_g1/assets/pelvis_contour_link.STL b/unitree_g1/assets/pelvis_contour_link.STL
index 72fa4ac8..42434339 100644
Binary files a/unitree_g1/assets/pelvis_contour_link.STL and b/unitree_g1/assets/pelvis_contour_link.STL differ
diff --git a/unitree_g1/assets/right_ankle_pitch_link.STL b/unitree_g1/assets/right_ankle_pitch_link.STL
index 36e3a64a..e77d8a2f 100644
Binary files a/unitree_g1/assets/right_ankle_pitch_link.STL and b/unitree_g1/assets/right_ankle_pitch_link.STL differ
diff --git a/unitree_g1/assets/right_ankle_roll_link.STL b/unitree_g1/assets/right_ankle_roll_link.STL
index d0a2657a..d4261dd7 100644
Binary files a/unitree_g1/assets/right_ankle_roll_link.STL and b/unitree_g1/assets/right_ankle_roll_link.STL differ
diff --git a/unitree_g1/assets/right_elbow_link.STL b/unitree_g1/assets/right_elbow_link.STL
new file mode 100644
index 00000000..f259e381
Binary files /dev/null and b/unitree_g1/assets/right_elbow_link.STL differ
diff --git a/unitree_g1/assets/right_elbow_pitch_link.STL b/unitree_g1/assets/right_elbow_pitch_link.STL
deleted file mode 100644
index a62a4220..00000000
Binary files a/unitree_g1/assets/right_elbow_pitch_link.STL and /dev/null differ
diff --git a/unitree_g1/assets/right_elbow_roll_link.STL b/unitree_g1/assets/right_elbow_roll_link.STL
deleted file mode 100644
index 6010e8b2..00000000
Binary files a/unitree_g1/assets/right_elbow_roll_link.STL and /dev/null differ
diff --git a/unitree_g1/assets/right_five_link.STL b/unitree_g1/assets/right_five_link.STL
deleted file mode 100644
index 37f9dc23..00000000
Binary files a/unitree_g1/assets/right_five_link.STL and /dev/null differ
diff --git a/unitree_g1/assets/right_four_link.STL b/unitree_g1/assets/right_four_link.STL
deleted file mode 100644
index be81a79c..00000000
Binary files a/unitree_g1/assets/right_four_link.STL and /dev/null differ
diff --git a/unitree_g1/assets/right_hand_index_0_link.STL b/unitree_g1/assets/right_hand_index_0_link.STL
new file mode 100644
index 00000000..f87ad321
Binary files /dev/null and b/unitree_g1/assets/right_hand_index_0_link.STL differ
diff --git a/unitree_g1/assets/right_hand_index_1_link.STL b/unitree_g1/assets/right_hand_index_1_link.STL
new file mode 100644
index 00000000..6dea51ad
Binary files /dev/null and b/unitree_g1/assets/right_hand_index_1_link.STL differ
diff --git a/unitree_g1/assets/right_hand_middle_0_link.STL b/unitree_g1/assets/right_hand_middle_0_link.STL
new file mode 100644
index 00000000..f87ad321
Binary files /dev/null and b/unitree_g1/assets/right_hand_middle_0_link.STL differ
diff --git a/unitree_g1/assets/right_hand_middle_1_link.STL b/unitree_g1/assets/right_hand_middle_1_link.STL
new file mode 100644
index 00000000..6dea51ad
Binary files /dev/null and b/unitree_g1/assets/right_hand_middle_1_link.STL differ
diff --git a/unitree_g1/assets/right_hand_palm_link.STL b/unitree_g1/assets/right_hand_palm_link.STL
new file mode 100644
index 00000000..5ae00a78
Binary files /dev/null and b/unitree_g1/assets/right_hand_palm_link.STL differ
diff --git a/unitree_g1/assets/right_hand_thumb_0_link.STL b/unitree_g1/assets/right_hand_thumb_0_link.STL
new file mode 100644
index 00000000..1cae7f18
Binary files /dev/null and b/unitree_g1/assets/right_hand_thumb_0_link.STL differ
diff --git a/unitree_g1/assets/right_hand_thumb_1_link.STL b/unitree_g1/assets/right_hand_thumb_1_link.STL
new file mode 100644
index 00000000..c141fbf5
Binary files /dev/null and b/unitree_g1/assets/right_hand_thumb_1_link.STL differ
diff --git a/unitree_g1/assets/right_hand_thumb_2_link.STL b/unitree_g1/assets/right_hand_thumb_2_link.STL
new file mode 100644
index 00000000..e942923c
Binary files /dev/null and b/unitree_g1/assets/right_hand_thumb_2_link.STL differ
diff --git a/unitree_g1/assets/right_hip_pitch_link.STL b/unitree_g1/assets/right_hip_pitch_link.STL
index 66b47ea2..998a0a0f 100644
Binary files a/unitree_g1/assets/right_hip_pitch_link.STL and b/unitree_g1/assets/right_hip_pitch_link.STL differ
diff --git a/unitree_g1/assets/right_hip_roll_link.STL b/unitree_g1/assets/right_hip_roll_link.STL
index 021dd7fe..47b2eebd 100644
Binary files a/unitree_g1/assets/right_hip_roll_link.STL and b/unitree_g1/assets/right_hip_roll_link.STL differ
diff --git a/unitree_g1/assets/right_hip_yaw_link.STL b/unitree_g1/assets/right_hip_yaw_link.STL
index 330e55a3..37185642 100644
Binary files a/unitree_g1/assets/right_hip_yaw_link.STL and b/unitree_g1/assets/right_hip_yaw_link.STL differ
diff --git a/unitree_g1/assets/right_knee_link.STL b/unitree_g1/assets/right_knee_link.STL
index 7be6860a..76d21a3d 100644
Binary files a/unitree_g1/assets/right_knee_link.STL and b/unitree_g1/assets/right_knee_link.STL differ
diff --git a/unitree_g1/assets/right_one_link.STL b/unitree_g1/assets/right_one_link.STL
deleted file mode 100644
index 93e103a3..00000000
Binary files a/unitree_g1/assets/right_one_link.STL and /dev/null differ
diff --git a/unitree_g1/assets/right_palm_link.STL b/unitree_g1/assets/right_palm_link.STL
deleted file mode 100644
index 7cd39cda..00000000
Binary files a/unitree_g1/assets/right_palm_link.STL and /dev/null differ
diff --git a/unitree_g1/assets/right_rubber_hand.STL b/unitree_g1/assets/right_rubber_hand.STL
new file mode 100644
index 00000000..0aacffbb
Binary files /dev/null and b/unitree_g1/assets/right_rubber_hand.STL differ
diff --git a/unitree_g1/assets/right_shoulder_pitch_link.STL b/unitree_g1/assets/right_shoulder_pitch_link.STL
index 7e1d01da..3f5b4ed4 100644
Binary files a/unitree_g1/assets/right_shoulder_pitch_link.STL and b/unitree_g1/assets/right_shoulder_pitch_link.STL differ
diff --git a/unitree_g1/assets/right_shoulder_roll_link.STL b/unitree_g1/assets/right_shoulder_roll_link.STL
index f653871e..179d6175 100644
Binary files a/unitree_g1/assets/right_shoulder_roll_link.STL and b/unitree_g1/assets/right_shoulder_roll_link.STL differ
diff --git a/unitree_g1/assets/right_shoulder_yaw_link.STL b/unitree_g1/assets/right_shoulder_yaw_link.STL
index 01aa95ab..2ba6076a 100644
Binary files a/unitree_g1/assets/right_shoulder_yaw_link.STL and b/unitree_g1/assets/right_shoulder_yaw_link.STL differ
diff --git a/unitree_g1/assets/right_six_link.STL b/unitree_g1/assets/right_six_link.STL
deleted file mode 100644
index be81a79c..00000000
Binary files a/unitree_g1/assets/right_six_link.STL and /dev/null differ
diff --git a/unitree_g1/assets/right_three_link.STL b/unitree_g1/assets/right_three_link.STL
deleted file mode 100644
index 37f9dc23..00000000
Binary files a/unitree_g1/assets/right_three_link.STL and /dev/null differ
diff --git a/unitree_g1/assets/right_two_link.STL b/unitree_g1/assets/right_two_link.STL
deleted file mode 100644
index 1af670c0..00000000
Binary files a/unitree_g1/assets/right_two_link.STL and /dev/null differ
diff --git a/unitree_g1/assets/right_wrist_pitch_link.STL b/unitree_g1/assets/right_wrist_pitch_link.STL
new file mode 100644
index 00000000..da194543
Binary files /dev/null and b/unitree_g1/assets/right_wrist_pitch_link.STL differ
diff --git a/unitree_g1/assets/right_wrist_roll_link.STL b/unitree_g1/assets/right_wrist_roll_link.STL
new file mode 100644
index 00000000..26868d22
Binary files /dev/null and b/unitree_g1/assets/right_wrist_roll_link.STL differ
diff --git a/unitree_g1/assets/right_wrist_yaw_link.STL b/unitree_g1/assets/right_wrist_yaw_link.STL
new file mode 100644
index 00000000..d7889028
Binary files /dev/null and b/unitree_g1/assets/right_wrist_yaw_link.STL differ
diff --git a/unitree_g1/assets/right_zero_link.STL b/unitree_g1/assets/right_zero_link.STL
deleted file mode 100644
index 4fd999bd..00000000
Binary files a/unitree_g1/assets/right_zero_link.STL and /dev/null differ
diff --git a/unitree_g1/assets/torso_link.STL b/unitree_g1/assets/torso_link.STL
deleted file mode 100644
index b6a6d12b..00000000
Binary files a/unitree_g1/assets/torso_link.STL and /dev/null differ
diff --git a/unitree_g1/assets/torso_link_rev_1_0.STL b/unitree_g1/assets/torso_link_rev_1_0.STL
new file mode 100644
index 00000000..8a759a70
Binary files /dev/null and b/unitree_g1/assets/torso_link_rev_1_0.STL differ
diff --git a/unitree_g1/assets/waist_roll_link_rev_1_0.STL b/unitree_g1/assets/waist_roll_link_rev_1_0.STL
new file mode 100644
index 00000000..a64f330c
Binary files /dev/null and b/unitree_g1/assets/waist_roll_link_rev_1_0.STL differ
diff --git a/unitree_g1/assets/waist_yaw_link_rev_1_0.STL b/unitree_g1/assets/waist_yaw_link_rev_1_0.STL
new file mode 100644
index 00000000..0fabb63d
Binary files /dev/null and b/unitree_g1/assets/waist_yaw_link_rev_1_0.STL differ
diff --git a/unitree_g1/g1.png b/unitree_g1/g1.png
index 9d56a7a7..8c4dde8b 100644
Binary files a/unitree_g1/g1.png and b/unitree_g1/g1.png differ
diff --git a/unitree_g1/g1.xml b/unitree_g1/g1.xml
index 77537bd3..80fa3121 100644
--- a/unitree_g1/g1.xml
+++ b/unitree_g1/g1.xml
@@ -1,35 +1,28 @@
-<mujoco model="g1">
-  <compiler angle="radian" meshdir="assets" autolimits="true"/>
+<mujoco model="g1_29dof_rev_1_0">
+  <compiler angle="radian" meshdir="assets"/>
 
-  <visual>
-    <global ellipsoidinertia="true"/>
-  </visual>
+  <option integrator="implicitfast"/>
 
   <default>
     <default class="g1">
       <site rgba="1 0 0 1" size="0.01" group="5"/>
-      <joint damping="0.5" armature="0.01" frictionloss="0.1"/>
-      <default class="arm_joint">
-        <motor ctrlrange="-20 20"/>
-      </default>
-      <default class="hand_joint">
-        <motor ctrlrange="-0.7 0.7"/>
-      </default>
+      <joint armature="0.01" frictionloss="0.3"/>
+      <position kp="500" dampratio="1" inheritrange="1"/>
       <default class="visual">
-        <geom group="2" type="mesh" contype="0" conaffinity="0" density="0" material="metallic"/>
+        <geom group="2" type="mesh" contype="0" conaffinity="0" density="0" material="metal"/>
       </default>
       <default class="collision">
         <geom group="3" type="mesh"/>
         <default class="foot">
-          <geom type="sphere" size="0.001"/>
+          <geom type="sphere" size="0.005" priority="1" friction="0.6" condim="3"/>
         </default>
       </default>
     </default>
   </default>
 
   <asset>
-    <material name="metallic" rgba="0.7 0.7 0.7 1"/>
     <material name="black" rgba="0.2 0.2 0.2 1"/>
+    <material name="metal" rgba="0.7 0.7 0.7 1"/>
 
     <mesh file="pelvis.STL"/>
     <mesh file="pelvis_contour_link.STL"/>
@@ -45,318 +38,258 @@
     <mesh file="right_knee_link.STL"/>
     <mesh file="right_ankle_pitch_link.STL"/>
     <mesh file="right_ankle_roll_link.STL"/>
-    <mesh file="torso_link.STL"/>
+    <mesh name="waist_yaw_link" file="waist_yaw_link_rev_1_0.STL"/>
+    <mesh name="waist_roll_link" file="waist_roll_link_rev_1_0.STL"/>
+    <mesh name="torso_link" file="torso_link_rev_1_0.STL"/>
+    <mesh file="logo_link.STL"/>
     <mesh file="head_link.STL"/>
     <mesh file="left_shoulder_pitch_link.STL"/>
     <mesh file="left_shoulder_roll_link.STL"/>
     <mesh file="left_shoulder_yaw_link.STL"/>
-    <mesh file="left_elbow_pitch_link.STL"/>
-    <mesh file="left_elbow_roll_link.STL"/>
+    <mesh file="left_elbow_link.STL"/>
+    <mesh file="left_wrist_roll_link.STL"/>
+    <mesh file="left_wrist_pitch_link.STL"/>
+    <mesh file="left_wrist_yaw_link.STL"/>
+    <mesh file="left_rubber_hand.STL"/>
     <mesh file="right_shoulder_pitch_link.STL"/>
     <mesh file="right_shoulder_roll_link.STL"/>
     <mesh file="right_shoulder_yaw_link.STL"/>
-    <mesh file="right_elbow_pitch_link.STL"/>
-    <mesh file="right_elbow_roll_link.STL"/>
-    <mesh file="logo_link.STL"/>
-    <mesh file="left_palm_link.STL"/>
-    <mesh file="left_zero_link.STL"/>
-    <mesh file="left_one_link.STL"/>
-    <mesh file="left_two_link.STL"/>
-    <mesh file="left_three_link.STL"/>
-    <mesh file="left_four_link.STL"/>
-    <mesh file="left_five_link.STL"/>
-    <mesh file="left_six_link.STL"/>
-    <mesh file="right_palm_link.STL"/>
-    <mesh file="right_zero_link.STL"/>
-    <mesh file="right_one_link.STL"/>
-    <mesh file="right_two_link.STL"/>
-    <mesh file="right_three_link.STL"/>
-    <mesh file="right_four_link.STL"/>
-    <mesh file="right_five_link.STL"/>
-    <mesh file="right_six_link.STL"/>
+    <mesh file="right_elbow_link.STL"/>
+    <mesh file="right_wrist_roll_link.STL"/>
+    <mesh file="right_wrist_pitch_link.STL"/>
+    <mesh file="right_wrist_yaw_link.STL"/>
+    <mesh file="right_rubber_hand.STL"/>
   </asset>
 
   <worldbody>
-    <light name="spotlight" mode="targetbodycom" target="pelvis" pos="0 -6 4" cutoff="30"/>
-    <body name="pelvis" pos="0 0 0.755" childclass="g1">
-      <inertial pos="0 0 -0.07605" quat="1 0 -0.000405289 0" mass="2.86" diaginertia="0.0079143 0.0069837 0.0059404"/>
+    <light pos="1 0 3.5" dir="0 0 -1" directional="true"/>
+    <body name="pelvis" pos="0 0 0.793" childclass="g1">
+      <inertial pos="0 0 -0.07605" quat="1 0 -0.000399148 0" mass="3.813" diaginertia="0.010549 0.0093089 0.0079184"/>
       <freejoint name="floating_base_joint"/>
-      <light name="tracking" mode="trackcom" pos="0 0 2"/>
-      <geom type="mesh" class="visual" mesh="pelvis"/>
-      <geom type="mesh" mesh="pelvis" class="collision"/>
-      <geom type="mesh" class="visual" material="black" mesh="pelvis_contour_link"/>
-      <body name="left_hip_pitch_link" pos="0 0.06445 -0.1027" quat="0.984807 0 -0.17365 0">
-        <inertial pos="0.001962 0.049392 -0.000941" quat="0.420735 0.907025 -0.0155675 -0.00670646" mass="1.299"
-          diaginertia="0.00138785 0.000955478 0.00086947"/>
-        <joint name="left_hip_pitch_joint" axis="0 1 0" range="-2.35 3.05"/>
-        <geom type="mesh" class="visual" material="black" mesh="left_hip_pitch_link"/>
-        <geom type="mesh" mesh="left_hip_pitch_link" class="collision"/>
-        <body name="left_hip_roll_link" pos="0 0.0523 0">
-          <inertial pos="0.024757 -0.001036 -0.086323" quat="0.977498 -0.00692636 0.210181 0.0165269" mass="1.446"
-            diaginertia="0.00244106 0.00230425 0.00142899"/>
-          <joint name="left_hip_roll_joint" axis="1 0 0" range="-0.26 2.53"/>
-          <geom type="mesh" class="visual" mesh="left_hip_roll_link"/>
-          <geom type="mesh" mesh="left_hip_roll_link" class="collision"/>
-          <body name="left_hip_yaw_link" pos="0.01966 -0.0012139 -0.1241">
-            <inertial pos="-0.053554 -0.011477 -0.14067" quat="0.645099 0.15885 0.202109 0.71956" mass="2.052"
-              diaginertia="0.0114475 0.0107868 0.00214501"/>
-            <joint name="left_hip_yaw_joint" axis="0 0 1" range="-2.75 2.75"/>
-            <geom type="mesh" class="visual" mesh="left_hip_yaw_link"/>
-            <geom type="mesh" mesh="left_hip_yaw_link" class="collision"/>
-            <body name="left_knee_link" pos="-0.078292 -0.0017335 -0.177225" quat="0.967714 0 0.252052 0">
-              <inertial pos="0.005505 0.006534 -0.116629" quat="0.799234 -0.0128894 0.0354278 0.599836" mass="2.252"
-                diaginertia="0.0127418 0.0124382 0.00192524"/>
-              <joint name="left_knee_joint" axis="0 1 0" range="-0.33489 2.5449"/>
-              <geom type="mesh" class="visual" mesh="left_knee_link"/>
-              <geom size="0.015 0.075" pos="0.007 0.005 -0.15" type="cylinder" class="collision"/>
-              <body name="left_ankle_pitch_link" pos="0 0.0040687 -0.30007" quat="0.99678 0 -0.0801788 0">
+      <geom class="visual" material="black" mesh="pelvis"/>
+      <geom class="visual" mesh="pelvis_contour_link"/>
+      <geom class="collision" mesh="pelvis_contour_link"/>
+      <site name="imu_in_pelvis" pos="0.04525 0 -0.08339" size="0.01"/>
+      <body name="left_hip_pitch_link" pos="0 0.064452 -0.1027">
+        <inertial pos="0.002741 0.047791 -0.02606" quat="0.954862 0.293964 0.0302556 0.030122" mass="1.35"
+          diaginertia="0.00181517 0.00153422 0.00116212"/>
+        <joint name="left_hip_pitch_joint" axis="0 1 0" range="-2.5307 2.8798" actuatorfrcrange="-88 88"/>
+        <geom class="visual" material="black" mesh="left_hip_pitch_link"/>
+        <geom class="collision" material="black" mesh="left_hip_pitch_link"/>
+        <body name="left_hip_roll_link" pos="0 0.052 -0.030465" quat="0.996179 0 -0.0873386 0">
+          <inertial pos="0.029812 -0.001045 -0.087934" quat="0.977808 -1.97119e-05 0.205576 -0.0403793" mass="1.52"
+            diaginertia="0.00254986 0.00241169 0.00148755"/>
+          <joint name="left_hip_roll_joint" axis="1 0 0" range="-0.5236 2.9671" actuatorfrcrange="-139 139"/>
+          <geom class="visual" mesh="left_hip_roll_link"/>
+          <geom class="collision" mesh="left_hip_roll_link"/>
+          <body name="left_hip_yaw_link" pos="0.025001 0 -0.12412">
+            <inertial pos="-0.057709 -0.010981 -0.15078" quat="0.600598 0.15832 0.223482 0.751181" mass="1.702"
+              diaginertia="0.00776166 0.00717575 0.00160139"/>
+            <joint name="left_hip_yaw_joint" axis="0 0 1" range="-2.7576 2.7576" actuatorfrcrange="-88 88"/>
+            <geom class="visual" mesh="left_hip_yaw_link"/>
+            <geom class="collision" mesh="left_hip_yaw_link"/>
+            <body name="left_knee_link" pos="-0.078273 0.0021489 -0.17734" quat="0.996179 0 0.0873386 0">
+              <inertial pos="0.005457 0.003964 -0.12074" quat="0.923418 -0.0327699 0.0158246 0.382067" mass="1.932"
+                diaginertia="0.0113804 0.0112778 0.00146458"/>
+              <joint name="left_knee_joint" axis="0 1 0" range="-0.087267 2.8798" actuatorfrcrange="-139 139"/>
+              <geom class="visual" mesh="left_knee_link"/>
+              <geom class="collision" mesh="left_knee_link"/>
+              <body name="left_ankle_pitch_link" pos="0 -9.4445e-05 -0.30001">
                 <inertial pos="-0.007269 0 0.011137" quat="0.603053 0.369225 0.369225 0.603053" mass="0.074"
                   diaginertia="1.89e-05 1.40805e-05 6.9195e-06"/>
-                <joint name="left_ankle_pitch_joint" axis="0 1 0" range="-0.68 0.73"/>
-                <geom type="mesh" class="visual" mesh="left_ankle_pitch_link"/>
-                <geom type="mesh" mesh="left_ankle_pitch_link" class="collision"/>
+                <joint name="left_ankle_pitch_joint" axis="0 1 0" range="-0.87267 0.5236" actuatorfrcrange="-50 50"/>
+                <geom class="visual" mesh="left_ankle_pitch_link"/>
+                <geom class="collision" mesh="left_ankle_pitch_link"/>
                 <body name="left_ankle_roll_link" pos="0 0 -0.017558">
-                  <site name="left_foot" pos="0 0 0"/>
-                  <inertial pos="0.024762 2e-05 -0.012526" quat="0.000771333 0.734476 0.000921291 0.678634" mass="0.391"
-                    diaginertia="0.00110394 0.0010657 0.000149255"/>
-                  <joint name="left_ankle_roll_joint" axis="1 0 0" range="-0.2618 0.2618"/>
-                  <geom type="mesh" class="visual" material="black" mesh="left_ankle_roll_link"/>
-                  <geom pos="-0.06 0.02 -0.03" class="foot"/>
-                  <geom pos="-0.06 -0.02 -0.03" class="foot"/>
-                  <geom pos="0.13 0.02 -0.03" class="foot"/>
-                  <geom pos="0.13 -0.02 -0.03" class="foot"/>
+                  <inertial pos="0.026505 0 -0.016425" quat="-0.000481092 0.728482 -0.000618967 0.685065" mass="0.608"
+                    diaginertia="0.00167218 0.0016161 0.000217621"/>
+                  <joint name="left_ankle_roll_joint" axis="1 0 0" range="-0.2618 0.2618" actuatorfrcrange="-50 50"/>
+                  <geom class="visual" material="black" mesh="left_ankle_roll_link"/>
+                  <geom class="foot" pos="-0.05 0.025 -0.03"/>
+                  <geom class="foot" pos="-0.05 -0.025 -0.03"/>
+                  <geom class="foot" pos="0.12 0.03 -0.03"/>
+                  <geom class="foot" pos="0.12 -0.03 -0.03"/>
+                  <site name="left_foot"/>
                 </body>
               </body>
             </body>
           </body>
         </body>
       </body>
-      <body name="right_hip_pitch_link" pos="0 -0.06445 -0.1027" quat="0.984807 0 -0.17365 0">
-        <inertial pos="0.001962 -0.049392 -0.000941" quat="0.907025 0.420735 0.00670646 0.0155675" mass="1.299"
-          diaginertia="0.00138785 0.000955478 0.00086947"/>
-        <joint name="right_hip_pitch_joint" axis="0 1 0" range="-2.35 3.05"/>
-        <geom type="mesh" class="visual" material="black" mesh="right_hip_pitch_link"/>
-        <geom type="mesh" mesh="right_hip_pitch_link" class="collision"/>
-        <body name="right_hip_roll_link" pos="0 -0.0523 0">
-          <inertial pos="0.024757 0.001036 -0.086323" quat="0.977498 0.00692636 0.210181 -0.0165269" mass="1.446"
-            diaginertia="0.00244106 0.00230425 0.00142899"/>
-          <joint name="right_hip_roll_joint" axis="1 0 0" range="-2.53 0.26"/>
-          <geom type="mesh" class="visual" mesh="right_hip_roll_link"/>
-          <geom type="mesh" mesh="right_hip_roll_link" class="collision"/>
-          <body name="right_hip_yaw_link" pos="0.01966 0.0012139 -0.1241">
-            <inertial pos="-0.053554 0.011477 -0.14067" quat="0.71956 0.202109 0.15885 0.645099" mass="2.052"
-              diaginertia="0.0114475 0.0107868 0.00214501"/>
-            <joint name="right_hip_yaw_joint" axis="0 0 1" range="-2.75 2.75"/>
-            <geom type="mesh" class="visual" mesh="right_hip_yaw_link"/>
-            <geom type="mesh" mesh="right_hip_yaw_link" class="collision"/>
-            <body name="right_knee_link" pos="-0.078292 0.0017335 -0.177225" quat="0.967714 0 0.252052 0">
-              <inertial pos="0.005505 -0.006534 -0.116629" quat="0.599836 0.0354278 -0.0128894 0.799234" mass="2.252"
-                diaginertia="0.0127418 0.0124382 0.00192524"/>
-              <joint name="right_knee_joint" axis="0 1 0" range="-0.33489 2.5449"/>
-              <geom type="mesh" class="visual" mesh="right_knee_link"/>
-              <geom size="0.015 0.075" pos="0.007 -0.005 -0.15" type="cylinder" class="collision"/>
-              <body name="right_ankle_pitch_link" pos="0 -0.0040687 -0.30007" quat="0.99678 0 -0.0801788 0">
+      <body name="right_hip_pitch_link" pos="0 -0.064452 -0.1027">
+        <inertial pos="0.002741 -0.047791 -0.02606" quat="0.954862 -0.293964 0.0302556 -0.030122" mass="1.35"
+          diaginertia="0.00181517 0.00153422 0.00116212"/>
+        <joint name="right_hip_pitch_joint" axis="0 1 0" range="-2.5307 2.8798" actuatorfrcrange="-88 88"/>
+        <geom class="visual" material="black" mesh="right_hip_pitch_link"/>
+        <geom class="collision" material="black" mesh="right_hip_pitch_link"/>
+        <body name="right_hip_roll_link" pos="0 -0.052 -0.030465" quat="0.996179 0 -0.0873386 0">
+          <inertial pos="0.029812 0.001045 -0.087934" quat="0.977808 1.97119e-05 0.205576 0.0403793" mass="1.52"
+            diaginertia="0.00254986 0.00241169 0.00148755"/>
+          <joint name="right_hip_roll_joint" axis="1 0 0" range="-2.9671 0.5236" actuatorfrcrange="-139 139"/>
+          <geom class="visual" mesh="right_hip_roll_link"/>
+          <geom class="collision" mesh="right_hip_roll_link"/>
+          <body name="right_hip_yaw_link" pos="0.025001 0 -0.12412">
+            <inertial pos="-0.057709 0.010981 -0.15078" quat="0.751181 0.223482 0.15832 0.600598" mass="1.702"
+              diaginertia="0.00776166 0.00717575 0.00160139"/>
+            <joint name="right_hip_yaw_joint" axis="0 0 1" range="-2.7576 2.7576" actuatorfrcrange="-88 88"/>
+            <geom class="visual" mesh="right_hip_yaw_link"/>
+            <geom class="collision" mesh="right_hip_yaw_link"/>
+            <body name="right_knee_link" pos="-0.078273 -0.0021489 -0.17734" quat="0.996179 0 0.0873386 0">
+              <inertial pos="0.005457 -0.003964 -0.12074" quat="0.923439 0.0345276 0.0116333 -0.382012" mass="1.932"
+                diaginertia="0.011374 0.0112843 0.00146452"/>
+              <joint name="right_knee_joint" axis="0 1 0" range="-0.087267 2.8798" actuatorfrcrange="-139 139"/>
+              <geom class="visual" mesh="right_knee_link"/>
+              <geom class="collision" mesh="right_knee_link"/>
+              <body name="right_ankle_pitch_link" pos="0 9.4445e-05 -0.30001">
                 <inertial pos="-0.007269 0 0.011137" quat="0.603053 0.369225 0.369225 0.603053" mass="0.074"
                   diaginertia="1.89e-05 1.40805e-05 6.9195e-06"/>
-                <joint name="right_ankle_pitch_joint" axis="0 1 0" range="-0.68 0.73"/>
-                <geom type="mesh" class="visual" mesh="right_ankle_pitch_link"/>
-                <geom type="mesh" mesh="right_ankle_pitch_link" class="collision"/>
+                <joint name="right_ankle_pitch_joint" axis="0 1 0" range="-0.87267 0.5236" actuatorfrcrange="-50 50"/>
+                <geom class="visual" mesh="right_ankle_pitch_link"/>
+                <geom class="collision" mesh="right_ankle_pitch_link"/>
                 <body name="right_ankle_roll_link" pos="0 0 -0.017558">
-                  <site name="right_foot" pos="0 0 0"/>
-                  <inertial pos="0.024762 -2e-05 -0.012526" quat="-0.000771333 0.734476 -0.000921291 0.678634"
-                    mass="0.391" diaginertia="0.00110394 0.0010657 0.000149255"/>
-                  <joint name="right_ankle_roll_joint" axis="1 0 0" range="-0.2618 0.2618"/>
-                  <geom type="mesh" class="visual" material="black" mesh="right_ankle_roll_link"/>
-                  <geom pos="-0.06 0.02 -0.03" class="foot"/>
-                  <geom pos="-0.06 -0.02 -0.03" class="foot"/>
-                  <geom pos="0.13 0.02 -0.03" class="foot"/>
-                  <geom pos="0.13 -0.02 -0.03" class="foot"/>
+                  <inertial pos="0.026505 0 -0.016425" quat="0.000481092 0.728482 0.000618967 0.685065" mass="0.608"
+                    diaginertia="0.00167218 0.0016161 0.000217621"/>
+                  <joint name="right_ankle_roll_joint" axis="1 0 0" range="-0.2618 0.2618" actuatorfrcrange="-50 50"/>
+                  <geom class="visual" material="black" mesh="right_ankle_roll_link"/>
+                  <geom class="foot" pos="-0.05 0.025 -0.03"/>
+                  <geom class="foot" pos="-0.05 -0.025 -0.03"/>
+                  <geom class="foot" pos="0.12 0.03 -0.03"/>
+                  <geom class="foot" pos="0.12 -0.03 -0.03"/>
+                  <site name="right_foot"/>
                 </body>
               </body>
             </body>
           </body>
         </body>
       </body>
-      <body name="torso_link">
-        <inertial pos="0.00187791 0.00229457 0.208747" quat="0.999957 0.00624375 -0.00636707 -0.0026338" mass="7.52036"
-          diaginertia="0.12848 0.111753 0.0350394"/>
-        <site name="imu" pos="-0.04233868314 0.00166 0.152067"/>
-        <site name="head" pos="0 0 0.5"/>
-        <joint name="torso_joint" axis="0 0 1" range="-2.618 2.618"/>
-        <geom type="mesh" class="visual" mesh="torso_link"/>
-        <geom type="mesh" mesh="torso_link" class="collision"/>
-        <geom type="mesh" class="visual" material="black" mesh="head_link"/>
-        <geom type="mesh" mesh="head_link" class="collision"/>
-        <geom type="mesh" class="visual" material="black" mesh="logo_link"/>
-        <geom type="mesh" mesh="logo_link" class="collision"/>
-        <body name="left_shoulder_pitch_link" pos="-0.0025 0.10396 0.25928" quat="0.990268 0.139172 0 0">
-          <inertial pos="-0.001431 0.048811 0.001304" quat="0.786417 0.588396 -0.180543 0.0523639" mass="0.713"
-            diaginertia="0.000466421 0.000440181 0.000410999"/>
-          <joint name="left_shoulder_pitch_joint" axis="0 1 0" range="-2.9671 2.7925"/>
-          <geom type="mesh" class="visual" mesh="left_shoulder_pitch_link"/>
-          <geom type="mesh" mesh="left_shoulder_pitch_link" class="collision"/>
-          <body name="left_shoulder_roll_link" pos="0 0.052 0" quat="0.990268 -0.139172 0 0">
-            <inertial pos="-0.003415 0.006955 -0.064598" quat="0.70683 0.0105364 0.00575207 0.707282" mass="0.642"
-              diaginertia="0.000683514 0.000616029 0.000372857"/>
-            <joint name="left_shoulder_roll_joint" axis="1 0 0" range="-1.5882 2.2515"/>
-            <geom type="mesh" class="visual" mesh="left_shoulder_roll_link"/>
-            <geom size="0.03 0.015" pos="-0.004 0.006 -0.053" type="cylinder" class="collision"/>
-            <body name="left_shoulder_yaw_link" pos="-0.00354 0.0062424 -0.1032">
-              <inertial pos="0.000375 -0.00444 -0.072374" quat="0.903834 -0.0374183 0.00985482 0.42613" mass="0.713"
-                diaginertia="0.000977874 0.000964661 0.000379065"/>
-              <joint name="left_shoulder_yaw_joint" axis="0 0 1" range="-2.618 2.618"/>
-              <geom type="mesh" class="visual" mesh="left_shoulder_yaw_link"/>
-              <geom type="mesh" mesh="left_shoulder_yaw_link" class="collision"/>
-              <body name="left_elbow_pitch_link" pos="0 0.00189 -0.0855">
-                <inertial pos="0.064497 0.002873 0" quat="0.582347 0.582495 0.400893 0.401069" mass="0.601"
-                  diaginertia="0.00049549 0.0004712 0.00025371"/>
-                <joint name="left_elbow_pitch_joint" axis="0 1 0" range="-0.2268 3.4208"/>
-                <geom type="mesh" class="visual" mesh="left_elbow_pitch_link"/>
-                <geom type="mesh" mesh="left_elbow_pitch_link" class="collision"/>
-                <body name="left_elbow_roll_link" pos="0.1 0 0">
-                  <inertial pos="0.133814 0.00147121 0.000265832" quat="0.496781 0.497877 0.498782 0.506502"
-                    mass="0.50826" diaginertia="0.00239763 0.00226639 0.000285577"/>
-                  <joint name="left_elbow_roll_joint" axis="1 0 0" range="-2.0943 2.0943"/>
-                  <geom type="mesh" class="visual" mesh="left_elbow_roll_link"/>
-                  <geom type="mesh" mesh="left_elbow_roll_link" class="collision"/>
-                  <geom pos="0.12 0 0" type="mesh" class="visual" mesh="left_palm_link"/>
-                  <geom pos="0.12 0 0" type="mesh" mesh="left_palm_link" class="collision"/>
-                  <body name="left_zero_link" pos="0.17 0 0">
-                    <inertial pos="-0.0260466 -0.00877511 1.08605e-05" quat="0.59241 0.381376 -0.592953 0.389884"
-                      mass="0.0509893" diaginertia="8.68047e-06 7.23365e-06 4.58058e-06"/>
-                    <joint name="left_zero_joint" axis="0 1 0" range="-0.523598 0.523598"/>
-                    <geom type="mesh" class="visual" mesh="left_zero_link"/>
-                    <site name="left_palm" pos="0 0 0"/>
-                    <body name="left_one_link" pos="-0.026525 -0.0188 -5e-05">
-                      <inertial pos="-0.00181538 -0.0327057 -0.000185871" quat="0.701646 0.698199 0.0817825 -0.116289"
-                        mass="0.047762" diaginertia="8.03051e-06 7.8455e-06 3.65282e-06"/>
-                      <joint name="left_one_joint" axis="0 0 1" range="-1 1.2"/>
-                      <geom type="mesh" class="visual" mesh="left_one_link"/>
-                      <geom size="0.01 0.015 0.01" pos="-0.001 -0.032 0" type="box" class="collision"/>
-                      <body name="left_two_link" pos="0 -0.0431 0">
-                        <inertial pos="-0.00135257 -0.0237591 -0.000119669"
-                          quat="0.717995 0.695316 -0.0180122 -0.0263741" mass="0.0138584"
-                          diaginertia="2.55338e-06 2.04399e-06 9.3995e-07"/>
-                        <joint name="left_two_joint" axis="0 0 1" range="0 1.84"/>
-                        <geom type="mesh" class="visual" mesh="left_two_link"/>
-                        <geom type="mesh" mesh="left_two_link" class="collision"/>
+      <body name="waist_yaw_link">
+        <inertial pos="0.003494 0.000233 0.018034" quat="0.289697 0.591001 -0.337795 0.672821" mass="0.214"
+          diaginertia="0.000163531 0.000107714 0.000102205"/>
+        <joint name="waist_yaw_joint" axis="0 0 1" range="-2.618 2.618" actuatorfrcrange="-88 88"/>
+        <geom class="visual" mesh="waist_yaw_link"/>
+        <body name="waist_roll_link" pos="-0.0039635 0 0.044">
+          <inertial pos="0 2.3e-05 0" quat="0.5 0.5 -0.5 0.5" mass="0.086" diaginertia="8.245e-06 7.079e-06 6.339e-06"/>
+          <joint name="waist_roll_joint" axis="1 0 0" range="-0.52 0.52" actuatorfrcrange="-50 50"/>
+          <geom class="visual" mesh="waist_roll_link"/>
+          <body name="torso_link">
+            <inertial pos="0.00203158 0.000339683 0.184568" quat="0.999803 -6.03319e-05 0.0198256 0.00131986"
+              mass="7.818" diaginertia="0.121847 0.109825 0.0273735"/>
+            <joint name="waist_pitch_joint" axis="0 1 0" range="-0.52 0.52" actuatorfrcrange="-50 50"/>
+            <geom class="visual" mesh="torso_link"/>
+            <geom class="collision" mesh="torso_link"/>
+            <geom pos="0.0039635 0 -0.044" quat="1 0 0 0" class="visual" material="black" mesh="logo_link"/>
+            <geom pos="0.0039635 0 -0.044" quat="1 0 0 0" class="collision" material="black" mesh="logo_link"/>
+            <geom pos="0.0039635 0 -0.044" quat="1 0 0 0" class="visual" material="black" mesh="head_link"/>
+            <geom pos="0.0039635 0 -0.044" quat="1 0 0 0" class="collision" material="black" mesh="head_link"/>
+            <site name="imu_in_torso" pos="-0.03959 -0.00224 0.14792" size="0.01"/>
+            <body name="left_shoulder_pitch_link" pos="0.0039563 0.10022 0.24778"
+              quat="0.990264 0.139201 1.38722e-05 -9.86868e-05">
+              <inertial pos="0 0.035892 -0.011628" quat="0.654152 0.0130458 -0.326267 0.68225" mass="0.718"
+                diaginertia="0.000465864 0.000432842 0.000406394"/>
+              <joint name="left_shoulder_pitch_joint" axis="0 1 0" range="-3.0892 2.6704" actuatorfrcrange="-25 25"/>
+              <geom class="visual" mesh="left_shoulder_pitch_link"/>
+              <geom size="0.03 0.025" pos="0 0.04 -0.01" quat="0.707107 0 0.707107 0" type="cylinder"
+                rgba="0.7 0.7 0.7 1" class="collision"/>
+              <body name="left_shoulder_roll_link" pos="0 0.038 -0.013831" quat="0.990268 -0.139172 0 0">
+                <inertial pos="-0.000227 0.00727 -0.063243" quat="0.701256 -0.0196223 -0.00710317 0.712604" mass="0.643"
+                  diaginertia="0.000691311 0.000618011 0.000388977"/>
+                <joint name="left_shoulder_roll_joint" axis="1 0 0" range="-1.5882 2.2515" actuatorfrcrange="-25 25"/>
+                <geom class="visual" mesh="left_shoulder_roll_link"/>
+                <geom size="0.03 0.015" pos="-0.004 0.006 -0.053" type="cylinder" rgba="0.7 0.7 0.7 1" class="collision"/>
+                <body name="left_shoulder_yaw_link" pos="0 0.00624 -0.1032">
+                  <inertial pos="0.010773 -0.002949 -0.072009" quat="0.716879 -0.0964829 -0.0679942 0.687134"
+                    mass="0.734" diaginertia="0.00106187 0.00103217 0.000400661"/>
+                  <joint name="left_shoulder_yaw_joint" axis="0 0 1" range="-2.618 2.618" actuatorfrcrange="-25 25"/>
+                  <geom class="visual" mesh="left_shoulder_yaw_link"/>
+                  <geom class="collision" mesh="left_shoulder_yaw_link"/>
+                  <body name="left_elbow_link" pos="0.015783 0 -0.080518">
+                    <inertial pos="0.064956 0.004454 -0.010062" quat="0.541765 0.636132 0.388821 0.388129" mass="0.6"
+                      diaginertia="0.000443035 0.000421612 0.000259353"/>
+                    <joint name="left_elbow_joint" axis="0 1 0" range="-1.0472 2.0944" actuatorfrcrange="-25 25"/>
+                    <geom class="visual" mesh="left_elbow_link"/>
+                    <geom class="collision" mesh="left_elbow_link"/>
+                    <body name="left_wrist_roll_link" pos="0.1 0.00188791 -0.01">
+                      <inertial pos="0.0171394 0.000537591 4.8864e-07" quat="0.575338 0.411667 -0.574906 0.411094"
+                        mass="0.085445" diaginertia="5.48211e-05 4.96646e-05 3.57798e-05"/>
+                      <joint name="left_wrist_roll_joint" axis="1 0 0" range="-1.97222 1.97222"
+                        actuatorfrcrange="-25 25"/>
+                      <geom class="visual" mesh="left_wrist_roll_link"/>
+                      <geom class="collision" mesh="left_wrist_roll_link"/>
+                      <body name="left_wrist_pitch_link" pos="0.038 0 0">
+                        <inertial pos="0.0229999 -0.00111685 -0.00111658" quat="0.249998 0.661363 0.293036 0.643608"
+                          mass="0.48405" diaginertia="0.000430353 0.000429873 0.000164648"/>
+                        <joint name="left_wrist_pitch_joint" axis="0 1 0" range="-1.61443 1.61443"
+                          actuatorfrcrange="-5 5"/>
+                        <geom class="visual" mesh="left_wrist_pitch_link"/>
+                        <geom class="collision" mesh="left_wrist_pitch_link"/>
+                        <body name="left_wrist_yaw_link" pos="0.046 0 0">
+                          <inertial pos="0.0708244 0.000191745 0.00161742" quat="0.510571 0.526295 0.468078 0.493188"
+                            mass="0.254576" diaginertia="0.000646113 0.000559993 0.000147566"/>
+                          <joint name="left_wrist_yaw_joint" axis="0 0 1" range="-1.61443 1.61443"
+                            actuatorfrcrange="-5 5"/>
+                          <geom class="visual" mesh="left_wrist_yaw_link"/>
+                          <geom class="collision" mesh="left_wrist_yaw_link"/>
+                          <geom pos="0.0415 0.003 0" quat="1 0 0 0" class="visual" mesh="left_rubber_hand"/>
+                        </body>
                       </body>
                     </body>
                   </body>
-                  <body name="left_three_link" pos="0.205 0.004 0.02395">
-                    <inertial pos="0.0362561 -0.0015725 0.000269899" quat="0.540443 0.459072 0.5397 0.45376"
-                      mass="0.0488469" diaginertia="8.91742e-06 8.64873e-06 3.8319e-06"/>
-                    <joint name="left_three_joint" axis="0 0 1" range="-1.84 0.3"/>
-                    <geom type="mesh" class="visual" mesh="left_three_link"/>
-                    <geom type="mesh" mesh="left_three_link" class="collision"/>
-                    <body name="left_four_link" pos="0.0471 -0.0036 0">
-                      <inertial pos="0.0237591 0.00135257 0.000119669" quat="0.478926 0.48905 0.526348 0.504399"
-                        mass="0.0138584" diaginertia="2.55338e-06 2.04399e-06 9.3995e-07"/>
-                      <joint name="left_four_joint" axis="0 0 1" range="-1.84 0"/>
-                      <geom type="mesh" class="visual" mesh="left_four_link"/>
-                      <geom type="mesh" mesh="left_four_link" class="collision"/>
-                    </body>
-                  </body>
-                  <body name="left_five_link" pos="0.205 0.004 -0.02395">
-                    <inertial pos="0.0362561 -0.0015725 0.000269899" quat="0.540443 0.459072 0.5397 0.45376"
-                      mass="0.0488469" diaginertia="8.91742e-06 8.64873e-06 3.8319e-06"/>
-                    <joint name="left_five_joint" axis="0 0 1" range="-1.84 0.3"/>
-                    <geom type="mesh" class="visual" mesh="left_five_link"/>
-                    <geom type="mesh" mesh="left_five_link" class="collision"/>
-                    <body name="left_six_link" pos="0.0471 -0.0036 0">
-                      <inertial pos="0.0237591 0.00135257 0.000119669" quat="0.478926 0.48905 0.526348 0.504399"
-                        mass="0.0138584" diaginertia="2.55338e-06 2.04399e-06 9.3995e-07"/>
-                      <joint name="left_six_joint" axis="0 0 1" range="-1.84 0"/>
-                      <geom type="mesh" class="visual" mesh="left_six_link"/>
-                      <geom type="mesh" mesh="left_six_link" class="collision"/>
-                    </body>
-                  </body>
                 </body>
               </body>
             </body>
-          </body>
-        </body>
-        <body name="right_shoulder_pitch_link" pos="-0.0025 -0.10396 0.25928" quat="0.990268 -0.139172 0 0">
-          <inertial pos="-0.001431 -0.048811 0.001304" quat="0.588396 0.786417 -0.0523639 0.180543" mass="0.713"
-            diaginertia="0.000466421 0.000440181 0.000410999"/>
-          <joint name="right_shoulder_pitch_joint" axis="0 1 0" range="-2.9671 2.7925"/>
-          <geom type="mesh" class="visual" mesh="right_shoulder_pitch_link"/>
-          <geom type="mesh" mesh="right_shoulder_pitch_link" class="collision"/>
-          <body name="right_shoulder_roll_link" pos="0 -0.052 0" quat="0.990268 0.139172 0 0">
-            <inertial pos="-0.003415 -0.006955 -0.064598" quat="0.707282 0.00575207 0.0105364 0.70683" mass="0.642"
-              diaginertia="0.000683514 0.000616029 0.000372857"/>
-            <joint name="right_shoulder_roll_joint" axis="1 0 0" range="-2.2515 1.5882"/>
-            <geom type="mesh" class="visual" mesh="right_shoulder_roll_link"/>
-            <geom size="0.03 0.015" pos="-0.004 -0.006 -0.053" type="cylinder" class="collision"/>
-            <body name="right_shoulder_yaw_link" pos="-0.00354 -0.0062424 -0.1032">
-              <inertial pos="0.000375 0.00444 -0.072374" quat="0.42613 0.00985482 -0.0374183 0.903834" mass="0.713"
-                diaginertia="0.000977874 0.000964661 0.000379065"/>
-              <joint name="right_shoulder_yaw_joint" axis="0 0 1" range="-2.618 2.618"/>
-              <geom type="mesh" class="visual" mesh="right_shoulder_yaw_link"/>
-              <geom type="mesh" mesh="right_shoulder_yaw_link" class="collision"/>
-              <body name="right_elbow_pitch_link" pos="0 -0.00189 -0.0855">
-                <inertial pos="0.064497 -0.002873 0" quat="0.401069 0.400893 0.582495 0.582347" mass="0.601"
-                  diaginertia="0.00049549 0.0004712 0.00025371"/>
-                <joint name="right_elbow_pitch_joint" axis="0 1 0" range="-0.2268 3.4208"/>
-                <geom type="mesh" class="visual" mesh="right_elbow_pitch_link"/>
-                <geom type="mesh" mesh="right_elbow_pitch_link" class="collision"/>
-                <body name="right_elbow_roll_link" pos="0.1 0 0">
-                  <inertial pos="0.133814 -0.00147121 0.000265832" quat="0.506502 0.498782 0.497877 0.496781"
-                    mass="0.50826" diaginertia="0.00239763 0.00226639 0.000285577"/>
-                  <joint name="right_elbow_roll_joint" axis="1 0 0" range="-2.0943 2.0943"/>
-                  <geom type="mesh" class="visual" mesh="right_elbow_roll_link"/>
-                  <geom type="mesh" mesh="right_elbow_roll_link" class="collision"/>
-                  <geom pos="0.12 0 0" type="mesh" class="visual" mesh="right_palm_link"/>
-                  <geom pos="0.12 0 0" type="mesh" mesh="right_palm_link" class="collision"/>
-                  <body name="right_zero_link" pos="0.17 0 0">
-                    <inertial pos="-0.0260466 0.00877511 1.08605e-05" quat="0.381376 0.59241 -0.389884 0.592953"
-                      mass="0.0509893" diaginertia="8.68047e-06 7.23365e-06 4.58058e-06"/>
-                    <joint name="right_zero_joint" axis="0 1 0" range="-0.523598 0.523598"/>
-                    <geom type="mesh" class="visual" mesh="right_zero_link"/>
-                    <site name="right_palm" pos="0 0 0"/>
-                    <body name="right_one_link" pos="-0.026525 0.0188 -5e-05">
-                      <inertial pos="-0.00181538 0.0327057 -0.000185871" quat="0.698199 0.701646 0.116289 -0.0817825"
-                        mass="0.047762" diaginertia="8.03051e-06 7.8455e-06 3.65282e-06"/>
-                      <joint name="right_one_joint" axis="0 0 1" range="-1.2 1"/>
-                      <geom type="mesh" class="visual" mesh="right_one_link"/>
-                      <geom size="0.01 0.015 0.01" pos="-0.001 0.032 0" type="box" class="collision"/>
-                      <body name="right_two_link" pos="0 0.0431 0">
-                        <inertial pos="-0.00135257 0.0237591 -0.000119669" quat="0.695316 0.717995 0.0263741 0.0180122"
-                          mass="0.0138584" diaginertia="2.55338e-06 2.04399e-06 9.3995e-07"/>
-                        <joint name="right_two_joint" axis="0 0 1" range="-1.84 0"/>
-                        <geom type="mesh" class="visual" mesh="right_two_link"/>
-                        <geom type="mesh" mesh="right_two_link" class="collision"/>
+            <body name="right_shoulder_pitch_link" pos="0.0039563 -0.10021 0.24778"
+              quat="0.990264 -0.139201 1.38722e-05 9.86868e-05">
+              <inertial pos="0 -0.035892 -0.011628" quat="0.68225 -0.326267 0.0130458 0.654152" mass="0.718"
+                diaginertia="0.000465864 0.000432842 0.000406394"/>
+              <joint name="right_shoulder_pitch_joint" axis="0 1 0" range="-3.0892 2.6704" actuatorfrcrange="-25 25"/>
+              <geom class="visual" mesh="right_shoulder_pitch_link"/>
+              <geom size="0.03 0.025" pos="0 -0.04 -0.01" quat="0.707107 0 0.707107 0" type="cylinder"
+                rgba="0.7 0.7 0.7 1" class="collision"/>
+              <body name="right_shoulder_roll_link" pos="0 -0.038 -0.013831" quat="0.990268 0.139172 0 0">
+                <inertial pos="-0.000227 -0.00727 -0.063243" quat="0.712604 -0.00710317 -0.0196223 0.701256"
+                  mass="0.643" diaginertia="0.000691311 0.000618011 0.000388977"/>
+                <joint name="right_shoulder_roll_joint" axis="1 0 0" range="-2.2515 1.5882" actuatorfrcrange="-25 25"/>
+                <geom class="visual" mesh="right_shoulder_roll_link"/>
+                <geom size="0.03 0.015" pos="-0.004 -0.006 -0.053" type="cylinder" rgba="0.7 0.7 0.7 1"
+                  class="collision"/>
+                <body name="right_shoulder_yaw_link" pos="0 -0.00624 -0.1032">
+                  <inertial pos="0.010773 0.002949 -0.072009" quat="0.687134 -0.0679942 -0.0964829 0.716879"
+                    mass="0.734" diaginertia="0.00106187 0.00103217 0.000400661"/>
+                  <joint name="right_shoulder_yaw_joint" axis="0 0 1" range="-2.618 2.618" actuatorfrcrange="-25 25"/>
+                  <geom class="visual" mesh="right_shoulder_yaw_link"/>
+                  <geom class="collision" mesh="right_shoulder_yaw_link"/>
+                  <body name="right_elbow_link" pos="0.015783 0 -0.080518">
+                    <inertial pos="0.064956 -0.004454 -0.010062" quat="0.388129 0.388821 0.636132 0.541765" mass="0.6"
+                      diaginertia="0.000443035 0.000421612 0.000259353"/>
+                    <joint name="right_elbow_joint" axis="0 1 0" range="-1.0472 2.0944" actuatorfrcrange="-25 25"/>
+                    <geom class="visual" mesh="right_elbow_link"/>
+                    <geom class="collision" mesh="right_elbow_link"/>
+                    <body name="right_wrist_roll_link" pos="0.1 -0.00188791 -0.01">
+                      <inertial pos="0.0171394 -0.000537591 4.8864e-07" quat="0.411667 0.575338 -0.411094 0.574906"
+                        mass="0.085445" diaginertia="5.48211e-05 4.96646e-05 3.57798e-05"/>
+                      <joint name="right_wrist_roll_joint" axis="1 0 0" range="-1.97222 1.97222"
+                        actuatorfrcrange="-25 25"/>
+                      <geom class="visual" mesh="right_wrist_roll_link"/>
+                      <geom class="collision" mesh="right_wrist_roll_link"/>
+                      <body name="right_wrist_pitch_link" pos="0.038 0 0">
+                        <inertial pos="0.0229999 0.00111685 -0.00111658" quat="0.643608 0.293036 0.661363 0.249998"
+                          mass="0.48405" diaginertia="0.000430353 0.000429873 0.000164648"/>
+                        <joint name="right_wrist_pitch_joint" axis="0 1 0" range="-1.61443 1.61443"
+                          actuatorfrcrange="-5 5"/>
+                        <geom class="visual" mesh="right_wrist_pitch_link"/>
+                        <geom class="collision" mesh="right_wrist_pitch_link"/>
+                        <body name="right_wrist_yaw_link" pos="0.046 0 0">
+                          <inertial pos="0.0708244 -0.000191745 0.00161742" quat="0.493188 0.468078 0.526295 0.510571"
+                            mass="0.254576" diaginertia="0.000646113 0.000559993 0.000147566"/>
+                          <joint name="right_wrist_yaw_joint" axis="0 0 1" range="-1.61443 1.61443"
+                            actuatorfrcrange="-5 5"/>
+                          <geom class="visual" mesh="right_wrist_yaw_link"/>
+                          <geom class="collision" mesh="right_wrist_yaw_link"/>
+                          <geom pos="0.0415 -0.003 0" quat="1 0 0 0" class="visual" mesh="right_rubber_hand"/>
+                        </body>
                       </body>
                     </body>
                   </body>
-                  <body name="right_three_link" pos="0.205 -0.004 0.02395">
-                    <inertial pos="0.0362561 0.0015725 0.000269899" quat="0.45376 0.5397 0.459072 0.540443"
-                      mass="0.0488469" diaginertia="8.91742e-06 8.64873e-06 3.8319e-06"/>
-                    <joint name="right_three_joint" axis="0 0 1" range="-0.3 1.84"/>
-                    <geom type="mesh" class="visual" mesh="right_three_link"/>
-                    <geom type="mesh" mesh="right_three_link" class="collision"/>
-                    <body name="right_four_link" pos="0.0471 0.0036 0">
-                      <inertial pos="0.0237591 -0.00135257 0.000119669" quat="0.504399 0.526348 0.48905 0.478926"
-                        mass="0.0138584" diaginertia="2.55338e-06 2.04399e-06 9.3995e-07"/>
-                      <joint name="right_four_joint" axis="0 0 1" range="0 1.84"/>
-                      <geom type="mesh" class="visual" mesh="right_four_link"/>
-                      <geom type="mesh" mesh="right_four_link" class="collision"/>
-                    </body>
-                  </body>
-                  <body name="right_five_link" pos="0.205 -0.004 -0.02395">
-                    <inertial pos="0.0362561 0.0015725 0.000269899" quat="0.45376 0.5397 0.459072 0.540443"
-                      mass="0.0488469" diaginertia="8.91742e-06 8.64873e-06 3.8319e-06"/>
-                    <joint name="right_five_joint" axis="0 0 1" range="-0.3 1.84"/>
-                    <geom type="mesh" class="visual" mesh="right_five_link"/>
-                    <geom type="mesh" mesh="right_five_link" class="collision"/>
-                    <body name="right_six_link" pos="0.0471 0.0036 0">
-                      <inertial pos="0.0237591 -0.00135257 0.000119669" quat="0.504399 0.526348 0.48905 0.478926"
-                        mass="0.0138584" diaginertia="2.55338e-06 2.04399e-06 9.3995e-07"/>
-                      <joint name="right_six_joint" axis="0 0 1" range="0 1.84"/>
-                      <geom type="mesh" class="visual" mesh="right_six_link"/>
-                      <geom type="mesh" mesh="right_six_link" class="collision"/>
-                    </body>
-                  </body>
                 </body>
               </body>
             </body>
@@ -367,68 +300,65 @@
   </worldbody>
 
   <actuator>
-    <motor class="g1" name="left_hip_pitch_joint" joint="left_hip_pitch_joint" ctrlrange="-88 88"/>
-    <motor class="g1" name="left_hip_roll_joint" joint="left_hip_roll_joint" ctrlrange="-88 88"/>
-    <motor class="g1" name="left_hip_yaw_joint" joint="left_hip_yaw_joint" ctrlrange="-88 88"/>
-    <motor class="g1" name="left_knee_joint" joint="left_knee_joint" ctrlrange="-139 139"/>
-    <motor class="g1" name="left_ankle_pitch_joint" joint="left_ankle_pitch_joint" ctrlrange="-40 40"/>
-    <motor class="g1" name="left_ankle_roll_joint" joint="left_ankle_roll_joint" ctrlrange="-40 40"/>
+    <position class="g1" name="left_hip_pitch_joint" joint="left_hip_pitch_joint"/>
+    <position class="g1" name="left_hip_roll_joint" joint="left_hip_roll_joint"/>
+    <position class="g1" name="left_hip_yaw_joint" joint="left_hip_yaw_joint"/>
+    <position class="g1" name="left_knee_joint" joint="left_knee_joint"/>
+    <position class="g1" name="left_ankle_pitch_joint" joint="left_ankle_pitch_joint"/>
+    <position class="g1" name="left_ankle_roll_joint" joint="left_ankle_roll_joint"/>
 
-    <motor class="g1" name="right_hip_pitch_joint" joint="right_hip_pitch_joint" ctrlrange="-88 88"/>
-    <motor class="g1" name="right_hip_roll_joint" joint="right_hip_roll_joint" ctrlrange="-88 88"/>
-    <motor class="g1" name="right_hip_yaw_joint" joint="right_hip_yaw_joint" ctrlrange="-88 88"/>
-    <motor class="g1" name="right_knee_joint" joint="right_knee_joint" ctrlrange="-139 139"/>
-    <motor class="g1" name="right_ankle_pitch_joint" joint="right_ankle_pitch_joint" ctrlrange="-40 40"/>
-    <motor class="g1" name="right_ankle_roll_joint" joint="right_ankle_roll_joint" ctrlrange="-40 40"/>
+    <position class="g1" name="right_hip_pitch_joint" joint="right_hip_pitch_joint"/>
+    <position class="g1" name="right_hip_roll_joint" joint="right_hip_roll_joint"/>
+    <position class="g1" name="right_hip_yaw_joint" joint="right_hip_yaw_joint"/>
+    <position class="g1" name="right_knee_joint" joint="right_knee_joint"/>
+    <position class="g1" name="right_ankle_pitch_joint" joint="right_ankle_pitch_joint"/>
+    <position class="g1" name="right_ankle_roll_joint" joint="right_ankle_roll_joint"/>
 
-    <motor class="g1" name="torso_joint" joint="torso_joint" ctrlrange="-88 88"/>
+    <position class="g1" name="waist_yaw_joint" joint="waist_yaw_joint"/>
+    <position class="g1" name="waist_roll_joint" joint="waist_roll_joint"/>
+    <position class="g1" name="waist_pitch_joint" joint="waist_pitch_joint"/>
 
-    <motor class="arm_joint" name="left_shoulder_pitch_joint" joint="left_shoulder_pitch_joint"/>
-    <motor class="arm_joint" name="left_shoulder_roll_joint" joint="left_shoulder_roll_joint"/>
-    <motor class="arm_joint" name="left_shoulder_yaw_joint" joint="left_shoulder_yaw_joint"/>
-    <motor class="arm_joint" name="left_elbow_pitch_joint" joint="left_elbow_pitch_joint"/>
-    <motor class="arm_joint" name="left_elbow_roll_joint" joint="left_elbow_roll_joint"/>
+    <position class="g1" name="left_shoulder_pitch_joint" joint="left_shoulder_pitch_joint"/>
+    <position class="g1" name="left_shoulder_roll_joint" joint="left_shoulder_roll_joint"/>
+    <position class="g1" name="left_shoulder_yaw_joint" joint="left_shoulder_yaw_joint"/>
+    <position class="g1" name="left_elbow_joint" joint="left_elbow_joint"/>
+    <position class="g1" name="left_wrist_roll_joint" joint="left_wrist_roll_joint"/>
+    <position class="g1" name="left_wrist_pitch_joint" joint="left_wrist_pitch_joint"/>
+    <position class="g1" name="left_wrist_yaw_joint" joint="left_wrist_yaw_joint"/>
 
-    <motor class="arm_joint" name="right_shoulder_pitch_joint" joint="right_shoulder_pitch_joint"/>
-    <motor class="arm_joint" name="right_shoulder_roll_joint" joint="right_shoulder_roll_joint"/>
-    <motor class="arm_joint" name="right_shoulder_yaw_joint" joint="right_shoulder_yaw_joint"/>
-    <motor class="arm_joint" name="right_elbow_pitch_joint" joint="right_elbow_pitch_joint"/>
-    <motor class="arm_joint" name="right_elbow_roll_joint" joint="right_elbow_roll_joint"/>
-
-    <motor class="hand_joint" name="left_zero_joint" joint="left_zero_joint"/>
-    <motor class="hand_joint" name="left_one_joint" joint="left_one_joint"/>
-    <motor class="hand_joint" name="left_two_joint" joint="left_two_joint"/>
-    <motor class="hand_joint" name="left_three_joint" joint="left_three_joint"/>
-    <motor class="hand_joint" name="left_four_joint" joint="left_four_joint"/>
-    <motor class="hand_joint" name="left_five_joint" joint="left_five_joint"/>
-    <motor class="hand_joint" name="left_six_joint" joint="left_six_joint"/>
-
-    <motor class="hand_joint" name="right_zero_joint" joint="right_zero_joint"/>
-    <motor class="hand_joint" name="right_one_joint" joint="right_one_joint"/>
-    <motor class="hand_joint" name="right_two_joint" joint="right_two_joint"/>
-    <motor class="hand_joint" name="right_three_joint" joint="right_three_joint"/>
-    <motor class="hand_joint" name="right_four_joint" joint="right_four_joint"/>
-    <motor class="hand_joint" name="right_five_joint" joint="right_five_joint"/>
-    <motor class="hand_joint" name="right_six_joint" joint="right_six_joint"/>
+    <position class="g1" name="right_shoulder_pitch_joint" joint="right_shoulder_pitch_joint"/>
+    <position class="g1" name="right_shoulder_roll_joint" joint="right_shoulder_roll_joint"/>
+    <position class="g1" name="right_shoulder_yaw_joint" joint="right_shoulder_yaw_joint"/>
+    <position class="g1" name="right_elbow_joint" joint="right_elbow_joint"/>
+    <position class="g1" name="right_wrist_roll_joint" joint="right_wrist_roll_joint"/>
+    <position class="g1" name="right_wrist_pitch_joint" joint="right_wrist_pitch_joint"/>
+    <position class="g1" name="right_wrist_yaw_joint" joint="right_wrist_yaw_joint"/>
   </actuator>
 
   <sensor>
-    <gyro name="imu-angular-velocity" site="imu" cutoff="34.9"/>
-    <accelerometer name="imu-linear-acceleration" site="imu" cutoff="157"/>
-    <framequat name="imu-body-quat" objtype="site" objname="imu"/>
+    <gyro site="imu_in_torso" name="imu-torso-angular-velocity" cutoff="34.9" noise="0.0005"/>
+    <accelerometer site="imu_in_torso" name="imu-torso-linear-acceleration" cutoff="157" noise="0.01"/>
+    <gyro site="imu_in_pelvis" name="imu-pelvis-angular-velocity" cutoff="34.9" noise="0.0005"/>
+    <accelerometer site="imu_in_pelvis" name="imu-pelvis-linear-acceleration" cutoff="157" noise="0.01"/>
   </sensor>
 
   <keyframe>
     <key name="stand"
       qpos="
-      0 0 0.75
+      0 0 0.79
       1 0 0 0
       0 0 0 0 0 0
       0 0 0 0 0 0
-      0
-      0 0 0 0 0
-      0 0 0 0 0
-      0 0 0 0 0 0 0
-      0 0 0 0 0 0 0"/>
+      0 0 0
+      0.2 0.2 0 1.28 0 0 0
+      0.2 -0.2 0 1.28 0 0 0
+      "
+      ctrl="
+      0 0 0 0 0 0
+      0 0 0 0 0 0
+      0 0 0
+      0.2 0.2 0 1.28 0 0 0
+      0.2 -0.2 0 1.28 0 0 0
+      "/>
   </keyframe>
 </mujoco>
diff --git a/unitree_g1/g1_with_hands.png b/unitree_g1/g1_with_hands.png
new file mode 100644
index 00000000..27bdf3ba
Binary files /dev/null and b/unitree_g1/g1_with_hands.png differ
diff --git a/unitree_g1/g1_with_hands.xml b/unitree_g1/g1_with_hands.xml
new file mode 100644
index 00000000..e8a1ad38
--- /dev/null
+++ b/unitree_g1/g1_with_hands.xml
@@ -0,0 +1,515 @@
+<mujoco model="g1_29dof_with_hand_rev_1_0">
+  <compiler angle="radian" meshdir="assets"/>
+
+  <option integrator="implicitfast"/>
+
+  <default>
+    <default class="g1">
+      <site rgba="1 0 0 1" size="0.01" group="5"/>
+      <joint armature="0.01" frictionloss="0.3"/>
+      <position kp="500" dampratio="1" inheritrange="1"/>
+      <default class="visual">
+        <geom group="2" type="mesh" contype="0" conaffinity="0" density="0" material="metal"/>
+      </default>
+      <default class="collision">
+        <geom group="3" type="mesh"/>
+        <default class="foot">
+          <geom type="sphere" size="0.005" priority="1" friction="0.6" condim="3"/>
+        </default>
+      </default>
+    </default>
+  </default>
+
+  <asset>
+    <material name="black" rgba="0.2 0.2 0.2 1"/>
+    <material name="metal" rgba="0.7 0.7 0.7 1"/>
+
+    <mesh file="pelvis.STL"/>
+    <mesh file="pelvis_contour_link.STL"/>
+    <mesh file="left_hip_pitch_link.STL"/>
+    <mesh file="left_hip_roll_link.STL"/>
+    <mesh file="left_hip_yaw_link.STL"/>
+    <mesh file="left_knee_link.STL"/>
+    <mesh file="left_ankle_pitch_link.STL"/>
+    <mesh file="left_ankle_roll_link.STL"/>
+    <mesh file="right_hip_pitch_link.STL"/>
+    <mesh file="right_hip_roll_link.STL"/>
+    <mesh file="right_hip_yaw_link.STL"/>
+    <mesh file="right_knee_link.STL"/>
+    <mesh file="right_ankle_pitch_link.STL"/>
+    <mesh file="right_ankle_roll_link.STL"/>
+    <mesh name="waist_yaw_link" file="waist_yaw_link_rev_1_0.STL"/>
+    <mesh name="waist_roll_link" file="waist_roll_link_rev_1_0.STL"/>
+    <mesh name="torso_link" file="torso_link_rev_1_0.STL"/>
+    <mesh file="logo_link.STL"/>
+    <mesh file="head_link.STL"/>
+    <mesh file="left_shoulder_pitch_link.STL"/>
+    <mesh file="left_shoulder_roll_link.STL"/>
+    <mesh file="left_shoulder_yaw_link.STL"/>
+    <mesh file="left_elbow_link.STL"/>
+    <mesh file="left_wrist_roll_link.STL"/>
+    <mesh name="left_wrist_pitch_link" file="left_wrist_pitch_link.STL"/>
+    <mesh file="left_wrist_yaw_link.STL"/>
+    <mesh file="left_hand_palm_link.STL"/>
+    <mesh file="left_hand_thumb_0_link.STL"/>
+    <mesh file="left_hand_thumb_1_link.STL"/>
+    <mesh file="left_hand_thumb_2_link.STL"/>
+    <mesh file="left_hand_middle_0_link.STL"/>
+    <mesh file="left_hand_middle_1_link.STL"/>
+    <mesh file="left_hand_index_0_link.STL"/>
+    <mesh file="left_hand_index_1_link.STL"/>
+    <mesh file="right_shoulder_pitch_link.STL"/>
+    <mesh file="right_shoulder_roll_link.STL"/>
+    <mesh file="right_shoulder_yaw_link.STL"/>
+    <mesh file="right_elbow_link.STL"/>
+    <mesh file="right_wrist_roll_link.STL"/>
+    <mesh file="right_wrist_pitch_link.STL"/>
+    <mesh file="right_wrist_yaw_link.STL"/>
+    <mesh file="right_hand_palm_link.STL"/>
+    <mesh file="right_hand_thumb_0_link.STL"/>
+    <mesh file="right_hand_thumb_1_link.STL"/>
+    <mesh file="right_hand_thumb_2_link.STL"/>
+    <mesh file="right_hand_middle_0_link.STL"/>
+    <mesh file="right_hand_middle_1_link.STL"/>
+    <mesh file="right_hand_index_0_link.STL"/>
+    <mesh file="right_hand_index_1_link.STL"/>
+  </asset>
+
+  <worldbody>
+    <light pos="1 0 3.5" dir="0 0 -1" directional="true"/>
+    <body name="pelvis" pos="0 0 0.793" childclass="g1">
+      <inertial pos="0 0 -0.07605" quat="1 0 -0.000399148 0" mass="3.813" diaginertia="0.010549 0.0093089 0.0079184"/>
+      <freejoint name="floating_base_joint"/>
+      <geom class="visual" material="black" mesh="pelvis"/>
+      <geom class="visual" mesh="pelvis_contour_link"/>
+      <geom class="collision" mesh="pelvis_contour_link"/>
+      <site name="imu_in_pelvis" size="0.01" pos="0.04525 0 -0.08339"/>
+      <body name="left_hip_pitch_link" pos="0 0.064452 -0.1027">
+        <inertial pos="0.002741 0.047791 -0.02606" quat="0.954862 0.293964 0.0302556 0.030122" mass="1.35"
+          diaginertia="0.00181517 0.00153422 0.00116212"/>
+        <joint name="left_hip_pitch_joint" axis="0 1 0" range="-2.5307 2.8798" actuatorfrcrange="-88 88"/>
+        <geom class="visual" material="black" mesh="left_hip_pitch_link"/>
+        <geom class="collision" mesh="left_hip_pitch_link"/>
+        <body name="left_hip_roll_link" pos="0 0.052 -0.030465" quat="0.996179 0 -0.0873386 0">
+          <inertial pos="0.029812 -0.001045 -0.087934" quat="0.977808 -1.97119e-05 0.205576 -0.0403793" mass="1.52"
+            diaginertia="0.00254986 0.00241169 0.00148755"/>
+          <joint name="left_hip_roll_joint" axis="1 0 0" range="-0.5236 2.9671" actuatorfrcrange="-139 139"/>
+          <geom class="visual" mesh="left_hip_roll_link"/>
+          <geom class="collision" mesh="left_hip_roll_link"/>
+          <body name="left_hip_yaw_link" pos="0.025001 0 -0.12412">
+            <inertial pos="-0.057709 -0.010981 -0.15078" quat="0.600598 0.15832 0.223482 0.751181" mass="1.702"
+              diaginertia="0.00776166 0.00717575 0.00160139"/>
+            <joint name="left_hip_yaw_joint" axis="0 0 1" range="-2.7576 2.7576" actuatorfrcrange="-88 88"/>
+            <geom class="visual" mesh="left_hip_yaw_link"/>
+            <geom class="collision" mesh="left_hip_yaw_link"/>
+            <body name="left_knee_link" pos="-0.078273 0.0021489 -0.17734" quat="0.996179 0 0.0873386 0">
+              <inertial pos="0.005457 0.003964 -0.12074" quat="0.923418 -0.0327699 0.0158246 0.382067" mass="1.932"
+                diaginertia="0.0113804 0.0112778 0.00146458"/>
+              <joint name="left_knee_joint" axis="0 1 0" range="-0.087267 2.8798" actuatorfrcrange="-139 139"/>
+              <geom class="visual" mesh="left_knee_link"/>
+              <geom class="collision" mesh="left_knee_link"/>
+              <body name="left_ankle_pitch_link" pos="0 -9.4445e-05 -0.30001">
+                <inertial pos="-0.007269 0 0.011137" quat="0.603053 0.369225 0.369225 0.603053" mass="0.074"
+                  diaginertia="1.89e-05 1.40805e-05 6.9195e-06"/>
+                <joint name="left_ankle_pitch_joint" axis="0 1 0" range="-0.87267 0.5236" actuatorfrcrange="-50 50"/>
+                <geom class="visual" mesh="left_ankle_pitch_link"/>
+                <geom class="collision" mesh="left_ankle_pitch_link"/>
+                <body name="left_ankle_roll_link" pos="0 0 -0.017558">
+                  <inertial pos="0.026505 0 -0.016425" quat="-0.000481092 0.728482 -0.000618967 0.685065" mass="0.608"
+                    diaginertia="0.00167218 0.0016161 0.000217621"/>
+                  <joint name="left_ankle_roll_joint" axis="1 0 0" range="-0.2618 0.2618" actuatorfrcrange="-50 50"/>
+                  <geom class="visual" material="black" mesh="left_ankle_roll_link"/>
+                  <geom class="foot" pos="-0.05 0.025 -0.03"/>
+                  <geom class="foot" pos="-0.05 -0.025 -0.03"/>
+                  <geom class="foot" pos="0.12 0.03 -0.03"/>
+                  <geom class="foot" pos="0.12 -0.03 -0.03"/>
+                  <site name="left_foot"/>
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+      <body name="right_hip_pitch_link" pos="0 -0.064452 -0.1027">
+        <inertial pos="0.002741 -0.047791 -0.02606" quat="0.954862 -0.293964 0.0302556 -0.030122" mass="1.35"
+          diaginertia="0.00181517 0.00153422 0.00116212"/>
+        <joint name="right_hip_pitch_joint" axis="0 1 0" range="-2.5307 2.8798" actuatorfrcrange="-88 88"/>
+        <geom class="visual" material="black" mesh="right_hip_pitch_link"/>
+        <geom class="collision" mesh="right_hip_pitch_link"/>
+        <body name="right_hip_roll_link" pos="0 -0.052 -0.030465" quat="0.996179 0 -0.0873386 0">
+          <inertial pos="0.029812 0.001045 -0.087934" quat="0.977808 1.97119e-05 0.205576 0.0403793" mass="1.52"
+            diaginertia="0.00254986 0.00241169 0.00148755"/>
+          <joint name="right_hip_roll_joint" axis="1 0 0" range="-2.9671 0.5236" actuatorfrcrange="-139 139"/>
+          <geom class="visual" mesh="right_hip_roll_link"/>
+          <geom class="collision" mesh="right_hip_roll_link"/>
+          <body name="right_hip_yaw_link" pos="0.025001 0 -0.12412">
+            <inertial pos="-0.057709 0.010981 -0.15078" quat="0.751181 0.223482 0.15832 0.600598" mass="1.702"
+              diaginertia="0.00776166 0.00717575 0.00160139"/>
+            <joint name="right_hip_yaw_joint" axis="0 0 1" range="-2.7576 2.7576" actuatorfrcrange="-88 88"/>
+            <geom class="visual" mesh="right_hip_yaw_link"/>
+            <geom class="collision" mesh="right_hip_yaw_link"/>
+            <body name="right_knee_link" pos="-0.078273 -0.0021489 -0.17734" quat="0.996179 0 0.0873386 0">
+              <inertial pos="0.005457 -0.003964 -0.12074" quat="0.923439 0.0345276 0.0116333 -0.382012" mass="1.932"
+                diaginertia="0.011374 0.0112843 0.00146452"/>
+              <joint name="right_knee_joint" axis="0 1 0" range="-0.087267 2.8798" actuatorfrcrange="-139 139"/>
+              <geom class="visual" mesh="right_knee_link"/>
+              <geom class="collision" mesh="right_knee_link"/>
+              <body name="right_ankle_pitch_link" pos="0 9.4445e-05 -0.30001">
+                <inertial pos="-0.007269 0 0.011137" quat="0.603053 0.369225 0.369225 0.603053" mass="0.074"
+                  diaginertia="1.89e-05 1.40805e-05 6.9195e-06"/>
+                <joint name="right_ankle_pitch_joint" axis="0 1 0" range="-0.87267 0.5236" actuatorfrcrange="-50 50"/>
+                <geom class="visual" mesh="right_ankle_pitch_link"/>
+                <geom class="collision" mesh="right_ankle_pitch_link"/>
+                <body name="right_ankle_roll_link" pos="0 0 -0.017558">
+                  <inertial pos="0.026505 0 -0.016425" quat="0.000481092 0.728482 0.000618967 0.685065" mass="0.608"
+                    diaginertia="0.00167218 0.0016161 0.000217621"/>
+                  <joint name="right_ankle_roll_joint" axis="1 0 0" range="-0.2618 0.2618" actuatorfrcrange="-50 50"/>
+                  <geom class="visual" material="black" mesh="right_ankle_roll_link"/>
+                  <geom class="foot" pos="-0.05 0.025 -0.03"/>
+                  <geom class="foot" pos="-0.05 -0.025 -0.03"/>
+                  <geom class="foot" pos="0.12 0.03 -0.03"/>
+                  <geom class="foot" pos="0.12 -0.03 -0.03"/>
+                  <site name="right_foot"/>
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+      <body name="waist_yaw_link">
+        <inertial pos="0.003494 0.000233 0.018034" quat="0.289697 0.591001 -0.337795 0.672821" mass="0.214"
+          diaginertia="0.000163531 0.000107714 0.000102205"/>
+        <joint name="waist_yaw_joint" axis="0 0 1" range="-2.618 2.618" actuatorfrcrange="-88 88"/>
+        <geom class="visual" mesh="waist_yaw_link"/>
+        <body name="waist_roll_link" pos="-0.0039635 0 0.044">
+          <inertial pos="0 2.3e-05 0" quat="0.5 0.5 -0.5 0.5" mass="0.086" diaginertia="8.245e-06 7.079e-06 6.339e-06"/>
+          <joint name="waist_roll_joint" axis="1 0 0" range="-0.52 0.52" actuatorfrcrange="-50 50"/>
+          <geom class="visual" mesh="waist_roll_link"/>
+          <body name="torso_link">
+            <inertial pos="0.00203158 0.000339683 0.184568" quat="0.999803 -6.03319e-05 0.0198256 0.00131986"
+              mass="7.818" diaginertia="0.121847 0.109825 0.0273735"/>
+            <joint name="waist_pitch_joint" axis="0 1 0" range="-0.52 0.52" actuatorfrcrange="-50 50"/>
+            <geom class="visual" mesh="torso_link"/>
+            <geom class="collision" mesh="torso_link"/>
+            <geom pos="0.0039635 0 -0.044" quat="1 0 0 0" class="visual" material="black" mesh="logo_link"/>
+            <geom pos="0.0039635 0 -0.044" class="visual" material="black" mesh="head_link"/>
+            <geom pos="0.0039635 0 -0.044" class="collision" mesh="head_link"/>
+            <site name="imu_in_torso" size="0.01" pos="-0.03959 -0.00224 0.14792"/>
+            <body name="left_shoulder_pitch_link" pos="0.0039563 0.10022 0.24778"
+              quat="0.990264 0.139201 1.38722e-05 -9.86868e-05">
+              <inertial pos="0 0.035892 -0.011628" quat="0.654152 0.0130458 -0.326267 0.68225" mass="0.718"
+                diaginertia="0.000465864 0.000432842 0.000406394"/>
+              <joint name="left_shoulder_pitch_joint" axis="0 1 0" range="-3.0892 2.6704" actuatorfrcrange="-25 25"/>
+              <geom class="visual" mesh="left_shoulder_pitch_link"/>
+              <geom size="0.03 0.025" pos="0 0.04 -0.01" quat="0.707107 0 0.707107 0" type="cylinder" class="collision"/>
+              <body name="left_shoulder_roll_link" pos="0 0.038 -0.013831" quat="0.990268 -0.139172 0 0">
+                <inertial pos="-0.000227 0.00727 -0.063243" quat="0.701256 -0.0196223 -0.00710317 0.712604" mass="0.643"
+                  diaginertia="0.000691311 0.000618011 0.000388977"/>
+                <joint name="left_shoulder_roll_joint" axis="1 0 0" range="-1.5882 2.2515" actuatorfrcrange="-25 25"/>
+                <geom class="visual" mesh="left_shoulder_roll_link"/>
+                <geom size="0.03 0.015" pos="-0.004 0.006 -0.053" type="cylinder" class="collision"/>
+                <body name="left_shoulder_yaw_link" pos="0 0.00624 -0.1032">
+                  <inertial pos="0.010773 -0.002949 -0.072009" quat="0.716879 -0.0964829 -0.0679942 0.687134"
+                    mass="0.734" diaginertia="0.00106187 0.00103217 0.000400661"/>
+                  <joint name="left_shoulder_yaw_joint" axis="0 0 1" range="-2.618 2.618" actuatorfrcrange="-25 25"/>
+                  <geom class="visual" mesh="left_shoulder_yaw_link"/>
+                  <geom class="collision" mesh="left_shoulder_yaw_link"/>
+                  <body name="left_elbow_link" pos="0.015783 0 -0.080518">
+                    <inertial pos="0.064956 0.004454 -0.010062" quat="0.541765 0.636132 0.388821 0.388129" mass="0.6"
+                      diaginertia="0.000443035 0.000421612 0.000259353"/>
+                    <joint name="left_elbow_joint" axis="0 1 0" range="-1.0472 2.0944" actuatorfrcrange="-25 25"/>
+                    <geom class="visual" mesh="left_elbow_link"/>
+                    <geom class="collision" mesh="left_elbow_link"/>
+                    <body name="left_wrist_roll_link" pos="0.1 0.00188791 -0.01">
+                      <inertial pos="0.0171394 0.000537591 4.8864e-07" quat="0.575338 0.411667 -0.574906 0.411094"
+                        mass="0.085445" diaginertia="5.48211e-05 4.96646e-05 3.57798e-05"/>
+                      <joint name="left_wrist_roll_joint" axis="1 0 0" range="-1.97222 1.97222"
+                        actuatorfrcrange="-25 25"/>
+                      <geom class="visual" mesh="left_wrist_roll_link"/>
+                      <geom class="collision" mesh="left_wrist_roll_link"/>
+                      <body name="left_wrist_pitch_link" pos="0.038 0 0">
+                        <inertial pos="0.0229999 -0.00111685 -0.00111658" quat="0.249998 0.661363 0.293036 0.643608"
+                          mass="0.48405" diaginertia="0.000430353 0.000429873 0.000164648"/>
+                        <joint name="left_wrist_pitch_joint" axis="0 1 0" range="-1.61443 1.61443"
+                          actuatorfrcrange="-5 5"/>
+                        <geom class="visual" mesh="left_wrist_pitch_link"/>
+                        <geom class="collision" mesh="left_wrist_pitch_link"/>
+                        <body name="left_wrist_yaw_link" pos="0.046 0 0">
+                          <inertial pos="0.0885506 0.00212216 -0.000374562" quat="0.487149 0.493844 0.513241 0.505358"
+                            mass="0.457415" diaginertia="0.00105989 0.000895419 0.000323842"/>
+                          <joint name="left_wrist_yaw_joint" axis="0 0 1" range="-1.61443 1.61443"
+                            actuatorfrcrange="-5 5"/>
+                          <geom class="visual" mesh="left_wrist_yaw_link"/>
+                          <geom class="collision" mesh="left_wrist_yaw_link"/>
+                          <geom pos="0.0415 0.003 0" quat="1 0 0 0" class="visual" mesh="left_hand_palm_link"/>
+                          <geom pos="0.0415 0.003 0" quat="1 0 0 0" class="collision" mesh="left_hand_palm_link"/>
+                          <body name="left_hand_thumb_0_link" pos="0.067 0.003 0">
+                            <inertial pos="-0.000884246 -0.00863407 0.000944293"
+                              quat="0.462991 0.643965 -0.460173 0.398986" mass="0.0862366"
+                              diaginertia="1.6546e-05 1.60058e-05 1.43741e-05"/>
+                            <joint name="left_hand_thumb_0_joint" axis="0 1 0" range="-1.0472 1.0472"
+                              actuatorfrcrange="-2.45 2.45"/>
+                            <geom class="visual" mesh="left_hand_thumb_0_link"/>
+                            <geom class="collision" mesh="left_hand_thumb_0_link"/>
+                            <body name="left_hand_thumb_1_link" pos="-0.0025 -0.0193 0">
+                              <inertial pos="-0.000827888 -0.0354744 -0.0003809"
+                                quat="0.685598 0.705471 -0.15207 0.0956069" mass="0.0588507"
+                                diaginertia="1.28514e-05 1.22902e-05 5.9666e-06"/>
+                              <joint name="left_hand_thumb_1_joint" axis="0 0 1" range="-0.724312 1.0472"
+                                actuatorfrcrange="-1.4 1.4"/>
+                              <geom class="visual" mesh="left_hand_thumb_1_link"/>
+                              <geom size="0.01 0.015 0.01" pos="-0.001 -0.032 0" type="box" class="collision"/>
+                              <body name="left_hand_thumb_2_link" pos="0 -0.0458 0">
+                                <inertial pos="-0.00171735 -0.0262819 0.000107789"
+                                  quat="0.703174 0.710977 -0.00017564 -0.00766553" mass="0.0203063"
+                                  diaginertia="4.61314e-06 3.86645e-06 1.53495e-06"/>
+                                <joint name="left_hand_thumb_2_joint" axis="0 0 1" range="0 1.74533"
+                                  actuatorfrcrange="-1.4 1.4"/>
+                                <geom class="visual" mesh="left_hand_thumb_2_link"/>
+                                <geom class="collision" mesh="left_hand_thumb_2_link"/>
+                              </body>
+                            </body>
+                          </body>
+                          <body name="left_hand_middle_0_link" pos="0.1192 0.0046 -0.0285">
+                            <inertial pos="0.0354744 0.000827888 0.0003809" quat="0.391313 0.552395 0.417187 0.606373"
+                              mass="0.0588507" diaginertia="1.28514e-05 1.22902e-05 5.9666e-06"/>
+                            <joint name="left_hand_middle_0_joint" axis="0 0 1" range="-1.5708 0"
+                              actuatorfrcrange="-1.4 1.4"/>
+                            <geom class="visual" mesh="left_hand_middle_0_link"/>
+                            <geom class="collision" mesh="left_hand_middle_0_link"/>
+                            <body name="left_hand_middle_1_link" pos="0.0458 0 0">
+                              <inertial pos="0.0262819 0.00171735 -0.000107789"
+                                quat="0.502612 0.491799 0.502639 0.502861" mass="0.0203063"
+                                diaginertia="4.61314e-06 3.86645e-06 1.53495e-06"/>
+                              <joint name="left_hand_middle_1_joint" axis="0 0 1" range="-1.74533 0"
+                                actuatorfrcrange="-1.4 1.4"/>
+                              <geom class="visual" mesh="left_hand_middle_1_link"/>
+                              <geom class="collision" mesh="left_hand_middle_1_link"/>
+                            </body>
+                          </body>
+                          <body name="left_hand_index_0_link" pos="0.1192 0.0046 0.0285">
+                            <inertial pos="0.0354744 0.000827888 0.0003809" quat="0.391313 0.552395 0.417187 0.606373"
+                              mass="0.0588507" diaginertia="1.28514e-05 1.22902e-05 5.9666e-06"/>
+                            <joint name="left_hand_index_0_joint" axis="0 0 1" range="-1.5708 0"
+                              actuatorfrcrange="-1.4 1.4"/>
+                            <geom class="visual" mesh="left_hand_index_0_link"/>
+                            <geom class="collision" mesh="left_hand_index_0_link"/>
+                            <body name="left_hand_index_1_link" pos="0.0458 0 0">
+                              <inertial pos="0.0262819 0.00171735 -0.000107789"
+                                quat="0.502612 0.491799 0.502639 0.502861" mass="0.0203063"
+                                diaginertia="4.61314e-06 3.86645e-06 1.53495e-06"/>
+                              <joint name="left_hand_index_1_joint" axis="0 0 1" range="-1.74533 0"
+                                actuatorfrcrange="-1.4 1.4"/>
+                              <geom class="visual" mesh="left_hand_index_1_link"/>
+                              <geom class="collision" mesh="left_hand_index_1_link"/>
+                            </body>
+                          </body>
+                        </body>
+                      </body>
+                    </body>
+                  </body>
+                </body>
+              </body>
+            </body>
+            <body name="right_shoulder_pitch_link" pos="0.0039563 -0.10021 0.24778"
+              quat="0.990264 -0.139201 1.38722e-05 9.86868e-05">
+              <inertial pos="0 -0.035892 -0.011628" quat="0.68225 -0.326267 0.0130458 0.654152" mass="0.718"
+                diaginertia="0.000465864 0.000432842 0.000406394"/>
+              <joint name="right_shoulder_pitch_joint" axis="0 1 0" range="-3.0892 2.6704" actuatorfrcrange="-25 25"/>
+              <geom class="visual" mesh="right_shoulder_pitch_link"/>
+              <geom size="0.03 0.025" pos="0 -0.04 -0.01" quat="0.707107 0 0.707107 0" type="cylinder" class="collision"/>
+              <body name="right_shoulder_roll_link" pos="0 -0.038 -0.013831" quat="0.990268 0.139172 0 0">
+                <inertial pos="-0.000227 -0.00727 -0.063243" quat="0.712604 -0.00710317 -0.0196223 0.701256"
+                  mass="0.643" diaginertia="0.000691311 0.000618011 0.000388977"/>
+                <joint name="right_shoulder_roll_joint" axis="1 0 0" range="-2.2515 1.5882" actuatorfrcrange="-25 25"/>
+                <geom class="visual" mesh="right_shoulder_roll_link"/>
+                <geom size="0.03 0.015" pos="-0.004 -0.006 -0.053" type="cylinder" class="collision"/>
+                <body name="right_shoulder_yaw_link" pos="0 -0.00624 -0.1032">
+                  <inertial pos="0.010773 0.002949 -0.072009" quat="0.687134 -0.0679942 -0.0964829 0.716879"
+                    mass="0.734" diaginertia="0.00106187 0.00103217 0.000400661"/>
+                  <joint name="right_shoulder_yaw_joint" axis="0 0 1" range="-2.618 2.618" actuatorfrcrange="-25 25"/>
+                  <geom class="visual" mesh="right_shoulder_yaw_link"/>
+                  <geom class="collision" mesh="right_shoulder_yaw_link"/>
+                  <body name="right_elbow_link" pos="0.015783 0 -0.080518">
+                    <inertial pos="0.064956 -0.004454 -0.010062" quat="0.388129 0.388821 0.636132 0.541765" mass="0.6"
+                      diaginertia="0.000443035 0.000421612 0.000259353"/>
+                    <joint name="right_elbow_joint" axis="0 1 0" range="-1.0472 2.0944" actuatorfrcrange="-25 25"/>
+                    <geom class="visual" mesh="right_elbow_link"/>
+                    <geom class="collision" mesh="right_elbow_link"/>
+                    <body name="right_wrist_roll_link" pos="0.1 -0.00188791 -0.01">
+                      <inertial pos="0.0171394 -0.000537591 4.8864e-07" quat="0.411667 0.575338 -0.411094 0.574906"
+                        mass="0.085445" diaginertia="5.48211e-05 4.96646e-05 3.57798e-05"/>
+                      <joint name="right_wrist_roll_joint" axis="1 0 0" range="-1.97222 1.97222"
+                        actuatorfrcrange="-25 25"/>
+                      <geom class="visual" mesh="right_wrist_roll_link"/>
+                      <geom class="collision" mesh="right_wrist_roll_link"/>
+                      <body name="right_wrist_pitch_link" pos="0.038 0 0">
+                        <inertial pos="0.0229999 0.00111685 -0.00111658" quat="0.643608 0.293036 0.661363 0.249998"
+                          mass="0.48405" diaginertia="0.000430353 0.000429873 0.000164648"/>
+                        <joint name="right_wrist_pitch_joint" axis="0 1 0" range="-1.61443 1.61443"
+                          actuatorfrcrange="-5 5"/>
+                        <geom class="visual" mesh="right_wrist_pitch_link"/>
+                        <geom class="collision" mesh="right_wrist_pitch_link"/>
+                        <body name="right_wrist_yaw_link" pos="0.046 0 0">
+                          <inertial pos="0.0885506 -0.00212216 -0.000374562" quat="0.505358 0.513241 0.493844 0.487149"
+                            mass="0.457415" diaginertia="0.00105989 0.000895419 0.000323842"/>
+                          <joint name="right_wrist_yaw_joint" axis="0 0 1" range="-1.61443 1.61443"
+                            actuatorfrcrange="-5 5"/>
+                          <geom class="visual" mesh="right_wrist_yaw_link"/>
+                          <geom class="collision" mesh="right_wrist_yaw_link"/>
+                          <geom pos="0.0415 -0.003 0" quat="1 0 0 0" class="visual" mesh="right_hand_palm_link"/>
+                          <geom pos="0.0415 -0.003 0" quat="1 0 0 0" class="collision" mesh="right_hand_palm_link"/>
+                          <body name="right_hand_thumb_0_link" pos="0.067 -0.003 0">
+                            <inertial pos="-0.000884246 0.00863407 0.000944293"
+                              quat="0.643965 0.462991 -0.398986 0.460173" mass="0.0862366"
+                              diaginertia="1.6546e-05 1.60058e-05 1.43741e-05"/>
+                            <joint name="right_hand_thumb_0_joint" axis="0 1 0" range="-1.0472 1.0472"
+                              actuatorfrcrange="-2.45 2.45"/>
+                            <geom class="visual" mesh="right_hand_thumb_0_link"/>
+                            <geom class="collision" mesh="right_hand_thumb_0_link"/>
+                            <body name="right_hand_thumb_1_link" pos="-0.0025 0.0193 0">
+                              <inertial pos="-0.000827888 0.0354744 -0.0003809"
+                                quat="0.705471 0.685598 -0.0956069 0.15207" mass="0.0588507"
+                                diaginertia="1.28514e-05 1.22902e-05 5.9666e-06"/>
+                              <joint name="right_hand_thumb_1_joint" axis="0 0 1" range="-1.0472 0.724312"
+                                actuatorfrcrange="-1.4 1.4"/>
+                              <geom class="visual" mesh="right_hand_thumb_1_link"/>
+                              <geom size="0.01 0.015 0.01" pos="-0.001 0.032 0" type="box" class="collision"/>
+                              <body name="right_hand_thumb_2_link" pos="0 0.0458 0">
+                                <inertial pos="-0.00171735 0.0262819 0.000107789"
+                                  quat="0.710977 0.703174 0.00766553 0.00017564" mass="0.0203063"
+                                  diaginertia="4.61314e-06 3.86645e-06 1.53495e-06"/>
+                                <joint name="right_hand_thumb_2_joint" axis="0 0 1" range="-1.74533 0"
+                                  actuatorfrcrange="-1.4 1.4"/>
+                                <geom class="visual" mesh="right_hand_thumb_2_link"/>
+                                <geom class="collision" mesh="right_hand_thumb_2_link"/>
+                              </body>
+                            </body>
+                          </body>
+                          <body name="right_hand_middle_0_link" pos="0.1192 -0.0046 -0.0285">
+                            <inertial pos="0.0354744 -0.000827888 0.0003809" quat="0.606373 0.417187 0.552395 0.391313"
+                              mass="0.0588507" diaginertia="1.28514e-05 1.22902e-05 5.9666e-06"/>
+                            <joint name="right_hand_middle_0_joint" axis="0 0 1" range="0 1.5708"
+                              actuatorfrcrange="-1.4 1.4"/>
+                            <geom class="visual" mesh="right_hand_middle_0_link"/>
+                            <geom class="collision" mesh="right_hand_middle_0_link"/>
+                            <body name="right_hand_middle_1_link" pos="0.0458 0 0">
+                              <inertial pos="0.0262819 -0.00171735 -0.000107789"
+                                quat="0.502861 0.502639 0.491799 0.502612" mass="0.0203063"
+                                diaginertia="4.61314e-06 3.86645e-06 1.53495e-06"/>
+                              <joint name="right_hand_middle_1_joint" axis="0 0 1" range="0 1.74533"
+                                actuatorfrcrange="-1.4 1.4"/>
+                              <geom class="visual" mesh="right_hand_middle_1_link"/>
+                              <geom class="collision" mesh="right_hand_middle_1_link"/>
+                            </body>
+                          </body>
+                          <body name="right_hand_index_0_link" pos="0.1192 -0.0046 0.0285">
+                            <inertial pos="0.0354744 -0.000827888 0.0003809" quat="0.606373 0.417187 0.552395 0.391313"
+                              mass="0.0588507" diaginertia="1.28514e-05 1.22902e-05 5.9666e-06"/>
+                            <joint name="right_hand_index_0_joint" axis="0 0 1" range="0 1.5708"
+                              actuatorfrcrange="-1.4 1.4"/>
+                            <geom class="visual" mesh="right_hand_index_0_link"/>
+                            <geom class="collision" mesh="right_hand_index_0_link"/>
+                            <body name="right_hand_index_1_link" pos="0.0458 0 0">
+                              <inertial pos="0.0262819 -0.00171735 -0.000107789"
+                                quat="0.502861 0.502639 0.491799 0.502612" mass="0.0203063"
+                                diaginertia="4.61314e-06 3.86645e-06 1.53495e-06"/>
+                              <joint name="right_hand_index_1_joint" axis="0 0 1" range="0 1.74533"
+                                actuatorfrcrange="-1.4 1.4"/>
+                              <geom class="visual" mesh="right_hand_index_1_link"/>
+                              <geom class="collision" mesh="right_hand_index_1_link"/>
+                            </body>
+                          </body>
+                        </body>
+                      </body>
+                    </body>
+                  </body>
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+    </body>
+  </worldbody>
+
+  <actuator>
+    <position class="g1" name="left_hip_pitch_joint" joint="left_hip_pitch_joint"/>
+    <position class="g1" name="left_hip_roll_joint" joint="left_hip_roll_joint"/>
+    <position class="g1" name="left_hip_yaw_joint" joint="left_hip_yaw_joint"/>
+    <position class="g1" name="left_knee_joint" joint="left_knee_joint"/>
+    <position class="g1" name="left_ankle_pitch_joint" joint="left_ankle_pitch_joint"/>
+    <position class="g1" name="left_ankle_roll_joint" joint="left_ankle_roll_joint"/>
+    <position class="g1" name="right_hip_pitch_joint" joint="right_hip_pitch_joint"/>
+    <position class="g1" name="right_hip_roll_joint" joint="right_hip_roll_joint"/>
+    <position class="g1" name="right_hip_yaw_joint" joint="right_hip_yaw_joint"/>
+    <position class="g1" name="right_knee_joint" joint="right_knee_joint"/>
+    <position class="g1" name="right_ankle_pitch_joint" joint="right_ankle_pitch_joint"/>
+    <position class="g1" name="right_ankle_roll_joint" joint="right_ankle_roll_joint"/>
+
+    <position class="g1" name="waist_yaw_joint" joint="waist_yaw_joint"/>
+    <position class="g1" name="waist_roll_joint" joint="waist_roll_joint"/>
+    <position class="g1" name="waist_pitch_joint" joint="waist_pitch_joint"/>
+
+    <position class="g1" name="left_shoulder_pitch_joint" joint="left_shoulder_pitch_joint"/>
+    <position class="g1" name="left_shoulder_roll_joint" joint="left_shoulder_roll_joint"/>
+    <position class="g1" name="left_shoulder_yaw_joint" joint="left_shoulder_yaw_joint"/>
+    <position class="g1" name="left_elbow_joint" joint="left_elbow_joint"/>
+    <position class="g1" name="left_wrist_roll_joint" joint="left_wrist_roll_joint"/>
+    <position class="g1" name="left_wrist_pitch_joint" joint="left_wrist_pitch_joint"/>
+    <position class="g1" name="left_wrist_yaw_joint" joint="left_wrist_yaw_joint"/>
+    <position class="g1" name="left_hand_thumb_0_joint" joint="left_hand_thumb_0_joint"/>
+    <position class="g1" name="left_hand_thumb_1_joint" joint="left_hand_thumb_1_joint"/>
+    <position class="g1" name="left_hand_thumb_2_joint" joint="left_hand_thumb_2_joint"/>
+    <position class="g1" name="left_hand_middle_0_joint" joint="left_hand_middle_0_joint"/>
+    <position class="g1" name="left_hand_middle_1_joint" joint="left_hand_middle_1_joint"/>
+    <position class="g1" name="left_hand_index_0_joint" joint="left_hand_index_0_joint"/>
+    <position class="g1" name="left_hand_index_1_joint" joint="left_hand_index_1_joint"/>
+
+    <position class="g1" name="right_shoulder_pitch_joint" joint="right_shoulder_pitch_joint"/>
+    <position class="g1" name="right_shoulder_roll_joint" joint="right_shoulder_roll_joint"/>
+    <position class="g1" name="right_shoulder_yaw_joint" joint="right_shoulder_yaw_joint"/>
+    <position class="g1" name="right_elbow_joint" joint="right_elbow_joint"/>
+    <position class="g1" name="right_wrist_roll_joint" joint="right_wrist_roll_joint"/>
+    <position class="g1" name="right_wrist_pitch_joint" joint="right_wrist_pitch_joint"/>
+    <position class="g1" name="right_wrist_yaw_joint" joint="right_wrist_yaw_joint"/>
+    <position class="g1" name="right_hand_thumb_0_joint" joint="right_hand_thumb_0_joint"/>
+    <position class="g1" name="right_hand_thumb_1_joint" joint="right_hand_thumb_1_joint"/>
+    <position class="g1" name="right_hand_thumb_2_joint" joint="right_hand_thumb_2_joint"/>
+    <position class="g1" name="right_hand_index_0_joint" joint="right_hand_index_0_joint"/>
+    <position class="g1" name="right_hand_index_1_joint" joint="right_hand_index_1_joint"/>
+    <position class="g1" name="right_hand_middle_0_joint" joint="right_hand_middle_0_joint"/>
+    <position class="g1" name="right_hand_middle_1_joint" joint="right_hand_middle_1_joint"/>
+  </actuator>
+
+  <sensor>
+    <gyro name="imu-torso-angular-velocity" site="imu_in_torso" noise="5e-4" cutoff="34.9"/>
+    <accelerometer name="imu-torso-linear-acceleration" site="imu_in_torso" noise="1e-2" cutoff="157"/>
+    <gyro name="imu-pelvis-angular-velocity" site="imu_in_pelvis" noise="5e-4" cutoff="34.9"/>
+    <accelerometer name="imu-pelvis-linear-acceleration" site="imu_in_pelvis" noise="1e-2" cutoff="157"/>
+  </sensor>
+
+  <keyframe>
+    <key name="stand"
+      qpos="
+      0 0 0.79
+      1 0 0 0
+      0 0 0 0 0 0
+      0 0 0 0 0 0
+      0 0 0
+      0.2 0.2 0 1.28 0 0 0
+      0 1.05 0 0 0 0 0
+      0.2 -0.2 0 1.28 0 0 0
+      0 -1.05 0 0 0 0 0
+      "
+      ctrl="
+      0 0 0 0 0 0
+      0 0 0 0 0 0
+      0 0 0
+      0.2 0.2 0 1.28 0 0 0
+      0 1.05 0 0 0 0 0
+      0.2 -0.2 0 1.28 0 0 0
+      0 -1.05 0 0 0 0 0
+      "/>
+  </keyframe>
+</mujoco>
diff --git a/unitree_g1/scene.xml b/unitree_g1/scene.xml
index de883fb1..549cc442 100644
--- a/unitree_g1/scene.xml
+++ b/unitree_g1/scene.xml
@@ -1,7 +1,7 @@
-<mujoco model="g1 scene">
+<mujoco model="g1_29dof_rev_1_0 scene">
   <include file="g1.xml"/>
 
-  <statistic center="1 -0.8 1.1" extent=".3"/>
+  <statistic center="1 -0.8 1.1" extent=".35"/>
 
   <visual>
     <headlight diffuse="0.6 0.6 0.6" ambient="0.1 0.1 0.1" specular="0.9 0.9 0.9"/>
diff --git a/unitree_g1/scene_with_hands.xml b/unitree_g1/scene_with_hands.xml
new file mode 100644
index 00000000..294f5d07
--- /dev/null
+++ b/unitree_g1/scene_with_hands.xml
@@ -0,0 +1,22 @@
+<mujoco model="g1_29dof_with_hand_rev_1_0 scene">
+  <include file="g1_with_hands.xml"/>
+
+  <statistic center="1 -0.8 1.1" extent=".35"/>
+
+  <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="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072"/>
+    <texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3"
+      markrgb="0.8 0.8 0.8" width="300" height="300"/>
+    <material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2"/>
+  </asset>
+
+  <worldbody>
+    <geom name="floor" size="0 0 0.05" type="plane" material="groundplane"/>
+  </worldbody>
+</mujoco>