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 queue_size=10) 90 91 self._pub_joint_cmd = rospy.Publisher( 92 ns + 'joint_command', 93 JointCommand, 94 tcp_nodelay=True, 95 queue_size=1) 96 97 self._pub_joint_cmd_timeout = rospy.Publisher( 98 ns + 'joint_command_timeout', 99 Float64, 100 latch=True, 101 queue_size=10) 102 103 _cartesian_state_sub = rospy.Subscriber( 104 ns + 'endpoint_state', 105 EndpointState, 106 self._on_endpoint_states, 107 queue_size=1, 108 tcp_nodelay=True) 109 110 joint_state_topic = 'robot/joint_states' 111 _joint_state_sub = rospy.Subscriber( 112 joint_state_topic, 113 JointState, 114 self._on_joint_states, 115 queue_size=1, 116 tcp_nodelay=True) 117 118 err_msg = ("%s limb init failed to get current joint_states " 119 "from %s") % (self.name.capitalize(), joint_state_topic) 120 baxter_dataflow.wait_for(lambda: len(self._joint_angle.keys()) > 0, 121 timeout_msg=err_msg)

122

123 - def _on_joint_states(self, msg):

124 for idx, name in enumerate(msg.name): 125 if self.name in name: 126 self._joint_angle[name] = msg.position[idx] 127 self._joint_velocity[name] = msg.velocity[idx] 128 self._joint_effort[name] = msg.effort[idx]

129

130 - def _on_endpoint_states(self, msg):

131 # Comments in this private method are for documentation purposes. 132 # _pose = {'position': (x, y, z), 'orientation': (x, y, z, w)} 133 self._cartesian_pose = { 134 'position': self.Point( 135 msg.pose.position.x, 136 msg.pose.position.y, 137 msg.pose.position.z, 138 ), 139 'orientation': self.Quaternion( 140 msg.pose.orientation.x, 141 msg.pose.orientation.y, 142 msg.pose.orientation.z, 143 msg.pose.orientation.w, 144 ), 145 } 146 # _twist = {'linear': (x, y, z), 'angular': (x, y, z)} 147 self._cartesian_velocity = { 148 'linear': self.Point( 149 msg.twist.linear.x, 150 msg.twist.linear.y, 151 msg.twist.linear.z, 152 ), 153 'angular': self.Point( 154 msg.twist.angular.x, 155 msg.twist.angular.y, 156 msg.twist.angular.z, 157 ), 158 } 159 # _wrench = {'force': (x, y, z), 'torque': (x, y, z)} 160 self._cartesian_effort = { 161 'force': self.Point( 162 msg.wrench.force.x, 163 msg.wrench.force.y, 164 msg.wrench.force.z, 165 ), 166 'torque': self.Point( 167 msg.wrench.torque.x, 168 msg.wrench.torque.y, 169 msg.wrench.torque.z, 170 ), 171 }

172

173 - def joint_names(self):

174 """ 175 Return the names of the joints for the specified limb. 176 177 @rtype: [str] 178 @return: ordered list of joint names from proximal to distal 179 (i.e. shoulder to wrist). 180 """ 181 return self._joint_names[self.name]

182

183 - def joint_angle(self, joint):

184 """ 185 Return the requested joint angle. 186 187 @type joint: str 188 @param joint: name of a joint 189 @rtype: float 190 @return: angle in radians of individual joint 191 """ 192 return self._joint_angle[joint]

193

194 - def joint_angles(self):

195 """ 196 Return all joint angles. 197 198 @rtype: dict({str:float}) 199 @return: unordered dict of joint name Keys to angle (rad) Values 200 """ 201 return deepcopy(self._joint_angle)

202

203 - def joint_velocity(self, joint):

204 """ 205 Return the requested joint velocity. 206 207 @type joint: str 208 @param joint: name of a joint 209 @rtype: float 210 @return: velocity in radians/s of individual joint 211 """ 212 return self._joint_velocity[joint]

213

214 - def joint_velocities(self):

215 """ 216 Return all joint velocities. 217 218 @rtype: dict({str:float}) 219 @return: unordered dict of joint name Keys to velocity (rad/s) Values 220 """ 221 return deepcopy(self._joint_velocity)

222

223 - def joint_effort(self, joint):

224 """ 225 Return the requested joint effort. 226 227 @type joint: str 228 @param joint: name of a joint 229 @rtype: float 230 @return: effort in Nm of individual joint 231 """ 232 return self._joint_effort[joint]

233

234 - def joint_efforts(self):

235 """ 236 Return all joint efforts. 237 238 @rtype: dict({str:float}) 239 @return: unordered dict of joint name Keys to effort (Nm) Values 240 """ 241 return deepcopy(self._joint_effort)

242

243 - def endpoint_pose(self):

244 """ 245 Return Cartesian endpoint pose {position, orientation}. 246 247 @rtype: dict({str:L{Limb.Point},str:L{Limb.Quaternion}}) 248 @return: position and orientation as named tuples in a dict 249 250 C{pose = {'position': (x, y, z), 'orientation': (x, y, z, w)}} 251 252 - 'position': Cartesian coordinates x,y,z in 253 namedtuple L{Limb.Point} 254 - 'orientation': quaternion x,y,z,w in named tuple 255 L{Limb.Quaternion} 256 """ 257 return deepcopy(self._cartesian_pose)

258

259 - def endpoint_velocity(self):

260 """ 261 Return Cartesian endpoint twist {linear, angular}. 262 263 @rtype: dict({str:L{Limb.Point},str:L{Limb.Point}}) 264 @return: linear and angular velocities as named tuples in a dict 265 266 C{twist = {'linear': (x, y, z), 'angular': (x, y, z)}} 267 268 - 'linear': Cartesian velocity in x,y,z directions in 269 namedtuple L{Limb.Point} 270 - 'angular': Angular velocity around x,y,z axes in named tuple 271 L{Limb.Point} 272 """ 273 return deepcopy(self._cartesian_velocity)

274

275 - def endpoint_effort(self):

276 """ 277 Return Cartesian endpoint wrench {force, torque}. 278 279 @rtype: dict({str:L{Limb.Point},str:L{Limb.Point}}) 280 @return: force and torque at endpoint as named tuples in a dict 281 282 C{wrench = {'force': (x, y, z), 'torque': (x, y, z)}} 283 284 - 'force': Cartesian force on x,y,z axes in 285 namedtuple L{Limb.Point} 286 - 'torque': Torque around x,y,z axes in named tuple 287 L{Limb.Point} 288 """ 289 return deepcopy(self._cartesian_effort)

290

291 - def set_command_timeout(self, timeout):

292 """ 293 Set the timeout in seconds for the joint controller 294 295 @type timeout: float 296 @param timeout: timeout in seconds 297 """ 298 self._pub_joint_cmd_timeout.publish(Float64(timeout))

299

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

301 """ 302 Clean exit from advanced control modes (joint torque or velocity). 303 304 Resets control to joint position mode with current positions. 305 306 @type timeout: float 307 @param timeout: control timeout in seconds [0.2] 308 """ 309 self.set_command_timeout(timeout) 310 self.set_joint_positions(self.joint_angles())

311

312 - def set_joint_position_speed(self, speed):

313 """ 314 Set ratio of max joint speed to use during joint position moves. 315 316 Set the proportion of maximum controllable velocity to use 317 during joint position control execution. The default ratio 318 is `0.3`, and can be set anywhere from [0.0-1.0] (clipped). 319 Once set, a speed ratio will persist until a new execution 320 speed is set. 321 322 @type speed: float 323 @param speed: ratio of maximum joint speed for execution 324 default= 0.3; range= [0.0-1.0] 325 """ 326 self._pub_speed_ratio.publish(Float64(speed))

327

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

329 """ 330 Commands the joints of this limb to the specified positions. 331 332 B{IMPORTANT:} 'raw' joint position control mode allows for commanding 333 joint positions, without modification, directly to the JCBs 334 (Joint Controller Boards). While this results in more unaffected 335 motions, 'raw' joint position control mode bypasses the safety system 336 modifications (e.g. collision avoidance). 337 Please use with caution. 338 339 @type positions: dict({str:float}) 340 @param positions: joint_name:angle command 341 @type raw: bool 342 @param raw: advanced, direct position control mode 343 """ 344 self._command_msg.names = positions.keys() 345 self._command_msg.command = positions.values() 346 if raw: 347 self._command_msg.mode = JointCommand.RAW_POSITION_MODE 348 else: 349 self._command_msg.mode = JointCommand.POSITION_MODE 350 self._pub_joint_cmd.publish(self._command_msg)

351

352 - def set_joint_velocities(self, velocities):

353 """ 354 Commands the joints of this limb to the specified velocities. 355 356 B{IMPORTANT}: set_joint_velocities must be commanded at a rate great 357 than the timeout specified by set_command_timeout. If the timeout is 358 exceeded before a new set_joint_velocities command is received, the 359 controller will switch modes back to position mode for safety purposes. 360 361 @type velocities: dict({str:float}) 362 @param velocities: joint_name:velocity command 363 """ 364 self._command_msg.names = velocities.keys() 365 self._command_msg.command = velocities.values() 366 self._command_msg.mode = JointCommand.VELOCITY_MODE 367 self._pub_joint_cmd.publish(self._command_msg)

368

369 - def set_joint_torques(self, torques):

370 """ 371 Commands the joints of this limb to the specified torques. 372 373 B{IMPORTANT}: set_joint_torques must be commanded at a rate great than 374 the timeout specified by set_command_timeout. If the timeout is 375 exceeded before a new set_joint_torques command is received, the 376 controller will switch modes back to position mode for safety purposes. 377 378 @type torques: dict({str:float}) 379 @param torques: joint_name:torque command 380 """ 381 self._command_msg.names = torques.keys() 382 self._command_msg.command = torques.values() 383 self._command_msg.mode = JointCommand.TORQUE_MODE 384 self._pub_joint_cmd.publish(self._command_msg)

385

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

387 """ 388 Command the joints to the center of their joint ranges 389 390 Neutral is defined as:: 391 ['*_s0', '*_s1', '*_e0', '*_e1', '*_w0', '*_w1', '*_w2'] 392 [0.0, -0.55, 0.0, 0.75, 0.0, 1.26, 0.0] 393 394 @type timeout: float 395 @param timeout: seconds to wait for move to finish [15] 396 """ 397 angles = dict(zip(self.joint_names(), 398 [0.0, -0.55, 0.0, 0.75, 0.0, 1.26, 0.0])) 399 return self.move_to_joint_positions(angles, timeout)

400

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

403 """ 404 (Blocking) Commands the limb to the provided positions. 405 406 Waits until the reported joint state matches that specified. 407 408 This function uses a low-pass filter to smooth the movement. 409 410 @type positions: dict({str:float}) 411 @param positions: joint_name:angle command 412 @type timeout: float 413 @param timeout: seconds to wait for move to finish [15] 414 @type threshold: float 415 @param threshold: position threshold in radians across each joint when 416 move is considered successful [0.008726646] 417 """ 418 cmd = self.joint_angles() 419 420 def filtered_cmd(): 421 # First Order Filter - 0.2 Hz Cutoff 422 for joint in positions.keys(): 423 cmd[joint] = 0.012488 * positions[joint] + 0.98751 * cmd[joint] 424 return cmd

425 426 def genf(joint, angle): 427 def joint_diff(): 428 return abs(angle - self._joint_angle[joint])

Source Code for Module baxter_interface.limb