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 import collections
29
30 from copy import deepcopy
31
32 import rospy
33
34 from sensor_msgs.msg import (
35 JointState
36 )
37 from std_msgs.msg import (
38 Float64,
39 )
40
41 import baxter_dataflow
42
43 from baxter_core_msgs.msg import (
44 JointCommand,
45 EndpointState,
46 )
47 from baxter_interface import settings
48
49
51 """
52 Interface class for a limb on the Baxter robot.
53 """
54
55
56 Point = collections.namedtuple('Point', ['x', 'y', 'z'])
57 Quaternion = collections.namedtuple('Quaternion', ['x', 'y', 'z', 'w'])
58
60 """
61 Constructor.
62
63 @type limb: str
64 @param limb: limb to interface
65 """
66 self.name = limb
67 self._joint_angle = dict()
68 self._joint_velocity = dict()
69 self._joint_effort = dict()
70 self._cartesian_pose = dict()
71 self._cartesian_velocity = dict()
72 self._cartesian_effort = dict()
73
74 self._joint_names = {
75 'left': ['left_s0', 'left_s1', 'left_e0', 'left_e1',
76 'left_w0', 'left_w1', 'left_w2'],
77 'right': ['right_s0', 'right_s1', 'right_e0', 'right_e1',
78 'right_w0', 'right_w1', 'right_w2']
79 }
80
81 ns = '/robot/limb/' + limb + '/'
82
83 self._command_msg = JointCommand()
84
85 self._pub_speed_ratio = rospy.Publisher(
86 ns + 'set_speed_ratio',
87 Float64,
88 latch=True)
89
90 self._pub_joint_cmd = rospy.Publisher(
91 ns + 'joint_command',
92 JointCommand,
93 tcp_nodelay=True)
94
95 self._pub_joint_cmd_timeout = rospy.Publisher(
96 ns + 'joint_command_timeout',
97 Float64,
98 latch=True)
99
100 _cartesian_state_sub = rospy.Subscriber(
101 ns + 'endpoint_state',
102 EndpointState,
103 self._on_endpoint_states,
104 queue_size=1,
105 tcp_nodelay=True)
106
107 joint_state_topic = 'robot/joint_states'
108 _joint_state_sub = rospy.Subscriber(
109 joint_state_topic,
110 JointState,
111 self._on_joint_states,
112 queue_size=1,
113 tcp_nodelay=True)
114
115 err_msg = ("%s limb init failed to get current joint_states "
116 "from %s") % (self.name.capitalize(), joint_state_topic)
117 baxter_dataflow.wait_for(lambda: len(self._joint_angle.keys()) > 0,
118 timeout_msg=err_msg)
119
121 for idx, name in enumerate(msg.name):
122 if self.name in name:
123 self._joint_angle[name] = msg.position[idx]
124 self._joint_velocity[name] = msg.velocity[idx]
125 self._joint_effort[name] = msg.effort[idx]
126
128
129
130 self._cartesian_pose = {
131 'position': self.Point(
132 msg.pose.position.x,
133 msg.pose.position.y,
134 msg.pose.position.z,
135 ),
136 'orientation': self.Quaternion(
137 msg.pose.orientation.x,
138 msg.pose.orientation.y,
139 msg.pose.orientation.z,
140 msg.pose.orientation.w,
141 ),
142 }
143
144 self._cartesian_velocity = {
145 'linear': self.Point(
146 msg.twist.linear.x,
147 msg.twist.linear.y,
148 msg.twist.linear.z,
149 ),
150 'angular': self.Point(
151 msg.twist.angular.x,
152 msg.twist.angular.y,
153 msg.twist.angular.z,
154 ),
155 }
156
157 self._cartesian_effort = {
158 'force': self.Point(
159 msg.wrench.force.x,
160 msg.wrench.force.y,
161 msg.wrench.force.z,
162 ),
163 'torque': self.Point(
164 msg.wrench.torque.x,
165 msg.wrench.torque.y,
166 msg.wrench.torque.z,
167 ),
168 }
169
171 """
172 Return the names of the joints for the specified limb.
173
174 @rtype: [str]
175 @return: ordered list of joint names from proximal to distal
176 (i.e. shoulder to wrist).
177 """
178 return self._joint_names[self.name]
179
181 """
182 Return the requested joint angle.
183
184 @type joint: str
185 @param joint: name of a joint
186 @rtype: float
187 @return: angle in radians of individual joint
188 """
189 return self._joint_angle[joint]
190
192 """
193 Return all joint angles.
194
195 @rtype: dict({str:float})
196 @return: unordered dict of joint name Keys to angle (rad) Values
197 """
198 return deepcopy(self._joint_angle)
199
201 """
202 Return the requested joint velocity.
203
204 @type joint: str
205 @param joint: name of a joint
206 @rtype: float
207 @return: velocity in radians/s of individual joint
208 """
209 return self._joint_velocity[joint]
210
212 """
213 Return all joint velocities.
214
215 @rtype: dict({str:float})
216 @return: unordered dict of joint name Keys to velocity (rad/s) Values
217 """
218 return deepcopy(self._joint_velocity)
219
221 """
222 Return the requested joint effort.
223
224 @type joint: str
225 @param joint: name of a joint
226 @rtype: float
227 @return: effort in Nm of individual joint
228 """
229 return self._joint_effort[joint]
230
232 """
233 Return all joint efforts.
234
235 @rtype: dict({str:float})
236 @return: unordered dict of joint name Keys to effort (Nm) Values
237 """
238 return deepcopy(self._joint_effort)
239
241 """
242 Return Cartesian endpoint pose {position, orientation}.
243
244 @rtype: dict({str:L{Limb.Point},str:L{Limb.Quaternion}})
245 @return: position and orientation as named tuples in a dict
246
247 C{pose = {'position': (x, y, z), 'orientation': (x, y, z, w)}}
248
249 - 'position': Cartesian coordinates x,y,z in
250 namedtuple L{Limb.Point}
251 - 'orientation': quaternion x,y,z,w in named tuple
252 L{Limb.Quaternion}
253 """
254 return deepcopy(self._cartesian_pose)
255
257 """
258 Return Cartesian endpoint twist {linear, angular}.
259
260 @rtype: dict({str:L{Limb.Point},str:L{Limb.Point}})
261 @return: linear and angular velocities as named tuples in a dict
262
263 C{twist = {'linear': (x, y, z), 'angular': (x, y, z)}}
264
265 - 'linear': Cartesian velocity in x,y,z directions in
266 namedtuple L{Limb.Point}
267 - 'angular': Angular velocity around x,y,z axes in named tuple
268 L{Limb.Point}
269 """
270 return deepcopy(self._cartesian_velocity)
271
273 """
274 Return Cartesian endpoint wrench {force, torque}.
275
276 @rtype: dict({str:L{Limb.Point},str:L{Limb.Point}})
277 @return: force and torque at endpoint as named tuples in a dict
278
279 C{wrench = {'force': (x, y, z), 'torque': (x, y, z)}}
280
281 - 'force': Cartesian force on x,y,z axes in
282 namedtuple L{Limb.Point}
283 - 'torque': Torque around x,y,z axes in named tuple
284 L{Limb.Point}
285 """
286 return deepcopy(self._cartesian_effort)
287
289 """
290 Set the timeout in seconds for the joint controller
291
292 @type timeout: float
293 @param timeout: timeout in seconds
294 """
295 self._pub_joint_cmd_timeout.publish(Float64(timeout))
296
298 """
299 Clean exit from advanced control modes (joint torque or velocity).
300
301 Resets control to joint position mode with current positions.
302
303 @type timeout: float
304 @param timeout: control timeout in seconds [0.2]
305 """
306 self.set_command_timeout(timeout)
307 self.set_joint_positions(self.joint_angles())
308
310 """
311 Set ratio of max joint speed to use during joint position moves.
312
313 Set the proportion of maximum controllable velocity to use
314 during joint position control execution. The default ratio
315 is `0.3`, and can be set anywhere from [0.0-1.0] (clipped).
316 Once set, a speed ratio will persist until a new execution
317 speed is set.
318
319 @type speed: float
320 @param speed: ratio of maximum joint speed for execution
321 default= 0.3; range= [0.0-1.0]
322 """
323 self._pub_speed_ratio.publish(Float64(speed))
324
326 """
327 Commands the joints of this limb to the specified positions.
328
329 B{IMPORTANT:} 'raw' joint position control mode allows for commanding
330 joint positions, without modification, directly to the JCBs
331 (Joint Controller Boards). While this results in more unaffected
332 motions, 'raw' joint position control mode bypasses the safety system
333 modifications (e.g. collision avoidance).
334 Please use with caution.
335
336 @type positions: dict({str:float})
337 @param positions: joint_name:angle command
338 @type raw: bool
339 @param raw: advanced, direct position control mode
340 """
341 self._command_msg.names = positions.keys()
342 self._command_msg.command = positions.values()
343 if raw:
344 self._command_msg.mode = JointCommand.RAW_POSITION_MODE
345 else:
346 self._command_msg.mode = JointCommand.POSITION_MODE
347 self._pub_joint_cmd.publish(self._command_msg)
348
350 """
351 Commands the joints of this limb to the specified velocities.
352
353 B{IMPORTANT}: set_joint_velocities must be commanded at a rate great
354 than the timeout specified by set_command_timeout. If the timeout is
355 exceeded before a new set_joint_velocities command is received, the
356 controller will switch modes back to position mode for safety purposes.
357
358 @type velocities: dict({str:float})
359 @param velocities: joint_name:velocity command
360 """
361 self._command_msg.names = velocities.keys()
362 self._command_msg.command = velocities.values()
363 self._command_msg.mode = JointCommand.VELOCITY_MODE
364 self._pub_joint_cmd.publish(self._command_msg)
365
367 """
368 Commands the joints of this limb to the specified torques.
369
370 B{IMPORTANT}: set_joint_torques must be commanded at a rate great than
371 the timeout specified by set_command_timeout. If the timeout is
372 exceeded before a new set_joint_torques command is received, the
373 controller will switch modes back to position mode for safety purposes.
374
375 @type torques: dict({str:float})
376 @param torques: joint_name:torque command
377 """
378 self._command_msg.names = torques.keys()
379 self._command_msg.command = torques.values()
380 self._command_msg.mode = JointCommand.TORQUE_MODE
381 self._pub_joint_cmd.publish(self._command_msg)
382
384 """
385 Command the joints to the center of their joint ranges
386
387 Neutral is defined as::
388 ['*_s0', '*_s1', '*_e0', '*_e1', '*_w0', '*_w1', '*_w2']
389 [0.0, -0.55, 0.0, 0.75, 0.0, 1.26, 0.0]
390
391 @type timeout: float
392 @param timeout: seconds to wait for move to finish [15]
393 """
394 angles = dict(zip(self.joint_names(),
395 [0.0, -0.55, 0.0, 0.75, 0.0, 1.26, 0.0]))
396 return self.move_to_joint_positions(angles, timeout)
397
400 """
401 (Blocking) Commands the limb to the provided positions.
402
403 Waits until the reported joint state matches that specified.
404
405 This function uses a low-pass filter to smooth the movement.
406
407 @type positions: dict({str:float})
408 @param positions: joint_name:angle command
409 @type timeout: float
410 @param timeout: seconds to wait for move to finish [15]
411 @type threshold: float
412 @param threshold: position threshold in radians across each joint when
413 move is considered successful [0.008726646]
414 """
415 cmd = self.joint_angles()
416
417 def filtered_cmd():
418
419 for joint in positions.keys():
420 cmd[joint] = 0.012488 * positions[joint] + 0.98751 * cmd[joint]
421 return cmd
422
423 def genf(joint, angle):
424 def joint_diff():
425 return abs(angle - self._joint_angle[joint])
426 return joint_diff
427
428 diffs = [genf(j, a) for j, a in positions.items() if
429 j in self._joint_angle]
430
431 self.set_joint_positions(filtered_cmd())
432 baxter_dataflow.wait_for(
433 lambda: (all(diff() < threshold for diff in diffs)),
434 timeout=timeout,
435 timeout_msg=("%s limb failed to reach commanded joint positions" %
436 (self.name.capitalize(),)),
437 rate=100,
438 raise_on_error=False,
439 body=lambda: self.set_joint_positions(filtered_cmd())
440 )
441