<mujoco model="so101">
<!--
MuJoCo physics model for SO101 robot arm — TEMPLATE FILE
Generated by mujoco_adapter._generate_robot_mujoco_xml() at launch time.
Placeholder substitutions (Python string replace before XML parse):
{{MESHES_DIR}} → absolute path to robot_description/meshes/lerobot/so101/
{{ROBOT_BASE_POS}} → "x y z" from layout.yaml robot_spawn (default: "0 0 0")
Camera convention:
Cameras are NOT defined here. They are injected dynamically from YAML peripherals
by mujoco_adapter._generate_robot_mujoco_xml(robot_spawn, peripherals).
Only cameras with driver="opencv" are injected.
Camera name convention: YAML name="top" → MJCF camera name="top_camera"
(must match _build_camera_remappings convention: f"{name}_camera")
Joint names "1"–"6" match the ros2_control joint names in so101_ros2_control.xacro.
Body hierarchy, joint origins, and axes are taken directly from the URDF:
IB_Robot/src/robot_description/urdf/lerobot/so101/so101_base.xacro
eulerseq="XYZ" (uppercase = extrinsic rotations) matches URDF's RPY convention:
URDF rpy=(r,p,y) = extrinsic XYZ: R = Rz(y)*Ry(p)*Rx(r)
MuJoCo eulerseq="XYZ": same formula → angles can be copied directly from URDF.
MuJoCo camera direction convention:
MuJoCo cameras look in -Z of their frame.
YAML transform stores Gazebo/URDF convention (camera looks in +Z).
mujoco_adapter applies: mj_roll = yaml_roll + pi (converting +Z to -Z)
-->
<compiler
meshdir="{{MESHES_DIR}}"
eulerseq="XYZ"
angle="radian" />
<!--
Physics solver settings — tuned for stable contact-rich grasping.
timestep="0.001": 1 ms step (was implicit 2 ms).
Keeps solref timeconst / timestep ≥ 10× (0.010/0.001), safely above the
≥4× stability floor. Controller still runs at 50 Hz (every 20 physics steps).
noslip_iterations="5": five Newton-smoothing passes on no-slip constraints.
Prevents micro-slip between gripper faces and grasped objects from
accumulating into macroscopic sliding / ejection.
cone="elliptic": exact elliptic friction cone for condim=4 contacts.
The default pyramidal cone over-approximates friction in the diagonal
direction, causing numerical chatter in the torsional friction channel
(condim=4 adds one torsional DOF). Elliptic cone eliminates this.
-->
<option gravity="0 0 -9.81" integrator="implicitfast"
timestep="0.001" noslip_iterations="5" cone="elliptic">
<flag contact="enable" />
</option>
<!-- ================================================================
Mesh assets (STL files from robot_description package)
================================================================ -->
<asset>
<material name="mat_robot" specular="0.25" shininess="0.1"/>
<mesh name="base_motor_holder" file="base_motor_holder_so101_v1.stl" />
<mesh name="base_so101" file="base_so101_v2.stl" />
<mesh name="sts3215" file="sts3215_03a_v1.stl" />
<mesh name="waveshare" file="waveshare_mounting_plate_so101_v2.stl" />
<mesh name="motor_holder_base" file="motor_holder_so101_base_v1.stl" />
<mesh name="rotation_pitch" file="rotation_pitch_so101_v1.stl" />
<mesh name="upper_arm_mesh" file="upper_arm_so101_v1.stl" />
<mesh name="under_arm_mesh" file="under_arm_so101_v1.stl" />
<mesh name="motor_holder_wrist" file="motor_holder_so101_wrist_v1.stl" />
<mesh name="sts3215_no_horn" file="sts3215_03a_no_horn_v1.stl" />
<mesh name="wrist_roll_pitch" file="wrist_roll_pitch_so101_v2.stl" />
<mesh name="wrist_roll_follower" file="wrist_roll_follower_so101_v1.stl" />
<mesh name="moving_jaw" file="moving_jaw_so101_v1.stl" />
</asset>
<!-- ================================================================
Default classes: separate visual (display-only) and
collision (physics) geoms.
================================================================ -->
<default>
<default class="visual">
<geom contype="0" conaffinity="0" group="1" rgba="1.0 0.5 0.0 1"
material="mat_robot" />
</default>
<default class="collision">
<geom contype="1" conaffinity="1" group="0" rgba="0 0 0 0" />
</default>
</default>
<worldbody>
<!-- 补光由场景 headlight ambient 提供，不再额外加点光源 -->
<!-- Cameras are injected here at runtime by mujoco_adapter._generate_robot_mujoco_xml()
based on YAML peripherals with driver="opencv". See file header for convention. -->
<geom type="plane" size="1 1 0.01" rgba="0 0 0 0" contype="0" conaffinity="0" />
<!--
base body — fixed to the world (no joint).
Joint "base_joint" in URDF is a fixed joint (world→base, origin=0,0,0).
{{ROBOT_BASE_POS}} is substituted at runtime from layout.yaml robot_spawn.
-->
<body name="base" pos="{{ROBOT_BASE_POS}}">
<inertial
pos="0.020739 0.00204287 0.065966"
mass="0.147"
fullinertia="0.000136117 0.000114686 0.000130364 4.59787e-07 9.75275e-08 -4.97151e-06" />
<!-- Collision proxy -->
<geom class="collision" type="box" size="0.055 0.04 0.055"
pos="0.02 0.02 0.055" />
<!-- Visual meshes — origins taken from URDF visual elements -->
<geom class="visual" type="mesh" mesh="base_motor_holder"
pos="0.0206915 0.0221255 0.0300817" euler="1.5708 0 0" />
<geom class="visual" type="mesh" mesh="base_so101"
pos="0.0207909 0.0221255 0.0300817" euler="1.5708 0 0" />
<geom class="visual" type="mesh" mesh="sts3215" rgba="0.1 0.1 0.1 1"
pos="0.0207909 -0.0105745 0.0761817" euler="0 0 -1.5708" />
<geom class="visual" type="mesh" mesh="waveshare"
pos="0.0205915 0.0467435 0.0798817" euler="1.5708 0 0" />
<!--
body shoulder
URDF joint "1": origin xyz="0.0207909 -0.0230745 0.0948817"
rpy="-3.14159 0 1.5708"
axis xyz="0 0 1" → MuJoCo hinge axis="0 0 1"
-->
<body name="shoulder" pos="0.0207909 -0.0230745 0.0948817"
euler="-3.14159 0 1.5708">
<joint name="1" type="hinge" axis="0 0 1"
range="-2.0693 2.0709" damping="0.5" />
<inertial
pos="-0.0307604 -1.66727e-05 -0.0252713"
mass="0.100006"
fullinertia="8.3759e-05 8.10403e-05 2.39783e-05 7.55525e-08 -1.16342e-06 1.54663e-07" />
<geom class="collision" type="box" size="0.04 0.025 0.055"
pos="-0.03 0 0" />
<geom class="visual" type="mesh" mesh="sts3215" rgba="0.1 0.1 0.1 1"
pos="-0.0303992 0.000422241 -0.0417" euler="1.5708 1.5708 0" />
<geom class="visual" type="mesh" mesh="motor_holder_base"
pos="-0.0675992 -0.000177759 0.0158499" euler="1.5708 -1.5708 0" />
<geom class="visual" type="mesh" mesh="rotation_pitch"
pos="0.0122008 2.22413e-05 0.0464" euler="-1.5708 0 0" />
<!--
body upper_arm
URDF joint "2": origin xyz="-0.0303992 -0.0182778 -0.0542"
rpy="-1.5708 -1.5708 0"
axis xyz="0 0 1"
-->
<body name="upper_arm" pos="-0.0303992 -0.0182778 -0.0542"
euler="-1.5708 -1.5708 0">
<joint name="2" type="hinge" axis="0 0 1"
range="-1.92 1.92" damping="0.5" />
<inertial
pos="-0.0898471 -0.00838224 0.0184089"
mass="0.103"
fullinertia="4.08002e-05 0.000147318 0.000142487 -1.97819e-05 -4.03016e-08 8.97326e-09" />
<geom class="collision" type="box" size="0.06 0.025 0.025"
pos="-0.06 0 0.018" />
<geom class="visual" type="mesh" mesh="sts3215" rgba="0.1 0.1 0.1 1"
pos="-0.11257 -0.0155 0.0187" euler="-3.14159 0 -1.5708" />
<geom class="visual" type="mesh" mesh="upper_arm_mesh"
pos="-0.065085 0.012 0.0182" euler="3.14159 0 0" />
<!--
body lower_arm
URDF joint "3": origin xyz="-0.11257 -0.028 0"
rpy="0 0 1.5708"
axis xyz="0 0 1"
-->
<body name="lower_arm" pos="-0.11257 -0.028 0"
euler="0 0 1.5708">
<joint name="3" type="hinge" axis="0 0 1"
range="-1.6813 1.6828" damping="0.5" />
<inertial
pos="-0.0980701 0.00324376 0.0182831"
mass="0.104"
fullinertia="2.87438e-05 0.000159844 0.00014529 7.41152e-06 1.26409e-06 -4.90188e-08" />
<geom class="collision" type="box" size="0.065 0.025 0.025"
pos="-0.065 0 0.018" />
<geom class="visual" type="mesh" mesh="under_arm_mesh"
pos="-0.0648499 -0.032 0.0182" euler="-3.14159 0 0" />
<geom class="visual" type="mesh" mesh="motor_holder_wrist"
pos="-0.0648499 -0.032 0.018" euler="-3.14159 0 0" />
<geom class="visual" type="mesh" mesh="sts3215" rgba="0.1 0.1 0.1 1"
pos="-0.1224 0.0052 0.0187" euler="-3.14159 0 -3.14159" />
<!--
body wrist
URDF joint "4": origin xyz="-0.1349 0.0052 0"
rpy="0 0 -1.5708"
axis xyz="0 0 1"
-->
<body name="wrist" pos="-0.1349 0.0052 0"
euler="0 0 -1.5708">
<joint name="4" type="hinge" axis="0 0 1"
range="-1.65806 1.65806" damping="0.5" />
<inertial
pos="-0.000103312 -0.0386143 0.0281156"
mass="0.079"
fullinertia="3.68263e-05 2.5391e-05 2.1e-05 1.7893e-08 -5.28128e-08 3.6412e-06" />
<geom class="collision" type="box" size="0.025 0.045 0.03"
pos="0 -0.04 0.028" />
<geom class="visual" type="mesh" mesh="sts3215_no_horn" rgba="0.1 0.1 0.1 1"
pos="0 -0.0424 0.0306" euler="1.5708 1.5708 0" />
<geom class="visual" type="mesh" mesh="wrist_roll_pitch"
pos="0 -0.028 0.0181" euler="-1.5708 -1.5708 0" />
<!--
body gripper
URDF joint "5": origin xyz="0 -0.0611 0.0181"
rpy="1.5708 0 3.14159"
axis xyz="0 0 1"
-->
<body name="gripper" pos="0 -0.0611 0.0181"
euler="1.5708 0 3.14159">
<joint name="5" type="hinge" axis="0 0 1"
range="-2.9115 2.9115" damping="0.5" />
<inertial
pos="0.000213627 0.000245138 -0.025187"
mass="0.087"
fullinertia="2.75087e-05 4.33657e-05 3.45059e-05 -3.35241e-07 -5.7352e-06 -5.17847e-08" />
<!--
Gripper body collision proxies (three geoms):
1. Motor housing box — STS3215 servo chassis.
Covers X[-0.020,+0.020] Y[-0.020,+0.020] Z[-0.065,+0.015].
2. Fixed jaw finger — wrist_roll_follower finger region.
STL analysis (Rx180 + offset → gripper frame):
Z[-0.085,-0.065]: X[-0.027,-0.010]
Z[-0.105,-0.085]: X[-0.020,-0.008]
Finger tip at Z≈-0.105 (old single box only reached Z=-0.065).
Box: X[-0.036,-0.008] Y[-0.011,+0.011] Z[-0.106,-0.020]
3. Palm back-plate — the large wrist_roll_follower body plate.
STL analysis: 54,418 vertices at Y[+0.020,+0.101] Z[-0.065,+0.015]
with NO prior collision coverage (穿模 primary cause).
Without this geom the banana has no physics boundary to stop it
from visually entering the gripper palm during arm approach or
wrist-roll orientations that face the palm toward the object.
Box: X[-0.024,+0.028] Y[+0.021,+0.101] Z[-0.065,+0.015]
center: (0.002, 0.061, -0.025) half-size: (0.026, 0.040, 0.040)
-->
<geom class="collision" type="box" size="0.02 0.02 0.04"
pos="0 0 -0.025"
friction="2.0 0.5 0.05" condim="4"
solimp="0.9 0.95 0.002 0.5 2" solref="0.010 1"/>
<geom class="collision" type="box" size="0.014 0.011 0.043"
pos="-0.022 0 -0.063"
friction="2.0 0.5 0.05" condim="4"
solimp="0.9 0.95 0.002 0.5 2" solref="0.010 1"/>
<geom class="collision" type="box" size="0.026 0.040 0.040"
pos="0.002 0.061 -0.025"/>
<geom class="visual" type="mesh" mesh="sts3215" rgba="0.1 0.1 0.1 1"
pos="0.0077 0.0001 -0.0234" euler="-1.5708 0 0" />
<geom class="visual" type="mesh" mesh="wrist_roll_follower"
pos="0 -0.000218214 0.000949706" euler="-3.14159 0 0" />
<!-- wrist_camera injected here by mujoco_adapter if YAML has
peripherals[name=wrist, driver=opencv, transform.parent_frame=gripper] -->
<!--
body jaw
URDF joint "6": origin xyz="0.0202 0.0188 -0.0234"
rpy="1.5708 0 0"
axis xyz="0 0 1"
-->
<body name="jaw" pos="0.0202 0.0188 -0.0234"
euler="1.5708 0 0">
<joint name="6" type="hinge" axis="0 0 1"
range="0.0 1.0" damping="0.3" />
<inertial
pos="-0.00157495 -0.0300244 0.0192755"
mass="0.012"
fullinertia="6.61427e-06 1.89032e-06 5.28738e-06 -3.19807e-07 -5.90717e-09 -1.09945e-07" />
<!--
Moving jaw finger — full-coverage collision box.
STL analysis: jaw_local Y[-0.082,+0.010] Z[-0.024,+0.024]
Old box (size 0.015 0.030 0.020 at pos 0 -0.030 0.019) only reached
jaw_local Y=-0.060 → gripper Z=-0.083; tip at Y=-0.082 → Z=-0.105
was completely uncovered (2.2 cm gap).
New box: jaw_local Y[-0.082,+0.010] fully covered.
center: (0, -0.036, 0.019) half-size: (0.012, 0.046, 0.021)
In gripper frame: X[0.008,0.032] Y[-0.021,+0.021] Z[-0.105,-0.013] ✓
-->
<geom class="collision" type="box" size="0.012 0.046 0.021"
pos="0 -0.036 0.019"
friction="2.0 0.5 0.05" condim="4"
solimp="0.9 0.95 0.002 0.5 2" solref="0.010 1"/>
<geom class="visual" type="mesh" mesh="moving_jaw"
pos="0 0 0.0189" euler="0 0 0" />
</body> <!-- jaw -->
</body> <!-- gripper -->
</body> <!-- wrist -->
</body> <!-- lower_arm -->
</body> <!-- upper_arm -->
</body> <!-- shoulder -->
</body> <!-- base -->
</worldbody>
<!-- Exclude self-collisions between adjacent links -->
<contact>
<exclude body1="base" body2="shoulder" />
<exclude body1="shoulder" body2="upper_arm" />
<exclude body1="upper_arm" body2="lower_arm" />
<exclude body1="lower_arm" body2="wrist" />
<exclude body1="wrist" body2="gripper" />
<exclude body1="gripper" body2="jaw" />
</contact>
<!-- Position actuators for ros2_control MujocoSystemInterface.
Actuator names match joint names so get_actuator_id() resolves them.
kp controls position stiffness; dampratio=1 gives critical damping. -->
<actuator>
<position name="1" joint="1" kp="500" dampratio="1.0" ctrlrange="-2.0693 2.0709"/>
<position name="2" joint="2" kp="500" dampratio="1.0" ctrlrange="-1.92 1.92"/>
<position name="3" joint="3" kp="500" dampratio="1.0" ctrlrange="-1.6813 1.6828"/>
<position name="4" joint="4" kp="500" dampratio="1.0" ctrlrange="-1.65806 1.65806"/>
<position name="5" joint="5" kp="500" dampratio="1.0" ctrlrange="-2.9115 2.9115"/>
<position name="6" joint="6" kp="200" dampratio="1.0" ctrlrange="0.0 1.0"/>
</actuator>
</mujoco>
