348 lines
16 KiB
XML
348 lines
16 KiB
XML
<mujoco model="dual_arm">
|
||
<compiler angle="radian" meshdir="meshes/"/>
|
||
|
||
<!-- 全局默认：给所有关节一点阻尼和等效转子惯量 -->
|
||
<default>
|
||
<joint damping="10" armature="0.001"/>
|
||
</default>
|
||
|
||
|
||
|
||
<!-- 稳定一点的时间步和积分器 -->
|
||
<!-- <option timestep="0.001" integrator="RK4"/>-->
|
||
|
||
<asset>
|
||
<mesh name="PELVIS_S" file="PELVIS_S.STL"/>
|
||
<mesh name="L_SHOULDER_P_S" file="L_SHOULDER_P_S.STL"/>
|
||
<mesh name="L_SHOULDER_R_S" file="L_SHOULDER_R_S.STL"/>
|
||
<mesh name="L_SHOULDER_Y_S" file="L_SHOULDER_Y_S.STL"/>
|
||
<mesh name="L_ELBOW_R_S" file="L_ELBOW_R_S.STL"/>
|
||
<mesh name="L_WRIST_P_S" file="L_WRIST_P_S.STL"/>
|
||
<mesh name="L_WRIST_Y_S" file="L_WRIST_Y_S.STL"/>
|
||
<mesh name="L_WRIST_R_S" file="L_WRIST_R_S.STL"/>
|
||
<mesh name="R_SHOULDER_P_S" file="R_SHOULDER_P_S.STL"/>
|
||
<mesh name="R_SHOULDER_R_S" file="R_SHOULDER_R_S.STL"/>
|
||
<mesh name="R_SHOULDER_Y_S" file="R_SHOULDER_Y_S.STL"/>
|
||
<mesh name="R_ELBOW_R_S" file="R_ELBOW_R_S.STL"/>
|
||
<mesh name="R_WRIST_P_S" file="R_WRIST_P_S.STL"/>
|
||
<mesh name="R_WRIST_Y_S" file="R_WRIST_Y_S.STL"/>
|
||
<mesh name="R_WRIST_R_S" file="R_WRIST_R_S.STL"/>
|
||
|
||
|
||
<!-- 天空背景，可要可不要 -->
|
||
<texture name="skybox" type="skybox"
|
||
builtin="gradient"
|
||
rgb1="0.4 0.6 0.8"
|
||
rgb2="0 0 0"
|
||
width="512" height="512"/>
|
||
|
||
<!-- 蓝色方格地面纹理：builtin="checker" -->
|
||
<texture name="grid"
|
||
type="2d"
|
||
builtin="checker"
|
||
width="512" height="512"
|
||
rgb1="0.2 0.3 0.4"
|
||
rgb2="0.1 0.2 0.3"
|
||
mark="cross"
|
||
markrgb="0.8 0.8 0.8"/>
|
||
|
||
<!-- 地面材质，绑定上面的纹理 -->
|
||
<material name="grid_floor"
|
||
texture="grid"
|
||
texrepeat="5 5"
|
||
reflectance="0.2"/>
|
||
</asset>
|
||
|
||
<worldbody>
|
||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
|
||
rgba="0.698039 0.698039 0.698039 1" mesh="PELVIS_S"/>
|
||
<geom type="mesh" rgba="0.698039 0.698039 0.698039 1" mesh="PELVIS_S"/>
|
||
|
||
<body name="L_SHOULDER_P_S" pos="0 0.0945 0.042">
|
||
<inertial pos="-0.00982259 0.0704593 1.15262e-06"
|
||
quat="0.706163 0.705933 -0.0386962 -0.0386741"
|
||
mass="0.880738"
|
||
diaginertia="0.000584874 0.000465648 0.000443849"/>
|
||
<joint name="L_SHOULDER_P" pos="0 0 0" axis="0 1 0"
|
||
range="-1.57 1.57" actuatorfrcrange="-120 120"/>
|
||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
|
||
rgba="0.898039 0.917647 0.929412 1" mesh="L_SHOULDER_P_S"/>
|
||
<geom type="mesh" rgba="0.898039 0.917647 0.929412 1"
|
||
mesh="L_SHOULDER_P_S"/>
|
||
|
||
<body name="L_SHOULDER_R_S" pos="0.035 0.0765 0">
|
||
<inertial pos="-0.0346025 0.0917393 -1.67085e-08"
|
||
quat="0.358778 0.609303 -0.358823 0.609323"
|
||
mass="0.594788"
|
||
diaginertia="0.000414771 0.000407636 0.000294296"/>
|
||
<joint name="L_SHOULDER_R" pos="0 0 0" axis="1 0 0"
|
||
range="-2 2" actuatorfrcrange="-120 120"/>
|
||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
|
||
rgba="0.890196 0.890196 0.913725 1" mesh="L_SHOULDER_R_S"/>
|
||
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1"
|
||
mesh="L_SHOULDER_R_S"/>
|
||
|
||
<body name="L_SHOULDER_Y_S" pos="-0.035 0.1475 0">
|
||
<inertial pos="-0.00440977 0.086362 9.50749e-09"
|
||
quat="0.705001 0.704998 -0.054559 -0.0545441"
|
||
mass="0.563406"
|
||
diaginertia="0.000329815 0.000297341 0.000211019"/>
|
||
<joint name="L_SHOULDER_Y" pos="0 0 0" axis="0 1 0"
|
||
range="-2.18 0" actuatorfrcrange="-80 80"/>
|
||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
|
||
rgba="0.890196 0.890196 0.913725 1" mesh="L_SHOULDER_Y_S"/>
|
||
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1"
|
||
mesh="L_SHOULDER_Y_S"/>
|
||
|
||
<body name="L_ELBOW_R_S" pos="0.034 0.1025 0">
|
||
<inertial pos="-0.0335624 0.06032 2.99656e-07"
|
||
quat="0.674756 0.674714 0.211333 0.211667"
|
||
mass="0.393572"
|
||
diaginertia="0.000189078 0.000181042 0.000139034"/>
|
||
<joint name="L_ELBOW_R" pos="0 0 0" axis="1 0 0"
|
||
range="-2.05 0" actuatorfrcrange="-50 50"/>
|
||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
|
||
rgba="0.890196 0.890196 0.913725 1" mesh="L_ELBOW_R_S"/>
|
||
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1"
|
||
mesh="L_ELBOW_R_S"/>
|
||
|
||
<body name="L_WRIST_P_S" pos="-0.034 0.0965 0">
|
||
<inertial pos="-1.39659e-10 0.0675973 0.0192006"
|
||
quat="0.467537 0.530481 -0.530481 0.467537"
|
||
mass="0.442332"
|
||
diaginertia="0.000489142 0.000476754 9.72811e-05"/>
|
||
<joint name="L_WRIST_P" pos="0 0 0" axis="0 1 0"
|
||
range="0 3.14" actuatorfrcrange="-50 50"/>
|
||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
|
||
rgba="0.698039 0.698039 0.698039 1" mesh="L_WRIST_P_S"/>
|
||
<geom type="mesh" rgba="0.698039 0.698039 0.698039 1"
|
||
mesh="L_WRIST_P_S"/>
|
||
|
||
<body name="L_WRIST_Y_S" pos="0 0.1525 0.039">
|
||
<inertial pos="-0.00464136 -5.06427e-10 -0.0341254"
|
||
quat="0.298107 0.641196 0.641196 0.298107"
|
||
mass="0.235738"
|
||
diaginertia="6.00636e-05 5.8497e-05 4.579e-05"/>
|
||
<joint name="L_WRIST_Y" pos="0 0 0" axis="0 0 1"
|
||
range="-0.78 0.78" actuatorfrcrange="-50 50"/>
|
||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
|
||
rgba="0.647059 0.619608 0.588235 1" mesh="L_WRIST_Y_S"/>
|
||
<geom type="mesh" rgba="0.647059 0.619608 0.588235 1"
|
||
mesh="L_WRIST_Y_S"/>
|
||
|
||
<body name="L_WRIST_R_S" pos="0.0258 0 -0.039">
|
||
<inertial pos="-0.0161477 0.0955047 -0.00499392"
|
||
quat="0.595488 0.369507 -0.591676 0.39847"
|
||
mass="0.504894"
|
||
diaginertia="0.000280921 0.000188575 0.000178055"/>
|
||
<joint name="L_WRIST_R" pos="0 0 0" axis="1 0 0"
|
||
range="-1.57 0.26" actuatorfrcrange="-50 50"/>
|
||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
|
||
rgba="0.890196 0.890196 0.913725 1" mesh="L_WRIST_R_S"/>
|
||
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1"
|
||
mesh="L_WRIST_R_S"/>
|
||
</body>
|
||
</body>
|
||
</body>
|
||
</body>
|
||
</body>
|
||
</body>
|
||
</body>
|
||
|
||
<body name="R_SHOULDER_P_S" pos="0 -0.0945 0.042">
|
||
<inertial pos="-0.00982259 -0.0704593 -1.1507e-06"
|
||
quat="0.706163 0.705933 0.0386962 0.0386741"
|
||
mass="0.880738"
|
||
diaginertia="0.000584874 0.000465648 0.000443849"/>
|
||
<joint name="R_SHOULDER_P" pos="0 0 0" axis="0 -1 0"
|
||
range="-1.57 1.57" actuatorfrcrange="-120 120"/>
|
||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
|
||
rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_P_S"/>
|
||
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1"
|
||
mesh="R_SHOULDER_P_S"/>
|
||
|
||
<body name="R_SHOULDER_R_S" pos="0.035 -0.0765 0">
|
||
<inertial pos="-0.0346025 -0.0917393 1.86281e-08"
|
||
quat="0.609323 0.358823 -0.609303 0.358778"
|
||
mass="0.594788"
|
||
diaginertia="0.000414771 0.000407636 0.000294296"/>
|
||
<joint name="R_SHOULDER_R" pos="0 0 0" axis="1 0 0"
|
||
range="-2 2" actuatorfrcrange="-120 120"/>
|
||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
|
||
rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_R_S"/>
|
||
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1"
|
||
mesh="R_SHOULDER_R_S"/>
|
||
|
||
<body name="R_SHOULDER_Y_S" pos="-0.035 -0.1475 0">
|
||
<inertial pos="-0.00440977 -0.086362 -7.58792e-09"
|
||
quat="0.705001 0.704998 0.054559 0.0545441"
|
||
mass="0.563406"
|
||
diaginertia="0.000329815 0.000297341 0.000211019"/>
|
||
<joint name="R_SHOULDER_Y" pos="0 0 0" axis="0 -1 0"
|
||
range="0 3.14" actuatorfrcrange="-80 80"/>
|
||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
|
||
rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_Y_S"/>
|
||
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1"
|
||
mesh="R_SHOULDER_Y_S"/>
|
||
|
||
<body name="R_ELBOW_R_S" pos="0.034 -0.1025 0">
|
||
<inertial pos="-0.0335624 -0.06032 -2.97736e-07"
|
||
quat="0.674756 0.674714 -0.211333 -0.211667"
|
||
mass="0.393572"
|
||
diaginertia="0.000189078 0.000181042 0.000139034"/>
|
||
<joint name="R_ELBOW_R" pos="0 0 0" axis="1 0 0"
|
||
range="0 2.05" actuatorfrcrange="-80 80"/>
|
||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
|
||
rgba="0.890196 0.890196 0.913725 1" mesh="R_ELBOW_R_S"/>
|
||
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1"
|
||
mesh="R_ELBOW_R_S"/>
|
||
|
||
<body name="R_WRIST_P_S" pos="-0.034 -0.0965 0">
|
||
<inertial pos="-1.39657e-10 -0.0675973 0.0192006"
|
||
quat="0.530481 0.467537 -0.467537 0.530481"
|
||
mass="0.442332"
|
||
diaginertia="0.000489142 0.000476754 9.72811e-05"/>
|
||
<joint name="R_WRIST_P" pos="0 0 0" axis="0 -1 0"
|
||
range="-3.14 0" actuatorfrcrange="-50 50"/>
|
||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
|
||
rgba="0.647059 0.619608 0.588235 1" mesh="R_WRIST_P_S"/>
|
||
<geom type="mesh" rgba="0.647059 0.619608 0.588235 1"
|
||
mesh="R_WRIST_P_S"/>
|
||
|
||
<body name="R_WRIST_Y_S" pos="0 -0.1525 0.039">
|
||
<inertial pos="-0.00464136 -5.06426e-10 -0.0341254"
|
||
quat="0.298107 0.641196 0.641196 0.298107"
|
||
mass="0.235738"
|
||
diaginertia="6.00636e-05 5.8497e-05 4.579e-05"/>
|
||
<joint name="R_WRIST_Y" pos="0 0 0" axis="0 0 1"
|
||
range="-0.78 0.78" actuatorfrcrange="-50 50"/>
|
||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
|
||
rgba="0.647059 0.619608 0.588235 1" mesh="R_WRIST_Y_S"/>
|
||
<geom type="mesh" rgba="0.647059 0.619608 0.588235 1"
|
||
mesh="R_WRIST_Y_S"/>
|
||
|
||
<body name="R_WRIST_R_S" pos="0.03 0 -0.039">
|
||
<inertial pos="-0.0201642 -0.11075 -0.00598955"
|
||
quat="0.483404 0.529786 -0.463203 0.520663"
|
||
mass="0.504366"
|
||
diaginertia="0.00027189 0.000186086 0.000130629"/>
|
||
<joint name="R_WRIST_R" pos="0 0 0" axis="1 0 0"
|
||
range="-0.26 1.57" actuatorfrcrange="-50 50"/>
|
||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
|
||
rgba="0.890196 0.890196 0.913725 1" mesh="R_WRIST_R_S"/>
|
||
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1"
|
||
mesh="R_WRIST_R_S"/>
|
||
|
||
<!-- 小标记点 -->
|
||
<geom size="0.004"
|
||
pos="0.00684256 -0.284077 0.00801525"
|
||
contype="0" conaffinity="0" group="1"
|
||
density="0" rgba="0 1 1 1"/>
|
||
<geom size="0.005"
|
||
pos="0.00684256 -0.284077 0.00801525"
|
||
rgba="0 1 1 1"/>
|
||
<geom size="0.004"
|
||
pos="-0.01212 -0.17655 0.07506"
|
||
quat="3.17467e-11 -3.17467e-11 -0.707107 0.707107"
|
||
contype="0" conaffinity="0" group="1"
|
||
density="0" rgba="0 1 0 1"/>
|
||
<geom size="0.005"
|
||
pos="-0.01212 -0.17655 0.07506"
|
||
quat="3.17467e-11 -3.17467e-11 -0.707107 0.707107"
|
||
rgba="0 1 0 1"/>
|
||
</body>
|
||
</body>
|
||
</body>
|
||
</body>
|
||
</body>
|
||
</body>
|
||
</body>
|
||
|
||
<light name="top_light"
|
||
mode="track"
|
||
directional="true"
|
||
diffuse="1 1 1"
|
||
specular="0.3 0.3 0.3"
|
||
pos="0 0 3"
|
||
dir="0 0 -1"/>
|
||
|
||
<geom name="floor"
|
||
type="plane"
|
||
pos="0 0 0"
|
||
size="0 0 0.05"
|
||
material="grid_floor"
|
||
condim="3"
|
||
friction="1 0.005 0.0001"/>
|
||
</worldbody>
|
||
|
||
<!-- <actuator>-->
|
||
<!-- <!– 左臂：力矩控制，ctrl = 关节力矩 (Nm) –>-->
|
||
<!-- <motor name="L_SHOULDER_P_torque" joint="L_SHOULDER_P"-->
|
||
<!-- gear="1" ctrlrange="-200 200"/>-->
|
||
<!-- <motor name="L_SHOULDER_R_torque" joint="L_SHOULDER_R"-->
|
||
<!-- gear="1" ctrlrange="-200 200"/>-->
|
||
<!-- <motor name="L_SHOULDER_Y_torque" joint="L_SHOULDER_Y"-->
|
||
<!-- gear="1" ctrlrange="-150 150"/>-->
|
||
<!-- <motor name="L_ELBOW_R_torque" joint="L_ELBOW_R"-->
|
||
<!-- gear="1" ctrlrange="-120 120"/>-->
|
||
<!-- <motor name="L_WRIST_P_torque" joint="L_WRIST_P"-->
|
||
<!-- gear="1" ctrlrange="-80 80"/>-->
|
||
<!-- <motor name="L_WRIST_Y_torque" joint="L_WRIST_Y"-->
|
||
<!-- gear="1" ctrlrange="-80 80"/>-->
|
||
<!-- <motor name="L_WRIST_R_torque" joint="L_WRIST_R"-->
|
||
<!-- gear="1" ctrlrange="-80 80"/>-->
|
||
|
||
<!-- <!– 右臂：力矩控制 –>-->
|
||
<!-- <motor name="R_SHOULDER_P_torque" joint="R_SHOULDER_P"-->
|
||
<!-- gear="1" ctrlrange="-200 200"/>-->
|
||
<!-- <motor name="R_SHOULDER_R_torque" joint="R_SHOULDER_R"-->
|
||
<!-- gear="1" ctrlrange="-200 200"/>-->
|
||
<!-- <motor name="R_SHOULDER_Y_torque" joint="R_SHOULDER_Y"-->
|
||
<!-- gear="1" ctrlrange="-150 150"/>-->
|
||
<!-- <motor name="R_ELBOW_R_torque" joint="R_ELBOW_R"-->
|
||
<!-- gear="1" ctrlrange="-120 120"/>-->
|
||
<!-- <motor name="R_WRIST_P_torque" joint="R_WRIST_P"-->
|
||
<!-- gear="1" ctrlrange="-80 80"/>-->
|
||
<!-- <motor name="R_WRIST_Y_torque" joint="R_WRIST_Y"-->
|
||
<!-- gear="1" ctrlrange="-80 80"/>-->
|
||
<!-- <motor name="R_WRIST_R_torque" joint="R_WRIST_R"-->
|
||
<!-- gear="1" ctrlrange="-80 80"/>-->
|
||
<!-- </actuator>-->
|
||
|
||
<actuator>
|
||
<!-- 左臂：位置控制，ctrl = 关节目标角度 (rad) -->
|
||
<position name="L_SHOULDER_P_pos" joint="L_SHOULDER_P"
|
||
kp="40" ctrlrange="-1.57 1.57"/>
|
||
<position name="L_SHOULDER_R_pos" joint="L_SHOULDER_R"
|
||
kp="40" ctrlrange="-1.57 1.57"/>
|
||
<position name="L_SHOULDER_Y_pos" joint="L_SHOULDER_Y"
|
||
kp="30" ctrlrange="-1.57 1.57"/>
|
||
<position name="L_ELBOW_R_pos" joint="L_ELBOW_R"
|
||
kp="30" ctrlrange="-2.09 0.0"/>
|
||
<position name="L_WRIST_P_pos" joint="L_WRIST_P"
|
||
kp="20" ctrlrange="-1.57 1.57"/>
|
||
<position name="L_WRIST_Y_pos" joint="L_WRIST_Y"
|
||
kp="20" ctrlrange="-1.57 1.57"/>
|
||
<position name="L_WRIST_R_pos" joint="L_WRIST_R"
|
||
kp="20" ctrlrange="-1.57 1.57"/>
|
||
|
||
<!-- 右臂：位置控制 -->
|
||
<position name="R_SHOULDER_P_pos" joint="R_SHOULDER_P"
|
||
kp="40" ctrlrange="-1.57 1.57"/>
|
||
<position name="R_SHOULDER_R_pos" joint="R_SHOULDER_R"
|
||
kp="40" ctrlrange="-1.57 1.57"/>
|
||
<position name="R_SHOULDER_Y_pos" joint="R_SHOULDER_Y"
|
||
kp="30" ctrlrange="-1.57 1.57"/>
|
||
<position name="R_ELBOW_R_pos" joint="R_ELBOW_R"
|
||
kp="40" ctrlrange="0 2.05"/>
|
||
<position name="R_WRIST_P_pos" joint="R_WRIST_P"
|
||
kp="20" ctrlrange="-1.57 1.57"/>
|
||
<position name="R_WRIST_Y_pos" joint="R_WRIST_Y"
|
||
kp="20" ctrlrange="-1.57 1.57"/>
|
||
<position name="R_WRIST_R_pos" joint="R_WRIST_R"
|
||
kp="20" ctrlrange="-1.57 1.57"/>
|
||
</actuator>
|
||
|
||
|
||
</mujoco>
|