Merge lp:~guihome/sr-ros-interface/manip-electric_corr3 into lp:sr-ros-interface
- manip-electric_corr3
- Merge into trunk
Proposed by
Guillaume W.
Status: | Merged |
---|---|
Merged at revision: | 390 |
Proposed branch: | lp:~guihome/sr-ros-interface/manip-electric_corr3 |
Merge into: | lp:sr-ros-interface |
Diff against target: |
783 lines (+152/-108) 34 files modified
sr_description/arm/xacro/base/shadowarm_base.urdf.xacro (+1/-1) sr_description/arm/xacro/hand_support/shadowarm_handsupport_motor.urdf.xacro (+1/-1) sr_description/arm/xacro/hand_support/shadowarm_handsupport_muscle.urdf.xacro (+1/-1) sr_description/arm/xacro/lower_arm/shadowarm_lowerarm.urdf.xacro (+1/-1) sr_description/arm/xacro/trunk/shadowarm_trunk.urdf.xacro (+1/-1) sr_description/arm/xacro/upper_arm/shadowarm_upperarm.urdf.xacro (+1/-1) sr_description/hand/xacro/finger/distal/distal.urdf.xacro (+1/-1) sr_description/hand/xacro/finger/distal/distal_ellipsoid.urdf.xacro (+1/-1) sr_description/hand/xacro/finger/middle/middle.urdf.xacro (+1/-0) sr_description/hand/xacro/finger/proximal/proximal.urdf.xacro (+1/-0) sr_description/hand/xacro/forearm/forearm_motor.urdf.xacro (+1/-1) sr_description/hand/xacro/forearm/forearm_muscle.urdf.xacro (+1/-1) sr_description/hand/xacro/thumb/thdistal.urdf.xacro (+1/-1) sr_description/hand/xacro/thumb/thdistal_ellipsoid.urdf.xacro (+1/-1) sr_description/hand/xacro/wrist/wrist.urdf.xacro (+1/-1) sr_description/loaders/load_support_model.launch (+3/-0) sr_description/other/xacro/desk/desk.urdf.xacro (+16/-18) sr_description/other/xacro/desk/desk_robot.urdf.xacro (+5/-0) sr_description/robots/arm_and_hand_motor.urdf.xacro (+3/-6) sr_description/robots/arm_and_hand_motor_ellipsoid.urdf.xacro (+3/-6) sr_description/robots/arm_and_hand_motor_three_finger.urdf.xacro (+3/-6) sr_description/robots/arm_and_hand_muscle.urdf.xacro (+4/-6) sr_description/robots/arm_and_sr_one_finger_motor.urdf.xacro (+4/-6) sr_description/robots/desk_arm_and_hand_motor.urdf.xacro (+4/-6) sr_description/robots/sr_arm_motor.urdf.xacro (+4/-7) sr_description/robots/sr_arm_muscle.urdf.xacro (+4/-6) sr_hand/launch/advanced/static_transforms.launch (+5/-4) sr_hand/launch/gazebo/gazebo_arm.launch (+4/-2) sr_hand/launch/gazebo/gazebo_arm_and_hand.launch (+6/-2) sr_hand/launch/sr_arm_and_hand.launch (+4/-3) sr_mechanism_controllers/src/joint_spline_trajectory_action_controller.cpp (+18/-9) sr_move_arm/src/reactive_actions/reactive_grasp.py (+16/-7) sr_tactile_sensors/include/sr_tactile_sensors/sr_generic_tactile_sensor.hpp (+1/-0) sr_tactile_sensors/src/sr_generic_tactile_sensor.cpp (+30/-1) |
To merge this branch: | bzr merge lp:~guihome/sr-ros-interface/manip-electric_corr3 |
Related bugs: |
Reviewer | Review Type | Date Requested | Status |
---|---|---|---|
Shadow Robot | Pending | ||
Review via email: mp+132254@code.launchpad.net |
Commit message
Description of the change
This update works in pair with the newer manipulation stack (removed frames), trying to make the stack more generic to add any platform support (hence renaming it support and separating from the main urdf)
Corrected joint_spline_
To post a comment you must log in.
Preview Diff
[H/L] Next/Prev Comment, [J/K] Next/Prev File, [N/P] Next/Prev Hunk
1 | === modified file 'sr_description/arm/xacro/base/shadowarm_base.urdf.xacro' |
2 | --- sr_description/arm/xacro/base/shadowarm_base.urdf.xacro 2012-07-23 13:49:05 +0000 |
3 | +++ sr_description/arm/xacro/base/shadowarm_base.urdf.xacro 2012-10-31 08:34:23 +0000 |
4 | @@ -26,7 +26,7 @@ |
5 | </link> |
6 | <gazebo reference="shadowarm_base"> |
7 | <material>Gazebo/Grey</material> |
8 | - <selfCollide>true</selfCollide> |
9 | + <selfCollide>false</selfCollide> |
10 | </gazebo> |
11 | |
12 | </xacro:macro> |
13 | |
14 | === modified file 'sr_description/arm/xacro/hand_support/shadowarm_handsupport_motor.urdf.xacro' |
15 | --- sr_description/arm/xacro/hand_support/shadowarm_handsupport_motor.urdf.xacro 2012-07-24 07:32:08 +0000 |
16 | +++ sr_description/arm/xacro/hand_support/shadowarm_handsupport_motor.urdf.xacro 2012-10-31 08:34:23 +0000 |
17 | @@ -26,7 +26,7 @@ |
18 | </link> |
19 | <gazebo reference="shadowarm_handsupport"> |
20 | <material>Gazebo/Grey</material> |
21 | - <selfCollide>true</selfCollide> |
22 | + <selfCollide>false</selfCollide> |
23 | </gazebo> |
24 | |
25 | <joint name="ElbowJRotate" type="revolute"> |
26 | |
27 | === modified file 'sr_description/arm/xacro/hand_support/shadowarm_handsupport_muscle.urdf.xacro' |
28 | --- sr_description/arm/xacro/hand_support/shadowarm_handsupport_muscle.urdf.xacro 2012-07-24 07:32:08 +0000 |
29 | +++ sr_description/arm/xacro/hand_support/shadowarm_handsupport_muscle.urdf.xacro 2012-10-31 08:34:23 +0000 |
30 | @@ -26,7 +26,7 @@ |
31 | </link> |
32 | <gazebo reference="shadowarm_handsupport"> |
33 | <material>Gazebo/Grey</material> |
34 | - <selfCollide>true</selfCollide> |
35 | + <selfCollide>false</selfCollide> |
36 | </gazebo> |
37 | |
38 | <joint name="ElbowJRotate" type="revolute"> |
39 | |
40 | === modified file 'sr_description/arm/xacro/lower_arm/shadowarm_lowerarm.urdf.xacro' |
41 | --- sr_description/arm/xacro/lower_arm/shadowarm_lowerarm.urdf.xacro 2012-07-23 12:23:41 +0000 |
42 | +++ sr_description/arm/xacro/lower_arm/shadowarm_lowerarm.urdf.xacro 2012-10-31 08:34:23 +0000 |
43 | @@ -26,7 +26,7 @@ |
44 | </link> |
45 | <gazebo reference="shadowarm_lowerarm"> |
46 | <material>Gazebo/Grey</material> |
47 | - <selfCollide>true</selfCollide> |
48 | + <selfCollide>false</selfCollide> |
49 | </gazebo> |
50 | |
51 | <joint name="ElbowJSwing" type="revolute"> |
52 | |
53 | === modified file 'sr_description/arm/xacro/trunk/shadowarm_trunk.urdf.xacro' |
54 | --- sr_description/arm/xacro/trunk/shadowarm_trunk.urdf.xacro 2012-07-23 12:23:41 +0000 |
55 | +++ sr_description/arm/xacro/trunk/shadowarm_trunk.urdf.xacro 2012-10-31 08:34:23 +0000 |
56 | @@ -27,7 +27,7 @@ |
57 | </link> |
58 | <gazebo reference="shadowarm_trunk"> |
59 | <material>Gazebo/Grey</material> |
60 | - <selfCollide>true</selfCollide> |
61 | + <selfCollide>false</selfCollide> |
62 | </gazebo> |
63 | |
64 | <joint name="ShoulderJRotate" type="revolute"> |
65 | |
66 | === modified file 'sr_description/arm/xacro/upper_arm/shadowarm_upperarm.urdf.xacro' |
67 | --- sr_description/arm/xacro/upper_arm/shadowarm_upperarm.urdf.xacro 2012-07-23 12:23:41 +0000 |
68 | +++ sr_description/arm/xacro/upper_arm/shadowarm_upperarm.urdf.xacro 2012-10-31 08:34:23 +0000 |
69 | @@ -26,7 +26,7 @@ |
70 | </link> |
71 | <gazebo reference="shadowarm_upperarm"> |
72 | <material>Gazebo/Grey</material> |
73 | - <selfCollide>true</selfCollide> |
74 | + <selfCollide>false</selfCollide> |
75 | </gazebo> |
76 | |
77 | <joint name="ShoulderJSwing" type="revolute"> |
78 | |
79 | === modified file 'sr_description/hand/xacro/finger/distal/distal.urdf.xacro' |
80 | --- sr_description/hand/xacro/finger/distal/distal.urdf.xacro 2012-07-23 13:49:05 +0000 |
81 | +++ sr_description/hand/xacro/finger/distal/distal.urdf.xacro 2012-10-31 08:34:23 +0000 |
82 | @@ -50,7 +50,7 @@ |
83 | <kd value="1.0" /> |
84 | |
85 | <material>Gazebo/Grey</material> |
86 | - <selfCollide>false</selfCollide> |
87 | + <selfCollide>true</selfCollide> |
88 | </gazebo> |
89 | |
90 | <joint name="${joint_prefix}J1" type="revolute"> |
91 | |
92 | === modified file 'sr_description/hand/xacro/finger/distal/distal_ellipsoid.urdf.xacro' |
93 | --- sr_description/hand/xacro/finger/distal/distal_ellipsoid.urdf.xacro 2012-07-23 13:49:05 +0000 |
94 | +++ sr_description/hand/xacro/finger/distal/distal_ellipsoid.urdf.xacro 2012-10-31 08:34:23 +0000 |
95 | @@ -55,7 +55,7 @@ |
96 | <kp value="10000000.0" /> |
97 | <kd value="1.0" /> |
98 | <material>Gazebo/Grey</material> |
99 | - <selfCollide>false</selfCollide> |
100 | + <selfCollide>true</selfCollide> |
101 | </gazebo> |
102 | |
103 | <joint name="${joint_prefix}J1" type="revolute"> |
104 | |
105 | === modified file 'sr_description/hand/xacro/finger/middle/middle.urdf.xacro' |
106 | --- sr_description/hand/xacro/finger/middle/middle.urdf.xacro 2012-07-23 13:49:05 +0000 |
107 | +++ sr_description/hand/xacro/finger/middle/middle.urdf.xacro 2012-10-31 08:34:23 +0000 |
108 | @@ -44,6 +44,7 @@ |
109 | </controller:gazebo_ros_bumper> |
110 | </sensor:contact> |
111 | <material>Gazebo/Grey</material> |
112 | + <selfCollide>true</selfCollide> |
113 | </gazebo> |
114 | |
115 | <joint name="${joint_prefix}J2" type="revolute"> |
116 | |
117 | === modified file 'sr_description/hand/xacro/finger/proximal/proximal.urdf.xacro' |
118 | --- sr_description/hand/xacro/finger/proximal/proximal.urdf.xacro 2012-07-23 13:49:05 +0000 |
119 | +++ sr_description/hand/xacro/finger/proximal/proximal.urdf.xacro 2012-10-31 08:34:23 +0000 |
120 | @@ -44,6 +44,7 @@ |
121 | </controller:gazebo_ros_bumper> |
122 | </sensor:contact> |
123 | <material>Gazebo/Grey</material> |
124 | + <selfCollide>true</selfCollide> |
125 | </gazebo> |
126 | |
127 | <joint name="${joint_prefix}J3" type="revolute"> |
128 | |
129 | === modified file 'sr_description/hand/xacro/forearm/forearm_motor.urdf.xacro' |
130 | --- sr_description/hand/xacro/forearm/forearm_motor.urdf.xacro 2012-07-23 14:42:29 +0000 |
131 | +++ sr_description/hand/xacro/forearm/forearm_motor.urdf.xacro 2012-10-31 08:34:23 +0000 |
132 | @@ -26,7 +26,7 @@ |
133 | </link> |
134 | <gazebo reference="forearm"> |
135 | <material>Gazebo/Grey</material> |
136 | - <selfCollide>true</selfCollide> |
137 | + <selfCollide>false</selfCollide> |
138 | </gazebo> |
139 | |
140 | </xacro:macro> |
141 | |
142 | === modified file 'sr_description/hand/xacro/forearm/forearm_muscle.urdf.xacro' |
143 | --- sr_description/hand/xacro/forearm/forearm_muscle.urdf.xacro 2012-07-23 14:42:29 +0000 |
144 | +++ sr_description/hand/xacro/forearm/forearm_muscle.urdf.xacro 2012-10-31 08:34:23 +0000 |
145 | @@ -26,7 +26,7 @@ |
146 | </link> |
147 | <gazebo reference="forearm"> |
148 | <material>Gazebo/Grey</material> |
149 | - <selfCollide>true</selfCollide> |
150 | + <selfCollide>false</selfCollide> |
151 | </gazebo> |
152 | |
153 | |
154 | |
155 | === modified file 'sr_description/hand/xacro/thumb/thdistal.urdf.xacro' |
156 | --- sr_description/hand/xacro/thumb/thdistal.urdf.xacro 2012-07-23 13:49:05 +0000 |
157 | +++ sr_description/hand/xacro/thumb/thdistal.urdf.xacro 2012-10-31 08:34:23 +0000 |
158 | @@ -53,7 +53,7 @@ |
159 | <kd value="1.0" /> |
160 | |
161 | <material>Gazebo/Grey</material> |
162 | - <selfCollide>false</selfCollide> |
163 | + <selfCollide>true</selfCollide> |
164 | </gazebo> |
165 | |
166 | <joint name="THJ1" type="revolute"> |
167 | |
168 | === modified file 'sr_description/hand/xacro/thumb/thdistal_ellipsoid.urdf.xacro' |
169 | --- sr_description/hand/xacro/thumb/thdistal_ellipsoid.urdf.xacro 2012-07-23 13:49:05 +0000 |
170 | +++ sr_description/hand/xacro/thumb/thdistal_ellipsoid.urdf.xacro 2012-10-31 08:34:23 +0000 |
171 | @@ -51,7 +51,7 @@ |
172 | <kd value="1.0" /> |
173 | |
174 | <material>Gazebo/Grey</material> |
175 | - <selfCollide>false</selfCollide> |
176 | + <selfCollide>true</selfCollide> |
177 | </gazebo> |
178 | |
179 | <joint name="THJ1" type="revolute"> |
180 | |
181 | === modified file 'sr_description/hand/xacro/wrist/wrist.urdf.xacro' |
182 | --- sr_description/hand/xacro/wrist/wrist.urdf.xacro 2012-07-23 13:49:05 +0000 |
183 | +++ sr_description/hand/xacro/wrist/wrist.urdf.xacro 2012-10-31 08:34:23 +0000 |
184 | @@ -43,7 +43,7 @@ |
185 | </link> |
186 | <gazebo reference="wrist"> |
187 | <material>PR2/Grey2</material> |
188 | - <selfCollide>true</selfCollide> |
189 | + <selfCollide>false</selfCollide> |
190 | </gazebo> |
191 | |
192 | <joint name="WRJ2" type="revolute"> |
193 | |
194 | === added file 'sr_description/loaders/load_support_model.launch' |
195 | --- sr_description/loaders/load_support_model.launch 1970-01-01 00:00:00 +0000 |
196 | +++ sr_description/loaders/load_support_model.launch 2012-10-31 08:34:23 +0000 |
197 | @@ -0,0 +1,3 @@ |
198 | +<launch> |
199 | + <param name="support_description" command="$(find xacro)/xacro.py '$(find sr_description)/other/xacro/desk/desk_robot.urdf.xacro'" /> |
200 | +</launch> |
201 | |
202 | === modified file 'sr_description/other/xacro/desk/desk.urdf.xacro' |
203 | --- sr_description/other/xacro/desk/desk.urdf.xacro 2012-07-23 13:49:05 +0000 |
204 | +++ sr_description/other/xacro/desk/desk.urdf.xacro 2012-10-31 08:34:23 +0000 |
205 | @@ -1,39 +1,35 @@ |
206 | <robot name="desk" xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body" xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#slider" xmlns:xacro="http://ros.org/wiki/xacro"> |
207 | <xacro:macro name="desk"> |
208 | - |
209 | + <link name="world"/> |
210 | <link name="desk_pillar"> |
211 | <inertial> |
212 | <origin xyz="0 0 0.5"/> |
213 | - <mass value="1000.0"/> |
214 | + <mass value="10.0"/> |
215 | <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> |
216 | </inertial> |
217 | <visual> |
218 | <origin rpy="0 0 0" xyz="0 0 0.51"/> |
219 | <geometry name="desk_visual"> |
220 | - <cylinder length="0.98" radius="0.1"/> |
221 | + <cylinder length="0.97" radius="0.1"/> |
222 | </geometry> |
223 | <material name="Green"/> |
224 | </visual> |
225 | <collision> |
226 | <origin rpy="0 0 0" xyz="0 0 0.51"/> |
227 | <geometry name="desk_collision"> |
228 | - <cylinder length=".98" radius="0.1"/> |
229 | + <cylinder length=".97" radius="0.05"/> |
230 | </geometry> |
231 | </collision> |
232 | </link> |
233 | <gazebo reference="desk_pillar"> |
234 | <material>Gazebo/Green</material> |
235 | - <mu1>1000.0</mu1> |
236 | - <mu2>1000.0</mu2> |
237 | - <kp>10000000.0</kp> |
238 | - <kd>1.0</kd> |
239 | - <selfCollide>true</selfCollide> |
240 | + <selfCollide>false</selfCollide> |
241 | </gazebo> |
242 | |
243 | <link name="desk_support"> |
244 | <inertial> |
245 | <origin xyz="0 0 0"/> |
246 | - <mass value="1000.0"/> |
247 | + <mass value="10.0"/> |
248 | <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> |
249 | </inertial> |
250 | <visual> |
251 | @@ -46,24 +42,26 @@ |
252 | <collision> |
253 | <origin rpy="0 0 0" xyz="0 0 0.01"/> |
254 | <geometry name="desk_s_collision"> |
255 | - <cylinder length="0.02" radius="1.0"/> |
256 | + <cylinder length="0.002" radius="0.01"/> |
257 | </geometry> |
258 | </collision> |
259 | </link> |
260 | <gazebo reference="desk_support"> |
261 | <material>Gazebo/Green</material> |
262 | - <mu1>1000.0</mu1> |
263 | - <mu2>1000.0</mu2> |
264 | - <kp>10000000.0</kp> |
265 | - <kd>1.0</kd> |
266 | - <selfCollide>true</selfCollide> |
267 | + <selfCollide>false</selfCollide> |
268 | </gazebo> |
269 | |
270 | <joint name="desk_joint" type="fixed"> |
271 | <parent link="desk_support"/> |
272 | <child link="desk_pillar"/> |
273 | - <origin rpy="0 0 0" xyz="0 0 0.02"/> |
274 | - </joint> |
275 | + <origin rpy="0 0 0" xyz="0 0 0"/> |
276 | + </joint> |
277 | + <joint name="desk_to_world_joint" type="fixed"> |
278 | + <parent link="world"/> |
279 | + <child link="desk_support"/> |
280 | + <origin rpy="0 0 0" xyz="0 0 0"/> |
281 | + </joint> |
282 | + |
283 | |
284 | </xacro:macro> |
285 | </robot> |
286 | |
287 | === added file 'sr_description/other/xacro/desk/desk_robot.urdf.xacro' |
288 | --- sr_description/other/xacro/desk/desk_robot.urdf.xacro 1970-01-01 00:00:00 +0000 |
289 | +++ sr_description/other/xacro/desk/desk_robot.urdf.xacro 2012-10-31 08:34:23 +0000 |
290 | @@ -0,0 +1,5 @@ |
291 | +<robot name="desk" xmlns:xacro="http://ros.org/wiki/xacro"> |
292 | + <include filename="$(find sr_description)/materials.urdf.xacro" /> |
293 | + <include filename="$(find sr_description)/other/xacro/desk/desk.urdf.xacro" /> |
294 | + <xacro:desk /> |
295 | +</robot> |
296 | |
297 | === modified file 'sr_description/robots/arm_and_hand_motor.urdf.xacro' |
298 | --- sr_description/robots/arm_and_hand_motor.urdf.xacro 2012-07-24 07:32:08 +0000 |
299 | +++ sr_description/robots/arm_and_hand_motor.urdf.xacro 2012-10-31 08:34:23 +0000 |
300 | @@ -5,12 +5,9 @@ |
301 | <include filename="$(find sr_description)/hand/xacro/full_hand_motor.urdf.xacro" /> |
302 | <include filename="$(find sr_description)/other/xacro/gazebo/gazebo.urdf.xacro" /> |
303 | |
304 | - <include filename="$(find sr_description)/other/xacro/desk/desk.urdf.xacro" /> |
305 | - |
306 | - <xacro:desk/> |
307 | - |
308 | - <joint name="arm_to_desk_fixed" type="fixed"> |
309 | - <parent link="desk_pillar"/> |
310 | + <link name="world" /> |
311 | + <joint name="arm_to_world_fixed" type="fixed"> |
312 | + <parent link="world"/> |
313 | <child link="shadowarm_base"/> |
314 | <origin rpy="0 0 0" xyz="0 0 1.0"/> |
315 | </joint> |
316 | |
317 | === modified file 'sr_description/robots/arm_and_hand_motor_ellipsoid.urdf.xacro' |
318 | --- sr_description/robots/arm_and_hand_motor_ellipsoid.urdf.xacro 2012-07-24 07:32:08 +0000 |
319 | +++ sr_description/robots/arm_and_hand_motor_ellipsoid.urdf.xacro 2012-10-31 08:34:23 +0000 |
320 | @@ -5,12 +5,9 @@ |
321 | <include filename="$(find sr_description)/hand/xacro/full_hand_motor_ellipsoid.urdf.xacro" /> |
322 | <include filename="$(find sr_description)/other/xacro/gazebo/gazebo.urdf.xacro" /> |
323 | |
324 | - <include filename="$(find sr_description)/other/xacro/desk/desk.urdf.xacro" /> |
325 | - |
326 | - <xacro:desk/> |
327 | - |
328 | - <joint name="arm_to_desk_fixed" type="fixed"> |
329 | - <parent link="desk_pillar"/> |
330 | + <link name="world" /> |
331 | + <joint name="arm_to_world_fixed" type="fixed"> |
332 | + <parent link="world"/> |
333 | <child link="shadowarm_base"/> |
334 | <origin rpy="0 0 0" xyz="0 0 1.0"/> |
335 | </joint> |
336 | |
337 | === modified file 'sr_description/robots/arm_and_hand_motor_three_finger.urdf.xacro' |
338 | --- sr_description/robots/arm_and_hand_motor_three_finger.urdf.xacro 2012-07-24 07:32:08 +0000 |
339 | +++ sr_description/robots/arm_and_hand_motor_three_finger.urdf.xacro 2012-10-31 08:34:23 +0000 |
340 | @@ -5,12 +5,9 @@ |
341 | <include filename="$(find sr_description)/hand/xacro/three_finger_unit_motor.urdf.xacro" /> |
342 | <include filename="$(find sr_description)/other/xacro/gazebo/gazebo.urdf.xacro" /> |
343 | |
344 | - <include filename="$(find sr_description)/other/xacro/desk/desk.urdf.xacro" /> |
345 | - |
346 | - <xacro:desk/> |
347 | - |
348 | - <joint name="arm_to_desk_fixed" type="fixed"> |
349 | - <parent link="desk_pillar"/> |
350 | + <link name="world" /> |
351 | + <joint name="arm_to_world_fixed" type="fixed"> |
352 | + <parent link="world"/> |
353 | <child link="shadowarm_base"/> |
354 | <origin rpy="0 0 0" xyz="0 0 1.0"/> |
355 | </joint> |
356 | |
357 | === modified file 'sr_description/robots/arm_and_hand_muscle.urdf.xacro' |
358 | --- sr_description/robots/arm_and_hand_muscle.urdf.xacro 2012-07-24 07:32:08 +0000 |
359 | +++ sr_description/robots/arm_and_hand_muscle.urdf.xacro 2012-10-31 08:34:23 +0000 |
360 | @@ -4,12 +4,10 @@ |
361 | <include filename="$(find sr_description)/arm/xacro/full_arm_muscle.urdf.xacro" /> |
362 | <include filename="$(find sr_description)/hand/xacro/full_hand_muscle.urdf.xacro" /> |
363 | <include filename="$(find sr_description)/other/xacro/gazebo/gazebo.urdf.xacro" /> |
364 | - <include filename="$(find sr_description)/other/xacro/desk/desk.urdf.xacro" /> |
365 | - |
366 | - <xacro:desk/> |
367 | - |
368 | - <joint name="arm_to_desk_fixed" type="fixed"> |
369 | - <parent link="desk_pillar"/> |
370 | + |
371 | + <link name="world" /> |
372 | + <joint name="arm_to_world_fixed" type="fixed"> |
373 | + <parent link="world"/> |
374 | <child link="shadowarm_base"/> |
375 | <origin rpy="0 0 0" xyz="0 0 1.0"/> |
376 | </joint> |
377 | |
378 | === modified file 'sr_description/robots/arm_and_sr_one_finger_motor.urdf.xacro' |
379 | --- sr_description/robots/arm_and_sr_one_finger_motor.urdf.xacro 2012-07-24 07:32:08 +0000 |
380 | +++ sr_description/robots/arm_and_sr_one_finger_motor.urdf.xacro 2012-10-31 08:34:23 +0000 |
381 | @@ -4,12 +4,10 @@ |
382 | <include filename="$(find sr_description)/arm/xacro/full_arm_motor.urdf.xacro" /> |
383 | <include filename="$(find sr_description)/hand/xacro/one_finger_unit_motor.urdf.xacro" /> |
384 | <include filename="$(find sr_description)/other/xacro/gazebo/gazebo.urdf.xacro" /> |
385 | - <include filename="$(find sr_description)/other/xacro/desk/desk.urdf.xacro" /> |
386 | - |
387 | - <xacro:desk/> |
388 | - |
389 | - <joint name="arm_to_desk_fixed" type="fixed"> |
390 | - <parent link="desk_pillar"/> |
391 | + |
392 | + <link name="world" /> |
393 | + <joint name="arm_to_world_fixed" type="fixed"> |
394 | + <parent link="world"/> |
395 | <child link="shadowarm_base"/> |
396 | <origin rpy="0 0 0" xyz="0 0 1.0"/> |
397 | </joint> |
398 | |
399 | === modified file 'sr_description/robots/desk_arm_and_hand_motor.urdf.xacro' |
400 | --- sr_description/robots/desk_arm_and_hand_motor.urdf.xacro 2012-07-24 07:32:08 +0000 |
401 | +++ sr_description/robots/desk_arm_and_hand_motor.urdf.xacro 2012-10-31 08:34:23 +0000 |
402 | @@ -4,12 +4,10 @@ |
403 | <include filename="$(find sr_description)/arm/xacro/full_arm_motor.urdf.xacro" /> |
404 | <include filename="$(find sr_description)/hand/xacro/full_hand_motor.urdf.xacro" /> |
405 | <include filename="$(find sr_description)/other/xacro/gazebo/gazebo.urdf.xacro" /> |
406 | - <include filename="$(find sr_description)/other/xacro/desk/desk.urdf.xacro" /> |
407 | - |
408 | - <xacro:desk/> |
409 | - |
410 | - <joint name="base_fix" type="fixed"> |
411 | - <parent link="desk_pillar"/> |
412 | + |
413 | + <link name="world" /> |
414 | + <joint name="arm_to_world_fixed" type="fixed"> |
415 | + <parent link="world"/> |
416 | <child link="shadowarm_base"/> |
417 | <origin rpy="0 0 0" xyz="0 0 1.0"/> |
418 | </joint> |
419 | |
420 | === modified file 'sr_description/robots/sr_arm_motor.urdf.xacro' |
421 | --- sr_description/robots/sr_arm_motor.urdf.xacro 2012-07-23 13:49:05 +0000 |
422 | +++ sr_description/robots/sr_arm_motor.urdf.xacro 2012-10-31 08:34:23 +0000 |
423 | @@ -6,13 +6,10 @@ |
424 | |
425 | <include filename="$(find sr_description)/materials.urdf.xacro" /> |
426 | <include filename="$(find sr_description)/arm/xacro/full_arm_motor.urdf.xacro" /> |
427 | - <include filename="$(find sr_description)/other/xacro/desk/desk.urdf.xacro" /> |
428 | <include filename="$(find sr_description)/other/xacro/gazebo/gazebo.urdf.xacro" /> |
429 | - |
430 | - <xacro:desk/> |
431 | - |
432 | - <joint name="arm_to_desk_fixed" type="fixed"> |
433 | - <parent link="desk_pillar"/> |
434 | + <link name="world" /> |
435 | + <joint name="arm_to_world_fixed" type="fixed"> |
436 | + <parent link="world"/> |
437 | <child link="shadowarm_base"/> |
438 | <origin rpy="0 0 0" xyz="0 0 1.0"/> |
439 | </joint> |
440 | @@ -20,4 +17,4 @@ |
441 | <xacro:sr_arm_motor> |
442 | </xacro:sr_arm_motor> |
443 | |
444 | -</robot> |
445 | \ No newline at end of file |
446 | +</robot> |
447 | |
448 | === modified file 'sr_description/robots/sr_arm_muscle.urdf.xacro' |
449 | --- sr_description/robots/sr_arm_muscle.urdf.xacro 2012-07-23 13:49:05 +0000 |
450 | +++ sr_description/robots/sr_arm_muscle.urdf.xacro 2012-10-31 08:34:23 +0000 |
451 | @@ -4,15 +4,13 @@ |
452 | xmlns:xacro="http://www.ros.org/wiki/xacro" |
453 | name="sr_arm_muscle"> |
454 | |
455 | - <include filename="$(find sr_description)/other/xacro/desk/desk.urdf.xacro" /> |
456 | <include filename="$(find sr_description)/materials.urdf.xacro" /> |
457 | <include filename="$(find sr_description)/arm/xacro/full_arm_muscle.urdf.xacro" /> |
458 | <include filename="$(find sr_description)/other/xacro/gazebo/gazebo.urdf.xacro" /> |
459 | |
460 | - <xacro:desk/> |
461 | - |
462 | - <joint name="arm_to_desk_fixed" type="fixed"> |
463 | - <parent link="desk_pillar"/> |
464 | + <link name="world" /> |
465 | + <joint name="arm_to_world_fixed" type="fixed"> |
466 | + <parent link="world"/> |
467 | <child link="shadowarm_base"/> |
468 | <origin rpy="0 0 0" xyz="0 0 1.0"/> |
469 | </joint> |
470 | @@ -20,4 +18,4 @@ |
471 | <xacro:sr_arm_muscle> |
472 | </xacro:sr_arm_muscle> |
473 | |
474 | -</robot> |
475 | \ No newline at end of file |
476 | +</robot> |
477 | |
478 | === modified file 'sr_hand/launch/advanced/static_transforms.launch' |
479 | --- sr_hand/launch/advanced/static_transforms.launch 2012-07-24 07:34:33 +0000 |
480 | +++ sr_hand/launch/advanced/static_transforms.launch 2012-10-31 08:34:23 +0000 |
481 | @@ -1,8 +1,9 @@ |
482 | <launch> |
483 | - <node pkg="tf" type="static_transform_publisher" name="world_to_fixed" |
484 | - args="0 0 0 0 0 0 /world /fixed 100"/> |
485 | - <node pkg="tf" type="static_transform_publisher" name="world_to_base_link" |
486 | - args="0 0 0 0 0 0 /world /base_link 100"/> |
487 | + |
488 | +<!-- we start this in sr_manipulation <node pkg="tf" type="static_transform_publisher" name="world_to_fixed" |
489 | + args="0 0 0 0 0 0 /world /fixed 100"/> --> |
490 | +<!-- <node pkg="tf" type="static_transform_publisher" name="world_to_base_link" |
491 | + args="0 0 0 0 0 0 /world /base_link 100"/> --> |
492 | <!-- <node pkg="tf" type="static_transform_publisher" name="base_link_to_fixed" |
493 | args="0 0 0 0 0 0 /base_link /fixed 100"/>--> |
494 | |
495 | |
496 | === modified file 'sr_hand/launch/gazebo/gazebo_arm.launch' |
497 | --- sr_hand/launch/gazebo/gazebo_arm.launch 2012-07-24 13:43:35 +0000 |
498 | +++ sr_hand/launch/gazebo/gazebo_arm.launch 2012-10-31 08:34:23 +0000 |
499 | @@ -2,6 +2,9 @@ |
500 | |
501 | <!-- start gazebo with an empty plane --> |
502 | <include file="$(find sr_hand)/launch/gazebo/loaders/empty_world.launch"/> |
503 | + <!-- spawn the support model --> |
504 | + <include file="$(find sr_description)/loaders/load_support_model.launch" /> |
505 | + <node name="spawn_support" pkg="gazebo" type="spawn_model" args="-urdf -param support_description -z 0.0 -model support_model" respawn="false" output="screen" /> |
506 | |
507 | <!-- spawn the arm model --> |
508 | <group if="$(optenv MUSCLE 0)"> |
509 | @@ -10,8 +13,7 @@ |
510 | <group unless="$(optenv MUSCLE 0)"> |
511 | <param name="robot_description" command="$(find xacro)/xacro.py '$(find sr_description)/robots/sr_arm_motor.urdf.xacro'" /> |
512 | </group> |
513 | - |
514 | - <node name="spawn_hand" pkg="gazebo" type="spawn_model" args="-urdf -param robot_description -z 0.01 -model shadow_model -J ShoulderJSwing 0.78 -J ElbowJSwing 2.0" respawn="false" output="screen" /> |
515 | + <node name="spawn_hand" pkg="gazebo" type="spawn_model" args="-urdf -param robot_description -z 0.0 -model shadow_model -J ShoulderJSwing 0.78 -J ElbowJSwing 2.0" respawn="false" output="screen" /> |
516 | |
517 | <!-- Controllers for the arm --> |
518 | <include file="$(find sr_hand)/launch/gazebo/loaders/arm_controllers.launch"/> |
519 | |
520 | === modified file 'sr_hand/launch/gazebo/gazebo_arm_and_hand.launch' |
521 | --- sr_hand/launch/gazebo/gazebo_arm_and_hand.launch 2012-08-28 09:41:05 +0000 |
522 | +++ sr_hand/launch/gazebo/gazebo_arm_and_hand.launch 2012-10-31 08:34:23 +0000 |
523 | @@ -3,9 +3,13 @@ |
524 | <!-- start gazebo with an empty plane --> |
525 | <include file="$(find sr_hand)/launch/gazebo/loaders/empty_world.launch"/> |
526 | |
527 | + <!-- spawn the support model --> |
528 | + <include file="$(find sr_description)/loaders/load_support_model.launch" /> |
529 | + <node name="spawn_support" pkg="gazebo" type="spawn_model" args="-urdf -param support_description -z 0.0 -model support_model" respawn="false" output="screen" /> |
530 | + |
531 | + <!-- spawn the arm and hand model --> |
532 | <include file="$(find sr_description)/loaders/load_arm_and_hand_model.launch" /> |
533 | - |
534 | - <node name="spawn_hand" pkg="gazebo" type="spawn_model" args="-urdf -param robot_description -z 0.01 -model shadow_model -J ShoulderJSwing 0.78 -J ElbowJSwing 2.0" respawn="false" output="screen" /> |
535 | + <node name="spawn_hand" pkg="gazebo" type="spawn_model" args="-urdf -param robot_description -z 0.0 -model shadow_model -J ShoulderJSwing 0.78 -J ElbowJSwing 2.0" respawn="false" output="screen" /> |
536 | |
537 | <!-- Controllers for the arm and hand --> |
538 | <include file="$(find sr_hand)/launch/gazebo/loaders/hand_controllers.launch"/> |
539 | |
540 | === modified file 'sr_hand/launch/sr_arm_and_hand.launch' |
541 | --- sr_hand/launch/sr_arm_and_hand.launch 2012-08-08 07:32:34 +0000 |
542 | +++ sr_hand/launch/sr_arm_and_hand.launch 2012-10-31 08:34:23 +0000 |
543 | @@ -75,11 +75,12 @@ |
544 | point at 0 0 0, to remap the arm base for both positions and |
545 | targets. |
546 | --> |
547 | - <node pkg="tf" type="static_transform_publisher" name="fixed_frame_pos_pub_arm" args="0 0 0 0 0 0 fixed /sr_arm/position/desk_support 100" /> |
548 | - <node pkg="tf" type="static_transform_publisher" name="fixed_frame_target_pub_arm" args="0 0 0 0 0 0 fixed /sr_arm/target/desk_support 100" /> |
549 | +<!-- try to get rid of /fixed replace it by /world --> |
550 | + <node pkg="tf" type="static_transform_publisher" name="fixed_frame_pos_pub_arm" args="0 0 0 0 0 0 /world /sr_arm/position/world 100" /> |
551 | + <node pkg="tf" type="static_transform_publisher" name="fixed_frame_target_pub_arm" args="0 0 0 0 0 0 /world /sr_arm/target/world 100" /> |
552 | |
553 | <node pkg="tf" type="static_transform_publisher" name="link_hand_arm_pos" args="0 0 0 2.3562 0 0 /sr_arm/position/shadowarm_handsupport /srh/position/forearm 100" /> |
554 | - <node pkg="tf" type="static_transform_publisher" name="link_hand_arm_target" args="0 0 0 2.3562 0 0 /sr_arm/target/shadowarm_handsupport /srh/target/forearm 100" /> |
555 | + <node pkg="tf" type="static_transform_publisher" name="link_hand_arm_target" args="0 0 0 2.3562 0 0 /sr_arm/target/shadowarm_handsupport /srh/target/forearm 100" /> |
556 | |
557 | <!-- Joint States positions merger to get one single vector for both arm and hand --> |
558 | <node pkg="sr_utilities" type="ordered_position_joint_states_merger.py" name="ordered_position_joint_states_merger"/> |
559 | |
560 | === modified file 'sr_mechanism_controllers/src/joint_spline_trajectory_action_controller.cpp' |
561 | --- sr_mechanism_controllers/src/joint_spline_trajectory_action_controller.cpp 2012-07-25 20:52:06 +0000 |
562 | +++ sr_mechanism_controllers/src/joint_spline_trajectory_action_controller.cpp 2012-10-31 08:34:23 +0000 |
563 | @@ -283,7 +283,8 @@ |
564 | { |
565 | bool success = true; |
566 | |
567 | - ros::Time time = ros::Time::now() + ros::Duration(0.1); |
568 | + ros::Time time = ros::Time::now() - ros::Duration(0.05); |
569 | + last_time_ = time; |
570 | ROS_DEBUG("Figuring out new trajectory at %.3lf, with data from %.3lf", |
571 | time.toSec(), goal->trajectory.header.stamp.toSec()); |
572 | |
573 | @@ -585,10 +586,12 @@ |
574 | void JointTrajectoryActionController::commandCB(const trajectory_msgs::JointTrajectoryConstPtr &msg) |
575 | { |
576 | bool success = true; |
577 | + |
578 | + ros::Time time = ros::Time::now()-ros::Duration(0.05); |
579 | + last_time_ = time; |
580 | |
581 | - ros::Time time = last_time_ + ros::Duration(0.01); |
582 | - ROS_DEBUG("Figuring out new trajectory at %.3lf, with data from %.3lf", |
583 | - time.toSec(), msg->header.stamp.toSec()); |
584 | + ROS_ERROR("Figuring out new trajectory at %.3lf, with data from %.3lf with %d waypoints", |
585 | + time.toSec(), msg->header.stamp.toSec(),msg->points.size()); |
586 | |
587 | boost::shared_ptr<SpecifiedTrajectory> new_traj_ptr(new SpecifiedTrajectory); |
588 | SpecifiedTrajectory &traj = *new_traj_ptr; |
589 | @@ -635,28 +638,34 @@ |
590 | for (size_t i = 0; i < msg->points.size(); ++i) |
591 | { |
592 | Segment seg; |
593 | - |
594 | + ROS_DEBUG("Current time %f and header time %f",msg->header.stamp.toSec(),ros::Time(0.0).toSec()); |
595 | if(msg->header.stamp == ros::Time(0.0)) |
596 | + { |
597 | seg.start_time = (time + msg->points[i].time_from_start).toSec() - durations[i]; |
598 | + ROS_DEBUG("Segment %d start time A %f,time_from_start %f, duration, %f",i,seg.start_time,msg->points[i].time_from_start.toSec(),durations[i]); |
599 | + } |
600 | else |
601 | + { |
602 | seg.start_time = (msg->header.stamp + msg->points[i].time_from_start).toSec() - durations[i]; |
603 | + ROS_DEBUG("Segment start time B %f",seg.start_time); |
604 | + } |
605 | seg.duration = durations[i]; |
606 | seg.splines.resize(joint_names_.size()); |
607 | |
608 | // Checks that the incoming segment has the right number of elements. |
609 | if (msg->points[i].accelerations.size() != 0 && msg->points[i].accelerations.size() != joint_names_.size()) |
610 | { |
611 | - ROS_ERROR("Command point %d has %d elements for the accelerations", (int)i, (int)msg->points[i].accelerations.size()); |
612 | + ROS_DEBUG("Command point %d has %d elements for the accelerations", (int)i, (int)msg->points[i].accelerations.size()); |
613 | return; |
614 | } |
615 | if (msg->points[i].velocities.size() != 0 && msg->points[i].velocities.size() != joint_names_.size()) |
616 | { |
617 | - ROS_ERROR("Command point %d has %d elements for the velocities", (int)i, (int)msg->points[i].velocities.size()); |
618 | + ROS_DEBUG("Command point %d has %d elements for the velocities", (int)i, (int)msg->points[i].velocities.size()); |
619 | return; |
620 | } |
621 | if (msg->points[i].positions.size() != joint_names_.size()) |
622 | { |
623 | - ROS_ERROR("Command point %d has %d elements for the positions", (int)i, (int)msg->points[i].positions.size()); |
624 | + ROS_DEBUG("Command point %d has %d elements for the positions", (int)i, (int)msg->points[i].positions.size()); |
625 | return; |
626 | } |
627 | |
628 | @@ -777,7 +786,7 @@ |
629 | // if the last trajectory is already in the past, stop the servoing |
630 | if( (traj[traj.size()-1].start_time+traj[traj.size()-1].duration) < time.toSec()) |
631 | { |
632 | - ROS_DEBUG("trajectory is finished %f<%f",(traj[traj.size()-1].start_time+traj[traj.size()-1].duration),time.toSec()); |
633 | + ROS_DEBUG("trajectory is finished %f<%f",(traj[traj.size()-1].start_time+traj[traj.size()-1].duration),time.toSec()); |
634 | break; |
635 | } |
636 | |
637 | |
638 | === modified file 'sr_move_arm/src/reactive_actions/reactive_grasp.py' |
639 | --- sr_move_arm/src/reactive_actions/reactive_grasp.py 2012-07-25 20:52:06 +0000 |
640 | +++ sr_move_arm/src/reactive_actions/reactive_grasp.py 2012-10-31 08:34:23 +0000 |
641 | @@ -10,7 +10,7 @@ |
642 | from object_manipulation_msgs.msg import GripperTranslation, ManipulationPhase |
643 | from sr_robot_msgs.msg import sendupdate, joint |
644 | from sr_robot_msgs.srv import which_fingers_are_touching |
645 | - |
646 | +from trajectory_msgs.msg import JointTrajectory |
647 | from actionlib import SimpleActionClient |
648 | #from motion_planning_msgs.msg import * |
649 | from arm_navigation_msgs.msg import * |
650 | @@ -28,6 +28,8 @@ |
651 | self.using_slip_detection = slip_detection |
652 | self.whicharm = which_arm |
653 | |
654 | + self.mypub = rospy.Publisher("/command",JointTrajectory) |
655 | + |
656 | #force to use when closing hard |
657 | self.close_force = 100 |
658 | if self.using_slip_detection: |
659 | @@ -42,7 +44,7 @@ |
660 | #root and tip name for listening |
661 | #to the current palm pose |
662 | self.tf_listener = tf.TransformListener() |
663 | - self.root_name = "base_link" |
664 | + self.root_name = "world" |
665 | self.tip_name = "palm" |
666 | |
667 | #for taking photos of each step |
668 | @@ -190,7 +192,13 @@ |
669 | #follow the joint_path if one is given |
670 | if joint_path != None: |
671 | self.check_preempt() |
672 | - joint_names = joint_path.joint_names |
673 | + |
674 | + self.mypub.publish(joint_path) |
675 | + rospy.logerr("Sent a trajectory !") |
676 | + rospy.sleep(10) |
677 | + rospy.logerr("Trajectory should be finished !") |
678 | + '''joint_names = joint_path.joint_names |
679 | + countTime = 5 |
680 | |
681 | for trajectory_step in joint_path.points: |
682 | sendupdate_msg = [] |
683 | @@ -203,11 +211,12 @@ |
684 | self.sr_hand_target_pub.publish(sendupdate(len(sendupdate_msg), sendupdate_msg) ) |
685 | |
686 | current_sleep_time = trajectory_step.time_from_start - last_time |
687 | - rospy.sleep( current_sleep_time ) |
688 | + rospy.sleep( countTime ) # current_sleep_time ) |
689 | + countTime=max(countTime-1,0.5) |
690 | last_time = trajectory_step.time_from_start |
691 | else: |
692 | #if no joint_path is given, go to the current_goal_pose |
693 | - self.command_cartesian( current_goal_pose ) |
694 | + self.command_cartesian( current_goal_pose )''' |
695 | |
696 | return self.reactive_approach_result_dict["success"] |
697 | |
698 | @@ -414,7 +423,7 @@ |
699 | approach_vect = scipy.array(grasp_pos) - scipy.array(approach_pos) |
700 | return approach_vect |
701 | |
702 | - def return_rel_pose(self, vector, frame, start_pose = None, orthogonal_to_vect = None, orthogonal_to_vect_frame = 'base_link'): |
703 | + def return_rel_pose(self, vector, frame, start_pose = None, orthogonal_to_vect = None, orthogonal_to_vect_frame = 'world'): |
704 | """ |
705 | convert a relative vector in frame to a pose in the base_link frame |
706 | if start_pose is not specified, uses current pose of the wrist |
707 | @@ -468,7 +477,7 @@ |
708 | |
709 | self.sr_hand_target_pub.publish(sendupdate(len(sendupdate_msg), sendupdate_msg) ) |
710 | |
711 | - def command_cartesian(self, pose, frame_id='base_link'): |
712 | + def command_cartesian(self, pose, frame_id='world'): |
713 | """ |
714 | Tries to go to the given pose with the arm. |
715 | """ |
716 | |
717 | === modified file 'sr_tactile_sensors/include/sr_tactile_sensors/sr_generic_tactile_sensor.hpp' |
718 | --- sr_tactile_sensors/include/sr_tactile_sensors/sr_generic_tactile_sensor.hpp 2011-11-01 15:24:16 +0000 |
719 | +++ sr_tactile_sensors/include/sr_tactile_sensors/sr_generic_tactile_sensor.hpp 2012-10-31 08:34:23 +0000 |
720 | @@ -34,6 +34,7 @@ |
721 | |
722 | #include <boost/smart_ptr.hpp> |
723 | #include <std_msgs/Float64.h> |
724 | +#include <XmlRpcValue.h> |
725 | #include <sr_robot_msgs/is_hand_occupied.h> |
726 | #include <sr_robot_msgs/which_fingers_are_touching.h> |
727 | |
728 | |
729 | === modified file 'sr_tactile_sensors/src/sr_generic_tactile_sensor.cpp' |
730 | --- sr_tactile_sensors/src/sr_generic_tactile_sensor.cpp 2011-11-01 15:24:16 +0000 |
731 | +++ sr_tactile_sensors/src/sr_generic_tactile_sensor.cpp 2012-10-31 08:34:23 +0000 |
732 | @@ -57,6 +57,7 @@ |
733 | SrTactileSensorManager::SrTactileSensorManager() : |
734 | n_tilde("~"), publish_rate(20.0) |
735 | { |
736 | + using namespace XmlRpc; |
737 | double publish_freq; |
738 | n_tilde.param("publish_frequency", publish_freq, 20.0); |
739 | publish_rate = ros::Rate(publish_freq); |
740 | @@ -64,7 +65,34 @@ |
741 | //initializing the thresholds to test if the hand is holding |
742 | //something or not (compared agains the pressure value). |
743 | double tmp[5]={117,117,113,111,0}; |
744 | - is_hand_occupied_thresholds = std::vector<double>(tmp, tmp+5); |
745 | + XmlRpc::XmlRpcValue threshold_xmlrpc; |
746 | + if(n_tilde.getParam("/grasp_touch_thresholds",threshold_xmlrpc)) |
747 | + { |
748 | + ROS_ASSERT(threshold_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeArray); |
749 | + if(threshold_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeArray) |
750 | + { |
751 | + if(threshold_xmlrpc.size() == 5) |
752 | + { |
753 | + for (int i = 0; i < threshold_xmlrpc.size(); ++i) |
754 | + { |
755 | + if(threshold_xmlrpc[i].getType() == XmlRpc::XmlRpcValue::TypeDouble) |
756 | + tmp[i] = static_cast<double>(threshold_xmlrpc[i]); |
757 | + else |
758 | + ROS_ERROR("grasp_touch_thresholds value %d is not a double, not loading",i+1); |
759 | + } |
760 | + } |
761 | + else |
762 | + ROS_ERROR("grasp_touch_thresholds must be of size 5, using default values"); |
763 | + } |
764 | + else |
765 | + ROS_ERROR("grasp_touch_thresholds must be an array, using default values"); |
766 | + } |
767 | + else |
768 | + ROS_WARN("grasp_touch_thresholds not set, using default values"); |
769 | + |
770 | + is_hand_occupied_thresholds = std::vector<double>(tmp, tmp+5); |
771 | + ROS_DEBUG("is_hand_occupied_thresholds:[%f %f %f %f %f]", is_hand_occupied_thresholds.at(0),is_hand_occupied_thresholds.at(1),is_hand_occupied_thresholds.at(2),is_hand_occupied_thresholds.at(3),is_hand_occupied_thresholds.at(4)); |
772 | + |
773 | |
774 | is_hand_occupied_server = n_tilde.advertiseService("is_hand_occupied", &SrTactileSensorManager::is_hand_occupied_cb, this); |
775 | which_fingers_are_touching_server = n_tilde.advertiseService("which_fingers_are_touching", &SrTactileSensorManager::which_fingers_are_touching_cb, this); |
776 | @@ -83,6 +111,7 @@ |
777 | if(tactile_sensors[i]->get_touch_data() < is_hand_occupied_thresholds[i]) |
778 | { |
779 | is_occupied = false; |
780 | + ROS_DEBUG("is_hand_occupied_thresholds %d with val %f is smaller than threshold %f",i,tactile_sensors[i]->get_touch_data(),is_hand_occupied_thresholds[i]); |
781 | break; |
782 | } |
783 | } |