274 lines
15 KiB
XML
274 lines
15 KiB
XML
<mujoco model="dual_arm">
|
||
<compiler angle="radian" meshdir="meshes/"/>
|
||
|
||
<default>
|
||
<joint damping="10" armature="0.001"/>
|
||
</default>
|
||
|
||
<visual>
|
||
<global offwidth="640" offheight="480"/>
|
||
<!-- 可选：明确深度裁剪范围，后面如果你用 depth buffer 转米会更稳定 -->
|
||
<map znear="0.02" zfar="5"/>
|
||
</visual>
|
||
|
||
<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"
|
||
rgba="1 1 1.0 1"
|
||
emission="0.9"
|
||
specular="0.5"
|
||
shininess="1"
|
||
reflectance="0.3"/>
|
||
|
||
<texture name="apriltag_tex" type="2d" file="../april_tag/tag36_11.png"/>
|
||
<material name="apriltag_mat"
|
||
texture="apriltag_tex"
|
||
texrepeat="1 1"
|
||
rgba="1 1 1 1"
|
||
emission="1"
|
||
specular="0"
|
||
shininess="0"/>
|
||
|
||
</asset>
|
||
|
||
<worldbody>
|
||
<body name="tag_board" pos="0.7 -0.2 1.1" euler="1.57 -1.57 0">
|
||
|
||
<!-- 15cm x 15cm 白板，同时贴 apriltag 纹理（纹理里自带白边） -->
|
||
<geom name="tag_board_geom"
|
||
type="box"
|
||
size="0.075 0.075 0.0005"
|
||
material="apriltag_mat"
|
||
rgba="1 1 1 1"
|
||
contype="0" conaffinity="0"/>
|
||
</body>
|
||
|
||
<geom size="0.05 0.6" pos="0 0 0.6" type="cylinder" contype="0" conaffinity="0" group="1" density="0"/>
|
||
<geom size="0.05 0.6" pos="0 0 0.6" type="cylinder"/>
|
||
<body name="PELVIS_S" pos="0 0 1.2">
|
||
<geom pos="0 0 0" quat="1 0 0 0" type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.698039 0.698039 0.698039 1" mesh="PELVIS_S"/>
|
||
<geom pos="0 0 0" quat="1 0 0 0" 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 0.26" 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="-1.57 0.78" 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="-3.14 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="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="-3.14 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="-3.14 3.14" 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="-0.78 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_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="-3.14 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 3.14" 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"/>
|
||
|
||
<!-- ====== 新增：可用于雅可比的 sites（来自你的 URDF fixed joint）====== -->
|
||
<!-- 指尖/末端 marker（URDF: R_FINGER_TIP_FIXED） -->
|
||
<site name="R_FINGER_TIP_SITE"
|
||
pos="0.00684256 -0.284077 0.00801525"
|
||
size="0.006"
|
||
rgba="0 1 1 1"/>
|
||
|
||
<!-- 相机 marker site（URDF: R_CAM_FIXED xyz+rpy） -->
|
||
<!-- 相机看向前方时，R_CAM_SITE 的 -Z 指向前方 -->
|
||
<site name="R_CAM_SITE"
|
||
pos="-0.01212 -0.17655 0.07506"
|
||
quat="3.17467e-11 -3.17467e-11 0.707107 0.707107"
|
||
size="0.006"
|
||
rgba="0 1 0 1"/>
|
||
|
||
<!-- 真正可渲染相机：用这个名字去 mj_name2id(m, mjOBJ_CAMERA,"hand_cam") -->
|
||
<camera name="hand_cam"
|
||
pos="-0.01212 -0.17655 0.07506"
|
||
quat="3.17467e-11 -3.17467e-11 0.707107 0.707107"
|
||
fovy="60"/>
|
||
|
||
<!-- ====== 原来的几何体（保持不变）====== -->
|
||
<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"/>
|
||
|
||
<!-- 这两个小球就是 URDF 的 R_FINGER_TIP 可视/碰撞球（建议关碰撞，避免干扰） -->
|
||
<geom type="sphere" size="0.004" pos="0.00684256 -0.284077 0.00801525"
|
||
contype="0" conaffinity="0" rgba="0 1 1 1"/>
|
||
<geom type="sphere" size="0.005" pos="0.00684256 -0.284077 0.00801525"
|
||
contype="0" conaffinity="0" rgba="0 1 1 1"/>
|
||
|
||
<!-- 这两个小球就是 URDF 的 R_CAM 可视/碰撞球（同样建议关碰撞） -->
|
||
<geom type="sphere" 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" rgba="0 1 0 1"/>
|
||
<geom type="sphere" size="0.005" pos="-0.01212 -0.17655 0.07506"
|
||
quat="3.17467e-11 -3.17467e-11 -0.707107 0.707107"
|
||
contype="0" conaffinity="0" rgba="0 1 0 1"/>
|
||
</body>
|
||
|
||
</body>
|
||
</body>
|
||
</body>
|
||
</body>
|
||
</body>
|
||
</body>
|
||
</body>
|
||
|
||
<light name="top_light"
|
||
mode="track"
|
||
directional="true"
|
||
diffuse="0.4 0.4 0.4"
|
||
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>
|
||
|
||
|
||
<!-- 关节空间 PD 位置控制：ctrl = 目标关节角度 (rad) -->
|
||
<actuator>
|
||
<!-- 左臂：位置控制 -->
|
||
<position name="L_SHOULDER_P_pos" joint="L_SHOULDER_P"
|
||
kp="200" ctrlrange="-1.57 0.26"/>
|
||
<position name="L_SHOULDER_R_pos" joint="L_SHOULDER_R"
|
||
kp="200" ctrlrange="-1.57 0.78"/>
|
||
<position name="L_SHOULDER_Y_pos" joint="L_SHOULDER_Y"
|
||
kp="200" ctrlrange="-3.14 3.14"/>
|
||
|
||
<position name="L_ELBOW_R_pos" joint="L_ELBOW_R"
|
||
kp="150" ctrlrange="-2.05 0.00"/>
|
||
|
||
<position name="L_WRIST_P_pos" joint="L_WRIST_P"
|
||
kp="80" ctrlrange="-3.14 3.14"/>
|
||
<position name="L_WRIST_Y_pos" joint="L_WRIST_Y"
|
||
kp="80" ctrlrange="-0.78 0.78"/>
|
||
<position name="L_WRIST_R_pos" joint="L_WRIST_R"
|
||
kp="80" ctrlrange="-1.57 0.26"/>
|
||
|
||
<!-- 右臂：位置控制 -->
|
||
<position name="R_SHOULDER_P_pos" joint="R_SHOULDER_P"
|
||
kp="2000" ctrlrange="-3.14 3.14"/>
|
||
<position name="R_SHOULDER_R_pos" joint="R_SHOULDER_R"
|
||
kp="2000" ctrlrange="-0.78 1.57"/>
|
||
<position name="R_SHOULDER_Y_pos" joint="R_SHOULDER_Y"
|
||
kp="2000" ctrlrange="-3.14 3.14"/>
|
||
|
||
<position name="R_ELBOW_R_pos" joint="R_ELBOW_R"
|
||
kp="1500" ctrlrange="0 2.05"/>
|
||
|
||
<position name="R_WRIST_P_pos" joint="R_WRIST_P"
|
||
kp="800" ctrlrange="-3.14 3.14"/>
|
||
<position name="R_WRIST_Y_pos" joint="R_WRIST_Y"
|
||
kp="800" ctrlrange="-0.78 0.78"/>
|
||
<position name="R_WRIST_R_pos" joint="R_WRIST_R"
|
||
kp="800" ctrlrange="-0.26 1.57"/>
|
||
</actuator>
|
||
|
||
|
||
|
||
</mujoco>
|