agenticros 0.1.9 → 0.1.11
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- package/dist/commands/init.d.ts.map +1 -1
- package/dist/commands/init.js +71 -4
- package/dist/commands/init.js.map +1 -1
- package/dist/menu.js +1 -1
- package/dist/menu.js.map +1 -1
- package/package.json +1 -1
- package/runtime/BUNDLE.json +1 -1
- package/runtime/ros2_ws/src/agenticros_sim/README.md +52 -13
- package/runtime/ros2_ws/src/agenticros_sim/config/arm_bridge.yaml +84 -0
- package/runtime/ros2_ws/src/agenticros_sim/config/arm_view.rviz +109 -0
- package/runtime/ros2_ws/src/agenticros_sim/launch/sim_arm.launch.py +223 -0
- package/runtime/ros2_ws/src/agenticros_sim/models/agenticros_amr/model.sdf +1 -1
- package/runtime/ros2_ws/src/agenticros_sim/models/agenticros_arm/model.config +26 -0
- package/runtime/ros2_ws/src/agenticros_sim/models/agenticros_arm/model.sdf +426 -0
- package/runtime/ros2_ws/src/agenticros_sim/urdf/agenticros_arm.urdf.xacro +205 -0
- package/runtime/ros2_ws/src/agenticros_sim/worlds/agenticros_indoor.sdf +1 -1
|
@@ -0,0 +1,426 @@
|
|
|
1
|
+
<?xml version="1.0"?>
|
|
2
|
+
<!--
|
|
3
|
+
agenticros_arm/model.sdf
|
|
4
|
+
|
|
5
|
+
6-DOF UR5e-shaped robotic arm for the AgenticROS Phase 3 demo.
|
|
6
|
+
|
|
7
|
+
Why "UR5e-shaped" rather than the actual UR description package:
|
|
8
|
+
* No apt deps. The ur_description package is ~50 MB and requires a
|
|
9
|
+
slew of ros-humble-ur-* trees we don't otherwise need. This file
|
|
10
|
+
gives the same visual silhouette and joint topology with nothing
|
|
11
|
+
but cylinders.
|
|
12
|
+
* Self-contained physics. All joint inertias, limits, and damping
|
|
13
|
+
are tuned here so the arm stays upright under gravity without
|
|
14
|
+
ros2_control or MoveIt2.
|
|
15
|
+
* Easy to author the URDF mirror (urdf/agenticros_arm.urdf.xacro)
|
|
16
|
+
because we control the link geometry directly.
|
|
17
|
+
|
|
18
|
+
Joint chain (matches the UR5e topology so MoveIt2 IK could be layered
|
|
19
|
+
on later in Phase 3b without changing the SDF):
|
|
20
|
+
|
|
21
|
+
base_link --shoulder_pan_joint (Z)--> shoulder_link
|
|
22
|
+
--shoulder_lift_joint (Y)--> upper_arm_link
|
|
23
|
+
--elbow_joint (Y)--> forearm_link
|
|
24
|
+
--wrist_1_joint (Y)--> wrist_1_link
|
|
25
|
+
--wrist_2_joint (Z)--> wrist_2_link
|
|
26
|
+
--wrist_3_joint (Y)--> wrist_3_link
|
|
27
|
+
--fixed --> tool0 (end-effector frame)
|
|
28
|
+
|
|
29
|
+
Control surface (Gazebo side; ros_gz_bridge re-emits as ROS topics):
|
|
30
|
+
|
|
31
|
+
cmd to /arm/JOINT/cmd_pos type=gz.msgs.Double ROS_TO_GZ
|
|
32
|
+
fb from /joint_states type=gz.msgs.Model GZ_TO_ROS
|
|
33
|
+
tf from /model/agenticros_arm/pose and /pose_static
|
|
34
|
+
|
|
35
|
+
Send a single joint angle as ROS std_msgs/Float64 to e.g.
|
|
36
|
+
/arm/shoulder_pan/cmd_pos and the JointPositionController plugin below
|
|
37
|
+
drives the joint to that angle with a PD loop (kp=500, kd=50). The
|
|
38
|
+
arm reaches the target in ~1-2 s and holds against gravity.
|
|
39
|
+
|
|
40
|
+
Limits:
|
|
41
|
+
shoulder_pan, elbow, wrist_ : +/-pi rad
|
|
42
|
+
shoulder_lift : +/-pi/2 rad (prevents self-collision)
|
|
43
|
+
velocity : 2 rad/s effort: 30 Nm
|
|
44
|
+
|
|
45
|
+
NOTE on physics tuning: link masses are heavier than the real UR5e
|
|
46
|
+
on purpose - higher inertia keeps PD control numerically stable on
|
|
47
|
+
the relatively coarse default gz physics step (1 ms). If you want a
|
|
48
|
+
snappier feel, drop the masses by 50% and bump kp to ~800.
|
|
49
|
+
-->
|
|
50
|
+
<sdf version="1.9">
|
|
51
|
+
<model name="agenticros_arm">
|
|
52
|
+
|
|
53
|
+
<!-- ============================================================
|
|
54
|
+
Static pedestal. world_joint pins this to the world so the
|
|
55
|
+
arm doesn't drift sideways when the wrist is moving.
|
|
56
|
+
============================================================ -->
|
|
57
|
+
<link name="base_link">
|
|
58
|
+
<pose>0 0 0.075 0 0 0</pose>
|
|
59
|
+
<inertial>
|
|
60
|
+
<mass>10.0</mass>
|
|
61
|
+
<inertia>
|
|
62
|
+
<ixx>0.05</ixx><iyy>0.05</iyy><izz>0.05</izz>
|
|
63
|
+
<ixy>0</ixy><ixz>0</ixz><iyz>0</iyz>
|
|
64
|
+
</inertia>
|
|
65
|
+
</inertial>
|
|
66
|
+
<collision name="col">
|
|
67
|
+
<geometry><cylinder><radius>0.08</radius><length>0.15</length></cylinder></geometry>
|
|
68
|
+
</collision>
|
|
69
|
+
<visual name="vis">
|
|
70
|
+
<geometry><cylinder><radius>0.08</radius><length>0.15</length></cylinder></geometry>
|
|
71
|
+
<material>
|
|
72
|
+
<ambient>0.15 0.15 0.15 1</ambient>
|
|
73
|
+
<diffuse>0.20 0.20 0.20 1</diffuse>
|
|
74
|
+
</material>
|
|
75
|
+
</visual>
|
|
76
|
+
</link>
|
|
77
|
+
|
|
78
|
+
<joint name="world_joint" type="fixed">
|
|
79
|
+
<parent>world</parent>
|
|
80
|
+
<child>base_link</child>
|
|
81
|
+
</joint>
|
|
82
|
+
|
|
83
|
+
<!-- ============================================================
|
|
84
|
+
shoulder_link (rotated around Z by shoulder_pan_joint)
|
|
85
|
+
============================================================ -->
|
|
86
|
+
<link name="shoulder_link">
|
|
87
|
+
<pose>0 0 0.205 0 0 0</pose>
|
|
88
|
+
<inertial>
|
|
89
|
+
<mass>3.0</mass>
|
|
90
|
+
<inertia>
|
|
91
|
+
<ixx>0.008</ixx><iyy>0.008</iyy><izz>0.005</izz>
|
|
92
|
+
<ixy>0</ixy><ixz>0</ixz><iyz>0</iyz>
|
|
93
|
+
</inertia>
|
|
94
|
+
</inertial>
|
|
95
|
+
<collision name="col">
|
|
96
|
+
<geometry><cylinder><radius>0.07</radius><length>0.10</length></cylinder></geometry>
|
|
97
|
+
</collision>
|
|
98
|
+
<visual name="vis">
|
|
99
|
+
<geometry><cylinder><radius>0.07</radius><length>0.10</length></cylinder></geometry>
|
|
100
|
+
<material>
|
|
101
|
+
<ambient>0.20 0.45 0.70 1</ambient>
|
|
102
|
+
<diffuse>0.25 0.50 0.75 1</diffuse>
|
|
103
|
+
</material>
|
|
104
|
+
</visual>
|
|
105
|
+
</link>
|
|
106
|
+
|
|
107
|
+
<joint name="shoulder_pan_joint" type="revolute">
|
|
108
|
+
<parent>base_link</parent>
|
|
109
|
+
<child>shoulder_link</child>
|
|
110
|
+
<pose>0 0 0.075 0 0 0</pose>
|
|
111
|
+
<axis>
|
|
112
|
+
<xyz>0 0 1</xyz>
|
|
113
|
+
<limit>
|
|
114
|
+
<lower>-3.14159</lower><upper>3.14159</upper>
|
|
115
|
+
<velocity>2.0</velocity><effort>30.0</effort>
|
|
116
|
+
</limit>
|
|
117
|
+
<dynamics><damping>2.0</damping><friction>0.5</friction></dynamics>
|
|
118
|
+
</axis>
|
|
119
|
+
</joint>
|
|
120
|
+
|
|
121
|
+
<!-- ============================================================
|
|
122
|
+
upper_arm_link (rotated around Y by shoulder_lift_joint)
|
|
123
|
+
The link's own frame has its long axis along +X so that
|
|
124
|
+
positive shoulder_lift swings the arm "up and forward".
|
|
125
|
+
============================================================ -->
|
|
126
|
+
<link name="upper_arm_link">
|
|
127
|
+
<pose>0.20 0 0.255 0 1.5707963 0</pose>
|
|
128
|
+
<inertial>
|
|
129
|
+
<mass>4.0</mass>
|
|
130
|
+
<inertia>
|
|
131
|
+
<ixx>0.02</ixx><iyy>0.06</iyy><izz>0.06</izz>
|
|
132
|
+
<ixy>0</ixy><ixz>0</ixz><iyz>0</iyz>
|
|
133
|
+
</inertia>
|
|
134
|
+
</inertial>
|
|
135
|
+
<collision name="col">
|
|
136
|
+
<geometry><cylinder><radius>0.045</radius><length>0.40</length></cylinder></geometry>
|
|
137
|
+
</collision>
|
|
138
|
+
<visual name="vis">
|
|
139
|
+
<geometry><cylinder><radius>0.045</radius><length>0.40</length></cylinder></geometry>
|
|
140
|
+
<material>
|
|
141
|
+
<ambient>0.85 0.55 0.10 1</ambient>
|
|
142
|
+
<diffuse>0.95 0.60 0.10 1</diffuse>
|
|
143
|
+
</material>
|
|
144
|
+
</visual>
|
|
145
|
+
</link>
|
|
146
|
+
|
|
147
|
+
<joint name="shoulder_lift_joint" type="revolute">
|
|
148
|
+
<parent>shoulder_link</parent>
|
|
149
|
+
<child>upper_arm_link</child>
|
|
150
|
+
<pose>0 0 0.05 0 0 0</pose>
|
|
151
|
+
<axis>
|
|
152
|
+
<xyz>0 1 0</xyz>
|
|
153
|
+
<limit>
|
|
154
|
+
<lower>-1.5707</lower><upper>1.5707</upper>
|
|
155
|
+
<velocity>2.0</velocity><effort>30.0</effort>
|
|
156
|
+
</limit>
|
|
157
|
+
<dynamics><damping>2.0</damping><friction>0.5</friction></dynamics>
|
|
158
|
+
<!-- Home: -pi/2 rotates the upper arm UP so gravity loads less.
|
|
159
|
+
Without this the arm spawns horizontal and visibly sags
|
|
160
|
+
before the PD controller catches up. -->
|
|
161
|
+
<initial_position>-1.5707963</initial_position>
|
|
162
|
+
</axis>
|
|
163
|
+
</joint>
|
|
164
|
+
|
|
165
|
+
<!-- ============================================================
|
|
166
|
+
forearm_link (rotated around Y by elbow_joint)
|
|
167
|
+
============================================================ -->
|
|
168
|
+
<link name="forearm_link">
|
|
169
|
+
<pose>0.575 0 0.255 0 1.5707963 0</pose>
|
|
170
|
+
<inertial>
|
|
171
|
+
<mass>2.5</mass>
|
|
172
|
+
<inertia>
|
|
173
|
+
<ixx>0.012</ixx><iyy>0.04</iyy><izz>0.04</izz>
|
|
174
|
+
<ixy>0</ixy><ixz>0</ixz><iyz>0</iyz>
|
|
175
|
+
</inertia>
|
|
176
|
+
</inertial>
|
|
177
|
+
<collision name="col">
|
|
178
|
+
<geometry><cylinder><radius>0.035</radius><length>0.35</length></cylinder></geometry>
|
|
179
|
+
</collision>
|
|
180
|
+
<visual name="vis">
|
|
181
|
+
<geometry><cylinder><radius>0.035</radius><length>0.35</length></cylinder></geometry>
|
|
182
|
+
<material>
|
|
183
|
+
<ambient>0.85 0.55 0.10 1</ambient>
|
|
184
|
+
<diffuse>0.95 0.60 0.10 1</diffuse>
|
|
185
|
+
</material>
|
|
186
|
+
</visual>
|
|
187
|
+
</link>
|
|
188
|
+
|
|
189
|
+
<joint name="elbow_joint" type="revolute">
|
|
190
|
+
<parent>upper_arm_link</parent>
|
|
191
|
+
<child>forearm_link</child>
|
|
192
|
+
<pose>0 0 0.20 0 0 0</pose>
|
|
193
|
+
<axis>
|
|
194
|
+
<xyz>0 1 0</xyz>
|
|
195
|
+
<limit>
|
|
196
|
+
<lower>-3.14159</lower><upper>3.14159</upper>
|
|
197
|
+
<velocity>2.0</velocity><effort>30.0</effort>
|
|
198
|
+
</limit>
|
|
199
|
+
<dynamics><damping>1.5</damping><friction>0.5</friction></dynamics>
|
|
200
|
+
<!-- Home: +pi/2 puts the forearm horizontal forming an L with the
|
|
201
|
+
upper arm (which is at -pi/2). This is the "ready to grasp"
|
|
202
|
+
pose; without it the elbow spawns straight and the arm sticks
|
|
203
|
+
out vertically. -->
|
|
204
|
+
<initial_position>1.5707963</initial_position>
|
|
205
|
+
</axis>
|
|
206
|
+
</joint>
|
|
207
|
+
|
|
208
|
+
<!-- ============================================================
|
|
209
|
+
wrist_1_link (rotated around Y by wrist_1_joint)
|
|
210
|
+
============================================================ -->
|
|
211
|
+
<link name="wrist_1_link">
|
|
212
|
+
<pose>0.80 0 0.255 0 1.5707963 0</pose>
|
|
213
|
+
<inertial>
|
|
214
|
+
<mass>0.8</mass>
|
|
215
|
+
<inertia>
|
|
216
|
+
<ixx>0.001</ixx><iyy>0.001</iyy><izz>0.001</izz>
|
|
217
|
+
<ixy>0</ixy><ixz>0</ixz><iyz>0</iyz>
|
|
218
|
+
</inertia>
|
|
219
|
+
</inertial>
|
|
220
|
+
<collision name="col">
|
|
221
|
+
<geometry><cylinder><radius>0.032</radius><length>0.10</length></cylinder></geometry>
|
|
222
|
+
</collision>
|
|
223
|
+
<visual name="vis">
|
|
224
|
+
<geometry><cylinder><radius>0.032</radius><length>0.10</length></cylinder></geometry>
|
|
225
|
+
<material>
|
|
226
|
+
<ambient>0.20 0.45 0.70 1</ambient>
|
|
227
|
+
<diffuse>0.25 0.50 0.75 1</diffuse>
|
|
228
|
+
</material>
|
|
229
|
+
</visual>
|
|
230
|
+
</link>
|
|
231
|
+
|
|
232
|
+
<joint name="wrist_1_joint" type="revolute">
|
|
233
|
+
<parent>forearm_link</parent>
|
|
234
|
+
<child>wrist_1_link</child>
|
|
235
|
+
<pose>0 0 0.175 0 0 0</pose>
|
|
236
|
+
<axis>
|
|
237
|
+
<xyz>0 1 0</xyz>
|
|
238
|
+
<limit>
|
|
239
|
+
<lower>-3.14159</lower><upper>3.14159</upper>
|
|
240
|
+
<velocity>2.0</velocity><effort>15.0</effort>
|
|
241
|
+
</limit>
|
|
242
|
+
<dynamics><damping>1.0</damping><friction>0.3</friction></dynamics>
|
|
243
|
+
</axis>
|
|
244
|
+
</joint>
|
|
245
|
+
|
|
246
|
+
<!-- ============================================================
|
|
247
|
+
wrist_2_link (rotated around Z by wrist_2_joint)
|
|
248
|
+
============================================================ -->
|
|
249
|
+
<link name="wrist_2_link">
|
|
250
|
+
<pose>0.875 0 0.255 0 0 0</pose>
|
|
251
|
+
<inertial>
|
|
252
|
+
<mass>0.6</mass>
|
|
253
|
+
<inertia>
|
|
254
|
+
<ixx>0.0008</ixx><iyy>0.0008</iyy><izz>0.0008</izz>
|
|
255
|
+
<ixy>0</ixy><ixz>0</ixz><iyz>0</iyz>
|
|
256
|
+
</inertia>
|
|
257
|
+
</inertial>
|
|
258
|
+
<collision name="col">
|
|
259
|
+
<geometry><cylinder><radius>0.030</radius><length>0.08</length></cylinder></geometry>
|
|
260
|
+
</collision>
|
|
261
|
+
<visual name="vis">
|
|
262
|
+
<geometry><cylinder><radius>0.030</radius><length>0.08</length></cylinder></geometry>
|
|
263
|
+
<material>
|
|
264
|
+
<ambient>0.85 0.55 0.10 1</ambient>
|
|
265
|
+
<diffuse>0.95 0.60 0.10 1</diffuse>
|
|
266
|
+
</material>
|
|
267
|
+
</visual>
|
|
268
|
+
</link>
|
|
269
|
+
|
|
270
|
+
<joint name="wrist_2_joint" type="revolute">
|
|
271
|
+
<parent>wrist_1_link</parent>
|
|
272
|
+
<child>wrist_2_link</child>
|
|
273
|
+
<pose>0 0 0.075 0 0 0</pose>
|
|
274
|
+
<axis>
|
|
275
|
+
<xyz>0 0 1</xyz>
|
|
276
|
+
<limit>
|
|
277
|
+
<lower>-3.14159</lower><upper>3.14159</upper>
|
|
278
|
+
<velocity>2.0</velocity><effort>15.0</effort>
|
|
279
|
+
</limit>
|
|
280
|
+
<dynamics><damping>1.0</damping><friction>0.3</friction></dynamics>
|
|
281
|
+
</axis>
|
|
282
|
+
</joint>
|
|
283
|
+
|
|
284
|
+
<!-- ============================================================
|
|
285
|
+
wrist_3_link (rotated around Y by wrist_3_joint).
|
|
286
|
+
Has a fixed end-effector frame `tool0` at the tip - this is
|
|
287
|
+
what MoveIt2 / IK code targets in Phase 3b.
|
|
288
|
+
============================================================ -->
|
|
289
|
+
<link name="wrist_3_link">
|
|
290
|
+
<pose>0.945 0 0.255 0 1.5707963 0</pose>
|
|
291
|
+
<inertial>
|
|
292
|
+
<mass>0.4</mass>
|
|
293
|
+
<inertia>
|
|
294
|
+
<ixx>0.0004</ixx><iyy>0.0004</iyy><izz>0.0004</izz>
|
|
295
|
+
<ixy>0</ixy><ixz>0</ixz><iyz>0</iyz>
|
|
296
|
+
</inertia>
|
|
297
|
+
</inertial>
|
|
298
|
+
<collision name="col">
|
|
299
|
+
<geometry><cylinder><radius>0.026</radius><length>0.06</length></cylinder></geometry>
|
|
300
|
+
</collision>
|
|
301
|
+
<visual name="vis">
|
|
302
|
+
<geometry><cylinder><radius>0.026</radius><length>0.06</length></cylinder></geometry>
|
|
303
|
+
<material>
|
|
304
|
+
<ambient>0.20 0.45 0.70 1</ambient>
|
|
305
|
+
<diffuse>0.25 0.50 0.75 1</diffuse>
|
|
306
|
+
</material>
|
|
307
|
+
</visual>
|
|
308
|
+
<!-- Tip marker so users can see "this is where tool0 lives" -->
|
|
309
|
+
<visual name="tool0_tip">
|
|
310
|
+
<pose>0 0 0.04 0 0 0</pose>
|
|
311
|
+
<geometry><sphere><radius>0.015</radius></sphere></geometry>
|
|
312
|
+
<material>
|
|
313
|
+
<ambient>0.9 0.1 0.1 1</ambient>
|
|
314
|
+
<diffuse>1.0 0.1 0.1 1</diffuse>
|
|
315
|
+
</material>
|
|
316
|
+
</visual>
|
|
317
|
+
</link>
|
|
318
|
+
|
|
319
|
+
<joint name="wrist_3_joint" type="revolute">
|
|
320
|
+
<parent>wrist_2_link</parent>
|
|
321
|
+
<child>wrist_3_link</child>
|
|
322
|
+
<pose>0 0 0.04 0 0 0</pose>
|
|
323
|
+
<axis>
|
|
324
|
+
<xyz>0 1 0</xyz>
|
|
325
|
+
<limit>
|
|
326
|
+
<lower>-3.14159</lower><upper>3.14159</upper>
|
|
327
|
+
<velocity>2.0</velocity><effort>10.0</effort>
|
|
328
|
+
</limit>
|
|
329
|
+
<dynamics><damping>0.5</damping><friction>0.2</friction></dynamics>
|
|
330
|
+
</axis>
|
|
331
|
+
</joint>
|
|
332
|
+
|
|
333
|
+
<!-- ===================== Plugins ===================== -->
|
|
334
|
+
|
|
335
|
+
<!--
|
|
336
|
+
One JointPositionController per joint. Each subscribes to a gz topic
|
|
337
|
+
of the form /arm/<joint>/cmd_pos (gz.msgs.Double). ros_gz_bridge
|
|
338
|
+
then re-exposes each as a ROS std_msgs/Float64 of the same name.
|
|
339
|
+
|
|
340
|
+
PD gains are deliberately high so the arm holds against gravity
|
|
341
|
+
without an integral term (which can cause oscillation in gz's
|
|
342
|
+
relatively coarse default step).
|
|
343
|
+
|
|
344
|
+
Plugin filename:
|
|
345
|
+
We use the "ignition-gazebo-*" prefix (Fortress / ign-gazebo 6)
|
|
346
|
+
rather than the more modern "gz-sim-*" prefix because the
|
|
347
|
+
ros-humble-ros-gz-bridge that ships with Humble is linked
|
|
348
|
+
against Fortress (libignition-msgs8 / libignition-transport11).
|
|
349
|
+
Fortress *does* ship a backward-compat shim that maps
|
|
350
|
+
"gz-sim-diff-drive-system" -> the Fortress library, which is
|
|
351
|
+
why agenticros_amr's diff-drive plugin works under the
|
|
352
|
+
gz-sim-* name; but no equivalent shim exists for the joint
|
|
353
|
+
position controller. Using the Fortress-native filename here
|
|
354
|
+
is the only one that loads reliably on Jetson Humble.
|
|
355
|
+
|
|
356
|
+
If you migrate this stack to Jazzy / Harmonic later, switch
|
|
357
|
+
the six filenames below to "gz-sim-joint-position-controller-system".
|
|
358
|
+
-->
|
|
359
|
+
<plugin filename="ignition-gazebo-joint-position-controller-system"
|
|
360
|
+
name="gz::sim::systems::JointPositionController">
|
|
361
|
+
<joint_name>shoulder_pan_joint</joint_name>
|
|
362
|
+
<topic>/arm/shoulder_pan/cmd_pos</topic>
|
|
363
|
+
<p_gain>800</p_gain><d_gain>80</d_gain><i_gain>0</i_gain>
|
|
364
|
+
<cmd_max>100</cmd_max><cmd_min>-100</cmd_min>
|
|
365
|
+
</plugin>
|
|
366
|
+
<plugin filename="ignition-gazebo-joint-position-controller-system"
|
|
367
|
+
name="gz::sim::systems::JointPositionController">
|
|
368
|
+
<joint_name>shoulder_lift_joint</joint_name>
|
|
369
|
+
<topic>/arm/shoulder_lift/cmd_pos</topic>
|
|
370
|
+
<p_gain>1200</p_gain><d_gain>120</d_gain><i_gain>0</i_gain>
|
|
371
|
+
<cmd_max>100</cmd_max><cmd_min>-100</cmd_min>
|
|
372
|
+
<!-- Match the joint's <initial_position> so the controller's first
|
|
373
|
+
setpoint isn't 0 (which would jerk the arm down at startup). -->
|
|
374
|
+
<initial_position>-1.5707963</initial_position>
|
|
375
|
+
</plugin>
|
|
376
|
+
<plugin filename="ignition-gazebo-joint-position-controller-system"
|
|
377
|
+
name="gz::sim::systems::JointPositionController">
|
|
378
|
+
<joint_name>elbow_joint</joint_name>
|
|
379
|
+
<topic>/arm/elbow/cmd_pos</topic>
|
|
380
|
+
<p_gain>800</p_gain><d_gain>80</d_gain><i_gain>0</i_gain>
|
|
381
|
+
<cmd_max>80</cmd_max><cmd_min>-80</cmd_min>
|
|
382
|
+
<initial_position>1.5707963</initial_position>
|
|
383
|
+
</plugin>
|
|
384
|
+
<plugin filename="ignition-gazebo-joint-position-controller-system"
|
|
385
|
+
name="gz::sim::systems::JointPositionController">
|
|
386
|
+
<joint_name>wrist_1_joint</joint_name>
|
|
387
|
+
<topic>/arm/wrist_1/cmd_pos</topic>
|
|
388
|
+
<p_gain>400</p_gain><d_gain>40</d_gain><i_gain>0</i_gain>
|
|
389
|
+
<cmd_max>15</cmd_max><cmd_min>-15</cmd_min>
|
|
390
|
+
</plugin>
|
|
391
|
+
<plugin filename="ignition-gazebo-joint-position-controller-system"
|
|
392
|
+
name="gz::sim::systems::JointPositionController">
|
|
393
|
+
<joint_name>wrist_2_joint</joint_name>
|
|
394
|
+
<topic>/arm/wrist_2/cmd_pos</topic>
|
|
395
|
+
<p_gain>300</p_gain><d_gain>30</d_gain><i_gain>0</i_gain>
|
|
396
|
+
<cmd_max>15</cmd_max><cmd_min>-15</cmd_min>
|
|
397
|
+
</plugin>
|
|
398
|
+
<plugin filename="ignition-gazebo-joint-position-controller-system"
|
|
399
|
+
name="gz::sim::systems::JointPositionController">
|
|
400
|
+
<joint_name>wrist_3_joint</joint_name>
|
|
401
|
+
<topic>/arm/wrist_3/cmd_pos</topic>
|
|
402
|
+
<p_gain>200</p_gain><d_gain>20</d_gain><i_gain>0</i_gain>
|
|
403
|
+
<cmd_max>10</cmd_max><cmd_min>-10</cmd_min>
|
|
404
|
+
</plugin>
|
|
405
|
+
|
|
406
|
+
<!-- Publishes /joint_states (used by robot_state_publisher + RViz). -->
|
|
407
|
+
<plugin filename="gz-sim-joint-state-publisher-system"
|
|
408
|
+
name="gz::sim::systems::JointStatePublisher">
|
|
409
|
+
<topic>joint_states</topic>
|
|
410
|
+
</plugin>
|
|
411
|
+
|
|
412
|
+
<!-- Publishes each link's pose as /tf so RViz can render motion. -->
|
|
413
|
+
<plugin filename="gz-sim-pose-publisher-system"
|
|
414
|
+
name="gz::sim::systems::PosePublisher">
|
|
415
|
+
<publish_link_pose>true</publish_link_pose>
|
|
416
|
+
<publish_sensor_pose>false</publish_sensor_pose>
|
|
417
|
+
<publish_collision_pose>false</publish_collision_pose>
|
|
418
|
+
<publish_visual_pose>false</publish_visual_pose>
|
|
419
|
+
<publish_nested_model_pose>false</publish_nested_model_pose>
|
|
420
|
+
<use_pose_vector_msg>true</use_pose_vector_msg>
|
|
421
|
+
<static_publisher>true</static_publisher>
|
|
422
|
+
<static_update_frequency>1</static_update_frequency>
|
|
423
|
+
</plugin>
|
|
424
|
+
|
|
425
|
+
</model>
|
|
426
|
+
</sdf>
|
|
@@ -0,0 +1,205 @@
|
|
|
1
|
+
<?xml version="1.0"?>
|
|
2
|
+
<!--
|
|
3
|
+
agenticros_arm.urdf.xacro
|
|
4
|
+
|
|
5
|
+
URDF mirror of models/agenticros_arm/model.sdf, fed to robot_state_publisher
|
|
6
|
+
so RViz's RobotModel display can render the arm. The forward-kinematics
|
|
7
|
+
topology MUST match the SDF link-by-link or RViz will draw the arm in the
|
|
8
|
+
wrong pose:
|
|
9
|
+
|
|
10
|
+
base_link -> shoulder_pan_joint -> shoulder_link
|
|
11
|
+
-> shoulder_lift_joint -> upper_arm_link
|
|
12
|
+
-> elbow_joint -> forearm_link
|
|
13
|
+
-> wrist_1_joint -> wrist_1_link
|
|
14
|
+
-> wrist_2_joint -> wrist_2_link
|
|
15
|
+
-> wrist_3_joint -> wrist_3_link
|
|
16
|
+
-> fixed -> tool0
|
|
17
|
+
|
|
18
|
+
Conventions:
|
|
19
|
+
* Each joint origin is expressed in its PARENT link's frame.
|
|
20
|
+
* Cylinder visuals are drawn along the link's local +Z by default; we
|
|
21
|
+
rotate them where needed to lay them flat along +X (matching how the
|
|
22
|
+
SDF orients each link via its <pose>).
|
|
23
|
+
* Joint axes match the SDF exactly (Z for pan/wrist_2, Y for the rest).
|
|
24
|
+
* Joint limits match the SDF; URDF doesn't really use them for
|
|
25
|
+
visualization but MoveIt2 (Phase 3b) will.
|
|
26
|
+
|
|
27
|
+
Physics, sensors, gravity tuning, and PD gains all live in the SDF; this
|
|
28
|
+
file is purely for visualization in RViz.
|
|
29
|
+
-->
|
|
30
|
+
<robot name="agenticros_arm" xmlns:xacro="http://www.ros.org/wiki/xacro">
|
|
31
|
+
|
|
32
|
+
<!-- ===== material palette (matches the SDF colours) ===== -->
|
|
33
|
+
<material name="agros_dark"> <color rgba="0.20 0.20 0.20 1"/></material>
|
|
34
|
+
<material name="agros_blue"> <color rgba="0.25 0.50 0.75 1"/></material>
|
|
35
|
+
<material name="agros_orange"><color rgba="0.95 0.60 0.10 1"/></material>
|
|
36
|
+
<material name="agros_red"> <color rgba="1.00 0.10 0.10 1"/></material>
|
|
37
|
+
|
|
38
|
+
<!-- ============================================================
|
|
39
|
+
base_link (static pedestal, fixed to world)
|
|
40
|
+
============================================================ -->
|
|
41
|
+
<link name="base_link">
|
|
42
|
+
<visual>
|
|
43
|
+
<origin xyz="0 0 0.075" rpy="0 0 0"/>
|
|
44
|
+
<geometry><cylinder radius="0.08" length="0.15"/></geometry>
|
|
45
|
+
<material name="agros_dark"/>
|
|
46
|
+
</visual>
|
|
47
|
+
<collision>
|
|
48
|
+
<origin xyz="0 0 0.075" rpy="0 0 0"/>
|
|
49
|
+
<geometry><cylinder radius="0.08" length="0.15"/></geometry>
|
|
50
|
+
</collision>
|
|
51
|
+
<inertial>
|
|
52
|
+
<mass value="10.0"/>
|
|
53
|
+
<inertia ixx="0.05" iyy="0.05" izz="0.05" ixy="0" ixz="0" iyz="0"/>
|
|
54
|
+
</inertial>
|
|
55
|
+
</link>
|
|
56
|
+
|
|
57
|
+
<!-- ============================================================
|
|
58
|
+
shoulder_link (rotated around Z)
|
|
59
|
+
============================================================ -->
|
|
60
|
+
<link name="shoulder_link">
|
|
61
|
+
<visual>
|
|
62
|
+
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
63
|
+
<geometry><cylinder radius="0.07" length="0.10"/></geometry>
|
|
64
|
+
<material name="agros_blue"/>
|
|
65
|
+
</visual>
|
|
66
|
+
<inertial>
|
|
67
|
+
<mass value="3.0"/>
|
|
68
|
+
<inertia ixx="0.008" iyy="0.008" izz="0.005" ixy="0" ixz="0" iyz="0"/>
|
|
69
|
+
</inertial>
|
|
70
|
+
</link>
|
|
71
|
+
<joint name="shoulder_pan_joint" type="revolute">
|
|
72
|
+
<parent link="base_link"/>
|
|
73
|
+
<child link="shoulder_link"/>
|
|
74
|
+
<origin xyz="0 0 0.205" rpy="0 0 0"/>
|
|
75
|
+
<axis xyz="0 0 1"/>
|
|
76
|
+
<limit lower="-3.14159" upper="3.14159" velocity="2.0" effort="30.0"/>
|
|
77
|
+
</joint>
|
|
78
|
+
|
|
79
|
+
<!-- ============================================================
|
|
80
|
+
upper_arm_link (rotated around Y) - laid along +X
|
|
81
|
+
============================================================ -->
|
|
82
|
+
<link name="upper_arm_link">
|
|
83
|
+
<visual>
|
|
84
|
+
<origin xyz="0.20 0 0" rpy="0 1.5707963 0"/>
|
|
85
|
+
<geometry><cylinder radius="0.045" length="0.40"/></geometry>
|
|
86
|
+
<material name="agros_orange"/>
|
|
87
|
+
</visual>
|
|
88
|
+
<inertial>
|
|
89
|
+
<mass value="4.0"/>
|
|
90
|
+
<inertia ixx="0.02" iyy="0.06" izz="0.06" ixy="0" ixz="0" iyz="0"/>
|
|
91
|
+
</inertial>
|
|
92
|
+
</link>
|
|
93
|
+
<joint name="shoulder_lift_joint" type="revolute">
|
|
94
|
+
<parent link="shoulder_link"/>
|
|
95
|
+
<child link="upper_arm_link"/>
|
|
96
|
+
<origin xyz="0 0 0.05" rpy="0 0 0"/>
|
|
97
|
+
<axis xyz="0 1 0"/>
|
|
98
|
+
<limit lower="-1.5707" upper="1.5707" velocity="2.0" effort="30.0"/>
|
|
99
|
+
</joint>
|
|
100
|
+
|
|
101
|
+
<!-- ============================================================
|
|
102
|
+
forearm_link (rotated around Y)
|
|
103
|
+
============================================================ -->
|
|
104
|
+
<link name="forearm_link">
|
|
105
|
+
<visual>
|
|
106
|
+
<origin xyz="0.175 0 0" rpy="0 1.5707963 0"/>
|
|
107
|
+
<geometry><cylinder radius="0.035" length="0.35"/></geometry>
|
|
108
|
+
<material name="agros_orange"/>
|
|
109
|
+
</visual>
|
|
110
|
+
<inertial>
|
|
111
|
+
<mass value="2.5"/>
|
|
112
|
+
<inertia ixx="0.012" iyy="0.04" izz="0.04" ixy="0" ixz="0" iyz="0"/>
|
|
113
|
+
</inertial>
|
|
114
|
+
</link>
|
|
115
|
+
<joint name="elbow_joint" type="revolute">
|
|
116
|
+
<parent link="upper_arm_link"/>
|
|
117
|
+
<child link="forearm_link"/>
|
|
118
|
+
<origin xyz="0.40 0 0" rpy="0 0 0"/>
|
|
119
|
+
<axis xyz="0 1 0"/>
|
|
120
|
+
<limit lower="-3.14159" upper="3.14159" velocity="2.0" effort="30.0"/>
|
|
121
|
+
</joint>
|
|
122
|
+
|
|
123
|
+
<!-- ============================================================
|
|
124
|
+
wrist_1_link (rotated around Y)
|
|
125
|
+
============================================================ -->
|
|
126
|
+
<link name="wrist_1_link">
|
|
127
|
+
<visual>
|
|
128
|
+
<origin xyz="0.05 0 0" rpy="0 1.5707963 0"/>
|
|
129
|
+
<geometry><cylinder radius="0.032" length="0.10"/></geometry>
|
|
130
|
+
<material name="agros_blue"/>
|
|
131
|
+
</visual>
|
|
132
|
+
<inertial>
|
|
133
|
+
<mass value="0.8"/>
|
|
134
|
+
<inertia ixx="0.001" iyy="0.001" izz="0.001" ixy="0" ixz="0" iyz="0"/>
|
|
135
|
+
</inertial>
|
|
136
|
+
</link>
|
|
137
|
+
<joint name="wrist_1_joint" type="revolute">
|
|
138
|
+
<parent link="forearm_link"/>
|
|
139
|
+
<child link="wrist_1_link"/>
|
|
140
|
+
<origin xyz="0.35 0 0" rpy="0 0 0"/>
|
|
141
|
+
<axis xyz="0 1 0"/>
|
|
142
|
+
<limit lower="-3.14159" upper="3.14159" velocity="2.0" effort="15.0"/>
|
|
143
|
+
</joint>
|
|
144
|
+
|
|
145
|
+
<!-- ============================================================
|
|
146
|
+
wrist_2_link (rotated around Z)
|
|
147
|
+
============================================================ -->
|
|
148
|
+
<link name="wrist_2_link">
|
|
149
|
+
<visual>
|
|
150
|
+
<origin xyz="0 0 0.04" rpy="0 0 0"/>
|
|
151
|
+
<geometry><cylinder radius="0.030" length="0.08"/></geometry>
|
|
152
|
+
<material name="agros_orange"/>
|
|
153
|
+
</visual>
|
|
154
|
+
<inertial>
|
|
155
|
+
<mass value="0.6"/>
|
|
156
|
+
<inertia ixx="0.0008" iyy="0.0008" izz="0.0008" ixy="0" ixz="0" iyz="0"/>
|
|
157
|
+
</inertial>
|
|
158
|
+
</link>
|
|
159
|
+
<joint name="wrist_2_joint" type="revolute">
|
|
160
|
+
<parent link="wrist_1_link"/>
|
|
161
|
+
<child link="wrist_2_link"/>
|
|
162
|
+
<origin xyz="0.10 0 0" rpy="0 0 0"/>
|
|
163
|
+
<axis xyz="0 0 1"/>
|
|
164
|
+
<limit lower="-3.14159" upper="3.14159" velocity="2.0" effort="15.0"/>
|
|
165
|
+
</joint>
|
|
166
|
+
|
|
167
|
+
<!-- ============================================================
|
|
168
|
+
wrist_3_link (rotated around Y)
|
|
169
|
+
============================================================ -->
|
|
170
|
+
<link name="wrist_3_link">
|
|
171
|
+
<visual>
|
|
172
|
+
<origin xyz="0.03 0 0" rpy="0 1.5707963 0"/>
|
|
173
|
+
<geometry><cylinder radius="0.026" length="0.06"/></geometry>
|
|
174
|
+
<material name="agros_blue"/>
|
|
175
|
+
</visual>
|
|
176
|
+
<inertial>
|
|
177
|
+
<mass value="0.4"/>
|
|
178
|
+
<inertia ixx="0.0004" iyy="0.0004" izz="0.0004" ixy="0" ixz="0" iyz="0"/>
|
|
179
|
+
</inertial>
|
|
180
|
+
</link>
|
|
181
|
+
<joint name="wrist_3_joint" type="revolute">
|
|
182
|
+
<parent link="wrist_2_link"/>
|
|
183
|
+
<child link="wrist_3_link"/>
|
|
184
|
+
<origin xyz="0 0 0.08" rpy="0 0 0"/>
|
|
185
|
+
<axis xyz="0 1 0"/>
|
|
186
|
+
<limit lower="-3.14159" upper="3.14159" velocity="2.0" effort="10.0"/>
|
|
187
|
+
</joint>
|
|
188
|
+
|
|
189
|
+
<!-- ============================================================
|
|
190
|
+
tool0 (end-effector frame). Tipped with a small red sphere so
|
|
191
|
+
the user can see "this is what an IK solver would target."
|
|
192
|
+
============================================================ -->
|
|
193
|
+
<link name="tool0">
|
|
194
|
+
<visual>
|
|
195
|
+
<geometry><sphere radius="0.015"/></geometry>
|
|
196
|
+
<material name="agros_red"/>
|
|
197
|
+
</visual>
|
|
198
|
+
</link>
|
|
199
|
+
<joint name="tool0_joint" type="fixed">
|
|
200
|
+
<parent link="wrist_3_link"/>
|
|
201
|
+
<child link="tool0"/>
|
|
202
|
+
<origin xyz="0.06 0 0" rpy="0 0 0"/>
|
|
203
|
+
</joint>
|
|
204
|
+
|
|
205
|
+
</robot>
|
|
@@ -13,7 +13,7 @@
|
|
|
13
13
|
* Standard physics + sensors + ros_gz system plugins so the AMR's depth
|
|
14
14
|
camera / IMU / lidar publish on Gazebo topics that the bridge picks up
|
|
15
15
|
-->
|
|
16
|
-
<sdf version="1.
|
|
16
|
+
<sdf version="1.9">
|
|
17
17
|
<world name="agenticros_indoor">
|
|
18
18
|
|
|
19
19
|
<!-- ====== System plugins ====== -->
|