From 2d7c2bc35ff444d57256eecf84b9c9e45c18ab4e Mon Sep 17 00:00:00 2001
From: gospar <antun.skuric@outlook.com>
Date: Tue, 14 Oct 2025 15:22:59 +0200
Subject: [PATCH 01/15] robust invese kinematics

---
 .../kinematics/analytical_kinematics.py           | 15 +++++++++++----
 1 file changed, 11 insertions(+), 4 deletions(-)

diff --git a/src/reachy_mini/kinematics/analytical_kinematics.py b/src/reachy_mini/kinematics/analytical_kinematics.py
index 0691b7d8..2422f2cd 100644
--- a/src/reachy_mini/kinematics/analytical_kinematics.py
+++ b/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, 
+                                                         body_yaw = body_yaw, 
+                                                         max_relative_yaw = np.deg2rad(65), 
+                                                         max_body_yaw = np.deg2rad(160))  # type: ignore[arg-type]
+       
+        return np.array(reachy_joints)  # type: ignore[arg-type]
 
     def fk(
         self,

From f8229803f7a7aca7237ff965ead0152331453b45 Mon Sep 17 00:00:00 2001
From: gospar <antun.skuric@outlook.com>
Date: Thu, 16 Oct 2025 09:08:52 +0200
Subject: [PATCH 02/15] remove elapsed time dumping

---
 src/reachy_mini/kinematics/analytical_kinematics.py | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/reachy_mini/kinematics/analytical_kinematics.py b/src/reachy_mini/kinematics/analytical_kinematics.py
index 2422f2cd..62cd5cb3 100644
--- a/src/reachy_mini/kinematics/analytical_kinematics.py
+++ b/src/reachy_mini/kinematics/analytical_kinematics.py
@@ -65,6 +65,8 @@ def ik(
 
         check_collision and no_iterations are not used by AnalyticalKinematics. We keep them for compatibility with the other kinematics engines
         """
+        # import time
+        # k = time.time()
         _pose = pose.copy()
         _pose[:3, 3][2] += self.head_z_offset
            
@@ -76,7 +78,7 @@ def ik(
                                                          body_yaw = body_yaw, 
                                                          max_relative_yaw = np.deg2rad(65), 
                                                          max_body_yaw = np.deg2rad(160))  # type: ignore[arg-type]
-       
+        # print("IK time {0:.3f}us".format((time.time() - k)*1e6))
         return np.array(reachy_joints)  # type: ignore[arg-type]
 
     def fk(

From 47a108616fd807528834e10f65edd8802e9c8b2f Mon Sep 17 00:00:00 2001
From: gospar <antun.skuric@outlook.com>
Date: Wed, 22 Oct 2025 10:15:10 +0200
Subject: [PATCH 03/15] bump the version of the rust_kinematics

---
 pyproject.toml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/pyproject.toml b/pyproject.toml
index a66a0efa..48853f59 100644
--- a/pyproject.toml
+++ b/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",
 ]

From 3f7209829ce6bd0cc1131853f2eadfbe725eb8fb Mon Sep 17 00:00:00 2001
From: gospar <antun.skuric@outlook.com>
Date: Wed, 22 Oct 2025 10:45:22 +0200
Subject: [PATCH 04/15] ci launch

---
 src/reachy_mini/kinematics/analytical_kinematics.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/reachy_mini/kinematics/analytical_kinematics.py b/src/reachy_mini/kinematics/analytical_kinematics.py
index 62cd5cb3..65ec2b5c 100644
--- a/src/reachy_mini/kinematics/analytical_kinematics.py
+++ b/src/reachy_mini/kinematics/analytical_kinematics.py
@@ -97,7 +97,7 @@ def fk(
 
         if no_iterations < 1:
             raise ValueError("no_iterations must be at least 1")
-
+    
         T_world_platform = None
         for _ in range(no_iterations):
             T_world_platform = np.array(

From 723652d44b756d279624b41266120d06104435c6 Mon Sep 17 00:00:00 2001
From: gospar <antun.skuric@outlook.com>
Date: Wed, 22 Oct 2025 10:53:20 +0200
Subject: [PATCH 05/15] update for mypy

---
 .../kinematics/analytical_kinematics.py            | 14 ++++++--------
 1 file changed, 6 insertions(+), 8 deletions(-)

diff --git a/src/reachy_mini/kinematics/analytical_kinematics.py b/src/reachy_mini/kinematics/analytical_kinematics.py
index 65ec2b5c..5ea41b9f 100644
--- a/src/reachy_mini/kinematics/analytical_kinematics.py
+++ b/src/reachy_mini/kinematics/analytical_kinematics.py
@@ -46,13 +46,13 @@ def __init__(self) -> None:
         for motor in self.motors:
             self.kin.add_branch(
                 motor["branch_position"],
-                np.linalg.inv(motor["T_motor_world"]),  # type: ignore[arg-type]
+                np.linalg.inv(motor["T_motor_world"]), 
                 1 if motor["solution"] else -1,
             )
 
         sleep_head_pose = SLEEP_HEAD_POSE.copy()
         sleep_head_pose[:3, 3][2] += self.head_z_offset
-        self.kin.reset_forward_kinematics(sleep_head_pose)  # type: ignore[arg-type]
+        self.kin.reset_forward_kinematics(sleep_head_pose)  
 
     def ik(
         self,
@@ -65,8 +65,6 @@ def ik(
 
         check_collision and no_iterations are not used by AnalyticalKinematics. We keep them for compatibility with the other kinematics engines
         """
-        # import time
-        # k = time.time()
         _pose = pose.copy()
         _pose[:3, 3][2] += self.head_z_offset
            
@@ -77,9 +75,9 @@ def ik(
         reachy_joints = self.kin.inverse_kinematics_safe(_pose, 
                                                          body_yaw = body_yaw, 
                                                          max_relative_yaw = np.deg2rad(65), 
-                                                         max_body_yaw = np.deg2rad(160))  # type: ignore[arg-type]
-        # print("IK time {0:.3f}us".format((time.time() - k)*1e6))
-        return np.array(reachy_joints)  # type: ignore[arg-type]
+                                                         max_body_yaw = np.deg2rad(160))  
+        
+        return np.array(reachy_joints)  
 
     def fk(
         self,
@@ -97,7 +95,7 @@ def fk(
 
         if no_iterations < 1:
             raise ValueError("no_iterations must be at least 1")
-    
+
         T_world_platform = None
         for _ in range(no_iterations):
             T_world_platform = np.array(

From 617f8cdab17c239cde0c8667fbc821d2d30662f3 Mon Sep 17 00:00:00 2001
From: gospar <antun.skuric@outlook.com>
Date: Wed, 22 Oct 2025 10:57:34 +0200
Subject: [PATCH 06/15] ignore arg type

---
 src/reachy_mini/kinematics/analytical_kinematics.py | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/reachy_mini/kinematics/analytical_kinematics.py b/src/reachy_mini/kinematics/analytical_kinematics.py
index 5ea41b9f..43c4fcde 100644
--- a/src/reachy_mini/kinematics/analytical_kinematics.py
+++ b/src/reachy_mini/kinematics/analytical_kinematics.py
@@ -46,13 +46,13 @@ def __init__(self) -> None:
         for motor in self.motors:
             self.kin.add_branch(
                 motor["branch_position"],
-                np.linalg.inv(motor["T_motor_world"]), 
+                np.linalg.inv(motor["T_motor_world"]),  # type: ignore[arg-type]
                 1 if motor["solution"] else -1,
             )
 
         sleep_head_pose = SLEEP_HEAD_POSE.copy()
         sleep_head_pose[:3, 3][2] += self.head_z_offset
-        self.kin.reset_forward_kinematics(sleep_head_pose)  
+        self.kin.reset_forward_kinematics(sleep_head_pose)  # type: ignore[arg-type]
 
     def ik(
         self,
@@ -72,7 +72,7 @@ def ik(
         # 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, 
+        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))  

From 4ef630a97f15234006c121cd48b20da4c5fdd497 Mon Sep 17 00:00:00 2001
From: gospar <antun.skuric@outlook.com>
Date: Thu, 23 Oct 2025 19:01:27 +0200
Subject: [PATCH 07/15] inital support for collision!

---
 .../daemon/backend/mujoco/backend copy.py     | 312 ++++++++++++
 .../daemon/backend/mujoco/backend.py          |  22 +
 .../reachy_mini/mjcf/reachy_mini copy 2.xml   | 450 ++++++++++++++++++
 .../reachy_mini/mjcf/reachy_mini copy.xml     | 448 +++++++++++++++++
 .../reachy_mini/mjcf/reachy_mini.xml          |  12 +-
 .../reachy_mini/mjcf/reachy_mini.xml.old      | 440 +++++++++++++++++
 6 files changed, 1683 insertions(+), 1 deletion(-)
 create mode 100644 src/reachy_mini/daemon/backend/mujoco/backend copy.py
 create mode 100644 src/reachy_mini/descriptions/reachy_mini/mjcf/reachy_mini copy 2.xml
 create mode 100644 src/reachy_mini/descriptions/reachy_mini/mjcf/reachy_mini copy.xml
 create mode 100644 src/reachy_mini/descriptions/reachy_mini/mjcf/reachy_mini.xml.old

diff --git a/src/reachy_mini/daemon/backend/mujoco/backend copy.py b/src/reachy_mini/daemon/backend/mujoco/backend copy.py
new file mode 100644
index 00000000..881ce03b
--- /dev/null
+++ b/src/reachy_mini/daemon/backend/mujoco/backend copy.py	
@@ -0,0 +1,312 @@
+"""Mujoco Backend for Reachy Mini.
+
+This module provides the MujocoBackend class for simulating the Reachy Mini robot using the MuJoCo physics engine.
+
+It includes methods for running the simulation, getting joint positions, and controlling the robot's joints.
+
+"""
+
+import json
+import time
+from dataclasses import dataclass
+from importlib.resources import files
+from threading import Thread
+from typing import Optional
+
+import mujoco
+import mujoco.viewer
+import numpy as np
+
+import reachy_mini
+from reachy_mini.daemon.backend.abstract import Backend
+from reachy_mini.daemon.backend.mujoco.utils import (
+    get_actuator_names,
+    get_joint_addr_from_name,
+    get_joint_id_from_name,
+)
+from reachy_mini.daemon.backend.mujoco.video_udp import UDPJPEGFrameSender
+
+
+class MujocoBackend(Backend):
+    """Simulated Reachy Mini using MuJoCo."""
+
+    def __init__(self, scene="empty", check_collision: bool = False):
+        """Initialize the MujocoBackend with a specified scene.
+
+        Args:
+            scene (str): The name of the scene to load. Default is "empty".
+            check_collision (bool): If True, enable collision checking. Default is False.
+
+        """
+        super().__init__(check_collision=check_collision)
+
+        from reachy_mini.reachy_mini import (
+            SLEEP_ANTENNAS_JOINT_POSITIONS,
+            SLEEP_HEAD_JOINT_POSITIONS,
+        )
+
+        self._SLEEP_ANTENNAS_JOINT_POSITIONS = SLEEP_ANTENNAS_JOINT_POSITIONS
+        self._SLEEP_HEAD_JOINT_POSITIONS = SLEEP_HEAD_JOINT_POSITIONS
+
+        mjcf_root_path = str(
+            files(reachy_mini).joinpath("descriptions/reachy_mini/mjcf/")
+        )
+        self.model = mujoco.MjModel.from_xml_path(  # type: ignore
+            f"{mjcf_root_path}/scenes/{scene}.xml"
+        )
+        self.data = mujoco.MjData(self.model)  # type: ignore
+        self.model.opt.timestep = 0.002  # s, simulation timestep, 500hz
+        self.decimation = 10  # -> 50hz control loop
+        self.rendering_timestep = 0.04  # s, rendering loop # 25Hz
+
+        self.camera_id = mujoco.mj_name2id(  # type: ignore
+            self.model,
+            mujoco.mjtObj.mjOBJ_CAMERA,  # type: ignore
+            "eye_camera",
+        )
+
+        self.head_id = mujoco.mj_name2id(  # type: ignore
+            self.model,
+            mujoco.mjtObj.mjOBJ_BODY,  # type: ignore
+            "pp01063_stewart_plateform",
+        )
+
+        self.platform_to_head_transform = np.array(
+            [
+                [8.66025292e-01, 5.00000194e-01, -1.83660327e-06, -1.34282000e-02],
+                [5.55111512e-16, -3.67320510e-06, -1.00000000e00, -1.20000000e-03],
+                [-5.00000194e-01, 8.66025292e-01, -3.18108852e-06, 3.65883000e-02],
+                [0, 0, 0, 1.00000000e00],
+            ]
+        )
+        # remove_z_offset  = np.eye(4)
+        # remove_z_offset[2, 3] = -0.177
+        # self.platform_to_head_transform = self.platform_to_head_transform @ remove_z_offset
+
+        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 = [
+            get_joint_id_from_name(self.model, n) for n in self.joint_names
+        ]
+        self.joint_qpos_addr = [
+            get_joint_addr_from_name(self.model, n) for n in self.joint_names
+        ]
+
+        # disable the collisions by default 
+        self.model.geom_contype[13] = 0 # head capsule
+        self.model.geom_conaffinity[13] = 0 
+        self.model.geom_contype[80] = 0 # top of the torso 
+        self.model.geom_conaffinity[80] = 0
+
+    def rendering_loop(self):
+        """Offline Rendering loop for the Mujoco simulation.
+
+        Capture the image from the virtual Reachy's camera and send it over UDP.
+        """
+        streamer_udp = UDPJPEGFrameSender()
+        camera_size = (1280, 720)
+        offscreen_renderer = mujoco.Renderer(
+            self.model, height=camera_size[1], width=camera_size[0]
+        )
+        while not self.should_stop.is_set():
+            start_t = time.time()
+            offscreen_renderer.update_scene(self.data, self.camera_id)
+            im = offscreen_renderer.render()
+            streamer_udp.send_frame(im)
+
+            took = time.time() - start_t
+            time.sleep(max(0, self.rendering_timestep - took))
+
+
+
+    def run(self):
+        """Run the Mujoco simulation with a viewer.
+
+        This method initializes the viewer and enters the main simulation loop.
+        It updates the joint positions at a rate and publishes the joint positions.
+        """
+        step = 1
+        with mujoco.viewer.launch_passive(
+            self.model, self.data, show_left_ui=False, show_right_ui=False
+        ) as viewer:
+            with viewer.lock():
+                viewer.cam.type = mujoco.mjtCamera.mjCAMERA_FREE  # type: ignore
+                viewer.cam.distance = 0.8  # ≃ ||pos - lookat||
+                viewer.cam.azimuth = 160  # degrees
+                viewer.cam.elevation = -20  # degrees
+                viewer.cam.lookat[:] = [0, 0, 0.15]
+
+                # force one render with your new camera
+                mujoco.mj_step(self.model, self.data)  # type: ignore
+                viewer.sync()
+
+                # im = self.get_camera()
+                # self.streamer_udp.send_frame(im)
+            with viewer.lock():
+                self.data.qpos[self.joint_qpos_addr] = np.array(
+                    self._SLEEP_HEAD_JOINT_POSITIONS
+                    + self._SLEEP_ANTENNAS_JOINT_POSITIONS
+                ).reshape(-1, 1)
+                self.data.ctrl[:] = np.array(
+                    self._SLEEP_HEAD_JOINT_POSITIONS
+                    + self._SLEEP_ANTENNAS_JOINT_POSITIONS
+                )
+
+                # recompute all kinematics, collisions, etc.
+                mujoco.mj_forward(self.model, self.data)  # type: ignore
+                
+            for i in range(20):
+                # one more frame so the viewer shows your startup pose
+                mujoco.mj_step(self.model, self.data)  # type: ignore
+            viewer.sync()
+
+            rendering_thread = Thread(target=self.rendering_loop, daemon=True)
+            rendering_thread.start()
+            
+            
+            #if self.check_collision:
+            # enable collisions
+            self.model.geom_contype[13] = 1 # head capsule
+            self.model.geom_conaffinity[13] = 1
+            self.model.geom_contype[80] = 1 # top of the torso 
+            self.model.geom_conaffinity[80] = 1
+
+            mujoco.mj_step(self.model, self.data)
+            
+            
+            
+            # 3) now enter your normal loop
+            while not self.should_stop.is_set():
+                start_t = time.time()
+
+                if step % self.decimation == 0:
+                    # update the current states
+                    self.current_head_joint_positions = (
+                        self.get_present_head_joint_positions()
+                    )
+                    self.current_antenna_joint_positions = (
+                        self.get_present_antenna_joint_positions()
+                    )
+                    self.current_head_pose = self.get_mj_present_head_pose()
+
+                    # Update the target head joint positions from IK if necessary
+                    # - does nothing if the targets did not change
+                    if self.ik_required:
+                        self.update_target_head_joints_from_ik(
+                            self.target_head_pose, self.target_body_yaw
+                        )
+
+                    if self.target_head_joint_positions is not None:
+                        self.data.ctrl[:7] = self.target_head_joint_positions
+                    if self.target_antenna_joint_positions is not None:
+                        self.data.ctrl[-2:] = self.target_antenna_joint_positions
+
+                    if (
+                        self.joint_positions_publisher is not None
+                        and self.pose_publisher is not None
+                    ):
+                        self.joint_positions_publisher.put(
+                            json.dumps(
+                                {
+                                    "head_joint_positions": self.current_head_joint_positions,
+                                    "antennas_joint_positions": self.current_antenna_joint_positions,
+                                }
+                            ).encode("utf-8")
+                        )
+                        self.pose_publisher.put(
+                            json.dumps(
+                                {
+                                    "head_pose": self.get_present_head_pose().tolist(),
+                                }
+                            ).encode("utf-8")
+                        )
+                    viewer.sync()
+
+                mujoco.mj_step(self.model, self.data)  # type: ignore
+
+                took = time.time() - start_t
+                time.sleep(max(0, self.model.opt.timestep - took))
+                # print(f"Step {step}: took {took*1000:.1f}ms")
+                step += 1
+                self.ready.set()
+
+    def get_mj_present_head_pose(self) -> np.ndarray:
+        """Get the current head pose from the Mujoco simulation.
+
+        Returns:
+            np.ndarray: The current head pose as a 4x4 transformation matrix.
+
+        """
+        mj_current_head_pose = np.eye(4)
+        mj_current_head_pose[:3, :3] = self.data.xmat[self.head_id].reshape(3, 3)
+        mj_current_head_pose[:3, 3] = self.data.xpos[self.head_id]
+        mj_current_head_pose = mj_current_head_pose @ self.platform_to_head_transform
+        mj_current_head_pose[2, 3] -= 0.177
+        return mj_current_head_pose
+
+    def close(self) -> None:
+        """Close the Mujoco backend."""
+        # TODO Do something in mujoco here ?
+        pass
+
+    def get_status(self) -> "MujocoBackendStatus":
+        """Get the status of the Mujoco backend.
+
+        Returns:
+            dict: An empty dictionary as the Mujoco backend does not have a specific status to report.
+
+        """
+        return MujocoBackendStatus()
+
+    def get_present_head_joint_positions(self):
+        """Get the current joint positions of the head."""
+        return self.data.qpos[self.joint_qpos_addr[:7]].flatten().tolist()
+
+    def get_present_antenna_joint_positions(self):
+        """Get the current joint positions of the antennas."""
+        return self.data.qpos[self.joint_qpos_addr[-2:]].flatten().tolist()
+
+    def enable_motors(self) -> None:
+        """Enable the motors.
+
+        Does nothing in the Mujoco backend as it does not have a concept of enabling/disabling motors.
+        """
+        pass
+
+    def disable_motors(self) -> None:
+        """Disable the motors.
+
+        Does nothing in the Mujoco backend as it does not have a concept of enabling/disabling motors.
+        """
+        pass
+
+    def set_head_operation_mode(self, mode: int) -> None:
+        """Set mode of operation for the head.
+
+        This does nothing in the Mujoco backend as it does not have a concept of operation modes.
+        """
+        pass
+
+    def set_antennas_operation_mode(self, mode: int) -> None:
+        """Set mode of operation for the antennas.
+
+        This does nothing in the Mujoco backend as it does not have a concept of operation modes.
+        """
+        pass
+
+
+@dataclass
+class MujocoBackendStatus:
+    """Dataclass to represent the status of the Mujoco backend.
+
+    Empty for now, as the Mujoco backend does not have a specific status to report.
+    """
+
+    error: Optional[str] = None
diff --git a/src/reachy_mini/daemon/backend/mujoco/backend.py b/src/reachy_mini/daemon/backend/mujoco/backend.py
index aba69eaa..1b855aa2 100644
--- a/src/reachy_mini/daemon/backend/mujoco/backend.py
+++ b/src/reachy_mini/daemon/backend/mujoco/backend.py
@@ -105,6 +105,20 @@ 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:
+                print(f"Geom {i} : contype={self.model.geom_contype[i]}, conaffinity={self.model.geom_conaffinity[i]}")
+                self.col_inds.append(i)
+                self.model.geom_contype[i] = 0
+                self.model.geom_conaffinity[i] = 0
+
+        # # disable the collisions by default
+        # self.model.geom_contype[13] = 0 # head capsule
+        # self.model.geom_conaffinity[13] = 0 
+        # self.model.geom_contype[80] = 0 # top of the torso 
+        # self.model.geom_conaffinity[80] = 0
 
     def rendering_loop(self) -> None:
         """Offline Rendering loop for the Mujoco simulation.
@@ -162,6 +176,14 @@ 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
+
         # one more frame so the viewer shows your startup pose
         mujoco.mj_step(self.model, self.data)
         if not self.headless:
diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/reachy_mini copy 2.xml b/src/reachy_mini/descriptions/reachy_mini/mjcf/reachy_mini copy 2.xml
new file mode 100644
index 00000000..8f8ccd2c
--- /dev/null
+++ b/src/reachy_mini/descriptions/reachy_mini/mjcf/reachy_mini copy 2.xml	
@@ -0,0 +1,450 @@
+<?xml version="1.0" ?>
+<!-- Generated using onshape-to-robot -->
+<!-- Onshape https://cad.onshape.com/documents/e305b26a84339af20c593973/w/ec7765eccdaa9a5960507125/e/251023ea04190ab16854eb80 -->
+<mujoco model="reachy_mini">
+  <compiler angle="radian" meshdir="assets" autolimits="true"/>
+  <default>
+    <default class="reachy_mini">
+      <joint frictionloss="0.1" armature="0.005"/>
+      <position kp="50" dampratio="1"/>
+      <default class="visual">
+        <geom type="mesh" contype="0" conaffinity="0" group="2"/>
+      </default>
+      <default class="collision">
+        <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.1" forcerange="-10.0 10.0"/>
+      <!-- <position kp="10.0" kv="0.0" forcerange="-10.0 10.0"/> -->
+      <default class="chosen_actuator"/>
+    </default>
+    <default class="xc330m288t">
+      <!-- probably wrong, would need to re-identify -->
+      <geom contype="0" conaffinity="0"/>
+      <joint damping="0.095" frictionloss="0.058" armature="0.0018"/>
+      <position kp="2.54" kv="0.0" forcerange="-0.8 0.8"/>
+    </default>
+    <default class="sts3215_345">
+      <!-- Standard sts3215 with 1/345 gear ratio -->
+      <!-- Confident that this is realistic (mini duck walks with these values) -->
+      <geom contype="0" conaffinity="0"/>
+      <joint damping="0.56" frictionloss="0.068" armature="0.027"/>
+      <position kp="17.11" kv="0.0" forcerange="-3.23 3.23"/>
+    </default>
+    <default class="sts3215_147">
+      <!-- sts3215 with 1/147 gear ratio -->
+      <!-- (Estimation based on the gear ratio difference) -->
+      <geom contype="0" conaffinity="0"/>
+      <joint damping="0.1" frictionloss="0.028" armature="0.011"/>
+      <position kp="17.11" kv="0.0" forcerange="-1.35 1.35"/>
+    </default>
+  </default>
+  <!-- Additional additional.xml -->
+  <contact>
+    <!-- <exclude name="exclude0" body1="torso" body2="sts32155"/> -->
+  </contact>
+  <default>
+    <equality solref="0.002 1" solimp="0.99 0.999 0.0005 0.5 2"/>
+  </default>
+  <worldbody>
+    <!-- Link pp01071_turning_bowl -->
+    <body name="pp01071_turning_bowl" pos="0 0 0" quat="1 0 0 0" childclass="reachy_mini">
+      <inertial pos="0.000667521 1.7878e-09 0.0118411" mass="0.0947234" fullinertia="0.000111359 0.000106563 0.000196298 1.02485e-11 8.96229e-07 4.95917e-13"/>
+      <!-- Part pp01071_turning_bowl -->
+      <geom type="mesh" class="visual" pos="-1.38778e-17 0 0.2345" quat="0.707107 0 -6.16298e-32 0.707107" mesh="pp01071_turning_bowl" material="pp01071_turning_bowl_material"/>
+      <!-- Part pp01072_turning_end -->
+      <geom type="mesh" class="visual" pos="3.46945e-17 -4.51028e-17 0.2345" quat="0.707107 7.85046e-17 -2.22045e-16 0.707107" mesh="pp01072_turning_end" material="pp01072_turning_end_material"/>
+      <!-- Link dc15_a01_case_b_dummy -->
+      <body name="dc15_a01_case_b_dummy" pos="4.85723e-17 -5.60663e-19 0.0339" quat="7.85046e-17 -0.707107 -0.707107 -7.85046e-17">
+        <!-- Joint from pp01071_turning_bowl to dc15_a01_case_b_dummy -->
+        <joint axis="0 0 1" name="all_yaw" type="hinge" class="chosen_actuator"/>
+        <inertial pos="-1.60377e-06 0.00140187 -0.0329593" mass="0.443795" fullinertia="0.00159333 0.0015483 0.00187065 2.80397e-07 1.98452e-07 4.26838e-05"/>
+        <!-- Part pp01065_stewart_side_plate -->
+        <geom type="mesh" class="visual" pos="-3.43031e-17 -4.85723e-17 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="pp01065_stewart_side_plate" material="pp01065_stewart_side_plate_material"/>
+        <!-- Part pp01067_bottom_body -->
+        <geom type="mesh" class="visual" pos="1.38767e-16 1.83881e-16 -0.20055" quat="9.16593e-17 1 -3.29597e-17 5.68353e-17" mesh="pp01067_bottom_body" material="pp01067_bottom_body_material"/>
+        <!-- Part dc15_a01_case_m_dummy -->
+        <geom type="mesh" class="visual" pos="-1.4706e-16 -8.67362e-17 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_case_m_dummy" material="dc15_a01_case_m_dummy_material"/>
+        <!-- Part dc15_a01_case_f_dummy -->
+        <geom type="mesh" class="visual" pos="0.0205 6.93889e-18 -0.00705" quat="3.33067e-16 3.64292e-17 1 -4.44089e-16" mesh="dc15_a01_case_f_dummy" material="dc15_a01_case_f_dummy_material"/>
+        <!-- Part dc15_a01_horn_dummy -->
+        <geom type="mesh" class="visual" pos="0.0205 1.73472e-17 -0.00705" quat="3.33067e-16 3.64292e-17 1 -4.44089e-16" mesh="dc15_a01_horn_dummy" material="dc15_a01_horn_dummy_material"/>
+        <!-- Part dc15_a01_horn_dummy_2 -->
+        <geom type="mesh" class="visual" pos="-0.01025 0.0177535 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_horn_dummy" material="dc15_a01_horn_dummy_material"/>
+        <!-- Part wj_wk00_0122topcabinetcase_95__configuration_default -->
+        <geom type="mesh" class="visual" pos="6.12843e-18 -0.0255 -0.00945" quat="0.5 0.5 -0.5 -0.5" mesh="wj_wk00_0122topcabinetcase_95__configuration_default" material="wj_wk00_0122topcabinetcase_95__configuration_default_material"/>
+        <!-- Part dc15_a01_case_m_dummy_2 -->
+        <geom type="mesh" class="visual" pos="-2.56295e-17 3.1225e-17 -0.00705" quat="1.11022e-16 2.60209e-17 1 -2.22045e-16" mesh="dc15_a01_case_m_dummy" material="dc15_a01_case_m_dummy_material"/>
+        <!-- Part dc15_a01_case_b_dummy -->
+        <geom type="mesh" class="visual" pos="-0.01025 -0.0177535 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_case_b_dummy" material="dc15_a01_case_b_dummy_material"/>
+        <!-- Part dc15_a01_case_b_dummy_2 -->
+        <geom type="mesh" class="visual" pos="0.0205 3.46945e-18 -0.00705" quat="3.33067e-16 3.64292e-17 1 -4.44089e-16" mesh="dc15_a01_case_b_dummy" material="dc15_a01_case_b_dummy_material"/>
+        <!-- Part pp01068_top_body -->
+        <geom type="mesh" class="collision" contype="1" conaffinity="1" pos="-8.55341e-16 -3.12493e-14 -0.20005" quat="9.19752e-14 1 -3.64292e-17 -2.66929e-15" mesh="pp01068_top_body" material="pp01068_top_body_material"/>
+        <!-- Part dc15_a01_case_f_dummy_2 -->
+        <geom type="mesh" class="visual" pos="-3.43031e-17 -3.46945e-18 -0.00705" quat="3.33067e-16 3.64292e-17 1 -4.44089e-16" mesh="dc15_a01_case_f_dummy" material="dc15_a01_case_f_dummy_material"/>
+        <!-- Part drive_palonier__configuration_default -->
+        <geom type="mesh" class="visual" pos="4.39371e-18 -0.0255 -0.00945" quat="0.5 0.5 -0.5 -0.5" mesh="drive_palonier__configuration_default" material="drive_palonier__configuration_default_material"/>
+        <!-- Part dc15_a01_case_f_dummy_3 -->
+        <geom type="mesh" class="visual" pos="-0.01025 0.0177535 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_case_f_dummy" material="dc15_a01_case_f_dummy_material"/>
+        <!-- Part dc15_a01_case_m_dummy_3 -->
+        <geom type="mesh" class="visual" pos="-3.43031e-17 -6.245e-17 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_case_m_dummy" material="dc15_a01_case_m_dummy_material"/>
+        <!-- Part dc15_a01_case_b_dummy_3 -->
+        <geom type="mesh" class="visual" pos="-9.1549e-17 -6.245e-17 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_case_b_dummy" material="dc15_a01_case_b_dummy_material"/>
+        <!-- Part wj_wk00_0123middlecase_56__configuration_default -->
+        <geom type="mesh" class="visual" pos="-1.75347e-05 -0.0254895 -0.00945" quat="0.5 0.5 -0.5 -0.5" mesh="wj_wk00_0123middlecase_56__configuration_default" material="wj_wk00_0123middlecase_56__configuration_default_material"/>
+        <!-- Part dc15_a01_case_b_dummy_4 -->
+        <geom type="mesh" class="visual" pos="-3.60378e-17 -1.04083e-17 -0.00705" quat="3.33067e-16 3.64292e-17 1 -4.44089e-16" mesh="dc15_a01_case_b_dummy" material="dc15_a01_case_b_dummy_material"/>
+        <!-- Part dc15_a01_case_f_dummy_4 -->
+        <geom type="mesh" class="visual" pos="-2.26857e-16 -1.00614e-16 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_case_f_dummy" material="dc15_a01_case_f_dummy_material"/>
+        <!-- Part dc15_a01_case_b_dummy_5 -->
+        <geom type="mesh" class="visual" pos="-0.01025 0.0177535 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_case_b_dummy" material="dc15_a01_case_b_dummy_material"/>
+        <!-- Part dc15_a01_horn_dummy_3 -->
+        <geom type="mesh" class="visual" pos="-1.15835e-16 -6.93889e-17 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_horn_dummy" material="dc15_a01_horn_dummy_material"/>
+        <!-- Part dc15_a01_horn_dummy_4 -->
+        <geom type="mesh" class="visual" pos="-0.01025 -0.0177535 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_horn_dummy" material="dc15_a01_horn_dummy_material"/>
+        <!-- Part dc15_a01_case_m_dummy_4 -->
+        <geom type="mesh" class="visual" pos="-0.01025 0.0177535 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_case_m_dummy" material="dc15_a01_case_m_dummy_material"/>
+        <!-- Part pp01065_stewart_side_plate_2 -->
+        <geom type="mesh" class="visual" pos="-1.34864e-17 6.59195e-17 -0.00705" quat="0 3.64292e-17 1 0" mesh="pp01065_stewart_side_plate" material="pp01065_stewart_side_plate_material"/>
+        <!-- Part dc15_a01_case_b_dummy_6 -->
+        <geom type="mesh" class="visual" pos="-6.37934e-17 -3.46945e-17 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_case_b_dummy" material="dc15_a01_case_b_dummy_material"/>
+        <!-- Part pp01066_stewart_mid_plate -->
+        <geom type="mesh" class="visual" pos="-8.28754e-17 -1.04083e-17 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="pp01066_stewart_mid_plate" material="pp01066_stewart_mid_plate_material"/>
+        <!-- Part dc15_a01_case_f_dummy_5 -->
+        <geom type="mesh" class="visual" pos="-3.77726e-17 -4.85723e-17 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_case_f_dummy" material="dc15_a01_case_f_dummy_material"/>
+        <!-- Part dc15_a01_horn_dummy_5 -->
+        <geom type="mesh" class="visual" pos="-5.51198e-17 -4.85723e-17 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_horn_dummy" material="dc15_a01_horn_dummy_material"/>
+        <!-- Part dc15_a01_case_m_dummy_5 -->
+        <geom type="mesh" class="visual" pos="0.0205 3.81639e-17 -0.00705" quat="1.11022e-16 2.60209e-17 1 -2.22045e-16" mesh="dc15_a01_case_m_dummy" material="dc15_a01_case_m_dummy_material"/>
+        <!-- Part wj_wk00_0124bottomcase_45__configuration_default -->
+        <geom type="mesh" class="visual" pos="-1.04636e-05 -0.0254966 -0.00945" quat="0.5 0.5 -0.5 -0.5" mesh="wj_wk00_0124bottomcase_45__configuration_default" material="wj_wk00_0124bottomcase_45__configuration_default_material"/>
+        <!-- Part dc15_a01_horn_dummy_6 -->
+        <geom type="mesh" class="visual" pos="-3.25684e-17 1.04083e-17 -0.00705" quat="3.33067e-16 3.64292e-17 1 -4.44089e-16" mesh="dc15_a01_horn_dummy" material="dc15_a01_horn_dummy_material"/>
+        <!-- Part dc15_a01_case_m_dummy_6 -->
+        <geom type="mesh" class="visual" pos="-0.01025 -0.0177535 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_case_m_dummy" material="dc15_a01_case_m_dummy_material"/>
+        <!-- Part pp01064_stewart_main_plate -->
+        <geom type="mesh" class="visual" pos="-2.73642e-17 -1.38778e-17 -0.00705" quat="1.11022e-16 -8.67362e-18 -1 -1.11022e-16" mesh="pp01064_stewart_main_plate" material="pp01064_stewart_main_plate_material"/>
+        <!-- Part dc15_a01_case_f_dummy_6 -->
+        <geom type="mesh" class="visual" pos="-0.01025 -0.0177535 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_case_f_dummy" material="dc15_a01_case_f_dummy_material"/>
+        <!-- Part pp01065_stewart_side_plate_3 -->
+        <geom type="mesh" class="visual" pos="-6.20587e-17 -1.04083e-17 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="pp01065_stewart_side_plate" material="pp01065_stewart_side_plate_material"/>
+        <!-- Link pp01062_stewart_arm -->
+        <body name="pp01062_stewart_arm" pos="0.0317603 0.0301725 -0.03705" quat="0.353553 -0.353553 -0.612372 0.612372">
+          <!-- Joint from dc15_a01_case_b_dummy to pp01062_stewart_arm -->
+          <joint axis="0 0 1" name="1" type="hinge" range="-1.3962634015954645 1.3962634015954627" class="chosen_actuator"/>
+          <inertial pos="0.0181787 2.77556e-17 0.00350559" mass="0.00272417" fullinertia="4.89963e-08 6.75761e-07 6.91798e-07 -2.63422e-25 -7.04811e-08 2.66043e-25"/>
+          <!-- Part ball -->
+          <geom type="mesh" class="visual" pos="0.038 2.77556e-17 0.00768" quat="0 1 0 0" mesh="ball" material="ball_material"/>
+          <!-- Part pp01062_stewart_arm -->
+          <geom type="mesh" class="visual" pos="-0.01025 0.03 0.0425915" quat="0.707107 0.707107 -1.11022e-16 8.49837e-17" mesh="pp01062_stewart_arm" material="pp01062_stewart_arm_material"/>
+          <!-- Link shape -->
+          <body name="shape" pos="0.038 2.77556e-17 0.00768" quat="0.0274567 -0.855096 0.517739 -0.00153082">
+            <!-- Joint from pp01062_stewart_arm to shape -->
+            <joint axis="0 0 1" name="passive_1" type="ball" class="chosen_actuator"/>
+            <inertial pos="-0.0450002 -2.17877e-17 -4.85069e-17" mass="0.00355525" fullinertia="1.7732e-08 3.06049e-06 3.0658e-06 3.75919e-22 -8.93073e-21 -1.00288e-21"/>
+            <!-- Part shape -->
+            <geom type="mesh" class="visual" pos="6.93889e-18 -1.38778e-17 0" quat="1 1.53003e-17 -1.11022e-16 -1.249e-16" mesh="shape" material="shape_material"/>
+            <!-- Part pp00xxx_stewart_rod -->
+            <geom type="mesh" class="visual" pos="0.00371031 0.0370326 -0.0564076" quat="0.855513 0.00634803 -0.517727 0.00384161" mesh="pp00xxx_stewart_rod" material="pp00xxx_stewart_rod_material"/>
+            <!-- Part shape_2 -->
+            <geom type="mesh" class="visual" pos="-0.0900003 -2.77556e-17 -8.32667e-17" quat="1.66533e-16 1.57422e-17 -1 1.11022e-16" mesh="shape" material="shape_material"/>
+            <!-- Frame closing_1_1 -->
+            <site group="3" name="closing_1_1" pos="-0.0900003 -2.77556e-17 -8.32667e-17" quat="1.66533e-16 1.57422e-17 -1 1.11022e-16"/>
+          </body>
+        </body>
+        <!-- Link pp01062_stewart_arm_2 -->
+        <body name="pp01062_stewart_arm_2" pos="0.0420103 0.012419 -0.03705" quat="0.612372 0.612372 -0.353553 -0.353553">
+          <!-- Joint from dc15_a01_case_b_dummy to pp01062_stewart_arm_2 -->
+          <joint axis="0 0 1" name="2" type="hinge" range="-1.3962634015954662 1.396263401595461" class="chosen_actuator"/>
+          <inertial pos="0.0181787 2.34834e-18 0.00350559" mass="0.00272417" fullinertia="4.89963e-08 6.75761e-07 6.91798e-07 2.48336e-22 -7.04811e-08 5.24015e-23"/>
+          <!-- Part ball_2 -->
+          <geom type="mesh" class="visual" pos="0.038 -1.38778e-17 0.00768" quat="1.66533e-16 -2.4037e-17 -1 -1.11022e-16" mesh="ball" material="ball_material"/>
+          <!-- Part pp01062_stewart_arm_2 -->
+          <geom type="mesh" class="visual" pos="-0.01025 0.03 0.0425915" quat="0.707107 0.707107 -1.11022e-16 8.24487e-17" mesh="pp01062_stewart_arm" material="pp01062_stewart_arm_material"/>
+          <!-- Link shape_2 -->
+          <body name="shape_2" pos="0.038 -8.32667e-17 0.00768" quat="0.0131699 -0.855436 -0.517228 0.0230506">
+            <!-- Joint from pp01062_stewart_arm_2 to shape_2 -->
+            <joint axis="0 0 1" name="passive_2" type="ball" class="chosen_actuator"/>
+            <inertial pos="-0.0450002 -1.00413e-17 -8.56034e-18" mass="0.00355525" fullinertia="1.7732e-08 3.06049e-06 3.0658e-06 9.05427e-21 -9.31125e-21 -1.13245e-20"/>
+            <!-- Part shape_3 -->
+            <geom type="mesh" class="visual" pos="-0.0900003 -2.77556e-17 2.08167e-17" quat="2.22045e-16 2.94872e-15 1 3.85803e-15" mesh="shape" material="shape_material"/>
+            <!-- Part shape_4 -->
+            <geom type="mesh" class="visual" pos="6.93889e-18 0 6.93889e-18" quat="1 -0 -0 -4.85723e-17" mesh="shape" material="shape_material"/>
+            <!-- Part pp00xxx_stewart_rod_2 -->
+            <geom type="mesh" class="visual" pos="0.00371031 0.0370326 -0.0564076" quat="0.855513 0.00634803 -0.517727 0.00384161" mesh="pp00xxx_stewart_rod" material="pp00xxx_stewart_rod_material"/>
+            <!-- Frame closing_2_2 -->
+            <site group="3" name="closing_2_2" pos="-0.0900003 -2.77556e-17 2.08167e-17" quat="2.78561e-15 -0.707107 0.707107 2.55257e-15"/>
+          </body>
+        </body>
+        <!-- Link pp01062_stewart_arm_3 -->
+        <body name="pp01062_stewart_arm_3" pos="0.01025 -0.0425915 -0.03705" quat="0.707107 -0.707107 -1.11022e-16 1.84877e-17">
+          <!-- Joint from dc15_a01_case_b_dummy to pp01062_stewart_arm_3 -->
+          <joint axis="0 0 1" name="3" type="hinge" range="-1.3962634015954645 1.3962634015954627" class="chosen_actuator"/>
+          <inertial pos="0.0181787 -1.87132e-19 0.00350559" mass="0.00272417" fullinertia="4.89963e-08 6.75761e-07 6.91798e-07 9.85709e-24 -7.04811e-08 -1.47314e-24"/>
+          <!-- Part pp01062_stewart_arm_3 -->
+          <geom type="mesh" class="visual" pos="-0.01025 0.03 0.0425915" quat="0.707107 0.707107 -1.11022e-16 1.0222e-16" mesh="pp01062_stewart_arm" material="pp01062_stewart_arm_material"/>
+          <!-- Part ball_3 -->
+          <geom type="mesh" class="visual" pos="0.038 0 0.00768" quat="1.45894e-16 -0.707107 0.707107 5.21114e-18" mesh="ball" material="ball_material"/>
+          <!-- Link shape_3 -->
+          <body name="shape_3" pos="0.038 4.16334e-17 0.00768" quat="0.85509 -0.027647 0.0318018 -0.516764">
+            <!-- Joint from pp01062_stewart_arm_3 to shape_3 -->
+            <joint axis="0 0 1" name="passive_3" type="ball" class="chosen_actuator"/>
+            <inertial pos="-0.0450002 -4.79199e-18 4.11785e-17" mass="0.00355525" fullinertia="1.7732e-08 3.06049e-06 3.0658e-06 9.80819e-22 -7.34923e-21 1.45141e-21"/>
+            <!-- Part shape_5 -->
+            <geom type="mesh" class="visual" pos="-0.0900003 0 1.31839e-16" quat="4.16334e-16 4.4964e-17 1 -2.22045e-16" mesh="shape" material="shape_material"/>
+            <!-- Part shape_6 -->
+            <geom type="mesh" class="visual" pos="1.38778e-17 0 0" quat="1 -0 -0 2.56739e-16" mesh="shape" material="shape_material"/>
+            <!-- Part pp00xxx_stewart_rod_3 -->
+            <geom type="mesh" class="visual" pos="0.00371031 0.0370326 -0.0564076" quat="0.855513 0.00634803 -0.517727 0.00384161" mesh="pp00xxx_stewart_rod" material="pp00xxx_stewart_rod_material"/>
+            <!-- Frame closing_3_2 -->
+            <site group="3" name="closing_3_2" pos="-0.0900003 0 1.31839e-16" quat="0 4.06771e-16 -3.33067e-16 -1"/>
+          </body>
+        </body>
+        <!-- Link pp01062_stewart_arm_4 -->
+        <body name="pp01062_stewart_arm_4" pos="-0.01025 -0.0425915 -0.03705" quat="4.44089e-16 -1.5721e-15 -0.707107 -0.707107">
+          <!-- Joint from dc15_a01_case_b_dummy to pp01062_stewart_arm_4 -->
+          <joint axis="0 0 1" name="4" type="hinge" range="-1.3962634015954831 1.396263401595444" class="chosen_actuator"/>
+          <inertial pos="0.0181787 -5.24899e-19 0.00350559" mass="0.00272417" fullinertia="4.89963e-08 6.75761e-07 6.91798e-07 4.36416e-22 -7.04811e-08 3.66848e-23"/>
+          <!-- Part ball_4 -->
+          <geom type="mesh" class="visual" pos="0.038 -2.77556e-17 0.00768" quat="4.51946e-15 -0.707107 -0.707107 -1.08859e-15" mesh="ball" material="ball_material"/>
+          <!-- Part pp01062_stewart_arm_4 -->
+          <geom type="mesh" class="visual" pos="-0.01025 0.03 0.0425915" quat="0.707107 0.707107 -1.11022e-16 1.0222e-16" mesh="pp01062_stewart_arm" material="pp01062_stewart_arm_material"/>
+          <!-- Link shape_4 -->
+          <body name="shape_4" pos="0.038 -2.77556e-17 0.00768" quat="0.854409 0.0439152 0.0416306 0.516065">
+            <!-- Joint from pp01062_stewart_arm_4 to shape_4 -->
+            <joint axis="0 0 1" name="passive_4" type="ball" class="chosen_actuator"/>
+            <inertial pos="-0.0450002 1.04447e-16 -1.1361e-17" mass="0.00355525" fullinertia="1.7732e-08 3.06049e-06 3.0658e-06 6.31755e-21 -5.94417e-21 1.35905e-20"/>
+            <!-- Part shape_7 -->
+            <geom type="mesh" class="visual" pos="-0.0900003 2.08167e-16 3.46945e-18" quat="3.33067e-16 0 -1 1.66533e-16" mesh="shape" material="shape_material"/>
+            <!-- Part pp00xxx_stewart_rod_4 -->
+            <geom type="mesh" class="visual" pos="0.00371031 0.0370326 -0.0564076" quat="0.855513 0.00634803 -0.517727 0.00384161" mesh="pp00xxx_stewart_rod" material="pp00xxx_stewart_rod_material"/>
+            <!-- Part shape_8 -->
+            <geom type="mesh" class="visual" pos="6.93889e-18 0 3.46945e-18" quat="1 0 3.33067e-16 1.31839e-16" mesh="shape" material="shape_material"/>
+            <!-- Frame closing_4_1 -->
+            <site group="3" name="closing_4_1" pos="-0.0900003 2.08167e-16 3.46945e-18" quat="3.33067e-16 0 -1 1.66533e-16"/>
+          </body>
+        </body>
+        <!-- Link pp01062_stewart_arm_5 -->
+        <body name="pp01062_stewart_arm_5" pos="-0.0420103 0.012419 -0.03705" quat="0.353553 -0.353553 0.612372 -0.612372">
+          <!-- Joint from dc15_a01_case_b_dummy to pp01062_stewart_arm_5 -->
+          <joint axis="0 0 1" name="5" type="hinge" range="-1.3962634015954662 1.396263401595461" class="chosen_actuator"/>
+          <inertial pos="0.0181787 3.11301e-35 0.00350559" mass="0.00272417" fullinertia="4.89963e-08 6.75761e-07 6.91798e-07 -4.49492e-25 -7.04811e-08 -1.26741e-25"/>
+          <!-- Part pp01062_stewart_arm_5 -->
+          <geom type="mesh" class="visual" pos="-0.01025 0.03 0.0425915" quat="0.707107 0.707107 -1.11022e-16 9.55014e-17" mesh="pp01062_stewart_arm" material="pp01062_stewart_arm_material"/>
+          <!-- Part ball_5 -->
+          <geom type="mesh" class="visual" pos="0.038 0 0.00768" quat="1.38778e-17 5.55459e-17 1 3.33067e-16" mesh="ball" material="ball_material"/>
+          <!-- Link pp00xxx_stewart_rod -->
+          <body name="pp00xxx_stewart_rod" pos="0.038 1.38778e-17 0.00768" quat="0.0908372 -0.850701 0.516198 -0.0399478">
+            <!-- Joint from pp01062_stewart_arm_5 to pp00xxx_stewart_rod -->
+            <joint axis="0 0 1" name="passive_5" type="ball" class="chosen_actuator"/>
+            <inertial pos="-0.0450002 7.44667e-17 -1.14016e-17" mass="0.00355525" fullinertia="1.7732e-08 3.06049e-06 3.0658e-06 4.55471e-21 -4.43821e-21 1.23083e-21"/>
+            <!-- Part shape_9 -->
+            <geom type="mesh" class="visual" pos="0 0 0" quat="1 0 -2.22045e-16 0" mesh="shape" material="shape_material"/>
+            <!-- Part pp00xxx_stewart_rod_5 -->
+            <geom type="mesh" class="visual" pos="0.00371031 0.0370326 -0.0564076" quat="0.855513 0.00634803 -0.517727 0.00384161" mesh="pp00xxx_stewart_rod" material="pp00xxx_stewart_rod_material"/>
+            <!-- Part shape_10 -->
+            <geom type="mesh" class="visual" pos="-0.0900003 1.38778e-16 6.93889e-18" quat="9.99201e-16 -4.04899e-16 1 1.9984e-15" mesh="shape" material="shape_material"/>
+            <!-- Frame closing_5_1 -->
+            <site group="3" name="closing_5_1" pos="-0.0900003 1.38778e-16 6.93889e-18" quat="9.99201e-16 -4.04899e-16 1 1.9984e-15"/>
+          </body>
+        </body>
+        <!-- Link pp01062_stewart_arm_6 -->
+        <body name="pp01062_stewart_arm_6" pos="-0.0317603 0.0301725 -0.03705" quat="0.612372 0.612372 0.353553 0.353553">
+          <!-- Joint from dc15_a01_case_b_dummy to pp01062_stewart_arm_6 -->
+          <joint axis="0 0 1" name="6" type="hinge" range="-1.3962634015954636 1.3962634015954636" class="chosen_actuator"/>
+          <inertial pos="0.0181787 -1.69382e-17 0.00350559" mass="0.00272417" fullinertia="4.89963e-08 6.75761e-07 6.91798e-07 1.6528e-22 -7.04811e-08 3.48685e-23"/>
+          <!-- Part pp01062_stewart_arm_6 -->
+          <geom type="mesh" class="visual" pos="-0.01025 0.03 0.0425915" quat="0.707107 0.707107 -1.11022e-16 1.00194e-16" mesh="pp01062_stewart_arm" material="pp01062_stewart_arm_material"/>
+          <!-- Part ball_6 -->
+          <geom type="mesh" class="visual" pos="0.038 -2.77556e-17 0.00768" quat="0 1 0 0" mesh="ball" material="ball_material"/>
+          <!-- Link shape_5 -->
+          <body name="shape_5" pos="0.038 -2.77556e-17 0.00768" quat="0.0131438 -0.855436 -0.517229 0.0230347">
+            <!-- Joint from pp01062_stewart_arm_6 to shape_5 -->
+            <joint axis="0 0 1" name="passive_6" type="ball" class="chosen_actuator"/>
+            <inertial pos="-0.0450002 7.95049e-16 -1.5589e-17" mass="0.00355525" fullinertia="1.7732e-08 3.06049e-06 3.0658e-06 4.84585e-20 -8.92792e-21 -1.86218e-20"/>
+            <!-- Part shape_11 -->
+            <geom type="mesh" class="visual" pos="-0.0900003 1.83187e-15 6.93889e-18" quat="1.77636e-15 6.61483e-15 1 6.27276e-15" mesh="shape" material="shape_material"/>
+            <!-- Part shape_12 -->
+            <geom type="mesh" class="visual" pos="6.93889e-18 0 0" quat="1 1.00165e-17 -8.32667e-17 0" mesh="shape" material="shape_material"/>
+            <!-- Part pp00xxx_stewart_rod_6 -->
+            <geom type="mesh" class="visual" pos="0.00371031 0.0370326 -0.0564076" quat="0.855513 0.00634803 -0.517727 0.00384161" mesh="pp00xxx_stewart_rod" material="pp00xxx_stewart_rod_material"/>
+            <!-- Link pp01063_stewart_plateform -->
+            <body name="pp01063_stewart_plateform" pos="-0.0900003 1.88738e-15 -2.08167e-17" quat="0.0131438 0.855436 0.517229 -0.0230347">
+              <!-- Joint from shape_5 to pp01063_stewart_plateform -->
+              <joint axis="0 0 1" name="passive_7" type="ball" class="chosen_actuator"/>
+              <inertial pos="-0.00488914 -0.0469134 0.0291732" mass="0.168092" fullinertia="0.000413571 0.000428808 0.000324749 -2.28564e-06 -5.24367e-05 -9.58295e-07"/>
+              <!-- Part small_lens -->
+              <geom type="mesh" class="visual" pos="0.0262444 -0.0524 0.0136834" quat="0.612372 0.612372 -0.353553 0.353553" mesh="small_lens" material="small_lens_material"/>
+              <!-- Part m12_lens -->
+              <geom type="mesh" class="visual" pos="0.010421 -0.0524 -0.0147088" quat="0.183013 0.683013 -0.683013 -0.183013" mesh="m12_lens" material="m12_lens_material"/>
+              <!-- Part pp01080_back_small_eye -->
+              <geom type="mesh" class="visual" pos="0.0262444 -0.0524 0.0136834" quat="0.612372 0.612372 -0.353553 0.353553" mesh="pp01080_back_small_eye" material="pp01080_back_small_eye_material"/>
+              <!-- Part m12_lens_2 -->
+              <geom type="mesh" class="visual" pos="0.042581 -0.0524 0.0417792" quat="0.183013 0.683013 -0.683013 -0.183013" mesh="m12_lens" material="m12_lens_material"/>
+              <!-- Part ball_7 -->
+              <geom type="mesh" class="visual" pos="-0.013 5.55112e-17 -5.55112e-17" quat="1 1.3274e-15 5.77316e-15 -2.63678e-15" mesh="ball" material="ball_material"/>
+              <!-- Part uc_a37_rev_a_step -->
+              <geom type="mesh" class="visual" pos="0.0239513 -0.04265 0.0271316" quat="9.36644e-18 0.866025 0 0.5" mesh="uc_a37_rev_a_step" material="uc_a37_rev_a_step_material"/>
+              <!-- Part ball_8 -->
+              <geom type="mesh" class="visual" pos="-8.67362e-19 2.77556e-17 0" quat="1 -0 -2.04613e-16 -5.18024e-19" mesh="ball" material="ball_material"/>
+              <!-- Part pp01070_head_head_back -->
+              <geom type="mesh" class="visual" pos="-0.0134282 -0.0524 0.0365883" quat="0.612372 0.612372 -0.353553 0.353553" mesh="pp01070_head_head_back" material="pp01070_head_head_back_material"/>
+              <!-- Part ball_9 -->
+              <geom type="mesh" class="visual" pos="0.0249723 5.55112e-17 0.0432533" quat="1.36964e-17 0.866025 -1.11022e-16 -0.5" mesh="ball" material="ball_material"/>
+              <!-- Part pp01079_back_big_eye -->
+              <geom type="mesh" class="visual" pos="0.0265844 -0.0524 0.013487" quat="0.612372 0.612372 -0.353553 0.353553" mesh="pp01079_back_big_eye" material="pp01079_back_big_eye_material"/>
+              <!-- Part ball_10 -->
+              <geom type="mesh" class="visual" pos="-0.0379723 5.55112e-17 0.0432533" quat="0.353553 -0.612372 0.612372 -0.353553" mesh="ball" material="ball_material"/>
+              <!-- Part pp01069_head_shell_front -->
+              <geom type="mesh"  class="visual" pos="-0.0142942 -0.0524 0.0370883" quat="0.612372 0.612372 -0.353553 0.353553"  mesh="pp01069_head_shell_front" material="pp01069_head_shell_front_material"/>
+              <!-- <geom type="capsule" class="collision"  quat="0.612372 0.612372 -0.353553 0.353553" name="head_collider" size="0.05"/> -->
+              <geom type="capsule" class="collision" name="head_collider" size="0.048" fromto="0.005 -0.05 0.060068 -0.0257422 -0.05 0.00716666"/>
+              <!-- Part pp01078_glasses -->
+              <geom type="mesh" class="visual" pos="0.025976 -0.0524 0.0138383" quat="0.612372 0.612372 -0.353553 0.353553" mesh="pp01078_glasses" material="pp01078_glasses_material"/>
+              <!-- Part pp01063_stewart_plateform -->
+              <geom type="mesh" class="visual" pos="-0.0065 0.109722 0.0325883" quat="0.353553 0.353553 0.612372 -0.612372" mesh="pp01063_stewart_plateform" material="pp01063_stewart_plateform_material"/>
+              <!-- Part ball_11 -->
+              <geom type="mesh" class="visual" pos="-0.0314723 2.77556e-17 0.0545117" quat="0.353553 -0.612372 0.612372 -0.353553" mesh="ball" material="ball_material"/>
+              <!-- Part big_lens -->
+              <geom type="mesh" class="visual" pos="0.0281677 -0.0524 0.0125729" quat="0.612372 0.612372 -0.353553 0.353553" mesh="big_lens" material="big_lens_material"/>
+              <!-- Part ball_12 -->
+              <geom type="mesh" class="visual" pos="0.0184723 5.55112e-17 0.0545117" quat="3.07775e-17 -0.866025 -0 0.5" mesh="ball" material="ball_material"/>
+              <!-- Frame closing_1_2 -->
+              <site group="3" name="closing_1_2" pos="0.0249723 5.55112e-17 0.0432533" quat="0.353553 0.612372 -0.612372 -0.353553"/>
+              <!-- Frame closing_2_1 -->
+              <site group="3" name="closing_2_1" pos="0.0184723 5.55112e-17 0.0545117" quat="3.07775e-17 -0.866025 -0 0.5"/>
+              <!-- Frame closing_3_1 -->
+              <site group="3" name="closing_3_1" pos="-0.0314723 2.77556e-17 0.0545117" quat="0.353553 -0.612372 0.612372 -0.353553"/>
+              <!-- Frame closing_4_2 -->
+              <site group="3" name="closing_4_2" pos="-0.0379723 5.55112e-17 0.0432533" quat="0.612372 0.353553 -0.353553 -0.612372"/>
+              <!-- Frame closing_5_2 -->
+              <site group="3" name="closing_5_2" pos="-0.013 5.55112e-17 -5.55112e-17" quat="0.707107 -3.16544e-15 5.10703e-15 -0.707107"/>
+              <!-- Frame head -->
+              <site group="3" name="head" pos="-0.0134282 -0.0012 0.0365883" quat="0.683013 0.683013 0.183013 -0.183013"/>
+              <!-- Frame camera -->
+              <body pos="0.0244171 -0.0524 0.0147383" quat="-0.000 -0.500 0.000 0.866">
+                <site group="3" name="camera"/>
+                <camera name="eye_camera"
+                 mode="fixed"
+                 resolution="1280 720"
+                />
+
+              </body>
+              <!-- Frame camera_optical -->
+              <site group="3" name="camera_optical" pos="0.0244171 -0.0524 0.0147383" quat="0.5 0 0.866025 -2.22045e-16"/>
+              <!-- Link test_antenna_body -->
+              <body name="test_antenna_body" pos="-0.0161053 -0.0744 0.100068" quat="0.964418 0.0356203 -0.25955 0.0355422">
+                <!-- Joint from pp01063_stewart_plateform to test_antenna_body -->
+                <joint axis="0 0 1" name="left_antenna" type="hinge" class="chosen_actuator"/>
+                <inertial pos="0.00147983 -0.0294086 0.00298611" mass="0.00456766" fullinertia="1.38179e-05 2.28739e-07 1.38227e-05 6.85535e-07 5.01796e-08 -9.97217e-07"/>
+                <!-- Part test_antenna -->
+                <geom type="mesh" class="visual" pos="0.0285777 0.0175124 -0.0571006" quat="0.716727 0.647969 0.191203 -0.172861" mesh="test_antenna" material="test_antenna_material"/>
+                <!-- Part test_antenna_body -->
+                <geom type="mesh" class="visual" pos="0.0285297 0.0184653 -0.0570171" quat="0.716727 0.647969 0.191203 -0.172861" mesh="test_antenna_body" material="test_antenna_body_material"/>
+              </body>
+              <!-- Link test_antenna -->
+              <body name="test_antenna" pos="-0.0697422 -0.0744 0.00716666" quat="0.706492 0.0909539 -0.701233 -0.0294913">
+                <!-- Joint from pp01063_stewart_plateform to test_antenna -->
+                <joint axis="0 0 1" name="right_antenna" type="hinge" class="chosen_actuator"/>
+                <inertial pos="0.00147983 -0.0294086 0.00298611" mass="0.00456766" fullinertia="1.38179e-05 2.28739e-07 1.38227e-05 6.85535e-07 5.01796e-08 -9.97217e-07"/>
+                <!-- Part test_antenna_2 -->
+                <geom type="mesh" class="visual" pos="0.0285777 0.0175124 -0.0571006" quat="0.716727 0.647969 0.191203 -0.172861" mesh="test_antenna" material="test_antenna_material"/>
+                <!-- Part test_antenna_body_2 -->
+                <geom type="mesh" class="visual" pos="0.0285297 0.0184653 -0.0570171" quat="0.716727 0.647969 0.191203 -0.172861" mesh="test_antenna_body" material="test_antenna_body_material"/>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+    </body>
+  </worldbody>
+  <asset>
+    <mesh file="dc15_a01_case_b_dummy.stl"/>
+    <mesh file="pp01078_glasses.stl"/>
+    <mesh file="big_lens.stl"/>
+    <mesh file="test_antenna_body.stl"/>
+    <mesh file="pp01065_stewart_side_plate.stl"/>
+    <mesh file="small_lens.stl"/>
+    <mesh file="pp01080_back_small_eye.stl"/>
+    <mesh file="pp01064_stewart_main_plate.stl"/>
+    <mesh file="shape.stl"/>
+    <mesh file="pp00xxx_stewart_rod.stl"/>
+    <mesh file="pp01079_back_big_eye.stl"/>
+    <mesh file="ball.stl"/>
+    <mesh file="wj_wk00_0123middlecase_56__configuration_default.stl"/>
+    <mesh file="test_antenna.stl"/>
+    <mesh file="pp01070_head_head_back.stl"/>
+    <mesh file="dc15_a01_case_f_dummy.stl"/>
+    <mesh file="drive_palonier__configuration_default.stl"/>
+    <mesh file="dc15_a01_horn_dummy.stl"/>
+    <mesh file="wj_wk00_0122topcabinetcase_95__configuration_default.stl"/>
+    <mesh file="pp01071_turning_bowl.stl"/>
+    <mesh file="pp01068_top_body.stl"/>
+    <mesh file="wj_wk00_0124bottomcase_45__configuration_default.stl"/>
+    <mesh file="uc_a37_rev_a_step.stl"/>
+    <mesh file="dc15_a01_case_m_dummy.stl"/>
+    <mesh file="pp01062_stewart_arm.stl"/>
+    <mesh file="pp01072_turning_end.stl"/>
+    <mesh file="m12_lens.stl"/>
+    <mesh file="pp01069_head_shell_front.stl"/>
+    <mesh file="pp01063_stewart_plateform.stl"/>
+    <mesh file="pp01066_stewart_mid_plate.stl"/>
+    <mesh file="pp01067_bottom_body.stl"/>
+    <material name="pp01071_turning_bowl_material" rgba="0.34902 0.376471 0.4 1"/>
+    <material name="pp01072_turning_end_material" rgba="0.262745 0.282353 0.301961 1"/>
+    <material name="pp01065_stewart_side_plate_material" rgba="0.196078 0.196078 0.196078 1"/>
+    <material name="pp01067_bottom_body_material" rgba="1 1 1 1"/>
+    <material name="dc15_a01_case_m_dummy_material" rgba="0.262745 0.282353 0.301961 1"/>
+    <material name="dc15_a01_case_f_dummy_material" rgba="0.262745 0.282353 0.301961 1"/>
+    <material name="dc15_a01_horn_dummy_material" rgba="0.262745 0.282353 0.301961 1"/>
+    <material name="wj_wk00_0122topcabinetcase_95__configuration_default_material" rgba="0.247059 0.243137 0.243137 1"/>
+    <material name="dc15_a01_case_b_dummy_material" rgba="0.262745 0.282353 0.301961 1"/>
+    <material name="pp01068_top_body_material" rgba="1 1 1 1"/>
+    <material name="drive_palonier__configuration_default_material" rgba="0.623529 0.666667 0.701961 1"/>
+    <material name="wj_wk00_0123middlecase_56__configuration_default_material" rgba="0.247059 0.243137 0.243137 1"/>
+    <material name="pp01066_stewart_mid_plate_material" rgba="0.196078 0.196078 0.196078 1"/>
+    <material name="wj_wk00_0124bottomcase_45__configuration_default_material" rgba="0.247059 0.243137 0.243137 1"/>
+    <material name="pp01064_stewart_main_plate_material" rgba="0.196078 0.196078 0.196078 1"/>
+    <material name="ball_material" rgba="0.901961 0.901961 0.901961 1"/>
+    <material name="pp01062_stewart_arm_material" rgba="0.196078 0.196078 0.196078 1"/>
+    <material name="shape_material" rgba="0.901961 0.901961 0.901961 1"/>
+    <material name="pp00xxx_stewart_rod_material" rgba="0.8 0.8 0.8 1"/>
+    <material name="small_lens_material" rgba="0.439216 0.47451 0.501961 0.298039"/>
+    <material name="m12_lens_material" rgba="0.262745 0.282353 0.301961 1"/>
+    <material name="pp01080_back_small_eye_material" rgba="0.168627 0.168627 0.168627 1"/>
+    <material name="uc_a37_rev_a_step_material" rgba="0.25098 0.25098 0.25098 1"/>
+    <material name="pp01070_head_head_back_material" rgba="1 1 1 1"/>
+    <material name="pp01079_back_big_eye_material" rgba="0.168627 0.168627 0.168627 1"/>
+    <material name="pp01069_head_shell_front_material" rgba="1 1 1 1"/>
+    <material name="pp01078_glasses_material" rgba="0.168627 0.168627 0.168627 1"/>
+    <material name="pp01063_stewart_plateform_material" rgba="1 1 1 1"/>
+    <material name="big_lens_material" rgba="0.439216 0.47451 0.501961 0.298039"/>
+    <material name="test_antenna_material" rgba="0.262745 0.282353 0.301961 1"/>
+    <material name="test_antenna_body_material" rgba="1 1 1 1"/>
+  </asset>
+  <actuator>
+    <position class="chosen_actuator" name="all_yaw" joint="all_yaw" />
+    <position class="chosen_actuator" name="1" joint="1" inheritrange="1"/>
+    <position class="chosen_actuator" name="2" joint="2" inheritrange="1"/>
+    <position class="chosen_actuator" name="3" joint="3" inheritrange="1"/>
+    <position class="chosen_actuator" name="4" joint="4" inheritrange="1"/>
+    <position class="chosen_actuator" name="5" joint="5" inheritrange="1"/>
+    <position class="chosen_actuator" name="6" joint="6" inheritrange="1"/>
+    <position class="chosen_actuator" name="left_antenna" joint="left_antenna"/>
+    <position class="chosen_actuator" name="right_antenna" joint="right_antenna"/>
+  </actuator>
+  <equality>
+    <connect site1="closing_1_1" site2="closing_1_2"/>
+    <connect site1="closing_2_1" site2="closing_2_2"/>
+    <connect site1="closing_3_1" site2="closing_3_2"/>
+    <connect site1="closing_4_1" site2="closing_4_2"/>
+    <connect site1="closing_5_1" site2="closing_5_2"/>
+  </equality>
+</mujoco>
diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/reachy_mini copy.xml b/src/reachy_mini/descriptions/reachy_mini/mjcf/reachy_mini copy.xml
new file mode 100644
index 00000000..a97dbc09
--- /dev/null
+++ b/src/reachy_mini/descriptions/reachy_mini/mjcf/reachy_mini copy.xml	
@@ -0,0 +1,448 @@
+<?xml version="1.0" ?>
+<!-- Generated using onshape-to-robot -->
+<!-- Onshape https://cad.onshape.com/documents/e305b26a84339af20c593973/w/ec7765eccdaa9a5960507125/e/251023ea04190ab16854eb80 -->
+<mujoco model="reachy_mini">
+  <compiler angle="radian" meshdir="assets" autolimits="true"/>
+  <default>
+    <default class="reachy_mini">
+      <joint frictionloss="0.1" armature="0.005"/>
+      <position kp="50" dampratio="1"/>
+      <default class="visual">
+        <geom type="mesh" contype="0" conaffinity="0" group="2"/>
+      </default>
+      <default class="collision">
+        <geom 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.1" forcerange="-20.0 20.0"/>
+      <!-- <position kp="10.0" kv="0.0" forcerange="-10.0 10.0"/> -->
+      <default class="chosen_actuator"/>
+    </default>
+    <default class="xc330m288t">
+      <!-- probably wrong, would need to re-identify -->
+      <geom contype="0" conaffinity="0"/>
+      <joint damping="0.095" frictionloss="0.058" armature="0.0018"/>
+      <position kp="2.54" kv="0.0" forcerange="-0.8 0.8"/>
+    </default>
+    <default class="sts3215_345">
+      <!-- Standard sts3215 with 1/345 gear ratio -->
+      <!-- Confident that this is realistic (mini duck walks with these values) -->
+      <geom contype="0" conaffinity="0"/>
+      <joint damping="0.56" frictionloss="0.068" armature="0.027"/>
+      <position kp="17.11" kv="0.0" forcerange="-3.23 3.23"/>
+    </default>
+    <default class="sts3215_147">
+      <!-- sts3215 with 1/147 gear ratio -->
+      <!-- (Estimation based on the gear ratio difference) -->
+      <geom contype="0" conaffinity="0"/>
+      <joint damping="0.1" frictionloss="0.028" armature="0.011"/>
+      <position kp="17.11" kv="0.0" forcerange="-1.35 1.35"/>
+    </default>
+  </default>
+  <!-- Additional additional.xml -->
+  <contact>
+    <!-- <exclude name="exclude0" body1="torso" body2="sts32155"/> -->
+  </contact>
+  <default>
+    <equality solref="0.002 1" solimp="0.99 0.999 0.0005 0.5 2"/>
+  </default>
+  <worldbody>
+    <!-- Link pp01071_turning_bowl -->
+    <body name="pp01071_turning_bowl" pos="0 0 0" quat="1 0 0 0" childclass="reachy_mini">
+      <inertial pos="0.000667521 1.7878e-09 0.0118411" mass="0.0947234" fullinertia="0.000111359 0.000106563 0.000196298 1.02485e-11 8.96229e-07 4.95917e-13"/>
+      <!-- Part pp01071_turning_bowl -->
+      <geom type="mesh" class="visual" pos="-1.38778e-17 0 0.2345" quat="0.707107 0 -6.16298e-32 0.707107" mesh="pp01071_turning_bowl" material="pp01071_turning_bowl_material"/>
+      <!-- Part pp01072_turning_end -->
+      <geom type="mesh" class="visual" pos="3.46945e-17 -4.51028e-17 0.2345" quat="0.707107 7.85046e-17 -2.22045e-16 0.707107" mesh="pp01072_turning_end" material="pp01072_turning_end_material"/>
+      <!-- Link dc15_a01_case_b_dummy -->
+      <body name="dc15_a01_case_b_dummy" pos="4.85723e-17 -5.60663e-19 0.0339" quat="7.85046e-17 -0.707107 -0.707107 -7.85046e-17">
+        <!-- Joint from pp01071_turning_bowl to dc15_a01_case_b_dummy -->
+        <joint axis="0 0 1" name="all_yaw" type="hinge" class="chosen_actuator"/>
+        <inertial pos="-1.60377e-06 0.00140187 -0.0329593" mass="0.443795" fullinertia="0.00159333 0.0015483 0.00187065 2.80397e-07 1.98452e-07 4.26838e-05"/>
+        <!-- Part pp01065_stewart_side_plate -->
+        <geom type="mesh" class="visual" pos="-3.43031e-17 -4.85723e-17 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="pp01065_stewart_side_plate" material="pp01065_stewart_side_plate_material"/>
+        <!-- Part pp01067_bottom_body -->
+        <geom type="mesh" class="visual" pos="1.38767e-16 1.83881e-16 -0.20055" quat="9.16593e-17 1 -3.29597e-17 5.68353e-17" mesh="pp01067_bottom_body" material="pp01067_bottom_body_material"/>
+        <!-- Part dc15_a01_case_m_dummy -->
+        <geom type="mesh" class="visual" pos="-1.4706e-16 -8.67362e-17 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_case_m_dummy" material="dc15_a01_case_m_dummy_material"/>
+        <!-- Part dc15_a01_case_f_dummy -->
+        <geom type="mesh" class="visual" pos="0.0205 6.93889e-18 -0.00705" quat="3.33067e-16 3.64292e-17 1 -4.44089e-16" mesh="dc15_a01_case_f_dummy" material="dc15_a01_case_f_dummy_material"/>
+        <!-- Part dc15_a01_horn_dummy -->
+        <geom type="mesh" class="visual" pos="0.0205 1.73472e-17 -0.00705" quat="3.33067e-16 3.64292e-17 1 -4.44089e-16" mesh="dc15_a01_horn_dummy" material="dc15_a01_horn_dummy_material"/>
+        <!-- Part dc15_a01_horn_dummy_2 -->
+        <geom type="mesh" class="visual" pos="-0.01025 0.0177535 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_horn_dummy" material="dc15_a01_horn_dummy_material"/>
+        <!-- Part wj_wk00_0122topcabinetcase_95__configuration_default -->
+        <geom type="mesh" class="visual" pos="6.12843e-18 -0.0255 -0.00945" quat="0.5 0.5 -0.5 -0.5" mesh="wj_wk00_0122topcabinetcase_95__configuration_default" material="wj_wk00_0122topcabinetcase_95__configuration_default_material"/>
+        <!-- Part dc15_a01_case_m_dummy_2 -->
+        <geom type="mesh" class="visual" pos="-2.56295e-17 3.1225e-17 -0.00705" quat="1.11022e-16 2.60209e-17 1 -2.22045e-16" mesh="dc15_a01_case_m_dummy" material="dc15_a01_case_m_dummy_material"/>
+        <!-- Part dc15_a01_case_b_dummy -->
+        <geom type="mesh" class="visual" pos="-0.01025 -0.0177535 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_case_b_dummy" material="dc15_a01_case_b_dummy_material"/>
+        <!-- Part dc15_a01_case_b_dummy_2 -->
+        <geom type="mesh" class="visual" pos="0.0205 3.46945e-18 -0.00705" quat="3.33067e-16 3.64292e-17 1 -4.44089e-16" mesh="dc15_a01_case_b_dummy" material="dc15_a01_case_b_dummy_material"/>
+        <!-- Part pp01068_top_body -->
+        <geom type="mesh" class="visual" pos="-8.55341e-16 -3.12493e-14 -0.20005" quat="9.19752e-14 1 -3.64292e-17 -2.66929e-15" mesh="pp01068_top_body" material="pp01068_top_body_material"/>
+        <!-- Part dc15_a01_case_f_dummy_2 -->
+        <geom type="mesh" class="visual" pos="-3.43031e-17 -3.46945e-18 -0.00705" quat="3.33067e-16 3.64292e-17 1 -4.44089e-16" mesh="dc15_a01_case_f_dummy" material="dc15_a01_case_f_dummy_material"/>
+        <!-- Part drive_palonier__configuration_default -->
+        <geom type="mesh" class="visual" pos="4.39371e-18 -0.0255 -0.00945" quat="0.5 0.5 -0.5 -0.5" mesh="drive_palonier__configuration_default" material="drive_palonier__configuration_default_material"/>
+        <!-- Part dc15_a01_case_f_dummy_3 -->
+        <geom type="mesh" class="visual" pos="-0.01025 0.0177535 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_case_f_dummy" material="dc15_a01_case_f_dummy_material"/>
+        <!-- Part dc15_a01_case_m_dummy_3 -->
+        <geom type="mesh" class="visual" pos="-3.43031e-17 -6.245e-17 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_case_m_dummy" material="dc15_a01_case_m_dummy_material"/>
+        <!-- Part dc15_a01_case_b_dummy_3 -->
+        <geom type="mesh" class="visual" pos="-9.1549e-17 -6.245e-17 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_case_b_dummy" material="dc15_a01_case_b_dummy_material"/>
+        <!-- Part wj_wk00_0123middlecase_56__configuration_default -->
+        <geom type="mesh" class="visual" pos="-1.75347e-05 -0.0254895 -0.00945" quat="0.5 0.5 -0.5 -0.5" mesh="wj_wk00_0123middlecase_56__configuration_default" material="wj_wk00_0123middlecase_56__configuration_default_material"/>
+        <!-- Part dc15_a01_case_b_dummy_4 -->
+        <geom type="mesh" class="visual" pos="-3.60378e-17 -1.04083e-17 -0.00705" quat="3.33067e-16 3.64292e-17 1 -4.44089e-16" mesh="dc15_a01_case_b_dummy" material="dc15_a01_case_b_dummy_material"/>
+        <!-- Part dc15_a01_case_f_dummy_4 -->
+        <geom type="mesh" class="visual" pos="-2.26857e-16 -1.00614e-16 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_case_f_dummy" material="dc15_a01_case_f_dummy_material"/>
+        <!-- Part dc15_a01_case_b_dummy_5 -->
+        <geom type="mesh" class="visual" pos="-0.01025 0.0177535 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_case_b_dummy" material="dc15_a01_case_b_dummy_material"/>
+        <!-- Part dc15_a01_horn_dummy_3 -->
+        <geom type="mesh" class="visual" pos="-1.15835e-16 -6.93889e-17 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_horn_dummy" material="dc15_a01_horn_dummy_material"/>
+        <!-- Part dc15_a01_horn_dummy_4 -->
+        <geom type="mesh" class="visual" pos="-0.01025 -0.0177535 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_horn_dummy" material="dc15_a01_horn_dummy_material"/>
+        <!-- Part dc15_a01_case_m_dummy_4 -->
+        <geom type="mesh" class="visual" pos="-0.01025 0.0177535 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_case_m_dummy" material="dc15_a01_case_m_dummy_material"/>
+        <!-- Part pp01065_stewart_side_plate_2 -->
+        <geom type="mesh" class="visual" pos="-1.34864e-17 6.59195e-17 -0.00705" quat="0 3.64292e-17 1 0" mesh="pp01065_stewart_side_plate" material="pp01065_stewart_side_plate_material"/>
+        <!-- Part dc15_a01_case_b_dummy_6 -->
+        <geom type="mesh" class="visual" pos="-6.37934e-17 -3.46945e-17 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_case_b_dummy" material="dc15_a01_case_b_dummy_material"/>
+        <!-- Part pp01066_stewart_mid_plate -->
+        <geom type="mesh" class="visual" pos="-8.28754e-17 -1.04083e-17 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="pp01066_stewart_mid_plate" material="pp01066_stewart_mid_plate_material"/>
+        <!-- Part dc15_a01_case_f_dummy_5 -->
+        <geom type="mesh" class="visual" pos="-3.77726e-17 -4.85723e-17 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_case_f_dummy" material="dc15_a01_case_f_dummy_material"/>
+        <!-- Part dc15_a01_horn_dummy_5 -->
+        <geom type="mesh" class="visual" pos="-5.51198e-17 -4.85723e-17 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_horn_dummy" material="dc15_a01_horn_dummy_material"/>
+        <!-- Part dc15_a01_case_m_dummy_5 -->
+        <geom type="mesh" class="visual" pos="0.0205 3.81639e-17 -0.00705" quat="1.11022e-16 2.60209e-17 1 -2.22045e-16" mesh="dc15_a01_case_m_dummy" material="dc15_a01_case_m_dummy_material"/>
+        <!-- Part wj_wk00_0124bottomcase_45__configuration_default -->
+        <geom type="mesh" class="visual" pos="-1.04636e-05 -0.0254966 -0.00945" quat="0.5 0.5 -0.5 -0.5" mesh="wj_wk00_0124bottomcase_45__configuration_default" material="wj_wk00_0124bottomcase_45__configuration_default_material"/>
+        <!-- Part dc15_a01_horn_dummy_6 -->
+        <geom type="mesh" class="visual" pos="-3.25684e-17 1.04083e-17 -0.00705" quat="3.33067e-16 3.64292e-17 1 -4.44089e-16" mesh="dc15_a01_horn_dummy" material="dc15_a01_horn_dummy_material"/>
+        <!-- Part dc15_a01_case_m_dummy_6 -->
+        <geom type="mesh" class="visual" pos="-0.01025 -0.0177535 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_case_m_dummy" material="dc15_a01_case_m_dummy_material"/>
+        <!-- Part pp01064_stewart_main_plate -->
+        <geom type="mesh" class="visual" pos="-2.73642e-17 -1.38778e-17 -0.00705" quat="1.11022e-16 -8.67362e-18 -1 -1.11022e-16" mesh="pp01064_stewart_main_plate" material="pp01064_stewart_main_plate_material"/>
+        <!-- Part dc15_a01_case_f_dummy_6 -->
+        <geom type="mesh" class="visual" pos="-0.01025 -0.0177535 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_case_f_dummy" material="dc15_a01_case_f_dummy_material"/>
+        <!-- Part pp01065_stewart_side_plate_3 -->
+        <geom type="mesh" class="visual" pos="-6.20587e-17 -1.04083e-17 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="pp01065_stewart_side_plate" material="pp01065_stewart_side_plate_material"/>
+        <!-- Link pp01062_stewart_arm -->
+        <body name="pp01062_stewart_arm" pos="0.0317603 0.0301725 -0.03705" quat="0.353553 -0.353553 -0.612372 0.612372">
+          <!-- Joint from dc15_a01_case_b_dummy to pp01062_stewart_arm -->
+          <joint axis="0 0 1" name="1" type="hinge" range="-1.3962634015954645 1.3962634015954627" class="chosen_actuator"/>
+          <inertial pos="0.0181787 2.77556e-17 0.00350559" mass="0.00272417" fullinertia="4.89963e-08 6.75761e-07 6.91798e-07 -2.63422e-25 -7.04811e-08 2.66043e-25"/>
+          <!-- Part ball -->
+          <geom type="mesh" class="visual" pos="0.038 2.77556e-17 0.00768" quat="0 1 0 0" mesh="ball" material="ball_material"/>
+          <!-- Part pp01062_stewart_arm -->
+          <geom type="mesh" class="visual" pos="-0.01025 0.03 0.0425915" quat="0.707107 0.707107 -1.11022e-16 8.49837e-17" mesh="pp01062_stewart_arm" material="pp01062_stewart_arm_material"/>
+          <!-- Link shape -->
+          <body name="shape" pos="0.038 2.77556e-17 0.00768" quat="0.0274567 -0.855096 0.517739 -0.00153082">
+            <!-- Joint from pp01062_stewart_arm to shape -->
+            <joint axis="0 0 1" name="passive_1" type="ball" class="chosen_actuator"/>
+            <inertial pos="-0.0450002 -2.17877e-17 -4.85069e-17" mass="0.00355525" fullinertia="1.7732e-08 3.06049e-06 3.0658e-06 3.75919e-22 -8.93073e-21 -1.00288e-21"/>
+            <!-- Part shape -->
+            <geom type="mesh" class="visual" pos="6.93889e-18 -1.38778e-17 0" quat="1 1.53003e-17 -1.11022e-16 -1.249e-16" mesh="shape" material="shape_material"/>
+            <!-- Part pp00xxx_stewart_rod -->
+            <geom type="mesh" class="visual" pos="0.00371031 0.0370326 -0.0564076" quat="0.855513 0.00634803 -0.517727 0.00384161" mesh="pp00xxx_stewart_rod" material="pp00xxx_stewart_rod_material"/>
+            <!-- Part shape_2 -->
+            <geom type="mesh" class="visual" pos="-0.0900003 -2.77556e-17 -8.32667e-17" quat="1.66533e-16 1.57422e-17 -1 1.11022e-16" mesh="shape" material="shape_material"/>
+            <!-- Frame closing_1_1 -->
+            <site group="3" name="closing_1_1" pos="-0.0900003 -2.77556e-17 -8.32667e-17" quat="1.66533e-16 1.57422e-17 -1 1.11022e-16"/>
+          </body>
+        </body>
+        <!-- Link pp01062_stewart_arm_2 -->
+        <body name="pp01062_stewart_arm_2" pos="0.0420103 0.012419 -0.03705" quat="0.612372 0.612372 -0.353553 -0.353553">
+          <!-- Joint from dc15_a01_case_b_dummy to pp01062_stewart_arm_2 -->
+          <joint axis="0 0 1" name="2" type="hinge" range="-1.3962634015954662 1.396263401595461" class="chosen_actuator"/>
+          <inertial pos="0.0181787 2.34834e-18 0.00350559" mass="0.00272417" fullinertia="4.89963e-08 6.75761e-07 6.91798e-07 2.48336e-22 -7.04811e-08 5.24015e-23"/>
+          <!-- Part ball_2 -->
+          <geom type="mesh" class="visual" pos="0.038 -1.38778e-17 0.00768" quat="1.66533e-16 -2.4037e-17 -1 -1.11022e-16" mesh="ball" material="ball_material"/>
+          <!-- Part pp01062_stewart_arm_2 -->
+          <geom type="mesh" class="visual" pos="-0.01025 0.03 0.0425915" quat="0.707107 0.707107 -1.11022e-16 8.24487e-17" mesh="pp01062_stewart_arm" material="pp01062_stewart_arm_material"/>
+          <!-- Link shape_2 -->
+          <body name="shape_2" pos="0.038 -8.32667e-17 0.00768" quat="0.0131699 -0.855436 -0.517228 0.0230506">
+            <!-- Joint from pp01062_stewart_arm_2 to shape_2 -->
+            <joint axis="0 0 1" name="passive_2" type="ball" class="chosen_actuator"/>
+            <inertial pos="-0.0450002 -1.00413e-17 -8.56034e-18" mass="0.00355525" fullinertia="1.7732e-08 3.06049e-06 3.0658e-06 9.05427e-21 -9.31125e-21 -1.13245e-20"/>
+            <!-- Part shape_3 -->
+            <geom type="mesh" class="visual" pos="-0.0900003 -2.77556e-17 2.08167e-17" quat="2.22045e-16 2.94872e-15 1 3.85803e-15" mesh="shape" material="shape_material"/>
+            <!-- Part shape_4 -->
+            <geom type="mesh" class="visual" pos="6.93889e-18 0 6.93889e-18" quat="1 -0 -0 -4.85723e-17" mesh="shape" material="shape_material"/>
+            <!-- Part pp00xxx_stewart_rod_2 -->
+            <geom type="mesh" class="visual" pos="0.00371031 0.0370326 -0.0564076" quat="0.855513 0.00634803 -0.517727 0.00384161" mesh="pp00xxx_stewart_rod" material="pp00xxx_stewart_rod_material"/>
+            <!-- Frame closing_2_2 -->
+            <site group="3" name="closing_2_2" pos="-0.0900003 -2.77556e-17 2.08167e-17" quat="2.78561e-15 -0.707107 0.707107 2.55257e-15"/>
+          </body>
+        </body>
+        <!-- Link pp01062_stewart_arm_3 -->
+        <body name="pp01062_stewart_arm_3" pos="0.01025 -0.0425915 -0.03705" quat="0.707107 -0.707107 -1.11022e-16 1.84877e-17">
+          <!-- Joint from dc15_a01_case_b_dummy to pp01062_stewart_arm_3 -->
+          <joint axis="0 0 1" name="3" type="hinge" range="-1.3962634015954645 1.3962634015954627" class="chosen_actuator"/>
+          <inertial pos="0.0181787 -1.87132e-19 0.00350559" mass="0.00272417" fullinertia="4.89963e-08 6.75761e-07 6.91798e-07 9.85709e-24 -7.04811e-08 -1.47314e-24"/>
+          <!-- Part pp01062_stewart_arm_3 -->
+          <geom type="mesh" class="visual" pos="-0.01025 0.03 0.0425915" quat="0.707107 0.707107 -1.11022e-16 1.0222e-16" mesh="pp01062_stewart_arm" material="pp01062_stewart_arm_material"/>
+          <!-- Part ball_3 -->
+          <geom type="mesh" class="visual" pos="0.038 0 0.00768" quat="1.45894e-16 -0.707107 0.707107 5.21114e-18" mesh="ball" material="ball_material"/>
+          <!-- Link shape_3 -->
+          <body name="shape_3" pos="0.038 4.16334e-17 0.00768" quat="0.85509 -0.027647 0.0318018 -0.516764">
+            <!-- Joint from pp01062_stewart_arm_3 to shape_3 -->
+            <joint axis="0 0 1" name="passive_3" type="ball" class="chosen_actuator"/>
+            <inertial pos="-0.0450002 -4.79199e-18 4.11785e-17" mass="0.00355525" fullinertia="1.7732e-08 3.06049e-06 3.0658e-06 9.80819e-22 -7.34923e-21 1.45141e-21"/>
+            <!-- Part shape_5 -->
+            <geom type="mesh" class="visual" pos="-0.0900003 0 1.31839e-16" quat="4.16334e-16 4.4964e-17 1 -2.22045e-16" mesh="shape" material="shape_material"/>
+            <!-- Part shape_6 -->
+            <geom type="mesh" class="visual" pos="1.38778e-17 0 0" quat="1 -0 -0 2.56739e-16" mesh="shape" material="shape_material"/>
+            <!-- Part pp00xxx_stewart_rod_3 -->
+            <geom type="mesh" class="visual" pos="0.00371031 0.0370326 -0.0564076" quat="0.855513 0.00634803 -0.517727 0.00384161" mesh="pp00xxx_stewart_rod" material="pp00xxx_stewart_rod_material"/>
+            <!-- Frame closing_3_2 -->
+            <site group="3" name="closing_3_2" pos="-0.0900003 0 1.31839e-16" quat="0 4.06771e-16 -3.33067e-16 -1"/>
+          </body>
+        </body>
+        <!-- Link pp01062_stewart_arm_4 -->
+        <body name="pp01062_stewart_arm_4" pos="-0.01025 -0.0425915 -0.03705" quat="4.44089e-16 -1.5721e-15 -0.707107 -0.707107">
+          <!-- Joint from dc15_a01_case_b_dummy to pp01062_stewart_arm_4 -->
+          <joint axis="0 0 1" name="4" type="hinge" range="-1.3962634015954831 1.396263401595444" class="chosen_actuator"/>
+          <inertial pos="0.0181787 -5.24899e-19 0.00350559" mass="0.00272417" fullinertia="4.89963e-08 6.75761e-07 6.91798e-07 4.36416e-22 -7.04811e-08 3.66848e-23"/>
+          <!-- Part ball_4 -->
+          <geom type="mesh" class="visual" pos="0.038 -2.77556e-17 0.00768" quat="4.51946e-15 -0.707107 -0.707107 -1.08859e-15" mesh="ball" material="ball_material"/>
+          <!-- Part pp01062_stewart_arm_4 -->
+          <geom type="mesh" class="visual" pos="-0.01025 0.03 0.0425915" quat="0.707107 0.707107 -1.11022e-16 1.0222e-16" mesh="pp01062_stewart_arm" material="pp01062_stewart_arm_material"/>
+          <!-- Link shape_4 -->
+          <body name="shape_4" pos="0.038 -2.77556e-17 0.00768" quat="0.854409 0.0439152 0.0416306 0.516065">
+            <!-- Joint from pp01062_stewart_arm_4 to shape_4 -->
+            <joint axis="0 0 1" name="passive_4" type="ball" class="chosen_actuator"/>
+            <inertial pos="-0.0450002 1.04447e-16 -1.1361e-17" mass="0.00355525" fullinertia="1.7732e-08 3.06049e-06 3.0658e-06 6.31755e-21 -5.94417e-21 1.35905e-20"/>
+            <!-- Part shape_7 -->
+            <geom type="mesh" class="visual" pos="-0.0900003 2.08167e-16 3.46945e-18" quat="3.33067e-16 0 -1 1.66533e-16" mesh="shape" material="shape_material"/>
+            <!-- Part pp00xxx_stewart_rod_4 -->
+            <geom type="mesh" class="visual" pos="0.00371031 0.0370326 -0.0564076" quat="0.855513 0.00634803 -0.517727 0.00384161" mesh="pp00xxx_stewart_rod" material="pp00xxx_stewart_rod_material"/>
+            <!-- Part shape_8 -->
+            <geom type="mesh" class="visual" pos="6.93889e-18 0 3.46945e-18" quat="1 0 3.33067e-16 1.31839e-16" mesh="shape" material="shape_material"/>
+            <!-- Frame closing_4_1 -->
+            <site group="3" name="closing_4_1" pos="-0.0900003 2.08167e-16 3.46945e-18" quat="3.33067e-16 0 -1 1.66533e-16"/>
+          </body>
+        </body>
+        <!-- Link pp01062_stewart_arm_5 -->
+        <body name="pp01062_stewart_arm_5" pos="-0.0420103 0.012419 -0.03705" quat="0.353553 -0.353553 0.612372 -0.612372">
+          <!-- Joint from dc15_a01_case_b_dummy to pp01062_stewart_arm_5 -->
+          <joint axis="0 0 1" name="5" type="hinge" range="-1.3962634015954662 1.396263401595461" class="chosen_actuator"/>
+          <inertial pos="0.0181787 3.11301e-35 0.00350559" mass="0.00272417" fullinertia="4.89963e-08 6.75761e-07 6.91798e-07 -4.49492e-25 -7.04811e-08 -1.26741e-25"/>
+          <!-- Part pp01062_stewart_arm_5 -->
+          <geom type="mesh" class="visual" pos="-0.01025 0.03 0.0425915" quat="0.707107 0.707107 -1.11022e-16 9.55014e-17" mesh="pp01062_stewart_arm" material="pp01062_stewart_arm_material"/>
+          <!-- Part ball_5 -->
+          <geom type="mesh" class="visual" pos="0.038 0 0.00768" quat="1.38778e-17 5.55459e-17 1 3.33067e-16" mesh="ball" material="ball_material"/>
+          <!-- Link pp00xxx_stewart_rod -->
+          <body name="pp00xxx_stewart_rod" pos="0.038 1.38778e-17 0.00768" quat="0.0908372 -0.850701 0.516198 -0.0399478">
+            <!-- Joint from pp01062_stewart_arm_5 to pp00xxx_stewart_rod -->
+            <joint axis="0 0 1" name="passive_5" type="ball" class="chosen_actuator"/>
+            <inertial pos="-0.0450002 7.44667e-17 -1.14016e-17" mass="0.00355525" fullinertia="1.7732e-08 3.06049e-06 3.0658e-06 4.55471e-21 -4.43821e-21 1.23083e-21"/>
+            <!-- Part shape_9 -->
+            <geom type="mesh" class="visual" pos="0 0 0" quat="1 0 -2.22045e-16 0" mesh="shape" material="shape_material"/>
+            <!-- Part pp00xxx_stewart_rod_5 -->
+            <geom type="mesh" class="visual" pos="0.00371031 0.0370326 -0.0564076" quat="0.855513 0.00634803 -0.517727 0.00384161" mesh="pp00xxx_stewart_rod" material="pp00xxx_stewart_rod_material"/>
+            <!-- Part shape_10 -->
+            <geom type="mesh" class="visual" pos="-0.0900003 1.38778e-16 6.93889e-18" quat="9.99201e-16 -4.04899e-16 1 1.9984e-15" mesh="shape" material="shape_material"/>
+            <!-- Frame closing_5_1 -->
+            <site group="3" name="closing_5_1" pos="-0.0900003 1.38778e-16 6.93889e-18" quat="9.99201e-16 -4.04899e-16 1 1.9984e-15"/>
+          </body>
+        </body>
+        <!-- Link pp01062_stewart_arm_6 -->
+        <body name="pp01062_stewart_arm_6" pos="-0.0317603 0.0301725 -0.03705" quat="0.612372 0.612372 0.353553 0.353553">
+          <!-- Joint from dc15_a01_case_b_dummy to pp01062_stewart_arm_6 -->
+          <joint axis="0 0 1" name="6" type="hinge" range="-1.3962634015954636 1.3962634015954636" class="chosen_actuator"/>
+          <inertial pos="0.0181787 -1.69382e-17 0.00350559" mass="0.00272417" fullinertia="4.89963e-08 6.75761e-07 6.91798e-07 1.6528e-22 -7.04811e-08 3.48685e-23"/>
+          <!-- Part pp01062_stewart_arm_6 -->
+          <geom type="mesh" class="visual" pos="-0.01025 0.03 0.0425915" quat="0.707107 0.707107 -1.11022e-16 1.00194e-16" mesh="pp01062_stewart_arm" material="pp01062_stewart_arm_material"/>
+          <!-- Part ball_6 -->
+          <geom type="mesh" class="visual" pos="0.038 -2.77556e-17 0.00768" quat="0 1 0 0" mesh="ball" material="ball_material"/>
+          <!-- Link shape_5 -->
+          <body name="shape_5" pos="0.038 -2.77556e-17 0.00768" quat="0.0131438 -0.855436 -0.517229 0.0230347">
+            <!-- Joint from pp01062_stewart_arm_6 to shape_5 -->
+            <joint axis="0 0 1" name="passive_6" type="ball" class="chosen_actuator"/>
+            <inertial pos="-0.0450002 7.95049e-16 -1.5589e-17" mass="0.00355525" fullinertia="1.7732e-08 3.06049e-06 3.0658e-06 4.84585e-20 -8.92792e-21 -1.86218e-20"/>
+            <!-- Part shape_11 -->
+            <geom type="mesh" class="visual" pos="-0.0900003 1.83187e-15 6.93889e-18" quat="1.77636e-15 6.61483e-15 1 6.27276e-15" mesh="shape" material="shape_material"/>
+            <!-- Part shape_12 -->
+            <geom type="mesh" class="visual" pos="6.93889e-18 0 0" quat="1 1.00165e-17 -8.32667e-17 0" mesh="shape" material="shape_material"/>
+            <!-- Part pp00xxx_stewart_rod_6 -->
+            <geom type="mesh" class="visual" pos="0.00371031 0.0370326 -0.0564076" quat="0.855513 0.00634803 -0.517727 0.00384161" mesh="pp00xxx_stewart_rod" material="pp00xxx_stewart_rod_material"/>
+            <!-- Link pp01063_stewart_plateform -->
+            <body name="pp01063_stewart_plateform" pos="-0.0900003 1.88738e-15 -2.08167e-17" quat="0.0131438 0.855436 0.517229 -0.0230347">
+              <!-- Joint from shape_5 to pp01063_stewart_plateform -->
+              <joint axis="0 0 1" name="passive_7" type="ball" class="chosen_actuator"/>
+              <inertial pos="-0.00488914 -0.0469134 0.0291732" mass="0.168092" fullinertia="0.000413571 0.000428808 0.000324749 -2.28564e-06 -5.24367e-05 -9.58295e-07"/>
+              <!-- Part small_lens -->
+              <geom type="mesh" class="visual" pos="0.0262444 -0.0524 0.0136834" quat="0.612372 0.612372 -0.353553 0.353553" mesh="small_lens" material="small_lens_material"/>
+              <!-- Part m12_lens -->
+              <geom type="mesh" class="visual" pos="0.010421 -0.0524 -0.0147088" quat="0.183013 0.683013 -0.683013 -0.183013" mesh="m12_lens" material="m12_lens_material"/>
+              <!-- Part pp01080_back_small_eye -->
+              <geom type="mesh" class="visual" pos="0.0262444 -0.0524 0.0136834" quat="0.612372 0.612372 -0.353553 0.353553" mesh="pp01080_back_small_eye" material="pp01080_back_small_eye_material"/>
+              <!-- Part m12_lens_2 -->
+              <geom type="mesh" class="visual" pos="0.042581 -0.0524 0.0417792" quat="0.183013 0.683013 -0.683013 -0.183013" mesh="m12_lens" material="m12_lens_material"/>
+              <!-- Part ball_7 -->
+              <geom type="mesh" class="visual" pos="-0.013 5.55112e-17 -5.55112e-17" quat="1 1.3274e-15 5.77316e-15 -2.63678e-15" mesh="ball" material="ball_material"/>
+              <!-- Part uc_a37_rev_a_step -->
+              <geom type="mesh" class="visual" pos="0.0239513 -0.04265 0.0271316" quat="9.36644e-18 0.866025 0 0.5" mesh="uc_a37_rev_a_step" material="uc_a37_rev_a_step_material"/>
+              <!-- Part ball_8 -->
+              <geom type="mesh" class="visual" pos="-8.67362e-19 2.77556e-17 0" quat="1 -0 -2.04613e-16 -5.18024e-19" mesh="ball" material="ball_material"/>
+              <!-- Part pp01070_head_head_back -->
+              <geom type="mesh" class="visual" pos="-0.0134282 -0.0524 0.0365883" quat="0.612372 0.612372 -0.353553 0.353553" mesh="pp01070_head_head_back" material="pp01070_head_head_back_material"/>
+              <!-- Part ball_9 -->
+              <geom type="mesh" class="visual" pos="0.0249723 5.55112e-17 0.0432533" quat="1.36964e-17 0.866025 -1.11022e-16 -0.5" mesh="ball" material="ball_material"/>
+              <!-- Part pp01079_back_big_eye -->
+              <geom type="mesh" class="visual" pos="0.0265844 -0.0524 0.013487" quat="0.612372 0.612372 -0.353553 0.353553" mesh="pp01079_back_big_eye" material="pp01079_back_big_eye_material"/>
+              <!-- Part ball_10 -->
+              <geom type="mesh" class="visual" pos="-0.0379723 5.55112e-17 0.0432533" quat="0.353553 -0.612372 0.612372 -0.353553" mesh="ball" material="ball_material"/>
+              <!-- Part pp01069_head_shell_front -->
+              <geom type="mesh" class="visual" pos="-0.0142942 -0.0524 0.0370883" quat="0.612372 0.612372 -0.353553 0.353553" mesh="pp01069_head_shell_front" material="pp01069_head_shell_front_material"/>
+              <!-- Part pp01078_glasses -->
+              <geom type="mesh" class="visual" pos="0.025976 -0.0524 0.0138383" quat="0.612372 0.612372 -0.353553 0.353553" mesh="pp01078_glasses" material="pp01078_glasses_material"/>
+              <!-- Part pp01063_stewart_plateform -->
+              <geom type="mesh" class="visual" pos="-0.0065 0.109722 0.0325883" quat="0.353553 0.353553 0.612372 -0.612372" mesh="pp01063_stewart_plateform" material="pp01063_stewart_plateform_material"/>
+              <!-- Part ball_11 -->
+              <geom type="mesh" class="visual" pos="-0.0314723 2.77556e-17 0.0545117" quat="0.353553 -0.612372 0.612372 -0.353553" mesh="ball" material="ball_material"/>
+              <!-- Part big_lens -->
+              <geom type="mesh" class="visual" pos="0.0281677 -0.0524 0.0125729" quat="0.612372 0.612372 -0.353553 0.353553" mesh="big_lens" material="big_lens_material"/>
+              <!-- Part ball_12 -->
+              <geom type="mesh" class="visual" pos="0.0184723 5.55112e-17 0.0545117" quat="3.07775e-17 -0.866025 -0 0.5" mesh="ball" material="ball_material"/>
+              <!-- Frame closing_1_2 -->
+              <site group="3" name="closing_1_2" pos="0.0249723 5.55112e-17 0.0432533" quat="0.353553 0.612372 -0.612372 -0.353553"/>
+              <!-- Frame closing_2_1 -->
+              <site group="3" name="closing_2_1" pos="0.0184723 5.55112e-17 0.0545117" quat="3.07775e-17 -0.866025 -0 0.5"/>
+              <!-- Frame closing_3_1 -->
+              <site group="3" name="closing_3_1" pos="-0.0314723 2.77556e-17 0.0545117" quat="0.353553 -0.612372 0.612372 -0.353553"/>
+              <!-- Frame closing_4_2 -->
+              <site group="3" name="closing_4_2" pos="-0.0379723 5.55112e-17 0.0432533" quat="0.612372 0.353553 -0.353553 -0.612372"/>
+              <!-- Frame closing_5_2 -->
+              <site group="3" name="closing_5_2" pos="-0.013 5.55112e-17 -5.55112e-17" quat="0.707107 -3.16544e-15 5.10703e-15 -0.707107"/>
+              <!-- Frame head -->
+              <site group="3" name="head" pos="-0.0134282 -0.0012 0.0365883" quat="0.683013 0.683013 0.183013 -0.183013"/>
+              <!-- Frame camera -->
+              <body pos="0.0244171 -0.0524 0.0147383" quat="-0.000 -0.500 0.000 0.866">
+                <site group="3" name="camera"/>
+                <camera name="eye_camera"
+                 mode="fixed"
+                 resolution="1280 720"
+                />
+
+              </body>
+              <!-- Frame camera_optical -->
+              <site group="3" name="camera_optical" pos="0.0244171 -0.0524 0.0147383" quat="0.5 0 0.866025 -2.22045e-16"/>
+              <!-- Link test_antenna_body -->
+              <body name="test_antenna_body" pos="-0.0161053 -0.0744 0.100068" quat="0.964418 0.0356203 -0.25955 0.0355422">
+                <!-- Joint from pp01063_stewart_plateform to test_antenna_body -->
+                <joint axis="0 0 1" name="left_antenna" type="hinge" class="chosen_actuator"/>
+                <inertial pos="0.00147983 -0.0294086 0.00298611" mass="0.00456766" fullinertia="1.38179e-05 2.28739e-07 1.38227e-05 6.85535e-07 5.01796e-08 -9.97217e-07"/>
+                <!-- Part test_antenna -->
+                <geom type="mesh" class="visual" pos="0.0285777 0.0175124 -0.0571006" quat="0.716727 0.647969 0.191203 -0.172861" mesh="test_antenna" material="test_antenna_material"/>
+                <!-- Part test_antenna_body -->
+                <geom type="mesh" class="visual" pos="0.0285297 0.0184653 -0.0570171" quat="0.716727 0.647969 0.191203 -0.172861" mesh="test_antenna_body" material="test_antenna_body_material"/>
+              </body>
+              <!-- Link test_antenna -->
+              <body name="test_antenna" pos="-0.0697422 -0.0744 0.00716666" quat="0.706492 0.0909539 -0.701233 -0.0294913">
+                <!-- Joint from pp01063_stewart_plateform to test_antenna -->
+                <joint axis="0 0 1" name="right_antenna" type="hinge" class="chosen_actuator"/>
+                <inertial pos="0.00147983 -0.0294086 0.00298611" mass="0.00456766" fullinertia="1.38179e-05 2.28739e-07 1.38227e-05 6.85535e-07 5.01796e-08 -9.97217e-07"/>
+                <!-- Part test_antenna_2 -->
+                <geom type="mesh" class="visual" pos="0.0285777 0.0175124 -0.0571006" quat="0.716727 0.647969 0.191203 -0.172861" mesh="test_antenna" material="test_antenna_material"/>
+                <!-- Part test_antenna_body_2 -->
+                <geom type="mesh" class="visual" pos="0.0285297 0.0184653 -0.0570171" quat="0.716727 0.647969 0.191203 -0.172861" mesh="test_antenna_body" material="test_antenna_body_material"/>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+    </body>
+  </worldbody>
+  <asset>
+    <mesh file="dc15_a01_case_b_dummy.stl"/>
+    <mesh file="pp01078_glasses.stl"/>
+    <mesh file="big_lens.stl"/>
+    <mesh file="test_antenna_body.stl"/>
+    <mesh file="pp01065_stewart_side_plate.stl"/>
+    <mesh file="small_lens.stl"/>
+    <mesh file="pp01080_back_small_eye.stl"/>
+    <mesh file="pp01064_stewart_main_plate.stl"/>
+    <mesh file="shape.stl"/>
+    <mesh file="pp00xxx_stewart_rod.stl"/>
+    <mesh file="pp01079_back_big_eye.stl"/>
+    <mesh file="ball.stl"/>
+    <mesh file="wj_wk00_0123middlecase_56__configuration_default.stl"/>
+    <mesh file="test_antenna.stl"/>
+    <mesh file="pp01070_head_head_back.stl"/>
+    <mesh file="dc15_a01_case_f_dummy.stl"/>
+    <mesh file="drive_palonier__configuration_default.stl"/>
+    <mesh file="dc15_a01_horn_dummy.stl"/>
+    <mesh file="wj_wk00_0122topcabinetcase_95__configuration_default.stl"/>
+    <mesh file="pp01071_turning_bowl.stl"/>
+    <mesh file="pp01068_top_body.stl"/>
+    <mesh file="wj_wk00_0124bottomcase_45__configuration_default.stl"/>
+    <mesh file="uc_a37_rev_a_step.stl"/>
+    <mesh file="dc15_a01_case_m_dummy.stl"/>
+    <mesh file="pp01062_stewart_arm.stl"/>
+    <mesh file="pp01072_turning_end.stl"/>
+    <mesh file="m12_lens.stl"/>
+    <mesh file="pp01069_head_shell_front.stl"/>
+    <mesh file="pp01063_stewart_plateform.stl"/>
+    <mesh file="pp01066_stewart_mid_plate.stl"/>
+    <mesh file="pp01067_bottom_body.stl"/>
+    <material name="pp01071_turning_bowl_material" rgba="0.34902 0.376471 0.4 1"/>
+    <material name="pp01072_turning_end_material" rgba="0.262745 0.282353 0.301961 1"/>
+    <material name="pp01065_stewart_side_plate_material" rgba="0.196078 0.196078 0.196078 1"/>
+    <material name="pp01067_bottom_body_material" rgba="1 1 1 1"/>
+    <material name="dc15_a01_case_m_dummy_material" rgba="0.262745 0.282353 0.301961 1"/>
+    <material name="dc15_a01_case_f_dummy_material" rgba="0.262745 0.282353 0.301961 1"/>
+    <material name="dc15_a01_horn_dummy_material" rgba="0.262745 0.282353 0.301961 1"/>
+    <material name="wj_wk00_0122topcabinetcase_95__configuration_default_material" rgba="0.247059 0.243137 0.243137 1"/>
+    <material name="dc15_a01_case_b_dummy_material" rgba="0.262745 0.282353 0.301961 1"/>
+    <material name="pp01068_top_body_material" rgba="1 1 1 1"/>
+    <material name="drive_palonier__configuration_default_material" rgba="0.623529 0.666667 0.701961 1"/>
+    <material name="wj_wk00_0123middlecase_56__configuration_default_material" rgba="0.247059 0.243137 0.243137 1"/>
+    <material name="pp01066_stewart_mid_plate_material" rgba="0.196078 0.196078 0.196078 1"/>
+    <material name="wj_wk00_0124bottomcase_45__configuration_default_material" rgba="0.247059 0.243137 0.243137 1"/>
+    <material name="pp01064_stewart_main_plate_material" rgba="0.196078 0.196078 0.196078 1"/>
+    <material name="ball_material" rgba="0.901961 0.901961 0.901961 1"/>
+    <material name="pp01062_stewart_arm_material" rgba="0.196078 0.196078 0.196078 1"/>
+    <material name="shape_material" rgba="0.901961 0.901961 0.901961 1"/>
+    <material name="pp00xxx_stewart_rod_material" rgba="0.8 0.8 0.8 1"/>
+    <material name="small_lens_material" rgba="0.439216 0.47451 0.501961 0.298039"/>
+    <material name="m12_lens_material" rgba="0.262745 0.282353 0.301961 1"/>
+    <material name="pp01080_back_small_eye_material" rgba="0.168627 0.168627 0.168627 1"/>
+    <material name="uc_a37_rev_a_step_material" rgba="0.25098 0.25098 0.25098 1"/>
+    <material name="pp01070_head_head_back_material" rgba="1 1 1 1"/>
+    <material name="pp01079_back_big_eye_material" rgba="0.168627 0.168627 0.168627 1"/>
+    <material name="pp01069_head_shell_front_material" rgba="1 1 1 1"/>
+    <material name="pp01078_glasses_material" rgba="0.168627 0.168627 0.168627 1"/>
+    <material name="pp01063_stewart_plateform_material" rgba="1 1 1 1"/>
+    <material name="big_lens_material" rgba="0.439216 0.47451 0.501961 0.298039"/>
+    <material name="test_antenna_material" rgba="0.262745 0.282353 0.301961 1"/>
+    <material name="test_antenna_body_material" rgba="1 1 1 1"/>
+  </asset>
+  <actuator>
+    <position class="chosen_actuator" name="all_yaw" joint="all_yaw" />
+    <position class="chosen_actuator" name="1" joint="1" inheritrange="1"/>
+    <position class="chosen_actuator" name="2" joint="2" inheritrange="1"/>
+    <position class="chosen_actuator" name="3" joint="3" inheritrange="1"/>
+    <position class="chosen_actuator" name="4" joint="4" inheritrange="1"/>
+    <position class="chosen_actuator" name="5" joint="5" inheritrange="1"/>
+    <position class="chosen_actuator" name="6" joint="6" inheritrange="1"/>
+    <position class="chosen_actuator" name="left_antenna" joint="left_antenna"/>
+    <position class="chosen_actuator" name="right_antenna" joint="right_antenna"/>
+  </actuator>
+  <equality>
+    <connect site1="closing_1_1" site2="closing_1_2"/>
+    <connect site1="closing_2_1" site2="closing_2_2"/>
+    <connect site1="closing_3_1" site2="closing_3_2"/>
+    <connect site1="closing_4_1" site2="closing_4_2"/>
+    <connect site1="closing_5_1" site2="closing_5_2"/>
+  </equality>
+</mujoco>
diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/reachy_mini.xml b/src/reachy_mini/descriptions/reachy_mini/mjcf/reachy_mini.xml
index bf5dafa5..83ee966a 100644
--- a/src/reachy_mini/descriptions/reachy_mini/mjcf/reachy_mini.xml
+++ b/src/reachy_mini/descriptions/reachy_mini/mjcf/reachy_mini.xml
@@ -11,7 +11,7 @@
         <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>
@@ -75,6 +75,11 @@
         <!-- 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="box" class="collision" name="body_front_collider" size="0.035 0.01 0.08" pos="0 0.06 0.07" euler="0 0 0"/>
+        <geom type="box" class="collision" name="body_back_collider" size="0.025 0.01 0.03" pos="0 -0.06 0.1" euler="0 0 0"/>
+        <geom type="box" class="collision" name="body_eft_collider" size="0.025 0.01 0.06" pos="-0.06 0 0.05" euler="0 0 1.57"/>
+        <geom type="box" class="collision" name="body_right_collider" size="0.025 0.01 0.06" pos="0.06 0 0.05" euler="0 0 -1.57"/>
+        <!-- <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" material="body_top_3dprint_material"/> -->
         <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"/>
@@ -471,9 +476,14 @@
                   resolution="1280 720"
                   fovy="80"
                 />
+                <geom type="capsule" class="collision" name="head_collider" size="0.048 0.022" pos="0 0 0.039" euler="0 1.57 0"/>
+        
               </body> 
               <!-- Frame head -->
               <site group="3" name="head" pos="-0.00611127 0.00370522 0.0291364" quat="0.465151 0.724371 -0.182968 -0.474809"/>
+              <!-- <geom type="capsule" class="collision" name="head_collider"  quat="0.612372 0.612372 -0.353553 0.353553" size="0.05"/> -->
+              <!-- <geom type="capsule" class="collision" name="head_collider" size="0.048 0.01" 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 -->
diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/reachy_mini.xml.old b/src/reachy_mini/descriptions/reachy_mini/mjcf/reachy_mini.xml.old
new file mode 100644
index 00000000..529ebb57
--- /dev/null
+++ b/src/reachy_mini/descriptions/reachy_mini/mjcf/reachy_mini.xml.old
@@ -0,0 +1,440 @@
+<?xml version="1.0" ?>
+<!-- Generated using onshape-to-robot -->
+<!-- Onshape https://cad.onshape.com/documents/e305b26a84339af20c593973/w/ec7765eccdaa9a5960507125/e/251023ea04190ab16854eb80 -->
+<mujoco model="reachy_mini">
+  <compiler angle="radian" meshdir="assets" autolimits="true"/>
+  <default>
+    <default class="reachy_mini">
+      <joint frictionloss="0.1" armature="0.005"/>
+      <position kp="50" dampratio="1"/>
+      <default class="visual">
+        <geom type="mesh" contype="0" conaffinity="0" group="2"/>
+      </default>
+      <default class="collision">
+        <geom 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.0" forcerange="-10.0 10.0"/>
+      <default class="chosen_actuator"/>
+    </default>
+    <default class="xc330m288t">
+      <!-- probably wrong, would need to re-identify -->
+      <geom contype="0" conaffinity="0"/>
+      <joint damping="0.095" frictionloss="0.058" armature="0.0018"/>
+      <position kp="2.54" kv="0.0" forcerange="-0.8 0.8"/>
+    </default>
+    <default class="sts3215_345">
+      <!-- Standard sts3215 with 1/345 gear ratio -->
+      <!-- Confident that this is realistic (mini duck walks with these values) -->
+      <geom contype="0" conaffinity="0"/>
+      <joint damping="0.56" frictionloss="0.068" armature="0.027"/>
+      <position kp="17.11" kv="0.0" forcerange="-3.23 3.23"/>
+    </default>
+    <default class="sts3215_147">
+      <!-- sts3215 with 1/147 gear ratio -->
+      <!-- (Estimation based on the gear ratio difference) -->
+      <geom contype="0" conaffinity="0"/>
+      <joint damping="0.1" frictionloss="0.028" armature="0.011"/>
+      <position kp="17.11" kv="0.0" forcerange="-1.35 1.35"/>
+    </default>
+  </default>
+  <!-- Additional additional.xml -->
+  <contact>
+    <!-- <exclude name="exclude0" body1="torso" body2="sts32155"/> -->
+  </contact>
+  <default>
+    <equality solref="0.002 1" solimp="0.99 0.999 0.0005 0.5 2"/>
+  </default>
+  <worldbody>
+    <!-- Link pp01071_turning_bowl -->
+    <body name="pp01071_turning_bowl" pos="0 0 0" quat="1 0 0 0" childclass="reachy_mini">
+      <inertial pos="0.000667521 1.7878e-09 0.0118411" mass="0.0947234" fullinertia="0.000111359 0.000106563 0.000196298 1.02485e-11 8.96229e-07 4.95917e-13"/>
+      <!-- Part pp01071_turning_bowl -->
+      <geom type="mesh" class="visual" pos="-1.38778e-17 0 0.2345" quat="0.707107 0 -6.16298e-32 0.707107" mesh="pp01071_turning_bowl" material="pp01071_turning_bowl_material"/>
+      <!-- Part pp01072_turning_end -->
+      <geom type="mesh" class="visual" pos="3.46945e-17 -4.51028e-17 0.2345" quat="0.707107 7.85046e-17 -2.22045e-16 0.707107" mesh="pp01072_turning_end" material="pp01072_turning_end_material"/>
+      <!-- Link dc15_a01_case_b_dummy -->
+      <body name="dc15_a01_case_b_dummy" pos="4.85723e-17 -5.60663e-19 0.0339" quat="7.85046e-17 -0.707107 -0.707107 -7.85046e-17">
+        <!-- Joint from pp01071_turning_bowl to dc15_a01_case_b_dummy -->
+        <joint axis="0 0 1" name="all_yaw" type="hinge" class="chosen_actuator"/>
+        <inertial pos="-1.60377e-06 0.00140187 -0.0329593" mass="0.443795" fullinertia="0.00159333 0.0015483 0.00187065 2.80397e-07 1.98452e-07 4.26838e-05"/>
+        <!-- Part pp01065_stewart_side_plate -->
+        <geom type="mesh" class="visual" pos="-3.43031e-17 -4.85723e-17 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="pp01065_stewart_side_plate" material="pp01065_stewart_side_plate_material"/>
+        <!-- Part pp01067_bottom_body -->
+        <geom type="mesh" class="visual" pos="1.38767e-16 1.83881e-16 -0.20055" quat="9.16593e-17 1 -3.29597e-17 5.68353e-17" mesh="pp01067_bottom_body" material="pp01067_bottom_body_material"/>
+        <!-- Part dc15_a01_case_m_dummy -->
+        <geom type="mesh" class="visual" pos="-1.4706e-16 -8.67362e-17 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_case_m_dummy" material="dc15_a01_case_m_dummy_material"/>
+        <!-- Part dc15_a01_case_f_dummy -->
+        <geom type="mesh" class="visual" pos="0.0205 6.93889e-18 -0.00705" quat="3.33067e-16 3.64292e-17 1 -4.44089e-16" mesh="dc15_a01_case_f_dummy" material="dc15_a01_case_f_dummy_material"/>
+        <!-- Part dc15_a01_horn_dummy -->
+        <geom type="mesh" class="visual" pos="0.0205 1.73472e-17 -0.00705" quat="3.33067e-16 3.64292e-17 1 -4.44089e-16" mesh="dc15_a01_horn_dummy" material="dc15_a01_horn_dummy_material"/>
+        <!-- Part dc15_a01_horn_dummy_2 -->
+        <geom type="mesh" class="visual" pos="-0.01025 0.0177535 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_horn_dummy" material="dc15_a01_horn_dummy_material"/>
+        <!-- Part wj_wk00_0122topcabinetcase_95__configuration_default -->
+        <geom type="mesh" class="visual" pos="6.12843e-18 -0.0255 -0.00945" quat="0.5 0.5 -0.5 -0.5" mesh="wj_wk00_0122topcabinetcase_95__configuration_default" material="wj_wk00_0122topcabinetcase_95__configuration_default_material"/>
+        <!-- Part dc15_a01_case_m_dummy_2 -->
+        <geom type="mesh" class="visual" pos="-2.56295e-17 3.1225e-17 -0.00705" quat="1.11022e-16 2.60209e-17 1 -2.22045e-16" mesh="dc15_a01_case_m_dummy" material="dc15_a01_case_m_dummy_material"/>
+        <!-- Part dc15_a01_case_b_dummy -->
+        <geom type="mesh" class="visual" pos="-0.01025 -0.0177535 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_case_b_dummy" material="dc15_a01_case_b_dummy_material"/>
+        <!-- Part dc15_a01_case_b_dummy_2 -->
+        <geom type="mesh" class="visual" pos="0.0205 3.46945e-18 -0.00705" quat="3.33067e-16 3.64292e-17 1 -4.44089e-16" mesh="dc15_a01_case_b_dummy" material="dc15_a01_case_b_dummy_material"/>
+        <!-- Part pp01068_top_body -->
+        <geom type="mesh" class="visual" pos="-8.55341e-16 -3.12493e-14 -0.20005" quat="9.19752e-14 1 -3.64292e-17 -2.66929e-15" mesh="pp01068_top_body" material="pp01068_top_body_material"/>
+        <!-- Part dc15_a01_case_f_dummy_2 -->
+        <geom type="mesh" class="visual" pos="-3.43031e-17 -3.46945e-18 -0.00705" quat="3.33067e-16 3.64292e-17 1 -4.44089e-16" mesh="dc15_a01_case_f_dummy" material="dc15_a01_case_f_dummy_material"/>
+        <!-- Part drive_palonier__configuration_default -->
+        <geom type="mesh" class="visual" pos="4.39371e-18 -0.0255 -0.00945" quat="0.5 0.5 -0.5 -0.5" mesh="drive_palonier__configuration_default" material="drive_palonier__configuration_default_material"/>
+        <!-- Part dc15_a01_case_f_dummy_3 -->
+        <geom type="mesh" class="visual" pos="-0.01025 0.0177535 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_case_f_dummy" material="dc15_a01_case_f_dummy_material"/>
+        <!-- Part dc15_a01_case_m_dummy_3 -->
+        <geom type="mesh" class="visual" pos="-3.43031e-17 -6.245e-17 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_case_m_dummy" material="dc15_a01_case_m_dummy_material"/>
+        <!-- Part dc15_a01_case_b_dummy_3 -->
+        <geom type="mesh" class="visual" pos="-9.1549e-17 -6.245e-17 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_case_b_dummy" material="dc15_a01_case_b_dummy_material"/>
+        <!-- Part wj_wk00_0123middlecase_56__configuration_default -->
+        <geom type="mesh" class="visual" pos="-1.75347e-05 -0.0254895 -0.00945" quat="0.5 0.5 -0.5 -0.5" mesh="wj_wk00_0123middlecase_56__configuration_default" material="wj_wk00_0123middlecase_56__configuration_default_material"/>
+        <!-- Part dc15_a01_case_b_dummy_4 -->
+        <geom type="mesh" class="visual" pos="-3.60378e-17 -1.04083e-17 -0.00705" quat="3.33067e-16 3.64292e-17 1 -4.44089e-16" mesh="dc15_a01_case_b_dummy" material="dc15_a01_case_b_dummy_material"/>
+        <!-- Part dc15_a01_case_f_dummy_4 -->
+        <geom type="mesh" class="visual" pos="-2.26857e-16 -1.00614e-16 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_case_f_dummy" material="dc15_a01_case_f_dummy_material"/>
+        <!-- Part dc15_a01_case_b_dummy_5 -->
+        <geom type="mesh" class="visual" pos="-0.01025 0.0177535 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_case_b_dummy" material="dc15_a01_case_b_dummy_material"/>
+        <!-- Part dc15_a01_horn_dummy_3 -->
+        <geom type="mesh" class="visual" pos="-1.15835e-16 -6.93889e-17 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_horn_dummy" material="dc15_a01_horn_dummy_material"/>
+        <!-- Part dc15_a01_horn_dummy_4 -->
+        <geom type="mesh" class="visual" pos="-0.01025 -0.0177535 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_horn_dummy" material="dc15_a01_horn_dummy_material"/>
+        <!-- Part dc15_a01_case_m_dummy_4 -->
+        <geom type="mesh" class="visual" pos="-0.01025 0.0177535 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_case_m_dummy" material="dc15_a01_case_m_dummy_material"/>
+        <!-- Part pp01065_stewart_side_plate_2 -->
+        <geom type="mesh" class="visual" pos="-1.34864e-17 6.59195e-17 -0.00705" quat="0 3.64292e-17 1 0" mesh="pp01065_stewart_side_plate" material="pp01065_stewart_side_plate_material"/>
+        <!-- Part dc15_a01_case_b_dummy_6 -->
+        <geom type="mesh" class="visual" pos="-6.37934e-17 -3.46945e-17 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_case_b_dummy" material="dc15_a01_case_b_dummy_material"/>
+        <!-- Part pp01066_stewart_mid_plate -->
+        <geom type="mesh" class="visual" pos="-8.28754e-17 -1.04083e-17 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="pp01066_stewart_mid_plate" material="pp01066_stewart_mid_plate_material"/>
+        <!-- Part dc15_a01_case_f_dummy_5 -->
+        <geom type="mesh" class="visual" pos="-3.77726e-17 -4.85723e-17 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_case_f_dummy" material="dc15_a01_case_f_dummy_material"/>
+        <!-- Part dc15_a01_horn_dummy_5 -->
+        <geom type="mesh" class="visual" pos="-5.51198e-17 -4.85723e-17 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_horn_dummy" material="dc15_a01_horn_dummy_material"/>
+        <!-- Part dc15_a01_case_m_dummy_5 -->
+        <geom type="mesh" class="visual" pos="0.0205 3.81639e-17 -0.00705" quat="1.11022e-16 2.60209e-17 1 -2.22045e-16" mesh="dc15_a01_case_m_dummy" material="dc15_a01_case_m_dummy_material"/>
+        <!-- Part wj_wk00_0124bottomcase_45__configuration_default -->
+        <geom type="mesh" class="visual" pos="-1.04636e-05 -0.0254966 -0.00945" quat="0.5 0.5 -0.5 -0.5" mesh="wj_wk00_0124bottomcase_45__configuration_default" material="wj_wk00_0124bottomcase_45__configuration_default_material"/>
+        <!-- Part dc15_a01_horn_dummy_6 -->
+        <geom type="mesh" class="visual" pos="-3.25684e-17 1.04083e-17 -0.00705" quat="3.33067e-16 3.64292e-17 1 -4.44089e-16" mesh="dc15_a01_horn_dummy" material="dc15_a01_horn_dummy_material"/>
+        <!-- Part dc15_a01_case_m_dummy_6 -->
+        <geom type="mesh" class="visual" pos="-0.01025 -0.0177535 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_case_m_dummy" material="dc15_a01_case_m_dummy_material"/>
+        <!-- Part pp01064_stewart_main_plate -->
+        <geom type="mesh" class="visual" pos="-2.73642e-17 -1.38778e-17 -0.00705" quat="1.11022e-16 -8.67362e-18 -1 -1.11022e-16" mesh="pp01064_stewart_main_plate" material="pp01064_stewart_main_plate_material"/>
+        <!-- Part dc15_a01_case_f_dummy_6 -->
+        <geom type="mesh" class="visual" pos="-0.01025 -0.0177535 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_case_f_dummy" material="dc15_a01_case_f_dummy_material"/>
+        <!-- Part pp01065_stewart_side_plate_3 -->
+        <geom type="mesh" class="visual" pos="-6.20587e-17 -1.04083e-17 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="pp01065_stewart_side_plate" material="pp01065_stewart_side_plate_material"/>
+        <!-- Link pp01062_stewart_arm -->
+        <body name="pp01062_stewart_arm" pos="0.0317603 0.0301725 -0.03705" quat="0.353553 -0.353553 -0.612372 0.612372">
+          <!-- Joint from dc15_a01_case_b_dummy to pp01062_stewart_arm -->
+          <joint axis="0 0 1" name="1" type="hinge" range="-1.3962634015954645 1.3962634015954627" class="chosen_actuator"/>
+          <inertial pos="0.0181787 2.77556e-17 0.00350559" mass="0.00272417" fullinertia="4.89963e-08 6.75761e-07 6.91798e-07 -2.63422e-25 -7.04811e-08 2.66043e-25"/>
+          <!-- Part ball -->
+          <geom type="mesh" class="visual" pos="0.038 2.77556e-17 0.00768" quat="0 1 0 0" mesh="ball" material="ball_material"/>
+          <!-- Part pp01062_stewart_arm -->
+          <geom type="mesh" class="visual" pos="-0.01025 0.03 0.0425915" quat="0.707107 0.707107 -1.11022e-16 8.49837e-17" mesh="pp01062_stewart_arm" material="pp01062_stewart_arm_material"/>
+          <!-- Link shape -->
+          <body name="shape" pos="0.038 2.77556e-17 0.00768" quat="0.0274567 -0.855096 0.517739 -0.00153082">
+            <!-- Joint from pp01062_stewart_arm to shape -->
+            <joint axis="0 0 1" name="passive_1" type="ball" class="chosen_actuator"/>
+            <inertial pos="-0.0450002 -2.17877e-17 -4.85069e-17" mass="0.00355525" fullinertia="1.7732e-08 3.06049e-06 3.0658e-06 3.75919e-22 -8.93073e-21 -1.00288e-21"/>
+            <!-- Part shape -->
+            <geom type="mesh" class="visual" pos="6.93889e-18 -1.38778e-17 0" quat="1 1.53003e-17 -1.11022e-16 -1.249e-16" mesh="shape" material="shape_material"/>
+            <!-- Part pp00xxx_stewart_rod -->
+            <geom type="mesh" class="visual" pos="0.00371031 0.0370326 -0.0564076" quat="0.855513 0.00634803 -0.517727 0.00384161" mesh="pp00xxx_stewart_rod" material="pp00xxx_stewart_rod_material"/>
+            <!-- Part shape_2 -->
+            <geom type="mesh" class="visual" pos="-0.0900003 -2.77556e-17 -8.32667e-17" quat="1.66533e-16 1.57422e-17 -1 1.11022e-16" mesh="shape" material="shape_material"/>
+            <!-- Frame closing_1_1 -->
+            <site group="3" name="closing_1_1" pos="-0.0900003 -2.77556e-17 -8.32667e-17" quat="1.66533e-16 1.57422e-17 -1 1.11022e-16"/>
+          </body>
+        </body>
+        <!-- Link pp01062_stewart_arm_2 -->
+        <body name="pp01062_stewart_arm_2" pos="0.0420103 0.012419 -0.03705" quat="0.612372 0.612372 -0.353553 -0.353553">
+          <!-- Joint from dc15_a01_case_b_dummy to pp01062_stewart_arm_2 -->
+          <joint axis="0 0 1" name="2" type="hinge" range="-1.3962634015954662 1.396263401595461" class="chosen_actuator"/>
+          <inertial pos="0.0181787 2.34834e-18 0.00350559" mass="0.00272417" fullinertia="4.89963e-08 6.75761e-07 6.91798e-07 2.48336e-22 -7.04811e-08 5.24015e-23"/>
+          <!-- Part ball_2 -->
+          <geom type="mesh" class="visual" pos="0.038 -1.38778e-17 0.00768" quat="1.66533e-16 -2.4037e-17 -1 -1.11022e-16" mesh="ball" material="ball_material"/>
+          <!-- Part pp01062_stewart_arm_2 -->
+          <geom type="mesh" class="visual" pos="-0.01025 0.03 0.0425915" quat="0.707107 0.707107 -1.11022e-16 8.24487e-17" mesh="pp01062_stewart_arm" material="pp01062_stewart_arm_material"/>
+          <!-- Link shape_2 -->
+          <body name="shape_2" pos="0.038 -8.32667e-17 0.00768" quat="0.0131699 -0.855436 -0.517228 0.0230506">
+            <!-- Joint from pp01062_stewart_arm_2 to shape_2 -->
+            <joint axis="0 0 1" name="passive_2" type="ball" class="chosen_actuator"/>
+            <inertial pos="-0.0450002 -1.00413e-17 -8.56034e-18" mass="0.00355525" fullinertia="1.7732e-08 3.06049e-06 3.0658e-06 9.05427e-21 -9.31125e-21 -1.13245e-20"/>
+            <!-- Part shape_3 -->
+            <geom type="mesh" class="visual" pos="-0.0900003 -2.77556e-17 2.08167e-17" quat="2.22045e-16 2.94872e-15 1 3.85803e-15" mesh="shape" material="shape_material"/>
+            <!-- Part shape_4 -->
+            <geom type="mesh" class="visual" pos="6.93889e-18 0 6.93889e-18" quat="1 -0 -0 -4.85723e-17" mesh="shape" material="shape_material"/>
+            <!-- Part pp00xxx_stewart_rod_2 -->
+            <geom type="mesh" class="visual" pos="0.00371031 0.0370326 -0.0564076" quat="0.855513 0.00634803 -0.517727 0.00384161" mesh="pp00xxx_stewart_rod" material="pp00xxx_stewart_rod_material"/>
+            <!-- Frame closing_2_2 -->
+            <site group="3" name="closing_2_2" pos="-0.0900003 -2.77556e-17 2.08167e-17" quat="2.78561e-15 -0.707107 0.707107 2.55257e-15"/>
+          </body>
+        </body>
+        <!-- Link pp01062_stewart_arm_3 -->
+        <body name="pp01062_stewart_arm_3" pos="0.01025 -0.0425915 -0.03705" quat="0.707107 -0.707107 -1.11022e-16 1.84877e-17">
+          <!-- Joint from dc15_a01_case_b_dummy to pp01062_stewart_arm_3 -->
+          <joint axis="0 0 1" name="3" type="hinge" range="-1.3962634015954645 1.3962634015954627" class="chosen_actuator"/>
+          <inertial pos="0.0181787 -1.87132e-19 0.00350559" mass="0.00272417" fullinertia="4.89963e-08 6.75761e-07 6.91798e-07 9.85709e-24 -7.04811e-08 -1.47314e-24"/>
+          <!-- Part pp01062_stewart_arm_3 -->
+          <geom type="mesh" class="visual" pos="-0.01025 0.03 0.0425915" quat="0.707107 0.707107 -1.11022e-16 1.0222e-16" mesh="pp01062_stewart_arm" material="pp01062_stewart_arm_material"/>
+          <!-- Part ball_3 -->
+          <geom type="mesh" class="visual" pos="0.038 0 0.00768" quat="1.45894e-16 -0.707107 0.707107 5.21114e-18" mesh="ball" material="ball_material"/>
+          <!-- Link shape_3 -->
+          <body name="shape_3" pos="0.038 4.16334e-17 0.00768" quat="0.85509 -0.027647 0.0318018 -0.516764">
+            <!-- Joint from pp01062_stewart_arm_3 to shape_3 -->
+            <joint axis="0 0 1" name="passive_3" type="ball" class="chosen_actuator"/>
+            <inertial pos="-0.0450002 -4.79199e-18 4.11785e-17" mass="0.00355525" fullinertia="1.7732e-08 3.06049e-06 3.0658e-06 9.80819e-22 -7.34923e-21 1.45141e-21"/>
+            <!-- Part shape_5 -->
+            <geom type="mesh" class="visual" pos="-0.0900003 0 1.31839e-16" quat="4.16334e-16 4.4964e-17 1 -2.22045e-16" mesh="shape" material="shape_material"/>
+            <!-- Part shape_6 -->
+            <geom type="mesh" class="visual" pos="1.38778e-17 0 0" quat="1 -0 -0 2.56739e-16" mesh="shape" material="shape_material"/>
+            <!-- Part pp00xxx_stewart_rod_3 -->
+            <geom type="mesh" class="visual" pos="0.00371031 0.0370326 -0.0564076" quat="0.855513 0.00634803 -0.517727 0.00384161" mesh="pp00xxx_stewart_rod" material="pp00xxx_stewart_rod_material"/>
+            <!-- Frame closing_3_2 -->
+            <site group="3" name="closing_3_2" pos="-0.0900003 0 1.31839e-16" quat="0 4.06771e-16 -3.33067e-16 -1"/>
+          </body>
+        </body>
+        <!-- Link pp01062_stewart_arm_4 -->
+        <body name="pp01062_stewart_arm_4" pos="-0.01025 -0.0425915 -0.03705" quat="4.44089e-16 -1.5721e-15 -0.707107 -0.707107">
+          <!-- Joint from dc15_a01_case_b_dummy to pp01062_stewart_arm_4 -->
+          <joint axis="0 0 1" name="4" type="hinge" range="-1.3962634015954831 1.396263401595444" class="chosen_actuator"/>
+          <inertial pos="0.0181787 -5.24899e-19 0.00350559" mass="0.00272417" fullinertia="4.89963e-08 6.75761e-07 6.91798e-07 4.36416e-22 -7.04811e-08 3.66848e-23"/>
+          <!-- Part ball_4 -->
+          <geom type="mesh" class="visual" pos="0.038 -2.77556e-17 0.00768" quat="4.51946e-15 -0.707107 -0.707107 -1.08859e-15" mesh="ball" material="ball_material"/>
+          <!-- Part pp01062_stewart_arm_4 -->
+          <geom type="mesh" class="visual" pos="-0.01025 0.03 0.0425915" quat="0.707107 0.707107 -1.11022e-16 1.0222e-16" mesh="pp01062_stewart_arm" material="pp01062_stewart_arm_material"/>
+          <!-- Link shape_4 -->
+          <body name="shape_4" pos="0.038 -2.77556e-17 0.00768" quat="0.854409 0.0439152 0.0416306 0.516065">
+            <!-- Joint from pp01062_stewart_arm_4 to shape_4 -->
+            <joint axis="0 0 1" name="passive_4" type="ball" class="chosen_actuator"/>
+            <inertial pos="-0.0450002 1.04447e-16 -1.1361e-17" mass="0.00355525" fullinertia="1.7732e-08 3.06049e-06 3.0658e-06 6.31755e-21 -5.94417e-21 1.35905e-20"/>
+            <!-- Part shape_7 -->
+            <geom type="mesh" class="visual" pos="-0.0900003 2.08167e-16 3.46945e-18" quat="3.33067e-16 0 -1 1.66533e-16" mesh="shape" material="shape_material"/>
+            <!-- Part pp00xxx_stewart_rod_4 -->
+            <geom type="mesh" class="visual" pos="0.00371031 0.0370326 -0.0564076" quat="0.855513 0.00634803 -0.517727 0.00384161" mesh="pp00xxx_stewart_rod" material="pp00xxx_stewart_rod_material"/>
+            <!-- Part shape_8 -->
+            <geom type="mesh" class="visual" pos="6.93889e-18 0 3.46945e-18" quat="1 0 3.33067e-16 1.31839e-16" mesh="shape" material="shape_material"/>
+            <!-- Frame closing_4_1 -->
+            <site group="3" name="closing_4_1" pos="-0.0900003 2.08167e-16 3.46945e-18" quat="3.33067e-16 0 -1 1.66533e-16"/>
+          </body>
+        </body>
+        <!-- Link pp01062_stewart_arm_5 -->
+        <body name="pp01062_stewart_arm_5" pos="-0.0420103 0.012419 -0.03705" quat="0.353553 -0.353553 0.612372 -0.612372">
+          <!-- Joint from dc15_a01_case_b_dummy to pp01062_stewart_arm_5 -->
+          <joint axis="0 0 1" name="5" type="hinge" range="-1.3962634015954662 1.396263401595461" class="chosen_actuator"/>
+          <inertial pos="0.0181787 3.11301e-35 0.00350559" mass="0.00272417" fullinertia="4.89963e-08 6.75761e-07 6.91798e-07 -4.49492e-25 -7.04811e-08 -1.26741e-25"/>
+          <!-- Part pp01062_stewart_arm_5 -->
+          <geom type="mesh" class="visual" pos="-0.01025 0.03 0.0425915" quat="0.707107 0.707107 -1.11022e-16 9.55014e-17" mesh="pp01062_stewart_arm" material="pp01062_stewart_arm_material"/>
+          <!-- Part ball_5 -->
+          <geom type="mesh" class="visual" pos="0.038 0 0.00768" quat="1.38778e-17 5.55459e-17 1 3.33067e-16" mesh="ball" material="ball_material"/>
+          <!-- Link pp00xxx_stewart_rod -->
+          <body name="pp00xxx_stewart_rod" pos="0.038 1.38778e-17 0.00768" quat="0.0908372 -0.850701 0.516198 -0.0399478">
+            <!-- Joint from pp01062_stewart_arm_5 to pp00xxx_stewart_rod -->
+            <joint axis="0 0 1" name="passive_5" type="ball" class="chosen_actuator"/>
+            <inertial pos="-0.0450002 7.44667e-17 -1.14016e-17" mass="0.00355525" fullinertia="1.7732e-08 3.06049e-06 3.0658e-06 4.55471e-21 -4.43821e-21 1.23083e-21"/>
+            <!-- Part shape_9 -->
+            <geom type="mesh" class="visual" pos="0 0 0" quat="1 0 -2.22045e-16 0" mesh="shape" material="shape_material"/>
+            <!-- Part pp00xxx_stewart_rod_5 -->
+            <geom type="mesh" class="visual" pos="0.00371031 0.0370326 -0.0564076" quat="0.855513 0.00634803 -0.517727 0.00384161" mesh="pp00xxx_stewart_rod" material="pp00xxx_stewart_rod_material"/>
+            <!-- Part shape_10 -->
+            <geom type="mesh" class="visual" pos="-0.0900003 1.38778e-16 6.93889e-18" quat="9.99201e-16 -4.04899e-16 1 1.9984e-15" mesh="shape" material="shape_material"/>
+            <!-- Frame closing_5_1 -->
+            <site group="3" name="closing_5_1" pos="-0.0900003 1.38778e-16 6.93889e-18" quat="9.99201e-16 -4.04899e-16 1 1.9984e-15"/>
+          </body>
+        </body>
+        <!-- Link pp01062_stewart_arm_6 -->
+        <body name="pp01062_stewart_arm_6" pos="-0.0317603 0.0301725 -0.03705" quat="0.612372 0.612372 0.353553 0.353553">
+          <!-- Joint from dc15_a01_case_b_dummy to pp01062_stewart_arm_6 -->
+          <joint axis="0 0 1" name="6" type="hinge" range="-1.3962634015954636 1.3962634015954636" class="chosen_actuator"/>
+          <inertial pos="0.0181787 -1.69382e-17 0.00350559" mass="0.00272417" fullinertia="4.89963e-08 6.75761e-07 6.91798e-07 1.6528e-22 -7.04811e-08 3.48685e-23"/>
+          <!-- Part pp01062_stewart_arm_6 -->
+          <geom type="mesh" class="visual" pos="-0.01025 0.03 0.0425915" quat="0.707107 0.707107 -1.11022e-16 1.00194e-16" mesh="pp01062_stewart_arm" material="pp01062_stewart_arm_material"/>
+          <!-- Part ball_6 -->
+          <geom type="mesh" class="visual" pos="0.038 -2.77556e-17 0.00768" quat="0 1 0 0" mesh="ball" material="ball_material"/>
+          <!-- Link shape_5 -->
+          <body name="shape_5" pos="0.038 -2.77556e-17 0.00768" quat="0.0131438 -0.855436 -0.517229 0.0230347">
+            <!-- Joint from pp01062_stewart_arm_6 to shape_5 -->
+            <joint axis="0 0 1" name="passive_6" type="ball" class="chosen_actuator"/>
+            <inertial pos="-0.0450002 7.95049e-16 -1.5589e-17" mass="0.00355525" fullinertia="1.7732e-08 3.06049e-06 3.0658e-06 4.84585e-20 -8.92792e-21 -1.86218e-20"/>
+            <!-- Part shape_11 -->
+            <geom type="mesh" class="visual" pos="-0.0900003 1.83187e-15 6.93889e-18" quat="1.77636e-15 6.61483e-15 1 6.27276e-15" mesh="shape" material="shape_material"/>
+            <!-- Part shape_12 -->
+            <geom type="mesh" class="visual" pos="6.93889e-18 0 0" quat="1 1.00165e-17 -8.32667e-17 0" mesh="shape" material="shape_material"/>
+            <!-- Part pp00xxx_stewart_rod_6 -->
+            <geom type="mesh" class="visual" pos="0.00371031 0.0370326 -0.0564076" quat="0.855513 0.00634803 -0.517727 0.00384161" mesh="pp00xxx_stewart_rod" material="pp00xxx_stewart_rod_material"/>
+            <!-- Link pp01063_stewart_plateform -->
+            <body name="pp01063_stewart_plateform" pos="-0.0900003 1.88738e-15 -2.08167e-17" quat="0.0131438 0.855436 0.517229 -0.0230347">
+              <!-- Joint from shape_5 to pp01063_stewart_plateform -->
+              <joint axis="0 0 1" name="passive_7" type="ball" class="chosen_actuator"/>
+              <inertial pos="-0.00488914 -0.0469134 0.0291732" mass="0.168092" fullinertia="0.000413571 0.000428808 0.000324749 -2.28564e-06 -5.24367e-05 -9.58295e-07"/>
+              <!-- Part small_lens -->
+              <geom type="mesh" class="visual" pos="0.0262444 -0.0524 0.0136834" quat="0.612372 0.612372 -0.353553 0.353553" mesh="small_lens" material="small_lens_material"/>
+              <!-- Part m12_lens -->
+              <geom type="mesh" class="visual" pos="0.010421 -0.0524 -0.0147088" quat="0.183013 0.683013 -0.683013 -0.183013" mesh="m12_lens" material="m12_lens_material"/>
+              <!-- Part pp01080_back_small_eye -->
+              <geom type="mesh" class="visual" pos="0.0262444 -0.0524 0.0136834" quat="0.612372 0.612372 -0.353553 0.353553" mesh="pp01080_back_small_eye" material="pp01080_back_small_eye_material"/>
+              <!-- Part m12_lens_2 -->
+              <geom type="mesh" class="visual" pos="0.042581 -0.0524 0.0417792" quat="0.183013 0.683013 -0.683013 -0.183013" mesh="m12_lens" material="m12_lens_material"/>
+              <!-- Part ball_7 -->
+              <geom type="mesh" class="visual" pos="-0.013 5.55112e-17 -5.55112e-17" quat="1 1.3274e-15 5.77316e-15 -2.63678e-15" mesh="ball" material="ball_material"/>
+              <!-- Part uc_a37_rev_a_step -->
+              <geom type="mesh" class="visual" pos="0.0239513 -0.04265 0.0271316" quat="9.36644e-18 0.866025 0 0.5" mesh="uc_a37_rev_a_step" material="uc_a37_rev_a_step_material"/>
+              <!-- Part ball_8 -->
+              <geom type="mesh" class="visual" pos="-8.67362e-19 2.77556e-17 0" quat="1 -0 -2.04613e-16 -5.18024e-19" mesh="ball" material="ball_material"/>
+              <!-- Part pp01070_head_head_back -->
+              <geom type="mesh" class="visual" pos="-0.0134282 -0.0524 0.0365883" quat="0.612372 0.612372 -0.353553 0.353553" mesh="pp01070_head_head_back" material="pp01070_head_head_back_material"/>
+              <!-- Part ball_9 -->
+              <geom type="mesh" class="visual" pos="0.0249723 5.55112e-17 0.0432533" quat="1.36964e-17 0.866025 -1.11022e-16 -0.5" mesh="ball" material="ball_material"/>
+              <!-- Part pp01079_back_big_eye -->
+              <geom type="mesh" class="visual" pos="0.0265844 -0.0524 0.013487" quat="0.612372 0.612372 -0.353553 0.353553" mesh="pp01079_back_big_eye" material="pp01079_back_big_eye_material"/>
+              <!-- Part ball_10 -->
+              <geom type="mesh" class="visual" pos="-0.0379723 5.55112e-17 0.0432533" quat="0.353553 -0.612372 0.612372 -0.353553" mesh="ball" material="ball_material"/>
+              <!-- Part pp01069_head_shell_front -->
+              <geom type="mesh" class="visual" pos="-0.0142942 -0.0524 0.0370883" quat="0.612372 0.612372 -0.353553 0.353553" mesh="pp01069_head_shell_front" material="pp01069_head_shell_front_material"/>
+              <!-- Part pp01078_glasses -->
+              <geom type="mesh" class="visual" pos="0.025976 -0.0524 0.0138383" quat="0.612372 0.612372 -0.353553 0.353553" mesh="pp01078_glasses" material="pp01078_glasses_material"/>
+              <!-- Part pp01063_stewart_plateform -->
+              <geom type="mesh" class="visual" pos="-0.0065 0.109722 0.0325883" quat="0.353553 0.353553 0.612372 -0.612372" mesh="pp01063_stewart_plateform" material="pp01063_stewart_plateform_material"/>
+              <!-- Part ball_11 -->
+              <geom type="mesh" class="visual" pos="-0.0314723 2.77556e-17 0.0545117" quat="0.353553 -0.612372 0.612372 -0.353553" mesh="ball" material="ball_material"/>
+              <!-- Part big_lens -->
+              <geom type="mesh" class="visual" pos="0.0281677 -0.0524 0.0125729" quat="0.612372 0.612372 -0.353553 0.353553" mesh="big_lens" material="big_lens_material"/>
+              <!-- Part ball_12 -->
+              <geom type="mesh" class="visual" pos="0.0184723 5.55112e-17 0.0545117" quat="3.07775e-17 -0.866025 -0 0.5" mesh="ball" material="ball_material"/>
+              <!-- Frame closing_1_2 -->
+              <site group="3" name="closing_1_2" pos="0.0249723 5.55112e-17 0.0432533" quat="0.353553 0.612372 -0.612372 -0.353553"/>
+              <!-- Frame closing_2_1 -->
+              <site group="3" name="closing_2_1" pos="0.0184723 5.55112e-17 0.0545117" quat="3.07775e-17 -0.866025 -0 0.5"/>
+              <!-- Frame closing_3_1 -->
+              <site group="3" name="closing_3_1" pos="-0.0314723 2.77556e-17 0.0545117" quat="0.353553 -0.612372 0.612372 -0.353553"/>
+              <!-- Frame closing_4_2 -->
+              <site group="3" name="closing_4_2" pos="-0.0379723 5.55112e-17 0.0432533" quat="0.612372 0.353553 -0.353553 -0.612372"/>
+              <!-- Frame closing_5_2 -->
+              <site group="3" name="closing_5_2" pos="-0.013 5.55112e-17 -5.55112e-17" quat="0.707107 -3.16544e-15 5.10703e-15 -0.707107"/>
+              <!-- Frame head -->
+              <site group="3" name="head" pos="-0.0134282 -0.0012 0.0365883" quat="0.683013 0.683013 0.183013 -0.183013"/>
+              <!-- Frame camera -->
+              <site group="3" name="camera" pos="0.0244171 -0.0524 0.0147383" quat="0.683013 0.683013 0.183013 -0.183013"/>
+              <!-- Frame camera_optical -->
+              <site group="3" name="camera_optical" pos="0.0244171 -0.0524 0.0147383" quat="0.5 0 0.866025 -2.22045e-16"/>
+              <!-- Link test_antenna_body -->
+              <body name="test_antenna_body" pos="-0.0161053 -0.0744 0.100068" quat="0.964418 0.0356203 -0.25955 0.0355422">
+                <!-- Joint from pp01063_stewart_plateform to test_antenna_body -->
+                <joint axis="0 0 1" name="left_antenna" type="hinge" class="chosen_actuator"/>
+                <inertial pos="0.00147983 -0.0294086 0.00298611" mass="0.00456766" fullinertia="1.38179e-05 2.28739e-07 1.38227e-05 6.85535e-07 5.01796e-08 -9.97217e-07"/>
+                <!-- Part test_antenna -->
+                <geom type="mesh" class="visual" pos="0.0285777 0.0175124 -0.0571006" quat="0.716727 0.647969 0.191203 -0.172861" mesh="test_antenna" material="test_antenna_material"/>
+                <!-- Part test_antenna_body -->
+                <geom type="mesh" class="visual" pos="0.0285297 0.0184653 -0.0570171" quat="0.716727 0.647969 0.191203 -0.172861" mesh="test_antenna_body" material="test_antenna_body_material"/>
+              </body>
+              <!-- Link test_antenna -->
+              <body name="test_antenna" pos="-0.0697422 -0.0744 0.00716666" quat="0.706492 0.0909539 -0.701233 -0.0294913">
+                <!-- Joint from pp01063_stewart_plateform to test_antenna -->
+                <joint axis="0 0 1" name="right_antenna" type="hinge" class="chosen_actuator"/>
+                <inertial pos="0.00147983 -0.0294086 0.00298611" mass="0.00456766" fullinertia="1.38179e-05 2.28739e-07 1.38227e-05 6.85535e-07 5.01796e-08 -9.97217e-07"/>
+                <!-- Part test_antenna_2 -->
+                <geom type="mesh" class="visual" pos="0.0285777 0.0175124 -0.0571006" quat="0.716727 0.647969 0.191203 -0.172861" mesh="test_antenna" material="test_antenna_material"/>
+                <!-- Part test_antenna_body_2 -->
+                <geom type="mesh" class="visual" pos="0.0285297 0.0184653 -0.0570171" quat="0.716727 0.647969 0.191203 -0.172861" mesh="test_antenna_body" material="test_antenna_body_material"/>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+    </body>
+  </worldbody>
+  <asset>
+    <mesh file="dc15_a01_case_b_dummy.stl"/>
+    <mesh file="pp01078_glasses.stl"/>
+    <mesh file="big_lens.stl"/>
+    <mesh file="test_antenna_body.stl"/>
+    <mesh file="pp01065_stewart_side_plate.stl"/>
+    <mesh file="small_lens.stl"/>
+    <mesh file="pp01080_back_small_eye.stl"/>
+    <mesh file="pp01064_stewart_main_plate.stl"/>
+    <mesh file="shape.stl"/>
+    <mesh file="pp00xxx_stewart_rod.stl"/>
+    <mesh file="pp01079_back_big_eye.stl"/>
+    <mesh file="ball.stl"/>
+    <mesh file="wj_wk00_0123middlecase_56__configuration_default.stl"/>
+    <mesh file="test_antenna.stl"/>
+    <mesh file="pp01070_head_head_back.stl"/>
+    <mesh file="dc15_a01_case_f_dummy.stl"/>
+    <mesh file="drive_palonier__configuration_default.stl"/>
+    <mesh file="dc15_a01_horn_dummy.stl"/>
+    <mesh file="wj_wk00_0122topcabinetcase_95__configuration_default.stl"/>
+    <mesh file="pp01071_turning_bowl.stl"/>
+    <mesh file="pp01068_top_body.stl"/>
+    <mesh file="wj_wk00_0124bottomcase_45__configuration_default.stl"/>
+    <mesh file="uc_a37_rev_a_step.stl"/>
+    <mesh file="dc15_a01_case_m_dummy.stl"/>
+    <mesh file="pp01062_stewart_arm.stl"/>
+    <mesh file="pp01072_turning_end.stl"/>
+    <mesh file="m12_lens.stl"/>
+    <mesh file="pp01069_head_shell_front.stl"/>
+    <mesh file="pp01063_stewart_plateform.stl"/>
+    <mesh file="pp01066_stewart_mid_plate.stl"/>
+    <mesh file="pp01067_bottom_body.stl"/>
+    <material name="pp01071_turning_bowl_material" rgba="0.34902 0.376471 0.4 1"/>
+    <material name="pp01072_turning_end_material" rgba="0.262745 0.282353 0.301961 1"/>
+    <material name="pp01065_stewart_side_plate_material" rgba="0.196078 0.196078 0.196078 1"/>
+    <material name="pp01067_bottom_body_material" rgba="1 1 1 1"/>
+    <material name="dc15_a01_case_m_dummy_material" rgba="0.262745 0.282353 0.301961 1"/>
+    <material name="dc15_a01_case_f_dummy_material" rgba="0.262745 0.282353 0.301961 1"/>
+    <material name="dc15_a01_horn_dummy_material" rgba="0.262745 0.282353 0.301961 1"/>
+    <material name="wj_wk00_0122topcabinetcase_95__configuration_default_material" rgba="0.247059 0.243137 0.243137 1"/>
+    <material name="dc15_a01_case_b_dummy_material" rgba="0.262745 0.282353 0.301961 1"/>
+    <material name="pp01068_top_body_material" rgba="1 1 1 1"/>
+    <material name="drive_palonier__configuration_default_material" rgba="0.623529 0.666667 0.701961 1"/>
+    <material name="wj_wk00_0123middlecase_56__configuration_default_material" rgba="0.247059 0.243137 0.243137 1"/>
+    <material name="pp01066_stewart_mid_plate_material" rgba="0.196078 0.196078 0.196078 1"/>
+    <material name="wj_wk00_0124bottomcase_45__configuration_default_material" rgba="0.247059 0.243137 0.243137 1"/>
+    <material name="pp01064_stewart_main_plate_material" rgba="0.196078 0.196078 0.196078 1"/>
+    <material name="ball_material" rgba="0.901961 0.901961 0.901961 1"/>
+    <material name="pp01062_stewart_arm_material" rgba="0.196078 0.196078 0.196078 1"/>
+    <material name="shape_material" rgba="0.901961 0.901961 0.901961 1"/>
+    <material name="pp00xxx_stewart_rod_material" rgba="0.8 0.8 0.8 1"/>
+    <material name="small_lens_material" rgba="0.439216 0.47451 0.501961 0.298039"/>
+    <material name="m12_lens_material" rgba="0.262745 0.282353 0.301961 1"/>
+    <material name="pp01080_back_small_eye_material" rgba="0.168627 0.168627 0.168627 1"/>
+    <material name="uc_a37_rev_a_step_material" rgba="0.25098 0.25098 0.25098 1"/>
+    <material name="pp01070_head_head_back_material" rgba="1 1 1 1"/>
+    <material name="pp01079_back_big_eye_material" rgba="0.168627 0.168627 0.168627 1"/>
+    <material name="pp01069_head_shell_front_material" rgba="1 1 1 1"/>
+    <material name="pp01078_glasses_material" rgba="0.168627 0.168627 0.168627 1"/>
+    <material name="pp01063_stewart_plateform_material" rgba="1 1 1 1"/>
+    <material name="big_lens_material" rgba="0.439216 0.47451 0.501961 0.298039"/>
+    <material name="test_antenna_material" rgba="0.262745 0.282353 0.301961 1"/>
+    <material name="test_antenna_body_material" rgba="1 1 1 1"/>
+  </asset>
+  <actuator>
+    <position class="chosen_actuator" name="all_yaw" joint="all_yaw"/>
+    <position class="chosen_actuator" name="1" joint="1" inheritrange="1"/>
+    <position class="chosen_actuator" name="2" joint="2" inheritrange="1"/>
+    <position class="chosen_actuator" name="3" joint="3" inheritrange="1"/>
+    <position class="chosen_actuator" name="4" joint="4" inheritrange="1"/>
+    <position class="chosen_actuator" name="5" joint="5" inheritrange="1"/>
+    <position class="chosen_actuator" name="6" joint="6" inheritrange="1"/>
+    <position class="chosen_actuator" name="left_antenna" joint="left_antenna"/>
+    <position class="chosen_actuator" name="right_antenna" joint="right_antenna"/>
+  </actuator>
+  <equality>
+    <connect site1="closing_1_1" site2="closing_1_2"/>
+    <connect site1="closing_2_1" site2="closing_2_2"/>
+    <connect site1="closing_3_1" site2="closing_3_2"/>
+    <connect site1="closing_4_1" site2="closing_4_2"/>
+    <connect site1="closing_5_1" site2="closing_5_2"/>
+  </equality>
+</mujoco>

From c20106309637c7ded9dc1e0c25304630031482ab Mon Sep 17 00:00:00 2001
From: gospar <antun.skuric@outlook.com>
Date: Fri, 24 Oct 2025 10:26:25 +0200
Subject: [PATCH 08/15] relatively good for now

---
 examples/gui_demos/mini_head_lookat_gui.py    | 100 ++++
 .../{ => gui_demos}/mini_head_position_gui.py |   5 -
 .../daemon/backend/mujoco/backend copy.py     | 312 ------------
 .../daemon/backend/mujoco/backend.py          |  16 +-
 .../reachy_mini/mjcf/joints_properties.xml    |   4 +-
 .../reachy_mini/mjcf/reachy_mini copy 2.xml   | 450 ------------------
 .../reachy_mini/mjcf/reachy_mini copy.xml     | 448 -----------------
 .../reachy_mini/mjcf/reachy_mini.xml          |  19 +-
 .../reachy_mini/mjcf/reachy_mini.xml.old      | 440 -----------------
 .../kinematics/placo_kinematics.py            |  10 +-
 10 files changed, 124 insertions(+), 1680 deletions(-)
 create mode 100644 examples/gui_demos/mini_head_lookat_gui.py
 rename examples/{ => gui_demos}/mini_head_position_gui.py (93%)
 delete mode 100644 src/reachy_mini/daemon/backend/mujoco/backend copy.py
 delete mode 100644 src/reachy_mini/descriptions/reachy_mini/mjcf/reachy_mini copy 2.xml
 delete mode 100644 src/reachy_mini/descriptions/reachy_mini/mjcf/reachy_mini copy.xml
 delete mode 100644 src/reachy_mini/descriptions/reachy_mini/mjcf/reachy_mini.xml.old

diff --git a/examples/gui_demos/mini_head_lookat_gui.py b/examples/gui_demos/mini_head_lookat_gui.py
new file mode 100644
index 00000000..035c5148
--- /dev/null
+++ b/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 scipy.spatial.transform import Rotation as R
+
+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()
diff --git a/examples/mini_head_position_gui.py b/examples/gui_demos/mini_head_position_gui.py
similarity index 93%
rename from examples/mini_head_position_gui.py
rename to examples/gui_demos/mini_head_position_gui.py
index 7b0f732d..d6df8dbd 100644
--- a/examples/mini_head_position_gui.py
+++ b/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)
 
diff --git a/src/reachy_mini/daemon/backend/mujoco/backend copy.py b/src/reachy_mini/daemon/backend/mujoco/backend copy.py
deleted file mode 100644
index 881ce03b..00000000
--- a/src/reachy_mini/daemon/backend/mujoco/backend copy.py	
+++ /dev/null
@@ -1,312 +0,0 @@
-"""Mujoco Backend for Reachy Mini.
-
-This module provides the MujocoBackend class for simulating the Reachy Mini robot using the MuJoCo physics engine.
-
-It includes methods for running the simulation, getting joint positions, and controlling the robot's joints.
-
-"""
-
-import json
-import time
-from dataclasses import dataclass
-from importlib.resources import files
-from threading import Thread
-from typing import Optional
-
-import mujoco
-import mujoco.viewer
-import numpy as np
-
-import reachy_mini
-from reachy_mini.daemon.backend.abstract import Backend
-from reachy_mini.daemon.backend.mujoco.utils import (
-    get_actuator_names,
-    get_joint_addr_from_name,
-    get_joint_id_from_name,
-)
-from reachy_mini.daemon.backend.mujoco.video_udp import UDPJPEGFrameSender
-
-
-class MujocoBackend(Backend):
-    """Simulated Reachy Mini using MuJoCo."""
-
-    def __init__(self, scene="empty", check_collision: bool = False):
-        """Initialize the MujocoBackend with a specified scene.
-
-        Args:
-            scene (str): The name of the scene to load. Default is "empty".
-            check_collision (bool): If True, enable collision checking. Default is False.
-
-        """
-        super().__init__(check_collision=check_collision)
-
-        from reachy_mini.reachy_mini import (
-            SLEEP_ANTENNAS_JOINT_POSITIONS,
-            SLEEP_HEAD_JOINT_POSITIONS,
-        )
-
-        self._SLEEP_ANTENNAS_JOINT_POSITIONS = SLEEP_ANTENNAS_JOINT_POSITIONS
-        self._SLEEP_HEAD_JOINT_POSITIONS = SLEEP_HEAD_JOINT_POSITIONS
-
-        mjcf_root_path = str(
-            files(reachy_mini).joinpath("descriptions/reachy_mini/mjcf/")
-        )
-        self.model = mujoco.MjModel.from_xml_path(  # type: ignore
-            f"{mjcf_root_path}/scenes/{scene}.xml"
-        )
-        self.data = mujoco.MjData(self.model)  # type: ignore
-        self.model.opt.timestep = 0.002  # s, simulation timestep, 500hz
-        self.decimation = 10  # -> 50hz control loop
-        self.rendering_timestep = 0.04  # s, rendering loop # 25Hz
-
-        self.camera_id = mujoco.mj_name2id(  # type: ignore
-            self.model,
-            mujoco.mjtObj.mjOBJ_CAMERA,  # type: ignore
-            "eye_camera",
-        )
-
-        self.head_id = mujoco.mj_name2id(  # type: ignore
-            self.model,
-            mujoco.mjtObj.mjOBJ_BODY,  # type: ignore
-            "pp01063_stewart_plateform",
-        )
-
-        self.platform_to_head_transform = np.array(
-            [
-                [8.66025292e-01, 5.00000194e-01, -1.83660327e-06, -1.34282000e-02],
-                [5.55111512e-16, -3.67320510e-06, -1.00000000e00, -1.20000000e-03],
-                [-5.00000194e-01, 8.66025292e-01, -3.18108852e-06, 3.65883000e-02],
-                [0, 0, 0, 1.00000000e00],
-            ]
-        )
-        # remove_z_offset  = np.eye(4)
-        # remove_z_offset[2, 3] = -0.177
-        # self.platform_to_head_transform = self.platform_to_head_transform @ remove_z_offset
-
-        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 = [
-            get_joint_id_from_name(self.model, n) for n in self.joint_names
-        ]
-        self.joint_qpos_addr = [
-            get_joint_addr_from_name(self.model, n) for n in self.joint_names
-        ]
-
-        # disable the collisions by default 
-        self.model.geom_contype[13] = 0 # head capsule
-        self.model.geom_conaffinity[13] = 0 
-        self.model.geom_contype[80] = 0 # top of the torso 
-        self.model.geom_conaffinity[80] = 0
-
-    def rendering_loop(self):
-        """Offline Rendering loop for the Mujoco simulation.
-
-        Capture the image from the virtual Reachy's camera and send it over UDP.
-        """
-        streamer_udp = UDPJPEGFrameSender()
-        camera_size = (1280, 720)
-        offscreen_renderer = mujoco.Renderer(
-            self.model, height=camera_size[1], width=camera_size[0]
-        )
-        while not self.should_stop.is_set():
-            start_t = time.time()
-            offscreen_renderer.update_scene(self.data, self.camera_id)
-            im = offscreen_renderer.render()
-            streamer_udp.send_frame(im)
-
-            took = time.time() - start_t
-            time.sleep(max(0, self.rendering_timestep - took))
-
-
-
-    def run(self):
-        """Run the Mujoco simulation with a viewer.
-
-        This method initializes the viewer and enters the main simulation loop.
-        It updates the joint positions at a rate and publishes the joint positions.
-        """
-        step = 1
-        with mujoco.viewer.launch_passive(
-            self.model, self.data, show_left_ui=False, show_right_ui=False
-        ) as viewer:
-            with viewer.lock():
-                viewer.cam.type = mujoco.mjtCamera.mjCAMERA_FREE  # type: ignore
-                viewer.cam.distance = 0.8  # ≃ ||pos - lookat||
-                viewer.cam.azimuth = 160  # degrees
-                viewer.cam.elevation = -20  # degrees
-                viewer.cam.lookat[:] = [0, 0, 0.15]
-
-                # force one render with your new camera
-                mujoco.mj_step(self.model, self.data)  # type: ignore
-                viewer.sync()
-
-                # im = self.get_camera()
-                # self.streamer_udp.send_frame(im)
-            with viewer.lock():
-                self.data.qpos[self.joint_qpos_addr] = np.array(
-                    self._SLEEP_HEAD_JOINT_POSITIONS
-                    + self._SLEEP_ANTENNAS_JOINT_POSITIONS
-                ).reshape(-1, 1)
-                self.data.ctrl[:] = np.array(
-                    self._SLEEP_HEAD_JOINT_POSITIONS
-                    + self._SLEEP_ANTENNAS_JOINT_POSITIONS
-                )
-
-                # recompute all kinematics, collisions, etc.
-                mujoco.mj_forward(self.model, self.data)  # type: ignore
-                
-            for i in range(20):
-                # one more frame so the viewer shows your startup pose
-                mujoco.mj_step(self.model, self.data)  # type: ignore
-            viewer.sync()
-
-            rendering_thread = Thread(target=self.rendering_loop, daemon=True)
-            rendering_thread.start()
-            
-            
-            #if self.check_collision:
-            # enable collisions
-            self.model.geom_contype[13] = 1 # head capsule
-            self.model.geom_conaffinity[13] = 1
-            self.model.geom_contype[80] = 1 # top of the torso 
-            self.model.geom_conaffinity[80] = 1
-
-            mujoco.mj_step(self.model, self.data)
-            
-            
-            
-            # 3) now enter your normal loop
-            while not self.should_stop.is_set():
-                start_t = time.time()
-
-                if step % self.decimation == 0:
-                    # update the current states
-                    self.current_head_joint_positions = (
-                        self.get_present_head_joint_positions()
-                    )
-                    self.current_antenna_joint_positions = (
-                        self.get_present_antenna_joint_positions()
-                    )
-                    self.current_head_pose = self.get_mj_present_head_pose()
-
-                    # Update the target head joint positions from IK if necessary
-                    # - does nothing if the targets did not change
-                    if self.ik_required:
-                        self.update_target_head_joints_from_ik(
-                            self.target_head_pose, self.target_body_yaw
-                        )
-
-                    if self.target_head_joint_positions is not None:
-                        self.data.ctrl[:7] = self.target_head_joint_positions
-                    if self.target_antenna_joint_positions is not None:
-                        self.data.ctrl[-2:] = self.target_antenna_joint_positions
-
-                    if (
-                        self.joint_positions_publisher is not None
-                        and self.pose_publisher is not None
-                    ):
-                        self.joint_positions_publisher.put(
-                            json.dumps(
-                                {
-                                    "head_joint_positions": self.current_head_joint_positions,
-                                    "antennas_joint_positions": self.current_antenna_joint_positions,
-                                }
-                            ).encode("utf-8")
-                        )
-                        self.pose_publisher.put(
-                            json.dumps(
-                                {
-                                    "head_pose": self.get_present_head_pose().tolist(),
-                                }
-                            ).encode("utf-8")
-                        )
-                    viewer.sync()
-
-                mujoco.mj_step(self.model, self.data)  # type: ignore
-
-                took = time.time() - start_t
-                time.sleep(max(0, self.model.opt.timestep - took))
-                # print(f"Step {step}: took {took*1000:.1f}ms")
-                step += 1
-                self.ready.set()
-
-    def get_mj_present_head_pose(self) -> np.ndarray:
-        """Get the current head pose from the Mujoco simulation.
-
-        Returns:
-            np.ndarray: The current head pose as a 4x4 transformation matrix.
-
-        """
-        mj_current_head_pose = np.eye(4)
-        mj_current_head_pose[:3, :3] = self.data.xmat[self.head_id].reshape(3, 3)
-        mj_current_head_pose[:3, 3] = self.data.xpos[self.head_id]
-        mj_current_head_pose = mj_current_head_pose @ self.platform_to_head_transform
-        mj_current_head_pose[2, 3] -= 0.177
-        return mj_current_head_pose
-
-    def close(self) -> None:
-        """Close the Mujoco backend."""
-        # TODO Do something in mujoco here ?
-        pass
-
-    def get_status(self) -> "MujocoBackendStatus":
-        """Get the status of the Mujoco backend.
-
-        Returns:
-            dict: An empty dictionary as the Mujoco backend does not have a specific status to report.
-
-        """
-        return MujocoBackendStatus()
-
-    def get_present_head_joint_positions(self):
-        """Get the current joint positions of the head."""
-        return self.data.qpos[self.joint_qpos_addr[:7]].flatten().tolist()
-
-    def get_present_antenna_joint_positions(self):
-        """Get the current joint positions of the antennas."""
-        return self.data.qpos[self.joint_qpos_addr[-2:]].flatten().tolist()
-
-    def enable_motors(self) -> None:
-        """Enable the motors.
-
-        Does nothing in the Mujoco backend as it does not have a concept of enabling/disabling motors.
-        """
-        pass
-
-    def disable_motors(self) -> None:
-        """Disable the motors.
-
-        Does nothing in the Mujoco backend as it does not have a concept of enabling/disabling motors.
-        """
-        pass
-
-    def set_head_operation_mode(self, mode: int) -> None:
-        """Set mode of operation for the head.
-
-        This does nothing in the Mujoco backend as it does not have a concept of operation modes.
-        """
-        pass
-
-    def set_antennas_operation_mode(self, mode: int) -> None:
-        """Set mode of operation for the antennas.
-
-        This does nothing in the Mujoco backend as it does not have a concept of operation modes.
-        """
-        pass
-
-
-@dataclass
-class MujocoBackendStatus:
-    """Dataclass to represent the status of the Mujoco backend.
-
-    Empty for now, as the Mujoco backend does not have a specific status to report.
-    """
-
-    error: Optional[str] = None
diff --git a/src/reachy_mini/daemon/backend/mujoco/backend.py b/src/reachy_mini/daemon/backend/mujoco/backend.py
index 1b855aa2..9f023866 100644
--- a/src/reachy_mini/daemon/backend/mujoco/backend.py
+++ b/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 = [
@@ -114,11 +109,6 @@ def __init__(
                 self.model.geom_contype[i] = 0
                 self.model.geom_conaffinity[i] = 0
 
-        # # disable the collisions by default
-        # self.model.geom_contype[13] = 0 # head capsule
-        # self.model.geom_conaffinity[13] = 0 
-        # self.model.geom_contype[80] = 0 # top of the torso 
-        # self.model.geom_conaffinity[80] = 0
 
     def rendering_loop(self) -> None:
         """Offline Rendering loop for the Mujoco simulation.
@@ -183,6 +173,10 @@ def run(self) -> None:
         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)
@@ -252,7 +246,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:
diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/joints_properties.xml b/src/reachy_mini/descriptions/reachy_mini/mjcf/joints_properties.xml
index 0d7260b4..7a166408 100644
--- a/src/reachy_mini/descriptions/reachy_mini/mjcf/joints_properties.xml
+++ b/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">
diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/reachy_mini copy 2.xml b/src/reachy_mini/descriptions/reachy_mini/mjcf/reachy_mini copy 2.xml
deleted file mode 100644
index 8f8ccd2c..00000000
--- a/src/reachy_mini/descriptions/reachy_mini/mjcf/reachy_mini copy 2.xml	
+++ /dev/null
@@ -1,450 +0,0 @@
-<?xml version="1.0" ?>
-<!-- Generated using onshape-to-robot -->
-<!-- Onshape https://cad.onshape.com/documents/e305b26a84339af20c593973/w/ec7765eccdaa9a5960507125/e/251023ea04190ab16854eb80 -->
-<mujoco model="reachy_mini">
-  <compiler angle="radian" meshdir="assets" autolimits="true"/>
-  <default>
-    <default class="reachy_mini">
-      <joint frictionloss="0.1" armature="0.005"/>
-      <position kp="50" dampratio="1"/>
-      <default class="visual">
-        <geom type="mesh" contype="0" conaffinity="0" group="2"/>
-      </default>
-      <default class="collision">
-        <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.1" forcerange="-10.0 10.0"/>
-      <!-- <position kp="10.0" kv="0.0" forcerange="-10.0 10.0"/> -->
-      <default class="chosen_actuator"/>
-    </default>
-    <default class="xc330m288t">
-      <!-- probably wrong, would need to re-identify -->
-      <geom contype="0" conaffinity="0"/>
-      <joint damping="0.095" frictionloss="0.058" armature="0.0018"/>
-      <position kp="2.54" kv="0.0" forcerange="-0.8 0.8"/>
-    </default>
-    <default class="sts3215_345">
-      <!-- Standard sts3215 with 1/345 gear ratio -->
-      <!-- Confident that this is realistic (mini duck walks with these values) -->
-      <geom contype="0" conaffinity="0"/>
-      <joint damping="0.56" frictionloss="0.068" armature="0.027"/>
-      <position kp="17.11" kv="0.0" forcerange="-3.23 3.23"/>
-    </default>
-    <default class="sts3215_147">
-      <!-- sts3215 with 1/147 gear ratio -->
-      <!-- (Estimation based on the gear ratio difference) -->
-      <geom contype="0" conaffinity="0"/>
-      <joint damping="0.1" frictionloss="0.028" armature="0.011"/>
-      <position kp="17.11" kv="0.0" forcerange="-1.35 1.35"/>
-    </default>
-  </default>
-  <!-- Additional additional.xml -->
-  <contact>
-    <!-- <exclude name="exclude0" body1="torso" body2="sts32155"/> -->
-  </contact>
-  <default>
-    <equality solref="0.002 1" solimp="0.99 0.999 0.0005 0.5 2"/>
-  </default>
-  <worldbody>
-    <!-- Link pp01071_turning_bowl -->
-    <body name="pp01071_turning_bowl" pos="0 0 0" quat="1 0 0 0" childclass="reachy_mini">
-      <inertial pos="0.000667521 1.7878e-09 0.0118411" mass="0.0947234" fullinertia="0.000111359 0.000106563 0.000196298 1.02485e-11 8.96229e-07 4.95917e-13"/>
-      <!-- Part pp01071_turning_bowl -->
-      <geom type="mesh" class="visual" pos="-1.38778e-17 0 0.2345" quat="0.707107 0 -6.16298e-32 0.707107" mesh="pp01071_turning_bowl" material="pp01071_turning_bowl_material"/>
-      <!-- Part pp01072_turning_end -->
-      <geom type="mesh" class="visual" pos="3.46945e-17 -4.51028e-17 0.2345" quat="0.707107 7.85046e-17 -2.22045e-16 0.707107" mesh="pp01072_turning_end" material="pp01072_turning_end_material"/>
-      <!-- Link dc15_a01_case_b_dummy -->
-      <body name="dc15_a01_case_b_dummy" pos="4.85723e-17 -5.60663e-19 0.0339" quat="7.85046e-17 -0.707107 -0.707107 -7.85046e-17">
-        <!-- Joint from pp01071_turning_bowl to dc15_a01_case_b_dummy -->
-        <joint axis="0 0 1" name="all_yaw" type="hinge" class="chosen_actuator"/>
-        <inertial pos="-1.60377e-06 0.00140187 -0.0329593" mass="0.443795" fullinertia="0.00159333 0.0015483 0.00187065 2.80397e-07 1.98452e-07 4.26838e-05"/>
-        <!-- Part pp01065_stewart_side_plate -->
-        <geom type="mesh" class="visual" pos="-3.43031e-17 -4.85723e-17 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="pp01065_stewart_side_plate" material="pp01065_stewart_side_plate_material"/>
-        <!-- Part pp01067_bottom_body -->
-        <geom type="mesh" class="visual" pos="1.38767e-16 1.83881e-16 -0.20055" quat="9.16593e-17 1 -3.29597e-17 5.68353e-17" mesh="pp01067_bottom_body" material="pp01067_bottom_body_material"/>
-        <!-- Part dc15_a01_case_m_dummy -->
-        <geom type="mesh" class="visual" pos="-1.4706e-16 -8.67362e-17 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_case_m_dummy" material="dc15_a01_case_m_dummy_material"/>
-        <!-- Part dc15_a01_case_f_dummy -->
-        <geom type="mesh" class="visual" pos="0.0205 6.93889e-18 -0.00705" quat="3.33067e-16 3.64292e-17 1 -4.44089e-16" mesh="dc15_a01_case_f_dummy" material="dc15_a01_case_f_dummy_material"/>
-        <!-- Part dc15_a01_horn_dummy -->
-        <geom type="mesh" class="visual" pos="0.0205 1.73472e-17 -0.00705" quat="3.33067e-16 3.64292e-17 1 -4.44089e-16" mesh="dc15_a01_horn_dummy" material="dc15_a01_horn_dummy_material"/>
-        <!-- Part dc15_a01_horn_dummy_2 -->
-        <geom type="mesh" class="visual" pos="-0.01025 0.0177535 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_horn_dummy" material="dc15_a01_horn_dummy_material"/>
-        <!-- Part wj_wk00_0122topcabinetcase_95__configuration_default -->
-        <geom type="mesh" class="visual" pos="6.12843e-18 -0.0255 -0.00945" quat="0.5 0.5 -0.5 -0.5" mesh="wj_wk00_0122topcabinetcase_95__configuration_default" material="wj_wk00_0122topcabinetcase_95__configuration_default_material"/>
-        <!-- Part dc15_a01_case_m_dummy_2 -->
-        <geom type="mesh" class="visual" pos="-2.56295e-17 3.1225e-17 -0.00705" quat="1.11022e-16 2.60209e-17 1 -2.22045e-16" mesh="dc15_a01_case_m_dummy" material="dc15_a01_case_m_dummy_material"/>
-        <!-- Part dc15_a01_case_b_dummy -->
-        <geom type="mesh" class="visual" pos="-0.01025 -0.0177535 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_case_b_dummy" material="dc15_a01_case_b_dummy_material"/>
-        <!-- Part dc15_a01_case_b_dummy_2 -->
-        <geom type="mesh" class="visual" pos="0.0205 3.46945e-18 -0.00705" quat="3.33067e-16 3.64292e-17 1 -4.44089e-16" mesh="dc15_a01_case_b_dummy" material="dc15_a01_case_b_dummy_material"/>
-        <!-- Part pp01068_top_body -->
-        <geom type="mesh" class="collision" contype="1" conaffinity="1" pos="-8.55341e-16 -3.12493e-14 -0.20005" quat="9.19752e-14 1 -3.64292e-17 -2.66929e-15" mesh="pp01068_top_body" material="pp01068_top_body_material"/>
-        <!-- Part dc15_a01_case_f_dummy_2 -->
-        <geom type="mesh" class="visual" pos="-3.43031e-17 -3.46945e-18 -0.00705" quat="3.33067e-16 3.64292e-17 1 -4.44089e-16" mesh="dc15_a01_case_f_dummy" material="dc15_a01_case_f_dummy_material"/>
-        <!-- Part drive_palonier__configuration_default -->
-        <geom type="mesh" class="visual" pos="4.39371e-18 -0.0255 -0.00945" quat="0.5 0.5 -0.5 -0.5" mesh="drive_palonier__configuration_default" material="drive_palonier__configuration_default_material"/>
-        <!-- Part dc15_a01_case_f_dummy_3 -->
-        <geom type="mesh" class="visual" pos="-0.01025 0.0177535 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_case_f_dummy" material="dc15_a01_case_f_dummy_material"/>
-        <!-- Part dc15_a01_case_m_dummy_3 -->
-        <geom type="mesh" class="visual" pos="-3.43031e-17 -6.245e-17 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_case_m_dummy" material="dc15_a01_case_m_dummy_material"/>
-        <!-- Part dc15_a01_case_b_dummy_3 -->
-        <geom type="mesh" class="visual" pos="-9.1549e-17 -6.245e-17 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_case_b_dummy" material="dc15_a01_case_b_dummy_material"/>
-        <!-- Part wj_wk00_0123middlecase_56__configuration_default -->
-        <geom type="mesh" class="visual" pos="-1.75347e-05 -0.0254895 -0.00945" quat="0.5 0.5 -0.5 -0.5" mesh="wj_wk00_0123middlecase_56__configuration_default" material="wj_wk00_0123middlecase_56__configuration_default_material"/>
-        <!-- Part dc15_a01_case_b_dummy_4 -->
-        <geom type="mesh" class="visual" pos="-3.60378e-17 -1.04083e-17 -0.00705" quat="3.33067e-16 3.64292e-17 1 -4.44089e-16" mesh="dc15_a01_case_b_dummy" material="dc15_a01_case_b_dummy_material"/>
-        <!-- Part dc15_a01_case_f_dummy_4 -->
-        <geom type="mesh" class="visual" pos="-2.26857e-16 -1.00614e-16 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_case_f_dummy" material="dc15_a01_case_f_dummy_material"/>
-        <!-- Part dc15_a01_case_b_dummy_5 -->
-        <geom type="mesh" class="visual" pos="-0.01025 0.0177535 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_case_b_dummy" material="dc15_a01_case_b_dummy_material"/>
-        <!-- Part dc15_a01_horn_dummy_3 -->
-        <geom type="mesh" class="visual" pos="-1.15835e-16 -6.93889e-17 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_horn_dummy" material="dc15_a01_horn_dummy_material"/>
-        <!-- Part dc15_a01_horn_dummy_4 -->
-        <geom type="mesh" class="visual" pos="-0.01025 -0.0177535 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_horn_dummy" material="dc15_a01_horn_dummy_material"/>
-        <!-- Part dc15_a01_case_m_dummy_4 -->
-        <geom type="mesh" class="visual" pos="-0.01025 0.0177535 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_case_m_dummy" material="dc15_a01_case_m_dummy_material"/>
-        <!-- Part pp01065_stewart_side_plate_2 -->
-        <geom type="mesh" class="visual" pos="-1.34864e-17 6.59195e-17 -0.00705" quat="0 3.64292e-17 1 0" mesh="pp01065_stewart_side_plate" material="pp01065_stewart_side_plate_material"/>
-        <!-- Part dc15_a01_case_b_dummy_6 -->
-        <geom type="mesh" class="visual" pos="-6.37934e-17 -3.46945e-17 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_case_b_dummy" material="dc15_a01_case_b_dummy_material"/>
-        <!-- Part pp01066_stewart_mid_plate -->
-        <geom type="mesh" class="visual" pos="-8.28754e-17 -1.04083e-17 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="pp01066_stewart_mid_plate" material="pp01066_stewart_mid_plate_material"/>
-        <!-- Part dc15_a01_case_f_dummy_5 -->
-        <geom type="mesh" class="visual" pos="-3.77726e-17 -4.85723e-17 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_case_f_dummy" material="dc15_a01_case_f_dummy_material"/>
-        <!-- Part dc15_a01_horn_dummy_5 -->
-        <geom type="mesh" class="visual" pos="-5.51198e-17 -4.85723e-17 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_horn_dummy" material="dc15_a01_horn_dummy_material"/>
-        <!-- Part dc15_a01_case_m_dummy_5 -->
-        <geom type="mesh" class="visual" pos="0.0205 3.81639e-17 -0.00705" quat="1.11022e-16 2.60209e-17 1 -2.22045e-16" mesh="dc15_a01_case_m_dummy" material="dc15_a01_case_m_dummy_material"/>
-        <!-- Part wj_wk00_0124bottomcase_45__configuration_default -->
-        <geom type="mesh" class="visual" pos="-1.04636e-05 -0.0254966 -0.00945" quat="0.5 0.5 -0.5 -0.5" mesh="wj_wk00_0124bottomcase_45__configuration_default" material="wj_wk00_0124bottomcase_45__configuration_default_material"/>
-        <!-- Part dc15_a01_horn_dummy_6 -->
-        <geom type="mesh" class="visual" pos="-3.25684e-17 1.04083e-17 -0.00705" quat="3.33067e-16 3.64292e-17 1 -4.44089e-16" mesh="dc15_a01_horn_dummy" material="dc15_a01_horn_dummy_material"/>
-        <!-- Part dc15_a01_case_m_dummy_6 -->
-        <geom type="mesh" class="visual" pos="-0.01025 -0.0177535 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_case_m_dummy" material="dc15_a01_case_m_dummy_material"/>
-        <!-- Part pp01064_stewart_main_plate -->
-        <geom type="mesh" class="visual" pos="-2.73642e-17 -1.38778e-17 -0.00705" quat="1.11022e-16 -8.67362e-18 -1 -1.11022e-16" mesh="pp01064_stewart_main_plate" material="pp01064_stewart_main_plate_material"/>
-        <!-- Part dc15_a01_case_f_dummy_6 -->
-        <geom type="mesh" class="visual" pos="-0.01025 -0.0177535 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_case_f_dummy" material="dc15_a01_case_f_dummy_material"/>
-        <!-- Part pp01065_stewart_side_plate_3 -->
-        <geom type="mesh" class="visual" pos="-6.20587e-17 -1.04083e-17 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="pp01065_stewart_side_plate" material="pp01065_stewart_side_plate_material"/>
-        <!-- Link pp01062_stewart_arm -->
-        <body name="pp01062_stewart_arm" pos="0.0317603 0.0301725 -0.03705" quat="0.353553 -0.353553 -0.612372 0.612372">
-          <!-- Joint from dc15_a01_case_b_dummy to pp01062_stewart_arm -->
-          <joint axis="0 0 1" name="1" type="hinge" range="-1.3962634015954645 1.3962634015954627" class="chosen_actuator"/>
-          <inertial pos="0.0181787 2.77556e-17 0.00350559" mass="0.00272417" fullinertia="4.89963e-08 6.75761e-07 6.91798e-07 -2.63422e-25 -7.04811e-08 2.66043e-25"/>
-          <!-- Part ball -->
-          <geom type="mesh" class="visual" pos="0.038 2.77556e-17 0.00768" quat="0 1 0 0" mesh="ball" material="ball_material"/>
-          <!-- Part pp01062_stewart_arm -->
-          <geom type="mesh" class="visual" pos="-0.01025 0.03 0.0425915" quat="0.707107 0.707107 -1.11022e-16 8.49837e-17" mesh="pp01062_stewart_arm" material="pp01062_stewart_arm_material"/>
-          <!-- Link shape -->
-          <body name="shape" pos="0.038 2.77556e-17 0.00768" quat="0.0274567 -0.855096 0.517739 -0.00153082">
-            <!-- Joint from pp01062_stewart_arm to shape -->
-            <joint axis="0 0 1" name="passive_1" type="ball" class="chosen_actuator"/>
-            <inertial pos="-0.0450002 -2.17877e-17 -4.85069e-17" mass="0.00355525" fullinertia="1.7732e-08 3.06049e-06 3.0658e-06 3.75919e-22 -8.93073e-21 -1.00288e-21"/>
-            <!-- Part shape -->
-            <geom type="mesh" class="visual" pos="6.93889e-18 -1.38778e-17 0" quat="1 1.53003e-17 -1.11022e-16 -1.249e-16" mesh="shape" material="shape_material"/>
-            <!-- Part pp00xxx_stewart_rod -->
-            <geom type="mesh" class="visual" pos="0.00371031 0.0370326 -0.0564076" quat="0.855513 0.00634803 -0.517727 0.00384161" mesh="pp00xxx_stewart_rod" material="pp00xxx_stewart_rod_material"/>
-            <!-- Part shape_2 -->
-            <geom type="mesh" class="visual" pos="-0.0900003 -2.77556e-17 -8.32667e-17" quat="1.66533e-16 1.57422e-17 -1 1.11022e-16" mesh="shape" material="shape_material"/>
-            <!-- Frame closing_1_1 -->
-            <site group="3" name="closing_1_1" pos="-0.0900003 -2.77556e-17 -8.32667e-17" quat="1.66533e-16 1.57422e-17 -1 1.11022e-16"/>
-          </body>
-        </body>
-        <!-- Link pp01062_stewart_arm_2 -->
-        <body name="pp01062_stewart_arm_2" pos="0.0420103 0.012419 -0.03705" quat="0.612372 0.612372 -0.353553 -0.353553">
-          <!-- Joint from dc15_a01_case_b_dummy to pp01062_stewart_arm_2 -->
-          <joint axis="0 0 1" name="2" type="hinge" range="-1.3962634015954662 1.396263401595461" class="chosen_actuator"/>
-          <inertial pos="0.0181787 2.34834e-18 0.00350559" mass="0.00272417" fullinertia="4.89963e-08 6.75761e-07 6.91798e-07 2.48336e-22 -7.04811e-08 5.24015e-23"/>
-          <!-- Part ball_2 -->
-          <geom type="mesh" class="visual" pos="0.038 -1.38778e-17 0.00768" quat="1.66533e-16 -2.4037e-17 -1 -1.11022e-16" mesh="ball" material="ball_material"/>
-          <!-- Part pp01062_stewart_arm_2 -->
-          <geom type="mesh" class="visual" pos="-0.01025 0.03 0.0425915" quat="0.707107 0.707107 -1.11022e-16 8.24487e-17" mesh="pp01062_stewart_arm" material="pp01062_stewart_arm_material"/>
-          <!-- Link shape_2 -->
-          <body name="shape_2" pos="0.038 -8.32667e-17 0.00768" quat="0.0131699 -0.855436 -0.517228 0.0230506">
-            <!-- Joint from pp01062_stewart_arm_2 to shape_2 -->
-            <joint axis="0 0 1" name="passive_2" type="ball" class="chosen_actuator"/>
-            <inertial pos="-0.0450002 -1.00413e-17 -8.56034e-18" mass="0.00355525" fullinertia="1.7732e-08 3.06049e-06 3.0658e-06 9.05427e-21 -9.31125e-21 -1.13245e-20"/>
-            <!-- Part shape_3 -->
-            <geom type="mesh" class="visual" pos="-0.0900003 -2.77556e-17 2.08167e-17" quat="2.22045e-16 2.94872e-15 1 3.85803e-15" mesh="shape" material="shape_material"/>
-            <!-- Part shape_4 -->
-            <geom type="mesh" class="visual" pos="6.93889e-18 0 6.93889e-18" quat="1 -0 -0 -4.85723e-17" mesh="shape" material="shape_material"/>
-            <!-- Part pp00xxx_stewart_rod_2 -->
-            <geom type="mesh" class="visual" pos="0.00371031 0.0370326 -0.0564076" quat="0.855513 0.00634803 -0.517727 0.00384161" mesh="pp00xxx_stewart_rod" material="pp00xxx_stewart_rod_material"/>
-            <!-- Frame closing_2_2 -->
-            <site group="3" name="closing_2_2" pos="-0.0900003 -2.77556e-17 2.08167e-17" quat="2.78561e-15 -0.707107 0.707107 2.55257e-15"/>
-          </body>
-        </body>
-        <!-- Link pp01062_stewart_arm_3 -->
-        <body name="pp01062_stewart_arm_3" pos="0.01025 -0.0425915 -0.03705" quat="0.707107 -0.707107 -1.11022e-16 1.84877e-17">
-          <!-- Joint from dc15_a01_case_b_dummy to pp01062_stewart_arm_3 -->
-          <joint axis="0 0 1" name="3" type="hinge" range="-1.3962634015954645 1.3962634015954627" class="chosen_actuator"/>
-          <inertial pos="0.0181787 -1.87132e-19 0.00350559" mass="0.00272417" fullinertia="4.89963e-08 6.75761e-07 6.91798e-07 9.85709e-24 -7.04811e-08 -1.47314e-24"/>
-          <!-- Part pp01062_stewart_arm_3 -->
-          <geom type="mesh" class="visual" pos="-0.01025 0.03 0.0425915" quat="0.707107 0.707107 -1.11022e-16 1.0222e-16" mesh="pp01062_stewart_arm" material="pp01062_stewart_arm_material"/>
-          <!-- Part ball_3 -->
-          <geom type="mesh" class="visual" pos="0.038 0 0.00768" quat="1.45894e-16 -0.707107 0.707107 5.21114e-18" mesh="ball" material="ball_material"/>
-          <!-- Link shape_3 -->
-          <body name="shape_3" pos="0.038 4.16334e-17 0.00768" quat="0.85509 -0.027647 0.0318018 -0.516764">
-            <!-- Joint from pp01062_stewart_arm_3 to shape_3 -->
-            <joint axis="0 0 1" name="passive_3" type="ball" class="chosen_actuator"/>
-            <inertial pos="-0.0450002 -4.79199e-18 4.11785e-17" mass="0.00355525" fullinertia="1.7732e-08 3.06049e-06 3.0658e-06 9.80819e-22 -7.34923e-21 1.45141e-21"/>
-            <!-- Part shape_5 -->
-            <geom type="mesh" class="visual" pos="-0.0900003 0 1.31839e-16" quat="4.16334e-16 4.4964e-17 1 -2.22045e-16" mesh="shape" material="shape_material"/>
-            <!-- Part shape_6 -->
-            <geom type="mesh" class="visual" pos="1.38778e-17 0 0" quat="1 -0 -0 2.56739e-16" mesh="shape" material="shape_material"/>
-            <!-- Part pp00xxx_stewart_rod_3 -->
-            <geom type="mesh" class="visual" pos="0.00371031 0.0370326 -0.0564076" quat="0.855513 0.00634803 -0.517727 0.00384161" mesh="pp00xxx_stewart_rod" material="pp00xxx_stewart_rod_material"/>
-            <!-- Frame closing_3_2 -->
-            <site group="3" name="closing_3_2" pos="-0.0900003 0 1.31839e-16" quat="0 4.06771e-16 -3.33067e-16 -1"/>
-          </body>
-        </body>
-        <!-- Link pp01062_stewart_arm_4 -->
-        <body name="pp01062_stewart_arm_4" pos="-0.01025 -0.0425915 -0.03705" quat="4.44089e-16 -1.5721e-15 -0.707107 -0.707107">
-          <!-- Joint from dc15_a01_case_b_dummy to pp01062_stewart_arm_4 -->
-          <joint axis="0 0 1" name="4" type="hinge" range="-1.3962634015954831 1.396263401595444" class="chosen_actuator"/>
-          <inertial pos="0.0181787 -5.24899e-19 0.00350559" mass="0.00272417" fullinertia="4.89963e-08 6.75761e-07 6.91798e-07 4.36416e-22 -7.04811e-08 3.66848e-23"/>
-          <!-- Part ball_4 -->
-          <geom type="mesh" class="visual" pos="0.038 -2.77556e-17 0.00768" quat="4.51946e-15 -0.707107 -0.707107 -1.08859e-15" mesh="ball" material="ball_material"/>
-          <!-- Part pp01062_stewart_arm_4 -->
-          <geom type="mesh" class="visual" pos="-0.01025 0.03 0.0425915" quat="0.707107 0.707107 -1.11022e-16 1.0222e-16" mesh="pp01062_stewart_arm" material="pp01062_stewart_arm_material"/>
-          <!-- Link shape_4 -->
-          <body name="shape_4" pos="0.038 -2.77556e-17 0.00768" quat="0.854409 0.0439152 0.0416306 0.516065">
-            <!-- Joint from pp01062_stewart_arm_4 to shape_4 -->
-            <joint axis="0 0 1" name="passive_4" type="ball" class="chosen_actuator"/>
-            <inertial pos="-0.0450002 1.04447e-16 -1.1361e-17" mass="0.00355525" fullinertia="1.7732e-08 3.06049e-06 3.0658e-06 6.31755e-21 -5.94417e-21 1.35905e-20"/>
-            <!-- Part shape_7 -->
-            <geom type="mesh" class="visual" pos="-0.0900003 2.08167e-16 3.46945e-18" quat="3.33067e-16 0 -1 1.66533e-16" mesh="shape" material="shape_material"/>
-            <!-- Part pp00xxx_stewart_rod_4 -->
-            <geom type="mesh" class="visual" pos="0.00371031 0.0370326 -0.0564076" quat="0.855513 0.00634803 -0.517727 0.00384161" mesh="pp00xxx_stewart_rod" material="pp00xxx_stewart_rod_material"/>
-            <!-- Part shape_8 -->
-            <geom type="mesh" class="visual" pos="6.93889e-18 0 3.46945e-18" quat="1 0 3.33067e-16 1.31839e-16" mesh="shape" material="shape_material"/>
-            <!-- Frame closing_4_1 -->
-            <site group="3" name="closing_4_1" pos="-0.0900003 2.08167e-16 3.46945e-18" quat="3.33067e-16 0 -1 1.66533e-16"/>
-          </body>
-        </body>
-        <!-- Link pp01062_stewart_arm_5 -->
-        <body name="pp01062_stewart_arm_5" pos="-0.0420103 0.012419 -0.03705" quat="0.353553 -0.353553 0.612372 -0.612372">
-          <!-- Joint from dc15_a01_case_b_dummy to pp01062_stewart_arm_5 -->
-          <joint axis="0 0 1" name="5" type="hinge" range="-1.3962634015954662 1.396263401595461" class="chosen_actuator"/>
-          <inertial pos="0.0181787 3.11301e-35 0.00350559" mass="0.00272417" fullinertia="4.89963e-08 6.75761e-07 6.91798e-07 -4.49492e-25 -7.04811e-08 -1.26741e-25"/>
-          <!-- Part pp01062_stewart_arm_5 -->
-          <geom type="mesh" class="visual" pos="-0.01025 0.03 0.0425915" quat="0.707107 0.707107 -1.11022e-16 9.55014e-17" mesh="pp01062_stewart_arm" material="pp01062_stewart_arm_material"/>
-          <!-- Part ball_5 -->
-          <geom type="mesh" class="visual" pos="0.038 0 0.00768" quat="1.38778e-17 5.55459e-17 1 3.33067e-16" mesh="ball" material="ball_material"/>
-          <!-- Link pp00xxx_stewart_rod -->
-          <body name="pp00xxx_stewart_rod" pos="0.038 1.38778e-17 0.00768" quat="0.0908372 -0.850701 0.516198 -0.0399478">
-            <!-- Joint from pp01062_stewart_arm_5 to pp00xxx_stewart_rod -->
-            <joint axis="0 0 1" name="passive_5" type="ball" class="chosen_actuator"/>
-            <inertial pos="-0.0450002 7.44667e-17 -1.14016e-17" mass="0.00355525" fullinertia="1.7732e-08 3.06049e-06 3.0658e-06 4.55471e-21 -4.43821e-21 1.23083e-21"/>
-            <!-- Part shape_9 -->
-            <geom type="mesh" class="visual" pos="0 0 0" quat="1 0 -2.22045e-16 0" mesh="shape" material="shape_material"/>
-            <!-- Part pp00xxx_stewart_rod_5 -->
-            <geom type="mesh" class="visual" pos="0.00371031 0.0370326 -0.0564076" quat="0.855513 0.00634803 -0.517727 0.00384161" mesh="pp00xxx_stewart_rod" material="pp00xxx_stewart_rod_material"/>
-            <!-- Part shape_10 -->
-            <geom type="mesh" class="visual" pos="-0.0900003 1.38778e-16 6.93889e-18" quat="9.99201e-16 -4.04899e-16 1 1.9984e-15" mesh="shape" material="shape_material"/>
-            <!-- Frame closing_5_1 -->
-            <site group="3" name="closing_5_1" pos="-0.0900003 1.38778e-16 6.93889e-18" quat="9.99201e-16 -4.04899e-16 1 1.9984e-15"/>
-          </body>
-        </body>
-        <!-- Link pp01062_stewart_arm_6 -->
-        <body name="pp01062_stewart_arm_6" pos="-0.0317603 0.0301725 -0.03705" quat="0.612372 0.612372 0.353553 0.353553">
-          <!-- Joint from dc15_a01_case_b_dummy to pp01062_stewart_arm_6 -->
-          <joint axis="0 0 1" name="6" type="hinge" range="-1.3962634015954636 1.3962634015954636" class="chosen_actuator"/>
-          <inertial pos="0.0181787 -1.69382e-17 0.00350559" mass="0.00272417" fullinertia="4.89963e-08 6.75761e-07 6.91798e-07 1.6528e-22 -7.04811e-08 3.48685e-23"/>
-          <!-- Part pp01062_stewart_arm_6 -->
-          <geom type="mesh" class="visual" pos="-0.01025 0.03 0.0425915" quat="0.707107 0.707107 -1.11022e-16 1.00194e-16" mesh="pp01062_stewart_arm" material="pp01062_stewart_arm_material"/>
-          <!-- Part ball_6 -->
-          <geom type="mesh" class="visual" pos="0.038 -2.77556e-17 0.00768" quat="0 1 0 0" mesh="ball" material="ball_material"/>
-          <!-- Link shape_5 -->
-          <body name="shape_5" pos="0.038 -2.77556e-17 0.00768" quat="0.0131438 -0.855436 -0.517229 0.0230347">
-            <!-- Joint from pp01062_stewart_arm_6 to shape_5 -->
-            <joint axis="0 0 1" name="passive_6" type="ball" class="chosen_actuator"/>
-            <inertial pos="-0.0450002 7.95049e-16 -1.5589e-17" mass="0.00355525" fullinertia="1.7732e-08 3.06049e-06 3.0658e-06 4.84585e-20 -8.92792e-21 -1.86218e-20"/>
-            <!-- Part shape_11 -->
-            <geom type="mesh" class="visual" pos="-0.0900003 1.83187e-15 6.93889e-18" quat="1.77636e-15 6.61483e-15 1 6.27276e-15" mesh="shape" material="shape_material"/>
-            <!-- Part shape_12 -->
-            <geom type="mesh" class="visual" pos="6.93889e-18 0 0" quat="1 1.00165e-17 -8.32667e-17 0" mesh="shape" material="shape_material"/>
-            <!-- Part pp00xxx_stewart_rod_6 -->
-            <geom type="mesh" class="visual" pos="0.00371031 0.0370326 -0.0564076" quat="0.855513 0.00634803 -0.517727 0.00384161" mesh="pp00xxx_stewart_rod" material="pp00xxx_stewart_rod_material"/>
-            <!-- Link pp01063_stewart_plateform -->
-            <body name="pp01063_stewart_plateform" pos="-0.0900003 1.88738e-15 -2.08167e-17" quat="0.0131438 0.855436 0.517229 -0.0230347">
-              <!-- Joint from shape_5 to pp01063_stewart_plateform -->
-              <joint axis="0 0 1" name="passive_7" type="ball" class="chosen_actuator"/>
-              <inertial pos="-0.00488914 -0.0469134 0.0291732" mass="0.168092" fullinertia="0.000413571 0.000428808 0.000324749 -2.28564e-06 -5.24367e-05 -9.58295e-07"/>
-              <!-- Part small_lens -->
-              <geom type="mesh" class="visual" pos="0.0262444 -0.0524 0.0136834" quat="0.612372 0.612372 -0.353553 0.353553" mesh="small_lens" material="small_lens_material"/>
-              <!-- Part m12_lens -->
-              <geom type="mesh" class="visual" pos="0.010421 -0.0524 -0.0147088" quat="0.183013 0.683013 -0.683013 -0.183013" mesh="m12_lens" material="m12_lens_material"/>
-              <!-- Part pp01080_back_small_eye -->
-              <geom type="mesh" class="visual" pos="0.0262444 -0.0524 0.0136834" quat="0.612372 0.612372 -0.353553 0.353553" mesh="pp01080_back_small_eye" material="pp01080_back_small_eye_material"/>
-              <!-- Part m12_lens_2 -->
-              <geom type="mesh" class="visual" pos="0.042581 -0.0524 0.0417792" quat="0.183013 0.683013 -0.683013 -0.183013" mesh="m12_lens" material="m12_lens_material"/>
-              <!-- Part ball_7 -->
-              <geom type="mesh" class="visual" pos="-0.013 5.55112e-17 -5.55112e-17" quat="1 1.3274e-15 5.77316e-15 -2.63678e-15" mesh="ball" material="ball_material"/>
-              <!-- Part uc_a37_rev_a_step -->
-              <geom type="mesh" class="visual" pos="0.0239513 -0.04265 0.0271316" quat="9.36644e-18 0.866025 0 0.5" mesh="uc_a37_rev_a_step" material="uc_a37_rev_a_step_material"/>
-              <!-- Part ball_8 -->
-              <geom type="mesh" class="visual" pos="-8.67362e-19 2.77556e-17 0" quat="1 -0 -2.04613e-16 -5.18024e-19" mesh="ball" material="ball_material"/>
-              <!-- Part pp01070_head_head_back -->
-              <geom type="mesh" class="visual" pos="-0.0134282 -0.0524 0.0365883" quat="0.612372 0.612372 -0.353553 0.353553" mesh="pp01070_head_head_back" material="pp01070_head_head_back_material"/>
-              <!-- Part ball_9 -->
-              <geom type="mesh" class="visual" pos="0.0249723 5.55112e-17 0.0432533" quat="1.36964e-17 0.866025 -1.11022e-16 -0.5" mesh="ball" material="ball_material"/>
-              <!-- Part pp01079_back_big_eye -->
-              <geom type="mesh" class="visual" pos="0.0265844 -0.0524 0.013487" quat="0.612372 0.612372 -0.353553 0.353553" mesh="pp01079_back_big_eye" material="pp01079_back_big_eye_material"/>
-              <!-- Part ball_10 -->
-              <geom type="mesh" class="visual" pos="-0.0379723 5.55112e-17 0.0432533" quat="0.353553 -0.612372 0.612372 -0.353553" mesh="ball" material="ball_material"/>
-              <!-- Part pp01069_head_shell_front -->
-              <geom type="mesh"  class="visual" pos="-0.0142942 -0.0524 0.0370883" quat="0.612372 0.612372 -0.353553 0.353553"  mesh="pp01069_head_shell_front" material="pp01069_head_shell_front_material"/>
-              <!-- <geom type="capsule" class="collision"  quat="0.612372 0.612372 -0.353553 0.353553" name="head_collider" size="0.05"/> -->
-              <geom type="capsule" class="collision" name="head_collider" size="0.048" fromto="0.005 -0.05 0.060068 -0.0257422 -0.05 0.00716666"/>
-              <!-- Part pp01078_glasses -->
-              <geom type="mesh" class="visual" pos="0.025976 -0.0524 0.0138383" quat="0.612372 0.612372 -0.353553 0.353553" mesh="pp01078_glasses" material="pp01078_glasses_material"/>
-              <!-- Part pp01063_stewart_plateform -->
-              <geom type="mesh" class="visual" pos="-0.0065 0.109722 0.0325883" quat="0.353553 0.353553 0.612372 -0.612372" mesh="pp01063_stewart_plateform" material="pp01063_stewart_plateform_material"/>
-              <!-- Part ball_11 -->
-              <geom type="mesh" class="visual" pos="-0.0314723 2.77556e-17 0.0545117" quat="0.353553 -0.612372 0.612372 -0.353553" mesh="ball" material="ball_material"/>
-              <!-- Part big_lens -->
-              <geom type="mesh" class="visual" pos="0.0281677 -0.0524 0.0125729" quat="0.612372 0.612372 -0.353553 0.353553" mesh="big_lens" material="big_lens_material"/>
-              <!-- Part ball_12 -->
-              <geom type="mesh" class="visual" pos="0.0184723 5.55112e-17 0.0545117" quat="3.07775e-17 -0.866025 -0 0.5" mesh="ball" material="ball_material"/>
-              <!-- Frame closing_1_2 -->
-              <site group="3" name="closing_1_2" pos="0.0249723 5.55112e-17 0.0432533" quat="0.353553 0.612372 -0.612372 -0.353553"/>
-              <!-- Frame closing_2_1 -->
-              <site group="3" name="closing_2_1" pos="0.0184723 5.55112e-17 0.0545117" quat="3.07775e-17 -0.866025 -0 0.5"/>
-              <!-- Frame closing_3_1 -->
-              <site group="3" name="closing_3_1" pos="-0.0314723 2.77556e-17 0.0545117" quat="0.353553 -0.612372 0.612372 -0.353553"/>
-              <!-- Frame closing_4_2 -->
-              <site group="3" name="closing_4_2" pos="-0.0379723 5.55112e-17 0.0432533" quat="0.612372 0.353553 -0.353553 -0.612372"/>
-              <!-- Frame closing_5_2 -->
-              <site group="3" name="closing_5_2" pos="-0.013 5.55112e-17 -5.55112e-17" quat="0.707107 -3.16544e-15 5.10703e-15 -0.707107"/>
-              <!-- Frame head -->
-              <site group="3" name="head" pos="-0.0134282 -0.0012 0.0365883" quat="0.683013 0.683013 0.183013 -0.183013"/>
-              <!-- Frame camera -->
-              <body pos="0.0244171 -0.0524 0.0147383" quat="-0.000 -0.500 0.000 0.866">
-                <site group="3" name="camera"/>
-                <camera name="eye_camera"
-                 mode="fixed"
-                 resolution="1280 720"
-                />
-
-              </body>
-              <!-- Frame camera_optical -->
-              <site group="3" name="camera_optical" pos="0.0244171 -0.0524 0.0147383" quat="0.5 0 0.866025 -2.22045e-16"/>
-              <!-- Link test_antenna_body -->
-              <body name="test_antenna_body" pos="-0.0161053 -0.0744 0.100068" quat="0.964418 0.0356203 -0.25955 0.0355422">
-                <!-- Joint from pp01063_stewart_plateform to test_antenna_body -->
-                <joint axis="0 0 1" name="left_antenna" type="hinge" class="chosen_actuator"/>
-                <inertial pos="0.00147983 -0.0294086 0.00298611" mass="0.00456766" fullinertia="1.38179e-05 2.28739e-07 1.38227e-05 6.85535e-07 5.01796e-08 -9.97217e-07"/>
-                <!-- Part test_antenna -->
-                <geom type="mesh" class="visual" pos="0.0285777 0.0175124 -0.0571006" quat="0.716727 0.647969 0.191203 -0.172861" mesh="test_antenna" material="test_antenna_material"/>
-                <!-- Part test_antenna_body -->
-                <geom type="mesh" class="visual" pos="0.0285297 0.0184653 -0.0570171" quat="0.716727 0.647969 0.191203 -0.172861" mesh="test_antenna_body" material="test_antenna_body_material"/>
-              </body>
-              <!-- Link test_antenna -->
-              <body name="test_antenna" pos="-0.0697422 -0.0744 0.00716666" quat="0.706492 0.0909539 -0.701233 -0.0294913">
-                <!-- Joint from pp01063_stewart_plateform to test_antenna -->
-                <joint axis="0 0 1" name="right_antenna" type="hinge" class="chosen_actuator"/>
-                <inertial pos="0.00147983 -0.0294086 0.00298611" mass="0.00456766" fullinertia="1.38179e-05 2.28739e-07 1.38227e-05 6.85535e-07 5.01796e-08 -9.97217e-07"/>
-                <!-- Part test_antenna_2 -->
-                <geom type="mesh" class="visual" pos="0.0285777 0.0175124 -0.0571006" quat="0.716727 0.647969 0.191203 -0.172861" mesh="test_antenna" material="test_antenna_material"/>
-                <!-- Part test_antenna_body_2 -->
-                <geom type="mesh" class="visual" pos="0.0285297 0.0184653 -0.0570171" quat="0.716727 0.647969 0.191203 -0.172861" mesh="test_antenna_body" material="test_antenna_body_material"/>
-              </body>
-            </body>
-          </body>
-        </body>
-      </body>
-    </body>
-  </worldbody>
-  <asset>
-    <mesh file="dc15_a01_case_b_dummy.stl"/>
-    <mesh file="pp01078_glasses.stl"/>
-    <mesh file="big_lens.stl"/>
-    <mesh file="test_antenna_body.stl"/>
-    <mesh file="pp01065_stewart_side_plate.stl"/>
-    <mesh file="small_lens.stl"/>
-    <mesh file="pp01080_back_small_eye.stl"/>
-    <mesh file="pp01064_stewart_main_plate.stl"/>
-    <mesh file="shape.stl"/>
-    <mesh file="pp00xxx_stewart_rod.stl"/>
-    <mesh file="pp01079_back_big_eye.stl"/>
-    <mesh file="ball.stl"/>
-    <mesh file="wj_wk00_0123middlecase_56__configuration_default.stl"/>
-    <mesh file="test_antenna.stl"/>
-    <mesh file="pp01070_head_head_back.stl"/>
-    <mesh file="dc15_a01_case_f_dummy.stl"/>
-    <mesh file="drive_palonier__configuration_default.stl"/>
-    <mesh file="dc15_a01_horn_dummy.stl"/>
-    <mesh file="wj_wk00_0122topcabinetcase_95__configuration_default.stl"/>
-    <mesh file="pp01071_turning_bowl.stl"/>
-    <mesh file="pp01068_top_body.stl"/>
-    <mesh file="wj_wk00_0124bottomcase_45__configuration_default.stl"/>
-    <mesh file="uc_a37_rev_a_step.stl"/>
-    <mesh file="dc15_a01_case_m_dummy.stl"/>
-    <mesh file="pp01062_stewart_arm.stl"/>
-    <mesh file="pp01072_turning_end.stl"/>
-    <mesh file="m12_lens.stl"/>
-    <mesh file="pp01069_head_shell_front.stl"/>
-    <mesh file="pp01063_stewart_plateform.stl"/>
-    <mesh file="pp01066_stewart_mid_plate.stl"/>
-    <mesh file="pp01067_bottom_body.stl"/>
-    <material name="pp01071_turning_bowl_material" rgba="0.34902 0.376471 0.4 1"/>
-    <material name="pp01072_turning_end_material" rgba="0.262745 0.282353 0.301961 1"/>
-    <material name="pp01065_stewart_side_plate_material" rgba="0.196078 0.196078 0.196078 1"/>
-    <material name="pp01067_bottom_body_material" rgba="1 1 1 1"/>
-    <material name="dc15_a01_case_m_dummy_material" rgba="0.262745 0.282353 0.301961 1"/>
-    <material name="dc15_a01_case_f_dummy_material" rgba="0.262745 0.282353 0.301961 1"/>
-    <material name="dc15_a01_horn_dummy_material" rgba="0.262745 0.282353 0.301961 1"/>
-    <material name="wj_wk00_0122topcabinetcase_95__configuration_default_material" rgba="0.247059 0.243137 0.243137 1"/>
-    <material name="dc15_a01_case_b_dummy_material" rgba="0.262745 0.282353 0.301961 1"/>
-    <material name="pp01068_top_body_material" rgba="1 1 1 1"/>
-    <material name="drive_palonier__configuration_default_material" rgba="0.623529 0.666667 0.701961 1"/>
-    <material name="wj_wk00_0123middlecase_56__configuration_default_material" rgba="0.247059 0.243137 0.243137 1"/>
-    <material name="pp01066_stewart_mid_plate_material" rgba="0.196078 0.196078 0.196078 1"/>
-    <material name="wj_wk00_0124bottomcase_45__configuration_default_material" rgba="0.247059 0.243137 0.243137 1"/>
-    <material name="pp01064_stewart_main_plate_material" rgba="0.196078 0.196078 0.196078 1"/>
-    <material name="ball_material" rgba="0.901961 0.901961 0.901961 1"/>
-    <material name="pp01062_stewart_arm_material" rgba="0.196078 0.196078 0.196078 1"/>
-    <material name="shape_material" rgba="0.901961 0.901961 0.901961 1"/>
-    <material name="pp00xxx_stewart_rod_material" rgba="0.8 0.8 0.8 1"/>
-    <material name="small_lens_material" rgba="0.439216 0.47451 0.501961 0.298039"/>
-    <material name="m12_lens_material" rgba="0.262745 0.282353 0.301961 1"/>
-    <material name="pp01080_back_small_eye_material" rgba="0.168627 0.168627 0.168627 1"/>
-    <material name="uc_a37_rev_a_step_material" rgba="0.25098 0.25098 0.25098 1"/>
-    <material name="pp01070_head_head_back_material" rgba="1 1 1 1"/>
-    <material name="pp01079_back_big_eye_material" rgba="0.168627 0.168627 0.168627 1"/>
-    <material name="pp01069_head_shell_front_material" rgba="1 1 1 1"/>
-    <material name="pp01078_glasses_material" rgba="0.168627 0.168627 0.168627 1"/>
-    <material name="pp01063_stewart_plateform_material" rgba="1 1 1 1"/>
-    <material name="big_lens_material" rgba="0.439216 0.47451 0.501961 0.298039"/>
-    <material name="test_antenna_material" rgba="0.262745 0.282353 0.301961 1"/>
-    <material name="test_antenna_body_material" rgba="1 1 1 1"/>
-  </asset>
-  <actuator>
-    <position class="chosen_actuator" name="all_yaw" joint="all_yaw" />
-    <position class="chosen_actuator" name="1" joint="1" inheritrange="1"/>
-    <position class="chosen_actuator" name="2" joint="2" inheritrange="1"/>
-    <position class="chosen_actuator" name="3" joint="3" inheritrange="1"/>
-    <position class="chosen_actuator" name="4" joint="4" inheritrange="1"/>
-    <position class="chosen_actuator" name="5" joint="5" inheritrange="1"/>
-    <position class="chosen_actuator" name="6" joint="6" inheritrange="1"/>
-    <position class="chosen_actuator" name="left_antenna" joint="left_antenna"/>
-    <position class="chosen_actuator" name="right_antenna" joint="right_antenna"/>
-  </actuator>
-  <equality>
-    <connect site1="closing_1_1" site2="closing_1_2"/>
-    <connect site1="closing_2_1" site2="closing_2_2"/>
-    <connect site1="closing_3_1" site2="closing_3_2"/>
-    <connect site1="closing_4_1" site2="closing_4_2"/>
-    <connect site1="closing_5_1" site2="closing_5_2"/>
-  </equality>
-</mujoco>
diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/reachy_mini copy.xml b/src/reachy_mini/descriptions/reachy_mini/mjcf/reachy_mini copy.xml
deleted file mode 100644
index a97dbc09..00000000
--- a/src/reachy_mini/descriptions/reachy_mini/mjcf/reachy_mini copy.xml	
+++ /dev/null
@@ -1,448 +0,0 @@
-<?xml version="1.0" ?>
-<!-- Generated using onshape-to-robot -->
-<!-- Onshape https://cad.onshape.com/documents/e305b26a84339af20c593973/w/ec7765eccdaa9a5960507125/e/251023ea04190ab16854eb80 -->
-<mujoco model="reachy_mini">
-  <compiler angle="radian" meshdir="assets" autolimits="true"/>
-  <default>
-    <default class="reachy_mini">
-      <joint frictionloss="0.1" armature="0.005"/>
-      <position kp="50" dampratio="1"/>
-      <default class="visual">
-        <geom type="mesh" contype="0" conaffinity="0" group="2"/>
-      </default>
-      <default class="collision">
-        <geom 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.1" forcerange="-20.0 20.0"/>
-      <!-- <position kp="10.0" kv="0.0" forcerange="-10.0 10.0"/> -->
-      <default class="chosen_actuator"/>
-    </default>
-    <default class="xc330m288t">
-      <!-- probably wrong, would need to re-identify -->
-      <geom contype="0" conaffinity="0"/>
-      <joint damping="0.095" frictionloss="0.058" armature="0.0018"/>
-      <position kp="2.54" kv="0.0" forcerange="-0.8 0.8"/>
-    </default>
-    <default class="sts3215_345">
-      <!-- Standard sts3215 with 1/345 gear ratio -->
-      <!-- Confident that this is realistic (mini duck walks with these values) -->
-      <geom contype="0" conaffinity="0"/>
-      <joint damping="0.56" frictionloss="0.068" armature="0.027"/>
-      <position kp="17.11" kv="0.0" forcerange="-3.23 3.23"/>
-    </default>
-    <default class="sts3215_147">
-      <!-- sts3215 with 1/147 gear ratio -->
-      <!-- (Estimation based on the gear ratio difference) -->
-      <geom contype="0" conaffinity="0"/>
-      <joint damping="0.1" frictionloss="0.028" armature="0.011"/>
-      <position kp="17.11" kv="0.0" forcerange="-1.35 1.35"/>
-    </default>
-  </default>
-  <!-- Additional additional.xml -->
-  <contact>
-    <!-- <exclude name="exclude0" body1="torso" body2="sts32155"/> -->
-  </contact>
-  <default>
-    <equality solref="0.002 1" solimp="0.99 0.999 0.0005 0.5 2"/>
-  </default>
-  <worldbody>
-    <!-- Link pp01071_turning_bowl -->
-    <body name="pp01071_turning_bowl" pos="0 0 0" quat="1 0 0 0" childclass="reachy_mini">
-      <inertial pos="0.000667521 1.7878e-09 0.0118411" mass="0.0947234" fullinertia="0.000111359 0.000106563 0.000196298 1.02485e-11 8.96229e-07 4.95917e-13"/>
-      <!-- Part pp01071_turning_bowl -->
-      <geom type="mesh" class="visual" pos="-1.38778e-17 0 0.2345" quat="0.707107 0 -6.16298e-32 0.707107" mesh="pp01071_turning_bowl" material="pp01071_turning_bowl_material"/>
-      <!-- Part pp01072_turning_end -->
-      <geom type="mesh" class="visual" pos="3.46945e-17 -4.51028e-17 0.2345" quat="0.707107 7.85046e-17 -2.22045e-16 0.707107" mesh="pp01072_turning_end" material="pp01072_turning_end_material"/>
-      <!-- Link dc15_a01_case_b_dummy -->
-      <body name="dc15_a01_case_b_dummy" pos="4.85723e-17 -5.60663e-19 0.0339" quat="7.85046e-17 -0.707107 -0.707107 -7.85046e-17">
-        <!-- Joint from pp01071_turning_bowl to dc15_a01_case_b_dummy -->
-        <joint axis="0 0 1" name="all_yaw" type="hinge" class="chosen_actuator"/>
-        <inertial pos="-1.60377e-06 0.00140187 -0.0329593" mass="0.443795" fullinertia="0.00159333 0.0015483 0.00187065 2.80397e-07 1.98452e-07 4.26838e-05"/>
-        <!-- Part pp01065_stewart_side_plate -->
-        <geom type="mesh" class="visual" pos="-3.43031e-17 -4.85723e-17 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="pp01065_stewart_side_plate" material="pp01065_stewart_side_plate_material"/>
-        <!-- Part pp01067_bottom_body -->
-        <geom type="mesh" class="visual" pos="1.38767e-16 1.83881e-16 -0.20055" quat="9.16593e-17 1 -3.29597e-17 5.68353e-17" mesh="pp01067_bottom_body" material="pp01067_bottom_body_material"/>
-        <!-- Part dc15_a01_case_m_dummy -->
-        <geom type="mesh" class="visual" pos="-1.4706e-16 -8.67362e-17 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_case_m_dummy" material="dc15_a01_case_m_dummy_material"/>
-        <!-- Part dc15_a01_case_f_dummy -->
-        <geom type="mesh" class="visual" pos="0.0205 6.93889e-18 -0.00705" quat="3.33067e-16 3.64292e-17 1 -4.44089e-16" mesh="dc15_a01_case_f_dummy" material="dc15_a01_case_f_dummy_material"/>
-        <!-- Part dc15_a01_horn_dummy -->
-        <geom type="mesh" class="visual" pos="0.0205 1.73472e-17 -0.00705" quat="3.33067e-16 3.64292e-17 1 -4.44089e-16" mesh="dc15_a01_horn_dummy" material="dc15_a01_horn_dummy_material"/>
-        <!-- Part dc15_a01_horn_dummy_2 -->
-        <geom type="mesh" class="visual" pos="-0.01025 0.0177535 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_horn_dummy" material="dc15_a01_horn_dummy_material"/>
-        <!-- Part wj_wk00_0122topcabinetcase_95__configuration_default -->
-        <geom type="mesh" class="visual" pos="6.12843e-18 -0.0255 -0.00945" quat="0.5 0.5 -0.5 -0.5" mesh="wj_wk00_0122topcabinetcase_95__configuration_default" material="wj_wk00_0122topcabinetcase_95__configuration_default_material"/>
-        <!-- Part dc15_a01_case_m_dummy_2 -->
-        <geom type="mesh" class="visual" pos="-2.56295e-17 3.1225e-17 -0.00705" quat="1.11022e-16 2.60209e-17 1 -2.22045e-16" mesh="dc15_a01_case_m_dummy" material="dc15_a01_case_m_dummy_material"/>
-        <!-- Part dc15_a01_case_b_dummy -->
-        <geom type="mesh" class="visual" pos="-0.01025 -0.0177535 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_case_b_dummy" material="dc15_a01_case_b_dummy_material"/>
-        <!-- Part dc15_a01_case_b_dummy_2 -->
-        <geom type="mesh" class="visual" pos="0.0205 3.46945e-18 -0.00705" quat="3.33067e-16 3.64292e-17 1 -4.44089e-16" mesh="dc15_a01_case_b_dummy" material="dc15_a01_case_b_dummy_material"/>
-        <!-- Part pp01068_top_body -->
-        <geom type="mesh" class="visual" pos="-8.55341e-16 -3.12493e-14 -0.20005" quat="9.19752e-14 1 -3.64292e-17 -2.66929e-15" mesh="pp01068_top_body" material="pp01068_top_body_material"/>
-        <!-- Part dc15_a01_case_f_dummy_2 -->
-        <geom type="mesh" class="visual" pos="-3.43031e-17 -3.46945e-18 -0.00705" quat="3.33067e-16 3.64292e-17 1 -4.44089e-16" mesh="dc15_a01_case_f_dummy" material="dc15_a01_case_f_dummy_material"/>
-        <!-- Part drive_palonier__configuration_default -->
-        <geom type="mesh" class="visual" pos="4.39371e-18 -0.0255 -0.00945" quat="0.5 0.5 -0.5 -0.5" mesh="drive_palonier__configuration_default" material="drive_palonier__configuration_default_material"/>
-        <!-- Part dc15_a01_case_f_dummy_3 -->
-        <geom type="mesh" class="visual" pos="-0.01025 0.0177535 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_case_f_dummy" material="dc15_a01_case_f_dummy_material"/>
-        <!-- Part dc15_a01_case_m_dummy_3 -->
-        <geom type="mesh" class="visual" pos="-3.43031e-17 -6.245e-17 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_case_m_dummy" material="dc15_a01_case_m_dummy_material"/>
-        <!-- Part dc15_a01_case_b_dummy_3 -->
-        <geom type="mesh" class="visual" pos="-9.1549e-17 -6.245e-17 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_case_b_dummy" material="dc15_a01_case_b_dummy_material"/>
-        <!-- Part wj_wk00_0123middlecase_56__configuration_default -->
-        <geom type="mesh" class="visual" pos="-1.75347e-05 -0.0254895 -0.00945" quat="0.5 0.5 -0.5 -0.5" mesh="wj_wk00_0123middlecase_56__configuration_default" material="wj_wk00_0123middlecase_56__configuration_default_material"/>
-        <!-- Part dc15_a01_case_b_dummy_4 -->
-        <geom type="mesh" class="visual" pos="-3.60378e-17 -1.04083e-17 -0.00705" quat="3.33067e-16 3.64292e-17 1 -4.44089e-16" mesh="dc15_a01_case_b_dummy" material="dc15_a01_case_b_dummy_material"/>
-        <!-- Part dc15_a01_case_f_dummy_4 -->
-        <geom type="mesh" class="visual" pos="-2.26857e-16 -1.00614e-16 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_case_f_dummy" material="dc15_a01_case_f_dummy_material"/>
-        <!-- Part dc15_a01_case_b_dummy_5 -->
-        <geom type="mesh" class="visual" pos="-0.01025 0.0177535 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_case_b_dummy" material="dc15_a01_case_b_dummy_material"/>
-        <!-- Part dc15_a01_horn_dummy_3 -->
-        <geom type="mesh" class="visual" pos="-1.15835e-16 -6.93889e-17 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_horn_dummy" material="dc15_a01_horn_dummy_material"/>
-        <!-- Part dc15_a01_horn_dummy_4 -->
-        <geom type="mesh" class="visual" pos="-0.01025 -0.0177535 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_horn_dummy" material="dc15_a01_horn_dummy_material"/>
-        <!-- Part dc15_a01_case_m_dummy_4 -->
-        <geom type="mesh" class="visual" pos="-0.01025 0.0177535 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_case_m_dummy" material="dc15_a01_case_m_dummy_material"/>
-        <!-- Part pp01065_stewart_side_plate_2 -->
-        <geom type="mesh" class="visual" pos="-1.34864e-17 6.59195e-17 -0.00705" quat="0 3.64292e-17 1 0" mesh="pp01065_stewart_side_plate" material="pp01065_stewart_side_plate_material"/>
-        <!-- Part dc15_a01_case_b_dummy_6 -->
-        <geom type="mesh" class="visual" pos="-6.37934e-17 -3.46945e-17 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_case_b_dummy" material="dc15_a01_case_b_dummy_material"/>
-        <!-- Part pp01066_stewart_mid_plate -->
-        <geom type="mesh" class="visual" pos="-8.28754e-17 -1.04083e-17 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="pp01066_stewart_mid_plate" material="pp01066_stewart_mid_plate_material"/>
-        <!-- Part dc15_a01_case_f_dummy_5 -->
-        <geom type="mesh" class="visual" pos="-3.77726e-17 -4.85723e-17 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_case_f_dummy" material="dc15_a01_case_f_dummy_material"/>
-        <!-- Part dc15_a01_horn_dummy_5 -->
-        <geom type="mesh" class="visual" pos="-5.51198e-17 -4.85723e-17 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_horn_dummy" material="dc15_a01_horn_dummy_material"/>
-        <!-- Part dc15_a01_case_m_dummy_5 -->
-        <geom type="mesh" class="visual" pos="0.0205 3.81639e-17 -0.00705" quat="1.11022e-16 2.60209e-17 1 -2.22045e-16" mesh="dc15_a01_case_m_dummy" material="dc15_a01_case_m_dummy_material"/>
-        <!-- Part wj_wk00_0124bottomcase_45__configuration_default -->
-        <geom type="mesh" class="visual" pos="-1.04636e-05 -0.0254966 -0.00945" quat="0.5 0.5 -0.5 -0.5" mesh="wj_wk00_0124bottomcase_45__configuration_default" material="wj_wk00_0124bottomcase_45__configuration_default_material"/>
-        <!-- Part dc15_a01_horn_dummy_6 -->
-        <geom type="mesh" class="visual" pos="-3.25684e-17 1.04083e-17 -0.00705" quat="3.33067e-16 3.64292e-17 1 -4.44089e-16" mesh="dc15_a01_horn_dummy" material="dc15_a01_horn_dummy_material"/>
-        <!-- Part dc15_a01_case_m_dummy_6 -->
-        <geom type="mesh" class="visual" pos="-0.01025 -0.0177535 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_case_m_dummy" material="dc15_a01_case_m_dummy_material"/>
-        <!-- Part pp01064_stewart_main_plate -->
-        <geom type="mesh" class="visual" pos="-2.73642e-17 -1.38778e-17 -0.00705" quat="1.11022e-16 -8.67362e-18 -1 -1.11022e-16" mesh="pp01064_stewart_main_plate" material="pp01064_stewart_main_plate_material"/>
-        <!-- Part dc15_a01_case_f_dummy_6 -->
-        <geom type="mesh" class="visual" pos="-0.01025 -0.0177535 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_case_f_dummy" material="dc15_a01_case_f_dummy_material"/>
-        <!-- Part pp01065_stewart_side_plate_3 -->
-        <geom type="mesh" class="visual" pos="-6.20587e-17 -1.04083e-17 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="pp01065_stewart_side_plate" material="pp01065_stewart_side_plate_material"/>
-        <!-- Link pp01062_stewart_arm -->
-        <body name="pp01062_stewart_arm" pos="0.0317603 0.0301725 -0.03705" quat="0.353553 -0.353553 -0.612372 0.612372">
-          <!-- Joint from dc15_a01_case_b_dummy to pp01062_stewart_arm -->
-          <joint axis="0 0 1" name="1" type="hinge" range="-1.3962634015954645 1.3962634015954627" class="chosen_actuator"/>
-          <inertial pos="0.0181787 2.77556e-17 0.00350559" mass="0.00272417" fullinertia="4.89963e-08 6.75761e-07 6.91798e-07 -2.63422e-25 -7.04811e-08 2.66043e-25"/>
-          <!-- Part ball -->
-          <geom type="mesh" class="visual" pos="0.038 2.77556e-17 0.00768" quat="0 1 0 0" mesh="ball" material="ball_material"/>
-          <!-- Part pp01062_stewart_arm -->
-          <geom type="mesh" class="visual" pos="-0.01025 0.03 0.0425915" quat="0.707107 0.707107 -1.11022e-16 8.49837e-17" mesh="pp01062_stewart_arm" material="pp01062_stewart_arm_material"/>
-          <!-- Link shape -->
-          <body name="shape" pos="0.038 2.77556e-17 0.00768" quat="0.0274567 -0.855096 0.517739 -0.00153082">
-            <!-- Joint from pp01062_stewart_arm to shape -->
-            <joint axis="0 0 1" name="passive_1" type="ball" class="chosen_actuator"/>
-            <inertial pos="-0.0450002 -2.17877e-17 -4.85069e-17" mass="0.00355525" fullinertia="1.7732e-08 3.06049e-06 3.0658e-06 3.75919e-22 -8.93073e-21 -1.00288e-21"/>
-            <!-- Part shape -->
-            <geom type="mesh" class="visual" pos="6.93889e-18 -1.38778e-17 0" quat="1 1.53003e-17 -1.11022e-16 -1.249e-16" mesh="shape" material="shape_material"/>
-            <!-- Part pp00xxx_stewart_rod -->
-            <geom type="mesh" class="visual" pos="0.00371031 0.0370326 -0.0564076" quat="0.855513 0.00634803 -0.517727 0.00384161" mesh="pp00xxx_stewart_rod" material="pp00xxx_stewart_rod_material"/>
-            <!-- Part shape_2 -->
-            <geom type="mesh" class="visual" pos="-0.0900003 -2.77556e-17 -8.32667e-17" quat="1.66533e-16 1.57422e-17 -1 1.11022e-16" mesh="shape" material="shape_material"/>
-            <!-- Frame closing_1_1 -->
-            <site group="3" name="closing_1_1" pos="-0.0900003 -2.77556e-17 -8.32667e-17" quat="1.66533e-16 1.57422e-17 -1 1.11022e-16"/>
-          </body>
-        </body>
-        <!-- Link pp01062_stewart_arm_2 -->
-        <body name="pp01062_stewart_arm_2" pos="0.0420103 0.012419 -0.03705" quat="0.612372 0.612372 -0.353553 -0.353553">
-          <!-- Joint from dc15_a01_case_b_dummy to pp01062_stewart_arm_2 -->
-          <joint axis="0 0 1" name="2" type="hinge" range="-1.3962634015954662 1.396263401595461" class="chosen_actuator"/>
-          <inertial pos="0.0181787 2.34834e-18 0.00350559" mass="0.00272417" fullinertia="4.89963e-08 6.75761e-07 6.91798e-07 2.48336e-22 -7.04811e-08 5.24015e-23"/>
-          <!-- Part ball_2 -->
-          <geom type="mesh" class="visual" pos="0.038 -1.38778e-17 0.00768" quat="1.66533e-16 -2.4037e-17 -1 -1.11022e-16" mesh="ball" material="ball_material"/>
-          <!-- Part pp01062_stewart_arm_2 -->
-          <geom type="mesh" class="visual" pos="-0.01025 0.03 0.0425915" quat="0.707107 0.707107 -1.11022e-16 8.24487e-17" mesh="pp01062_stewart_arm" material="pp01062_stewart_arm_material"/>
-          <!-- Link shape_2 -->
-          <body name="shape_2" pos="0.038 -8.32667e-17 0.00768" quat="0.0131699 -0.855436 -0.517228 0.0230506">
-            <!-- Joint from pp01062_stewart_arm_2 to shape_2 -->
-            <joint axis="0 0 1" name="passive_2" type="ball" class="chosen_actuator"/>
-            <inertial pos="-0.0450002 -1.00413e-17 -8.56034e-18" mass="0.00355525" fullinertia="1.7732e-08 3.06049e-06 3.0658e-06 9.05427e-21 -9.31125e-21 -1.13245e-20"/>
-            <!-- Part shape_3 -->
-            <geom type="mesh" class="visual" pos="-0.0900003 -2.77556e-17 2.08167e-17" quat="2.22045e-16 2.94872e-15 1 3.85803e-15" mesh="shape" material="shape_material"/>
-            <!-- Part shape_4 -->
-            <geom type="mesh" class="visual" pos="6.93889e-18 0 6.93889e-18" quat="1 -0 -0 -4.85723e-17" mesh="shape" material="shape_material"/>
-            <!-- Part pp00xxx_stewart_rod_2 -->
-            <geom type="mesh" class="visual" pos="0.00371031 0.0370326 -0.0564076" quat="0.855513 0.00634803 -0.517727 0.00384161" mesh="pp00xxx_stewart_rod" material="pp00xxx_stewart_rod_material"/>
-            <!-- Frame closing_2_2 -->
-            <site group="3" name="closing_2_2" pos="-0.0900003 -2.77556e-17 2.08167e-17" quat="2.78561e-15 -0.707107 0.707107 2.55257e-15"/>
-          </body>
-        </body>
-        <!-- Link pp01062_stewart_arm_3 -->
-        <body name="pp01062_stewart_arm_3" pos="0.01025 -0.0425915 -0.03705" quat="0.707107 -0.707107 -1.11022e-16 1.84877e-17">
-          <!-- Joint from dc15_a01_case_b_dummy to pp01062_stewart_arm_3 -->
-          <joint axis="0 0 1" name="3" type="hinge" range="-1.3962634015954645 1.3962634015954627" class="chosen_actuator"/>
-          <inertial pos="0.0181787 -1.87132e-19 0.00350559" mass="0.00272417" fullinertia="4.89963e-08 6.75761e-07 6.91798e-07 9.85709e-24 -7.04811e-08 -1.47314e-24"/>
-          <!-- Part pp01062_stewart_arm_3 -->
-          <geom type="mesh" class="visual" pos="-0.01025 0.03 0.0425915" quat="0.707107 0.707107 -1.11022e-16 1.0222e-16" mesh="pp01062_stewart_arm" material="pp01062_stewart_arm_material"/>
-          <!-- Part ball_3 -->
-          <geom type="mesh" class="visual" pos="0.038 0 0.00768" quat="1.45894e-16 -0.707107 0.707107 5.21114e-18" mesh="ball" material="ball_material"/>
-          <!-- Link shape_3 -->
-          <body name="shape_3" pos="0.038 4.16334e-17 0.00768" quat="0.85509 -0.027647 0.0318018 -0.516764">
-            <!-- Joint from pp01062_stewart_arm_3 to shape_3 -->
-            <joint axis="0 0 1" name="passive_3" type="ball" class="chosen_actuator"/>
-            <inertial pos="-0.0450002 -4.79199e-18 4.11785e-17" mass="0.00355525" fullinertia="1.7732e-08 3.06049e-06 3.0658e-06 9.80819e-22 -7.34923e-21 1.45141e-21"/>
-            <!-- Part shape_5 -->
-            <geom type="mesh" class="visual" pos="-0.0900003 0 1.31839e-16" quat="4.16334e-16 4.4964e-17 1 -2.22045e-16" mesh="shape" material="shape_material"/>
-            <!-- Part shape_6 -->
-            <geom type="mesh" class="visual" pos="1.38778e-17 0 0" quat="1 -0 -0 2.56739e-16" mesh="shape" material="shape_material"/>
-            <!-- Part pp00xxx_stewart_rod_3 -->
-            <geom type="mesh" class="visual" pos="0.00371031 0.0370326 -0.0564076" quat="0.855513 0.00634803 -0.517727 0.00384161" mesh="pp00xxx_stewart_rod" material="pp00xxx_stewart_rod_material"/>
-            <!-- Frame closing_3_2 -->
-            <site group="3" name="closing_3_2" pos="-0.0900003 0 1.31839e-16" quat="0 4.06771e-16 -3.33067e-16 -1"/>
-          </body>
-        </body>
-        <!-- Link pp01062_stewart_arm_4 -->
-        <body name="pp01062_stewart_arm_4" pos="-0.01025 -0.0425915 -0.03705" quat="4.44089e-16 -1.5721e-15 -0.707107 -0.707107">
-          <!-- Joint from dc15_a01_case_b_dummy to pp01062_stewart_arm_4 -->
-          <joint axis="0 0 1" name="4" type="hinge" range="-1.3962634015954831 1.396263401595444" class="chosen_actuator"/>
-          <inertial pos="0.0181787 -5.24899e-19 0.00350559" mass="0.00272417" fullinertia="4.89963e-08 6.75761e-07 6.91798e-07 4.36416e-22 -7.04811e-08 3.66848e-23"/>
-          <!-- Part ball_4 -->
-          <geom type="mesh" class="visual" pos="0.038 -2.77556e-17 0.00768" quat="4.51946e-15 -0.707107 -0.707107 -1.08859e-15" mesh="ball" material="ball_material"/>
-          <!-- Part pp01062_stewart_arm_4 -->
-          <geom type="mesh" class="visual" pos="-0.01025 0.03 0.0425915" quat="0.707107 0.707107 -1.11022e-16 1.0222e-16" mesh="pp01062_stewart_arm" material="pp01062_stewart_arm_material"/>
-          <!-- Link shape_4 -->
-          <body name="shape_4" pos="0.038 -2.77556e-17 0.00768" quat="0.854409 0.0439152 0.0416306 0.516065">
-            <!-- Joint from pp01062_stewart_arm_4 to shape_4 -->
-            <joint axis="0 0 1" name="passive_4" type="ball" class="chosen_actuator"/>
-            <inertial pos="-0.0450002 1.04447e-16 -1.1361e-17" mass="0.00355525" fullinertia="1.7732e-08 3.06049e-06 3.0658e-06 6.31755e-21 -5.94417e-21 1.35905e-20"/>
-            <!-- Part shape_7 -->
-            <geom type="mesh" class="visual" pos="-0.0900003 2.08167e-16 3.46945e-18" quat="3.33067e-16 0 -1 1.66533e-16" mesh="shape" material="shape_material"/>
-            <!-- Part pp00xxx_stewart_rod_4 -->
-            <geom type="mesh" class="visual" pos="0.00371031 0.0370326 -0.0564076" quat="0.855513 0.00634803 -0.517727 0.00384161" mesh="pp00xxx_stewart_rod" material="pp00xxx_stewart_rod_material"/>
-            <!-- Part shape_8 -->
-            <geom type="mesh" class="visual" pos="6.93889e-18 0 3.46945e-18" quat="1 0 3.33067e-16 1.31839e-16" mesh="shape" material="shape_material"/>
-            <!-- Frame closing_4_1 -->
-            <site group="3" name="closing_4_1" pos="-0.0900003 2.08167e-16 3.46945e-18" quat="3.33067e-16 0 -1 1.66533e-16"/>
-          </body>
-        </body>
-        <!-- Link pp01062_stewart_arm_5 -->
-        <body name="pp01062_stewart_arm_5" pos="-0.0420103 0.012419 -0.03705" quat="0.353553 -0.353553 0.612372 -0.612372">
-          <!-- Joint from dc15_a01_case_b_dummy to pp01062_stewart_arm_5 -->
-          <joint axis="0 0 1" name="5" type="hinge" range="-1.3962634015954662 1.396263401595461" class="chosen_actuator"/>
-          <inertial pos="0.0181787 3.11301e-35 0.00350559" mass="0.00272417" fullinertia="4.89963e-08 6.75761e-07 6.91798e-07 -4.49492e-25 -7.04811e-08 -1.26741e-25"/>
-          <!-- Part pp01062_stewart_arm_5 -->
-          <geom type="mesh" class="visual" pos="-0.01025 0.03 0.0425915" quat="0.707107 0.707107 -1.11022e-16 9.55014e-17" mesh="pp01062_stewart_arm" material="pp01062_stewart_arm_material"/>
-          <!-- Part ball_5 -->
-          <geom type="mesh" class="visual" pos="0.038 0 0.00768" quat="1.38778e-17 5.55459e-17 1 3.33067e-16" mesh="ball" material="ball_material"/>
-          <!-- Link pp00xxx_stewart_rod -->
-          <body name="pp00xxx_stewart_rod" pos="0.038 1.38778e-17 0.00768" quat="0.0908372 -0.850701 0.516198 -0.0399478">
-            <!-- Joint from pp01062_stewart_arm_5 to pp00xxx_stewart_rod -->
-            <joint axis="0 0 1" name="passive_5" type="ball" class="chosen_actuator"/>
-            <inertial pos="-0.0450002 7.44667e-17 -1.14016e-17" mass="0.00355525" fullinertia="1.7732e-08 3.06049e-06 3.0658e-06 4.55471e-21 -4.43821e-21 1.23083e-21"/>
-            <!-- Part shape_9 -->
-            <geom type="mesh" class="visual" pos="0 0 0" quat="1 0 -2.22045e-16 0" mesh="shape" material="shape_material"/>
-            <!-- Part pp00xxx_stewart_rod_5 -->
-            <geom type="mesh" class="visual" pos="0.00371031 0.0370326 -0.0564076" quat="0.855513 0.00634803 -0.517727 0.00384161" mesh="pp00xxx_stewart_rod" material="pp00xxx_stewart_rod_material"/>
-            <!-- Part shape_10 -->
-            <geom type="mesh" class="visual" pos="-0.0900003 1.38778e-16 6.93889e-18" quat="9.99201e-16 -4.04899e-16 1 1.9984e-15" mesh="shape" material="shape_material"/>
-            <!-- Frame closing_5_1 -->
-            <site group="3" name="closing_5_1" pos="-0.0900003 1.38778e-16 6.93889e-18" quat="9.99201e-16 -4.04899e-16 1 1.9984e-15"/>
-          </body>
-        </body>
-        <!-- Link pp01062_stewart_arm_6 -->
-        <body name="pp01062_stewart_arm_6" pos="-0.0317603 0.0301725 -0.03705" quat="0.612372 0.612372 0.353553 0.353553">
-          <!-- Joint from dc15_a01_case_b_dummy to pp01062_stewart_arm_6 -->
-          <joint axis="0 0 1" name="6" type="hinge" range="-1.3962634015954636 1.3962634015954636" class="chosen_actuator"/>
-          <inertial pos="0.0181787 -1.69382e-17 0.00350559" mass="0.00272417" fullinertia="4.89963e-08 6.75761e-07 6.91798e-07 1.6528e-22 -7.04811e-08 3.48685e-23"/>
-          <!-- Part pp01062_stewart_arm_6 -->
-          <geom type="mesh" class="visual" pos="-0.01025 0.03 0.0425915" quat="0.707107 0.707107 -1.11022e-16 1.00194e-16" mesh="pp01062_stewart_arm" material="pp01062_stewart_arm_material"/>
-          <!-- Part ball_6 -->
-          <geom type="mesh" class="visual" pos="0.038 -2.77556e-17 0.00768" quat="0 1 0 0" mesh="ball" material="ball_material"/>
-          <!-- Link shape_5 -->
-          <body name="shape_5" pos="0.038 -2.77556e-17 0.00768" quat="0.0131438 -0.855436 -0.517229 0.0230347">
-            <!-- Joint from pp01062_stewart_arm_6 to shape_5 -->
-            <joint axis="0 0 1" name="passive_6" type="ball" class="chosen_actuator"/>
-            <inertial pos="-0.0450002 7.95049e-16 -1.5589e-17" mass="0.00355525" fullinertia="1.7732e-08 3.06049e-06 3.0658e-06 4.84585e-20 -8.92792e-21 -1.86218e-20"/>
-            <!-- Part shape_11 -->
-            <geom type="mesh" class="visual" pos="-0.0900003 1.83187e-15 6.93889e-18" quat="1.77636e-15 6.61483e-15 1 6.27276e-15" mesh="shape" material="shape_material"/>
-            <!-- Part shape_12 -->
-            <geom type="mesh" class="visual" pos="6.93889e-18 0 0" quat="1 1.00165e-17 -8.32667e-17 0" mesh="shape" material="shape_material"/>
-            <!-- Part pp00xxx_stewart_rod_6 -->
-            <geom type="mesh" class="visual" pos="0.00371031 0.0370326 -0.0564076" quat="0.855513 0.00634803 -0.517727 0.00384161" mesh="pp00xxx_stewart_rod" material="pp00xxx_stewart_rod_material"/>
-            <!-- Link pp01063_stewart_plateform -->
-            <body name="pp01063_stewart_plateform" pos="-0.0900003 1.88738e-15 -2.08167e-17" quat="0.0131438 0.855436 0.517229 -0.0230347">
-              <!-- Joint from shape_5 to pp01063_stewart_plateform -->
-              <joint axis="0 0 1" name="passive_7" type="ball" class="chosen_actuator"/>
-              <inertial pos="-0.00488914 -0.0469134 0.0291732" mass="0.168092" fullinertia="0.000413571 0.000428808 0.000324749 -2.28564e-06 -5.24367e-05 -9.58295e-07"/>
-              <!-- Part small_lens -->
-              <geom type="mesh" class="visual" pos="0.0262444 -0.0524 0.0136834" quat="0.612372 0.612372 -0.353553 0.353553" mesh="small_lens" material="small_lens_material"/>
-              <!-- Part m12_lens -->
-              <geom type="mesh" class="visual" pos="0.010421 -0.0524 -0.0147088" quat="0.183013 0.683013 -0.683013 -0.183013" mesh="m12_lens" material="m12_lens_material"/>
-              <!-- Part pp01080_back_small_eye -->
-              <geom type="mesh" class="visual" pos="0.0262444 -0.0524 0.0136834" quat="0.612372 0.612372 -0.353553 0.353553" mesh="pp01080_back_small_eye" material="pp01080_back_small_eye_material"/>
-              <!-- Part m12_lens_2 -->
-              <geom type="mesh" class="visual" pos="0.042581 -0.0524 0.0417792" quat="0.183013 0.683013 -0.683013 -0.183013" mesh="m12_lens" material="m12_lens_material"/>
-              <!-- Part ball_7 -->
-              <geom type="mesh" class="visual" pos="-0.013 5.55112e-17 -5.55112e-17" quat="1 1.3274e-15 5.77316e-15 -2.63678e-15" mesh="ball" material="ball_material"/>
-              <!-- Part uc_a37_rev_a_step -->
-              <geom type="mesh" class="visual" pos="0.0239513 -0.04265 0.0271316" quat="9.36644e-18 0.866025 0 0.5" mesh="uc_a37_rev_a_step" material="uc_a37_rev_a_step_material"/>
-              <!-- Part ball_8 -->
-              <geom type="mesh" class="visual" pos="-8.67362e-19 2.77556e-17 0" quat="1 -0 -2.04613e-16 -5.18024e-19" mesh="ball" material="ball_material"/>
-              <!-- Part pp01070_head_head_back -->
-              <geom type="mesh" class="visual" pos="-0.0134282 -0.0524 0.0365883" quat="0.612372 0.612372 -0.353553 0.353553" mesh="pp01070_head_head_back" material="pp01070_head_head_back_material"/>
-              <!-- Part ball_9 -->
-              <geom type="mesh" class="visual" pos="0.0249723 5.55112e-17 0.0432533" quat="1.36964e-17 0.866025 -1.11022e-16 -0.5" mesh="ball" material="ball_material"/>
-              <!-- Part pp01079_back_big_eye -->
-              <geom type="mesh" class="visual" pos="0.0265844 -0.0524 0.013487" quat="0.612372 0.612372 -0.353553 0.353553" mesh="pp01079_back_big_eye" material="pp01079_back_big_eye_material"/>
-              <!-- Part ball_10 -->
-              <geom type="mesh" class="visual" pos="-0.0379723 5.55112e-17 0.0432533" quat="0.353553 -0.612372 0.612372 -0.353553" mesh="ball" material="ball_material"/>
-              <!-- Part pp01069_head_shell_front -->
-              <geom type="mesh" class="visual" pos="-0.0142942 -0.0524 0.0370883" quat="0.612372 0.612372 -0.353553 0.353553" mesh="pp01069_head_shell_front" material="pp01069_head_shell_front_material"/>
-              <!-- Part pp01078_glasses -->
-              <geom type="mesh" class="visual" pos="0.025976 -0.0524 0.0138383" quat="0.612372 0.612372 -0.353553 0.353553" mesh="pp01078_glasses" material="pp01078_glasses_material"/>
-              <!-- Part pp01063_stewart_plateform -->
-              <geom type="mesh" class="visual" pos="-0.0065 0.109722 0.0325883" quat="0.353553 0.353553 0.612372 -0.612372" mesh="pp01063_stewart_plateform" material="pp01063_stewart_plateform_material"/>
-              <!-- Part ball_11 -->
-              <geom type="mesh" class="visual" pos="-0.0314723 2.77556e-17 0.0545117" quat="0.353553 -0.612372 0.612372 -0.353553" mesh="ball" material="ball_material"/>
-              <!-- Part big_lens -->
-              <geom type="mesh" class="visual" pos="0.0281677 -0.0524 0.0125729" quat="0.612372 0.612372 -0.353553 0.353553" mesh="big_lens" material="big_lens_material"/>
-              <!-- Part ball_12 -->
-              <geom type="mesh" class="visual" pos="0.0184723 5.55112e-17 0.0545117" quat="3.07775e-17 -0.866025 -0 0.5" mesh="ball" material="ball_material"/>
-              <!-- Frame closing_1_2 -->
-              <site group="3" name="closing_1_2" pos="0.0249723 5.55112e-17 0.0432533" quat="0.353553 0.612372 -0.612372 -0.353553"/>
-              <!-- Frame closing_2_1 -->
-              <site group="3" name="closing_2_1" pos="0.0184723 5.55112e-17 0.0545117" quat="3.07775e-17 -0.866025 -0 0.5"/>
-              <!-- Frame closing_3_1 -->
-              <site group="3" name="closing_3_1" pos="-0.0314723 2.77556e-17 0.0545117" quat="0.353553 -0.612372 0.612372 -0.353553"/>
-              <!-- Frame closing_4_2 -->
-              <site group="3" name="closing_4_2" pos="-0.0379723 5.55112e-17 0.0432533" quat="0.612372 0.353553 -0.353553 -0.612372"/>
-              <!-- Frame closing_5_2 -->
-              <site group="3" name="closing_5_2" pos="-0.013 5.55112e-17 -5.55112e-17" quat="0.707107 -3.16544e-15 5.10703e-15 -0.707107"/>
-              <!-- Frame head -->
-              <site group="3" name="head" pos="-0.0134282 -0.0012 0.0365883" quat="0.683013 0.683013 0.183013 -0.183013"/>
-              <!-- Frame camera -->
-              <body pos="0.0244171 -0.0524 0.0147383" quat="-0.000 -0.500 0.000 0.866">
-                <site group="3" name="camera"/>
-                <camera name="eye_camera"
-                 mode="fixed"
-                 resolution="1280 720"
-                />
-
-              </body>
-              <!-- Frame camera_optical -->
-              <site group="3" name="camera_optical" pos="0.0244171 -0.0524 0.0147383" quat="0.5 0 0.866025 -2.22045e-16"/>
-              <!-- Link test_antenna_body -->
-              <body name="test_antenna_body" pos="-0.0161053 -0.0744 0.100068" quat="0.964418 0.0356203 -0.25955 0.0355422">
-                <!-- Joint from pp01063_stewart_plateform to test_antenna_body -->
-                <joint axis="0 0 1" name="left_antenna" type="hinge" class="chosen_actuator"/>
-                <inertial pos="0.00147983 -0.0294086 0.00298611" mass="0.00456766" fullinertia="1.38179e-05 2.28739e-07 1.38227e-05 6.85535e-07 5.01796e-08 -9.97217e-07"/>
-                <!-- Part test_antenna -->
-                <geom type="mesh" class="visual" pos="0.0285777 0.0175124 -0.0571006" quat="0.716727 0.647969 0.191203 -0.172861" mesh="test_antenna" material="test_antenna_material"/>
-                <!-- Part test_antenna_body -->
-                <geom type="mesh" class="visual" pos="0.0285297 0.0184653 -0.0570171" quat="0.716727 0.647969 0.191203 -0.172861" mesh="test_antenna_body" material="test_antenna_body_material"/>
-              </body>
-              <!-- Link test_antenna -->
-              <body name="test_antenna" pos="-0.0697422 -0.0744 0.00716666" quat="0.706492 0.0909539 -0.701233 -0.0294913">
-                <!-- Joint from pp01063_stewart_plateform to test_antenna -->
-                <joint axis="0 0 1" name="right_antenna" type="hinge" class="chosen_actuator"/>
-                <inertial pos="0.00147983 -0.0294086 0.00298611" mass="0.00456766" fullinertia="1.38179e-05 2.28739e-07 1.38227e-05 6.85535e-07 5.01796e-08 -9.97217e-07"/>
-                <!-- Part test_antenna_2 -->
-                <geom type="mesh" class="visual" pos="0.0285777 0.0175124 -0.0571006" quat="0.716727 0.647969 0.191203 -0.172861" mesh="test_antenna" material="test_antenna_material"/>
-                <!-- Part test_antenna_body_2 -->
-                <geom type="mesh" class="visual" pos="0.0285297 0.0184653 -0.0570171" quat="0.716727 0.647969 0.191203 -0.172861" mesh="test_antenna_body" material="test_antenna_body_material"/>
-              </body>
-            </body>
-          </body>
-        </body>
-      </body>
-    </body>
-  </worldbody>
-  <asset>
-    <mesh file="dc15_a01_case_b_dummy.stl"/>
-    <mesh file="pp01078_glasses.stl"/>
-    <mesh file="big_lens.stl"/>
-    <mesh file="test_antenna_body.stl"/>
-    <mesh file="pp01065_stewart_side_plate.stl"/>
-    <mesh file="small_lens.stl"/>
-    <mesh file="pp01080_back_small_eye.stl"/>
-    <mesh file="pp01064_stewart_main_plate.stl"/>
-    <mesh file="shape.stl"/>
-    <mesh file="pp00xxx_stewart_rod.stl"/>
-    <mesh file="pp01079_back_big_eye.stl"/>
-    <mesh file="ball.stl"/>
-    <mesh file="wj_wk00_0123middlecase_56__configuration_default.stl"/>
-    <mesh file="test_antenna.stl"/>
-    <mesh file="pp01070_head_head_back.stl"/>
-    <mesh file="dc15_a01_case_f_dummy.stl"/>
-    <mesh file="drive_palonier__configuration_default.stl"/>
-    <mesh file="dc15_a01_horn_dummy.stl"/>
-    <mesh file="wj_wk00_0122topcabinetcase_95__configuration_default.stl"/>
-    <mesh file="pp01071_turning_bowl.stl"/>
-    <mesh file="pp01068_top_body.stl"/>
-    <mesh file="wj_wk00_0124bottomcase_45__configuration_default.stl"/>
-    <mesh file="uc_a37_rev_a_step.stl"/>
-    <mesh file="dc15_a01_case_m_dummy.stl"/>
-    <mesh file="pp01062_stewart_arm.stl"/>
-    <mesh file="pp01072_turning_end.stl"/>
-    <mesh file="m12_lens.stl"/>
-    <mesh file="pp01069_head_shell_front.stl"/>
-    <mesh file="pp01063_stewart_plateform.stl"/>
-    <mesh file="pp01066_stewart_mid_plate.stl"/>
-    <mesh file="pp01067_bottom_body.stl"/>
-    <material name="pp01071_turning_bowl_material" rgba="0.34902 0.376471 0.4 1"/>
-    <material name="pp01072_turning_end_material" rgba="0.262745 0.282353 0.301961 1"/>
-    <material name="pp01065_stewart_side_plate_material" rgba="0.196078 0.196078 0.196078 1"/>
-    <material name="pp01067_bottom_body_material" rgba="1 1 1 1"/>
-    <material name="dc15_a01_case_m_dummy_material" rgba="0.262745 0.282353 0.301961 1"/>
-    <material name="dc15_a01_case_f_dummy_material" rgba="0.262745 0.282353 0.301961 1"/>
-    <material name="dc15_a01_horn_dummy_material" rgba="0.262745 0.282353 0.301961 1"/>
-    <material name="wj_wk00_0122topcabinetcase_95__configuration_default_material" rgba="0.247059 0.243137 0.243137 1"/>
-    <material name="dc15_a01_case_b_dummy_material" rgba="0.262745 0.282353 0.301961 1"/>
-    <material name="pp01068_top_body_material" rgba="1 1 1 1"/>
-    <material name="drive_palonier__configuration_default_material" rgba="0.623529 0.666667 0.701961 1"/>
-    <material name="wj_wk00_0123middlecase_56__configuration_default_material" rgba="0.247059 0.243137 0.243137 1"/>
-    <material name="pp01066_stewart_mid_plate_material" rgba="0.196078 0.196078 0.196078 1"/>
-    <material name="wj_wk00_0124bottomcase_45__configuration_default_material" rgba="0.247059 0.243137 0.243137 1"/>
-    <material name="pp01064_stewart_main_plate_material" rgba="0.196078 0.196078 0.196078 1"/>
-    <material name="ball_material" rgba="0.901961 0.901961 0.901961 1"/>
-    <material name="pp01062_stewart_arm_material" rgba="0.196078 0.196078 0.196078 1"/>
-    <material name="shape_material" rgba="0.901961 0.901961 0.901961 1"/>
-    <material name="pp00xxx_stewart_rod_material" rgba="0.8 0.8 0.8 1"/>
-    <material name="small_lens_material" rgba="0.439216 0.47451 0.501961 0.298039"/>
-    <material name="m12_lens_material" rgba="0.262745 0.282353 0.301961 1"/>
-    <material name="pp01080_back_small_eye_material" rgba="0.168627 0.168627 0.168627 1"/>
-    <material name="uc_a37_rev_a_step_material" rgba="0.25098 0.25098 0.25098 1"/>
-    <material name="pp01070_head_head_back_material" rgba="1 1 1 1"/>
-    <material name="pp01079_back_big_eye_material" rgba="0.168627 0.168627 0.168627 1"/>
-    <material name="pp01069_head_shell_front_material" rgba="1 1 1 1"/>
-    <material name="pp01078_glasses_material" rgba="0.168627 0.168627 0.168627 1"/>
-    <material name="pp01063_stewart_plateform_material" rgba="1 1 1 1"/>
-    <material name="big_lens_material" rgba="0.439216 0.47451 0.501961 0.298039"/>
-    <material name="test_antenna_material" rgba="0.262745 0.282353 0.301961 1"/>
-    <material name="test_antenna_body_material" rgba="1 1 1 1"/>
-  </asset>
-  <actuator>
-    <position class="chosen_actuator" name="all_yaw" joint="all_yaw" />
-    <position class="chosen_actuator" name="1" joint="1" inheritrange="1"/>
-    <position class="chosen_actuator" name="2" joint="2" inheritrange="1"/>
-    <position class="chosen_actuator" name="3" joint="3" inheritrange="1"/>
-    <position class="chosen_actuator" name="4" joint="4" inheritrange="1"/>
-    <position class="chosen_actuator" name="5" joint="5" inheritrange="1"/>
-    <position class="chosen_actuator" name="6" joint="6" inheritrange="1"/>
-    <position class="chosen_actuator" name="left_antenna" joint="left_antenna"/>
-    <position class="chosen_actuator" name="right_antenna" joint="right_antenna"/>
-  </actuator>
-  <equality>
-    <connect site1="closing_1_1" site2="closing_1_2"/>
-    <connect site1="closing_2_1" site2="closing_2_2"/>
-    <connect site1="closing_3_1" site2="closing_3_2"/>
-    <connect site1="closing_4_1" site2="closing_4_2"/>
-    <connect site1="closing_5_1" site2="closing_5_2"/>
-  </equality>
-</mujoco>
diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/reachy_mini.xml b/src/reachy_mini/descriptions/reachy_mini/mjcf/reachy_mini.xml
index 83ee966a..dc66a337 100644
--- a/src/reachy_mini/descriptions/reachy_mini/mjcf/reachy_mini.xml
+++ b/src/reachy_mini/descriptions/reachy_mini/mjcf/reachy_mini.xml
@@ -19,8 +19,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 class="xc330m288t">
@@ -75,10 +75,15 @@
         <!-- 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="box" class="collision" name="body_front_collider" size="0.035 0.01 0.08" pos="0 0.06 0.07" euler="0 0 0"/>
-        <geom type="box" class="collision" name="body_back_collider" size="0.025 0.01 0.03" pos="0 -0.06 0.1" euler="0 0 0"/>
-        <geom type="box" class="collision" name="body_eft_collider" size="0.025 0.01 0.06" pos="-0.06 0 0.05" euler="0 0 1.57"/>
-        <geom type="box" class="collision" name="body_right_collider" size="0.025 0.01 0.06" pos="0.06 0 0.05" euler="0 0 -1.57"/>
+        <!-- <geom type="box" class="collision" name="body_front_collider2" size="0.03 0.005 0.03" pos="0.02 0.065 0.115" euler="0.3 0.1 -0.2"/>
+        <geom type="box" class="collision" name="body_front_collider1" size="0.03 0.005 0.03" pos="-0.02 0.065 0.115" euler="0.3 -0.1 0.2"/> -->
+        <geom type="box" class="collision" name="body_front_collider" size="0.035 0.005 0.08" pos="0 0.062 0.07" euler="0 0 0"/>
+
+        <geom type="box" class="collision" name="body_front_collider2" size="0.032 0.005 0.01" pos="0.05 0.055 0.12" euler="1.15 0 2.1"/>
+        <geom type="box" class="collision" name="body_front_collider1" size="0.032 0.005 0.01" pos="-0.05 0.055 0.12" euler="1.15 0 -2.1"/>
+        <geom type="box" class="collision" name="body_back_collider" size="0.04 0.005 0.03" pos="0 -0.057 0.1" euler="0 0 0"/>
+        <geom type="box" class="collision" name="body_eft_collider" size="0.04 0.005 0.06" pos="-0.068 -0.02 0.045" euler="-0.3 0 1.77"/>
+        <geom type="box" class="collision" name="body_right_collider" size="0.04 0.005 0.06" pos="0.068 -0.02 0.045" euler="-0.3 0 -1.77"/>
         <!-- <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" material="body_top_3dprint_material"/> -->
         <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 -->
@@ -476,7 +481,7 @@
                   resolution="1280 720"
                   fovy="80"
                 />
-                <geom type="capsule" class="collision" name="head_collider" size="0.048 0.022" pos="0 0 0.039" euler="0 1.57 0"/>
+                <geom type="capsule" class="collision" name="head_collider" size="0.051 0.03" pos="0 0 0.042" euler="0 1.57 0"/>
         
               </body> 
               <!-- Frame head -->
diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/reachy_mini.xml.old b/src/reachy_mini/descriptions/reachy_mini/mjcf/reachy_mini.xml.old
deleted file mode 100644
index 529ebb57..00000000
--- a/src/reachy_mini/descriptions/reachy_mini/mjcf/reachy_mini.xml.old
+++ /dev/null
@@ -1,440 +0,0 @@
-<?xml version="1.0" ?>
-<!-- Generated using onshape-to-robot -->
-<!-- Onshape https://cad.onshape.com/documents/e305b26a84339af20c593973/w/ec7765eccdaa9a5960507125/e/251023ea04190ab16854eb80 -->
-<mujoco model="reachy_mini">
-  <compiler angle="radian" meshdir="assets" autolimits="true"/>
-  <default>
-    <default class="reachy_mini">
-      <joint frictionloss="0.1" armature="0.005"/>
-      <position kp="50" dampratio="1"/>
-      <default class="visual">
-        <geom type="mesh" contype="0" conaffinity="0" group="2"/>
-      </default>
-      <default class="collision">
-        <geom 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.0" forcerange="-10.0 10.0"/>
-      <default class="chosen_actuator"/>
-    </default>
-    <default class="xc330m288t">
-      <!-- probably wrong, would need to re-identify -->
-      <geom contype="0" conaffinity="0"/>
-      <joint damping="0.095" frictionloss="0.058" armature="0.0018"/>
-      <position kp="2.54" kv="0.0" forcerange="-0.8 0.8"/>
-    </default>
-    <default class="sts3215_345">
-      <!-- Standard sts3215 with 1/345 gear ratio -->
-      <!-- Confident that this is realistic (mini duck walks with these values) -->
-      <geom contype="0" conaffinity="0"/>
-      <joint damping="0.56" frictionloss="0.068" armature="0.027"/>
-      <position kp="17.11" kv="0.0" forcerange="-3.23 3.23"/>
-    </default>
-    <default class="sts3215_147">
-      <!-- sts3215 with 1/147 gear ratio -->
-      <!-- (Estimation based on the gear ratio difference) -->
-      <geom contype="0" conaffinity="0"/>
-      <joint damping="0.1" frictionloss="0.028" armature="0.011"/>
-      <position kp="17.11" kv="0.0" forcerange="-1.35 1.35"/>
-    </default>
-  </default>
-  <!-- Additional additional.xml -->
-  <contact>
-    <!-- <exclude name="exclude0" body1="torso" body2="sts32155"/> -->
-  </contact>
-  <default>
-    <equality solref="0.002 1" solimp="0.99 0.999 0.0005 0.5 2"/>
-  </default>
-  <worldbody>
-    <!-- Link pp01071_turning_bowl -->
-    <body name="pp01071_turning_bowl" pos="0 0 0" quat="1 0 0 0" childclass="reachy_mini">
-      <inertial pos="0.000667521 1.7878e-09 0.0118411" mass="0.0947234" fullinertia="0.000111359 0.000106563 0.000196298 1.02485e-11 8.96229e-07 4.95917e-13"/>
-      <!-- Part pp01071_turning_bowl -->
-      <geom type="mesh" class="visual" pos="-1.38778e-17 0 0.2345" quat="0.707107 0 -6.16298e-32 0.707107" mesh="pp01071_turning_bowl" material="pp01071_turning_bowl_material"/>
-      <!-- Part pp01072_turning_end -->
-      <geom type="mesh" class="visual" pos="3.46945e-17 -4.51028e-17 0.2345" quat="0.707107 7.85046e-17 -2.22045e-16 0.707107" mesh="pp01072_turning_end" material="pp01072_turning_end_material"/>
-      <!-- Link dc15_a01_case_b_dummy -->
-      <body name="dc15_a01_case_b_dummy" pos="4.85723e-17 -5.60663e-19 0.0339" quat="7.85046e-17 -0.707107 -0.707107 -7.85046e-17">
-        <!-- Joint from pp01071_turning_bowl to dc15_a01_case_b_dummy -->
-        <joint axis="0 0 1" name="all_yaw" type="hinge" class="chosen_actuator"/>
-        <inertial pos="-1.60377e-06 0.00140187 -0.0329593" mass="0.443795" fullinertia="0.00159333 0.0015483 0.00187065 2.80397e-07 1.98452e-07 4.26838e-05"/>
-        <!-- Part pp01065_stewart_side_plate -->
-        <geom type="mesh" class="visual" pos="-3.43031e-17 -4.85723e-17 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="pp01065_stewart_side_plate" material="pp01065_stewart_side_plate_material"/>
-        <!-- Part pp01067_bottom_body -->
-        <geom type="mesh" class="visual" pos="1.38767e-16 1.83881e-16 -0.20055" quat="9.16593e-17 1 -3.29597e-17 5.68353e-17" mesh="pp01067_bottom_body" material="pp01067_bottom_body_material"/>
-        <!-- Part dc15_a01_case_m_dummy -->
-        <geom type="mesh" class="visual" pos="-1.4706e-16 -8.67362e-17 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_case_m_dummy" material="dc15_a01_case_m_dummy_material"/>
-        <!-- Part dc15_a01_case_f_dummy -->
-        <geom type="mesh" class="visual" pos="0.0205 6.93889e-18 -0.00705" quat="3.33067e-16 3.64292e-17 1 -4.44089e-16" mesh="dc15_a01_case_f_dummy" material="dc15_a01_case_f_dummy_material"/>
-        <!-- Part dc15_a01_horn_dummy -->
-        <geom type="mesh" class="visual" pos="0.0205 1.73472e-17 -0.00705" quat="3.33067e-16 3.64292e-17 1 -4.44089e-16" mesh="dc15_a01_horn_dummy" material="dc15_a01_horn_dummy_material"/>
-        <!-- Part dc15_a01_horn_dummy_2 -->
-        <geom type="mesh" class="visual" pos="-0.01025 0.0177535 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_horn_dummy" material="dc15_a01_horn_dummy_material"/>
-        <!-- Part wj_wk00_0122topcabinetcase_95__configuration_default -->
-        <geom type="mesh" class="visual" pos="6.12843e-18 -0.0255 -0.00945" quat="0.5 0.5 -0.5 -0.5" mesh="wj_wk00_0122topcabinetcase_95__configuration_default" material="wj_wk00_0122topcabinetcase_95__configuration_default_material"/>
-        <!-- Part dc15_a01_case_m_dummy_2 -->
-        <geom type="mesh" class="visual" pos="-2.56295e-17 3.1225e-17 -0.00705" quat="1.11022e-16 2.60209e-17 1 -2.22045e-16" mesh="dc15_a01_case_m_dummy" material="dc15_a01_case_m_dummy_material"/>
-        <!-- Part dc15_a01_case_b_dummy -->
-        <geom type="mesh" class="visual" pos="-0.01025 -0.0177535 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_case_b_dummy" material="dc15_a01_case_b_dummy_material"/>
-        <!-- Part dc15_a01_case_b_dummy_2 -->
-        <geom type="mesh" class="visual" pos="0.0205 3.46945e-18 -0.00705" quat="3.33067e-16 3.64292e-17 1 -4.44089e-16" mesh="dc15_a01_case_b_dummy" material="dc15_a01_case_b_dummy_material"/>
-        <!-- Part pp01068_top_body -->
-        <geom type="mesh" class="visual" pos="-8.55341e-16 -3.12493e-14 -0.20005" quat="9.19752e-14 1 -3.64292e-17 -2.66929e-15" mesh="pp01068_top_body" material="pp01068_top_body_material"/>
-        <!-- Part dc15_a01_case_f_dummy_2 -->
-        <geom type="mesh" class="visual" pos="-3.43031e-17 -3.46945e-18 -0.00705" quat="3.33067e-16 3.64292e-17 1 -4.44089e-16" mesh="dc15_a01_case_f_dummy" material="dc15_a01_case_f_dummy_material"/>
-        <!-- Part drive_palonier__configuration_default -->
-        <geom type="mesh" class="visual" pos="4.39371e-18 -0.0255 -0.00945" quat="0.5 0.5 -0.5 -0.5" mesh="drive_palonier__configuration_default" material="drive_palonier__configuration_default_material"/>
-        <!-- Part dc15_a01_case_f_dummy_3 -->
-        <geom type="mesh" class="visual" pos="-0.01025 0.0177535 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_case_f_dummy" material="dc15_a01_case_f_dummy_material"/>
-        <!-- Part dc15_a01_case_m_dummy_3 -->
-        <geom type="mesh" class="visual" pos="-3.43031e-17 -6.245e-17 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_case_m_dummy" material="dc15_a01_case_m_dummy_material"/>
-        <!-- Part dc15_a01_case_b_dummy_3 -->
-        <geom type="mesh" class="visual" pos="-9.1549e-17 -6.245e-17 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_case_b_dummy" material="dc15_a01_case_b_dummy_material"/>
-        <!-- Part wj_wk00_0123middlecase_56__configuration_default -->
-        <geom type="mesh" class="visual" pos="-1.75347e-05 -0.0254895 -0.00945" quat="0.5 0.5 -0.5 -0.5" mesh="wj_wk00_0123middlecase_56__configuration_default" material="wj_wk00_0123middlecase_56__configuration_default_material"/>
-        <!-- Part dc15_a01_case_b_dummy_4 -->
-        <geom type="mesh" class="visual" pos="-3.60378e-17 -1.04083e-17 -0.00705" quat="3.33067e-16 3.64292e-17 1 -4.44089e-16" mesh="dc15_a01_case_b_dummy" material="dc15_a01_case_b_dummy_material"/>
-        <!-- Part dc15_a01_case_f_dummy_4 -->
-        <geom type="mesh" class="visual" pos="-2.26857e-16 -1.00614e-16 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_case_f_dummy" material="dc15_a01_case_f_dummy_material"/>
-        <!-- Part dc15_a01_case_b_dummy_5 -->
-        <geom type="mesh" class="visual" pos="-0.01025 0.0177535 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_case_b_dummy" material="dc15_a01_case_b_dummy_material"/>
-        <!-- Part dc15_a01_horn_dummy_3 -->
-        <geom type="mesh" class="visual" pos="-1.15835e-16 -6.93889e-17 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_horn_dummy" material="dc15_a01_horn_dummy_material"/>
-        <!-- Part dc15_a01_horn_dummy_4 -->
-        <geom type="mesh" class="visual" pos="-0.01025 -0.0177535 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_horn_dummy" material="dc15_a01_horn_dummy_material"/>
-        <!-- Part dc15_a01_case_m_dummy_4 -->
-        <geom type="mesh" class="visual" pos="-0.01025 0.0177535 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="dc15_a01_case_m_dummy" material="dc15_a01_case_m_dummy_material"/>
-        <!-- Part pp01065_stewart_side_plate_2 -->
-        <geom type="mesh" class="visual" pos="-1.34864e-17 6.59195e-17 -0.00705" quat="0 3.64292e-17 1 0" mesh="pp01065_stewart_side_plate" material="pp01065_stewart_side_plate_material"/>
-        <!-- Part dc15_a01_case_b_dummy_6 -->
-        <geom type="mesh" class="visual" pos="-6.37934e-17 -3.46945e-17 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_case_b_dummy" material="dc15_a01_case_b_dummy_material"/>
-        <!-- Part pp01066_stewart_mid_plate -->
-        <geom type="mesh" class="visual" pos="-8.28754e-17 -1.04083e-17 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="pp01066_stewart_mid_plate" material="pp01066_stewart_mid_plate_material"/>
-        <!-- Part dc15_a01_case_f_dummy_5 -->
-        <geom type="mesh" class="visual" pos="-3.77726e-17 -4.85723e-17 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_case_f_dummy" material="dc15_a01_case_f_dummy_material"/>
-        <!-- Part dc15_a01_horn_dummy_5 -->
-        <geom type="mesh" class="visual" pos="-5.51198e-17 -4.85723e-17 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_horn_dummy" material="dc15_a01_horn_dummy_material"/>
-        <!-- Part dc15_a01_case_m_dummy_5 -->
-        <geom type="mesh" class="visual" pos="0.0205 3.81639e-17 -0.00705" quat="1.11022e-16 2.60209e-17 1 -2.22045e-16" mesh="dc15_a01_case_m_dummy" material="dc15_a01_case_m_dummy_material"/>
-        <!-- Part wj_wk00_0124bottomcase_45__configuration_default -->
-        <geom type="mesh" class="visual" pos="-1.04636e-05 -0.0254966 -0.00945" quat="0.5 0.5 -0.5 -0.5" mesh="wj_wk00_0124bottomcase_45__configuration_default" material="wj_wk00_0124bottomcase_45__configuration_default_material"/>
-        <!-- Part dc15_a01_horn_dummy_6 -->
-        <geom type="mesh" class="visual" pos="-3.25684e-17 1.04083e-17 -0.00705" quat="3.33067e-16 3.64292e-17 1 -4.44089e-16" mesh="dc15_a01_horn_dummy" material="dc15_a01_horn_dummy_material"/>
-        <!-- Part dc15_a01_case_m_dummy_6 -->
-        <geom type="mesh" class="visual" pos="-0.01025 -0.0177535 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_case_m_dummy" material="dc15_a01_case_m_dummy_material"/>
-        <!-- Part pp01064_stewart_main_plate -->
-        <geom type="mesh" class="visual" pos="-2.73642e-17 -1.38778e-17 -0.00705" quat="1.11022e-16 -8.67362e-18 -1 -1.11022e-16" mesh="pp01064_stewart_main_plate" material="pp01064_stewart_main_plate_material"/>
-        <!-- Part dc15_a01_case_f_dummy_6 -->
-        <geom type="mesh" class="visual" pos="-0.01025 -0.0177535 -0.00705" quat="1.07797e-16 0.866025 0.5 3.39116e-18" mesh="dc15_a01_case_f_dummy" material="dc15_a01_case_f_dummy_material"/>
-        <!-- Part pp01065_stewart_side_plate_3 -->
-        <geom type="mesh" class="visual" pos="-6.20587e-17 -1.04083e-17 -0.00705" quat="5.09617e-17 0.866025 -0.5 9.50505e-17" mesh="pp01065_stewart_side_plate" material="pp01065_stewart_side_plate_material"/>
-        <!-- Link pp01062_stewart_arm -->
-        <body name="pp01062_stewart_arm" pos="0.0317603 0.0301725 -0.03705" quat="0.353553 -0.353553 -0.612372 0.612372">
-          <!-- Joint from dc15_a01_case_b_dummy to pp01062_stewart_arm -->
-          <joint axis="0 0 1" name="1" type="hinge" range="-1.3962634015954645 1.3962634015954627" class="chosen_actuator"/>
-          <inertial pos="0.0181787 2.77556e-17 0.00350559" mass="0.00272417" fullinertia="4.89963e-08 6.75761e-07 6.91798e-07 -2.63422e-25 -7.04811e-08 2.66043e-25"/>
-          <!-- Part ball -->
-          <geom type="mesh" class="visual" pos="0.038 2.77556e-17 0.00768" quat="0 1 0 0" mesh="ball" material="ball_material"/>
-          <!-- Part pp01062_stewart_arm -->
-          <geom type="mesh" class="visual" pos="-0.01025 0.03 0.0425915" quat="0.707107 0.707107 -1.11022e-16 8.49837e-17" mesh="pp01062_stewart_arm" material="pp01062_stewart_arm_material"/>
-          <!-- Link shape -->
-          <body name="shape" pos="0.038 2.77556e-17 0.00768" quat="0.0274567 -0.855096 0.517739 -0.00153082">
-            <!-- Joint from pp01062_stewart_arm to shape -->
-            <joint axis="0 0 1" name="passive_1" type="ball" class="chosen_actuator"/>
-            <inertial pos="-0.0450002 -2.17877e-17 -4.85069e-17" mass="0.00355525" fullinertia="1.7732e-08 3.06049e-06 3.0658e-06 3.75919e-22 -8.93073e-21 -1.00288e-21"/>
-            <!-- Part shape -->
-            <geom type="mesh" class="visual" pos="6.93889e-18 -1.38778e-17 0" quat="1 1.53003e-17 -1.11022e-16 -1.249e-16" mesh="shape" material="shape_material"/>
-            <!-- Part pp00xxx_stewart_rod -->
-            <geom type="mesh" class="visual" pos="0.00371031 0.0370326 -0.0564076" quat="0.855513 0.00634803 -0.517727 0.00384161" mesh="pp00xxx_stewart_rod" material="pp00xxx_stewart_rod_material"/>
-            <!-- Part shape_2 -->
-            <geom type="mesh" class="visual" pos="-0.0900003 -2.77556e-17 -8.32667e-17" quat="1.66533e-16 1.57422e-17 -1 1.11022e-16" mesh="shape" material="shape_material"/>
-            <!-- Frame closing_1_1 -->
-            <site group="3" name="closing_1_1" pos="-0.0900003 -2.77556e-17 -8.32667e-17" quat="1.66533e-16 1.57422e-17 -1 1.11022e-16"/>
-          </body>
-        </body>
-        <!-- Link pp01062_stewart_arm_2 -->
-        <body name="pp01062_stewart_arm_2" pos="0.0420103 0.012419 -0.03705" quat="0.612372 0.612372 -0.353553 -0.353553">
-          <!-- Joint from dc15_a01_case_b_dummy to pp01062_stewart_arm_2 -->
-          <joint axis="0 0 1" name="2" type="hinge" range="-1.3962634015954662 1.396263401595461" class="chosen_actuator"/>
-          <inertial pos="0.0181787 2.34834e-18 0.00350559" mass="0.00272417" fullinertia="4.89963e-08 6.75761e-07 6.91798e-07 2.48336e-22 -7.04811e-08 5.24015e-23"/>
-          <!-- Part ball_2 -->
-          <geom type="mesh" class="visual" pos="0.038 -1.38778e-17 0.00768" quat="1.66533e-16 -2.4037e-17 -1 -1.11022e-16" mesh="ball" material="ball_material"/>
-          <!-- Part pp01062_stewart_arm_2 -->
-          <geom type="mesh" class="visual" pos="-0.01025 0.03 0.0425915" quat="0.707107 0.707107 -1.11022e-16 8.24487e-17" mesh="pp01062_stewart_arm" material="pp01062_stewart_arm_material"/>
-          <!-- Link shape_2 -->
-          <body name="shape_2" pos="0.038 -8.32667e-17 0.00768" quat="0.0131699 -0.855436 -0.517228 0.0230506">
-            <!-- Joint from pp01062_stewart_arm_2 to shape_2 -->
-            <joint axis="0 0 1" name="passive_2" type="ball" class="chosen_actuator"/>
-            <inertial pos="-0.0450002 -1.00413e-17 -8.56034e-18" mass="0.00355525" fullinertia="1.7732e-08 3.06049e-06 3.0658e-06 9.05427e-21 -9.31125e-21 -1.13245e-20"/>
-            <!-- Part shape_3 -->
-            <geom type="mesh" class="visual" pos="-0.0900003 -2.77556e-17 2.08167e-17" quat="2.22045e-16 2.94872e-15 1 3.85803e-15" mesh="shape" material="shape_material"/>
-            <!-- Part shape_4 -->
-            <geom type="mesh" class="visual" pos="6.93889e-18 0 6.93889e-18" quat="1 -0 -0 -4.85723e-17" mesh="shape" material="shape_material"/>
-            <!-- Part pp00xxx_stewart_rod_2 -->
-            <geom type="mesh" class="visual" pos="0.00371031 0.0370326 -0.0564076" quat="0.855513 0.00634803 -0.517727 0.00384161" mesh="pp00xxx_stewart_rod" material="pp00xxx_stewart_rod_material"/>
-            <!-- Frame closing_2_2 -->
-            <site group="3" name="closing_2_2" pos="-0.0900003 -2.77556e-17 2.08167e-17" quat="2.78561e-15 -0.707107 0.707107 2.55257e-15"/>
-          </body>
-        </body>
-        <!-- Link pp01062_stewart_arm_3 -->
-        <body name="pp01062_stewart_arm_3" pos="0.01025 -0.0425915 -0.03705" quat="0.707107 -0.707107 -1.11022e-16 1.84877e-17">
-          <!-- Joint from dc15_a01_case_b_dummy to pp01062_stewart_arm_3 -->
-          <joint axis="0 0 1" name="3" type="hinge" range="-1.3962634015954645 1.3962634015954627" class="chosen_actuator"/>
-          <inertial pos="0.0181787 -1.87132e-19 0.00350559" mass="0.00272417" fullinertia="4.89963e-08 6.75761e-07 6.91798e-07 9.85709e-24 -7.04811e-08 -1.47314e-24"/>
-          <!-- Part pp01062_stewart_arm_3 -->
-          <geom type="mesh" class="visual" pos="-0.01025 0.03 0.0425915" quat="0.707107 0.707107 -1.11022e-16 1.0222e-16" mesh="pp01062_stewart_arm" material="pp01062_stewart_arm_material"/>
-          <!-- Part ball_3 -->
-          <geom type="mesh" class="visual" pos="0.038 0 0.00768" quat="1.45894e-16 -0.707107 0.707107 5.21114e-18" mesh="ball" material="ball_material"/>
-          <!-- Link shape_3 -->
-          <body name="shape_3" pos="0.038 4.16334e-17 0.00768" quat="0.85509 -0.027647 0.0318018 -0.516764">
-            <!-- Joint from pp01062_stewart_arm_3 to shape_3 -->
-            <joint axis="0 0 1" name="passive_3" type="ball" class="chosen_actuator"/>
-            <inertial pos="-0.0450002 -4.79199e-18 4.11785e-17" mass="0.00355525" fullinertia="1.7732e-08 3.06049e-06 3.0658e-06 9.80819e-22 -7.34923e-21 1.45141e-21"/>
-            <!-- Part shape_5 -->
-            <geom type="mesh" class="visual" pos="-0.0900003 0 1.31839e-16" quat="4.16334e-16 4.4964e-17 1 -2.22045e-16" mesh="shape" material="shape_material"/>
-            <!-- Part shape_6 -->
-            <geom type="mesh" class="visual" pos="1.38778e-17 0 0" quat="1 -0 -0 2.56739e-16" mesh="shape" material="shape_material"/>
-            <!-- Part pp00xxx_stewart_rod_3 -->
-            <geom type="mesh" class="visual" pos="0.00371031 0.0370326 -0.0564076" quat="0.855513 0.00634803 -0.517727 0.00384161" mesh="pp00xxx_stewart_rod" material="pp00xxx_stewart_rod_material"/>
-            <!-- Frame closing_3_2 -->
-            <site group="3" name="closing_3_2" pos="-0.0900003 0 1.31839e-16" quat="0 4.06771e-16 -3.33067e-16 -1"/>
-          </body>
-        </body>
-        <!-- Link pp01062_stewart_arm_4 -->
-        <body name="pp01062_stewart_arm_4" pos="-0.01025 -0.0425915 -0.03705" quat="4.44089e-16 -1.5721e-15 -0.707107 -0.707107">
-          <!-- Joint from dc15_a01_case_b_dummy to pp01062_stewart_arm_4 -->
-          <joint axis="0 0 1" name="4" type="hinge" range="-1.3962634015954831 1.396263401595444" class="chosen_actuator"/>
-          <inertial pos="0.0181787 -5.24899e-19 0.00350559" mass="0.00272417" fullinertia="4.89963e-08 6.75761e-07 6.91798e-07 4.36416e-22 -7.04811e-08 3.66848e-23"/>
-          <!-- Part ball_4 -->
-          <geom type="mesh" class="visual" pos="0.038 -2.77556e-17 0.00768" quat="4.51946e-15 -0.707107 -0.707107 -1.08859e-15" mesh="ball" material="ball_material"/>
-          <!-- Part pp01062_stewart_arm_4 -->
-          <geom type="mesh" class="visual" pos="-0.01025 0.03 0.0425915" quat="0.707107 0.707107 -1.11022e-16 1.0222e-16" mesh="pp01062_stewart_arm" material="pp01062_stewart_arm_material"/>
-          <!-- Link shape_4 -->
-          <body name="shape_4" pos="0.038 -2.77556e-17 0.00768" quat="0.854409 0.0439152 0.0416306 0.516065">
-            <!-- Joint from pp01062_stewart_arm_4 to shape_4 -->
-            <joint axis="0 0 1" name="passive_4" type="ball" class="chosen_actuator"/>
-            <inertial pos="-0.0450002 1.04447e-16 -1.1361e-17" mass="0.00355525" fullinertia="1.7732e-08 3.06049e-06 3.0658e-06 6.31755e-21 -5.94417e-21 1.35905e-20"/>
-            <!-- Part shape_7 -->
-            <geom type="mesh" class="visual" pos="-0.0900003 2.08167e-16 3.46945e-18" quat="3.33067e-16 0 -1 1.66533e-16" mesh="shape" material="shape_material"/>
-            <!-- Part pp00xxx_stewart_rod_4 -->
-            <geom type="mesh" class="visual" pos="0.00371031 0.0370326 -0.0564076" quat="0.855513 0.00634803 -0.517727 0.00384161" mesh="pp00xxx_stewart_rod" material="pp00xxx_stewart_rod_material"/>
-            <!-- Part shape_8 -->
-            <geom type="mesh" class="visual" pos="6.93889e-18 0 3.46945e-18" quat="1 0 3.33067e-16 1.31839e-16" mesh="shape" material="shape_material"/>
-            <!-- Frame closing_4_1 -->
-            <site group="3" name="closing_4_1" pos="-0.0900003 2.08167e-16 3.46945e-18" quat="3.33067e-16 0 -1 1.66533e-16"/>
-          </body>
-        </body>
-        <!-- Link pp01062_stewart_arm_5 -->
-        <body name="pp01062_stewart_arm_5" pos="-0.0420103 0.012419 -0.03705" quat="0.353553 -0.353553 0.612372 -0.612372">
-          <!-- Joint from dc15_a01_case_b_dummy to pp01062_stewart_arm_5 -->
-          <joint axis="0 0 1" name="5" type="hinge" range="-1.3962634015954662 1.396263401595461" class="chosen_actuator"/>
-          <inertial pos="0.0181787 3.11301e-35 0.00350559" mass="0.00272417" fullinertia="4.89963e-08 6.75761e-07 6.91798e-07 -4.49492e-25 -7.04811e-08 -1.26741e-25"/>
-          <!-- Part pp01062_stewart_arm_5 -->
-          <geom type="mesh" class="visual" pos="-0.01025 0.03 0.0425915" quat="0.707107 0.707107 -1.11022e-16 9.55014e-17" mesh="pp01062_stewart_arm" material="pp01062_stewart_arm_material"/>
-          <!-- Part ball_5 -->
-          <geom type="mesh" class="visual" pos="0.038 0 0.00768" quat="1.38778e-17 5.55459e-17 1 3.33067e-16" mesh="ball" material="ball_material"/>
-          <!-- Link pp00xxx_stewart_rod -->
-          <body name="pp00xxx_stewart_rod" pos="0.038 1.38778e-17 0.00768" quat="0.0908372 -0.850701 0.516198 -0.0399478">
-            <!-- Joint from pp01062_stewart_arm_5 to pp00xxx_stewart_rod -->
-            <joint axis="0 0 1" name="passive_5" type="ball" class="chosen_actuator"/>
-            <inertial pos="-0.0450002 7.44667e-17 -1.14016e-17" mass="0.00355525" fullinertia="1.7732e-08 3.06049e-06 3.0658e-06 4.55471e-21 -4.43821e-21 1.23083e-21"/>
-            <!-- Part shape_9 -->
-            <geom type="mesh" class="visual" pos="0 0 0" quat="1 0 -2.22045e-16 0" mesh="shape" material="shape_material"/>
-            <!-- Part pp00xxx_stewart_rod_5 -->
-            <geom type="mesh" class="visual" pos="0.00371031 0.0370326 -0.0564076" quat="0.855513 0.00634803 -0.517727 0.00384161" mesh="pp00xxx_stewart_rod" material="pp00xxx_stewart_rod_material"/>
-            <!-- Part shape_10 -->
-            <geom type="mesh" class="visual" pos="-0.0900003 1.38778e-16 6.93889e-18" quat="9.99201e-16 -4.04899e-16 1 1.9984e-15" mesh="shape" material="shape_material"/>
-            <!-- Frame closing_5_1 -->
-            <site group="3" name="closing_5_1" pos="-0.0900003 1.38778e-16 6.93889e-18" quat="9.99201e-16 -4.04899e-16 1 1.9984e-15"/>
-          </body>
-        </body>
-        <!-- Link pp01062_stewart_arm_6 -->
-        <body name="pp01062_stewart_arm_6" pos="-0.0317603 0.0301725 -0.03705" quat="0.612372 0.612372 0.353553 0.353553">
-          <!-- Joint from dc15_a01_case_b_dummy to pp01062_stewart_arm_6 -->
-          <joint axis="0 0 1" name="6" type="hinge" range="-1.3962634015954636 1.3962634015954636" class="chosen_actuator"/>
-          <inertial pos="0.0181787 -1.69382e-17 0.00350559" mass="0.00272417" fullinertia="4.89963e-08 6.75761e-07 6.91798e-07 1.6528e-22 -7.04811e-08 3.48685e-23"/>
-          <!-- Part pp01062_stewart_arm_6 -->
-          <geom type="mesh" class="visual" pos="-0.01025 0.03 0.0425915" quat="0.707107 0.707107 -1.11022e-16 1.00194e-16" mesh="pp01062_stewart_arm" material="pp01062_stewart_arm_material"/>
-          <!-- Part ball_6 -->
-          <geom type="mesh" class="visual" pos="0.038 -2.77556e-17 0.00768" quat="0 1 0 0" mesh="ball" material="ball_material"/>
-          <!-- Link shape_5 -->
-          <body name="shape_5" pos="0.038 -2.77556e-17 0.00768" quat="0.0131438 -0.855436 -0.517229 0.0230347">
-            <!-- Joint from pp01062_stewart_arm_6 to shape_5 -->
-            <joint axis="0 0 1" name="passive_6" type="ball" class="chosen_actuator"/>
-            <inertial pos="-0.0450002 7.95049e-16 -1.5589e-17" mass="0.00355525" fullinertia="1.7732e-08 3.06049e-06 3.0658e-06 4.84585e-20 -8.92792e-21 -1.86218e-20"/>
-            <!-- Part shape_11 -->
-            <geom type="mesh" class="visual" pos="-0.0900003 1.83187e-15 6.93889e-18" quat="1.77636e-15 6.61483e-15 1 6.27276e-15" mesh="shape" material="shape_material"/>
-            <!-- Part shape_12 -->
-            <geom type="mesh" class="visual" pos="6.93889e-18 0 0" quat="1 1.00165e-17 -8.32667e-17 0" mesh="shape" material="shape_material"/>
-            <!-- Part pp00xxx_stewart_rod_6 -->
-            <geom type="mesh" class="visual" pos="0.00371031 0.0370326 -0.0564076" quat="0.855513 0.00634803 -0.517727 0.00384161" mesh="pp00xxx_stewart_rod" material="pp00xxx_stewart_rod_material"/>
-            <!-- Link pp01063_stewart_plateform -->
-            <body name="pp01063_stewart_plateform" pos="-0.0900003 1.88738e-15 -2.08167e-17" quat="0.0131438 0.855436 0.517229 -0.0230347">
-              <!-- Joint from shape_5 to pp01063_stewart_plateform -->
-              <joint axis="0 0 1" name="passive_7" type="ball" class="chosen_actuator"/>
-              <inertial pos="-0.00488914 -0.0469134 0.0291732" mass="0.168092" fullinertia="0.000413571 0.000428808 0.000324749 -2.28564e-06 -5.24367e-05 -9.58295e-07"/>
-              <!-- Part small_lens -->
-              <geom type="mesh" class="visual" pos="0.0262444 -0.0524 0.0136834" quat="0.612372 0.612372 -0.353553 0.353553" mesh="small_lens" material="small_lens_material"/>
-              <!-- Part m12_lens -->
-              <geom type="mesh" class="visual" pos="0.010421 -0.0524 -0.0147088" quat="0.183013 0.683013 -0.683013 -0.183013" mesh="m12_lens" material="m12_lens_material"/>
-              <!-- Part pp01080_back_small_eye -->
-              <geom type="mesh" class="visual" pos="0.0262444 -0.0524 0.0136834" quat="0.612372 0.612372 -0.353553 0.353553" mesh="pp01080_back_small_eye" material="pp01080_back_small_eye_material"/>
-              <!-- Part m12_lens_2 -->
-              <geom type="mesh" class="visual" pos="0.042581 -0.0524 0.0417792" quat="0.183013 0.683013 -0.683013 -0.183013" mesh="m12_lens" material="m12_lens_material"/>
-              <!-- Part ball_7 -->
-              <geom type="mesh" class="visual" pos="-0.013 5.55112e-17 -5.55112e-17" quat="1 1.3274e-15 5.77316e-15 -2.63678e-15" mesh="ball" material="ball_material"/>
-              <!-- Part uc_a37_rev_a_step -->
-              <geom type="mesh" class="visual" pos="0.0239513 -0.04265 0.0271316" quat="9.36644e-18 0.866025 0 0.5" mesh="uc_a37_rev_a_step" material="uc_a37_rev_a_step_material"/>
-              <!-- Part ball_8 -->
-              <geom type="mesh" class="visual" pos="-8.67362e-19 2.77556e-17 0" quat="1 -0 -2.04613e-16 -5.18024e-19" mesh="ball" material="ball_material"/>
-              <!-- Part pp01070_head_head_back -->
-              <geom type="mesh" class="visual" pos="-0.0134282 -0.0524 0.0365883" quat="0.612372 0.612372 -0.353553 0.353553" mesh="pp01070_head_head_back" material="pp01070_head_head_back_material"/>
-              <!-- Part ball_9 -->
-              <geom type="mesh" class="visual" pos="0.0249723 5.55112e-17 0.0432533" quat="1.36964e-17 0.866025 -1.11022e-16 -0.5" mesh="ball" material="ball_material"/>
-              <!-- Part pp01079_back_big_eye -->
-              <geom type="mesh" class="visual" pos="0.0265844 -0.0524 0.013487" quat="0.612372 0.612372 -0.353553 0.353553" mesh="pp01079_back_big_eye" material="pp01079_back_big_eye_material"/>
-              <!-- Part ball_10 -->
-              <geom type="mesh" class="visual" pos="-0.0379723 5.55112e-17 0.0432533" quat="0.353553 -0.612372 0.612372 -0.353553" mesh="ball" material="ball_material"/>
-              <!-- Part pp01069_head_shell_front -->
-              <geom type="mesh" class="visual" pos="-0.0142942 -0.0524 0.0370883" quat="0.612372 0.612372 -0.353553 0.353553" mesh="pp01069_head_shell_front" material="pp01069_head_shell_front_material"/>
-              <!-- Part pp01078_glasses -->
-              <geom type="mesh" class="visual" pos="0.025976 -0.0524 0.0138383" quat="0.612372 0.612372 -0.353553 0.353553" mesh="pp01078_glasses" material="pp01078_glasses_material"/>
-              <!-- Part pp01063_stewart_plateform -->
-              <geom type="mesh" class="visual" pos="-0.0065 0.109722 0.0325883" quat="0.353553 0.353553 0.612372 -0.612372" mesh="pp01063_stewart_plateform" material="pp01063_stewart_plateform_material"/>
-              <!-- Part ball_11 -->
-              <geom type="mesh" class="visual" pos="-0.0314723 2.77556e-17 0.0545117" quat="0.353553 -0.612372 0.612372 -0.353553" mesh="ball" material="ball_material"/>
-              <!-- Part big_lens -->
-              <geom type="mesh" class="visual" pos="0.0281677 -0.0524 0.0125729" quat="0.612372 0.612372 -0.353553 0.353553" mesh="big_lens" material="big_lens_material"/>
-              <!-- Part ball_12 -->
-              <geom type="mesh" class="visual" pos="0.0184723 5.55112e-17 0.0545117" quat="3.07775e-17 -0.866025 -0 0.5" mesh="ball" material="ball_material"/>
-              <!-- Frame closing_1_2 -->
-              <site group="3" name="closing_1_2" pos="0.0249723 5.55112e-17 0.0432533" quat="0.353553 0.612372 -0.612372 -0.353553"/>
-              <!-- Frame closing_2_1 -->
-              <site group="3" name="closing_2_1" pos="0.0184723 5.55112e-17 0.0545117" quat="3.07775e-17 -0.866025 -0 0.5"/>
-              <!-- Frame closing_3_1 -->
-              <site group="3" name="closing_3_1" pos="-0.0314723 2.77556e-17 0.0545117" quat="0.353553 -0.612372 0.612372 -0.353553"/>
-              <!-- Frame closing_4_2 -->
-              <site group="3" name="closing_4_2" pos="-0.0379723 5.55112e-17 0.0432533" quat="0.612372 0.353553 -0.353553 -0.612372"/>
-              <!-- Frame closing_5_2 -->
-              <site group="3" name="closing_5_2" pos="-0.013 5.55112e-17 -5.55112e-17" quat="0.707107 -3.16544e-15 5.10703e-15 -0.707107"/>
-              <!-- Frame head -->
-              <site group="3" name="head" pos="-0.0134282 -0.0012 0.0365883" quat="0.683013 0.683013 0.183013 -0.183013"/>
-              <!-- Frame camera -->
-              <site group="3" name="camera" pos="0.0244171 -0.0524 0.0147383" quat="0.683013 0.683013 0.183013 -0.183013"/>
-              <!-- Frame camera_optical -->
-              <site group="3" name="camera_optical" pos="0.0244171 -0.0524 0.0147383" quat="0.5 0 0.866025 -2.22045e-16"/>
-              <!-- Link test_antenna_body -->
-              <body name="test_antenna_body" pos="-0.0161053 -0.0744 0.100068" quat="0.964418 0.0356203 -0.25955 0.0355422">
-                <!-- Joint from pp01063_stewart_plateform to test_antenna_body -->
-                <joint axis="0 0 1" name="left_antenna" type="hinge" class="chosen_actuator"/>
-                <inertial pos="0.00147983 -0.0294086 0.00298611" mass="0.00456766" fullinertia="1.38179e-05 2.28739e-07 1.38227e-05 6.85535e-07 5.01796e-08 -9.97217e-07"/>
-                <!-- Part test_antenna -->
-                <geom type="mesh" class="visual" pos="0.0285777 0.0175124 -0.0571006" quat="0.716727 0.647969 0.191203 -0.172861" mesh="test_antenna" material="test_antenna_material"/>
-                <!-- Part test_antenna_body -->
-                <geom type="mesh" class="visual" pos="0.0285297 0.0184653 -0.0570171" quat="0.716727 0.647969 0.191203 -0.172861" mesh="test_antenna_body" material="test_antenna_body_material"/>
-              </body>
-              <!-- Link test_antenna -->
-              <body name="test_antenna" pos="-0.0697422 -0.0744 0.00716666" quat="0.706492 0.0909539 -0.701233 -0.0294913">
-                <!-- Joint from pp01063_stewart_plateform to test_antenna -->
-                <joint axis="0 0 1" name="right_antenna" type="hinge" class="chosen_actuator"/>
-                <inertial pos="0.00147983 -0.0294086 0.00298611" mass="0.00456766" fullinertia="1.38179e-05 2.28739e-07 1.38227e-05 6.85535e-07 5.01796e-08 -9.97217e-07"/>
-                <!-- Part test_antenna_2 -->
-                <geom type="mesh" class="visual" pos="0.0285777 0.0175124 -0.0571006" quat="0.716727 0.647969 0.191203 -0.172861" mesh="test_antenna" material="test_antenna_material"/>
-                <!-- Part test_antenna_body_2 -->
-                <geom type="mesh" class="visual" pos="0.0285297 0.0184653 -0.0570171" quat="0.716727 0.647969 0.191203 -0.172861" mesh="test_antenna_body" material="test_antenna_body_material"/>
-              </body>
-            </body>
-          </body>
-        </body>
-      </body>
-    </body>
-  </worldbody>
-  <asset>
-    <mesh file="dc15_a01_case_b_dummy.stl"/>
-    <mesh file="pp01078_glasses.stl"/>
-    <mesh file="big_lens.stl"/>
-    <mesh file="test_antenna_body.stl"/>
-    <mesh file="pp01065_stewart_side_plate.stl"/>
-    <mesh file="small_lens.stl"/>
-    <mesh file="pp01080_back_small_eye.stl"/>
-    <mesh file="pp01064_stewart_main_plate.stl"/>
-    <mesh file="shape.stl"/>
-    <mesh file="pp00xxx_stewart_rod.stl"/>
-    <mesh file="pp01079_back_big_eye.stl"/>
-    <mesh file="ball.stl"/>
-    <mesh file="wj_wk00_0123middlecase_56__configuration_default.stl"/>
-    <mesh file="test_antenna.stl"/>
-    <mesh file="pp01070_head_head_back.stl"/>
-    <mesh file="dc15_a01_case_f_dummy.stl"/>
-    <mesh file="drive_palonier__configuration_default.stl"/>
-    <mesh file="dc15_a01_horn_dummy.stl"/>
-    <mesh file="wj_wk00_0122topcabinetcase_95__configuration_default.stl"/>
-    <mesh file="pp01071_turning_bowl.stl"/>
-    <mesh file="pp01068_top_body.stl"/>
-    <mesh file="wj_wk00_0124bottomcase_45__configuration_default.stl"/>
-    <mesh file="uc_a37_rev_a_step.stl"/>
-    <mesh file="dc15_a01_case_m_dummy.stl"/>
-    <mesh file="pp01062_stewart_arm.stl"/>
-    <mesh file="pp01072_turning_end.stl"/>
-    <mesh file="m12_lens.stl"/>
-    <mesh file="pp01069_head_shell_front.stl"/>
-    <mesh file="pp01063_stewart_plateform.stl"/>
-    <mesh file="pp01066_stewart_mid_plate.stl"/>
-    <mesh file="pp01067_bottom_body.stl"/>
-    <material name="pp01071_turning_bowl_material" rgba="0.34902 0.376471 0.4 1"/>
-    <material name="pp01072_turning_end_material" rgba="0.262745 0.282353 0.301961 1"/>
-    <material name="pp01065_stewart_side_plate_material" rgba="0.196078 0.196078 0.196078 1"/>
-    <material name="pp01067_bottom_body_material" rgba="1 1 1 1"/>
-    <material name="dc15_a01_case_m_dummy_material" rgba="0.262745 0.282353 0.301961 1"/>
-    <material name="dc15_a01_case_f_dummy_material" rgba="0.262745 0.282353 0.301961 1"/>
-    <material name="dc15_a01_horn_dummy_material" rgba="0.262745 0.282353 0.301961 1"/>
-    <material name="wj_wk00_0122topcabinetcase_95__configuration_default_material" rgba="0.247059 0.243137 0.243137 1"/>
-    <material name="dc15_a01_case_b_dummy_material" rgba="0.262745 0.282353 0.301961 1"/>
-    <material name="pp01068_top_body_material" rgba="1 1 1 1"/>
-    <material name="drive_palonier__configuration_default_material" rgba="0.623529 0.666667 0.701961 1"/>
-    <material name="wj_wk00_0123middlecase_56__configuration_default_material" rgba="0.247059 0.243137 0.243137 1"/>
-    <material name="pp01066_stewart_mid_plate_material" rgba="0.196078 0.196078 0.196078 1"/>
-    <material name="wj_wk00_0124bottomcase_45__configuration_default_material" rgba="0.247059 0.243137 0.243137 1"/>
-    <material name="pp01064_stewart_main_plate_material" rgba="0.196078 0.196078 0.196078 1"/>
-    <material name="ball_material" rgba="0.901961 0.901961 0.901961 1"/>
-    <material name="pp01062_stewart_arm_material" rgba="0.196078 0.196078 0.196078 1"/>
-    <material name="shape_material" rgba="0.901961 0.901961 0.901961 1"/>
-    <material name="pp00xxx_stewart_rod_material" rgba="0.8 0.8 0.8 1"/>
-    <material name="small_lens_material" rgba="0.439216 0.47451 0.501961 0.298039"/>
-    <material name="m12_lens_material" rgba="0.262745 0.282353 0.301961 1"/>
-    <material name="pp01080_back_small_eye_material" rgba="0.168627 0.168627 0.168627 1"/>
-    <material name="uc_a37_rev_a_step_material" rgba="0.25098 0.25098 0.25098 1"/>
-    <material name="pp01070_head_head_back_material" rgba="1 1 1 1"/>
-    <material name="pp01079_back_big_eye_material" rgba="0.168627 0.168627 0.168627 1"/>
-    <material name="pp01069_head_shell_front_material" rgba="1 1 1 1"/>
-    <material name="pp01078_glasses_material" rgba="0.168627 0.168627 0.168627 1"/>
-    <material name="pp01063_stewart_plateform_material" rgba="1 1 1 1"/>
-    <material name="big_lens_material" rgba="0.439216 0.47451 0.501961 0.298039"/>
-    <material name="test_antenna_material" rgba="0.262745 0.282353 0.301961 1"/>
-    <material name="test_antenna_body_material" rgba="1 1 1 1"/>
-  </asset>
-  <actuator>
-    <position class="chosen_actuator" name="all_yaw" joint="all_yaw"/>
-    <position class="chosen_actuator" name="1" joint="1" inheritrange="1"/>
-    <position class="chosen_actuator" name="2" joint="2" inheritrange="1"/>
-    <position class="chosen_actuator" name="3" joint="3" inheritrange="1"/>
-    <position class="chosen_actuator" name="4" joint="4" inheritrange="1"/>
-    <position class="chosen_actuator" name="5" joint="5" inheritrange="1"/>
-    <position class="chosen_actuator" name="6" joint="6" inheritrange="1"/>
-    <position class="chosen_actuator" name="left_antenna" joint="left_antenna"/>
-    <position class="chosen_actuator" name="right_antenna" joint="right_antenna"/>
-  </actuator>
-  <equality>
-    <connect site1="closing_1_1" site2="closing_1_2"/>
-    <connect site1="closing_2_1" site2="closing_2_2"/>
-    <connect site1="closing_3_1" site2="closing_3_2"/>
-    <connect site1="closing_4_1" site2="closing_4_2"/>
-    <connect site1="closing_5_1" site2="closing_5_2"/>
-  </equality>
-</mujoco>
diff --git a/src/reachy_mini/kinematics/placo_kinematics.py b/src/reachy_mini/kinematics/placo_kinematics.py
index 01896d90..5e87a9ea 100644
--- a/src/reachy_mini/kinematics/placo_kinematics.py
+++ b/src/reachy_mini/kinematics/placo_kinematics.py
@@ -89,7 +89,7 @@ def __init__(
         # This will allow independent control of the torso and the head yaw
         # until this constraint is reached
         yaw_constraint = self.ik_solver.add_yaw_constraint(
-            "dummy_torso_yaw", "head", np.deg2rad(65.0)
+            "dummy_torso_yaw", "head", np.deg2rad(55.0)
         )
         yaw_constraint.configure("rel_yaw", "hard")
 
@@ -105,18 +105,18 @@ def __init__(
         # Add a cone constraint for the head to not exceed a certain angle
         # This is to avoid the head from looking too far up or down
         self.fk_cone = self.ik_solver.add_cone_constraint(
-            "body_foot_3dprint", "head", np.deg2rad(40.0)
+            "body_foot_3dprint", "head", np.deg2rad(30.0)
         )
         self.fk_cone.configure("cone", "hard")
         self.fk_yaw_constraint = self.fk_solver.add_yaw_constraint(
-            "dummy_torso_yaw", "head", np.deg2rad(65.0)
+            "dummy_torso_yaw", "head", np.deg2rad(55.0)
         )
         self.fk_yaw_constraint.configure("rel_yaw", "hard")
 
         # Add a cone constraint for the head to not exceed a certain angle
         # This is to avoid the head from looking too far up or down
         fk_cone = self.fk_solver.add_cone_constraint(
-            "body_foot_3dprint", "head", np.deg2rad(40.0)
+            "body_foot_3dprint", "head", np.deg2rad(30.0)
         )
         fk_cone.configure("cone", "hard")
 
@@ -142,7 +142,7 @@ def __init__(
         # regularization
         self.ik_yaw_joint_task = self.ik_solver.add_joints_task()
         self.ik_yaw_joint_task.set_joints({"yaw_body": 0})
-        if self.automatic_body_yaw:
+        if not self.automatic_body_yaw:
             self.ik_yaw_joint_task.configure("joints", "soft", 5e-5)
         else:
             self.ik_yaw_joint_task.configure("joints", "soft", 1.0)

From 6b8ae1a4ff76a45f928e6fc3ee0d2030254f4489 Mon Sep 17 00:00:00 2001
From: gospar <antun.skuric@outlook.com>
Date: Fri, 24 Oct 2025 10:28:50 +0200
Subject: [PATCH 09/15] ruff

---
 examples/gui_demos/mini_head_lookat_gui.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/examples/gui_demos/mini_head_lookat_gui.py b/examples/gui_demos/mini_head_lookat_gui.py
index 035c5148..fd45f296 100644
--- a/examples/gui_demos/mini_head_lookat_gui.py
+++ b/examples/gui_demos/mini_head_lookat_gui.py
@@ -4,11 +4,11 @@
 import tkinter as tk
 
 import numpy as np
-from scipy.spatial.transform import Rotation as R
 
 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:

From 55088898325fb8e71602b99ba4a202922ba81471 Mon Sep 17 00:00:00 2001
From: gospar <antun.skuric@outlook.com>
Date: Fri, 24 Oct 2025 10:49:09 +0200
Subject: [PATCH 10/15] remove print

---
 src/reachy_mini/daemon/backend/mujoco/backend.py | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/reachy_mini/daemon/backend/mujoco/backend.py b/src/reachy_mini/daemon/backend/mujoco/backend.py
index 9f023866..1499d788 100644
--- a/src/reachy_mini/daemon/backend/mujoco/backend.py
+++ b/src/reachy_mini/daemon/backend/mujoco/backend.py
@@ -104,7 +104,6 @@ def __init__(
         self.col_inds = []
         for i,type in enumerate(self.model.geom_contype):
             if type != 0:
-                print(f"Geom {i} : contype={self.model.geom_contype[i]}, conaffinity={self.model.geom_conaffinity[i]}")
                 self.col_inds.append(i)
                 self.model.geom_contype[i] = 0
                 self.model.geom_conaffinity[i] = 0

From 0647847d32b42430ee9059502591237ed49f9767 Mon Sep 17 00:00:00 2001
From: gospar <antun.skuric@outlook.com>
Date: Mon, 27 Oct 2025 18:15:34 +0100
Subject: [PATCH 11/15] much better collision avoidance

---
 .../daemon/backend/mujoco/backend.py          |  5 +++
 .../body_top_3dprint_collider_back_0.stl      |  3 ++
 .../body_top_3dprint_collider_back_1.stl      |  3 ++
 .../body_top_3dprint_collider_back_2.stl      |  3 ++
 .../body_top_3dprint_collider_back_3.stl      |  3 ++
 .../body_top_3dprint_collider_front_0.stl     |  3 ++
 .../body_top_3dprint_collider_front_1.stl     |  3 ++
 .../body_top_3dprint_collider_front_2.stl     |  3 ++
 .../coarse/head_one_3dprint_collider_0.stl    |  3 ++
 .../fine/body_top_3dprint_collider_back_0.stl |  3 ++
 .../fine/body_top_3dprint_collider_back_1.stl |  3 ++
 .../fine/body_top_3dprint_collider_back_2.stl |  3 ++
 .../fine/body_top_3dprint_collider_back_3.stl |  3 ++
 .../body_top_3dprint_collider_front_0.stl     |  3 ++
 .../body_top_3dprint_collider_front_1.stl     |  3 ++
 .../body_top_3dprint_collider_front_2.stl     |  3 ++
 .../fine/head_one_3dprint_collider_0.stl      |  3 ++
 .../reachy_mini/mjcf/reachy_mini.xml          | 37 +++++++++++--------
 .../kinematics/placo_kinematics.py            |  4 +-
 19 files changed, 77 insertions(+), 17 deletions(-)
 create mode 100644 src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_back_0.stl
 create mode 100644 src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_back_1.stl
 create mode 100644 src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_back_2.stl
 create mode 100644 src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_back_3.stl
 create mode 100644 src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_front_0.stl
 create mode 100644 src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_front_1.stl
 create mode 100644 src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_front_2.stl
 create mode 100644 src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/head_one_3dprint_collider_0.stl
 create mode 100644 src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_back_0.stl
 create mode 100644 src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_back_1.stl
 create mode 100644 src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_back_2.stl
 create mode 100644 src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_back_3.stl
 create mode 100644 src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_front_0.stl
 create mode 100644 src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_front_1.stl
 create mode 100644 src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_front_2.stl
 create mode 100644 src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/head_one_3dprint_collider_0.stl

diff --git a/src/reachy_mini/daemon/backend/mujoco/backend.py b/src/reachy_mini/daemon/backend/mujoco/backend.py
index 1499d788..6decdb55 100644
--- a/src/reachy_mini/daemon/backend/mujoco/backend.py
+++ b/src/reachy_mini/daemon/backend/mujoco/backend.py
@@ -185,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()
diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_back_0.stl b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_back_0.stl
new file mode 100644
index 00000000..b134982c
--- /dev/null
+++ b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_back_0.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:8f92539090c8fbef3a292ffbdc6a1019828ad5e33a5b87354ad08032a59d0c35
+size 4884
diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_back_1.stl b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_back_1.stl
new file mode 100644
index 00000000..dc98ea93
--- /dev/null
+++ b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_back_1.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:9186fdc67813c3b7f3fba721cbfbb14d5e6f20bcabe1897d488913b6879bfd78
+size 4784
diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_back_2.stl b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_back_2.stl
new file mode 100644
index 00000000..592ecb1e
--- /dev/null
+++ b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_back_2.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2db1d2726afd373e145a7b3cb8340983eeff49c07f8e4057e2d4eec7f91c35df
+size 4884
diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_back_3.stl b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_back_3.stl
new file mode 100644
index 00000000..2bea29e7
--- /dev/null
+++ b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_back_3.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:3e7916bfcdccd254c51143774575c7a0924a1a1939fd9a4755ae3b8f2c920bdc
+size 4884
diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_front_0.stl b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_front_0.stl
new file mode 100644
index 00000000..728659d3
--- /dev/null
+++ b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_front_0.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:94d9c386d10344ae4fa03baae3ea0ca935c8ab2a5d41352a4b4c59c3ac2fd9e1
+size 4884
diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_front_1.stl b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_front_1.stl
new file mode 100644
index 00000000..d296a17e
--- /dev/null
+++ b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_front_1.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:e893cc8cd9581722ffe7798031076190cdc2264edc70694d925be5cf326b8bc2
+size 4884
diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_front_2.stl b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_front_2.stl
new file mode 100644
index 00000000..d990c548
--- /dev/null
+++ b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_front_2.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:1ec16d901b544cd898eb1836d4f61fc16855919bd856c82f0fd23ac180e684af
+size 4884
diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/head_one_3dprint_collider_0.stl b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/head_one_3dprint_collider_0.stl
new file mode 100644
index 00000000..2dbfd40c
--- /dev/null
+++ b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/head_one_3dprint_collider_0.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:7ab3bd4077363b5c0196434dba82051b19ec63ec76ef93e76b28cdd1c0187ddd
+size 4884
diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_back_0.stl b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_back_0.stl
new file mode 100644
index 00000000..6e405fc6
--- /dev/null
+++ b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_back_0.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:7c22bb171ab47155a4fe2946cd267f8e43cba901dd11d3106082bbe1848f90b4
+size 190084
diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_back_1.stl b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_back_1.stl
new file mode 100644
index 00000000..34019f99
--- /dev/null
+++ b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_back_1.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:9a2c6189ab48a1b296ab5e0695363ab20021154dc7823d3eef8a188b992c1682
+size 195784
diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_back_2.stl b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_back_2.stl
new file mode 100644
index 00000000..67603dad
--- /dev/null
+++ b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_back_2.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:cbdf0d34da912a29174148bd78b90e3541755f9bc20d2a5cc624e3565aef3d94
+size 78684
diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_back_3.stl b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_back_3.stl
new file mode 100644
index 00000000..37d62560
--- /dev/null
+++ b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_back_3.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:4e7c26eb2033477e711d6ea535b955a91f443b74d21a4d8e5ed1bfb72c10c401
+size 245084
diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_front_0.stl b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_front_0.stl
new file mode 100644
index 00000000..f0f3c8da
--- /dev/null
+++ b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_front_0.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:27dcf3bcfc55a11f5fe3a6a5f01fe253fb5e9cf9d736aa9b83a44d286aeef38c
+size 185384
diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_front_1.stl b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_front_1.stl
new file mode 100644
index 00000000..ce58be8b
--- /dev/null
+++ b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_front_1.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:89940a91f8e1bf1d1b7c4844bc58a8a03f5c3983cfc174573e8f60f6a47c1109
+size 182484
diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_front_2.stl b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_front_2.stl
new file mode 100644
index 00000000..8964aa93
--- /dev/null
+++ b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_front_2.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:6ccf48cd480051da28caee941ad1e130f74cebfd2c474d5791d5e014e10fcdfd
+size 219184
diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/head_one_3dprint_collider_0.stl b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/head_one_3dprint_collider_0.stl
new file mode 100644
index 00000000..1d636e5f
--- /dev/null
+++ b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/head_one_3dprint_collider_0.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:5a28add53d3bc49c5b7f6d9e8f8f0884bbfa1ad9fe81af5ca58f01d08a072ed9
+size 1359584
diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/reachy_mini.xml b/src/reachy_mini/descriptions/reachy_mini/mjcf/reachy_mini.xml
index dc66a337..bb32faf5 100644
--- a/src/reachy_mini/descriptions/reachy_mini/mjcf/reachy_mini.xml
+++ b/src/reachy_mini/descriptions/reachy_mini/mjcf/reachy_mini.xml
@@ -75,16 +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="box" class="collision" name="body_front_collider2" size="0.03 0.005 0.03" pos="0.02 0.065 0.115" euler="0.3 0.1 -0.2"/>
-        <geom type="box" class="collision" name="body_front_collider1" size="0.03 0.005 0.03" pos="-0.02 0.065 0.115" euler="0.3 -0.1 0.2"/> -->
-        <geom type="box" class="collision" name="body_front_collider" size="0.035 0.005 0.08" pos="0 0.062 0.07" euler="0 0 0"/>
-
-        <geom type="box" class="collision" name="body_front_collider2" size="0.032 0.005 0.01" pos="0.05 0.055 0.12" euler="1.15 0 2.1"/>
-        <geom type="box" class="collision" name="body_front_collider1" size="0.032 0.005 0.01" pos="-0.05 0.055 0.12" euler="1.15 0 -2.1"/>
-        <geom type="box" class="collision" name="body_back_collider" size="0.04 0.005 0.03" pos="0 -0.057 0.1" euler="0 0 0"/>
-        <geom type="box" class="collision" name="body_eft_collider" size="0.04 0.005 0.06" pos="-0.068 -0.02 0.045" euler="-0.3 0 1.77"/>
-        <geom type="box" class="collision" name="body_right_collider" size="0.04 0.005 0.06" pos="0.068 -0.02 0.045" euler="-0.3 0 -1.77"/>
-        <!-- <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" material="body_top_3dprint_material"/> -->
+        <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"/>
@@ -410,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"/>
@@ -481,14 +480,11 @@
                   resolution="1280 720"
                   fovy="80"
                 />
-                <geom type="capsule" class="collision" name="head_collider" size="0.051 0.03" pos="0 0 0.042" euler="0 1.57 0"/>
-        
               </body> 
+              
               <!-- Frame head -->
               <site group="3" name="head" pos="-0.00611127 0.00370522 0.0291364" quat="0.465151 0.724371 -0.182968 -0.474809"/>
-              <!-- <geom type="capsule" class="collision" name="head_collider"  quat="0.612372 0.612372 -0.353553 0.353553" size="0.05"/> -->
-              <!-- <geom type="capsule" class="collision" name="head_collider" size="0.048 0.01" 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 -->
@@ -565,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"/>
diff --git a/src/reachy_mini/kinematics/placo_kinematics.py b/src/reachy_mini/kinematics/placo_kinematics.py
index 5e87a9ea..3c977bdd 100644
--- a/src/reachy_mini/kinematics/placo_kinematics.py
+++ b/src/reachy_mini/kinematics/placo_kinematics.py
@@ -105,7 +105,7 @@ def __init__(
         # Add a cone constraint for the head to not exceed a certain angle
         # This is to avoid the head from looking too far up or down
         self.fk_cone = self.ik_solver.add_cone_constraint(
-            "body_foot_3dprint", "head", np.deg2rad(30.0)
+            "body_foot_3dprint", "head", np.deg2rad(35.0)
         )
         self.fk_cone.configure("cone", "hard")
         self.fk_yaw_constraint = self.fk_solver.add_yaw_constraint(
@@ -116,7 +116,7 @@ def __init__(
         # Add a cone constraint for the head to not exceed a certain angle
         # This is to avoid the head from looking too far up or down
         fk_cone = self.fk_solver.add_cone_constraint(
-            "body_foot_3dprint", "head", np.deg2rad(30.0)
+            "body_foot_3dprint", "head", np.deg2rad(35.0)
         )
         fk_cone.configure("cone", "hard")
 

From a84465b4497801e2f37274f48fb4328061903033 Mon Sep 17 00:00:00 2001
From: gospar <antun.skuric@outlook.com>
Date: Tue, 28 Oct 2025 11:05:34 +0100
Subject: [PATCH 12/15] a bit better head

---
 .../assets/collision/coarse/head_one_3dprint_collider_0.stl   | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/head_one_3dprint_collider_0.stl b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/head_one_3dprint_collider_0.stl
index 2dbfd40c..b5a03639 100644
--- a/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/head_one_3dprint_collider_0.stl
+++ b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/head_one_3dprint_collider_0.stl
@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:7ab3bd4077363b5c0196434dba82051b19ec63ec76ef93e76b28cdd1c0187ddd
-size 4884
+oid sha256:9ba0e5a4dc95869e16212e69fa3712ea7f021378cf76db0b1a8d2300eaab3c64
+size 6884

From 878a813b9bb3be9d54eaa7e119b29075339e1770 Mon Sep 17 00:00:00 2001
From: gospar <antun.skuric@outlook.com>
Date: Tue, 28 Oct 2025 11:30:21 +0100
Subject: [PATCH 13/15] info in readme

---
 .../descriptions/reachy_mini/mjcf/assets/collision/README.m      | 1 +
 1 file changed, 1 insertion(+)
 create mode 100644 src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/README.m

diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/README.m b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/README.m
new file mode 100644
index 00000000..edda0bc7
--- /dev/null
+++ b/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
\ No newline at end of file

From 88ec248dc6b1d9e0954f28f8b1dc72b5e25dab8f Mon Sep 17 00:00:00 2001
From: gospar <antun.skuric@outlook.com>
Date: Thu, 30 Oct 2025 10:13:39 +0100
Subject: [PATCH 14/15] new behavior for placo and collisions

---
 .../urdf/assets/collision/README.m            |    1 +
 .../body_top_3dprint_collider_back_0.stl      |    3 +
 .../body_top_3dprint_collider_back_1.stl      |    3 +
 .../body_top_3dprint_collider_back_2.stl      |    3 +
 .../body_top_3dprint_collider_back_3.stl      |    3 +
 .../body_top_3dprint_collider_front_0.stl     |    3 +
 .../body_top_3dprint_collider_front_1.stl     |    3 +
 .../body_top_3dprint_collider_front_2.stl     |    3 +
 .../coarse/head_one_3dprint_collider_0.stl    |    3 +
 .../fine/body_top_3dprint_collider_back_0.stl |    3 +
 .../fine/body_top_3dprint_collider_back_1.stl |    3 +
 .../fine/body_top_3dprint_collider_back_2.stl |    3 +
 .../fine/body_top_3dprint_collider_back_3.stl |    3 +
 .../body_top_3dprint_collider_front_0.stl     |    3 +
 .../body_top_3dprint_collider_front_1.stl     |    3 +
 .../body_top_3dprint_collider_front_2.stl     |    3 +
 .../fine/head_one_3dprint_collider_0.stl      |    3 +
 .../urdf/robot_simple_collision.urdf          | 2399 +++++++++++++++++
 .../kinematics/placo_kinematics.py            |   58 +-
 19 files changed, 2470 insertions(+), 36 deletions(-)
 create mode 100644 src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/README.m
 create mode 100644 src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/coarse/body_top_3dprint_collider_back_0.stl
 create mode 100644 src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/coarse/body_top_3dprint_collider_back_1.stl
 create mode 100644 src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/coarse/body_top_3dprint_collider_back_2.stl
 create mode 100644 src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/coarse/body_top_3dprint_collider_back_3.stl
 create mode 100644 src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/coarse/body_top_3dprint_collider_front_0.stl
 create mode 100644 src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/coarse/body_top_3dprint_collider_front_1.stl
 create mode 100644 src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/coarse/body_top_3dprint_collider_front_2.stl
 create mode 100644 src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/coarse/head_one_3dprint_collider_0.stl
 create mode 100644 src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/fine/body_top_3dprint_collider_back_0.stl
 create mode 100644 src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/fine/body_top_3dprint_collider_back_1.stl
 create mode 100644 src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/fine/body_top_3dprint_collider_back_2.stl
 create mode 100644 src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/fine/body_top_3dprint_collider_back_3.stl
 create mode 100644 src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/fine/body_top_3dprint_collider_front_0.stl
 create mode 100644 src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/fine/body_top_3dprint_collider_front_1.stl
 create mode 100644 src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/fine/body_top_3dprint_collider_front_2.stl
 create mode 100644 src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/fine/head_one_3dprint_collider_0.stl
 create mode 100644 src/reachy_mini/descriptions/reachy_mini/urdf/robot_simple_collision.urdf

diff --git a/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/README.m b/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/README.m
new file mode 100644
index 00000000..edda0bc7
--- /dev/null
+++ b/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/README.m
@@ -0,0 +1 @@
+Created with: https://github.com/pollen-robotics/reachy_mini_stl_convexify
\ No newline at end of file
diff --git a/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/coarse/body_top_3dprint_collider_back_0.stl b/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/coarse/body_top_3dprint_collider_back_0.stl
new file mode 100644
index 00000000..b134982c
--- /dev/null
+++ b/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/coarse/body_top_3dprint_collider_back_0.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:8f92539090c8fbef3a292ffbdc6a1019828ad5e33a5b87354ad08032a59d0c35
+size 4884
diff --git a/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/coarse/body_top_3dprint_collider_back_1.stl b/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/coarse/body_top_3dprint_collider_back_1.stl
new file mode 100644
index 00000000..dc98ea93
--- /dev/null
+++ b/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/coarse/body_top_3dprint_collider_back_1.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:9186fdc67813c3b7f3fba721cbfbb14d5e6f20bcabe1897d488913b6879bfd78
+size 4784
diff --git a/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/coarse/body_top_3dprint_collider_back_2.stl b/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/coarse/body_top_3dprint_collider_back_2.stl
new file mode 100644
index 00000000..592ecb1e
--- /dev/null
+++ b/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/coarse/body_top_3dprint_collider_back_2.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2db1d2726afd373e145a7b3cb8340983eeff49c07f8e4057e2d4eec7f91c35df
+size 4884
diff --git a/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/coarse/body_top_3dprint_collider_back_3.stl b/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/coarse/body_top_3dprint_collider_back_3.stl
new file mode 100644
index 00000000..2bea29e7
--- /dev/null
+++ b/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/coarse/body_top_3dprint_collider_back_3.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:3e7916bfcdccd254c51143774575c7a0924a1a1939fd9a4755ae3b8f2c920bdc
+size 4884
diff --git a/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/coarse/body_top_3dprint_collider_front_0.stl b/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/coarse/body_top_3dprint_collider_front_0.stl
new file mode 100644
index 00000000..728659d3
--- /dev/null
+++ b/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/coarse/body_top_3dprint_collider_front_0.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:94d9c386d10344ae4fa03baae3ea0ca935c8ab2a5d41352a4b4c59c3ac2fd9e1
+size 4884
diff --git a/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/coarse/body_top_3dprint_collider_front_1.stl b/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/coarse/body_top_3dprint_collider_front_1.stl
new file mode 100644
index 00000000..d296a17e
--- /dev/null
+++ b/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/coarse/body_top_3dprint_collider_front_1.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:e893cc8cd9581722ffe7798031076190cdc2264edc70694d925be5cf326b8bc2
+size 4884
diff --git a/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/coarse/body_top_3dprint_collider_front_2.stl b/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/coarse/body_top_3dprint_collider_front_2.stl
new file mode 100644
index 00000000..d990c548
--- /dev/null
+++ b/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/coarse/body_top_3dprint_collider_front_2.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:1ec16d901b544cd898eb1836d4f61fc16855919bd856c82f0fd23ac180e684af
+size 4884
diff --git a/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/coarse/head_one_3dprint_collider_0.stl b/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/coarse/head_one_3dprint_collider_0.stl
new file mode 100644
index 00000000..b5a03639
--- /dev/null
+++ b/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/coarse/head_one_3dprint_collider_0.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:9ba0e5a4dc95869e16212e69fa3712ea7f021378cf76db0b1a8d2300eaab3c64
+size 6884
diff --git a/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/fine/body_top_3dprint_collider_back_0.stl b/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/fine/body_top_3dprint_collider_back_0.stl
new file mode 100644
index 00000000..6e405fc6
--- /dev/null
+++ b/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/fine/body_top_3dprint_collider_back_0.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:7c22bb171ab47155a4fe2946cd267f8e43cba901dd11d3106082bbe1848f90b4
+size 190084
diff --git a/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/fine/body_top_3dprint_collider_back_1.stl b/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/fine/body_top_3dprint_collider_back_1.stl
new file mode 100644
index 00000000..34019f99
--- /dev/null
+++ b/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/fine/body_top_3dprint_collider_back_1.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:9a2c6189ab48a1b296ab5e0695363ab20021154dc7823d3eef8a188b992c1682
+size 195784
diff --git a/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/fine/body_top_3dprint_collider_back_2.stl b/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/fine/body_top_3dprint_collider_back_2.stl
new file mode 100644
index 00000000..67603dad
--- /dev/null
+++ b/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/fine/body_top_3dprint_collider_back_2.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:cbdf0d34da912a29174148bd78b90e3541755f9bc20d2a5cc624e3565aef3d94
+size 78684
diff --git a/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/fine/body_top_3dprint_collider_back_3.stl b/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/fine/body_top_3dprint_collider_back_3.stl
new file mode 100644
index 00000000..37d62560
--- /dev/null
+++ b/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/fine/body_top_3dprint_collider_back_3.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:4e7c26eb2033477e711d6ea535b955a91f443b74d21a4d8e5ed1bfb72c10c401
+size 245084
diff --git a/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/fine/body_top_3dprint_collider_front_0.stl b/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/fine/body_top_3dprint_collider_front_0.stl
new file mode 100644
index 00000000..f0f3c8da
--- /dev/null
+++ b/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/fine/body_top_3dprint_collider_front_0.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:27dcf3bcfc55a11f5fe3a6a5f01fe253fb5e9cf9d736aa9b83a44d286aeef38c
+size 185384
diff --git a/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/fine/body_top_3dprint_collider_front_1.stl b/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/fine/body_top_3dprint_collider_front_1.stl
new file mode 100644
index 00000000..ce58be8b
--- /dev/null
+++ b/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/fine/body_top_3dprint_collider_front_1.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:89940a91f8e1bf1d1b7c4844bc58a8a03f5c3983cfc174573e8f60f6a47c1109
+size 182484
diff --git a/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/fine/body_top_3dprint_collider_front_2.stl b/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/fine/body_top_3dprint_collider_front_2.stl
new file mode 100644
index 00000000..8964aa93
--- /dev/null
+++ b/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/fine/body_top_3dprint_collider_front_2.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:6ccf48cd480051da28caee941ad1e130f74cebfd2c474d5791d5e014e10fcdfd
+size 219184
diff --git a/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/fine/head_one_3dprint_collider_0.stl b/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/fine/head_one_3dprint_collider_0.stl
new file mode 100644
index 00000000..1d636e5f
--- /dev/null
+++ b/src/reachy_mini/descriptions/reachy_mini/urdf/assets/collision/fine/head_one_3dprint_collider_0.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:5a28add53d3bc49c5b7f6d9e8f8f0884bbfa1ad9fe81af5ca58f01d08a072ed9
+size 1359584
diff --git a/src/reachy_mini/descriptions/reachy_mini/urdf/robot_simple_collision.urdf b/src/reachy_mini/descriptions/reachy_mini/urdf/robot_simple_collision.urdf
new file mode 100644
index 00000000..1326b3e7
--- /dev/null
+++ b/src/reachy_mini/descriptions/reachy_mini/urdf/robot_simple_collision.urdf
@@ -0,0 +1,2399 @@
+<?xml version="1.0" ?>
+<!-- Generated using onshape-to-robot -->
+<!-- Onshape https://cad.onshape.com/documents/7836ed39be931c6ece17e007/w/39d557f31a0ba179c9e7bd5b/e/c82436d68c7d00e142abc581 -->
+<robot name="robot">
+  <!-- Link body_foot_3dprint -->
+  <link name="body_foot_3dprint">
+    <inertial>
+      <origin xyz="0.00290221 0.000112574 0.0132607" rpy="0 0 0"/>
+      <mass value="0.0617881"/>
+      <inertia ixx="6.91521e-05" ixy="-1.98794e-07" ixz="-4.91678e-08" iyy="6.07324e-05" iyz="-3.02441e-08" izz="0.000110345"/>
+    </inertial>
+    <!-- Part body_foot_3dprint -->
+    <visual>
+      <origin xyz="-2.45997e-07 6.93889e-18 0.2298" rpy="6.12323e-17 -2.07076e-30 1.5708"/>
+      <geometry>
+        <mesh filename="package://assets/body_foot_3dprint.stl"/>
+      </geometry>
+      <material name="body_foot_3dprint_material">
+        <color rgba="0.301961 0.301961 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part dc15_a01_horn_dummy -->
+    <visual>
+      <origin xyz="-2.45997e-07 6.93889e-18 0.0413" rpy="-1.5708 6.01506e-30 -1.5708"/>
+      <geometry>
+        <mesh filename="package://assets/dc15_a01_horn_dummy.stl"/>
+      </geometry>
+      <material name="dc15_a01_horn_dummy_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part body_turning_3dprint -->
+    <visual>
+      <origin xyz="-2.45997e-07 6.93889e-18 0.2298" rpy="6.12323e-17 -2.91879e-28 1.5708"/>
+      <geometry>
+        <mesh filename="package://assets/body_turning_3dprint.stl"/>
+      </geometry>
+      <material name="body_turning_3dprint_material">
+        <color rgba="0.301961 0.301961 0.301961 1"/>
+      </material>
+    </visual>
+      </link>
+  <!-- Link body_down_3dprint -->
+  <link name="body_down_3dprint">
+    <inertial>
+      <origin xyz="2.19196e-06 0.000620543 0.0201687" rpy="0 0 0"/>
+      <mass value="0.78209"/>
+      <inertia ixx="0.00233791" ixy="1.10376e-07" ixz="-1.41892e-07" iyy="0.00224458" iyz="-4.46122e-05" izz="0.00254424"/>
+    </inertial>
+    <!-- Part body_down_3dprint -->
+    <visual>
+      <origin xyz="3.79972e-17 -3.70588e-18 0.195" rpy="5.72119e-18 -0 -3.14159"/>
+      <geometry>
+        <mesh filename="package://assets/body_down_3dprint.stl"/>
+      </geometry>
+      <material name="body_down_3dprint_material">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+        <!-- Part stewart_tricap_3dprint -->
+    <visual>
+      <origin xyz="4.80615e-16 -1.03807e-15 0.195" rpy="5.72119e-18 -3.9443e-31 3.14159"/>
+      <geometry>
+        <mesh filename="package://assets/stewart_tricap_3dprint.stl"/>
+      </geometry>
+      <material name="stewart_tricap_3dprint_material">
+        <color rgba="0.301961 0.301961 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part 5w_speaker -->
+    <visual>
+      <origin xyz="8.17568e-16 -0.005 0.0535833" rpy="1.5708 -3.9443e-31 3.14159"/>
+      <geometry>
+        <mesh filename="package://assets/5w_speaker.stl"/>
+      </geometry>
+      <material name="5w_speaker_material">
+        <color rgba="0.196078 0.196078 0.196078 1"/>
+      </material>
+    </visual>
+        <!-- Part bearing_85x110x13 -->
+    <visual>
+      <origin xyz="3.79972e-17 -3.70588e-18 -0.0115" rpy="1.5708 -5.55112e-17 1.5708"/>
+      <geometry>
+        <mesh filename="package://assets/bearing_85x110x13.stl"/>
+      </geometry>
+      <material name="bearing_85x110x13_material">
+        <color rgba="0.768627 0.886275 0.952941 1"/>
+      </material>
+    </visual>
+        <!-- Part body_top_3dprint -->
+    <visual>
+      <origin xyz="6.43558e-16 9.98888e-16 0.195" rpy="5.72119e-18 -0 -3.14159"/>
+      <geometry>
+        <mesh filename="package://assets/body_top_3dprint.stl"/>
+      </geometry>
+      <material name="body_top_3dprint_material">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision name="body_top_front_collision1">
+      <origin xyz="0 -0.055 0.055" rpy="1.57 -0 -3.14159"/>
+      <geometry>
+        <!-- <box size="0.1 0.01 0.1"/> -->
+        <cylinder radius="0.08" length="0.01"/>
+      </geometry>
+    </collision>
+    <collision name="body_top_front_collision2">
+      <origin xyz="0 0.058 0.07" rpy="1.57 -0 -3.14159"/>
+      <geometry>
+        <!-- <box size="0.1 0.01 0.13"/> -->
+        <cylinder radius="0.08" length="0.01"/>
+      </geometry>
+    </collision>
+    <collision name="body_top_front_collision3">
+      <origin xyz="-0.058 0 0.07" rpy="5.72119e-18 -0 -3.14159"/>
+      <geometry>
+        <box size="0.01 0.1 0.05"/>
+      </geometry>
+    </collision>
+    <collision name="body_top_front_collision4">
+      <origin xyz="0.058 0 0.07" rpy="5.72119e-18 -0 -3.14159"/>
+      <geometry>
+        <box size="0.01 0.1 0.05"/>
+      </geometry>
+    </collision>
+    <!-- Placo is not really frinedly with these complex shapes so we use simpler ones -->
+    <!-- <collision name="body_top_front_collision1">
+      <origin xyz="6.43558e-16 9.98888e-16 0.195" rpy="5.72119e-18 -0 -3.14159"/>
+      <geometry>
+      <mesh filename="package://assets/collision/coarse/body_top_3dprint_collider_front_0.stl"/>
+      </geometry>
+    </collision>
+    <collision name="body_top_front_collision2">
+      <origin xyz="6.43558e-16 9.98888e-16 0.195" rpy="5.72119e-18 -0 -3.14159"/>
+      <geometry>
+      <mesh filename="package://assets/collision/coarse/body_top_3dprint_collider_front_1.stl"/>
+      </geometry>
+    </collision>
+    <collision name="body_top_front_collision3">
+      <origin xyz="6.43558e-16 9.98888e-16 0.195" rpy="5.72119e-18 -0 -3.14159"/>
+      <geometry>
+      <mesh filename="package://assets/collision/coarse/body_top_3dprint_collider_front_2.stl"/>
+      </geometry>
+    </collision>
+    <collision name="body_top_back_collision1">
+      <origin xyz="6.43558e-16 9.98888e-16 0.195" rpy="5.72119e-18 -0 -3.14159"/>
+      <geometry>
+        <mesh filename="package://assets/collision/coarse/body_top_3dprint_collider_back_0.stl"/>
+      </geometry>
+    </collision>
+    <collision name="body_top_back_collision2">
+      <origin xyz="6.43558e-16 9.98888e-16 0.195" rpy="5.72119e-18 -0 -3.14159"/>
+      <geometry>
+        <mesh filename="package://assets/collision/coarse/body_top_3dprint_collider_back_1.stl"/>
+      </geometry>
+    </collision>
+    <collision name="body_top_back_collision3">
+      <origin xyz="6.43558e-16 9.98888e-16 0.195" rpy="5.72119e-18 -0 -3.14159"/>
+      <geometry>
+        <mesh filename="package://assets/collision/coarse/body_top_3dprint_collider_back_2.stl"/>
+      </geometry>
+    </collision>
+    <collision name="body_top_back_collision4">
+      <origin xyz="6.43558e-16 9.98888e-16 0.195" rpy="5.72119e-18 -0 -3.14159"/>
+      <geometry>
+        <mesh filename="package://assets/collision/coarse/body_top_3dprint_collider_back_3.stl"/>
+      </geometry>
+    </collision> -->
+        <!-- Part stewart_main_plate_3dprint -->
+    <visual>
+      <origin xyz="-3.07628e-26 -5.29396e-23 0.195" rpy="5.72119e-18 -0 -3.14159"/>
+      <geometry>
+        <mesh filename="package://assets/stewart_main_plate_3dprint.stl"/>
+      </geometry>
+      <material name="stewart_main_plate_3dprint_material">
+        <color rgba="0.301961 0.301961 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part phs_1_7x20_5_dc10_2 -->
+    <visual>
+      <origin xyz="-0.01 -0.0501606 0.0418333" rpy="-3.14159 -1.63452e-15 3.14159"/>
+      <geometry>
+        <mesh filename="package://assets/phs_1_7x20_5_dc10_2.stl"/>
+      </geometry>
+      <material name="phs_1_7x20_5_dc10_2_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part dc15_a01_case_b_dummy -->
+    <visual>
+      <origin xyz="-0.01 -0.0501606 0.0418333" rpy="-3.14159 -1.63452e-15 3.14159"/>
+      <geometry>
+        <mesh filename="package://assets/dc15_a01_case_b_dummy.stl"/>
+      </geometry>
+      <material name="dc15_a01_case_b_dummy_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part dc15_a01_case_m_dummy -->
+    <visual>
+      <origin xyz="-0.01 -0.0501606 0.0418333" rpy="-3.14159 -1.63452e-15 3.14159"/>
+      <geometry>
+        <mesh filename="package://assets/dc15_a01_case_m_dummy.stl"/>
+      </geometry>
+      <material name="dc15_a01_case_m_dummy_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part dc15_a01_led_cap2_dummy -->
+    <visual>
+      <origin xyz="-0.01 -0.0501606 0.0418333" rpy="-3.14159 -1.63452e-15 3.14159"/>
+      <geometry>
+        <mesh filename="package://assets/dc15_a01_led_cap2_dummy.stl"/>
+      </geometry>
+      <material name="dc15_a01_led_cap2_dummy_material">
+        <color rgba="0.87451 0.447059 0.317647 1"/>
+      </material>
+    </visual>
+        <!-- Part bts2_m2_6x8 -->
+    <visual>
+      <origin xyz="-0.01 -0.0501606 0.0418333" rpy="-3.14159 -1.63452e-15 3.14159"/>
+      <geometry>
+        <mesh filename="package://assets/bts2_m2_6x8.stl"/>
+      </geometry>
+      <material name="bts2_m2_6x8_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part dc15_a01_case_f_dummy -->
+    <visual>
+      <origin xyz="-0.01 -0.0501606 0.0418333" rpy="-3.14159 -1.63452e-15 3.14159"/>
+      <geometry>
+        <mesh filename="package://assets/dc15_a01_case_f_dummy.stl"/>
+      </geometry>
+      <material name="dc15_a01_case_f_dummy_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part b3b_eh_1 -->
+    <visual>
+      <origin xyz="-0.01 -0.0501606 0.0418333" rpy="-3.14159 -1.63452e-15 3.14159"/>
+      <geometry>
+        <mesh filename="package://assets/b3b_eh_1.stl"/>
+      </geometry>
+      <material name="b3b_eh_1_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part b3b_eh -->
+    <visual>
+      <origin xyz="-0.01 -0.0501606 0.0418333" rpy="-3.14159 -1.63452e-15 3.14159"/>
+      <geometry>
+        <mesh filename="package://assets/b3b_eh.stl"/>
+      </geometry>
+      <material name="b3b_eh_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part phs_1_7x20_5_dc10_3 -->
+    <visual>
+      <origin xyz="-0.01 -0.0501606 0.0418333" rpy="-3.14159 -1.63452e-15 3.14159"/>
+      <geometry>
+        <mesh filename="package://assets/phs_1_7x20_5_dc10_3.stl"/>
+      </geometry>
+      <material name="phs_1_7x20_5_dc10_3_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part phs_1_7x20_5_dc10_1 -->
+    <visual>
+      <origin xyz="-0.01 -0.0501606 0.0418333" rpy="-3.14159 -1.63452e-15 3.14159"/>
+      <geometry>
+        <mesh filename="package://assets/phs_1_7x20_5_dc10_1.stl"/>
+      </geometry>
+      <material name="phs_1_7x20_5_dc10_1_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part phs_1_7x20_5_dc10 -->
+    <visual>
+      <origin xyz="-0.01 -0.0501606 0.0418333" rpy="-3.14159 -1.63452e-15 3.14159"/>
+      <geometry>
+        <mesh filename="package://assets/phs_1_7x20_5_dc10.stl"/>
+      </geometry>
+      <material name="phs_1_7x20_5_dc10_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part phs_1_7x20_5_dc10_2_2 -->
+    <visual>
+      <origin xyz="-0.0384404 0.0337406 0.0418333" rpy="-3.14159 -1.68259e-15 1.0472"/>
+      <geometry>
+        <mesh filename="package://assets/phs_1_7x20_5_dc10_2.stl"/>
+      </geometry>
+      <material name="phs_1_7x20_5_dc10_2_2_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part dc15_a01_case_b_dummy_2 -->
+    <visual>
+      <origin xyz="-0.0384404 0.0337406 0.0418333" rpy="-3.14159 -1.68259e-15 1.0472"/>
+      <geometry>
+        <mesh filename="package://assets/dc15_a01_case_b_dummy.stl"/>
+      </geometry>
+      <material name="dc15_a01_case_b_dummy_2_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part dc15_a01_case_m_dummy_2 -->
+    <visual>
+      <origin xyz="-0.0384404 0.0337406 0.0418333" rpy="-3.14159 -1.68259e-15 1.0472"/>
+      <geometry>
+        <mesh filename="package://assets/dc15_a01_case_m_dummy.stl"/>
+      </geometry>
+      <material name="dc15_a01_case_m_dummy_2_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part dc15_a01_led_cap2_dummy_2 -->
+    <visual>
+      <origin xyz="-0.0384404 0.0337406 0.0418333" rpy="-3.14159 -1.68259e-15 1.0472"/>
+      <geometry>
+        <mesh filename="package://assets/dc15_a01_led_cap2_dummy.stl"/>
+      </geometry>
+      <material name="dc15_a01_led_cap2_dummy_2_material">
+        <color rgba="0.87451 0.447059 0.317647 1"/>
+      </material>
+    </visual>
+        <!-- Part bts2_m2_6x8_2 -->
+    <visual>
+      <origin xyz="-0.0384404 0.0337406 0.0418333" rpy="-3.14159 -1.68259e-15 1.0472"/>
+      <geometry>
+        <mesh filename="package://assets/bts2_m2_6x8.stl"/>
+      </geometry>
+      <material name="bts2_m2_6x8_2_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part dc15_a01_case_f_dummy_2 -->
+    <visual>
+      <origin xyz="-0.0384404 0.0337406 0.0418333" rpy="-3.14159 -1.68259e-15 1.0472"/>
+      <geometry>
+        <mesh filename="package://assets/dc15_a01_case_f_dummy.stl"/>
+      </geometry>
+      <material name="dc15_a01_case_f_dummy_2_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part b3b_eh_1_2 -->
+    <visual>
+      <origin xyz="-0.0384404 0.0337406 0.0418333" rpy="-3.14159 -1.68259e-15 1.0472"/>
+      <geometry>
+        <mesh filename="package://assets/b3b_eh_1.stl"/>
+      </geometry>
+      <material name="b3b_eh_1_2_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part b3b_eh_2 -->
+    <visual>
+      <origin xyz="-0.0384404 0.0337406 0.0418333" rpy="-3.14159 -1.68259e-15 1.0472"/>
+      <geometry>
+        <mesh filename="package://assets/b3b_eh.stl"/>
+      </geometry>
+      <material name="b3b_eh_2_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part phs_1_7x20_5_dc10_3_2 -->
+    <visual>
+      <origin xyz="-0.0384404 0.0337406 0.0418333" rpy="-3.14159 -1.68259e-15 1.0472"/>
+      <geometry>
+        <mesh filename="package://assets/phs_1_7x20_5_dc10_3.stl"/>
+      </geometry>
+      <material name="phs_1_7x20_5_dc10_3_2_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part phs_1_7x20_5_dc10_1_2 -->
+    <visual>
+      <origin xyz="-0.0384404 0.0337406 0.0418333" rpy="-3.14159 -1.68259e-15 1.0472"/>
+      <geometry>
+        <mesh filename="package://assets/phs_1_7x20_5_dc10_1.stl"/>
+      </geometry>
+      <material name="phs_1_7x20_5_dc10_1_2_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part phs_1_7x20_5_dc10_2 -->
+    <visual>
+      <origin xyz="-0.0384404 0.0337406 0.0418333" rpy="-3.14159 -1.68259e-15 1.0472"/>
+      <geometry>
+        <mesh filename="package://assets/phs_1_7x20_5_dc10.stl"/>
+      </geometry>
+      <material name="phs_1_7x20_5_dc10_2_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part phs_1_7x20_5_dc10_2_3 -->
+    <visual>
+      <origin xyz="0 -5.29396e-23 0.0065" rpy="-1.5708 -0 5.65819e-31"/>
+      <geometry>
+        <mesh filename="package://assets/phs_1_7x20_5_dc10_2.stl"/>
+      </geometry>
+      <material name="phs_1_7x20_5_dc10_2_3_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part dc15_a01_case_b_dummy_3 -->
+    <visual>
+      <origin xyz="0 -5.29396e-23 0.0065" rpy="-1.5708 -0 5.65819e-31"/>
+      <geometry>
+        <mesh filename="package://assets/dc15_a01_case_b_dummy.stl"/>
+      </geometry>
+      <material name="dc15_a01_case_b_dummy_3_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part dc15_a01_case_m_dummy_3 -->
+    <visual>
+      <origin xyz="0 -5.29396e-23 0.0065" rpy="-1.5708 -0 5.65819e-31"/>
+      <geometry>
+        <mesh filename="package://assets/dc15_a01_case_m_dummy.stl"/>
+      </geometry>
+      <material name="dc15_a01_case_m_dummy_3_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part dc15_a01_led_cap2_dummy_3 -->
+    <visual>
+      <origin xyz="0 -5.29396e-23 0.0065" rpy="-1.5708 -0 5.65819e-31"/>
+      <geometry>
+        <mesh filename="package://assets/dc15_a01_led_cap2_dummy.stl"/>
+      </geometry>
+      <material name="dc15_a01_led_cap2_dummy_3_material">
+        <color rgba="0.87451 0.447059 0.317647 1"/>
+      </material>
+    </visual>
+        <!-- Part bts2_m2_6x8_3 -->
+    <visual>
+      <origin xyz="0 -5.29396e-23 0.0065" rpy="-1.5708 -0 5.65819e-31"/>
+      <geometry>
+        <mesh filename="package://assets/bts2_m2_6x8.stl"/>
+      </geometry>
+      <material name="bts2_m2_6x8_3_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part dc15_a01_case_f_dummy_3 -->
+    <visual>
+      <origin xyz="0 -5.29396e-23 0.0065" rpy="-1.5708 -0 5.65819e-31"/>
+      <geometry>
+        <mesh filename="package://assets/dc15_a01_case_f_dummy.stl"/>
+      </geometry>
+      <material name="dc15_a01_case_f_dummy_3_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part b3b_eh_1_3 -->
+    <visual>
+      <origin xyz="0 -5.29396e-23 0.0065" rpy="-1.5708 -0 5.65819e-31"/>
+      <geometry>
+        <mesh filename="package://assets/b3b_eh_1.stl"/>
+      </geometry>
+      <material name="b3b_eh_1_3_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part b3b_eh_3 -->
+    <visual>
+      <origin xyz="0 -5.29396e-23 0.0065" rpy="-1.5708 -0 5.65819e-31"/>
+      <geometry>
+        <mesh filename="package://assets/b3b_eh.stl"/>
+      </geometry>
+      <material name="b3b_eh_3_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part phs_1_7x20_5_dc10_3_3 -->
+    <visual>
+      <origin xyz="0 -5.29396e-23 0.0065" rpy="-1.5708 -0 5.65819e-31"/>
+      <geometry>
+        <mesh filename="package://assets/phs_1_7x20_5_dc10_3.stl"/>
+      </geometry>
+      <material name="phs_1_7x20_5_dc10_3_3_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part phs_1_7x20_5_dc10_1_3 -->
+    <visual>
+      <origin xyz="0 -5.29396e-23 0.0065" rpy="-1.5708 -0 5.65819e-31"/>
+      <geometry>
+        <mesh filename="package://assets/phs_1_7x20_5_dc10_1.stl"/>
+      </geometry>
+      <material name="phs_1_7x20_5_dc10_1_3_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part phs_1_7x20_5_dc10_3 -->
+    <visual>
+      <origin xyz="0 -5.29396e-23 0.0065" rpy="-1.5708 -0 5.65819e-31"/>
+      <geometry>
+        <mesh filename="package://assets/phs_1_7x20_5_dc10.stl"/>
+      </geometry>
+      <material name="phs_1_7x20_5_dc10_3_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part phs_1_7x20_5_dc10_2_4 -->
+    <visual>
+      <origin xyz="0.0384404 0.0337406 0.0418333" rpy="-3.14159 1.68259e-15 -1.0472"/>
+      <geometry>
+        <mesh filename="package://assets/phs_1_7x20_5_dc10_2.stl"/>
+      </geometry>
+      <material name="phs_1_7x20_5_dc10_2_4_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part dc15_a01_case_b_dummy_4 -->
+    <visual>
+      <origin xyz="0.0384404 0.0337406 0.0418333" rpy="-3.14159 1.68259e-15 -1.0472"/>
+      <geometry>
+        <mesh filename="package://assets/dc15_a01_case_b_dummy.stl"/>
+      </geometry>
+      <material name="dc15_a01_case_b_dummy_4_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part dc15_a01_case_m_dummy_4 -->
+    <visual>
+      <origin xyz="0.0384404 0.0337406 0.0418333" rpy="-3.14159 1.68259e-15 -1.0472"/>
+      <geometry>
+        <mesh filename="package://assets/dc15_a01_case_m_dummy.stl"/>
+      </geometry>
+      <material name="dc15_a01_case_m_dummy_4_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part dc15_a01_led_cap2_dummy_4 -->
+    <visual>
+      <origin xyz="0.0384404 0.0337406 0.0418333" rpy="-3.14159 1.68259e-15 -1.0472"/>
+      <geometry>
+        <mesh filename="package://assets/dc15_a01_led_cap2_dummy.stl"/>
+      </geometry>
+      <material name="dc15_a01_led_cap2_dummy_4_material">
+        <color rgba="0.87451 0.447059 0.317647 1"/>
+      </material>
+    </visual>
+        <!-- Part bts2_m2_6x8_4 -->
+    <visual>
+      <origin xyz="0.0384404 0.0337406 0.0418333" rpy="-3.14159 1.68259e-15 -1.0472"/>
+      <geometry>
+        <mesh filename="package://assets/bts2_m2_6x8.stl"/>
+      </geometry>
+      <material name="bts2_m2_6x8_4_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part dc15_a01_case_f_dummy_4 -->
+    <visual>
+      <origin xyz="0.0384404 0.0337406 0.0418333" rpy="-3.14159 1.68259e-15 -1.0472"/>
+      <geometry>
+        <mesh filename="package://assets/dc15_a01_case_f_dummy.stl"/>
+      </geometry>
+      <material name="dc15_a01_case_f_dummy_4_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part b3b_eh_1_4 -->
+    <visual>
+      <origin xyz="0.0384404 0.0337406 0.0418333" rpy="-3.14159 1.68259e-15 -1.0472"/>
+      <geometry>
+        <mesh filename="package://assets/b3b_eh_1.stl"/>
+      </geometry>
+      <material name="b3b_eh_1_4_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part b3b_eh_4 -->
+    <visual>
+      <origin xyz="0.0384404 0.0337406 0.0418333" rpy="-3.14159 1.68259e-15 -1.0472"/>
+      <geometry>
+        <mesh filename="package://assets/b3b_eh.stl"/>
+      </geometry>
+      <material name="b3b_eh_4_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part phs_1_7x20_5_dc10_3_4 -->
+    <visual>
+      <origin xyz="0.0384404 0.0337406 0.0418333" rpy="-3.14159 1.68259e-15 -1.0472"/>
+      <geometry>
+        <mesh filename="package://assets/phs_1_7x20_5_dc10_3.stl"/>
+      </geometry>
+      <material name="phs_1_7x20_5_dc10_3_4_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part phs_1_7x20_5_dc10_1_4 -->
+    <visual>
+      <origin xyz="0.0384404 0.0337406 0.0418333" rpy="-3.14159 1.68259e-15 -1.0472"/>
+      <geometry>
+        <mesh filename="package://assets/phs_1_7x20_5_dc10_1.stl"/>
+      </geometry>
+      <material name="phs_1_7x20_5_dc10_1_4_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part phs_1_7x20_5_dc10_4 -->
+    <visual>
+      <origin xyz="0.0384404 0.0337406 0.0418333" rpy="-3.14159 1.68259e-15 -1.0472"/>
+      <geometry>
+        <mesh filename="package://assets/phs_1_7x20_5_dc10.stl"/>
+      </geometry>
+      <material name="phs_1_7x20_5_dc10_4_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part phs_1_7x20_5_dc10_2_5 -->
+    <visual>
+      <origin xyz="0.01 -0.0501606 0.0418333" rpy="-3.14159 1.63452e-15 -3.14159"/>
+      <geometry>
+        <mesh filename="package://assets/phs_1_7x20_5_dc10_2.stl"/>
+      </geometry>
+      <material name="phs_1_7x20_5_dc10_2_5_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part dc15_a01_case_b_dummy_5 -->
+    <visual>
+      <origin xyz="0.01 -0.0501606 0.0418333" rpy="-3.14159 1.63452e-15 -3.14159"/>
+      <geometry>
+        <mesh filename="package://assets/dc15_a01_case_b_dummy.stl"/>
+      </geometry>
+      <material name="dc15_a01_case_b_dummy_5_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part dc15_a01_case_m_dummy_5 -->
+    <visual>
+      <origin xyz="0.01 -0.0501606 0.0418333" rpy="-3.14159 1.63452e-15 -3.14159"/>
+      <geometry>
+        <mesh filename="package://assets/dc15_a01_case_m_dummy.stl"/>
+      </geometry>
+      <material name="dc15_a01_case_m_dummy_5_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part dc15_a01_led_cap2_dummy_5 -->
+    <visual>
+      <origin xyz="0.01 -0.0501606 0.0418333" rpy="-3.14159 1.63452e-15 -3.14159"/>
+      <geometry>
+        <mesh filename="package://assets/dc15_a01_led_cap2_dummy.stl"/>
+      </geometry>
+      <material name="dc15_a01_led_cap2_dummy_5_material">
+        <color rgba="0.87451 0.447059 0.317647 1"/>
+      </material>
+    </visual>
+        <!-- Part bts2_m2_6x8_5 -->
+    <visual>
+      <origin xyz="0.01 -0.0501606 0.0418333" rpy="-3.14159 1.63452e-15 -3.14159"/>
+      <geometry>
+        <mesh filename="package://assets/bts2_m2_6x8.stl"/>
+      </geometry>
+      <material name="bts2_m2_6x8_5_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part dc15_a01_case_f_dummy_5 -->
+    <visual>
+      <origin xyz="0.01 -0.0501606 0.0418333" rpy="-3.14159 1.63452e-15 -3.14159"/>
+      <geometry>
+        <mesh filename="package://assets/dc15_a01_case_f_dummy.stl"/>
+      </geometry>
+      <material name="dc15_a01_case_f_dummy_5_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part b3b_eh_1_5 -->
+    <visual>
+      <origin xyz="0.01 -0.0501606 0.0418333" rpy="-3.14159 1.63452e-15 -3.14159"/>
+      <geometry>
+        <mesh filename="package://assets/b3b_eh_1.stl"/>
+      </geometry>
+      <material name="b3b_eh_1_5_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part b3b_eh_5 -->
+    <visual>
+      <origin xyz="0.01 -0.0501606 0.0418333" rpy="-3.14159 1.63452e-15 -3.14159"/>
+      <geometry>
+        <mesh filename="package://assets/b3b_eh.stl"/>
+      </geometry>
+      <material name="b3b_eh_5_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part phs_1_7x20_5_dc10_3_5 -->
+    <visual>
+      <origin xyz="0.01 -0.0501606 0.0418333" rpy="-3.14159 1.63452e-15 -3.14159"/>
+      <geometry>
+        <mesh filename="package://assets/phs_1_7x20_5_dc10_3.stl"/>
+      </geometry>
+      <material name="phs_1_7x20_5_dc10_3_5_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part phs_1_7x20_5_dc10_1_5 -->
+    <visual>
+      <origin xyz="0.01 -0.0501606 0.0418333" rpy="-3.14159 1.63452e-15 -3.14159"/>
+      <geometry>
+        <mesh filename="package://assets/phs_1_7x20_5_dc10_1.stl"/>
+      </geometry>
+      <material name="phs_1_7x20_5_dc10_1_5_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part phs_1_7x20_5_dc10_5 -->
+    <visual>
+      <origin xyz="0.01 -0.0501606 0.0418333" rpy="-3.14159 1.63452e-15 -3.14159"/>
+      <geometry>
+        <mesh filename="package://assets/phs_1_7x20_5_dc10.stl"/>
+      </geometry>
+      <material name="phs_1_7x20_5_dc10_5_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part phs_1_7x20_5_dc10_2_6 -->
+    <visual>
+      <origin xyz="-0.0484404 0.01642 0.0418333" rpy="-3.14159 1.58644e-15 1.0472"/>
+      <geometry>
+        <mesh filename="package://assets/phs_1_7x20_5_dc10_2.stl"/>
+      </geometry>
+      <material name="phs_1_7x20_5_dc10_2_6_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part dc15_a01_case_b_dummy_6 -->
+    <visual>
+      <origin xyz="-0.0484404 0.01642 0.0418333" rpy="-3.14159 1.58644e-15 1.0472"/>
+      <geometry>
+        <mesh filename="package://assets/dc15_a01_case_b_dummy.stl"/>
+      </geometry>
+      <material name="dc15_a01_case_b_dummy_6_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part dc15_a01_case_m_dummy_6 -->
+    <visual>
+      <origin xyz="-0.0484404 0.01642 0.0418333" rpy="-3.14159 1.58644e-15 1.0472"/>
+      <geometry>
+        <mesh filename="package://assets/dc15_a01_case_m_dummy.stl"/>
+      </geometry>
+      <material name="dc15_a01_case_m_dummy_6_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part dc15_a01_led_cap2_dummy_6 -->
+    <visual>
+      <origin xyz="-0.0484404 0.01642 0.0418333" rpy="-3.14159 1.58644e-15 1.0472"/>
+      <geometry>
+        <mesh filename="package://assets/dc15_a01_led_cap2_dummy.stl"/>
+      </geometry>
+      <material name="dc15_a01_led_cap2_dummy_6_material">
+        <color rgba="0.87451 0.447059 0.317647 1"/>
+      </material>
+    </visual>
+        <!-- Part bts2_m2_6x8_6 -->
+    <visual>
+      <origin xyz="-0.0484404 0.01642 0.0418333" rpy="-3.14159 1.58644e-15 1.0472"/>
+      <geometry>
+        <mesh filename="package://assets/bts2_m2_6x8.stl"/>
+      </geometry>
+      <material name="bts2_m2_6x8_6_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part dc15_a01_case_f_dummy_6 -->
+    <visual>
+      <origin xyz="-0.0484404 0.01642 0.0418333" rpy="-3.14159 1.58644e-15 1.0472"/>
+      <geometry>
+        <mesh filename="package://assets/dc15_a01_case_f_dummy.stl"/>
+      </geometry>
+      <material name="dc15_a01_case_f_dummy_6_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part b3b_eh_1_6 -->
+    <visual>
+      <origin xyz="-0.0484404 0.01642 0.0418333" rpy="-3.14159 1.58644e-15 1.0472"/>
+      <geometry>
+        <mesh filename="package://assets/b3b_eh_1.stl"/>
+      </geometry>
+      <material name="b3b_eh_1_6_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part b3b_eh_6 -->
+    <visual>
+      <origin xyz="-0.0484404 0.01642 0.0418333" rpy="-3.14159 1.58644e-15 1.0472"/>
+      <geometry>
+        <mesh filename="package://assets/b3b_eh.stl"/>
+      </geometry>
+      <material name="b3b_eh_6_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part phs_1_7x20_5_dc10_3_6 -->
+    <visual>
+      <origin xyz="-0.0484404 0.01642 0.0418333" rpy="-3.14159 1.58644e-15 1.0472"/>
+      <geometry>
+        <mesh filename="package://assets/phs_1_7x20_5_dc10_3.stl"/>
+      </geometry>
+      <material name="phs_1_7x20_5_dc10_3_6_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part phs_1_7x20_5_dc10_1_6 -->
+    <visual>
+      <origin xyz="-0.0484404 0.01642 0.0418333" rpy="-3.14159 1.58644e-15 1.0472"/>
+      <geometry>
+        <mesh filename="package://assets/phs_1_7x20_5_dc10_1.stl"/>
+      </geometry>
+      <material name="phs_1_7x20_5_dc10_1_6_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part phs_1_7x20_5_dc10_6 -->
+    <visual>
+      <origin xyz="-0.0484404 0.01642 0.0418333" rpy="-3.14159 1.58644e-15 1.0472"/>
+      <geometry>
+        <mesh filename="package://assets/phs_1_7x20_5_dc10.stl"/>
+      </geometry>
+      <material name="phs_1_7x20_5_dc10_6_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part phs_1_7x20_5_dc10_2_7 -->
+    <visual>
+      <origin xyz="0.0484404 0.01642 0.0418333" rpy="-3.14159 -1.58644e-15 -1.0472"/>
+      <geometry>
+        <mesh filename="package://assets/phs_1_7x20_5_dc10_2.stl"/>
+      </geometry>
+      <material name="phs_1_7x20_5_dc10_2_7_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part dc15_a01_case_b_dummy_7 -->
+    <visual>
+      <origin xyz="0.0484404 0.01642 0.0418333" rpy="-3.14159 -1.58644e-15 -1.0472"/>
+      <geometry>
+        <mesh filename="package://assets/dc15_a01_case_b_dummy.stl"/>
+      </geometry>
+      <material name="dc15_a01_case_b_dummy_7_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part dc15_a01_case_m_dummy_7 -->
+    <visual>
+      <origin xyz="0.0484404 0.01642 0.0418333" rpy="-3.14159 -1.58644e-15 -1.0472"/>
+      <geometry>
+        <mesh filename="package://assets/dc15_a01_case_m_dummy.stl"/>
+      </geometry>
+      <material name="dc15_a01_case_m_dummy_7_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part dc15_a01_led_cap2_dummy_7 -->
+    <visual>
+      <origin xyz="0.0484404 0.01642 0.0418333" rpy="-3.14159 -1.58644e-15 -1.0472"/>
+      <geometry>
+        <mesh filename="package://assets/dc15_a01_led_cap2_dummy.stl"/>
+      </geometry>
+      <material name="dc15_a01_led_cap2_dummy_7_material">
+        <color rgba="0.87451 0.447059 0.317647 1"/>
+      </material>
+    </visual>
+        <!-- Part bts2_m2_6x8_7 -->
+    <visual>
+      <origin xyz="0.0484404 0.01642 0.0418333" rpy="-3.14159 -1.58644e-15 -1.0472"/>
+      <geometry>
+        <mesh filename="package://assets/bts2_m2_6x8.stl"/>
+      </geometry>
+      <material name="bts2_m2_6x8_7_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part dc15_a01_case_f_dummy_7 -->
+    <visual>
+      <origin xyz="0.0484404 0.01642 0.0418333" rpy="-3.14159 -1.58644e-15 -1.0472"/>
+      <geometry>
+        <mesh filename="package://assets/dc15_a01_case_f_dummy.stl"/>
+      </geometry>
+      <material name="dc15_a01_case_f_dummy_7_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part b3b_eh_1_7 -->
+    <visual>
+      <origin xyz="0.0484404 0.01642 0.0418333" rpy="-3.14159 -1.58644e-15 -1.0472"/>
+      <geometry>
+        <mesh filename="package://assets/b3b_eh_1.stl"/>
+      </geometry>
+      <material name="b3b_eh_1_7_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part b3b_eh_7 -->
+    <visual>
+      <origin xyz="0.0484404 0.01642 0.0418333" rpy="-3.14159 -1.58644e-15 -1.0472"/>
+      <geometry>
+        <mesh filename="package://assets/b3b_eh.stl"/>
+      </geometry>
+      <material name="b3b_eh_7_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part phs_1_7x20_5_dc10_3_7 -->
+    <visual>
+      <origin xyz="0.0484404 0.01642 0.0418333" rpy="-3.14159 -1.58644e-15 -1.0472"/>
+      <geometry>
+        <mesh filename="package://assets/phs_1_7x20_5_dc10_3.stl"/>
+      </geometry>
+      <material name="phs_1_7x20_5_dc10_3_7_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part phs_1_7x20_5_dc10_1_7 -->
+    <visual>
+      <origin xyz="0.0484404 0.01642 0.0418333" rpy="-3.14159 -1.58644e-15 -1.0472"/>
+      <geometry>
+        <mesh filename="package://assets/phs_1_7x20_5_dc10_1.stl"/>
+      </geometry>
+      <material name="phs_1_7x20_5_dc10_1_7_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part phs_1_7x20_5_dc10_7 -->
+    <visual>
+      <origin xyz="0.0484404 0.01642 0.0418333" rpy="-3.14159 -1.58644e-15 -1.0472"/>
+      <geometry>
+        <mesh filename="package://assets/phs_1_7x20_5_dc10.stl"/>
+      </geometry>
+      <material name="phs_1_7x20_5_dc10_7_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+      </link>
+  <!-- Link dc15_a01_horn_dummy -->
+  <link name="dc15_a01_horn_dummy">
+    <inertial>
+      <origin xyz="0.0237505 1.13786e-09 0.00335838" rpy="0 0 0"/>
+      <mass value="0.00366675"/>
+      <inertia ixx="8.57063e-08" ixy="1.81822e-13" ixz="-1.93125e-07" iyy="1.28019e-06" iyz="1.09002e-14" izz="1.26546e-06"/>
+    </inertial>
+    <!-- Part dc15_a01_horn_dummy_2 -->
+    <visual>
+      <origin xyz="3.46945e-18 0 -0.0065" rpy="1.5708 -2.09044e-30 -1.5708"/>
+      <geometry>
+        <mesh filename="package://assets/dc15_a01_horn_dummy.stl"/>
+      </geometry>
+      <material name="dc15_a01_horn_dummy_2_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part mp01062_stewart_arm_3 -->
+    <visual>
+      <origin xyz="-0.01 -0.153167 0.0436606" rpy="1.5708 -1.0472 6.12323e-17"/>
+      <geometry>
+        <mesh filename="package://assets/mp01062_stewart_arm_3.stl"/>
+      </geometry>
+      <material name="mp01062_stewart_arm_3_material">
+        <color rgba="0.301961 0.301961 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part stewart_link_ball -->
+    <visual>
+      <origin xyz="0.0825 -1.6545e-10 0.007" rpy="4.32628e-14 -5.84082e-17 6.12323e-17"/>
+      <geometry>
+        <mesh filename="package://assets/stewart_link_ball.stl"/>
+      </geometry>
+      <material name="stewart_link_ball_material">
+        <color rgba="0.439216 0.47451 0.501961 1"/>
+      </material>
+    </visual>
+      </link>
+  <!-- Link passive_1_link_x -->
+  <link name="passive_1_link_x">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="1e-09"/>
+      <inertia ixx="1e-09" ixy="0" ixz="0" iyy="1e-09" iyz="0" izz="1e-09"/>
+    </inertial>
+  </link>
+  <!-- Link passive_1_link_y -->
+  <link name="passive_1_link_y">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="1e-09"/>
+      <inertia ixx="1e-09" ixy="0" ixz="0" iyy="1e-09" iyz="0" izz="1e-09"/>
+    </inertial>
+  </link>
+  <!-- Link stewart_link_rod -->
+  <link name="stewart_link_rod">
+    <inertial>
+      <origin xyz="0.0425001 8.43768e-11 -1.88428e-09" rpy="0 0 0"/>
+      <mass value="0.00503424"/>
+      <inertia ixx="2.63976e-08" ixy="-3.69517e-13" ixz="7.12534e-14" iyy="3.65211e-06" iyz="5.21041e-14" izz="3.66473e-06"/>
+    </inertial>
+    <!-- Part stewart_link_rod -->
+    <visual>
+      <origin xyz="0.0425 0 6.93889e-18" rpy="-4.80812e-17 3.69195e-17 -3.11194e-17"/>
+      <geometry>
+        <mesh filename="package://assets/stewart_link_rod.stl"/>
+      </geometry>
+      <material name="stewart_link_rod_material">
+        <color rgba="0.811765 0.858824 0.898039 1"/>
+      </material>
+    </visual>
+      </link>
+  <!-- Frame closing_1_1 (dummy link + fixed joint) -->
+  <link name="closing_1_1">
+    <origin xyz="0 0 0" rpy="0 -0 0"/>
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="1e-9"/>
+      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+    </inertial>
+  </link>
+  <joint name="closing_1_1_frame" type="fixed">
+    <origin xyz="0.085 -1.38778e-17 0" rpy="-4.80812e-17 3.69195e-17 -3.11194e-17"/>
+    <parent link="stewart_link_rod"/>
+    <child link="closing_1_1"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+  <!-- Joint from passive_1_link_y to stewart_link_rod -->
+  <joint name="passive_1_z" type="revolute">
+    <origin xyz="6.93889e-18 0 0" rpy="-4.80812e-17 3.69195e-17 -3.11194e-17"/>
+    <parent link="passive_1_link_y"/>
+    <child link="stewart_link_rod"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="1e+08" lower="-3.141592653589793" upper="3.141592653589793"/>
+  </joint>
+  <!-- Joint from passive_1_link_x to passive_1_link_y -->
+  <joint name="passive_1_y" type="revolute">
+    <origin xyz="6.93889e-18 0 0" rpy="-4.80812e-17 3.69195e-17 -3.11194e-17"/>
+    <parent link="passive_1_link_x"/>
+    <child link="passive_1_link_y"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="10" velocity="1e+08" lower="-3.141592653589793" upper="3.141592653589793"/>
+  </joint>
+  <!-- Joint from dc15_a01_horn_dummy to passive_1_link_x -->
+  <joint name="passive_1_x" type="revolute">
+    <origin xyz="0.04 -1.6545e-10 0.007" rpy="-0.13754 -0.0882156 2.10349"/>
+    <parent link="dc15_a01_horn_dummy"/>
+    <child link="passive_1_link_x"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="10" velocity="1e+08" lower="-3.141592653589793" upper="3.141592653589793"/>
+  </joint>
+  <!-- Joint from body_down_3dprint to dc15_a01_horn_dummy -->
+  <joint name="stewart_1" type="revolute">
+    <origin xyz="-0.0328112 0.0304906 0.0418333" rpy="1.5708 5.91241e-14 1.0472"/>
+    <parent link="body_down_3dprint"/>
+    <child link="dc15_a01_horn_dummy"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="8" lower="-0.837758" upper="1.39626"/>
+  </joint>
+  <!-- Link dc15_a01_horn_dummy_2 -->
+  <link name="dc15_a01_horn_dummy_2">
+    <inertial>
+      <origin xyz="0.0237505 1.27591e-09 0.00335838" rpy="0 0 0"/>
+      <mass value="0.00366675"/>
+      <inertia ixx="8.57063e-08" ixy="1.79436e-13" ixz="-1.93125e-07" iyy="1.28019e-06" iyz="1.03818e-14" izz="1.26546e-06"/>
+    </inertial>
+    <!-- Part stewart_link_ball_2 -->
+    <visual>
+      <origin xyz="-0.0025 4.16334e-17 0.007" rpy="4.75464e-14 5.37396e-16 -3.14159"/>
+      <geometry>
+        <mesh filename="package://assets/stewart_link_ball.stl"/>
+      </geometry>
+      <material name="stewart_link_ball_2_material">
+        <color rgba="0.439216 0.47451 0.501961 1"/>
+      </material>
+    </visual>
+        <!-- Part dc15_a01_horn_dummy_3 -->
+    <visual>
+      <origin xyz="-1.73472e-18 0 -0.0065" rpy="1.5708 -3.99138e-30 -1.5708"/>
+      <geometry>
+        <mesh filename="package://assets/dc15_a01_horn_dummy.stl"/>
+      </geometry>
+      <material name="dc15_a01_horn_dummy_3_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part mp01062_stewart_arm_3_2 -->
+    <visual>
+      <origin xyz="-0.01 -0.153167 0.0436606" rpy="1.5708 -1.0472 3.6476e-15"/>
+      <geometry>
+        <mesh filename="package://assets/mp01062_stewart_arm_3.stl"/>
+      </geometry>
+      <material name="mp01062_stewart_arm_3_2_material">
+        <color rgba="0.301961 0.301961 0.301961 1"/>
+      </material>
+    </visual>
+      </link>
+  <!-- Link passive_2_link_x -->
+  <link name="passive_2_link_x">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="1e-09"/>
+      <inertia ixx="1e-09" ixy="0" ixz="0" iyy="1e-09" iyz="0" izz="1e-09"/>
+    </inertial>
+  </link>
+  <!-- Link passive_2_link_y -->
+  <link name="passive_2_link_y">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="1e-09"/>
+      <inertia ixx="1e-09" ixy="0" ixz="0" iyy="1e-09" iyz="0" izz="1e-09"/>
+    </inertial>
+  </link>
+  <!-- Link stewart_link_rod_2 -->
+  <link name="stewart_link_rod_2">
+    <inertial>
+      <origin xyz="0.0214999 -0.0364688 -0.00374429" rpy="0 0 0"/>
+      <mass value="0.00503424"/>
+      <inertia ixx="2.72424e-06" ixy="1.57393e-06" ixz="1.6134e-07" iyy="9.82586e-07" iyz="-2.75528e-07" izz="3.63642e-06"/>
+    </inertial>
+    <!-- Part stewart_link_rod_2 -->
+    <visual>
+      <origin xyz="0.0215 -0.0364689 -0.0037443" rpy="3.06617 -0.0882156 2.10349"/>
+      <geometry>
+        <mesh filename="package://assets/stewart_link_rod.stl"/>
+      </geometry>
+      <material name="stewart_link_rod_2_material">
+        <color rgba="0.811765 0.858824 0.898039 1"/>
+      </material>
+    </visual>
+      </link>
+  <!-- Frame closing_2_1 (dummy link + fixed joint) -->
+  <link name="closing_2_1">
+    <origin xyz="0 0 0" rpy="0 -0 0"/>
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="1e-9"/>
+      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+    </inertial>
+  </link>
+  <joint name="closing_2_1_frame" type="fixed">
+    <origin xyz="0.043 -0.0729378 -0.00748861" rpy="3.06617 -0.0882156 2.10349"/>
+    <parent link="stewart_link_rod_2"/>
+    <child link="closing_2_1"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+  <!-- Joint from passive_2_link_y to stewart_link_rod_2 -->
+  <joint name="passive_2_z" type="revolute">
+    <origin xyz="-6.93889e-18 0 6.93889e-18" rpy="-4.29816e-30 7.32263e-17 -8.42229e-30"/>
+    <parent link="passive_2_link_y"/>
+    <child link="stewart_link_rod_2"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="1e+08" lower="-3.141592653589793" upper="3.141592653589793"/>
+  </joint>
+  <!-- Joint from passive_2_link_x to passive_2_link_y -->
+  <joint name="passive_2_y" type="revolute">
+    <origin xyz="-6.93889e-18 0 6.93889e-18" rpy="-4.29816e-30 7.32263e-17 -8.42229e-30"/>
+    <parent link="passive_2_link_x"/>
+    <child link="passive_2_link_y"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="10" velocity="1e+08" lower="-3.141592653589793" upper="3.141592653589793"/>
+  </joint>
+  <!-- Joint from dc15_a01_horn_dummy_2 to passive_2_link_x -->
+  <joint name="passive_2_x" type="revolute">
+    <origin xyz="0.04 1.249e-16 0.007" rpy="-3.14159 5.37396e-16 -3.14159"/>
+    <parent link="dc15_a01_horn_dummy_2"/>
+    <child link="passive_2_link_x"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="10" velocity="1e+08" lower="-3.141592653589793" upper="3.141592653589793"/>
+  </joint>
+  <!-- Joint from body_down_3dprint to dc15_a01_horn_dummy_2 -->
+  <joint name="stewart_2" type="revolute">
+    <origin xyz="-0.0428112 0.01317 0.0418333" rpy="-1.5708 1.47282e-13 -2.0944"/>
+    <parent link="body_down_3dprint"/>
+    <child link="dc15_a01_horn_dummy_2"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="8" lower="-1.39626" upper="1.22173"/>
+  </joint>
+  <!-- Link dc15_a01_horn_dummy_3 -->
+  <link name="dc15_a01_horn_dummy_3">
+    <inertial>
+      <origin xyz="0.0237505 1.27591e-09 0.00335838" rpy="0 0 0"/>
+      <mass value="0.00366675"/>
+      <inertia ixx="8.57063e-08" ixy="1.79436e-13" ixz="-1.93125e-07" iyy="1.28019e-06" iyz="1.03818e-14" izz="1.26546e-06"/>
+    </inertial>
+    <!-- Part dc15_a01_horn_dummy_4 -->
+    <visual>
+      <origin xyz="-1.73472e-18 1.38778e-17 -0.0065" rpy="1.5708 1.89883e-30 -1.5708"/>
+      <geometry>
+        <mesh filename="package://assets/dc15_a01_horn_dummy.stl"/>
+      </geometry>
+      <material name="dc15_a01_horn_dummy_4_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part stewart_link_ball_3 -->
+    <visual>
+      <origin xyz="0.04 -0.0425 0.007" rpy="-2.8701e-15 -2.95696e-14 -1.5708"/>
+      <geometry>
+        <mesh filename="package://assets/stewart_link_ball.stl"/>
+      </geometry>
+      <material name="stewart_link_ball_3_material">
+        <color rgba="0.439216 0.47451 0.501961 1"/>
+      </material>
+    </visual>
+        <!-- Part mp01062_stewart_arm_3_3 -->
+    <visual>
+      <origin xyz="-0.01 -0.153167 0.0436606" rpy="1.5708 -1.0472 6.12323e-17"/>
+      <geometry>
+        <mesh filename="package://assets/mp01062_stewart_arm_3.stl"/>
+      </geometry>
+      <material name="mp01062_stewart_arm_3_3_material">
+        <color rgba="0.301961 0.301961 0.301961 1"/>
+      </material>
+    </visual>
+      </link>
+  <!-- Link passive_3_link_x -->
+  <link name="passive_3_link_x">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="1e-09"/>
+      <inertia ixx="1e-09" ixy="0" ixz="0" iyy="1e-09" iyz="0" izz="1e-09"/>
+    </inertial>
+  </link>
+  <!-- Link passive_3_link_y -->
+  <link name="passive_3_link_y">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="1e-09"/>
+      <inertia ixx="1e-09" ixy="0" ixz="0" iyy="1e-09" iyz="0" izz="1e-09"/>
+    </inertial>
+  </link>
+  <!-- Link stewart_link_rod_3 -->
+  <link name="stewart_link_rod_3">
+    <inertial>
+      <origin xyz="-0.0424999 8.43768e-11 -1.88428e-09" rpy="0 0 0"/>
+      <mass value="0.00503424"/>
+      <inertia ixx="2.63976e-08" ixy="-3.69517e-13" ixz="7.12534e-14" iyy="3.65211e-06" iyz="5.21041e-14" izz="3.66473e-06"/>
+    </inertial>
+    <!-- Part stewart_link_rod_3 -->
+    <visual>
+      <origin xyz="-0.0425 -1.38778e-17 0" rpy="-4.71809e-17 -7.61919e-18 5.54539e-17"/>
+      <geometry>
+        <mesh filename="package://assets/stewart_link_rod.stl"/>
+      </geometry>
+      <material name="stewart_link_rod_3_material">
+        <color rgba="0.811765 0.858824 0.898039 1"/>
+      </material>
+    </visual>
+      </link>
+  <!-- Frame closing_3_1 (dummy link + fixed joint) -->
+  <link name="closing_3_1">
+    <origin xyz="0 0 0" rpy="0 -0 0"/>
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="1e-9"/>
+      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+    </inertial>
+  </link>
+  <joint name="closing_3_1_frame" type="fixed">
+    <origin xyz="-0.085 -1.38778e-17 0" rpy="-4.71809e-17 -7.61919e-18 5.54539e-17"/>
+    <parent link="stewart_link_rod_3"/>
+    <child link="closing_3_1"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+  <!-- Joint from passive_3_link_y to stewart_link_rod_3 -->
+  <joint name="passive_3_z" type="revolute">
+    <origin xyz="-6.93889e-18 0 -1.38778e-17" rpy="-4.71809e-17 -7.61919e-18 5.54539e-17"/>
+    <parent link="passive_3_link_y"/>
+    <child link="stewart_link_rod_3"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="1e+08" lower="-3.141592653589793" upper="3.141592653589793"/>
+  </joint>
+  <!-- Joint from passive_3_link_x to passive_3_link_y -->
+  <joint name="passive_3_y" type="revolute">
+    <origin xyz="-6.93889e-18 0 -1.38778e-17" rpy="-4.71809e-17 -7.61919e-18 5.54539e-17"/>
+    <parent link="passive_3_link_x"/>
+    <child link="passive_3_link_y"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="10" velocity="1e+08" lower="-3.141592653589793" upper="3.141592653589793"/>
+  </joint>
+  <!-- Joint from dc15_a01_horn_dummy_3 to passive_3_link_x -->
+  <joint name="passive_3_x" type="revolute">
+    <origin xyz="0.04 8.32667e-17 0.007" rpy="0.373569 0.0882156 -1.0381"/>
+    <parent link="dc15_a01_horn_dummy_3"/>
+    <child link="passive_3_link_x"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="10" velocity="1e+08" lower="-3.141592653589793" upper="3.141592653589793"/>
+  </joint>
+  <!-- Joint from body_down_3dprint to dc15_a01_horn_dummy_3 -->
+  <joint name="stewart_3" type="revolute">
+    <origin xyz="-0.01 -0.0436606 0.0418333" rpy="1.5708 5.72146e-14 3.14159"/>
+    <parent link="body_down_3dprint"/>
+    <child link="dc15_a01_horn_dummy_3"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="8" lower="-0.837758" upper="1.39626"/>
+  </joint>
+  <!-- Link dc15_a01_horn_dummy_4 -->
+  <link name="dc15_a01_horn_dummy_4">
+    <inertial>
+      <origin xyz="0.0237505 1.27591e-09 0.00335838" rpy="0 0 0"/>
+      <mass value="0.00366675"/>
+      <inertia ixx="8.57063e-08" ixy="1.79436e-13" ixz="-1.93125e-07" iyy="1.28019e-06" iyz="1.03818e-14" izz="1.26546e-06"/>
+    </inertial>
+    <!-- Part stewart_link_ball_4 -->
+    <visual>
+      <origin xyz="0.04 0.0425 0.007" rpy="-2.36664e-29 2.12546e-15 1.5708"/>
+      <geometry>
+        <mesh filename="package://assets/stewart_link_ball.stl"/>
+      </geometry>
+      <material name="stewart_link_ball_4_material">
+        <color rgba="0.439216 0.47451 0.501961 1"/>
+      </material>
+    </visual>
+        <!-- Part mp01062_stewart_arm_3_4 -->
+    <visual>
+      <origin xyz="-0.01 -0.153167 0.0436606" rpy="1.5708 -1.0472 6.12323e-17"/>
+      <geometry>
+        <mesh filename="package://assets/mp01062_stewart_arm_3.stl"/>
+      </geometry>
+      <material name="mp01062_stewart_arm_3_4_material">
+        <color rgba="0.301961 0.301961 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part dc15_a01_horn_dummy_5 -->
+    <visual>
+      <origin xyz="-1.73472e-18 -1.38778e-17 -0.0065" rpy="1.5708 1.27364e-31 -1.5708"/>
+      <geometry>
+        <mesh filename="package://assets/dc15_a01_horn_dummy.stl"/>
+      </geometry>
+      <material name="dc15_a01_horn_dummy_5_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+      </link>
+  <!-- Link passive_4_link_x -->
+  <link name="passive_4_link_x">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="1e-09"/>
+      <inertia ixx="1e-09" ixy="0" ixz="0" iyy="1e-09" iyz="0" izz="1e-09"/>
+    </inertial>
+  </link>
+  <!-- Link passive_4_link_y -->
+  <link name="passive_4_link_y">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="1e-09"/>
+      <inertia ixx="1e-09" ixy="0" ixz="0" iyy="1e-09" iyz="0" izz="1e-09"/>
+    </inertial>
+  </link>
+  <!-- Link stewart_link_rod_4 -->
+  <link name="stewart_link_rod_4">
+    <inertial>
+      <origin xyz="-0.0424999 8.43769e-11 -1.88428e-09" rpy="0 0 0"/>
+      <mass value="0.00503424"/>
+      <inertia ixx="2.63976e-08" ixy="-3.69517e-13" ixz="7.12534e-14" iyy="3.65211e-06" iyz="5.21041e-14" izz="3.66473e-06"/>
+    </inertial>
+    <!-- Part stewart_link_rod_4 -->
+    <visual>
+      <origin xyz="-0.0425 1.38778e-17 0" rpy="-3.54762e-18 2.76754e-18 -1.40989e-17"/>
+      <geometry>
+        <mesh filename="package://assets/stewart_link_rod.stl"/>
+      </geometry>
+      <material name="stewart_link_rod_4_material">
+        <color rgba="0.811765 0.858824 0.898039 1"/>
+      </material>
+    </visual>
+      </link>
+  <!-- Frame closing_4_1 (dummy link + fixed joint) -->
+  <link name="closing_4_1">
+    <origin xyz="0 0 0" rpy="0 -0 0"/>
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="1e-9"/>
+      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+    </inertial>
+  </link>
+  <joint name="closing_4_1_frame" type="fixed">
+    <origin xyz="-0.085 2.77556e-17 0" rpy="-3.54762e-18 2.76754e-18 -1.40989e-17"/>
+    <parent link="stewart_link_rod_4"/>
+    <child link="closing_4_1"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+  <!-- Joint from passive_4_link_y to stewart_link_rod_4 -->
+  <joint name="passive_4_z" type="revolute">
+    <origin xyz="6.93889e-18 0 0" rpy="-3.54762e-18 2.76754e-18 -1.40989e-17"/>
+    <parent link="passive_4_link_y"/>
+    <child link="stewart_link_rod_4"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="1e+08" lower="-3.141592653589793" upper="3.141592653589793"/>
+  </joint>
+  <!-- Joint from passive_4_link_x to passive_4_link_y -->
+  <joint name="passive_4_y" type="revolute">
+    <origin xyz="6.93889e-18 0 0" rpy="-3.54762e-18 2.76754e-18 -1.40989e-17"/>
+    <parent link="passive_4_link_x"/>
+    <child link="passive_4_link_y"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="10" velocity="1e+08" lower="-3.141592653589793" upper="3.141592653589793"/>
+  </joint>
+  <!-- Joint from dc15_a01_horn_dummy_4 to passive_4_link_x -->
+  <joint name="passive_4_x" type="revolute">
+    <origin xyz="0.04 2.77556e-17 0.007" rpy="-0.0860846 0.0882156 1.0381"/>
+    <parent link="dc15_a01_horn_dummy_4"/>
+    <child link="passive_4_link_x"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="10" velocity="1e+08" lower="-3.141592653589793" upper="3.141592653589793"/>
+  </joint>
+  <!-- Joint from body_down_3dprint to dc15_a01_horn_dummy_4 -->
+  <joint name="stewart_4" type="revolute">
+    <origin xyz="0.01 -0.0436606 0.0418333" rpy="-1.5708 -7.49452e-14 3.47181e-14"/>
+    <parent link="body_down_3dprint"/>
+    <child link="dc15_a01_horn_dummy_4"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="8" lower="-1.39626" upper="0.837758"/>
+  </joint>
+  <!-- Link dc15_a01_horn_dummy_5 -->
+  <link name="dc15_a01_horn_dummy_5">
+    <inertial>
+      <origin xyz="0.0237505 1.27591e-09 0.00335838" rpy="0 0 0"/>
+      <mass value="0.00366675"/>
+      <inertia ixx="8.57063e-08" ixy="1.79436e-13" ixz="-1.93125e-07" iyy="1.28019e-06" iyz="1.03818e-14" izz="1.26546e-06"/>
+    </inertial>
+    <!-- Part mp01062_stewart_arm_3_5 -->
+    <visual>
+      <origin xyz="-0.01 -0.153167 0.0436606" rpy="1.5708 -1.0472 6.12323e-17"/>
+      <geometry>
+        <mesh filename="package://assets/mp01062_stewart_arm_3.stl"/>
+      </geometry>
+      <material name="mp01062_stewart_arm_3_5_material">
+        <color rgba="0.301961 0.301961 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part stewart_link_ball_5 -->
+    <visual>
+      <origin xyz="-0.0025 8.32667e-17 0.007" rpy="1.14239e-16 1.38095e-14 -3.14159"/>
+      <geometry>
+        <mesh filename="package://assets/stewart_link_ball.stl"/>
+      </geometry>
+      <material name="stewart_link_ball_5_material">
+        <color rgba="0.439216 0.47451 0.501961 1"/>
+      </material>
+    </visual>
+        <!-- Part dc15_a01_horn_dummy_6 -->
+    <visual>
+      <origin xyz="0 0 -0.0065" rpy="1.5708 -1.18329e-29 -1.5708"/>
+      <geometry>
+        <mesh filename="package://assets/dc15_a01_horn_dummy.stl"/>
+      </geometry>
+      <material name="dc15_a01_horn_dummy_6_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+      </link>
+  <!-- Link passive_5_link_x -->
+  <link name="passive_5_link_x">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="1e-09"/>
+      <inertia ixx="1e-09" ixy="0" ixz="0" iyy="1e-09" iyz="0" izz="1e-09"/>
+    </inertial>
+  </link>
+  <!-- Link passive_5_link_y -->
+  <link name="passive_5_link_y">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="1e-09"/>
+      <inertia ixx="1e-09" ixy="0" ixz="0" iyy="1e-09" iyz="0" izz="1e-09"/>
+    </inertial>
+  </link>
+  <!-- Link stewart_link_rod_5 -->
+  <link name="stewart_link_rod_5">
+    <inertial>
+      <origin xyz="-0.0424999 8.43768e-11 -1.88428e-09" rpy="0 0 0"/>
+      <mass value="0.00503424"/>
+      <inertia ixx="2.63976e-08" ixy="-3.69517e-13" ixz="7.12534e-14" iyy="3.65211e-06" iyz="5.21041e-14" izz="3.66473e-06"/>
+    </inertial>
+    <!-- Part stewart_link_rod_5 -->
+    <visual>
+      <origin xyz="-0.0425 0 0" rpy="-1.10888e-16 -3.21444e-17 3.55033e-17"/>
+      <geometry>
+        <mesh filename="package://assets/stewart_link_rod.stl"/>
+      </geometry>
+      <material name="stewart_link_rod_5_material">
+        <color rgba="0.811765 0.858824 0.898039 1"/>
+      </material>
+    </visual>
+      </link>
+  <!-- Frame closing_5_1 (dummy link + fixed joint) -->
+  <link name="closing_5_1">
+    <origin xyz="0 0 0" rpy="0 -0 0"/>
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="1e-9"/>
+      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+    </inertial>
+  </link>
+  <joint name="closing_5_1_frame" type="fixed">
+    <origin xyz="-0.085 -1.38778e-17 0" rpy="-1.10888e-16 -3.21444e-17 3.55033e-17"/>
+    <parent link="stewart_link_rod_5"/>
+    <child link="closing_5_1"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+  <!-- Joint from passive_5_link_y to stewart_link_rod_5 -->
+  <joint name="passive_5_z" type="revolute">
+    <origin xyz="0 0 -6.93889e-18" rpy="-1.10888e-16 -3.21444e-17 3.55033e-17"/>
+    <parent link="passive_5_link_y"/>
+    <child link="stewart_link_rod_5"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="1e+08" lower="-3.141592653589793" upper="3.141592653589793"/>
+  </joint>
+  <!-- Joint from passive_5_link_x to passive_5_link_y -->
+  <joint name="passive_5_y" type="revolute">
+    <origin xyz="0 0 -6.93889e-18" rpy="-1.10888e-16 -3.21444e-17 3.55033e-17"/>
+    <parent link="passive_5_link_x"/>
+    <child link="passive_5_link_y"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="10" velocity="1e+08" lower="-3.141592653589793" upper="3.141592653589793"/>
+  </joint>
+  <!-- Joint from dc15_a01_horn_dummy_5 to passive_5_link_x -->
+  <joint name="passive_5_x" type="revolute">
+    <origin xyz="0.04 5.55112e-17 0.007" rpy="0.123977 0.0882156 -1.0381"/>
+    <parent link="dc15_a01_horn_dummy_5"/>
+    <child link="passive_5_link_x"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="10" velocity="1e+08" lower="-3.141592653589793" upper="3.141592653589793"/>
+  </joint>
+  <!-- Joint from body_down_3dprint to dc15_a01_horn_dummy_5 -->
+  <joint name="stewart_5" type="revolute">
+    <origin xyz="0.0428112 0.01317 0.0418333" rpy="1.5708 -1.79054e-13 -1.0472"/>
+    <parent link="body_down_3dprint"/>
+    <child link="dc15_a01_horn_dummy_5"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="8" lower="-1.22173" upper="1.39626"/>
+  </joint>
+  <!-- Link dc15_a01_horn_dummy_6 -->
+  <link name="dc15_a01_horn_dummy_6">
+    <inertial>
+      <origin xyz="0.0237505 1.27591e-09 0.00335838" rpy="0 0 0"/>
+      <mass value="0.00366675"/>
+      <inertia ixx="8.57063e-08" ixy="1.79436e-13" ixz="-1.93125e-07" iyy="1.28019e-06" iyz="1.03818e-14" izz="1.26546e-06"/>
+    </inertial>
+    <!-- Part dc15_a01_horn_dummy_7 -->
+    <visual>
+      <origin xyz="3.46945e-18 0 -0.0065" rpy="1.5708 2.54036e-31 -1.5708"/>
+      <geometry>
+        <mesh filename="package://assets/dc15_a01_horn_dummy.stl"/>
+      </geometry>
+      <material name="dc15_a01_horn_dummy_7_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part stewart_link_ball_6 -->
+    <visual>
+      <origin xyz="0.0825 8.32667e-17 0.007" rpy="2.5071e-14 -2.28835e-17 6.12323e-17"/>
+      <geometry>
+        <mesh filename="package://assets/stewart_link_ball.stl"/>
+      </geometry>
+      <material name="stewart_link_ball_6_material">
+        <color rgba="0.439216 0.47451 0.501961 1"/>
+      </material>
+    </visual>
+        <!-- Part mp01062_stewart_arm_3_6 -->
+    <visual>
+      <origin xyz="-0.01 -0.153167 0.0436606" rpy="1.5708 -1.0472 6.12323e-17"/>
+      <geometry>
+        <mesh filename="package://assets/mp01062_stewart_arm_3.stl"/>
+      </geometry>
+      <material name="mp01062_stewart_arm_3_6_material">
+        <color rgba="0.301961 0.301961 0.301961 1"/>
+      </material>
+    </visual>
+      </link>
+  <!-- Link passive_6_link_x -->
+  <link name="passive_6_link_x">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="1e-09"/>
+      <inertia ixx="1e-09" ixy="0" ixz="0" iyy="1e-09" iyz="0" izz="1e-09"/>
+    </inertial>
+  </link>
+  <!-- Link passive_6_link_y -->
+  <link name="passive_6_link_y">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="1e-09"/>
+      <inertia ixx="1e-09" ixy="0" ixz="0" iyy="1e-09" iyz="0" izz="1e-09"/>
+    </inertial>
+  </link>
+  <!-- Link stewart_link_rod_6 -->
+  <link name="stewart_link_rod_6">
+    <inertial>
+      <origin xyz="-0.0424999 8.43768e-11 -1.88428e-09" rpy="0 0 0"/>
+      <mass value="0.00503424"/>
+      <inertia ixx="2.63976e-08" ixy="-3.69517e-13" ixz="7.12534e-14" iyy="3.65211e-06" iyz="5.21041e-14" izz="3.66473e-06"/>
+    </inertial>
+    <!-- Part stewart_link_rod_6 -->
+    <visual>
+      <origin xyz="-0.0425 0 0" rpy="-7.27418e-17 2.10388e-17 4.15523e-17"/>
+      <geometry>
+        <mesh filename="package://assets/stewart_link_rod.stl"/>
+      </geometry>
+      <material name="stewart_link_rod_6_material">
+        <color rgba="0.811765 0.858824 0.898039 1"/>
+      </material>
+    </visual>
+      </link>
+  <!-- Link passive_7_link_x -->
+  <link name="passive_7_link_x">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="1e-09"/>
+      <inertia ixx="1e-09" ixy="0" ixz="0" iyy="1e-09" iyz="0" izz="1e-09"/>
+    </inertial>
+  </link>
+  <!-- Link passive_7_link_y -->
+  <link name="passive_7_link_y">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="1e-09"/>
+      <inertia ixx="1e-09" ixy="0" ixz="0" iyy="1e-09" iyz="0" izz="1e-09"/>
+    </inertial>
+  </link>
+  <!-- Link xl_330 -->
+  <link name="xl_330">
+    <inertial>
+      <origin xyz="-0.0487051 -0.0237648 0.020658" rpy="0 0 0"/>
+      <mass value="0.259259"/>
+      <inertia ixx="0.000634651" ixy="3.65691e-05" ixz="-2.18087e-05" iyy="0.000587786" iyz="9.29467e-05" izz="0.000460283"/>
+    </inertial>
+    <!-- Part m12_fisheye_lens_1_8mm -->
+    <visual>
+      <origin xyz="-0.0387195 -0.0329451 -0.0240431" rpy="1.43435 -0.544115 2.16946"/>
+      <geometry>
+        <mesh filename="package://assets/m12_fisheye_lens_1_8mm.stl"/>
+      </geometry>
+      <material name="m12_fisheye_lens_1_8mm_material">
+        <color rgba="0.498039 0.498039 0.498039 1"/>
+      </material>
+    </visual>
+        <!-- Part stewart_link_ball_7 -->
+    <visual>
+      <origin xyz="0.0175286 0.0420772 0.0551172" rpy="-2.11906 0.116645 -2.61388"/>
+      <geometry>
+        <mesh filename="package://assets/stewart_link_ball__2.stl"/>
+      </geometry>
+      <material name="stewart_link_ball_7_material">
+        <color rgba="0.439216 0.47451 0.501961 1"/>
+      </material>
+    </visual>
+        <!-- Part big_lens_d40 -->
+    <visual>
+      <origin xyz="-0.0262968 -0.0511554 -0.0373805" rpy="1.43435 -0.544115 2.16946"/>
+      <geometry>
+        <mesh filename="package://assets/big_lens_d40.stl"/>
+      </geometry>
+      <material name="big_lens_d40_material">
+        <color rgba="0.439216 0.47451 0.501961 0.301961"/>
+      </material>
+    </visual>
+        <!-- Part neck_reference_3dprint -->
+    <visual>
+      <origin xyz="-0.0535717 -0.0190178 0.0256147" rpy="1.79194 -1.01151 -1.23139"/>
+      <geometry>
+        <mesh filename="package://assets/neck_reference_3dprint.stl"/>
+      </geometry>
+      <material name="neck_reference_3dprint_material">
+        <color rgba="0.901961 0.901961 0.901961 1"/>
+      </material>
+    </visual>
+        <!-- Part head_back_3dprint -->
+    <visual>
+      <origin xyz="-0.0535717 -0.0190178 0.0256147" rpy="1.79194 -1.00151 -1.23139"/>
+      <geometry>
+         <mesh filename="package://assets/head_back_3dprint.stl"/> 
+      </geometry>
+      <material name="head_back_3dprint_material">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision name="head_collision">
+      <origin xyz="-0.0535717 -0.0190178 0.0256147" rpy="0.22194 0.57 -0.8"/>
+      <geometry>
+            <!-- <box size="0.12 0.09 0.07"/> -->
+            <cylinder radius="0.049" length="0.14"/>
+      </geometry>
+    </collision>
+    <!-- Placo is not really frinedly with these complex shapes so we use simpler ones -->
+    <!-- <collision name="head_collision">
+      <origin xyz="-0.0535717 -0.0190178 0.0256147" rpy="1.79194 -1.01151 -1.23139"/>
+      <geometry>
+      <mesh filename="package://assets/collision/coarse/head_one_3dprint_collider_0.stl"/>
+      </geometry>
+    </collision> -->
+        <!-- Part arducam -->
+    <visual>
+      <origin xyz="-0.045905 -0.0448628 -0.0036254" rpy="-1.79194 1.01151 1.9102"/>
+      <geometry>
+        <mesh filename="package://assets/arducam.stl"/>
+      </geometry>
+      <material name="arducam_material">
+        <color rgba="0.498039 0.498039 0.498039 1"/>
+      </material>
+    </visual>
+        <!-- Part pp01102_arducam_carter -->
+    <visual>
+      <origin xyz="-0.0311515 -0.0518836 0.00154345" rpy="1.79194 -1.01151 -1.23139"/>
+      <geometry>
+        <mesh filename="package://assets/pp01102_arducam_carter.stl"/>
+      </geometry>
+      <material name="pp01102_arducam_carter_material">
+        <color rgba="0.301961 0.301961 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part antenna_holder_l_3dprint -->
+    <visual>
+      <origin xyz="-0.0536682 -0.0188765 0.0257182" rpy="1.79194 -1.01151 -1.23139"/>
+      <geometry>
+        <mesh filename="package://assets/antenna_holder_l_3dprint.stl"/>
+      </geometry>
+      <material name="antenna_holder_l_3dprint_material">
+        <color rgba="0.498039 0.498039 0.498039 1"/>
+      </material>
+    </visual>
+        <!-- Part stewart_link_ball_8 -->
+    <visual>
+      <origin xyz="-0.00630385 -0.00639493 0.0739606" rpy="-2.90143 1.05953 -0.832675"/>
+      <geometry>
+        <mesh filename="package://assets/stewart_link_ball__2.stl"/>
+      </geometry>
+      <material name="stewart_link_ball_8_material">
+        <color rgba="0.439216 0.47451 0.501961 1"/>
+      </material>
+    </visual>
+        <!-- Part lens_cap_d30_3dprint -->
+    <visual>
+      <origin xyz="-0.0536562 -0.018894 0.0257054" rpy="1.79194 -1.01151 -1.23139"/>
+      <geometry>
+        <mesh filename="package://assets/lens_cap_d30_3dprint.stl"/>
+      </geometry>
+      <material name="lens_cap_d30_3dprint_material">
+        <color rgba="0.0980392 0.0980392 0.0980392 1"/>
+      </material>
+    </visual>
+        <!-- Part lens_cap_d40_3dprint -->
+    <visual>
+      <origin xyz="-0.0536391 -0.0189191 0.025687" rpy="1.79194 -1.01151 -1.23139"/>
+      <geometry>
+        <mesh filename="package://assets/lens_cap_d40_3dprint.stl"/>
+      </geometry>
+      <material name="lens_cap_d40_3dprint_material">
+        <color rgba="0.0980392 0.0980392 0.0980392 1"/>
+      </material>
+    </visual>
+        <!-- Part antenna_holder_r_3dprint -->
+    <visual>
+      <origin xyz="-0.0536682 -0.0188765 0.0257182" rpy="1.79194 -1.01151 -1.23139"/>
+      <geometry>
+        <mesh filename="package://assets/antenna_holder_r_3dprint.stl"/>
+      </geometry>
+      <material name="antenna_holder_r_3dprint_material">
+        <color rgba="0.498039 0.498039 0.498039 1"/>
+      </material>
+    </visual>
+        <!-- Part stewart_link_ball_9 -->
+    <visual>
+      <origin xyz="0.0150556 0.0490816 0.0432504" rpy="-2.11906 0.116645 -2.61388"/>
+      <geometry>
+        <mesh filename="package://assets/stewart_link_ball__2.stl"/>
+      </geometry>
+      <material name="stewart_link_ball_9_material">
+        <color rgba="0.439216 0.47451 0.501961 1"/>
+      </material>
+    </visual>
+        <!-- Part small_lens_d30 -->
+    <visual>
+      <origin xyz="-0.0244054 -0.0696173 0.0280123" rpy="1.43435 -0.544115 2.16946"/>
+      <geometry>
+        <mesh filename="package://assets/small_lens_d30.stl"/>
+      </geometry>
+      <material name="small_lens_d30_material">
+        <color rgba="0.439216 0.47451 0.501961 0.301961"/>
+      </material>
+    </visual>
+        <!-- Part stewart_link_ball_10 -->
+    <visual>
+      <origin xyz="-0.0109132 -0.00132777 0.0861703" rpy="-2.90143 1.05953 -0.832675"/>
+      <geometry>
+        <mesh filename="package://assets/stewart_link_ball__2.stl"/>
+      </geometry>
+      <material name="stewart_link_ball_10_material">
+        <color rgba="0.439216 0.47451 0.501961 1"/>
+      </material>
+    </visual>
+        <!-- Part stewart_link_ball_11 -->
+    <visual>
+      <origin xyz="-0.0215 0.0366458 0.00104087" rpy="0.11668 0.0244934 -1.04022"/>
+      <geometry>
+        <mesh filename="package://assets/stewart_link_ball__2.stl"/>
+      </geometry>
+      <material name="stewart_link_ball_11_material">
+        <color rgba="0.439216 0.47451 0.501961 1"/>
+      </material>
+    </visual>
+        <!-- Part glasses_dolder_3dprint -->
+    <visual>
+      <origin xyz="-0.0535717 -0.0190178 0.0256147" rpy="1.79194 -1.01151 -1.23139"/>
+      <geometry>
+        <mesh filename="package://assets/glasses_dolder_3dprint.stl"/>
+      </geometry>
+      <material name="glasses_dolder_3dprint_material">
+        <color rgba="0.196078 0.196078 0.196078 1"/>
+      </material>
+    </visual>
+        <!-- Part head_front_3dprint -->
+    <visual>
+      <origin xyz="-0.0535717 -0.0190178 0.0256147" rpy="1.79194 -1.01151 -1.23139"/>
+      <geometry>
+        <mesh filename="package://assets/head_front_3dprint.stl"/>
+      </geometry>
+      <material name="head_front_3dprint_material">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+        <!-- Part head_mic_3dprint -->
+    <visual>
+      <origin xyz="-0.0535717 -0.0190178 0.0256147" rpy="1.79194 -1.01151 -1.23139"/>
+      <geometry>
+        <mesh filename="package://assets/head_mic_3dprint.stl"/>
+      </geometry>
+      <material name="head_mic_3dprint_material">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+        <!-- Part m12_fisheye_lens_1_8mm_2 -->
+    <visual>
+      <origin xyz="-0.0272066 -0.0655112 0.0310197" rpy="1.43435 -0.544115 2.16946"/>
+      <geometry>
+        <mesh filename="package://assets/m12_fisheye_lens_1_8mm.stl"/>
+      </geometry>
+      <material name="m12_fisheye_lens_1_8mm_2_material">
+        <color rgba="0.498039 0.498039 0.498039 1"/>
+      </material>
+    </visual>
+        <!-- Part stewart_link_ball_12 -->
+    <visual>
+      <origin xyz="-0.0285824 0.0487174 0.00138374" rpy="0.11668 0.0244934 -1.04022"/>
+      <geometry>
+        <mesh filename="package://assets/stewart_link_ball__2.stl"/>
+      </geometry>
+      <material name="stewart_link_ball_12_material">
+        <color rgba="0.439216 0.47451 0.501961 1"/>
+      </material>
+    </visual>
+        <!-- Part phs_1_7x20_5_dc10_2_8 -->
+    <visual>
+      <origin xyz="-0.0737021 -0.0344474 0.0784637" rpy="1.36569 0.508471 2.10225"/>
+      <geometry>
+        <mesh filename="package://assets/phs_1_7x20_5_dc10_2.stl"/>
+      </geometry>
+      <material name="phs_1_7x20_5_dc10_2_8_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part dc15_a01_case_b_dummy_8 -->
+    <visual>
+      <origin xyz="-0.0737021 -0.0344474 0.0784637" rpy="1.36569 0.508471 2.10225"/>
+      <geometry>
+        <mesh filename="package://assets/dc15_a01_case_b_dummy.stl"/>
+      </geometry>
+      <material name="dc15_a01_case_b_dummy_8_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part dc15_a01_case_m_dummy_8 -->
+    <visual>
+      <origin xyz="-0.0737021 -0.0344474 0.0784637" rpy="1.36569 0.508471 2.10225"/>
+      <geometry>
+        <mesh filename="package://assets/dc15_a01_case_m_dummy.stl"/>
+      </geometry>
+      <material name="dc15_a01_case_m_dummy_8_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part dc15_a01_led_cap2_dummy_8 -->
+    <visual>
+      <origin xyz="-0.0737021 -0.0344474 0.0784637" rpy="1.36569 0.508471 2.10225"/>
+      <geometry>
+        <mesh filename="package://assets/dc15_a01_led_cap2_dummy.stl"/>
+      </geometry>
+      <material name="dc15_a01_led_cap2_dummy_8_material">
+        <color rgba="0.87451 0.447059 0.317647 1"/>
+      </material>
+    </visual>
+        <!-- Part bts2_m2_6x8_8 -->
+    <visual>
+      <origin xyz="-0.0737021 -0.0344474 0.0784637" rpy="1.36569 0.508471 2.10225"/>
+      <geometry>
+        <mesh filename="package://assets/bts2_m2_6x8.stl"/>
+      </geometry>
+      <material name="bts2_m2_6x8_8_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part dc15_a01_case_f_dummy_8 -->
+    <visual>
+      <origin xyz="-0.0737021 -0.0344474 0.0784637" rpy="1.36569 0.508471 2.10225"/>
+      <geometry>
+        <mesh filename="package://assets/dc15_a01_case_f_dummy.stl"/>
+      </geometry>
+      <material name="dc15_a01_case_f_dummy_8_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part b3b_eh_1_8 -->
+    <visual>
+      <origin xyz="-0.0737021 -0.0344474 0.0784637" rpy="1.36569 0.508471 2.10225"/>
+      <geometry>
+        <mesh filename="package://assets/b3b_eh_1.stl"/>
+      </geometry>
+      <material name="b3b_eh_1_8_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part b3b_eh_8 -->
+    <visual>
+      <origin xyz="-0.0737021 -0.0344474 0.0784637" rpy="1.36569 0.508471 2.10225"/>
+      <geometry>
+        <mesh filename="package://assets/b3b_eh.stl"/>
+      </geometry>
+      <material name="b3b_eh_8_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part phs_1_7x20_5_dc10_3_8 -->
+    <visual>
+      <origin xyz="-0.0737021 -0.0344474 0.0784637" rpy="1.36569 0.508471 2.10225"/>
+      <geometry>
+        <mesh filename="package://assets/phs_1_7x20_5_dc10_3.stl"/>
+      </geometry>
+      <material name="phs_1_7x20_5_dc10_3_8_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part phs_1_7x20_5_dc10_1_8 -->
+    <visual>
+      <origin xyz="-0.0737021 -0.0344474 0.0784637" rpy="1.36569 0.508471 2.10225"/>
+      <geometry>
+        <mesh filename="package://assets/phs_1_7x20_5_dc10_1.stl"/>
+      </geometry>
+      <material name="phs_1_7x20_5_dc10_1_8_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part phs_1_7x20_5_dc10_8 -->
+    <visual>
+      <origin xyz="-0.0737021 -0.0344474 0.0784637" rpy="1.36569 0.508471 2.10225"/>
+      <geometry>
+        <mesh filename="package://assets/phs_1_7x20_5_dc10.stl"/>
+      </geometry>
+      <material name="phs_1_7x20_5_dc10_8_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part phs_1_7x20_5_dc10_2_9 -->
+    <visual>
+      <origin xyz="-0.0909991 0.0145437 -0.0045371" rpy="0.0684236 1.39152 0.701924"/>
+      <geometry>
+        <mesh filename="package://assets/phs_1_7x20_5_dc10_2.stl"/>
+      </geometry>
+      <material name="phs_1_7x20_5_dc10_2_9_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part dc15_a01_case_b_dummy_9 -->
+    <visual>
+      <origin xyz="-0.0909991 0.0145437 -0.0045371" rpy="0.0684236 1.39152 0.701924"/>
+      <geometry>
+        <mesh filename="package://assets/dc15_a01_case_b_dummy.stl"/>
+      </geometry>
+      <material name="dc15_a01_case_b_dummy_9_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part dc15_a01_case_m_dummy_9 -->
+    <visual>
+      <origin xyz="-0.0909991 0.0145437 -0.0045371" rpy="0.0684236 1.39152 0.701924"/>
+      <geometry>
+        <mesh filename="package://assets/dc15_a01_case_m_dummy.stl"/>
+      </geometry>
+      <material name="dc15_a01_case_m_dummy_9_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part dc15_a01_led_cap2_dummy_9 -->
+    <visual>
+      <origin xyz="-0.0909991 0.0145437 -0.0045371" rpy="0.0684236 1.39152 0.701924"/>
+      <geometry>
+        <mesh filename="package://assets/dc15_a01_led_cap2_dummy.stl"/>
+      </geometry>
+      <material name="dc15_a01_led_cap2_dummy_9_material">
+        <color rgba="0.87451 0.447059 0.317647 1"/>
+      </material>
+    </visual>
+        <!-- Part bts2_m2_6x8_9 -->
+    <visual>
+      <origin xyz="-0.0909991 0.0145437 -0.0045371" rpy="0.0684236 1.39152 0.701924"/>
+      <geometry>
+        <mesh filename="package://assets/bts2_m2_6x8.stl"/>
+      </geometry>
+      <material name="bts2_m2_6x8_9_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part dc15_a01_case_f_dummy_9 -->
+    <visual>
+      <origin xyz="-0.0909991 0.0145437 -0.0045371" rpy="0.0684236 1.39152 0.701924"/>
+      <geometry>
+        <mesh filename="package://assets/dc15_a01_case_f_dummy.stl"/>
+      </geometry>
+      <material name="dc15_a01_case_f_dummy_9_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part b3b_eh_1_9 -->
+    <visual>
+      <origin xyz="-0.0909991 0.0145437 -0.0045371" rpy="0.0684236 1.39152 0.701924"/>
+      <geometry>
+        <mesh filename="package://assets/b3b_eh_1.stl"/>
+      </geometry>
+      <material name="b3b_eh_1_9_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part b3b_eh_9 -->
+    <visual>
+      <origin xyz="-0.0909991 0.0145437 -0.0045371" rpy="0.0684236 1.39152 0.701924"/>
+      <geometry>
+        <mesh filename="package://assets/b3b_eh.stl"/>
+      </geometry>
+      <material name="b3b_eh_9_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part phs_1_7x20_5_dc10_3_9 -->
+    <visual>
+      <origin xyz="-0.0909991 0.0145437 -0.0045371" rpy="0.0684236 1.39152 0.701924"/>
+      <geometry>
+        <mesh filename="package://assets/phs_1_7x20_5_dc10_3.stl"/>
+      </geometry>
+      <material name="phs_1_7x20_5_dc10_3_9_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part phs_1_7x20_5_dc10_1_9 -->
+    <visual>
+      <origin xyz="-0.0909991 0.0145437 -0.0045371" rpy="0.0684236 1.39152 0.701924"/>
+      <geometry>
+        <mesh filename="package://assets/phs_1_7x20_5_dc10_1.stl"/>
+      </geometry>
+      <material name="phs_1_7x20_5_dc10_1_9_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part phs_1_7x20_5_dc10_9 -->
+    <visual>
+      <origin xyz="-0.0909991 0.0145437 -0.0045371" rpy="0.0684236 1.39152 0.701924"/>
+      <geometry>
+        <mesh filename="package://assets/phs_1_7x20_5_dc10.stl"/>
+      </geometry>
+      <material name="phs_1_7x20_5_dc10_9_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+      </link>
+  <!-- Frame closing_1_2 (dummy link + fixed joint) -->
+  <link name="closing_1_2">
+    <origin xyz="0 0 0" rpy="0 -0 0"/>
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="1e-9"/>
+      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+    </inertial>
+  </link>
+  <joint name="closing_1_2_frame" type="fixed">
+    <origin xyz="0.00768882 -0.0217774 0.0368953" rpy="-2.90143 1.05953 -0.832675"/>
+    <parent link="xl_330"/>
+    <child link="closing_1_2"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+  <!-- Frame closing_2_2 (dummy link + fixed joint) -->
+  <link name="closing_2_2">
+    <origin xyz="0 0 0" rpy="0 -0 0"/>
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="1e-9"/>
+      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+    </inertial>
+  </link>
+  <joint name="closing_2_2_frame" type="fixed">
+    <origin xyz="0.00307947 -0.0167102 0.049105" rpy="-2.90143 1.05953 -0.832675"/>
+    <parent link="xl_330"/>
+    <child link="closing_2_2"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+  <!-- Frame closing_3_2 (dummy link + fixed joint) -->
+  <link name="closing_3_2">
+    <origin xyz="0 0 0" rpy="0 -0 0"/>
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="1e-9"/>
+      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+    </inertial>
+  </link>
+  <joint name="closing_3_2_frame" type="fixed">
+    <origin xyz="-0.0189403 0.0208215 0.050171" rpy="-2.92045 -1.01151 -1.23139"/>
+    <parent link="xl_330"/>
+    <child link="closing_3_2"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+  <!-- Frame closing_4_2 (dummy link + fixed joint) -->
+  <link name="closing_4_2">
+    <origin xyz="0 0 0" rpy="0 -0 0"/>
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="1e-9"/>
+      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+    </inertial>
+  </link>
+  <joint name="closing_4_2_frame" type="fixed">
+    <origin xyz="-0.0214133 0.0278259 0.0383042" rpy="-2.92045 -1.01151 -1.23139"/>
+    <parent link="xl_330"/>
+    <child link="closing_4_2"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+  <!-- Frame closing_5_2 (dummy link + fixed joint) -->
+  <link name="closing_5_2">
+    <origin xyz="0 0 0" rpy="0 -0 0"/>
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="1e-9"/>
+      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+    </inertial>
+  </link>
+  <joint name="closing_5_2_frame" type="fixed">
+    <origin xyz="-0.00708235 0.0120716 0.000342873" rpy="-3.11693 -0.116645 0.52771"/>
+    <parent link="xl_330"/>
+    <child link="closing_5_2"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+  <!-- Frame camera_optical (dummy link + fixed joint) -->
+  <link name="camera_optical">
+    <origin xyz="0 0 0" rpy="0 -0 0"/>
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="1e-9"/>
+      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+    </inertial>
+  </link>
+  <joint name="camera_optical_frame" type="fixed">
+    <origin xyz="-0.0321159 -0.05047 0.00257878" rpy="2.92045 1.01151 1.9102"/>
+    <parent link="xl_330"/>
+    <child link="camera_optical"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+  <!-- Frame camera (dummy link + fixed joint) -->
+  <link name="camera">
+    <origin xyz="0 0 0" rpy="0 -0 0"/>
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="1e-9"/>
+      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+    </inertial>
+  </link>
+  <joint name="camera_frame" type="fixed">
+    <origin xyz="-0.0321159 -0.05047 0.00257878" rpy="1.70724 0.544115 -0.97213"/>
+    <parent link="xl_330"/>
+    <child link="camera"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+  <!-- Frame head (dummy link + fixed joint) -->
+  <link name="head">
+    <origin xyz="0 0 0" rpy="0 -0 0"/>
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="1e-9"/>
+      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+    </inertial>
+  </link>
+  <joint name="head_frame" type="fixed">
+    <origin xyz="-0.00611127 0.00370522 0.0291364" rpy="1.70724 0.544115 -0.97213"/>
+    <parent link="xl_330"/>
+    <child link="head"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+  <!-- Link dc15_a01_horn_dummy_7 -->
+  <link name="dc15_a01_horn_dummy_7">
+    <inertial>
+      <origin xyz="2.09613e-05 -0.0378463 -0.00866471" rpy="0 0 0"/>
+      <mass value="0.0105926"/>
+      <inertia ixx="2.69314e-05" ixy="2.22219e-09" ixz="3.62599e-10" iyy="2.98203e-07" iyz="-6.63107e-07" izz="2.68876e-05"/>
+    </inertial>
+    <!-- Part antenna -->
+    <visual>
+      <origin xyz="8.00228e-15 -0.0588 -0.0103" rpy="2.21412e-15 -1.16302e-14 1.20994e-13"/>
+      <geometry>
+        <mesh filename="package://assets/antenna.stl"/>
+      </geometry>
+      <material name="antenna_material">
+        <color rgba="0 0 0 1"/>
+      </material>
+    </visual>
+        <!-- Part dc15_a01_horn_dummy_8 -->
+    <visual>
+      <origin xyz="3.46945e-18 5.55112e-17 0.0065" rpy="-1.5708 -9.90471e-15 1.20979e-13"/>
+      <geometry>
+        <mesh filename="package://assets/dc15_a01_horn_dummy.stl"/>
+      </geometry>
+      <material name="dc15_a01_horn_dummy_8_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part antenna_interface_3dprint -->
+    <visual>
+      <origin xyz="5.20417e-18 5.55112e-17 1.38778e-17" rpy="-1.5708 -3.37306e-16 -3.14159"/>
+      <geometry>
+        <mesh filename="package://assets/antenna_interface_3dprint.stl"/>
+      </geometry>
+      <material name="antenna_interface_3dprint_material">
+        <color rgba="0.713725 0.760784 0.8 1"/>
+      </material>
+    </visual>
+        <!-- Part antenna_body_3dprint -->
+    <visual>
+      <origin xyz="5.72459e-17 1.38778e-16 -0.0028" rpy="-1.5708 1.15929e-14 -3.14159"/>
+      <geometry>
+        <mesh filename="package://assets/antenna_body_3dprint.stl"/>
+      </geometry>
+      <material name="antenna_body_3dprint_material">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+      </link>
+  <!-- Joint from xl_330 to dc15_a01_horn_dummy_7 -->
+  <joint name="right_antenna" type="revolute">
+    <origin xyz="-0.0948524 0.0197779 -0.00445785" rpy="1.63922 1.39152 0.701924"/>
+    <parent link="xl_330"/>
+    <child link="dc15_a01_horn_dummy_7"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="8" lower="-3.141592653589793" upper="3.141592653589793"/>
+  </joint>
+  <!-- Link dc15_a01_horn_dummy_8 -->
+  <link name="dc15_a01_horn_dummy_8">
+    <inertial>
+      <origin xyz="2.09613e-05 -0.0378463 -0.00866471" rpy="0 0 0"/>
+      <mass value="0.0105926"/>
+      <inertia ixx="2.69314e-05" ixy="2.22219e-09" ixz="3.62599e-10" iyy="2.98203e-07" iyz="-6.63107e-07" izz="2.68876e-05"/>
+    </inertial>
+    <!-- Part dc15_a01_horn_dummy_9 -->
+    <visual>
+      <origin xyz="3.46945e-18 -2.77556e-17 0.0065" rpy="-1.5708 -2.80299e-14 1.15234e-13"/>
+      <geometry>
+        <mesh filename="package://assets/dc15_a01_horn_dummy.stl"/>
+      </geometry>
+      <material name="dc15_a01_horn_dummy_9_material">
+        <color rgba="0.262745 0.282353 0.301961 1"/>
+      </material>
+    </visual>
+        <!-- Part antenna_body_3dprint_2 -->
+    <visual>
+      <origin xyz="5.89806e-17 2.77556e-17 -0.0028" rpy="-1.5708 1.85671e-14 -3.14159"/>
+      <geometry>
+        <mesh filename="package://assets/antenna_body_3dprint.stl"/>
+      </geometry>
+      <material name="antenna_body_3dprint_2_material">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+        <!-- Part antenna_interface_3dprint_2 -->
+    <visual>
+      <origin xyz="5.20417e-18 0 0" rpy="-1.5708 2.85907e-14 -3.14159"/>
+      <geometry>
+        <mesh filename="package://assets/antenna_interface_3dprint.stl"/>
+      </geometry>
+      <material name="antenna_interface_3dprint_2_material">
+        <color rgba="0.713725 0.760784 0.8 1"/>
+      </material>
+    </visual>
+        <!-- Part antenna_2 -->
+    <visual>
+      <origin xyz="6.79838e-15 -0.0588 -0.0103" rpy="5.3124e-16 -1.84481e-14 1.16503e-13"/>
+      <geometry>
+        <mesh filename="package://assets/antenna.stl"/>
+      </geometry>
+      <material name="antenna_2_material">
+        <color rgba="0 0 0 1"/>
+      </material>
+    </visual>
+      </link>
+  <!-- Joint from xl_330 to dc15_a01_horn_dummy_8 -->
+  <joint name="left_antenna" type="revolute">
+    <origin xyz="-0.0764135 -0.0324475 0.0840224" rpy="2.93649 0.508471 2.10225"/>
+    <parent link="xl_330"/>
+    <child link="dc15_a01_horn_dummy_8"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="8" lower="-3.141592653589793" upper="3.141592653589793"/>
+  </joint>
+  <!-- Joint from passive_7_link_y to xl_330 -->
+  <joint name="passive_7_z" type="revolute">
+    <origin xyz="0 0 0" rpy="-7.27418e-17 -2.10388e-17 -4.15523e-17"/>
+    <parent link="passive_7_link_y"/>
+    <child link="xl_330"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="1e+08" lower="-3.141592653589793" upper="3.141592653589793"/>
+  </joint>
+  <!-- Joint from passive_7_link_x to passive_7_link_y -->
+  <joint name="passive_7_y" type="revolute">
+    <origin xyz="0 0 0" rpy="-7.27418e-17 -2.10388e-17 -4.15523e-17"/>
+    <parent link="passive_7_link_x"/>
+    <child link="passive_7_link_y"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="10" velocity="1e+08" lower="-3.141592653589793" upper="3.141592653589793"/>
+  </joint>
+  <!-- Joint from stewart_link_rod_6 to passive_7_link_x -->
+  <joint name="passive_7_x" type="revolute">
+    <origin xyz="-0.085 0 0" rpy="3.14159 2.10388e-17 4.15523e-17"/>
+    <parent link="stewart_link_rod_6"/>
+    <child link="passive_7_link_x"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="10" velocity="1e+08" lower="-3.141592653589793" upper="3.141592653589793"/>
+  </joint>
+  <!-- Joint from passive_6_link_y to stewart_link_rod_6 -->
+  <joint name="passive_6_z" type="revolute">
+    <origin xyz="1.38778e-17 0 0" rpy="-7.27418e-17 2.10388e-17 4.15523e-17"/>
+    <parent link="passive_6_link_y"/>
+    <child link="stewart_link_rod_6"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="1e+08" lower="-3.141592653589793" upper="3.141592653589793"/>
+  </joint>
+  <!-- Joint from passive_6_link_x to passive_6_link_y -->
+  <joint name="passive_6_y" type="revolute">
+    <origin xyz="1.38778e-17 0 0" rpy="-7.27418e-17 2.10388e-17 4.15523e-17"/>
+    <parent link="passive_6_link_x"/>
+    <child link="passive_6_link_y"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="10" velocity="1e+08" lower="-3.141592653589793" upper="3.141592653589793"/>
+  </joint>
+  <!-- Joint from dc15_a01_horn_dummy_6 to passive_6_link_x -->
+  <joint name="passive_6_x" type="revolute">
+    <origin xyz="0.04 9.71445e-17 0.007" rpy="3.0613 0.0882156 1.0381"/>
+    <parent link="dc15_a01_horn_dummy_6"/>
+    <child link="passive_6_link_x"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="10" velocity="1e+08" lower="-3.141592653589793" upper="3.141592653589793"/>
+  </joint>
+  <!-- Joint from body_down_3dprint to dc15_a01_horn_dummy_6 -->
+  <joint name="stewart_6" type="revolute">
+    <origin xyz="0.0328112 0.0304906 0.0418333" rpy="-1.5708 -5.32144e-14 2.0944"/>
+    <parent link="body_down_3dprint"/>
+    <child link="dc15_a01_horn_dummy_6"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="8" lower="-1.39626" upper="0.837758"/>
+  </joint>
+  <!-- Joint from body_foot_3dprint to body_down_3dprint -->
+  <joint name="yaw_body" type="revolute">
+    <origin xyz="-2.45997e-07 6.93889e-18 0.0348" rpy="-3.74039e-16 1.77636e-15 -1.5708"/>
+    <parent link="body_foot_3dprint"/>
+    <child link="body_down_3dprint"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="8" lower="-2.79253" upper="2.79253"/>
+  </joint>
+    <!-- Dummy Link head_oriented_yaw used for constraint setting in placo-kinematics -->
+  <link name="dummy_torso_yaw">
+    <visual>
+      <geometry>
+        <box size="0 0 0"/>
+      </geometry>
+      <material name="blue"/>
+    </visual>
+  </link>
+  <!-- Joint from pp01071_turning_bowl to dc15_a01_case_b_dummy -->
+  <joint name="dummy_yaw" type="fixed">
+    <origin xyz="0 0 0" rpy="3.14 0 1.57"/>
+    <!-- <origin xyz="4.85723e-17 -5.60663e-19 0.0339" rpy="3.14159 -2.22045e-16 1.5708"/> -->
+    <parent link="body_down_3dprint"/>
+    <!-- <parent link="dc15_a01_case_b_dummy"/>   -->
+    <child link="dummy_torso_yaw"/>
+    <axis xyz="0 0 1"/>
+  </joint>
+</robot>
diff --git a/src/reachy_mini/kinematics/placo_kinematics.py b/src/reachy_mini/kinematics/placo_kinematics.py
index 3c977bdd..fb9f0b2d 100644
--- a/src/reachy_mini/kinematics/placo_kinematics.py
+++ b/src/reachy_mini/kinematics/placo_kinematics.py
@@ -41,13 +41,15 @@ def __init__(
         )  # 0.1 degrees tolerance for the FK reached condition
 
         if not urdf_path.endswith(".urdf"):
-            urdf_path = f"{urdf_path}/{'robot.urdf' if check_collision else 'robot_no_collision.urdf'}"
+            urdf_path = f"{urdf_path}/{'robot_simple_collision.urdf' if check_collision else 'robot_no_collision.urdf'}"
 
         self.robot = placo.RobotWrapper(
-            urdf_path, placo.Flags.ignore_collisions + placo.Flags.collision_as_visual
+            urdf_path, placo.Flags.collision_as_visual + placo.Flags.ignore_collisions 
         )
+        
+        flags = 0 if check_collision else placo.Flags.ignore_collisions + placo.Flags.collision_as_visual
         self.robot_ik = placo.RobotWrapper(
-            urdf_path, placo.Flags.ignore_collisions + placo.Flags.collision_as_visual
+            urdf_path, flags
         )
 
         self.ik_solver = placo.KinematicsSolver(self.robot_ik)
@@ -240,6 +242,11 @@ def __init__(
         self.robot.update_kinematics()
 
         if self.check_collision:
+            ik_col = self.ik_solver.add_avoid_self_collisions_constraint()
+            ik_col.self_collisions_margin = 0.001 # 1mm
+            ik_col.self_collisions_trigger = 0.002 # 2mm
+            ik_col.configure("avoid_self_collisions", "hard")
+        
             # setup the collision model
             self.config_collision_model()
 
@@ -396,16 +403,7 @@ def ik(
                     self._logger.warning(f"IK solver failed: {e}, no solution found!")
                     return None
                 self.robot_ik.update_kinematics()
-
-        # verify that there is no collision
-        if self.check_collision and self.compute_collision():
-            self._logger.warning(
-                "IK: Collision detected, using the previous configuration..."
-            )
-            self._update_state_to_initial(self.robot_ik)  # revert to the inital state
-            self.robot_ik.update_kinematics()
-            return None
-
+                
         # Get the joint angles
         return np.array(self._get_joint_values(self.robot_ik))
 
@@ -487,13 +485,6 @@ def fk(
                     return None
                 self.robot.update_kinematics()
 
-        if self.check_collision and self.compute_collision():
-            self._logger.warning("FK: Collision detected, using the previous config...")
-            self._update_state_to_initial(self.robot)  # revert to the previous state
-            self.robot.state.q = q
-            self.robot.update_kinematics()
-            # return None
-
         # Get the head frame transformation
         T_world_head = self.robot.get_T_world_frame("head")
         T_world_head[:3, 3][2] -= self.head_z_offset  # offset the height of the head
@@ -505,20 +496,14 @@ def config_collision_model(self) -> None:
 
         Add collision pairs between the torso and the head colliders.
         """
-        geom_model = self.robot.collision_model
-
-        # name_torso_collider = "dc15_a01_case_b_dummy_10"
-        # names_head_colliders = ["pp01063_stewart_plateform_7", "pp01063_stewart_plateform_11"]
+        geom_model = self.robot_ik.collision_model
 
-        id_torso_collider = 12  # geom_model.getGeometryObjectId(name_torso_collider)
-        id_head_colliders = [
-            74,
-            78,
-        ]  # [geom_model.getGeometryObjectId(name) for name in names_head_colliders]
+        id_torso_colliders = list(range(len(geom_model.geometryObjects)-1))
+        id_head_collider = len(geom_model.geometryObjects)-1 
 
-        for i in id_head_colliders:
+        for i in id_torso_colliders:
             geom_model.addCollisionPair(
-                pin.CollisionPair(id_torso_collider, i)
+                pin.CollisionPair(id_head_collider, i)
             )  # torso with head colliders
 
     def compute_collision(self, margin: float = 0.005) -> bool:
@@ -531,17 +516,18 @@ def compute_collision(self, margin: float = 0.005) -> bool:
             True if there is a collision, False otherwise.
 
         """
-        collision_data = self.robot.collision_model.createData()
-        data = self.robot.model.createData()
+        collision_data = self.robot_ik.collision_model.createData()
+        data = self.robot_ik.model.createData()
 
         # pin.computeCollisions(
         pin.computeDistances(
-            self.robot.model,
+            self.robot_ik.model,
             data,
-            self.robot.collision_model,
+            self.robot_ik.collision_model,
             collision_data,
-            self.robot.state.q,
+            self.robot_ik.state.q,
         )
+        
 
         # Iterate over all collision pairs
         for distance_result in collision_data.distanceResults:

From f5fa37a53bd76b76169bd23c262e47e1a864871e Mon Sep 17 00:00:00 2001
From: gospar <antun.skuric@outlook.com>
Date: Thu, 30 Oct 2025 10:34:37 +0100
Subject: [PATCH 15/15] ruff stuff

---
 src/reachy_mini/kinematics/placo_kinematics.py | 3 ---
 1 file changed, 3 deletions(-)

diff --git a/src/reachy_mini/kinematics/placo_kinematics.py b/src/reachy_mini/kinematics/placo_kinematics.py
index fb9f0b2d..97b3321d 100644
--- a/src/reachy_mini/kinematics/placo_kinematics.py
+++ b/src/reachy_mini/kinematics/placo_kinematics.py
@@ -451,9 +451,6 @@ def fk(
             }
         )
 
-        # save the initial configuration
-        q = self.robot.state.q.copy()
-
         done = True
         # do the inital ik with 2 iterations
         for i in range(no_iterations):