lss_4dof.urdf.xacro 8.83 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro" >

  <xacro:include filename="$(find lss_4dof_description)/model/materials.urdf"/>
  <xacro:include filename="$(find lss_4dof_description)/model/lss_4dof.inertia.xacro"/>
  <xacro:include filename="$(find lss_4dof_description)/model/lss_4dof.transmission.xacro"/>

  <xacro:property name="M_PI" value="3.1415926535897931" />
  <xacro:property name="velocity_scale_factor" value="1.0"/>
  
  <xacro:macro name="lss_4dof" params="parent name *origin">


    <!-- The base fixed to its parent. -->
    <joint name="${parent}_${name}_base_joint" type="fixed">
      <xacro:insert_block name="origin"/>
      <parent link="${parent}"/>
      <child link="${name}_base_link"/>
    </joint>
    
    <link name="${name}_base_link">
      <inertial>
        <xacro:insert_block name="negligible_inertia"/>
      </inertial>
      <visual>
        <origin xyz="-0.009 0.0068 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://lss_4dof_description/meshes/Arm_Base.stl" scale="0.001 0.001 0.001" />
        </geometry>
        <material name="Arm/White"/>
      </visual>
      <collision>
33
        <origin xyz="0.0 0.0 0.021" rpy="0 0 0"/>
34
        <geometry>
35
          <cylinder radius="0.055" length="0.042"/>
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
        </geometry>
      </collision>
    </link>
    
    <!-- 1-Axis -->
    <joint name="${name}_axis_1_joint" type="revolute">
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <axis xyz="0 0 1"/>
      <limit lower="${-180 * M_PI / 180}" upper="${180 * M_PI / 180}"
             effort="204" velocity="${velocity_scale_factor * 180 * M_PI / 180}" />
      <safety_controller soft_lower_limit="${-180 * M_PI / 180}"
                         soft_upper_limit="${180 * M_PI / 180}"
                         k_position="30"
                         k_velocity="30"/>
      <dynamics friction="1.0" damping="1.0"/>
      <parent link="${name}_base_link"/>
      <child link="${name}_axis_1_link"/>
    </joint>

    <link name="${name}_axis_1_link">
      <inertial>
        <xacro:insert_block name="negligible_inertia"/>
      </inertial>
      <visual>
60
        <origin xyz="-0.016 0.016 0.0425" rpy="0 0 0"/>
61
62
63
64
65
66
        <geometry>
          <mesh filename="package://lss_4dof_description/meshes/Link_1.stl" scale="0.001 0.001 0.001" /> 
        </geometry>
        <material name="Arm/White"/>
      </visual>
      <collision>
67
        <origin xyz="0.0 0.0 0.072" rpy="0 0 0"/>
68
        <geometry>
69
          <box size="0.04 0.027 0.07"/>
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
        </geometry>
      </collision>
    </link>

    <!-- 2-Axis -->
    <joint name="${name}_axis_2_joint" type="revolute">
      <!-- add 0.0142 in z and sub 0.007 in y -->
      <!-- <origin xyz="0.004 0.007 0.083" rpy="0 0 0"/> -->
      <origin xyz="0 0.0 0.0955" rpy="0 0 0"/>
      <axis xyz="1 0 0"/>
      <limit lower="${-110 * M_PI / 180}" upper="${100 * M_PI / 180}"
             effort="204" velocity="${velocity_scale_factor * 180 * M_PI / 180}" />
      <safety_controller soft_lower_limit="${-110 * M_PI / 180}"
                         soft_upper_limit="${100 * M_PI / 180}"
                         k_position="30"
                         k_velocity="30"/>
      <dynamics friction="1.0" damping="1.0"/>
      <parent link="${name}_axis_1_link"/>
      <child link="${name}_axis_2_link"/>
    </joint>

    <link name="${name}_axis_2_link">
      <inertial>
        <xacro:insert_block name="negligible_inertia"/>
      </inertial>
      <visual>
        <origin xyz="0.004 -0.0148 -0.0125" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://lss_4dof_description/meshes/Link_2.stl" scale="0.001 0.001 0.001" /> 
        </geometry>
        <material name="Arm/White"/>
      </visual>
      <collision>
103
        <origin xyz="0.0 0.0 0.087" rpy="0 0 0"/>
104
        <geometry>
105
          <box size="0.025 0.025 0.127"/>
106
107
108
109
110
111
112
113
        </geometry>
      </collision>
    </link>

    <!-- 3-Axis -->
    <joint name="${name}_axis_3_joint" type="revolute">
      <origin xyz="0 -0.025 0.14" rpy="0 0 0"/>
      <axis xyz="1 0 0"/>
114
      <limit lower="${-30 * M_PI / 180}" upper="${178 * M_PI / 180}"
115
116
             effort="204" velocity="${velocity_scale_factor * 180 * M_PI / 180}" />
      <safety_controller soft_lower_limit="${-30 * M_PI / 180}"
117
                         soft_upper_limit="${175 * M_PI / 180}"
118
119
120
121
122
123
124
125
126
127
128
129
                         k_position="30"
                         k_velocity="30"/>
      <dynamics friction="1.0" damping="1.0"/>
      <parent link="${name}_axis_2_link"/>
      <child link="${name}_axis_3_link"/>
    </joint>

    <link name="${name}_axis_3_link">
      <inertial>
        <xacro:insert_block name="negligible_inertia"/>
      </inertial>
      <visual>
130
        <origin xyz="0.004 0.007 -0.013" rpy="0 0 0"/>
131
132
133
134
135
136
        <geometry>
          <mesh filename="package://lss_4dof_description/meshes/Link_3.stl" scale="0.001 0.001 0.001" /> 
        </geometry>
        <material name="Arm/White"/>
      </visual>
      <collision>
137
        <origin xyz="0.0 0.0 0.087" rpy="0 0 0"/>
138
        <geometry>
139
          <box size="0.025 0.025 0.127"/>
140
141
142
143
144
145
        </geometry>
      </collision>
    </link>

    <!-- 4-Axis -->
    <joint name="${name}_axis_4_joint" type="revolute">
146
147
      <origin xyz="0 -0.0005 0.1628" rpy="0 0 0"/>
      <axis xyz="1 0 0"/>
148
      <limit lower="${-124 * M_PI / 180}" upper="${124 * M_PI / 180}"
149
             effort="204" velocity="${velocity_scale_factor * 180 * M_PI / 180}" />
150
151
      <safety_controller soft_lower_limit="${-120 * M_PI / 180}"
                         soft_upper_limit="${120 * M_PI / 180}"
152
153
154
155
156
157
158
159
160
161
162
163
                         k_position="30"
                         k_velocity="30"/>
      <dynamics friction="1.0" damping="1.0"/>
      <parent link="${name}_axis_3_link"/>
      <child link="${name}_axis_4_link"/>
    </joint>

    <link name="${name}_axis_4_link">
      <inertial>
        <xacro:insert_block name="negligible_inertia"/>
      </inertial>
      <visual>
164
        <origin xyz="0.016 0.0127 -0.0128" rpy="0 0 0"/>
165
166
167
168
169
        <geometry>
          <mesh filename="package://lss_4dof_description/meshes/Link_4.stl" scale="0.001 0.001 0.001" /> 
        </geometry>
        <material name="Arm/White"/>
      </visual>
170
      <collision>
171
        <origin xyz="0.0 0.0 0.042" rpy="0 0 0"/>
172
        <geometry>
173
          <box size="0.06 0.03 0.04"/>
174
175
176
177
178
        </geometry>
      </collision>
    </link>

    <!-- Gripper -->
179
    <joint name="${name}_gripper_l" type="revolute">
180
181
182
183
184
185
186
187
188
189
190
191
192
      <origin xyz="0.0155 -0.02 0.0505" rpy="0 0 0"/>
      <axis xyz="0 1 0"/>
      <limit lower="${-2 * M_PI / 180}" upper="${75 * M_PI / 180}"
             effort="204" velocity="${velocity_scale_factor * 180 * M_PI / 180}" />
      <safety_controller soft_lower_limit="${-1 * M_PI / 180}"
                         soft_upper_limit="${75 * M_PI / 180}"
                         k_position="30"
                         k_velocity="30"/>
      <dynamics friction="1.0" damping="1.0"/>
      <parent link="${name}_axis_4_link"/>
      <child link="${name}_gripper_l_link"/>
    </joint>

193
    <joint name="${name}_gripper_r" type="revolute">
194
      <origin xyz="-0.010 -0.02 0.0505" rpy="0 0 0"/>
195
      <mimic joint="${name}_gripper_l" multiplier="-1" offset="0" />
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
      <axis xyz="0 1 0"/>
      <limit lower="${-2 * M_PI / 180}" upper="${75 * M_PI / 180}"
             effort="204" velocity="${velocity_scale_factor * 180 * M_PI / 180}" />
      <safety_controller soft_lower_limit="${-1 * M_PI / 180}"
                         soft_upper_limit="${75 * M_PI / 180}"
                         k_position="30"
                         k_velocity="30"/>
      <dynamics friction="1.0" damping="1.0"/>
      <parent link="${name}_axis_4_link"/>
      <child link="${name}_gripper_r_link"/>
    </joint>

    <link name="${name}_gripper_l_link">
      <inertial>
        <xacro:insert_block name="negligible_inertia"/>
      </inertial>
      <visual>
        <origin xyz="-0.008 -0.02 -0.012" rpy="0 0 0"/>
        <geometry>
           <mesh filename="package://lss_4dof_description/meshes/Gripper_L.stl" scale="0.001 0.001 0.001" /> 
        </geometry>
        <material name="Arm/White"/>
      </visual>
219
      <collision>
220
        <origin xyz="0.0 -0.002 0.035" rpy="0 0 0"/>
221
        <geometry>
222
          <box size="0.02 0.01 0.05"/>
223
224
225
        </geometry>
      </collision>
    </link>
226
227
228
229
230
231
232
233
234
235
236
237
238

    <link name="${name}_gripper_r_link">
      <inertial>
        <xacro:insert_block name="negligible_inertia"/>
      </inertial>
      <visual>
        <origin xyz="-0.0055 -0.02 -0.012" rpy="0 0 0"/>
        <geometry>
           <mesh filename="package://lss_4dof_description/meshes/Gripper_R.stl" scale="0.001 0.001 0.001" /> 
        </geometry>
        <material name="Arm/White"/>
      </visual>
      <collision>
239
        <origin xyz="0.0 -0.002 0.035" rpy="0 0 0"/>
240
        <geometry>
241
          <box size="0.02 0.01 0.05"/>
242
243
244
245
        </geometry>
      </collision>
    </link>

246
247
248
249
    <xacro:lss_4dof_transmission name="${name}"/>
  </xacro:macro>

</robot>