<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="20": twenty Newton-smoothing passes on no-slip constraints.
Prevents micro-slip between gripper faces and grasped objects from
accumulating into macroscopic sliding / ejection.
Higher count (was 5) needed so gripper-banana no-slip converges even when
the arm receives a simultaneous contact impulse from the table or plate.
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="20" 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" />
<!-- CoACD decomposition of wrist_roll_follower (8 convex pieces) -->
<mesh name="wrist_roll_follower_col_0" file="wrist_roll_follower_col_0.stl" />
<mesh name="wrist_roll_follower_col_1" file="wrist_roll_follower_col_1.stl" />
<mesh name="wrist_roll_follower_col_2" file="wrist_roll_follower_col_2.stl" />
<mesh name="wrist_roll_follower_col_3" file="wrist_roll_follower_col_3.stl" />
<mesh name="wrist_roll_follower_col_4" file="wrist_roll_follower_col_4.stl" />
<mesh name="wrist_roll_follower_col_5" file="wrist_roll_follower_col_5.stl" />
<mesh name="wrist_roll_follower_col_6" file="wrist_roll_follower_col_6.stl" />
<mesh name="wrist_roll_follower_col_7" file="wrist_roll_follower_col_7.stl" />
<mesh name="moving_jaw" file="moving_jaw_so101_v1.stl" />
<!-- CoACD decomposition of moving_jaw (8 convex pieces) -->
<mesh name="moving_jaw_col_0" file="moving_jaw_col_0.stl" />
<mesh name="moving_jaw_col_1" file="moving_jaw_col_1.stl" />
<mesh name="moving_jaw_col_2" file="moving_jaw_col_2.stl" />
<mesh name="moving_jaw_col_3" file="moving_jaw_col_3.stl" />
<mesh name="moving_jaw_col_4" file="moving_jaw_col_4.stl" />
<mesh name="moving_jaw_col_5" file="moving_jaw_col_5.stl" />
<mesh name="moving_jaw_col_6" file="moving_jaw_col_6.stl" />
<mesh name="moving_jaw_col_7" file="moving_jaw_col_7.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" />
<!--
gripper_col: contact parameters for fixed jaw (wrist_roll_follower) and
moving jaw (moving_jaw) CoACD mesh pieces — 16 geoms total.
Change friction here to tune all of them at once.
friction="F 1.0 0.05": F=sliding (↑ = more grip), 1.0=torsional, 0.05=rolling.
Effective banana-gripper friction = min(gripper_F, banana_F).
-->
<default class="gripper_col">
<geom friction="10.0 1.0 0.05" condim="4"
solimp="0.99 0.999 0.001 0.5 2" solref="0.004 1"/>
</default>
</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="1.5" armature="0.005"/>
<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" />
<!-- No collision geom for shoulder: joints 1-4 rely on joint limits and
damping for self-collision avoidance. In pick-and-place scenes the
arm should not contact objects; re-add box proxies here if needed. -->
<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="1.5" armature="0.005"/>
<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" />
<!-- No collision geom for upper_arm: see shoulder comment above. -->
<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="1.5" armature="0.005"/>
<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" />
<!-- No collision geom for lower_arm: see shoulder comment above. -->
<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="1.5" armature="0.005"/>
<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" />
<!-- No collision geom for wrist: see shoulder comment above. -->
<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="1.5" armature="0.005"/>
<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" />
<!-- Only finger mesh collisions — no arm boxes -->
<!-- Fixed jaw: CoACD 8-piece convex decomposition of wrist_roll_follower.
Same pos/euler as visual geom. -->
<geom class="gripper_col" type="mesh" mesh="wrist_roll_follower_col_0" pos="0 -0.000218214 0.000949706" euler="-3.14159 0 0"/>
<geom class="gripper_col" type="mesh" mesh="wrist_roll_follower_col_1" pos="0 -0.000218214 0.000949706" euler="-3.14159 0 0"/>
<geom class="gripper_col" type="mesh" mesh="wrist_roll_follower_col_2" pos="0 -0.000218214 0.000949706" euler="-3.14159 0 0"/>
<geom class="gripper_col" type="mesh" mesh="wrist_roll_follower_col_3" pos="0 -0.000218214 0.000949706" euler="-3.14159 0 0"/>
<geom class="gripper_col" type="mesh" mesh="wrist_roll_follower_col_4" pos="0 -0.000218214 0.000949706" euler="-3.14159 0 0"/>
<geom class="gripper_col" type="mesh" mesh="wrist_roll_follower_col_5" pos="0 -0.000218214 0.000949706" euler="-3.14159 0 0"/>
<geom class="gripper_col" type="mesh" mesh="wrist_roll_follower_col_6" pos="0 -0.000218214 0.000949706" euler="-3.14159 0 0"/>
<geom class="gripper_col" type="mesh" mesh="wrist_roll_follower_col_7" pos="0 -0.000218214 0.000949706" euler="-3.14159 0 0"/>
<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="1.5"
armature="0.028" frictionloss="0.052" />
<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: CoACD 8-piece convex decomposition.
Same pos/euler as visual geom. -->
<geom class="gripper_col" type="mesh" mesh="moving_jaw_col_0" pos="0 0 0.0189" euler="0 0 0"/>
<geom class="gripper_col" type="mesh" mesh="moving_jaw_col_1" pos="0 0 0.0189" euler="0 0 0"/>
<geom class="gripper_col" type="mesh" mesh="moving_jaw_col_2" pos="0 0 0.0189" euler="0 0 0"/>
<geom class="gripper_col" type="mesh" mesh="moving_jaw_col_3" pos="0 0 0.0189" euler="0 0 0"/>
<geom class="gripper_col" type="mesh" mesh="moving_jaw_col_4" pos="0 0 0.0189" euler="0 0 0"/>
<geom class="gripper_col" type="mesh" mesh="moving_jaw_col_5" pos="0 0 0.0189" euler="0 0 0"/>
<geom class="gripper_col" type="mesh" mesh="moving_jaw_col_6" pos="0 0 0.0189" euler="0 0 0"/>
<geom class="gripper_col" type="mesh" mesh="moving_jaw_col_7" pos="0 0 0.0189" euler="0 0 0"/>
<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.
Joints 1-5: kp + dampratio=1.0 (MuJoCo auto-computes critical kv each timestep).
Joint 6 (gripper): kp + explicit kv; armature is nonzero so kv is fixed and
well-defined regardless of timestep. -->
<actuator>
<position name="1" joint="1" kp="998.22" dampratio="1.0" ctrlrange="-2.0693 2.0709" forcerange="-2.75 2.75"/>
<position name="2" joint="2" kp="998.22" dampratio="1.0" ctrlrange="-1.92 1.92" forcerange="-2.75 2.75"/>
<position name="3" joint="3" kp="998.22" dampratio="1.0" ctrlrange="-1.6813 1.6828" forcerange="-2.75 2.75"/>
<position name="4" joint="4" kp="998.22" dampratio="1.0" ctrlrange="-1.65806 1.65806" forcerange="-2.75 2.75"/>
<position name="5" joint="5" kp="998.22" dampratio="1.0" ctrlrange="-2.9115 2.9115" forcerange="-2.75 2.75"/>
<position name="6" joint="6" kp="748.66" kv="5.5" ctrlrange="0.0 1.0" forcerange="-2.75 2.75"/>
</actuator>
</mujoco>
