SmartBench/cmvr-es
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
490c141ca8
cmvr-es/model/xiaoyan_description/dual_arm_collision.urdf
lgv 0257ac85ca feat(collision): add self-collision monitoring task 
Add Pinocchio and Coal based self-collision checking with collision-pair filtering and displacement-based sampling. Integrate a periodic safety task with warning and stop thresholds, plus the simplified collision URDF and runtime configuration.
2026-07-27 15:37:39 +08:00

517 lines
20 KiB
XML
Raw Blame History

<?xml version='1.0' encoding='utf-8'?>
<robot name="dual_arm">
<!-- <mujoco>-->
<!-- <compiler-->
<!-- meshdir="meshes"-->
<!-- balanceinertia="true"-->
<!-- discardvisual="false" />-->
<!-- </mujoco>-->
<link name="base_link">
<visual>
<origin xyz="0 0 0.6" rpy="0 0 0" />
<geometry>
<cylinder radius="0.05" length="1.2" />
</geometry>
<material name="gray">
<color rgba="0.5 0.5 0.5 1.0" />
</material>
</visual>
<collision name="base_column">
<origin xyz="0 0 0.6" rpy="0 0 0" />
<geometry>
<cylinder radius="0.05" length="1.2" />
</geometry>
</collision>
</link>
<joint name="base_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0 1.2" />
<parent link="base_link" />
<child link="PELVIS_S" />
</joint>
<link name="PELVIS_S">
<inertial>
<origin xyz="3.78529087037144E-05 3.81781425684836E-07 0.0386396273530852" rpy="0 0 0" />
<mass value="2.10624590271277" />
<inertia ixx="0.00165706865324979" ixy="3.13663044821662E-09" ixz="6.84209533216046E-07" iyy="0.00136736758630241" iyz="-1.01014607878816E-10" izz="0.00168295663089359" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/PELVIS_S.STL" />
</geometry>
<material name="">
<color rgba="0.698039215686274 0.698039215686274 0.698039215686274 1" />
</material>
</visual>
<collision name="AUTO_COLLISION_BOX">
<origin xyz="0 0 0.0402499998957" rpy="0 0 0" />
<geometry>
<box size="0.0769999921322 0.216000005603 0.0804999986589" />
</geometry>
</collision>
</link>
<link name="L_SHOULDER_P_S">
<inertial>
<origin xyz="-0.00982258725282134 0.0704593083751867 1.15261874861217E-06" rpy="0 0 0" />
<mass value="0.880737519698403" />
<inertia ixx="0.000583190308378148" ixy="-1.53153074560473E-05" ixz="6.58466471754072E-09" iyy="0.000445532440067177" iyz="6.37001404038516E-09" izz="0.000465648071069952" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/L_SHOULDER_P_S.STL" />
</geometry>
<material name="">
<color rgba="0.898039215686275 0.917647058823529 0.929411764705882 1" />
</material>
</visual>
<collision name="AUTO_COLLISION_BOX">
<origin xyz="-0.00434836359419 0.0607499987818 3.99221537266e-08" rpy="1.57079632679 0.000113532653235 3.14159265359" />
<geometry>
<box size="0.0717031309621 0.0729998270933 0.104499998502" />
</geometry>
</collision>
</link>
<joint name="L_SHOULDER_P" type="revolute">
<origin xyz="0 0.0945 0.042" rpy="0 0 0" />
<parent link="PELVIS_S" />
<child link="L_SHOULDER_P_S" />
<axis xyz="0 1 0" />
<limit lower="-1.57" upper="1.57" effort="120" velocity="3.351" />
</joint>
<link name="L_SHOULDER_R_S">
<inertial>
<origin xyz="-0.0346025282975857 0.091739327988169 -1.67085072999562E-08" rpy="0 0 0" />
<mass value="0.594788424442483" />
<inertia ixx="0.000380970396712656" ixy="4.80751844573946E-05" ixz="-1.34913746586865E-11" iyy="0.000320962178597474" iyz="-1.00039763442409E-09" izz="0.000414771047901155" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/L_SHOULDER_R_S.STL" />
</geometry>
<material name="">
<color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
</material>
</visual>
<collision name="AUTO_COLLISION_BOX">
<origin xyz="-0.0392500016196 0.0562499994412 0" rpy="0 0 0" />
<geometry>
<box size="0.0785000020061 0.172499997541 0.0649999976158" />
</geometry>
</collision>
</link>
<joint name="L_SHOULDER_R" type="revolute">
<origin xyz="0.035 0.0765 0" rpy="0 0 0" />
<parent link="L_SHOULDER_P_S" />
<child link="L_SHOULDER_R_S" />
<axis xyz="1 0 0" />
<limit lower="-2" upper="2" effort="120" velocity="3.351" />
</joint>
<link name="L_SHOULDER_Y_S">
<inertial>
<origin xyz="-0.00440976862014801 0.0863620459174068 9.50748668682166E-09" rpy="0 0 0" />
<mass value="0.563406026626801" />
<inertia ixx="0.000327003834287552" ixy="-1.8057438966771E-05" ixz="7.28778136267901E-10" iyy="0.000213830709361531" iyz="1.49273668517817E-10" izz="0.000297341029639189" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/L_SHOULDER_Y_S.STL" />
</geometry>
<material name="">
<color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
</material>
</visual>
<collision name="AUTO_COLLISION_BOX">
<origin xyz="-0.000899999402463 0.062000001315 0" rpy="0 0 0" />
<geometry>
<box size="0.0609999988228 0.144000004046 0.0629920661449" />
</geometry>
</collision>
</link>
<joint name="L_SHOULDER_Y" type="revolute">
<origin xyz="-0.035 0.1475 0" rpy="0 0 0" />
<parent link="L_SHOULDER_R_S" />
<child link="L_SHOULDER_Y_S" />
<axis xyz="0 1 0" />
<limit lower="-2.18" upper="0" effort="80" velocity="3.8758" />
</joint>
<link name="L_ELBOW_R_S">
<inertial>
<origin xyz="-0.0335624237303443 0.0603199964106564 2.99655911639718E-07" rpy="0 0 0" />
<mass value="0.393571904406493" />
<inertia ixx="0.00017277119386253" ixy="2.34549801867355E-05" ixz="-1.90560556659271E-09" iyy="0.000155340245267897" iyz="8.82493073600007E-09" izz="0.00018104232734159" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/L_ELBOW_R_S.STL" />
</geometry>
<material name="">
<color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
</material>
</visual>
<collision name="AUTO_COLLISION_BOX">
<origin xyz="-0.0345000004275 0.03925000038 1.86264514923e-09" rpy="0 0 0" />
<geometry>
<box size="0.068999995688 0.128500001505 0.0579999983311" />
</geometry>
</collision>
</link>
<joint name="L_ELBOW_R" type="revolute">
<origin xyz="0.034 0.1025 0" rpy="0 0 0" />
<parent link="L_SHOULDER_Y_S" />
<child link="L_ELBOW_R_S" />
<axis xyz="1 0 0" />
<limit lower="-2.05" upper="0" effort="50" velocity="4.71" />
</joint>
<link name="L_WRIST_P_S">
<inertial>
<origin xyz="-1.39659173115092E-10 0.0675972604744393 0.019200551565574" rpy="0 0 0" />
<mass value="0.442332465815496" />
<inertia ixx="0.000476754055455895" ixy="-4.61505824713347E-15" ixz="6.03808112272229E-18" iyy="0.000103466585850499" iyz="-4.88426705501198E-05" izz="0.000482956345754213" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/L_WRIST_P_S.STL" />
</geometry>
<material name="">
<color rgba="0.698039215686274 0.698039215686274 0.698039215686274 1" />
</material>
</visual>
<collision name="AUTO_COLLISION_CYLINDER">
<origin xyz="0 0.0887468701694 0.0122111458903" rpy="-1.57079632679 0 0" />
<geometry>
<cylinder radius="0.0376536743138" length="0.167493741494" />
</geometry>
</collision>
</link>
<joint name="L_WRIST_P" type="revolute">
<origin xyz="-0.034 0.0965 0" rpy="0 0 0" />
<parent link="L_ELBOW_R_S" />
<child link="L_WRIST_P_S" />
<axis xyz="0 1 0" />
<limit lower="0" upper="3.14" effort="50" velocity="4.71" />
</joint>
<link name="L_WRIST_Y_S">
<inertial>
<origin xyz="-0.00464135887331864 -5.06426789392833E-10 -0.0341253783666609" rpy="0 0 0" />
<mass value="0.23573847772002" />
<inertia ixx="5.10686940455785E-05" ixy="7.45706654358429E-16" ixz="6.2619568923368E-06" iyy="6.00636019624519E-05" iyz="1.27989654155383E-16" izz="5.32182903609189E-05" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/L_WRIST_Y_S.STL" />
</geometry>
<material name="">
<color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
</material>
</visual>
<collision name="AUTO_COLLISION_BOX">
<origin xyz="-0.00375000014901 0 -0.0327469492331" rpy="-1.57079632679 0 1.57079632679" />
<geometry>
<box size="0.0409999005497 0.0534939002246 0.0595000013709" />
</geometry>
</collision>
</link>
<joint name="L_WRIST_Y" type="revolute">
<origin xyz="0 0.1525 0.039" rpy="0 0 0" />
<parent link="L_WRIST_P_S" />
<child link="L_WRIST_Y_S" />
<axis xyz="0 0 1" />
<limit lower="-0.78" upper="0.78" effort="50" velocity="0.79" />
</joint>
<link name="L_WRIST_R_S">
<inertial>
<origin xyz="-0.0161477357620243 0.0955046558857351 -0.00499392489444117" rpy="0 0 0" />
<mass value="0.504894112043562" />
<inertia ixx="0.000186833711781125" ixy="-3.99488705622731E-06" ixz="-1.51920720398336E-06" iyy="0.000179960980467837" iyz="3.6992669535716E-06" izz="0.000280756101568308" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/L_WRIST_R_S.STL" />
</geometry>
<material name="">
<color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
</material>
</visual>
<collision name="AUTO_COLLISION_BOX">
<origin xyz="-0.00501008890569 0.110783384182 -0.0104531301185" rpy="0 0 0" />
<geometry>
<box size="0.119021866471 0.253566769883 0.0697322357446" />
</geometry>
</collision>
</link>
<joint name="L_WRIST_R" type="revolute">
<origin xyz="0.0258 0 -0.039" rpy="0 0 0" />
<parent link="L_WRIST_Y_S" />
<child link="L_WRIST_R_S" />
<axis xyz="1 0 0" />
<limit lower="-1.57" upper="0.26" effort="50" velocity="4.71" />
</joint>
<link name="R_SHOULDER_P_S">
<inertial>
<origin xyz="-0.00982258725282141 -0.0704593093873431 -1.15069920925137E-06" rpy="0 0 0" />
<mass value="0.880737519698404" />
<inertia ixx="0.000583190308378149" ixy="1.53153074560471E-05" ixz="-6.58466471736542E-09" iyy="0.000445532440067178" iyz="6.37001403998779E-09" izz="0.000465648071069953" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/R_SHOULDER_P_S.STL" />
</geometry>
<material name="">
<color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
</material>
</visual>
<collision name="AUTO_COLLISION_BOX">
<origin xyz="-0.00434836359419 -0.0607499992475 3.99221537266e-08" rpy="1.57079632679 -0.000113532653235 0" />
<geometry>
<box size="0.0717031309621 0.0729998270933 0.104499997571" />
</geometry>
</collision>
</link>
<joint name="R_SHOULDER_P" type="revolute">
<origin xyz="0 -0.0945 0.042" rpy="0 0 0" />
<parent link="PELVIS_S" />
<child link="R_SHOULDER_P_S" />
<axis xyz="0 -1 0" />
<limit lower="-3.14" upper="3.14" effort="120" velocity="3.351" />
</joint>
<link name="R_SHOULDER_R_S">
<inertial>
<origin xyz="-0.0346025282975784 -0.09173932900033 1.86280643132974E-08" rpy="0 0 0" />
<mass value="0.59478842444248" />
<inertia ixx="0.000380970396712653" ixy="-4.80751844573946E-05" ixz="1.34913746041655E-11" iyy="0.000320962178597472" iyz="-1.00039763417375E-09" izz="0.000414771047901152" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/R_SHOULDER_R_S.STL" />
</geometry>
<material name="">
<color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
</material>
</visual>
<collision name="AUTO_COLLISION_BOX">
<origin xyz="-0.0392500016195 -0.0562499994412 0" rpy="0 0 0" />
<geometry>
<box size="0.0785000020061 0.172499997541 0.0649999976158" />
</geometry>
</collision>
</link>
<joint name="R_SHOULDER_R" type="revolute">
<origin xyz="0.035 -0.0765 0" rpy="0 0 0" />
<parent link="R_SHOULDER_P_S" />
<child link="R_SHOULDER_R_S" />
<axis xyz="1 0 0" />
<limit lower="-0.78" upper="1.57" effort="120" velocity="3.351" />
</joint>
<link name="R_SHOULDER_Y_S">
<inertial>
<origin xyz="-0.00440976862014946 -0.0863620469295699 -7.58791862676134E-09" rpy="0 0 0" />
<mass value="0.563406026626801" />
<inertia ixx="0.000327003834287551" ixy="1.80574389667711E-05" ixz="-7.28778136077729E-10" iyy="0.000213830709361532" iyz="1.49273668428957E-10" izz="0.000297341029639189" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/R_SHOULDER_Y_S.STL" />
</geometry>
<material name="">
<color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
</material>
</visual>
<collision name="AUTO_COLLISION_BOX">
<origin xyz="-0.000899999402463 -0.0620000017807 0" rpy="0 0 0" />
<geometry>
<box size="0.0609999988228 0.144000003114 0.0629920661449" />
</geometry>
</collision>
</link>
<joint name="R_SHOULDER_Y" type="revolute">
<origin xyz="-0.035 -0.1475 0" rpy="0 0 0" />
<parent link="R_SHOULDER_R_S" />
<child link="R_SHOULDER_Y_S" />
<axis xyz="0 -1 0" />
<limit lower="-3.14" upper="3.14" effort="80" velocity="3.8758" />
</joint>
<link name="R_ELBOW_R_S">
<inertial>
<origin xyz="-0.0335624237303424 -0.0603199974228191 -2.97736340082455E-07" rpy="0 0 0" />
<mass value="0.393571904406492" />
<inertia ixx="0.000172771193862529" ixy="-2.34549801867353E-05" ixz="1.90560556668771E-09" iyy="0.000155340245267897" iyz="8.82493073602971E-09" izz="0.00018104232734159" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/R_ELBOW_R_S.STL" />
</geometry>
<material name="">
<color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
</material>
</visual>
<collision name="AUTO_COLLISION_BOX">
<origin xyz="-0.0345000004275 -0.03925000038 1.86264514923e-09" rpy="0 0 0" />
<geometry>
<box size="0.068999995688 0.128500001505 0.0579999983311" />
</geometry>
</collision>
</link>
<joint name="R_ELBOW_R" type="revolute">
<origin xyz="0.034 -0.1025 0" rpy="0 0 0" />
<parent link="R_SHOULDER_Y_S" />
<child link="R_ELBOW_R_S" />
<axis xyz="1 0 0" />
<limit lower="0" upper="2.05" effort="80" velocity="3.8758" />
</joint>
<link name="R_WRIST_P_S">
<inertial>
<origin xyz="-1.39656577968772E-10 -0.0675972614865965 0.0192005515655728" rpy="0 0 0" />
<mass value="0.442332465815497" />
<inertia ixx="0.000476754055455896" ixy="-4.61462558956188E-15" ixz="-5.91762926004267E-18" iyy="0.0001034665858505" iyz="4.88426705501212E-05" izz="0.000482956345754214" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/R_WRIST_P_S.STL" />
</geometry>
<material name="">
<color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
</material>
</visual>
<collision name="AUTO_COLLISION_CYLINDER">
<origin xyz="0 -0.0887468706351 0.0122110777877" rpy="-1.57079632679 0 0" />
<geometry>
<cylinder radius="0.0376537318089" length="0.167493740562" />
</geometry>
</collision>
</link>
<joint name="R_WRIST_P" type="revolute">
<origin xyz="-0.034 -0.0965 0" rpy="0 0 0" />
<parent link="R_ELBOW_R_S" />
<child link="R_WRIST_P_S" />
<axis xyz="0 -1 0" />
<limit lower="-3.14" upper="3.14" effort="50" velocity="4.71" />
</joint>
<link name="R_WRIST_Y_S">
<inertial>
<origin xyz="-0.00464135887330083 -5.06426456325926E-10 -0.0341253783666614" rpy="0 0 0" />
<mass value="0.235738477720019" />
<inertia ixx="5.1068694045578E-05" ixy="7.45771487459288E-16" ixz="6.26195689233664E-06" iyy="6.00636019624515E-05" iyz="1.27828046342987E-16" izz="5.32182903609188E-05" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/R_WRIST_Y_S.STL" />
</geometry>
<material name="">
<color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
</material>
</visual>
<collision name="AUTO_COLLISION_BOX">
<origin xyz="-0.00375000014901 0 -0.0327469492331" rpy="-1.57079632679 0 1.57079632679" />
<geometry>
<box size="0.0409999005497 0.0534939002246 0.0595000013709" />
</geometry>
</collision>
</link>
<joint name="R_WRIST_Y" type="revolute">
<origin xyz="0 -0.1525 0.039" rpy="0 0 0" />
<parent link="R_WRIST_P_S" />
<child link="R_WRIST_Y_S" />
<axis xyz="0 0 1" />
<limit lower="-0.78" upper="0.78" effort="50" velocity="0.79" />
</joint>
<link name="R_WRIST_R_S">
<inertial>
<origin xyz="-0.0201642233698132 -0.11074968386657 -0.00598955339232021" rpy="0 0 0" />
<mass value="0.504366058218534" />
<inertia ixx="0.000185559065855467" ixy="5.75889766751509E-06" ixz="2.40683898454438E-06" iyy="0.000131246007872084" iyz="1.60533277040148E-06" izz="0.000271800951396149" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/R_WRIST_R_S.STL" />
</geometry>
<material name="">
<color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
</material>
</visual>
<collision name="AUTO_COLLISION_BOX">
<origin xyz="-0.0136338975281 -0.109391543083 -0.014153839089" rpy="0 0 0" />
<geometry>
<box size="0.111523482949 0.251791322604 0.0779597964138" />
</geometry>
</collision>
</link>
<joint name="R_WRIST_R" type="revolute">
<origin xyz="0.03 0 -0.039" rpy="0 0 0" />
<parent link="R_WRIST_Y_S" />
<child link="R_WRIST_R_S" />
<axis xyz="1 0 0" />
<limit lower="-0.57" upper="1.57" effort="50" velocity="4.71" />
</joint>
<link name="R_FINGER_TIP">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0" />
<mass value="0" />
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<sphere radius="0.004" />
</geometry>
<material name="">
<color rgba="0 1 1 1" />
<!-- 绿色 -->
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<sphere radius="0.005" />
</geometry>
</collision>
</link>
<joint name="R_FINGER_TIP_FIXED" type="fixed">
<origin xyz="0.00684256 -0.284077 0.00801525" rpy="0 0 0" />
<parent link="R_WRIST_R_S" />
<child link="R_FINGER_TIP" />
<axis xyz="0 0 1" />
</joint>
<link name="R_CAM">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0" />
<mass value="0" />
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<sphere radius="0.004" />
</geometry>
<material name="">
<color rgba="0 1 0 1" />
<!-- 绿色 -->
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<sphere radius="0.005" />
</geometry>
</collision>
</link>
<joint name="R_CAM_FIXED" type="fixed">
<origin xyz="-0.01212 -0.17655 0.07506" rpy="-1.5707963267 0 3.1415926535" />
<parent link="R_WRIST_R_S" />
<child link="R_CAM" />
<axis xyz="0 0 1" />
</joint>
</robot>
Reference in New Issue View Git Blame Copy Permalink