445 lines
36 KiB
XML
445 lines
36 KiB
XML
<mujoco model="DianaMed">
|
|
<compiler angle="radian"/>
|
|
|
|
<visual>
|
|
<scale
|
|
framewidth="0.01"
|
|
camera="0.03"
|
|
/>
|
|
|
|
<rgba haze="0.15 0.25 0.35 1"/>
|
|
</visual>
|
|
|
|
<asset>
|
|
<mesh name="base_link" file="meshes/base_link.STL"/>
|
|
<mesh name="link1" file="meshes/link1.STL"/>
|
|
<mesh name="link2" file="meshes/link2.STL"/>
|
|
<mesh name="link3" file="meshes/link3.STL"/>
|
|
<mesh name="link4" file="meshes/link4.STL"/>
|
|
<mesh name="link5" file="meshes/link5.STL"/>
|
|
<mesh name="link6" file="meshes/link6.STL"/>
|
|
<mesh name="link7" file="meshes/link7.STL"/>
|
|
<mesh name="realsense_cam" file="meshes/cam.STL" />
|
|
</asset>
|
|
<!-- gripper -->
|
|
<asset>
|
|
<mesh name="electric_gripper_base" file="meshes/electric_gripper_base.stl" />
|
|
<mesh name="standard_narrow" file="meshes/standard_narrow.stl" />
|
|
<mesh name="half_round_tip" file= "meshes/half_round_tip.stl" />
|
|
<mesh name="connector_plate" file="meshes/connector_plate.stl" />
|
|
</asset>
|
|
<worldbody>
|
|
<body name="cylindrical_base_left">
|
|
<geom pos="-0.8 1.0 0.22" contype="1" conaffinity="1" group="1" euler = "0 0 0" type="cylinder" rgba="0.8 0.8 0.9 1" size="0.107 0.22" friction="0.9 0.9 0.9"/>
|
|
</body>
|
|
<body name="left_base_link" pos="-0.8 1.0 0.44" quat="1 0 0 0">
|
|
<geom type="mesh" contype="1" conaffinity="1" group="1" pos="0 0 0" rgba="0.4 0.4 0.4 1" mesh="base_link"/>
|
|
<body name="left_link1" pos="8.7058e-05 -0.00063474 0.2985" quat="-3.67321e-06 1 0 0">
|
|
<inertial pos="4.57641e-06 -0.0439746 0.0236145" quat="0.685631 -0.172505 0.172644 0.685818" mass="2.72698" diaginertia="0.0122186 0.0113371 0.00692616"/>
|
|
<joint name="l_j1" pos="0 0 0" axis="0 0 1" limited="true" range="-2.56 2.56" damping="0.1"/>
|
|
<geom type="mesh" contype="1" conaffinity="1" group="1" rgba="0.4 0.4 0.4 1" mesh="link1"/>
|
|
<body name="left_link2" pos="0 0 -0.00092116" quat="0.707105 0.707108 0 0">
|
|
<inertial pos="8.26088e-06 -0.040728 -0.0363117" quat="0.596612 0.379593 -0.379572 0.596564" mass="3.18168" diaginertia="0.0189254 0.0180721 0.00757505"/>
|
|
<joint name="l_j2" pos="0 0 0" axis="0 0 1" limited="true" range="-1.57 1.57" damping="0.1"/>
|
|
<geom type="mesh" contype="1" conaffinity="1" group="1" rgba="0.4 0.4 0.4 1" mesh="link2"/>
|
|
<body name="left_link3" pos="0 -0.45818 0.0005" quat="0.707105 -0.707108 0 0">
|
|
<inertial pos="0.0148079 -0.0159925 0.127198" quat="0.874546 -0.136758 -0.0611978 0.461217" mass="3.93616" diaginertia="0.0569273 0.0547312 0.00935482"/>
|
|
<joint name="l_j3" pos="0 0 0" axis="0 0 1" limited="true" range="-2.56 2.56" damping="0.1"/>
|
|
<geom type="mesh" contype="1" conaffinity="1" group="1" rgba="0.4 0.4 0.4 1" mesh="link3"/>
|
|
<body name="left_link4" pos="0.064454 0.0005 -0.00070508" quat="0.707105 0.707108 0 0">
|
|
<inertial pos="-0.0145884 -0.0117113 -0.0406158" quat="0.00473736 0.651043 0.564294 0.507635" mass="1.58469" diaginertia="0.00499445 0.00482606 0.00267104"/>
|
|
<joint name="l_j4" pos="0 0 0" axis="0 0 1" limited="true" range="0.0 3.05" damping="0.1"/>
|
|
<geom type="mesh" contype="1" conaffinity="1" group="1" rgba="0.4 0.4 0.4 1" mesh="link4"/>
|
|
<body name="left_link5" pos="-0.052254 -0.45668 0.0005" quat="0.707105 -0.707108 0 0">
|
|
<inertial pos="-0.00209922 0.0288297 0.222845" quat="0.985159 0.138333 -0.00891494 0.101222" mass="1.92688" diaginertia="0.0293358 0.0290794 0.00210656"/>
|
|
<joint name="l_j5" pos="0 0 0" axis="0 0 1" limited="true" range="-3.05 3.05" damping="0.1"/>
|
|
<geom type="mesh" contype="1" conaffinity="1" group="1" rgba="0.4 0.4 0.4 1" mesh="link5"/>
|
|
<body name="left_link6" pos="0 -0.005 -1.1713e-05" quat="-2.59734e-06 -2.59735e-06 -0.707108 -0.707105">
|
|
<inertial pos="0.038358 -0.00071885 -0.0021112" quat="0.0131349 0.678937 -0.0198942 0.733809" mass="1.7304" diaginertia="0.005268 0.00492614 0.00345065"/>
|
|
<joint name="l_j6" pos="0 0 0" axis="0 0 1" limited="true" range="-3.14 3.14" damping="0.01"/>
|
|
<geom type="mesh" contype="1" conaffinity="1" group="1" rgba="0.4 0.4 0.4 1" mesh="link6"/>
|
|
<body name="left_link7" pos="0.076728 -0.1059 0" quat="0.707105 -0.707108 0 0">
|
|
<joint name="l_j7" pos="0 0 0" axis="0 0 1" limited="true" range="-3.14 3.14" damping="0.01"/>
|
|
<inertial pos="0.00068467 1.5999e-05 0.020834" quat="0.000547318 0.708756 -0.000448023 0.705453" mass="0.16696" diaginertia="0.000143911 8.8902e-05 8.75358e-05"/>
|
|
<geom type="mesh" contype="1" conaffinity="1" group="1" rgba="0.4 0.4 0.4 1" mesh="link7"/>
|
|
<!-- <geom type="mesh" rgba="1 1 1 1" mesh="link7"/> -->
|
|
<body name="gripper_base_left" pos="0 0 0" quat="0 1 0 0">
|
|
<inertial pos="0 0 0" quat="-0.5 0.5 0.5 0.5" mass="0.0001" diaginertia="3e-08 2e-08 2e-08" />
|
|
<geom type="mesh" contype="1" conaffinity="1" group="1" mesh="connector_plate" name="connector_plate_left" pos="0 0 0.0018" rgba="0.09 0.09 0.09 1"/>
|
|
<geom type="mesh" contype="1" conaffinity="1" group="1" mesh="electric_gripper_base" name="electric_gripper_base_left" pos="0 0 0.0194" rgba="0.09 0.09 0.09 1"/>
|
|
<geom size="0.029 0.05" quat="0 0 0.707107 0.707107" type="cylinder" group="0" name="gripper_base_col_left" pos="0.004 0.0 0.04"/>
|
|
<!-- This site was added for visualization. -->
|
|
<body name="eef_left" pos="0 0 0.109" quat="0.707105 0 0 -0.707105">
|
|
<!-- <site name="grip_site_left" pos="0 0 0" size="0.01 0.01 0.01" rgba="0 1 0 0.5" type="sphere" group="1"/> -->
|
|
<!-- This site was added for visualization. -->
|
|
<!-- <site name="grip_site_cylinder" pos="0 0 0" size="0.005 10" rgba="0 1 0 0.3" type="cylinder" group="1"/> -->
|
|
<body name="ee_cam_left" pos="0.00 0.046 -0.075" euler="0.0 0.0 -0.0">
|
|
<inertial pos="0 0 0" quat="1 0 0 0" mass="0" diaginertia="0 0 0" />
|
|
<geom type="mesh" contype="1" conaffinity="1" group="1" rgba="0.69804 0.69804 0.69804 1" mesh="realsense_cam" />
|
|
<camera name="rs_cam_left" mode="fixed" pos="0.0 0.0 0.01" euler="0.0 9.4 0.0 " fovy="50" resolution="1920 1200"/>
|
|
</body>
|
|
</body>
|
|
<body name="l_finger_left" pos="0 0.01 0.0444">
|
|
<inertial pos="0 0 0" quat="0 0 0 -1" mass="0.0001" diaginertia="0.001 0.001 0.001" />
|
|
<joint name="l_finger_joint_left" pos="0 0 0" axis="0 1 0" type="slide" limited="true" range="-0.0115 0.020833" damping="100" armature="1.0" frictionloss="1.0"/>
|
|
<geom name="l_finger_left" quat="0 0 0 -1" type="mesh" contype="1" conaffinity="1" group="1" mesh="standard_narrow" rgba="0.09 0.09 0.09 1"/>
|
|
<geom size="0.005 0.00675 0.0375" pos="0 0.01725 0.04" quat="0 0 0 -1" type="box" group="0" conaffinity="1" contype="1" name="l_finger_g0_left" friction="0 0 0"/>
|
|
<geom size="0.005 0.025 0.0085" pos="-0.005 -0.003 0.0083" quat="0 0 0 -1" type="box" group="0" conaffinity="1" contype="1" name="l_finger_g1_left" friction="0 0 0"/>
|
|
<body name="l_finger_tip_left" pos="0 0.01725 0.075">
|
|
<inertial pos="0 0 0" quat="0 0 0 1" mass="0.0001" diaginertia="0.001 0.001 0.001" />
|
|
<geom name="l_fingertip_g0_vis_left" quat="0 0 0 1" type="mesh" contype="1" conaffinity="1" group="1" mesh="half_round_tip" rgba="0.09 0.09 0.09 1"/>
|
|
<geom size="0.004 0.004 0.0185" pos="0 -0.0045 -0.015" quat="0 0 0 -1" type="box" group="0" conaffinity="1" contype="1" name="l_fingertip_g0_left" friction="0.95 0.3 0.1"/>
|
|
<geom size="0.0035 0.004 0.0165" pos="0 -0.0047 -0.017" type="box" conaffinity="1" contype="1" name="l_fingerpad_g0_left" friction="0.95 0.3 0.1"/>
|
|
</body>
|
|
</body>
|
|
<body name="r_finger_left" pos="0 -0.01 0.0444">
|
|
<inertial pos="0 0 0" mass="0.0001" diaginertia="0.001 0.001 0.001" />
|
|
<joint name="r_finger_joint_left" pos="0 0 0" axis="0 -1 0" type="slide" limited="true" range="-0.020833 0.0115" damping="100" armature="1.0" frictionloss="1.0"/>
|
|
<geom name="r_finger_left" type="mesh" contype="1" conaffinity="1" group="1" mesh="standard_narrow" rgba="0.09 0.09 0.09 1"/>
|
|
<geom size="0.005 0.00675 0.0375" pos="0 -0.01725 0.04" type="box" group="0" conaffinity="1" contype="1" name="r_finger_g0_left" friction="0 0 0"/>
|
|
<geom size="0.005 0.025 0.0085" pos="0.005 0.003 0.0083" type="box" group="0" conaffinity="1" contype="1" name="r_finger_g1_left" friction="0 0 0"/>
|
|
<body name="r_finger_tip_left" pos="0 -0.01725 0.075">
|
|
<inertial pos="0 0 0" mass="0.0001" diaginertia="0.001 0.001 0.001" />
|
|
<geom name="r_fingertip_g0_vis_left" type="mesh" contype="1" conaffinity="1" group="1" mesh="half_round_tip" rgba="0.09 0.09 0.09 1"/>
|
|
<geom size="0.004 0.004 0.0185" pos="0 0.0045 -0.015" type="box" group="0" conaffinity="1" contype="1" name="r_fingertip_g0_left" friction="0.95 0.3 0.1"/>
|
|
<geom size="0.0035 0.004 0.0165" pos="0 0.0047 -0.017" type="box" conaffinity="1" contype="1" name="r_fingerpad_g0_left" friction="0.95 0.3 0.1"/>
|
|
|
|
</body>
|
|
</body>
|
|
</body>
|
|
</body>
|
|
</body>
|
|
</body>
|
|
</body>
|
|
</body>
|
|
</body>
|
|
</body>
|
|
</body>
|
|
<body name="cylindrical_base_right">
|
|
<geom pos="0.8 1.0 0.22" contype="1" conaffinity="1" group="1" euler = "0 0 0" type="cylinder" rgba="0.8 0.8 0.9 1" size="0.107 0.22" friction="0.9 0.9 0.9"/>
|
|
</body>
|
|
<body name="right_base_link" pos="0.8 1.0 0.44" quat="0 0 0 -1" >
|
|
<geom type="mesh" contype="1" conaffinity="1" group="1" pos="0 0 0" rgba="0.4 0.4 0.4 1" mesh="base_link" />
|
|
<body name="right_link1" pos="8.7058e-05 -0.00063474 0.2985" quat="0 1 0 0">
|
|
<inertial pos="4.57641e-06 -0.0439746 0.0236145" quat="0.685631 -0.172505 0.172644 0.685818" mass="2.72698" diaginertia="0.0122186 0.0113371 0.00692616"/>
|
|
<joint name="r_j1" pos="0 0 0" axis="0 0 1" limited="true" range="-2.56 2.56" damping="0.1"/>
|
|
<geom type="mesh" contype="1" conaffinity="1" group="1" rgba="0.4 0.4 0.4 1" mesh="link1"/>
|
|
<body name="right_link2" pos="0 0 -0.00092116" quat="0.707105 0.707108 0 0">
|
|
<inertial pos="8.26088e-06 -0.040728 -0.0363117" quat="0.596612 0.379593 -0.379572 0.596564" mass="3.18168" diaginertia="0.0189254 0.0180721 0.00757505"/>
|
|
<joint name="r_j2" pos="0 0 0" axis="0 0 1" limited="true" range="-1.57 1.57" damping="0.1"/>
|
|
<geom type="mesh" contype="1" conaffinity="1" group="1" rgba="0.4 0.4 0.4 1" mesh="link2"/>
|
|
<body name="right_link3" pos="0 -0.45818 0.0005" quat="0.707105 -0.707108 0 0">
|
|
<inertial pos="0.0148079 -0.0159925 0.127198" quat="0.874546 -0.136758 -0.0611978 0.461217" mass="3.93616" diaginertia="0.0569273 0.0547312 0.00935482"/>
|
|
<joint name="r_j3" pos="0 0 0" axis="0 0 1" limited="true" range="-2.56 2.56" damping="0.1"/>
|
|
<geom type="mesh" contype="1" conaffinity="1" group="1" rgba="0.4 0.4 0.4 1" mesh="link3"/>
|
|
<body name="right_link4" pos="0.064454 0.0005 -0.00070508" quat="0.707105 0.707108 0 0">
|
|
<inertial pos="-0.0145884 -0.0117113 -0.0406158" quat="0.00473736 0.651043 0.564294 0.507635" mass="1.58469" diaginertia="0.00499445 0.00482606 0.00267104"/>
|
|
<joint name="r_j4" pos="0 0 0" axis="0 0 1" limited="true" range="0.0 3.05" damping="0.1"/>
|
|
<geom type="mesh" contype="1" conaffinity="1" group="1" rgba="0.4 0.4 0.4 1" mesh="link4"/>
|
|
<body name="right_link5" pos="-0.052254 -0.45668 0.0005" quat="0.707105 -0.707108 0 0">
|
|
<inertial pos="-0.00209922 0.0288297 0.222845" quat="0.985159 0.138333 -0.00891494 0.101222" mass="1.92688" diaginertia="0.0293358 0.0290794 0.00210656"/>
|
|
<joint name="r_j5" pos="0 0 0" axis="0 0 1" limited="true" range="-3.05 3.05" damping="0.1"/>
|
|
<geom type="mesh" contype="1" conaffinity="1" group="1" rgba="0.4 0.4 0.4 1" mesh="link5"/>
|
|
<body name="right_link6" pos="0 -0.005 -1.1713e-05" quat="-2.59734e-06 -2.59735e-06 -0.707108 -0.707105">
|
|
<inertial pos="0.038358 -0.00071885 -0.0021112" quat="0.0131349 0.678937 -0.0198942 0.733809" mass="1.7304" diaginertia="0.005268 0.00492614 0.00345065"/>
|
|
<joint name="r_j6" pos="0 0 0" axis="0 0 1" limited="true" range="-3.14 3.14" damping="0.01"/>
|
|
<geom type="mesh" contype="1" conaffinity="1" group="1" rgba="0.4 0.4 0.4 1" mesh="link6"/>
|
|
<body name="right_link7" pos="0.076728 -0.1059 0" quat="0.707105 -0.707108 0 0">
|
|
<joint name="r_j7" pos="0 0 0" axis="0 0 1" limited="true" range="-3.14 3.14" damping="0.01"/>
|
|
<inertial pos="0.00068467 1.5999e-05 0.020834" quat="0.000547318 0.708756 -0.000448023 0.705453" mass="0.16696" diaginertia="0.000143911 8.8902e-05 8.75358e-05"/>
|
|
<geom type="mesh" contype="1" conaffinity="1" group="1" rgba="0.4 0.4 0.4 1" mesh="link7"/>
|
|
<!-- <geom type="mesh" rgba="1 1 1 1" mesh="link7"/> -->
|
|
<body name="gripper_base_right" pos="0 0 0" quat="0 1 0 0">
|
|
<inertial pos="0 0 0" quat="-0.5 0.5 0.5 0.5" mass="0.0001" diaginertia="3e-08 2e-08 2e-08" />
|
|
<geom type="mesh" contype="1" conaffinity="1" group="1" mesh="connector_plate" name="connector_plate_right" pos="0 0 0.0018" rgba="0.09 0.09 0.09 1"/>
|
|
<geom type="mesh" contype="1" conaffinity="1" group="1" mesh="electric_gripper_base" name="electric_gripper_base_right" pos="0 0 0.0194" rgba="0.09 0.09 0.09 1"/>
|
|
<geom size="0.029 0.05" quat="0 0 0.707107 0.707107" type="cylinder" group="0" name="gripper_base_col_right" pos="0.004 0.0 0.04"/>
|
|
<!-- This site was added for visualization. -->
|
|
<body name="eef_right" pos="0 0 0.109" quat="0.707105 0 0 -0.707105">
|
|
<!-- <site name="grip_site_right" pos="0 0 0" size="0.01 0.01 0.01" rgba="1 0 0 0.5" type="sphere" group="1"/> -->
|
|
<!-- This site was added for visualization. -->
|
|
<!-- <site name="grip_site_cylinder" pos="0 0 0" size="0.005 10" rgba="0 1 0 0.3" type="cylinder" group="1"/> -->
|
|
<body name="ee_cam_right" pos="0.00 0.046 -0.075" euler="0.0 0.0 -0.0">
|
|
<inertial pos="0 0 0" quat="1 0 0 0" mass="0" diaginertia="0 0 0" />
|
|
<geom type="mesh" contype="1" conaffinity="1" group="1" rgba="0.69804 0.69804 0.69804 1" mesh="realsense_cam" />
|
|
<camera name="rs_cam_right" mode="fixed" pos="0.0 0.0 0.01 " euler="0.0 9.4 0.0 " fovy="50" resolution="1920 1200"/>
|
|
</body>
|
|
</body>
|
|
<body name="l_finger_right" pos="0 0.01 0.0444">
|
|
<inertial pos="0 0 0" quat="0 0 0 -1" mass="0.0001" diaginertia="0.001 0.001 0.001" />
|
|
<joint name="l_finger_joint_right" pos="0 0 0" axis="0 1 0" type="slide" limited="true" range="-0.0115 0.020833" damping="100" armature="1.0" frictionloss="1.0"/>
|
|
<geom name="l_finger_right" quat="0 0 0 -1" type="mesh" contype="1" conaffinity="1" group="1" mesh="standard_narrow" rgba="0.09 0.09 0.09 1"/>
|
|
<geom size="0.005 0.00675 0.0375" pos="0 0.01725 0.04" quat="0 0 0 -1" type="box" group="0" conaffinity="1" contype="1" name="l_finger_g0_right" friction="0 0 0"/>
|
|
<geom size="0.005 0.025 0.0085" pos="-0.005 -0.003 0.0083" quat="0 0 0 -1" type="box" group="0" conaffinity="1" contype="1" name="l_finger_g1_right" friction="0 0 0"/>
|
|
<body name="l_finger_tip_right" pos="0 0.01725 0.075">
|
|
<inertial pos="0 0 0" quat="0 0 0 1" mass="0.0001" diaginertia="0.001 0.001 0.001" />
|
|
<geom name="l_fingertip_g0_vis_right" quat="0 0 0 1" type="mesh" contype="1" conaffinity="1" group="1" mesh="half_round_tip" rgba="0.09 0.09 0.09 1"/>
|
|
<geom size="0.004 0.004 0.0185" pos="0 -0.0045 -0.015" quat="0 0 0 -1" type="box" group="0" conaffinity="1" contype="1" name="l_fingertip_g0_right" friction="0.95 0.3 0.1"/>
|
|
<geom size="0.0035 0.004 0.0165" pos="0 -0.0047 -0.017" type="box" conaffinity="1" contype="1" name="l_fingerpad_g0_right" friction="0.95 0.3 0.1"/>
|
|
</body>
|
|
</body>
|
|
<body name="r_finger_right" pos="0 -0.01 0.0444">
|
|
<inertial pos="0 0 0" mass="0.0001" diaginertia="0.001 0.001 0.001" />
|
|
<joint name="r_finger_joint_right" pos="0 0 0" axis="0 -1 0" type="slide" limited="true" range="-0.020833 0.0115" damping="100" armature="1.0" frictionloss="1.0"/>
|
|
<geom name="r_finger_right" type="mesh" contype="1" conaffinity="1" group="1" mesh="standard_narrow" rgba="0.09 0.09 0.09 1"/>
|
|
<geom size="0.005 0.00675 0.0375" pos="0 -0.01725 0.04" type="box" group="0" conaffinity="1" contype="1" name="r_finger_g0_right" friction="0 0 0"/>
|
|
<geom size="0.005 0.025 0.0085" pos="0.005 0.003 0.0083" type="box" group="0" conaffinity="1" contype="1" name="r_finger_g1_right" friction="0 0 0"/>
|
|
<body name="r_finger_tip_right" pos="0 -0.01725 0.075">
|
|
<inertial pos="0 0 0" mass="0.0001" diaginertia="0.001 0.001 0.001" />
|
|
<geom name="r_fingertip_g0_vis_right" type="mesh" contype="1" conaffinity="1" group="1" mesh="half_round_tip" rgba="0.09 0.09 0.09 1"/>
|
|
<geom size="0.004 0.004 0.0185" pos="0 0.0045 -0.015" type="box" group="0" conaffinity="1" contype="1" name="r_fingertip_g0_right" friction="0.95 0.3 0.1"/>
|
|
<geom size="0.0035 0.004 0.0165" pos="0 0.0047 -0.017" type="box" conaffinity="1" contype="1" name="r_fingerpad_g0_right" friction="0.95 0.3 0.1"/>
|
|
|
|
</body>
|
|
</body>
|
|
</body>
|
|
</body>
|
|
</body>
|
|
</body>
|
|
</body>
|
|
</body>
|
|
</body>
|
|
</body>
|
|
</body>
|
|
</worldbody>
|
|
|
|
<actuator>
|
|
<motor name="a1_l" ctrllimited="true" ctrlrange="-100.0 100.0" joint="l_j1"/>
|
|
<motor name="a2_l" ctrllimited="true" ctrlrange="-100.0 100.0" joint="l_j2"/>
|
|
<motor name="a3_l" ctrllimited="true" ctrlrange="-100.0 100.0" joint="l_j3"/>
|
|
<motor name="a4_l" ctrllimited="true" ctrlrange="-100.0 100.0" joint="l_j4"/>
|
|
<motor name="a5_l" ctrllimited="true" ctrlrange="-100.0 100.0" joint="l_j5"/>
|
|
<motor name="a6_l" ctrllimited="true" ctrlrange="-100.0 100.0" joint="l_j6"/>
|
|
<motor name="a7_l" ctrllimited="true" ctrlrange="-100.0 100.0" joint="l_j7"/>
|
|
|
|
<motor name="a1_r" ctrllimited="true" ctrlrange="-100.0 100.0" joint="r_j1"/>
|
|
<motor name="a2_r" ctrllimited="true" ctrlrange="-100.0 100.0" joint="r_j2"/>
|
|
<motor name="a3_r" ctrllimited="true" ctrlrange="-100.0 100.0" joint="r_j3"/>
|
|
<motor name="a4_r" ctrllimited="true" ctrlrange="-100.0 100.0" joint="r_j4"/>
|
|
<motor name="a5_r" ctrllimited="true" ctrlrange="-100.0 100.0" joint="r_j5"/>
|
|
<motor name="a6_r" ctrllimited="true" ctrlrange="-100.0 100.0" joint="r_j6"/>
|
|
<motor name="a7_r" ctrllimited="true" ctrlrange="-100.0 100.0" joint="r_j7"/>
|
|
|
|
<motor name="gripper_left" ctrllimited="true" ctrlrange="-100.0 100.0" joint="l_finger_joint_left" />
|
|
<motor name="gripper_right" ctrllimited="true" ctrlrange="-100.0 100.0" joint="l_finger_joint_right" />
|
|
</actuator>
|
|
|
|
<equality>
|
|
<joint joint1="r_finger_joint_left" joint2="l_finger_joint_left"/>
|
|
<joint joint1="r_finger_joint_right" joint2="l_finger_joint_right"/>
|
|
</equality>
|
|
|
|
<contact>
|
|
<exclude body1="left_base_link" body2="left_link1"/>
|
|
<exclude body1="left_link1" body2="left_link2"/>
|
|
<exclude body1="left_link2" body2="left_link3"/>
|
|
<exclude body1="left_link3" body2="left_link4"/>
|
|
<exclude body1="left_link4" body2="left_link5"/>
|
|
<exclude body1="left_link5" body2="left_link6"/>
|
|
<exclude body1="left_link6" body2="left_link7"/>
|
|
<exclude body1="left_link7" body2="gripper_base_left"/>
|
|
<exclude body1="gripper_base_left" body2="eef_left"/>
|
|
<exclude body1="eef_left" body2="ee_cam_left"/>
|
|
<exclude body1="ee_cam_left" body2="l_finger_left"/>
|
|
<exclude body1="l_finger_left" body2="l_finger_tip_left"/>
|
|
<exclude body1="l_finger_tip_left" body2="r_finger_left"/>
|
|
<exclude body1="r_finger_left" body2="r_finger_tip_left"/>
|
|
|
|
<exclude body1="left_link1" body2="left_link3"/>
|
|
<exclude body1="left_link1" body2="left_link4"/>
|
|
<exclude body1="left_link1" body2="left_link5"/>
|
|
<exclude body1="left_link1" body2="left_link6"/>
|
|
<exclude body1="left_link1" body2="left_link7"/>
|
|
<exclude body1="left_link1" body2="gripper_base_left"/>
|
|
<exclude body1="left_link1" body2="eef_left"/>
|
|
<exclude body1="left_link1" body2="ee_cam_left"/>
|
|
<exclude body1="left_link1" body2="l_finger_left"/>
|
|
<exclude body1="left_link1" body2="l_finger_tip_left"/>
|
|
<exclude body1="left_link1" body2="r_finger_left"/>
|
|
<exclude body1="left_link1" body2="r_finger_tip_left"/>
|
|
|
|
<exclude body1="left_link2" body2="left_link4"/>
|
|
<exclude body1="left_link2" body2="left_link5"/>
|
|
<exclude body1="left_link2" body2="left_link6"/>
|
|
<exclude body1="left_link2" body2="left_link7"/>
|
|
<exclude body1="left_link2" body2="gripper_base_left"/>
|
|
<exclude body1="left_link2" body2="eef_left"/>
|
|
<exclude body1="left_link2" body2="ee_cam_left"/>
|
|
<exclude body1="left_link2" body2="l_finger_left"/>
|
|
<exclude body1="left_link2" body2="l_finger_tip_left"/>
|
|
<exclude body1="left_link2" body2="r_finger_left"/>
|
|
<exclude body1="left_link2" body2="r_finger_tip_left"/>
|
|
|
|
<exclude body1="left_link3" body2="left_link5"/>
|
|
<exclude body1="left_link3" body2="left_link6"/>
|
|
<exclude body1="left_link3" body2="left_link7"/>
|
|
<exclude body1="left_link3" body2="gripper_base_left"/>
|
|
<exclude body1="left_link3" body2="eef_left"/>
|
|
<exclude body1="left_link3" body2="ee_cam_left"/>
|
|
<exclude body1="left_link3" body2="l_finger_left"/>
|
|
<exclude body1="left_link3" body2="l_finger_tip_left"/>
|
|
<exclude body1="left_link3" body2="r_finger_left"/>
|
|
<exclude body1="left_link3" body2="r_finger_tip_left"/>
|
|
|
|
<exclude body1="left_link4" body2="left_link6"/>
|
|
<exclude body1="left_link4" body2="left_link7"/>
|
|
<exclude body1="left_link4" body2="gripper_base_left"/>
|
|
<exclude body1="left_link4" body2="eef_left"/>
|
|
<exclude body1="left_link4" body2="ee_cam_left"/>
|
|
<exclude body1="left_link4" body2="l_finger_left"/>
|
|
<exclude body1="left_link4" body2="l_finger_tip_left"/>
|
|
<exclude body1="left_link4" body2="r_finger_left"/>
|
|
<exclude body1="left_link4" body2="r_finger_tip_left"/>
|
|
|
|
<exclude body1="left_link5" body2="left_link7"/>
|
|
<exclude body1="left_link5" body2="gripper_base_left"/>
|
|
<exclude body1="left_link5" body2="eef_left"/>
|
|
<exclude body1="left_link5" body2="ee_cam_left"/>
|
|
<exclude body1="left_link5" body2="l_finger_left"/>
|
|
<exclude body1="left_link5" body2="l_finger_tip_left"/>
|
|
<exclude body1="left_link5" body2="r_finger_left"/>
|
|
<exclude body1="left_link5" body2="r_finger_tip_left"/>
|
|
|
|
<exclude body1="left_link6" body2="gripper_base_left"/>
|
|
<exclude body1="left_link6" body2="eef_left"/>
|
|
<exclude body1="left_link6" body2="ee_cam_left"/>
|
|
<exclude body1="left_link6" body2="l_finger_left"/>
|
|
<exclude body1="left_link6" body2="l_finger_tip_left"/>
|
|
<exclude body1="left_link6" body2="r_finger_left"/>
|
|
<exclude body1="left_link6" body2="r_finger_tip_left"/>
|
|
|
|
<exclude body1="left_link7" body2="eef_left"/>
|
|
<exclude body1="left_link7" body2="ee_cam_left"/>
|
|
<exclude body1="left_link7" body2="l_finger_left"/>
|
|
<exclude body1="left_link7" body2="l_finger_tip_left"/>
|
|
<exclude body1="left_link7" body2="r_finger_left"/>
|
|
<exclude body1="left_link7" body2="r_finger_tip_left"/>
|
|
|
|
<exclude body1="gripper_base_left" body2="ee_cam_left"/>
|
|
<exclude body1="gripper_base_left" body2="l_finger_left"/>
|
|
<exclude body1="gripper_base_left" body2="l_finger_tip_left"/>
|
|
<exclude body1="gripper_base_left" body2="r_finger_left"/>
|
|
<exclude body1="gripper_base_left" body2="r_finger_tip_left"/>
|
|
|
|
<exclude body1="eef_left" body2="l_finger_left"/>
|
|
<exclude body1="eef_left" body2="l_finger_tip_left"/>
|
|
<exclude body1="eef_left" body2="r_finger_left"/>
|
|
<exclude body1="eef_left" body2="r_finger_tip_left"/>
|
|
|
|
<exclude body1="ee_cam_left" body2="l_finger_tip_left"/>
|
|
<exclude body1="ee_cam_left" body2="r_finger_left"/>
|
|
<exclude body1="ee_cam_left" body2="r_finger_tip_left"/>
|
|
|
|
<exclude body1="l_finger_left" body2="r_finger_left"/>
|
|
<exclude body1="l_finger_left" body2="r_finger_tip_left"/>
|
|
|
|
<exclude body1="l_finger_tip_left" body2="r_finger_tip_left"/>
|
|
|
|
<exclude body1="right_base_link" body2="right_link1"/>
|
|
<exclude body1="right_link1" body2="right_link2"/>
|
|
<exclude body1="right_link2" body2="right_link3"/>
|
|
<exclude body1="right_link3" body2="right_link4"/>
|
|
<exclude body1="right_link4" body2="right_link5"/>
|
|
<exclude body1="right_link5" body2="right_link6"/>
|
|
<exclude body1="right_link6" body2="right_link7"/>
|
|
<exclude body1="right_link7" body2="gripper_base_right"/>
|
|
<exclude body1="gripper_base_right" body2="eef_right"/>
|
|
<exclude body1="eef_right" body2="ee_cam_right"/>
|
|
<exclude body1="ee_cam_right" body2="l_finger_right"/>
|
|
<exclude body1="l_finger_right" body2="l_finger_tip_right"/>
|
|
<exclude body1="l_finger_tip_right" body2="r_finger_right"/>
|
|
<exclude body1="r_finger_right" body2="r_finger_tip_right"/>
|
|
|
|
<exclude body1="right_link1" body2="right_link3"/>
|
|
<exclude body1="right_link1" body2="right_link4"/>
|
|
<exclude body1="right_link1" body2="right_link5"/>
|
|
<exclude body1="right_link1" body2="right_link6"/>
|
|
<exclude body1="right_link1" body2="right_link7"/>
|
|
<exclude body1="right_link1" body2="gripper_base_right"/>
|
|
<exclude body1="right_link1" body2="eef_right"/>
|
|
<exclude body1="right_link1" body2="ee_cam_right"/>
|
|
<exclude body1="right_link1" body2="l_finger_right"/>
|
|
<exclude body1="right_link1" body2="l_finger_tip_right"/>
|
|
<exclude body1="right_link1" body2="r_finger_right"/>
|
|
<exclude body1="right_link1" body2="r_finger_tip_right"/>
|
|
|
|
<exclude body1="right_link2" body2="right_link4"/>
|
|
<exclude body1="right_link2" body2="right_link5"/>
|
|
<exclude body1="right_link2" body2="right_link6"/>
|
|
<exclude body1="right_link2" body2="right_link7"/>
|
|
<exclude body1="right_link2" body2="gripper_base_right"/>
|
|
<exclude body1="right_link2" body2="eef_right"/>
|
|
<exclude body1="right_link2" body2="ee_cam_right"/>
|
|
<exclude body1="right_link2" body2="l_finger_right"/>
|
|
<exclude body1="right_link2" body2="l_finger_tip_right"/>
|
|
<exclude body1="right_link2" body2="r_finger_right"/>
|
|
<exclude body1="right_link2" body2="r_finger_tip_right"/>
|
|
|
|
<exclude body1="right_link3" body2="right_link5"/>
|
|
<exclude body1="right_link3" body2="right_link6"/>
|
|
<exclude body1="right_link3" body2="right_link7"/>
|
|
<exclude body1="right_link3" body2="gripper_base_right"/>
|
|
<exclude body1="right_link3" body2="eef_right"/>
|
|
<exclude body1="right_link3" body2="ee_cam_right"/>
|
|
<exclude body1="right_link3" body2="l_finger_right"/>
|
|
<exclude body1="right_link3" body2="l_finger_tip_right"/>
|
|
<exclude body1="right_link3" body2="r_finger_right"/>
|
|
<exclude body1="right_link3" body2="r_finger_tip_right"/>
|
|
|
|
<exclude body1="right_link4" body2="right_link6"/>
|
|
<exclude body1="right_link4" body2="right_link7"/>
|
|
<exclude body1="right_link4" body2="gripper_base_right"/>
|
|
<exclude body1="right_link4" body2="eef_right"/>
|
|
<exclude body1="right_link4" body2="ee_cam_right"/>
|
|
<exclude body1="right_link4" body2="l_finger_right"/>
|
|
<exclude body1="right_link4" body2="l_finger_tip_right"/>
|
|
<exclude body1="right_link4" body2="r_finger_right"/>
|
|
<exclude body1="right_link4" body2="r_finger_tip_right"/>
|
|
|
|
<exclude body1="right_link5" body2="right_link7"/>
|
|
<exclude body1="right_link5" body2="gripper_base_right"/>
|
|
<exclude body1="right_link5" body2="eef_right"/>
|
|
<exclude body1="right_link5" body2="ee_cam_right"/>
|
|
<exclude body1="right_link5" body2="l_finger_right"/>
|
|
<exclude body1="right_link5" body2="l_finger_tip_right"/>
|
|
<exclude body1="right_link5" body2="r_finger_right"/>
|
|
<exclude body1="right_link5" body2="r_finger_tip_right"/>
|
|
|
|
<exclude body1="right_link6" body2="gripper_base_right"/>
|
|
<exclude body1="right_link6" body2="eef_right"/>
|
|
<exclude body1="right_link6" body2="ee_cam_right"/>
|
|
<exclude body1="right_link6" body2="l_finger_right"/>
|
|
<exclude body1="right_link6" body2="l_finger_tip_right"/>
|
|
<exclude body1="right_link6" body2="r_finger_right"/>
|
|
<exclude body1="right_link6" body2="r_finger_tip_right"/>
|
|
|
|
<exclude body1="right_link7" body2="eef_right"/>
|
|
<exclude body1="right_link7" body2="ee_cam_right"/>
|
|
<exclude body1="right_link7" body2="l_finger_right"/>
|
|
<exclude body1="right_link7" body2="l_finger_tip_right"/>
|
|
<exclude body1="right_link7" body2="r_finger_right"/>
|
|
<exclude body1="right_link7" body2="r_finger_tip_right"/>
|
|
|
|
<exclude body1="gripper_base_right" body2="ee_cam_right"/>
|
|
<exclude body1="gripper_base_right" body2="l_finger_right"/>
|
|
<exclude body1="gripper_base_right" body2="l_finger_tip_right"/>
|
|
<exclude body1="gripper_base_right" body2="r_finger_right"/>
|
|
<exclude body1="gripper_base_right" body2="r_finger_tip_right"/>
|
|
|
|
<exclude body1="eef_right" body2="l_finger_right"/>
|
|
<exclude body1="eef_right" body2="l_finger_tip_right"/>
|
|
<exclude body1="eef_right" body2="r_finger_right"/>
|
|
<exclude body1="eef_right" body2="r_finger_tip_right"/>
|
|
|
|
<exclude body1="ee_cam_right" body2="l_finger_tip_right"/>
|
|
<exclude body1="ee_cam_right" body2="r_finger_right"/>
|
|
<exclude body1="ee_cam_right" body2="r_finger_tip_right"/>
|
|
|
|
<exclude body1="l_finger_right" body2="r_finger_right"/>
|
|
<exclude body1="l_finger_right" body2="r_finger_tip_right"/>
|
|
|
|
<exclude body1="l_finger_tip_right" body2="r_finger_tip_right"/>
|
|
|
|
</contact>
|
|
</mujoco>
|