Feat collisions mujoco

examples/gui_demos/mini_head_lookat_gui.py

@@ -0,0 +1,100 @@
+"""Reachy Mini Head Position GUI Example."""
+
+import time
+import tkinter as tk
+
+import numpy as np
+
+from reachy_mini import ReachyMini
+from reachy_mini.utils import create_head_pose
+
+
+def main():
+    """Run a GUI to set the head position and orientation of Reachy Mini."""
+    with ReachyMini(media_backend="no_media") as mini:
+        t0 = time.time()
+
+        root = tk.Tk()
+        root.title("Set Look At XYZ Position")
+
+        # Add sliders for X, Y, Z position
+        x_var = tk.DoubleVar(value=0.0)
+        y_var = tk.DoubleVar(value=0.0)
+        z_var = tk.DoubleVar(value=0.0)
+
+        tk.Label(root, text="X (m):").grid(row=0, column=0)
+        tk.Scale(
+            root,
+            variable=x_var,
+            from_=-0.2,
+            to=0.2,
+            resolution=0.001,
+            orient=tk.HORIZONTAL,
+            length=200,
+        ).grid(row=0, column=1)
+        tk.Label(root, text="Y (m):").grid(row=1, column=0)
+        tk.Scale(
+            root,
+            variable=y_var,
+            from_=-0.2,
+            to=0.2,
+            resolution=0.001,
+            orient=tk.HORIZONTAL,
+            length=200,
+        ).grid(row=1, column=1)
+        tk.Label(root, text="Z (m):").grid(row=2, column=0)
+        tk.Scale(
+            root,
+            variable=z_var,
+            from_=-0.2,
+            to=0.2,
+            resolution=0.001,
+            orient=tk.HORIZONTAL,
+            length=200,
+        ).grid(row=2, column=1)
+
+        tk.Label(root, text="Body Yaw (deg):").grid(row=3, column=0)
+        body_yaw_var = tk.DoubleVar(value=0.0)
+        tk.Scale(
+            root,
+            variable=body_yaw_var,
+            from_=-180,
+            to=180,
+            resolution=1.0,
+            orient=tk.HORIZONTAL,
+            length=200,
+        ).grid(row=3, column=1)
+
+        mini.goto_target(create_head_pose(), antennas=[0.0, 0.0], duration=1.0)
+
+        # Run the GUI in a non-blocking way
+        root.update()
+
+        try:
+            while True:
+                t = time.time() - t0
+                target = np.deg2rad(30) * np.sin(2 * np.pi * 0.5 * t)
+
+                head = np.eye(4)
+                head[:3, 3] = [0, 0, 0.0]
+
+                head = mini.look_at_world(x_var.get(), y_var.get(), z_var.get(), duration=0.3, perform_movement=False)
+
+                root.update()
+
+                mini.set_target(
+                    head=head,
+                    body_yaw=np.deg2rad(body_yaw_var.get()),
+                    antennas=np.array([target, -target]),
+                )
+
+
+
+        except KeyboardInterrupt:
+            pass
+        finally:
+            root.destroy()
+
+
+if __name__ == "__main__":
+    main()

examples/mini_head_position_gui.py → examples/gui_demos/mini_head_position_gui.py

@@ -83,11 +83,6 @@ def main():
             length=200,
         ).grid(row=6, column=1)
 
-        # add a checkbox to enable/disable collision checking
-        collision_check_var = tk.BooleanVar(value=False)
-        tk.Checkbutton(root, text="Check Collision", variable=collision_check_var).grid(
-            row=7, column=1
-        )
 
         mini.goto_target(create_head_pose(), antennas=[0.0, 0.0], duration=1.0)
 

pyproject.toml

@@ -24,7 +24,7 @@ dependencies = [
     "huggingface-hub==0.34.4",
     "sounddevice==0.5.1",
     "soundfile==0.13.1",    
-    "reachy-mini-rust-kinematics>=1.0.1",
+    "reachy-mini-rust-kinematics>=1.0.2",
     "asgiref",
     "aiohttp",
     "log-throttling==0.0.3",

src/reachy_mini/daemon/backend/mujoco/backend.py

@@ -92,11 +92,6 @@ def __init__(
 
         self.current_head_pose = np.eye(4)
 
-        # print("Joints in the model:")
-        # for i in range(self.model.njoint):
-        #     name = mujoco.mj_id2joint(self.model, i)
-        #     print(f"  {i}: {name}")
-
         self.joint_names = get_actuator_names(self.model)
 
         self.joint_ids = [
@@ -105,6 +100,14 @@ def __init__(
         self.joint_qpos_addr = [
             get_joint_addr_from_name(self.model, n) for n in self.joint_names
         ]
+
+        self.col_inds = []
+        for i,type in enumerate(self.model.geom_contype):
+            if type != 0:
+                self.col_inds.append(i)
+                self.model.geom_contype[i] = 0
+                self.model.geom_conaffinity[i] = 0
+
 
     def rendering_loop(self) -> None:
         """Offline Rendering loop for the Mujoco simulation.
@@ -162,6 +165,18 @@ def run(self) -> None:
                 # recompute all kinematics, collisions, etc.
                 mujoco.mj_forward(self.model, self.data)
 
+        for i in range(100):
+            mujoco.mj_step(self.model, self.data)
+
+        # enable collisions
+        for i in self.col_inds:
+            self.model.geom_contype[i] = 1
+            self.model.geom_conaffinity[i] = 1
+
+
+        for i in range(100):
+            mujoco.mj_step(self.model, self.data)
+
         # one more frame so the viewer shows your startup pose
         mujoco.mj_step(self.model, self.data)
         if not self.headless:
@@ -170,6 +185,11 @@ def run(self) -> None:
             rendering_thread = Thread(target=self.rendering_loop, daemon=True)
             rendering_thread.start()
 
+        # Update the internal states of the IK and FK to the current configuration
+        # This is important to avoid jumps when starting the robot (beore wake-up)
+        self.head_kinematics.ik(self.get_mj_present_head_pose(), no_iterations=20)
+        self.head_kinematics.fk(self.get_present_head_joint_positions(), no_iterations=20)
+
         # 3) now enter your normal loop
         while not self.should_stop.is_set():
             start_t = time.time()
@@ -230,7 +250,7 @@ def run(self) -> None:
 
             took = time.time() - start_t
             time.sleep(max(0, self.model.opt.timestep - took))
-            # print(f"Step {step}: took {took*1000:.1f}ms")
+            #print(f"Step {step}: took {took*1e6:.1f}us")
             step += 1
 
         if not self.headless:

src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/README.m

@@ -0,0 +1 @@
+Created with: https://github.com/pollen-robotics/reachy_mini_stl_convexify

src/reachy_mini/descriptions/reachy_mini/mjcf/joints_properties.xml

@@ -2,8 +2,8 @@
 <default>
     <default class="perfect_actuator">
         <geom contype="0" conaffinity="0"/>
-        <joint damping="0.095" frictionloss="0.001" armature="0.001"/>
-        <position kp="10.0" kv="0.18" forcerange="-10.0 10.0"/>
+        <joint damping="0.15" frictionloss="0.1" armature="0.001"/>
+        <position kp="10.0" kv="0.1" forcerange="-20.0 20.0"/>
         <default class="chosen_actuator"></default>
     </default>
     <default class="xc330m288t">

src/reachy_mini/descriptions/reachy_mini/mjcf/reachy_mini.xml

@@ -11,16 +11,16 @@
         <geom type="mesh" contype="0" conaffinity="0" group="2"/>
       </default>
       <default class="collision">
-        <geom group="3"/>
+        <geom type="mesh" contype="1" conaffinity="1"  group="3"/>
       </default>
     </default>
   </default>
   <!-- Additional joints_properties.xml -->
   <default>
     <default class="perfect_actuator">
       <geom contype="0" conaffinity="0"/>
-      <joint damping="0.095" frictionloss="0.001" armature="0.001"/>
-      <position kp="10.0" kv="0.18" forcerange="-10.0 10.0"/>
+        <joint damping="0.15" frictionloss="0.1" armature="0.001"/>
+        <position kp="10.0" kv="0.1" forcerange="-20.0 20.0"/>
       <default class="chosen_actuator"/>
     </default>
     <default class="xc330m288t">
@@ -75,6 +75,14 @@
         <!-- Part bearing_85x110x13 -->
         <geom type="mesh" class="visual" pos="3.79972e-17 -3.70588e-18 -0.0115" quat="0.5 0.5 0.5 0.5" mesh="bearing_85x110x13" material="bearing_85x110x13_material"/>
         <!-- Part body_top_3dprint -->
+        <geom type="mesh" class="collision" pos="6.43558e-16 9.98888e-16 0.195" quat="1.38781e-17 -0 1.67393e-17 1" mesh="body_top_3dprint_collider_front_0"/>
+        <geom type="mesh" class="collision" pos="6.43558e-16 9.98888e-16 0.195" quat="1.38781e-17 -0 1.67393e-17 1" mesh="body_top_3dprint_collider_front_1"/>
+        <geom type="mesh" class="collision" pos="6.43558e-16 9.98888e-16 0.195" quat="1.38781e-17 -0 1.67393e-17 1" mesh="body_top_3dprint_collider_front_2"/>
+        <geom type="mesh" class="collision" pos="6.43558e-16 9.98888e-16 0.195" quat="1.38781e-17 -0 1.67393e-17 1" mesh="body_top_3dprint_collider_back_0"/>
+        <geom type="mesh" class="collision" pos="6.43558e-16 9.98888e-16 0.195" quat="1.38781e-17 -0 1.67393e-17 1" mesh="body_top_3dprint_collider_back_1"/>
+        <geom type="mesh" class="collision" pos="6.43558e-16 9.98888e-16 0.195" quat="1.38781e-17 -0 1.67393e-17 1" mesh="body_top_3dprint_collider_back_2"/>
+        <geom type="mesh" class="collision" pos="6.43558e-16 9.98888e-16 0.195" quat="1.38781e-17 -0 1.67393e-17 1" mesh="body_top_3dprint_collider_back_3"/>
+
         <geom type="mesh" class="visual" pos="6.43558e-16 9.98888e-16 0.195" quat="1.38781e-17 -0 1.67393e-17 1" mesh="body_top_3dprint" material="body_top_3dprint_material"/>
         <!-- Part stewart_main_plate_3dprint -->
         <geom type="mesh" class="visual" pos="-3.07628e-26 -5.29396e-23 0.195" quat="1.38846e-17 0 1.67459e-17 1" mesh="stewart_main_plate_3dprint" material="stewart_main_plate_3dprint_material"/>
@@ -400,6 +408,7 @@
               <!-- Part glasses_dolder_3dprint -->
               <geom type="mesh" class="visual" pos="-0.0535717 -0.0190178 0.0256147" quat="0.664652 0.38283 -0.641585 -0.00682952" mesh="glasses_dolder_3dprint" material="glasses_dolder_3dprint_material"/>
               <!-- Part head_front_3dprint -->
+              <geom type="mesh" class="collision" pos="-0.0535717 -0.0190178 0.0256147" quat="0.664652 0.38283 -0.641585 -0.00682952" mesh="head_one_3dprint_collider_0"/>
               <geom type="mesh" class="visual" pos="-0.0535717 -0.0190178 0.0256147" quat="0.664652 0.38283 -0.641585 -0.00682952" mesh="head_front_3dprint" material="head_front_3dprint_material"/>
               <!-- Part head_mic_3dprint -->
               <geom type="mesh" class="visual" pos="-0.0535717 -0.0190178 0.0256147" quat="0.664652 0.38283 -0.641585 -0.00682952" mesh="head_mic_3dprint" material="head_mic_3dprint_material"/>
@@ -472,8 +481,10 @@
                   fovy="80"
                 />
               </body> 
+
               <!-- Frame head -->
               <site group="3" name="head" pos="-0.00611127 0.00370522 0.0291364" quat="0.465151 0.724371 -0.182968 -0.474809"/>
+
               <!-- Link dc15_a01_horn_dummy_7 -->
               <body name="dc15_a01_horn_dummy_7" pos="-0.0948524 0.0197779 -0.00445785" quat="0.652996 0.376401 0.603671 -0.259807">
                 <!-- Joint from xl_330 to dc15_a01_horn_dummy_7 -->
@@ -550,6 +561,17 @@
     <mesh file="phs_1_7x20_5_dc10.stl"/>
     <mesh file="phs_1_7x20_5_dc10_1.stl"/>
     <mesh file="head_back_3dprint.stl"/>
+
+    <!-- Collision models defualt: coarse - there is also fine (but much more detailed models)-->
+    <mesh file="collision/coarse/head_one_3dprint_collider_0.stl"/>
+    <mesh file="collision/coarse/body_top_3dprint_collider_front_0.stl"/>
+    <mesh file="collision/coarse/body_top_3dprint_collider_front_1.stl"/>
+    <mesh file="collision/coarse/body_top_3dprint_collider_front_2.stl"/>
+    <mesh file="collision/coarse/body_top_3dprint_collider_back_0.stl"/>
+    <mesh file="collision/coarse/body_top_3dprint_collider_back_1.stl"/>
+    <mesh file="collision/coarse/body_top_3dprint_collider_back_2.stl"/>
+    <mesh file="collision/coarse/body_top_3dprint_collider_back_3.stl"/>
+
     <material name="body_foot_3dprint_material" rgba="0.301961 0.301961 0.301961 1"/>
     <material name="dc15_a01_horn_dummy_material" rgba="0.262745 0.282353 0.301961 1"/>
     <material name="body_turning_3dprint_material" rgba="0.301961 0.301961 0.301961 1"/>

src/reachy_mini/kinematics/analytical_kinematics.py

@@ -67,10 +67,17 @@ def ik(
         """
         _pose = pose.copy()
         _pose[:3, 3][2] += self.head_z_offset
-
-        stewart_joints = self.kin.inverse_kinematics(_pose, body_yaw)  # type: ignore[arg-type]
-
-        return np.array([body_yaw] + stewart_joints)
+
+        # inverse kinematics solution that modulates the body yaw to 
+        # stay within the mechanical limits (max_body_yaw) 
+        # additionally it makes sure the the relative yaw between the body and the head
+        # stays within the mechanical limits (max_relative_yaw)
+        reachy_joints = self.kin.inverse_kinematics_safe(_pose,  # type: ignore[arg-type]
+                                                         body_yaw = body_yaw, 
+                                                         max_relative_yaw = np.deg2rad(65), 
+                                                         max_body_yaw = np.deg2rad(160))  
+
+        return np.array(reachy_joints)  
 
     def fk(
         self,