baxter_interface.limb

51 """ 52 Interface class for a limb on the Baxter robot. 53 """ 54 55 # Containers 56 Point = collections.namedtuple('Point', ['x', 'y', 'z']) 57 Quaternion = collections.namedtuple('Quaternion', ['x', 'y', 'z', 'w']) 58

59 - def __init__(self, limb):

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

120 - def _on_joint_states(self, msg):

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

127 - def _on_endpoint_states(self, msg):

128 # Comments in this private method are for documentation purposes. 129 # _pose = {'position': (x, y, z), 'orientation': (x, y, z, w)} 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 # _twist = {'linear': (x, y, z), 'angular': (x, y, z)} 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 # _wrench = {'force': (x, y, z), 'torque': (x, y, z)} 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

170 - def joint_names(self):

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

180 - def joint_angle(self, joint):

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

191 - def joint_angles(self):

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

200 - def joint_velocity(self, joint):

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

211 - def joint_velocities(self):

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

220 - def joint_effort(self, joint):

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

231 - def joint_efforts(self):

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

240 - def endpoint_pose(self):

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

256 - def endpoint_velocity(self):

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

272 - def endpoint_effort(self):

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

288 - def set_command_timeout(self, timeout):

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

297 - def exit_control_mode(self, timeout=0.2):

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

309 - def set_joint_position_speed(self, speed):

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

325 - def set_joint_positions(self, positions, raw=False):

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

349 - def set_joint_velocities(self, velocities):

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

366 - def set_joint_torques(self, torques):

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

383 - def move_to_neutral(self, timeout=15.0):

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

398 - def move_to_joint_positions(self, positions, timeout=15.0, 399 threshold=settings.JOINT_ANGLE_TOLERANCE):

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 # First Order Filter - 0.2 Hz Cutoff 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])

Source Code for Module baxter_interface.limb