blanchon's picture
Update
6ce4ca6
<?xml version="1.0" encoding="utf-8"?>
<robot name="so_arm101">
<!-- metadata -->
<property name="manufacturer" value="LeRobot"/>
<property name="model_id" value="so_arm101"/>
<!-- materials -->
<material name="orange">
<color rgba="1.0 0.5 0.0 1.0"/>
</material>
<material name="black">
<color rgba="0.1 0.1 0.1 1.0"/>
</material>
<!-- base link -->
<link name="Base">
<inertial>
<!-- approximate mass/inertia -->
<mass value="1.0"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/Base.stl"/>
</geometry>
<material name="orange"/>
</visual>
<visual>
<geometry>
<mesh filename="meshes/Base_Motor.stl"/>
</geometry>
<material name="black"/>
</visual>
<collision>
<geometry>
<mesh filename="meshes/Base.stl"/>
</geometry>
</collision>
</link>
<!-- rotation-pitch link -->
<link name="Rotation_Pitch">
<inertial>
<mass value="0.119226"/>
<origin xyz="-9.07886e-05 0.0590972 0.031089" rpy="0 0 0"/>
<inertia ixx="5.94278e-05" ixy="0" ixz="0" iyy="5.89975e-05" iyz="0" izz="3.13712e-05"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/Rotation_Pitch.stl"/>
</geometry>
<material name="orange"/>
</visual>
<visual>
<geometry>
<mesh filename="meshes/Rotation_Pitch_Motor.stl"/>
</geometry>
<material name="black"/>
</visual>
<collision>
<geometry>
<mesh filename="meshes/Rotation_Pitch.stl"/>
</geometry>
</collision>
</link>
<!-- upper arm link -->
<link name="Upper_Arm">
<inertial>
<mass value="0.162409"/>
<origin xyz="-1.72052e-05 0.0701802 0.00310545" rpy="0 0 0"/>
<inertia ixx="0.000213312" ixy="0" ixz="0" iyy="0.000167164" iyz="0" izz="7.01522e-05"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/Upper_Arm.stl"/>
</geometry>
<material name="orange"/>
</visual>
<visual>
<geometry>
<mesh filename="meshes/Upper_Arm_Motor.stl"/>
</geometry>
<material name="black"/>
</visual>
<collision>
<geometry>
<mesh filename="meshes/Upper_Arm.stl"/>
</geometry>
</collision>
</link>
<!-- lower arm link -->
<link name="Lower_Arm">
<inertial>
<mass value="0.147968"/>
<origin xyz="-0.00339604 0.00137796 0.0768007" rpy="0 0 0"/>
<inertia ixx="0.000138803" ixy="0" ixz="0" iyy="0.000107748" iyz="0" izz="4.84242e-05"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/Lower_Arm.stl"/>
</geometry>
<material name="orange"/>
</visual>
<visual>
<geometry>
<mesh filename="meshes/Lower_Arm_Motor.stl"/>
</geometry>
<material name="black"/>
</visual>
<collision>
<geometry>
<mesh filename="meshes/Lower_Arm.stl"/>
</geometry>
</collision>
</link>
<!-- wrist pitch-roll link -->
<link name="Wrist_Pitch_Roll">
<inertial>
<mass value="0.0661321"/>
<origin xyz="-0.00852653 -0.0352279 -2.34622e-05" rpy="0 0 0"/>
<inertia ixx="3.45403e-05" ixy="0" ixz="0" iyy="2.39041e-05" iyz="0" izz="1.94704e-05"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/Wrist_Pitch_Roll.stl"/>
</geometry>
<material name="orange"/>
</visual>
<visual>
<geometry>
<mesh filename="meshes/Wrist_Pitch_Roll_Motor.stl"/>
</geometry>
<material name="black"/>
</visual>
<collision>
<geometry>
<mesh filename="meshes/Wrist_Pitch_Roll.stl"/>
</geometry>
</collision>
</link>
<!-- fixed jaw link -->
<link name="Fixed_Jaw">
<inertial>
<mass value="0.0929859"/>
<origin xyz="0.00552377 -0.0280167 0.000483583" rpy="0 0 0"/>
<inertia ixx="5.03136e-05" ixy="0" ixz="0" iyy="4.64098e-05" iyz="0" izz="2.72961e-05"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/Fixed_Jaw.stl"/>
</geometry>
<material name="orange"/>
</visual>
<visual>
<geometry>
<mesh filename="meshes/Fixed_Jaw_Motor.stl"/>
</geometry>
<material name="black"/>
</visual>
</link>
<!-- moving jaw link -->
<link name="Moving_Jaw">
<inertial>
<mass value="0.0202444"/>
<origin xyz="-0.00161745 -0.0303473 0.000449646" rpy="0 0 0"/>
<inertia ixx="1.11265e-05" ixy="0" ixz="0" iyy="8.99651e-06" iyz="0" izz="2.99548e-06"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/Moving_Jaw.stl"/>
</geometry>
<material name="orange"/>
</visual>
</link>
<!-- Camera link attached to Fixed_Jaw -->
<link name="wrist_camera">
<visual>
<geometry>
<box size="0.02 0.01 0.01"/> <!-- Small camera representation -->
</geometry>
<material name="black"/>
</visual>
<inertial>
<mass value="0.005"/> <!-- ~5g for small camera module -->
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
</inertial>
</link>
<!-- joints -->
<joint name="Rotation" type="revolute">
<parent link="Base"/>
<child link="Rotation_Pitch"/>
<origin xyz="0 -0.0452 0.0165" rpy="1.57079 0 0"/>
<axis xyz="0 -1 0"/>
<limit lower="-2" upper="2" effort="35" velocity="1"/>
<safety_controller>
<!-- soft limits inside hard limits -->
<soft_lower_limit>-1.9</soft_lower_limit>
<soft_upper_limit>1.9</soft_upper_limit>
</safety_controller>
</joint>
<joint name="Pitch" type="revolute">
<parent link="Rotation_Pitch"/>
<child link="Upper_Arm"/>
<origin xyz="0 0.1025 0.0306" rpy="-1.8 0 0"/>
<axis xyz="1 0 0"/>
<limit lower="0" upper="3.5" effort="35" velocity="1"/>
<safety_controller>
<soft_lower_limit>0.1</soft_lower_limit>
<soft_upper_limit>3.4</soft_upper_limit>
</safety_controller>
</joint>
<joint name="Elbow" type="revolute">
<parent link="Upper_Arm"/>
<child link="Lower_Arm"/>
<origin xyz="0 0.11257 0.028" rpy="1.57079 0 0"/>
<axis xyz="1 0 0"/>
<limit lower="-3.14158" upper="0" effort="35" velocity="1"/>
<safety_controller>
<soft_lower_limit>-3.0</soft_lower_limit>
<soft_upper_limit>-0.1</soft_upper_limit>
</safety_controller>
</joint>
<joint name="Wrist_Pitch" type="revolute">
<parent link="Lower_Arm"/>
<child link="Wrist_Pitch_Roll"/>
<origin xyz="0 0.0052 0.1349" rpy="-1 0 0"/>
<axis xyz="1 0 0"/>
<limit lower="-2.5" upper="1.2" effort="35" velocity="1"/>
<safety_controller>
<soft_lower_limit>-2.4</soft_lower_limit>
<soft_upper_limit>1.1</soft_upper_limit>
</safety_controller>
</joint>
<joint name="Wrist_Roll" type="revolute">
<parent link="Wrist_Pitch_Roll"/>
<child link="Fixed_Jaw"/>
<origin xyz="0 -0.0601 0" rpy="0 1.57079 0"/>
<axis xyz="0 1 0"/>
<limit lower="-3.14158" upper="3.14158" effort="35" velocity="1"/>
<safety_controller>
<soft_lower_limit>-3.0</soft_lower_limit>
<soft_upper_limit>3.0</soft_upper_limit>
</safety_controller>
</joint>
<joint name="Jaw" type="revolute">
<parent link="Fixed_Jaw"/>
<child link="Moving_Jaw"/>
<origin xyz="-0.0202 -0.0244 0" rpy="0 3.14158 -0.18"/>
<axis xyz="0 0 1"/>
<limit lower="-0.2" upper="2.0" effort="35" velocity="1"/>
<safety_controller>
<soft_lower_limit>-0.1</soft_lower_limit>
<soft_upper_limit>1.9</soft_upper_limit>
</safety_controller>
</joint>
<joint name="camera_mount" type="fixed">
<parent link="Fixed_Jaw"/>
<child link="wrist_camera"/>
<origin xyz="0.005 -0.025 0.03" rpy="1.5708 1.5708 0"/> <!-- Side-mounted, looking forward -->
</joint>
</robot>