diff --git a/judo/models/meshes/g1/head_link.STL b/judo/models/meshes/g1/head_link.STL
new file mode 100644
index 00000000..2ee5fba1
Binary files /dev/null and b/judo/models/meshes/g1/head_link.STL differ
diff --git a/judo/models/meshes/g1/left_ankle_pitch_link.STL b/judo/models/meshes/g1/left_ankle_pitch_link.STL
new file mode 100644
index 00000000..69de8490
Binary files /dev/null and b/judo/models/meshes/g1/left_ankle_pitch_link.STL differ
diff --git a/judo/models/meshes/g1/left_ankle_roll_link.STL b/judo/models/meshes/g1/left_ankle_roll_link.STL
new file mode 100644
index 00000000..8864e9f9
Binary files /dev/null and b/judo/models/meshes/g1/left_ankle_roll_link.STL differ
diff --git a/judo/models/meshes/g1/left_elbow_link.STL b/judo/models/meshes/g1/left_elbow_link.STL
new file mode 100644
index 00000000..1a96d99b
Binary files /dev/null and b/judo/models/meshes/g1/left_elbow_link.STL differ
diff --git a/judo/models/meshes/g1/left_hand_index_0_link.STL b/judo/models/meshes/g1/left_hand_index_0_link.STL
new file mode 100644
index 00000000..8069369a
Binary files /dev/null and b/judo/models/meshes/g1/left_hand_index_0_link.STL differ
diff --git a/judo/models/meshes/g1/left_hand_index_1_link.STL b/judo/models/meshes/g1/left_hand_index_1_link.STL
new file mode 100644
index 00000000..89d231d7
Binary files /dev/null and b/judo/models/meshes/g1/left_hand_index_1_link.STL differ
diff --git a/judo/models/meshes/g1/left_hand_middle_0_link.STL b/judo/models/meshes/g1/left_hand_middle_0_link.STL
new file mode 100644
index 00000000..8069369a
Binary files /dev/null and b/judo/models/meshes/g1/left_hand_middle_0_link.STL differ
diff --git a/judo/models/meshes/g1/left_hand_middle_1_link.STL b/judo/models/meshes/g1/left_hand_middle_1_link.STL
new file mode 100644
index 00000000..89d231d7
Binary files /dev/null and b/judo/models/meshes/g1/left_hand_middle_1_link.STL differ
diff --git a/judo/models/meshes/g1/left_hand_palm_link.STL b/judo/models/meshes/g1/left_hand_palm_link.STL
new file mode 100644
index 00000000..7d595ed8
Binary files /dev/null and b/judo/models/meshes/g1/left_hand_palm_link.STL differ
diff --git a/judo/models/meshes/g1/left_hand_thumb_0_link.STL b/judo/models/meshes/g1/left_hand_thumb_0_link.STL
new file mode 100644
index 00000000..3028bb4d
Binary files /dev/null and b/judo/models/meshes/g1/left_hand_thumb_0_link.STL differ
diff --git a/judo/models/meshes/g1/left_hand_thumb_1_link.STL b/judo/models/meshes/g1/left_hand_thumb_1_link.STL
new file mode 100644
index 00000000..d1c080c8
Binary files /dev/null and b/judo/models/meshes/g1/left_hand_thumb_1_link.STL differ
diff --git a/judo/models/meshes/g1/left_hand_thumb_2_link.STL b/judo/models/meshes/g1/left_hand_thumb_2_link.STL
new file mode 100644
index 00000000..8b32e966
Binary files /dev/null and b/judo/models/meshes/g1/left_hand_thumb_2_link.STL differ
diff --git a/judo/models/meshes/g1/left_hip_pitch_link.STL b/judo/models/meshes/g1/left_hip_pitch_link.STL
new file mode 100644
index 00000000..5b751c76
Binary files /dev/null and b/judo/models/meshes/g1/left_hip_pitch_link.STL differ
diff --git a/judo/models/meshes/g1/left_hip_roll_link.STL b/judo/models/meshes/g1/left_hip_roll_link.STL
new file mode 100644
index 00000000..778437ff
Binary files /dev/null and b/judo/models/meshes/g1/left_hip_roll_link.STL differ
diff --git a/judo/models/meshes/g1/left_hip_yaw_link.STL b/judo/models/meshes/g1/left_hip_yaw_link.STL
new file mode 100644
index 00000000..383093ab
Binary files /dev/null and b/judo/models/meshes/g1/left_hip_yaw_link.STL differ
diff --git a/judo/models/meshes/g1/left_knee_link.STL b/judo/models/meshes/g1/left_knee_link.STL
new file mode 100644
index 00000000..f2e98e54
Binary files /dev/null and b/judo/models/meshes/g1/left_knee_link.STL differ
diff --git a/judo/models/meshes/g1/left_rubber_hand.STL b/judo/models/meshes/g1/left_rubber_hand.STL
new file mode 100644
index 00000000..c44830f1
Binary files /dev/null and b/judo/models/meshes/g1/left_rubber_hand.STL differ
diff --git a/judo/models/meshes/g1/left_shoulder_pitch_link.STL b/judo/models/meshes/g1/left_shoulder_pitch_link.STL
new file mode 100644
index 00000000..e698311f
Binary files /dev/null and b/judo/models/meshes/g1/left_shoulder_pitch_link.STL differ
diff --git a/judo/models/meshes/g1/left_shoulder_roll_link.STL b/judo/models/meshes/g1/left_shoulder_roll_link.STL
new file mode 100644
index 00000000..80bca84a
Binary files /dev/null and b/judo/models/meshes/g1/left_shoulder_roll_link.STL differ
diff --git a/judo/models/meshes/g1/left_shoulder_yaw_link.STL b/judo/models/meshes/g1/left_shoulder_yaw_link.STL
new file mode 100644
index 00000000..281e6990
Binary files /dev/null and b/judo/models/meshes/g1/left_shoulder_yaw_link.STL differ
diff --git a/judo/models/meshes/g1/left_wrist_pitch_link.STL b/judo/models/meshes/g1/left_wrist_pitch_link.STL
new file mode 100644
index 00000000..82cc224a
Binary files /dev/null and b/judo/models/meshes/g1/left_wrist_pitch_link.STL differ
diff --git a/judo/models/meshes/g1/left_wrist_roll_link.STL b/judo/models/meshes/g1/left_wrist_roll_link.STL
new file mode 100644
index 00000000..f3c263a7
Binary files /dev/null and b/judo/models/meshes/g1/left_wrist_roll_link.STL differ
diff --git a/judo/models/meshes/g1/left_wrist_yaw_link.STL b/judo/models/meshes/g1/left_wrist_yaw_link.STL
new file mode 100644
index 00000000..31be4fd4
Binary files /dev/null and b/judo/models/meshes/g1/left_wrist_yaw_link.STL differ
diff --git a/judo/models/meshes/g1/logo_link.STL b/judo/models/meshes/g1/logo_link.STL
new file mode 100644
index 00000000..e9792098
Binary files /dev/null and b/judo/models/meshes/g1/logo_link.STL differ
diff --git a/judo/models/meshes/g1/pelvis.STL b/judo/models/meshes/g1/pelvis.STL
new file mode 100644
index 00000000..691a779b
Binary files /dev/null and b/judo/models/meshes/g1/pelvis.STL differ
diff --git a/judo/models/meshes/g1/pelvis_contour_link.STL b/judo/models/meshes/g1/pelvis_contour_link.STL
new file mode 100644
index 00000000..42434339
Binary files /dev/null and b/judo/models/meshes/g1/pelvis_contour_link.STL differ
diff --git a/judo/models/meshes/g1/right_ankle_pitch_link.STL b/judo/models/meshes/g1/right_ankle_pitch_link.STL
new file mode 100644
index 00000000..e77d8a2f
Binary files /dev/null and b/judo/models/meshes/g1/right_ankle_pitch_link.STL differ
diff --git a/judo/models/meshes/g1/right_ankle_roll_link.STL b/judo/models/meshes/g1/right_ankle_roll_link.STL
new file mode 100644
index 00000000..d4261dd7
Binary files /dev/null and b/judo/models/meshes/g1/right_ankle_roll_link.STL differ
diff --git a/judo/models/meshes/g1/right_elbow_link.STL b/judo/models/meshes/g1/right_elbow_link.STL
new file mode 100644
index 00000000..f259e381
Binary files /dev/null and b/judo/models/meshes/g1/right_elbow_link.STL differ
diff --git a/judo/models/meshes/g1/right_hand_index_0_link.STL b/judo/models/meshes/g1/right_hand_index_0_link.STL
new file mode 100644
index 00000000..f87ad321
Binary files /dev/null and b/judo/models/meshes/g1/right_hand_index_0_link.STL differ
diff --git a/judo/models/meshes/g1/right_hand_index_1_link.STL b/judo/models/meshes/g1/right_hand_index_1_link.STL
new file mode 100644
index 00000000..6dea51ad
Binary files /dev/null and b/judo/models/meshes/g1/right_hand_index_1_link.STL differ
diff --git a/judo/models/meshes/g1/right_hand_middle_0_link.STL b/judo/models/meshes/g1/right_hand_middle_0_link.STL
new file mode 100644
index 00000000..f87ad321
Binary files /dev/null and b/judo/models/meshes/g1/right_hand_middle_0_link.STL differ
diff --git a/judo/models/meshes/g1/right_hand_middle_1_link.STL b/judo/models/meshes/g1/right_hand_middle_1_link.STL
new file mode 100644
index 00000000..6dea51ad
Binary files /dev/null and b/judo/models/meshes/g1/right_hand_middle_1_link.STL differ
diff --git a/judo/models/meshes/g1/right_hand_palm_link.STL b/judo/models/meshes/g1/right_hand_palm_link.STL
new file mode 100644
index 00000000..5ae00a78
Binary files /dev/null and b/judo/models/meshes/g1/right_hand_palm_link.STL differ
diff --git a/judo/models/meshes/g1/right_hand_thumb_0_link.STL b/judo/models/meshes/g1/right_hand_thumb_0_link.STL
new file mode 100644
index 00000000..1cae7f18
Binary files /dev/null and b/judo/models/meshes/g1/right_hand_thumb_0_link.STL differ
diff --git a/judo/models/meshes/g1/right_hand_thumb_1_link.STL b/judo/models/meshes/g1/right_hand_thumb_1_link.STL
new file mode 100644
index 00000000..c141fbf5
Binary files /dev/null and b/judo/models/meshes/g1/right_hand_thumb_1_link.STL differ
diff --git a/judo/models/meshes/g1/right_hand_thumb_2_link.STL b/judo/models/meshes/g1/right_hand_thumb_2_link.STL
new file mode 100644
index 00000000..e942923c
Binary files /dev/null and b/judo/models/meshes/g1/right_hand_thumb_2_link.STL differ
diff --git a/judo/models/meshes/g1/right_hip_pitch_link.STL b/judo/models/meshes/g1/right_hip_pitch_link.STL
new file mode 100644
index 00000000..998a0a0f
Binary files /dev/null and b/judo/models/meshes/g1/right_hip_pitch_link.STL differ
diff --git a/judo/models/meshes/g1/right_hip_roll_link.STL b/judo/models/meshes/g1/right_hip_roll_link.STL
new file mode 100644
index 00000000..47b2eebd
Binary files /dev/null and b/judo/models/meshes/g1/right_hip_roll_link.STL differ
diff --git a/judo/models/meshes/g1/right_hip_yaw_link.STL b/judo/models/meshes/g1/right_hip_yaw_link.STL
new file mode 100644
index 00000000..37185642
Binary files /dev/null and b/judo/models/meshes/g1/right_hip_yaw_link.STL differ
diff --git a/judo/models/meshes/g1/right_knee_link.STL b/judo/models/meshes/g1/right_knee_link.STL
new file mode 100644
index 00000000..76d21a3d
Binary files /dev/null and b/judo/models/meshes/g1/right_knee_link.STL differ
diff --git a/judo/models/meshes/g1/right_rubber_hand.STL b/judo/models/meshes/g1/right_rubber_hand.STL
new file mode 100644
index 00000000..0aacffbb
Binary files /dev/null and b/judo/models/meshes/g1/right_rubber_hand.STL differ
diff --git a/judo/models/meshes/g1/right_shoulder_pitch_link.STL b/judo/models/meshes/g1/right_shoulder_pitch_link.STL
new file mode 100644
index 00000000..3f5b4ed4
Binary files /dev/null and b/judo/models/meshes/g1/right_shoulder_pitch_link.STL differ
diff --git a/judo/models/meshes/g1/right_shoulder_roll_link.STL b/judo/models/meshes/g1/right_shoulder_roll_link.STL
new file mode 100644
index 00000000..179d6175
Binary files /dev/null and b/judo/models/meshes/g1/right_shoulder_roll_link.STL differ
diff --git a/judo/models/meshes/g1/right_shoulder_yaw_link.STL b/judo/models/meshes/g1/right_shoulder_yaw_link.STL
new file mode 100644
index 00000000..2ba6076a
Binary files /dev/null and b/judo/models/meshes/g1/right_shoulder_yaw_link.STL differ
diff --git a/judo/models/meshes/g1/right_wrist_pitch_link.STL b/judo/models/meshes/g1/right_wrist_pitch_link.STL
new file mode 100644
index 00000000..da194543
Binary files /dev/null and b/judo/models/meshes/g1/right_wrist_pitch_link.STL differ
diff --git a/judo/models/meshes/g1/right_wrist_roll_link.STL b/judo/models/meshes/g1/right_wrist_roll_link.STL
new file mode 100644
index 00000000..26868d22
Binary files /dev/null and b/judo/models/meshes/g1/right_wrist_roll_link.STL differ
diff --git a/judo/models/meshes/g1/right_wrist_yaw_link.STL b/judo/models/meshes/g1/right_wrist_yaw_link.STL
new file mode 100644
index 00000000..d7889028
Binary files /dev/null and b/judo/models/meshes/g1/right_wrist_yaw_link.STL differ
diff --git a/judo/models/meshes/g1/torso_link_rev_1_0.STL b/judo/models/meshes/g1/torso_link_rev_1_0.STL
new file mode 100644
index 00000000..8a759a70
Binary files /dev/null and b/judo/models/meshes/g1/torso_link_rev_1_0.STL differ
diff --git a/judo/models/meshes/g1/waist_roll_link_rev_1_0.STL b/judo/models/meshes/g1/waist_roll_link_rev_1_0.STL
new file mode 100644
index 00000000..a64f330c
Binary files /dev/null and b/judo/models/meshes/g1/waist_roll_link_rev_1_0.STL differ
diff --git a/judo/models/meshes/g1/waist_yaw_link_rev_1_0.STL b/judo/models/meshes/g1/waist_yaw_link_rev_1_0.STL
new file mode 100644
index 00000000..0fabb63d
Binary files /dev/null and b/judo/models/meshes/g1/waist_yaw_link_rev_1_0.STL differ
diff --git a/judo/models/xml/g1_manipulation.xml b/judo/models/xml/g1_manipulation.xml
new file mode 100644
index 00000000..aff9b8cd
--- /dev/null
+++ b/judo/models/xml/g1_manipulation.xml
@@ -0,0 +1,494 @@
+<mujoco model="g1_29dof_with_hand_rev_1_0">
+  <compiler angle="radian" assetdir="../meshes/g1/" />
+  <option timestep="0.005" integrator="implicitfast" cone="elliptic" impratio="100"/>
+
+  <default>
+    <default class="g1">
+      <site rgba="1 0 0 1" size="0.01" group="5"/>
+      <joint armature="0.01" frictionloss="0.3"/>
+      <position kp="35" 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"/>
+      <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="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>
+    <!-- object to be manipulated-->
+    <body name="box" pos="0.4 0 1" quat="1 0 0 0">
+      <freejoint name="box_joint"/>
+      <geom name="box" type="box" size=".1 .1 .1" mass="1.0" class="collision" />
+      <!-- face 1 -->
+      <geom name="cube_visual_face_1_1" pos="0.1 0.0 0.0" size="0.015" class="visual" type="sphere"/>
+
+      <!-- face 2 -->
+      <geom name="cube_visual_face_2_1" pos="-0.1 -0.03 0.03" size="0.015" class="visual" type="sphere"/>
+      <geom name="cube_visual_face_2_2" pos="-0.1 0.03 -0.03" size="0.015" class="visual" type="sphere"/>
+
+      <!-- face 3 -->
+      <geom name="cube_visual_face_3_1" pos="-0.03 0.1 0.03" size="0.015" class="visual" type="sphere"/>
+      <geom name="cube_visual_face_3_2" pos="0.0 0.1 0.0" size="0.015" class="visual" type="sphere"/>
+      <geom name="cube_visual_face_3_3" pos="0.03 0.1 -0.03" size="0.015" class="visual" type="sphere"/>
+
+      <!-- face 4 -->
+      <geom name="cube_visual_face_4_1" pos="-0.03 -0.1  0.03" size="0.015" class="visual" type="sphere"/>
+      <geom name="cube_visual_face_4_2" pos="-0.03 -0.1  -0.03" size="0.015" class="visual" type="sphere"/>
+      <geom name="cube_visual_face_4_3" pos="0.03 -0.1 0.03" size="0.015" class="visual" type="sphere"/>
+      <geom name="cube_visual_face_4_4" pos="0.03 -0.1 -0.03" size="0.015" class="visual" type="sphere"/>
+
+      <!-- face 5 -->
+      <geom name="cube_visual_face_5_1" pos="-0.03 0.03 0.1" size="0.015" class="visual" type="sphere"/>
+      <geom name="cube_visual_face_5_2" pos="-0.03 -0.03 0.1" size="0.015" class="visual" type="sphere"/>
+      <geom name="cube_visual_face_5_3" pos="0.03 0.03 0.1" size="0.015" class="visual" type="sphere"/>
+      <geom name="cube_visual_face_5_4" pos="0.03 -0.03 0.1" size="0.015" class="visual" type="sphere"/>
+      <geom name="cube_visual_face_5_5" pos="0.0 0.0 0.1" size="0.015" class="visual" type="sphere"/>
+
+      <!-- face 6 -->
+      <geom name="cube_visual_face_6_1" pos="0.03 0.05 -0.1" size="0.015" class="visual" type="sphere"/>
+      <geom name="cube_visual_face_6_2" pos="0.03 -0.05 -0.1" size="0.015" class="visual" type="sphere"/>
+      <geom name="cube_visual_face_6_3" pos="0.0 0.05 -0.1" size="0.015" class="visual" type="sphere"/>
+      <geom name="cube_visual_face_6_4" pos="0.0 -0.05 -0.1" size="0.015" class="visual" type="sphere"/>
+      <geom name="cube_visual_face_6_5" pos="-0.03 0.05 -0.1" size="0.015" class="visual" type="sphere"/>
+      <geom name="cube_visual_face_6_6" pos="-0.03 -0.05 -0.1" size="0.015" class="visual" type="sphere"/>
+    </body>
+
+    <!-- mocap body for goal visualization purposes -->
+    <body name="goal_vis" pos="1 0 1" quat="1 0 0 0" mocap="true">
+      <geom name="goal_vis" type="box" size=".1 .1 .1" mass="0.1" class="visual"/>
+      <!-- face 1 -->
+      <geom name="cube_vis_visual_face_1_1" pos="0.1 0.0 0.0" size="0.015" class="visual" type="sphere"/>
+
+      <!-- face 2 -->
+      <geom name="cube_vis_visual_face_2_1" pos="-0.1 -0.03 0.03" size="0.015" class="visual" type="sphere"/>
+      <geom name="cube_vis_visual_face_2_2" pos="-0.1 0.03 -0.03" size="0.015" class="visual" type="sphere"/>
+
+      <!-- face 3 -->
+      <geom name="cube_vis_visual_face_3_1" pos="-0.03 0.1 0.03" size="0.015" class="visual" type="sphere"/>
+      <geom name="cube_vis_visual_face_3_2" pos="0.0 0.1 0.0" size="0.015" class="visual" type="sphere"/>
+      <geom name="cube_vis_visual_face_3_3" pos="0.03 0.1 -0.03" size="0.015" class="visual" type="sphere"/>
+
+      <!-- face 4 -->
+      <geom name="cube_vis_visual_face_4_1" pos="-0.03 -0.1  0.03" size="0.015" class="visual" type="sphere"/>
+      <geom name="cube_vis_visual_face_4_2" pos="-0.03 -0.1  -0.03" size="0.015" class="visual" type="sphere"/>
+      <geom name="cube_vis_visual_face_4_3" pos="0.03 -0.1 0.03" size="0.015" class="visual" type="sphere"/>
+      <geom name="cube_vis_visual_face_4_4" pos="0.03 -0.1 -0.03" size="0.015" class="visual" type="sphere"/>
+
+      <!-- face 5 -->
+      <geom name="cube_vis_visual_face_5_1" pos="-0.03 0.03 0.1" size="0.015" class="visual" type="sphere"/>
+      <geom name="cube_vis_visual_face_5_2" pos="-0.03 -0.03 0.1" size="0.015" class="visual" type="sphere"/>
+      <geom name="cube_vis_visual_face_5_3" pos="0.03 0.03 0.1" size="0.015" class="visual" type="sphere"/>
+      <geom name="cube_vis_visual_face_5_4" pos="0.03 -0.03 0.1" size="0.015" class="visual" type="sphere"/>
+      <geom name="cube_vis_visual_face_5_5" pos="0.0 0.0 0.1" size="0.015" class="visual" type="sphere"/>
+
+      <!-- face 6 -->
+      <geom name="cube_vis_visual_face_6_1" pos="0.03 0.05 -0.1" size="0.015" class="visual" type="sphere"/>
+      <geom name="cube_vis_visual_face_6_2" pos="0.03 -0.05 -0.1" size="0.015" class="visual" type="sphere"/>
+      <geom name="cube_vis_visual_face_6_3" pos="0.0 0.05 -0.1" size="0.015" class="visual" type="sphere"/>
+      <geom name="cube_vis_visual_face_6_4" pos="0.0 -0.05 -0.1" size="0.015" class="visual" type="sphere"/>
+      <geom name="cube_vis_visual_face_6_5" pos="-0.03 0.05 -0.1" size="0.015" class="visual" type="sphere"/>
+      <geom name="cube_vis_visual_face_6_6" pos="-0.03 -0.05 -0.1" size="0.015" class="visual" type="sphere"/>
+    </body>
+
+    <!-- environment -->
+    <body name="table">
+      <geom name="table" type="box" pos="0.6 0 0.7" size="0.5 0.5 0.025" mass="30" class="collision" />
+    </body>
+    <body name="ground">
+      <geom name="ground" type="plane" size="2 2 0.01" friction="0.3" priority="5"/>
+    </body>
+
+  </worldbody>
+
+  <actuator>
+    <position class="g1" name="waist_yaw_joint" joint="waist_yaw_joint" kp="150"/>
+    <position class="g1" name="waist_roll_joint" joint="waist_roll_joint" kp="150"/>
+    <position class="g1" name="waist_pitch_joint" joint="waist_pitch_joint" kp="150"/>
+
+    <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>
+    <framepos name="trace_box" objtype="body" objname="box"/>
+    <framepos name="trace_left_hand" objtype="body" objname="left_wrist_yaw_link"/>
+    <framepos name="trace_right_hand" objtype="body" objname="right_wrist_yaw_link"/>
+
+    <framepos name="right_hand_thumb" objtype="body" objname="right_hand_thumb_2_link"/>
+    <framepos name="right_hand_index" objtype="body" objname="right_hand_index_1_link"/>
+    <framepos name="right_hand_middle" objtype="body" objname="right_hand_middle_1_link"/>
+
+    <framepos name="left_hand_thumb" objtype="body" objname="left_hand_thumb_2_link"/>
+    <framepos name="left_hand_index" objtype="body" objname="left_hand_index_1_link"/>
+    <framepos name="left_hand_middle" objtype="body" objname="left_hand_middle_1_link"/>
+  </sensor>
+
+  <keyframe>
+    <key name="stand"
+      qpos="
+      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
+      0.4 0 0.8
+      1 0 0 0
+      "
+      ctrl="
+      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/judo/tasks/__init__.py b/judo/tasks/__init__.py
index 0a7537f7..fc65c193 100644
--- a/judo/tasks/__init__.py
+++ b/judo/tasks/__init__.py
@@ -7,6 +7,7 @@
 from judo.tasks.cartpole import Cartpole, CartpoleConfig
 from judo.tasks.cylinder_push import CylinderPush, CylinderPushConfig
 from judo.tasks.fr3_pick import FR3Pick, FR3PickConfig
+from judo.tasks.g1_manipulation import G1Manipulation, G1ManipulationConfig
 from judo.tasks.leap_cube import LeapCube, LeapCubeConfig
 from judo.tasks.leap_cube_down import LeapCubeDown, LeapCubeDownConfig
 
@@ -14,6 +15,7 @@
     "cylinder_push": (CylinderPush, CylinderPushConfig),
     "cartpole": (Cartpole, CartpoleConfig),
     "fr3_pick": (FR3Pick, FR3PickConfig),
+    "g1_manipulation": (G1Manipulation, G1ManipulationConfig),
     "leap_cube": (LeapCube, LeapCubeConfig),
     "leap_cube_down": (LeapCubeDown, LeapCubeDownConfig),
     "caltech_leap_cube": (CaltechLeapCube, CaltechLeapCubeConfig),
@@ -43,6 +45,8 @@ def register_task(name: str, task_type: Type[Task], task_config_type: Type[TaskC
     "CylinderPushConfig",
     "FR3Pick",
     "FR3PickConfig",
+    "G1Manipulation",
+    "G1ManipulationConfig",
     "LeapCube",
     "LeapCubeConfig",
     "LeapCubeDown",
diff --git a/judo/tasks/g1_manipulation.py b/judo/tasks/g1_manipulation.py
new file mode 100644
index 00000000..4bdd0b7a
--- /dev/null
+++ b/judo/tasks/g1_manipulation.py
@@ -0,0 +1,177 @@
+# Copyright (c) 2025 Robotics and AI Institute LLC. All rights reserved.
+
+from dataclasses import dataclass
+from typing import Any
+
+import mujoco
+import numpy as np
+
+from judo import MODEL_PATH
+from judo.gui import slider
+from judo.tasks.base import Task, TaskConfig
+from judo.utils.math_utils import quat_diff, quat_diff_so3
+
+XML_PATH = str(MODEL_PATH / "xml/g1_manipulation.xml")
+QPOS_HOME = np.array(
+    [
+      0, 0, 0,
+      0.2, 0.2, 0, 0, 0, 0, 0,
+      0, 1.05, 0, 0, 0, 0, 0,
+      0.2, -0.2, 0, 0, 0, 0, 0,
+      0, -1.05, 0, 0, 0, 0, 0,
+      0.4, 0, 1.0,
+      1, 0, 0, 0
+    ]
+)  # fmt: skip
+
+
+@slider("w_object_pos", 0.0, 10.0, 1.00)
+@slider("w_object_rot", 0.0, 10.0, 0.0)
+@slider("w_hand_proximity", 0.0, 10.0, 0.1)
+@dataclass
+class G1ManipulationConfig(TaskConfig):
+    """Reward configuration G1 task."""
+
+    cutoff_time: float = 0.25
+    w_object_pos: float = 0.05
+    w_object_rot: float = 0.2
+    w_hand_proximity: float = 0.1
+
+    dropped_threshold: float = 0.7
+    too_far_threshold: float = 0.6
+    angle_threshold: float = 0.2  # radians
+
+
+class G1Manipulation(Task[G1ManipulationConfig]):
+    """Defines the G1 manipulation task."""
+
+    def __init__(self, model_path: str = XML_PATH, sim_model_path: str | None = None) -> None:
+        """Initializes the G1 manipulation task."""
+        super().__init__(model_path, sim_model_path=sim_model_path)
+
+        # object indices
+        self.object_pose_adr = self.get_joint_position_start_index("box_joint")
+        self.object_pos_slice = slice(self.object_pose_adr + 0, self.object_pose_adr + 3)
+        self.object_quat_slice = slice(self.object_pose_adr + 3, self.object_pose_adr + 7)
+
+        # robot indices
+        self.joint_pos_idx = np.arange(31)
+        self.finger_names = [
+            "left_hand_thumb",
+            "left_hand_index",
+            "left_hand_middle",
+            "right_hand_thumb",
+            "right_hand_index",
+            "right_hand_middle",
+        ]
+        self.num_fingers = len(self.finger_names)
+
+        # sensors
+        self.finger_sensor_indices = [self.get_sensor_start_index(finger_name) for finger_name in self.finger_names]
+
+        # dynamic goal pose
+        self.system_metadata: dict[str, Any] = {
+            "goal_pos": np.array([0.5, 0.0, 0.85]),
+            "goal_quat": np.array([1.0, 0.0, 0.0, 0.0]),
+        }
+
+        self.reset()
+
+    def pre_rollout(self, curr_state: np.ndarray, config: G1ManipulationConfig) -> None:
+        """Check for dropped objects or goal reached."""
+        has_dropped = self.data.qpos[self.object_pose_adr + 2] < config.dropped_threshold
+        is_too_far = (
+            np.linalg.norm(self.data.qpos[self.object_pos_slice][:2] - self.system_metadata["goal_pos"][:2])
+            > config.too_far_threshold
+        )
+
+        # we reset here if the cube has dropped
+        if has_dropped or is_too_far:
+            self.reset()
+
+        # check whether goal quat needs to be updated
+        goal_quat = self.system_metadata["goal_quat"]
+        q_diff = quat_diff(self.data.qpos[self.object_quat_slice], goal_quat)
+        sin_a_2 = np.linalg.norm(q_diff[1:])
+        angle = 2 * np.arctan2(sin_a_2, q_diff[0])
+        if angle > np.pi:
+            angle -= 2 * np.pi
+        at_goal = np.abs(angle) < config.angle_threshold
+        if at_goal:
+            self._update_goal_quat()
+
+    def reward(
+        self,
+        states: np.ndarray,
+        sensors: np.ndarray,
+        controls: np.ndarray,
+        config: G1ManipulationConfig,
+        system_metadata: dict[str, Any] | None = None,
+    ) -> np.ndarray:
+        """Implements the G1 manipulation task reward."""
+        if system_metadata is None:
+            system_metadata = {}
+        goal_pos = system_metadata.get("goal_pos", np.array([0.5, 0.0, 0.85]))
+        goal_quat = system_metadata.get("goal_quat", np.array([1.0, 0.0, 0.0, 0.0]))
+
+        # weights
+        w_object_pos = config.w_object_pos
+        w_object_rot = config.w_object_rot
+        w_hand_proximity = config.w_hand_proximity
+        # "standard" tracking task
+        object_pos_traj = states[..., self.object_pos_slice]
+        object_quat_traj = states[..., self.object_quat_slice]
+
+        object_pos_diff = object_pos_traj[..., 0:2] - goal_pos[0:2]  # do not penalize z-axis
+        object_quat_diff = quat_diff_so3(object_quat_traj, goal_quat)
+
+        object_pos_cost = w_object_pos * 0.5 * np.square(object_pos_diff).sum(-1).mean(-1)
+        object_quat_cost = w_object_rot * 0.5 * np.square(object_quat_diff).sum(-1).mean(-1)
+        rewards = -(object_pos_cost + object_quat_cost)
+
+        hand_proximity_cost = np.zeros((states.shape[0],))
+        for finger_idx in self.finger_sensor_indices:
+            hand_proximity_diff = object_pos_traj - sensors[..., finger_idx : finger_idx + 3]
+            hand_proximity_cost += (
+                w_hand_proximity * 0.5 * np.square(hand_proximity_diff).sum(-1).mean(-1) / self.num_fingers
+            )
+
+        rewards = -hand_proximity_cost - object_pos_cost - object_quat_cost
+        return rewards
+
+    def reset(self) -> None:
+        """Resets the model to a default state with random goal."""
+        self.data.qpos[:] = QPOS_HOME
+        self.data.qvel[:] = 0.0
+        self.data.ctrl[:] = self.reset_command
+        self._update_goal_quat()
+        mujoco.mj_forward(self.model, self.data)
+
+    @property
+    def reset_command(self) -> np.ndarray:
+        """Returns the reset command for the robot."""
+        return self.data.qpos[self.joint_pos_idx]
+
+    def _update_goal_quat(self) -> None:
+        """Updates the goal quaternion."""
+        # generate uniformly random quaternion
+        # https://stackoverflow.com/a/44031492
+        uvw = np.random.rand(3)
+        goal_quat = np.array(
+            [
+                np.sqrt(1 - uvw[0]) * np.sin(2 * np.pi * uvw[1]),
+                np.sqrt(1 - uvw[0]) * np.cos(2 * np.pi * uvw[1]),
+                np.sqrt(uvw[0]) * np.sin(2 * np.pi * uvw[2]),
+                np.sqrt(uvw[0]) * np.cos(2 * np.pi * uvw[2]),
+            ]
+        )
+        self.data.mocap_quat[0] = goal_quat
+        self.system_metadata["goal_quat"] = goal_quat
+
+    @property
+    def actuator_ctrlrange(self) -> np.ndarray:
+        """Mujoco actuator limits for this task."""
+        limits = self.model.actuator_ctrlrange.copy()
+        limits[0:3, 0] = -0.001
+        limits[0:3, 1] = +0.001
+        return limits  # type: ignore