|
11 | 11 | </joint> |
12 | 12 | </xacro:unless> |
13 | 13 |
|
14 | | - <xacro:link_detailed_coarse name="link0"> |
| 14 | + <xacro:link_with_sc name="link0"> |
15 | 15 | <self_collision_geometries> |
16 | 16 | <xacro:collision_capsule xyz="-0.075 0 0.06" direction="x" radius="${0.06+safety_distance}" length="0.03" /> |
17 | 17 | </self_collision_geometries> |
18 | | - </xacro:link_detailed_coarse> |
| 18 | + </xacro:link_with_sc> |
19 | 19 |
|
20 | | - <xacro:link_detailed_coarse name="link1"> |
| 20 | + <xacro:link_with_sc name="link1"> |
21 | 21 | <self_collision_geometries> |
22 | 22 | <xacro:collision_capsule xyz="0 0 -191.5e-3" radius="${0.06+safety_distance}" length="0.283" /> |
23 | 23 | </self_collision_geometries> |
24 | | - </xacro:link_detailed_coarse> |
| 24 | + </xacro:link_with_sc> |
25 | 25 |
|
26 | 26 | <joint name="${arm_id}_joint1" type="revolute"> |
27 | 27 | <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973" /> |
|
33 | 33 | <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16" /> |
34 | 34 | </joint> |
35 | 35 |
|
36 | | - <xacro:link_detailed_coarse name="link2"> |
| 36 | + <xacro:link_with_sc name="link2"> |
37 | 37 | <self_collision_geometries> |
38 | 38 | <xacro:collision_capsule xyz="0 0 0" radius="${0.06+safety_distance}" length="0.12" /> |
39 | 39 | </self_collision_geometries> |
40 | | - </xacro:link_detailed_coarse> |
| 40 | + </xacro:link_with_sc> |
41 | 41 |
|
42 | 42 | <joint name="${arm_id}_joint2" type="revolute"> |
43 | 43 | <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628" /> |
|
49 | 49 | <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16" /> |
50 | 50 | </joint> |
51 | 51 |
|
52 | | - <xacro:link_detailed_coarse name="link3"> |
| 52 | + <xacro:link_with_sc name="link3"> |
53 | 53 | <self_collision_geometries> |
54 | 54 | <xacro:collision_capsule xyz="0 0 -0.145" radius="${0.06+safety_distance}" length="0.15" /> |
55 | 55 | </self_collision_geometries> |
56 | | - </xacro:link_detailed_coarse> |
| 56 | + </xacro:link_with_sc> |
57 | 57 |
|
58 | 58 | <joint name="${arm_id}_joint3" type="revolute"> |
59 | 59 | <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973" /> |
|
65 | 65 | <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16" /> |
66 | 66 | </joint> |
67 | 67 |
|
68 | | - <xacro:link_detailed_coarse name="link4"> |
| 68 | + <xacro:link_with_sc name="link4"> |
69 | 69 | <self_collision_geometries> |
70 | 70 | <xacro:collision_capsule xyz="0 0 0" radius="${0.06+safety_distance}" length="0.12" /> |
71 | 71 | </self_collision_geometries> |
72 | | - </xacro:link_detailed_coarse> |
| 72 | + </xacro:link_with_sc> |
73 | 73 |
|
74 | 74 | <joint name="${arm_id}_joint4" type="revolute"> |
75 | 75 | <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698" /> |
|
81 | 81 | <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16" /> |
82 | 82 | </joint> |
83 | 83 |
|
84 | | - <xacro:link_detailed_coarse name="link5"> |
| 84 | + <xacro:link_with_sc name="link5"> |
85 | 85 | <self_collision_geometries> |
86 | 86 | <xacro:collision_capsule xyz="0 0 -0.26" radius="${0.060+safety_distance}" length="0.10" /> |
87 | 87 | <xacro:collision_capsule xyz="0 0.08 -0.13" radius="${0.025+safety_distance}" length="0.14" /> |
88 | 88 | </self_collision_geometries> |
89 | | - </xacro:link_detailed_coarse> |
| 89 | + </xacro:link_with_sc> |
90 | 90 |
|
91 | 91 | <joint name="${arm_id}_joint5" type="revolute"> |
92 | 92 | <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973" /> |
|
98 | 98 | <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16" /> |
99 | 99 | </joint> |
100 | 100 |
|
101 | | - <xacro:link_detailed_coarse name="link6"> |
| 101 | + <xacro:link_with_sc name="link6"> |
102 | 102 | <self_collision_geometries> |
103 | 103 | <xacro:collision_capsule xyz="0 0 -0.03" radius="${0.05+safety_distance}" length="0.08" /> |
104 | 104 | </self_collision_geometries> |
105 | | - </xacro:link_detailed_coarse> |
| 105 | + </xacro:link_with_sc> |
106 | 106 |
|
107 | 107 | <joint name="${arm_id}_joint6" type="revolute"> |
108 | 108 | <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525" /> |
|
114 | 114 | <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16" /> |
115 | 115 | </joint> |
116 | 116 |
|
117 | | - <xacro:link_detailed_coarse name="link7" rpy="0 0 ${pi/4}"> |
| 117 | + <xacro:link_with_sc name="link7" rpy="0 0 ${pi/4}"> |
118 | 118 | <self_collision_geometries> |
119 | 119 | <xacro:collision_capsule xyz="0 0 0.01" direction="z" radius="${0.04+safety_distance}" length="0.14" /> |
120 | 120 | <xacro:collision_capsule xyz="0.06 0 0.082" direction="x" radius="${0.03+safety_distance}" length="0.01" /> |
121 | 121 | </self_collision_geometries> |
122 | | - </xacro:link_detailed_coarse> |
| 122 | + </xacro:link_with_sc> |
123 | 123 |
|
124 | 124 | <joint name="${arm_id}_joint7" type="revolute"> |
125 | 125 | <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/> |
|
0 commit comments