Unify humanoid examples.

kevinzakka · Mar 5, 2025 · 4c97a6e · 4c97a6e
1 parent 97aeb94
commit 4c97a6e
Show file tree

Hide file tree

Showing 8 changed files with 147 additions and 161 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -9,10 +9,9 @@ All notable changes to this project will be documented in this file.
 ### Added
 
 - `Configuration.check_limits` now logs joint limit violations to `debug` rather than `warn` when `safety_break=False`.
-- Added a new humanoid example using the Apollo robot.
 - Added `utils.get_subtree_body_ids` to get all bodies belonging to the subtree starting at a given body.
   - Example usage of this function can be found in the [ALOHA example script](examples/arm_aloha.py) where it is used to selectively apply gravity compensation torques to the left and right arm bodies.
-- Add new G1 example with a tabletop manipulation focus.
+- Add G1 and Apollo humanoid example with a tabletop manipulation focus.
 
 ![Banner for mink](https://github.com/kevinzakka/mink/blob/assets/g1_teleop.gif?raw=true)
 

diff --git a/examples/apptronik_apollo/apollo.xml b/examples/apptronik_apollo/apollo.xml
@@ -1,5 +1,5 @@
 <mujoco model="apptronik_apollo">
-  <compiler angle="radian" meshdir="assets/" texturedir="assets/"/>
+  <compiler angle="radian" meshdir="assets/"/>
 
   <option timestep="0.005" integrator="implicitfast"/>
 
@@ -25,7 +25,7 @@
       </default>
     </default>
     <default class="collision">
-      <geom group="3" rgba="0 1 0 0.4"/>
+      <geom group="3" rgba="0 1 0 0.4" contype="1" conaffinity="1"/>
     </default>
   </default>
 
@@ -168,7 +168,8 @@
                           <geom name="pinky_mesh_2" class="visual_dark" pos="-0.0330569 -0.0546993 -0.167156" quat="-0.0657942 -0.683567 0.00142601 0.726915" mesh="idx-F2"/>
                           <geom name="thumb_mesh_1" class="visual_dark" pos="0.0240477 -0.0463856 -0.0695448" quat="-0.986553 0.125649 -0.0560269 -0.0882391" mesh="thumb-F1"/>
                           <geom name="thumb_mesh_2" class="visual_dark" pos="0.0499644 -0.0457352 -0.0874305" quat="0.500891 -0.859176 -0.0358978 0.0981657" mesh="thumb-F2-left"/>
-                          <geom name="collision_l_hand_plate" class="collision" size="0.041 0.02 0.083" pos="0.00749988 -0.025 -0.112" type="box"/>
+                          <!-- <geom name="collision_l_hand_plate" class="collision" size="0.041 0.02 0.083" pos="0.00749988 -0.025 -0.112" type="box"/> -->
+                          <geom name="collision_l_hand_plate" class="collision" size="0.055 0.083" pos="0.00749988 -0.04 -0.112" type="capsule"/>
                           <site name="left_palm" pos="0 -0.05 -.08"/>
                         </body>
                       </body>
@@ -222,7 +223,7 @@
                           <geom class="visual_dark" pos="-0.0330568 0.0546993 -0.167156" quat="0.683567 0.0657951 0.726915 0.00142511" mesh="idx-F2"/>
                           <geom class="visual_dark" pos="0.0240477 0.0463855 -0.0695448" quat="-0.125649 0.986553 -0.0882404 -0.0560268" mesh="thumb-F1-MIR"/>
                           <geom class="visual_dark" pos="0.0499644 0.0457351 -0.0874305" quat="-0.859175 0.500891 -0.0981665 0.0358989" mesh="thumb-F2-right"/>
-                          <geom name="collision_r_hand_plate" class="collision" size="0.041 0.02 0.083" pos="0.00749988 0.025 -0.112" type="box"/>
+                          <geom name="collision_r_hand_plate" class="collision" size="0.055 0.083" pos="0.00749988 0.04 -0.112" type="capsule"/>
                           <site name="right_palm" pos="0 0.05 -.08"/>
                         </body>
                       </body>

diff --git a/examples/apptronik_apollo/assets/wood.png b/examples/apptronik_apollo/assets/wood.png
diff --git a/examples/apptronik_apollo/scene_table.xml b/examples/apptronik_apollo/scene_table.xml
@@ -0,0 +1,58 @@
+<mujoco model="apollo scene">
+  <include file="apollo.xml"/>
+
+  <statistic center="0.15 0 1.2" extent="1" meansize="0.08"/>
+
+  <visual>
+    <headlight diffuse="0.6 0.6 0.6" ambient="0.1 0.1 0.1" specular="0 0 0"/>
+    <global azimuth="160" elevation="-20"/>
+  </visual>
+
+  <asset>
+    <texture name="grid" type="2d" builtin="checker" rgb1=".2 .3 .4" rgb2=".1 0.15 0.2" width="512" height="512"
+      mark="cross" markrgb=".8 .8 .8"/>
+    <material name="grid" texture="grid" texrepeat="1 1" texuniform="true"/>
+    <texture type="2d" name="wood" file="assets/wood.png"/>
+    <material name="wood" texture="wood"/>
+  </asset>
+
+  <worldbody>
+    <light pos="0 0 3" directional="true"/>
+    <geom name="table" type="box" size=".6 .5 .45" mass="1" material="wood" pos="0.8 0 .45" group="1" contype="1" conaffinity="1"/>
+    <body name="com_target" pos="0.5 0 .5" mocap="true">
+      <geom type="sphere" size=".06" contype="0" conaffinity="0" rgba=".6 .3 .3 .2"/>
+      <site type="sphere" size="0.01" rgba="0 0 1 1" group="1"/>
+    </body>
+    <body name="left_palm_target" pos="0.5 0 .5" quat="0 1 0 0" mocap="true">
+      <geom type="box" size=".04 .04 .04" contype="0" conaffinity="0" rgba=".6 .3 .3 .2"/>
+      <site type="sphere" size="0.01" rgba="0 0 1 1" group="1"/>
+    </body>
+    <body name="right_palm_target" pos="0.5 0 .5" quat="0 1 0 0" mocap="true">
+      <geom type="box" size=".04 .04 .04" contype="0" conaffinity="0" rgba=".6 .3 .3 .2"/>
+      <site type="sphere" size="0.01" rgba="0 0 1 1" group="1"/>
+    </body>
+    <body name="left_foot_target" pos="0.5 0 .5" quat="0 1 0 0" mocap="true">
+      <geom type="box" size=".04 .04 .04" contype="0" conaffinity="0" rgba=".6 .3 .3 .2"/>
+      <site type="sphere" size="0.01" rgba="0 0 1 1" group="1"/>
+    </body>
+    <body name="right_foot_target" pos="0.5 0 .5" quat="0 1 0 0" mocap="true">
+      <geom type="box" size=".04 .04 .04" contype="0" conaffinity="0" rgba=".6 .3 .3 .2"/>
+      <site type="sphere" size="0.01" rgba="0 0 1 1" group="1"/>
+    </body>
+    <body name="head_target" pos="0.5 0 .5" quat="0 1 0 0" mocap="true">
+      <geom type="sphere" size=".05" contype="0" conaffinity="0" rgba=".6 .3 .3 .2"/>
+      <site type="sphere" size="0.01" rgba="0 0 1 1" group="1"/>
+    </body>
+  </worldbody>
+
+  <keyframe>
+    <key name='teleop' qpos='-0.124446 -0.0493532 1.04921 1 -5.76769e-17 1.29078e-16 -3.02977e-17 0 0 0.333084 0 0 0 0.2 0 0.1 -0.4 0 0 0.10337 -1.29492 0.037696 -1.23241 -0.565512 0 0 0 0.08 0.1 -0.477 1.033 -0.03 -0.58 -0.08 -0.1 -0.477 1.033 0.03 -0.58'/>
+  </keyframe>
+
+  <sensor>
+    <fromto geom1="collision_r_hand_plate" geom2="table" cutoff="1"/>
+    <fromto geom1="collision_l_hand_plate" geom2="table" cutoff="1"/>
+    <fromto geom1="collision_r_hand_plate" geom2="collision_capsule_body_r_thigh" cutoff="1"/>
+    <fromto geom1="collision_l_hand_plate" geom2="collision_capsule_body_l_thigh" cutoff="1"/>
+  </sensor>
+</mujoco>
diff --git a/examples/humanoid_apollo.py b/examples/humanoid_apollo.py
@@ -8,7 +8,7 @@
 import mink
 
 _HERE = Path(__file__).parent
-_XML = _HERE / "apptronik_apollo" / "scene.xml"
+_XML = _HERE / "apptronik_apollo" / "scene_table.xml"
 
 
 def compute_look_at_rotation(
@@ -49,25 +49,27 @@ def compute_look_at_rotation(
             frame_name="base_link",
             frame_type="body",
             position_cost=0.0,
-            orientation_cost=10.0,
+            orientation_cost=1.0,
+            lm_damping=1.0,
         ),
         torso_orientation_task := mink.FrameTask(
             frame_name="torso_link",
             frame_type="body",
             position_cost=0.0,
-            orientation_cost=10.0,
+            orientation_cost=1.0,
+            lm_damping=1.0,
         ),
-        posture_task := mink.PostureTask(model, cost=1.0),
-        com_task := mink.ComTask(cost=200.0),
+        posture_task := mink.PostureTask(model, cost=1e-1),
+        com_task := mink.ComTask(cost=10.0),
     ]
 
     feet_tasks = []
     for foot in feet:
         task = mink.FrameTask(
             frame_name=foot,
             frame_type="site",
-            position_cost=200.0,
-            orientation_cost=10.0,
+            position_cost=100.0,
+            orientation_cost=1.0,
             lm_damping=1.0,
         )
         feet_tasks.append(task)
@@ -78,8 +80,8 @@ def compute_look_at_rotation(
         task = mink.FrameTask(
             frame_name=hand,
             frame_type="site",
-            position_cost=200.0,
-            orientation_cost=0.0,
+            position_cost=5.0,
+            orientation_cost=1.0,
             lm_damping=1.0,
         )
         hand_tasks.append(task)
@@ -89,11 +91,31 @@ def compute_look_at_rotation(
         frame_name="head",
         frame_type="site",
         position_cost=0.0,
-        orientation_cost=10.0,
+        orientation_cost=1.0,
         lm_damping=1.0,
     )
     tasks.append(head_task)
 
+    # Enable collision avoidance between the following geoms.
+    # left hand - table, right hand - table
+    # left hand - left thigh, right hand - right thigh
+    collision_pairs = [
+        (["collision_r_hand_plate", "collision_l_hand_plate"], ["table"]),
+        (["collision_r_hand_plate"], ["collision_capsule_body_r_thigh"]),
+        (["collision_l_hand_plate"], ["collision_capsule_body_l_thigh"]),
+    ]
+    collision_avoidance_limit = mink.CollisionAvoidanceLimit(
+        model=model,
+        geom_pairs=collision_pairs,  # type: ignore
+        minimum_distance_from_collisions=0.05,
+        collision_detection_distance=0.1,
+    )
+    print(collision_avoidance_limit.geom_id_pairs)
+    limits = [
+        mink.ConfigurationLimit(model),
+        collision_avoidance_limit,
+    ]
+
     com_mid = model.body("com_target").mocapid[0]
     feet_mid = [model.body(f"{foot}_target").mocapid[0] for foot in feet]
     hands_mid = [model.body(f"{hand}_target").mocapid[0] for hand in hands]
@@ -108,7 +130,7 @@ def compute_look_at_rotation(
         mujoco.mjv_defaultFreeCamera(model, viewer.cam)
 
         # Initialize to the home keyframe.
-        configuration.update_from_keyframe("stand")
+        configuration.update_from_keyframe("teleop")
         posture_task.set_target_from_configuration(configuration)
         pelvis_orientation_task.set_target_from_configuration(configuration)
         torso_orientation_task.set_target_from_configuration(configuration)
@@ -135,10 +157,18 @@ def compute_look_at_rotation(
             )
             head_task.set_target(head_target)
 
-            vel = mink.solve_ik(configuration, tasks, rate.dt, solver, 1e-1)
+            # Compute velocity and integrate into the next configuration.
+            vel = mink.solve_ik(
+                configuration, tasks, rate.dt, solver, 1e-1, limits=limits
+            )
             configuration.integrate_inplace(vel, rate.dt)
             mujoco.mj_camlight(model, data)
 
+            # Note the below are optional: they are used to visualize the output of the
+            # fromto sensor which is used by the collision avoidance constraint.
+            mujoco.mj_fwdPosition(model, data)
+            mujoco.mj_sensorPos(model, data)
+
             # Visualize at fixed FPS.
             viewer.sync()
             rate.sleep()
diff --git a/examples/humanoid_g1.py b/examples/humanoid_g1.py
@@ -7,14 +7,13 @@
 import mink
 
 _HERE = Path(__file__).parent
-_XML = _HERE / "unitree_g1" / "scene.xml"
+_XML = _HERE / "unitree_g1" / "scene_table.xml"
 
 
 if __name__ == "__main__":
     model = mujoco.MjModel.from_xml_path(_XML.as_posix())
 
     configuration = mink.Configuration(model)
-
     feet = ["right_foot", "left_foot"]
     hands = ["right_palm", "left_palm"]
 
@@ -23,27 +22,27 @@
             frame_name="pelvis",
             frame_type="body",
             position_cost=0.0,
-            orientation_cost=10.0,
+            orientation_cost=1.0,
             lm_damping=1.0,
         ),
         torso_orientation_task := mink.FrameTask(
             frame_name="torso_link",
             frame_type="body",
             position_cost=0.0,
-            orientation_cost=10.0,
+            orientation_cost=1.0,
             lm_damping=1.0,
         ),
-        posture_task := mink.PostureTask(model, cost=1.0),
-        com_task := mink.ComTask(cost=50.0),
+        posture_task := mink.PostureTask(model, cost=1e-1),
+        com_task := mink.ComTask(cost=10.0),
     ]
 
     feet_tasks = []
     for foot in feet:
         task = mink.FrameTask(
             frame_name=foot,
             frame_type="site",
-            position_cost=200.0,
-            orientation_cost=10.0,
+            position_cost=10.0,
+            orientation_cost=1.0,
             lm_damping=1.0,
         )
         feet_tasks.append(task)
@@ -54,28 +53,48 @@
         task = mink.FrameTask(
             frame_name=hand,
             frame_type="site",
-            position_cost=200.0,
-            orientation_cost=10.0,
+            position_cost=5.0,
+            orientation_cost=1.0,
             lm_damping=1.0,
         )
         hand_tasks.append(task)
     tasks.extend(hand_tasks)
 
+    # Enable collision avoidance between the following geoms.
+    # left hand - table, right hand - table
+    # left hand - left thigh, right hand - right thigh
+    collision_pairs = [
+        (["left_hand_collision", "right_hand_collision"], ["table"]),
+        (["left_hand_collision"], ["left_thigh"]),
+        (["right_hand_collision"], ["right_thigh"]),
+    ]
+    collision_avoidance_limit = mink.CollisionAvoidanceLimit(
+        model=model,
+        geom_pairs=collision_pairs,  # type: ignore
+        minimum_distance_from_collisions=0.05,
+        collision_detection_distance=0.1,
+    )
+
+    limits = [
+        mink.ConfigurationLimit(model),
+        collision_avoidance_limit,
+    ]
+
     com_mid = model.body("com_target").mocapid[0]
     feet_mid = [model.body(f"{foot}_target").mocapid[0] for foot in feet]
     hands_mid = [model.body(f"{hand}_target").mocapid[0] for hand in hands]
 
     model = configuration.model
     data = configuration.data
-    solver = "quadprog"
+    solver = "osqp"
 
     with mujoco.viewer.launch_passive(
         model=model, data=data, show_left_ui=False, show_right_ui=False
     ) as viewer:
         mujoco.mjv_defaultFreeCamera(model, viewer.cam)
 
         # Initialize to the home keyframe.
-        configuration.update_from_keyframe("stand")
+        configuration.update_from_keyframe("teleop")
         posture_task.set_target_from_configuration(configuration)
         pelvis_orientation_task.set_target_from_configuration(configuration)
         torso_orientation_task.set_target_from_configuration(configuration)
@@ -93,10 +112,17 @@
                 foot_task.set_target(mink.SE3.from_mocap_id(data, feet_mid[i]))
                 hand_task.set_target(mink.SE3.from_mocap_id(data, hands_mid[i]))
 
-            vel = mink.solve_ik(configuration, tasks, rate.dt, solver, 1e-1)
+            vel = mink.solve_ik(
+                configuration, tasks, rate.dt, solver, 1e-1, limits=limits
+            )
             configuration.integrate_inplace(vel, rate.dt)
             mujoco.mj_camlight(model, data)
 
+            # Note the below are optional: they are used to visualize the output of the
+            # fromto sensor which is used by the collision avoidance constraint.
+            mujoco.mj_fwdPosition(model, data)
+            mujoco.mj_sensorPos(model, data)
+
             # Visualize at fixed FPS.
             viewer.sync()
             rate.sleep()