00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015 import numpy as np, math
00016 from threading import RLock, Timer
00017 import sys
00018
00019 import roslib; roslib.load_manifest('hrl_pr2_lib')
00020 roslib.load_manifest('force_torque')
00021 import force_torque.FTClient as ftc
00022 import tf
00023
00024 import rospy
00025
00026 import actionlib
00027 from actionlib_msgs.msg import GoalStatus
00028
00029 from kinematics_msgs.srv import GetPositionFK, GetPositionFKRequest, GetPositionFKResponse
00030 from kinematics_msgs.srv import GetPositionIK, GetPositionIKRequest, GetPositionIKResponse
00031 from pr2_controllers_msgs.msg import JointTrajectoryAction, JointTrajectoryGoal, JointTrajectoryControllerState
00032 from pr2_controllers_msgs.msg import Pr2GripperCommandGoal, Pr2GripperCommandAction, Pr2GripperCommand
00033 from trajectory_msgs.msg import JointTrajectoryPoint
00034 from geometry_msgs.msg import PoseStamped
00035
00036 from teleop_controllers.msg import JTTeleopControllerState
00037
00038 from std_msgs.msg import Float64
00039 from sensor_msgs.msg import JointState
00040
00041 import hrl_lib.transforms as tr
00042 import time
00043 import functools as ft
00044
00045 import tf.transformations as tftrans
00046 import operator as op
00047 import types
00048
00049 from visualization_msgs.msg import Marker
00050
00051
00052 node_name = "pr2_arms"
00053
00054 def log(str):
00055 rospy.loginfo(node_name + ": " + str)
00056
00057
00058
00059
00060
00061
00062 def make_column(x):
00063 if (type(x) == type([])
00064 or (type(x) == np.ndarray and x.ndim == 1)
00065 or type(x) == type(())):
00066 return np.matrix(x).T
00067 if type(x) == np.ndarray:
00068 x = np.matrix(x)
00069 if x.shape[0] == 1:
00070 return x.T
00071 return x
00072
00073
00074
00075
00076
00077
00078 def make_list(col):
00079 if type(col) == type([]) or type(col) == type(()):
00080 return col
00081 return [col[i,0] for i in range(col.shape[0])]
00082
00083
00084
00085
00086
00087
00088 class PR2Arms(object):
00089
00090
00091
00092
00093
00094
00095
00096
00097 def __init__(self, send_delay=50000000, gripper_point=(0.23,0.0,0.0),
00098 force_torque = False):
00099 log("Loading PR2Arms")
00100
00101 self.send_delay = send_delay
00102 self.off_point = gripper_point
00103 self.joint_names_list = [['r_shoulder_pan_joint',
00104 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint',
00105 'r_elbow_flex_joint', 'r_forearm_roll_joint',
00106 'r_wrist_flex_joint', 'r_wrist_roll_joint'],
00107 ['l_shoulder_pan_joint',
00108 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint',
00109 'l_elbow_flex_joint', 'l_forearm_roll_joint',
00110 'l_wrist_flex_joint', 'l_wrist_roll_joint']]
00111
00112 rospy.wait_for_service('pr2_right_arm_kinematics/get_fk');
00113 self.fk_srv = [rospy.ServiceProxy('pr2_right_arm_kinematics/get_fk', GetPositionFK),
00114 rospy.ServiceProxy('pr2_left_arm_kinematics/get_fk', GetPositionFK)]
00115
00116 rospy.wait_for_service('pr2_right_arm_kinematics/get_ik');
00117 self.ik_srv = [rospy.ServiceProxy('pr2_right_arm_kinematics/get_ik', GetPositionIK),
00118 rospy.ServiceProxy('pr2_left_arm_kinematics/get_ik', GetPositionIK)]
00119
00120 self.joint_action_client = [actionlib.SimpleActionClient('r_arm_controller/joint_trajectory_action', JointTrajectoryAction),
00121 actionlib.SimpleActionClient('l_arm_controller/joint_trajectory_action', JointTrajectoryAction)]
00122
00123 self.gripper_action_client = [actionlib.SimpleActionClient('r_gripper_controller/gripper_action', Pr2GripperCommandAction),actionlib.SimpleActionClient('l_gripper_controller/gripper_action', Pr2GripperCommandAction)]
00124
00125 self.joint_action_client[0].wait_for_server()
00126 self.joint_action_client[1].wait_for_server()
00127 self.gripper_action_client[0].wait_for_server()
00128 self.gripper_action_client[1].wait_for_server()
00129
00130 self.arm_state_lock = [RLock(), RLock()]
00131 self.r_arm_cart_pub = rospy.Publisher('/r_cart/command_pose', PoseStamped)
00132 self.l_arm_cart_pub = rospy.Publisher('/l_cart/command_pose', PoseStamped)
00133
00134 rospy.Subscriber('/r_cart/state', JTTeleopControllerState, self.r_cart_state_cb)
00135 rospy.Subscriber('/l_cart/state', JTTeleopControllerState, self.l_cart_state_cb)
00136
00137 if force_torque:
00138 self.r_arm_ftc = ftc.FTClient('force_torque_ft2')
00139 self.tf_lstnr = tf.TransformListener()
00140
00141 self.arm_angles = [None, None]
00142 self.arm_efforts = [None, None]
00143 self.jtg = [None, None]
00144 self.cur_traj = [None, None]
00145 self.cur_traj_timer = [None, None]
00146 self.cur_traj_pos = [None, None]
00147
00148 self.marker_pub = rospy.Publisher('/pr2_arms/viz_marker', Marker)
00149 rospy.Subscriber('/joint_states', JointState,
00150 self.joint_states_cb, queue_size=2)
00151
00152 rospy.sleep(1.)
00153
00154 log("Finished loading SimpleArmManger")
00155
00156
00157
00158
00159
00160
00161 def joint_states_cb(self, data):
00162 arm_angles = [[], []]
00163 arm_efforts = [[], []]
00164 r_jt_idx_list = [0]*7
00165 l_jt_idx_list = [0]*7
00166 for i, jt_nm in enumerate(self.joint_names_list[0]):
00167 r_jt_idx_list[i] = data.name.index(jt_nm)
00168 for i, jt_nm in enumerate(self.joint_names_list[1]):
00169 l_jt_idx_list[i] = data.name.index(jt_nm)
00170
00171 for i in range(7):
00172 idx = r_jt_idx_list[i]
00173 if data.name[idx] != self.joint_names_list[0][i]:
00174 raise RuntimeError('joint angle name does not match. Expected: %s, Actual: %s i: %d'%('r_'+nm+'_joint', data.name[idx], i))
00175 ang = self.normalize_ang(data.position[idx])
00176 arm_angles[0] += [ang]
00177 arm_efforts[0] += [data.effort[idx]]
00178
00179 idx = l_jt_idx_list[i]
00180 if data.name[idx] != self.joint_names_list[1][i]:
00181 raise RuntimeError('joint angle name does not match. Expected: %s, Actual: %s i: %d'%('r_'+nm+'_joint', data.name[idx], i))
00182 ang = self.normalize_ang(data.position[idx])
00183 arm_angles[1] += [ang]
00184 arm_efforts[1] += [data.effort[idx]]
00185
00186 self.arm_state_lock[0].acquire()
00187 self.arm_angles[0] = arm_angles[0]
00188 self.arm_efforts[0] = arm_efforts[0]
00189 self.arm_state_lock[0].release()
00190
00191 self.arm_state_lock[1].acquire()
00192 self.arm_angles[1] = arm_angles[1]
00193 self.arm_efforts[1] = arm_efforts[1]
00194 self.arm_state_lock[1].release()
00195
00196 def r_cart_state_cb(self, msg):
00197 trans, quat = self.tf_lstnr.lookupTransform('/torso_lift_link',
00198 'r_gripper_tool_frame', rospy.Time(0))
00199 rot = tr.quaternion_to_matrix(quat)
00200 tip = np.matrix([0.12, 0., 0.]).T
00201 self.r_ee_pos = rot*tip + np.matrix(trans).T
00202 self.r_ee_rot = rot
00203
00204
00205 marker = Marker()
00206 marker.header.frame_id = 'torso_lift_link'
00207 time_stamp = rospy.Time.now()
00208 marker.header.stamp = time_stamp
00209 marker.ns = 'aloha land'
00210 marker.type = Marker.SPHERE
00211 marker.action = Marker.ADD
00212 marker.pose.position.x = self.r_ee_pos[0,0]
00213 marker.pose.position.y = self.r_ee_pos[1,0]
00214 marker.pose.position.z = self.r_ee_pos[2,0]
00215 size = 0.02
00216 marker.scale.x = size
00217 marker.scale.y = size
00218 marker.scale.z = size
00219 marker.lifetime = rospy.Duration()
00220
00221 marker.id = 71
00222 marker.pose.orientation.x = 0
00223 marker.pose.orientation.y = 0
00224 marker.pose.orientation.z = 0
00225 marker.pose.orientation.w = 1
00226
00227 color = (0.5, 0., 0.0)
00228 marker.color.r = color[0]
00229 marker.color.g = color[1]
00230 marker.color.b = color[2]
00231 marker.color.a = 1.
00232 self.marker_pub.publish(marker)
00233
00234
00235
00236
00237 ros_pt = msg.x_desi_filtered.pose.position
00238 x, y, z = ros_pt.x, ros_pt.y, ros_pt.z
00239 self.r_cep_pos = np.matrix([x, y, z]).T
00240 pt = rot.T * (np.matrix([x,y,z]).T - np.matrix(trans).T)
00241 pt = pt + tip
00242 self.r_cep_pos_hooktip = rot*pt + np.matrix(trans).T
00243 ros_quat = msg.x_desi_filtered.pose.orientation
00244 quat = (ros_quat.x, ros_quat.y, ros_quat.z, ros_quat.w)
00245 self.r_cep_rot = tr.quaternion_to_matrix(quat)
00246
00247 def l_cart_state_cb(self, msg):
00248 ros_pt = msg.x_desi_filtered.pose.position
00249 self.l_cep_pos = np.matrix([ros_pt.x, ros_pt.y, ros_pt.z]).T
00250 ros_quat = msg.x_desi_filtered.pose.orientation
00251 quat = (ros_quat.x, ros_quat.y, ros_quat.z, ros_quat.w)
00252 self.l_cep_rot = tr.quaternion_to_matrix(quat)
00253
00254 def normalize_ang(self, ang):
00255 while ang >= 2 * np.pi:
00256 ang -= 2 * np.pi
00257 while ang < 0.:
00258 ang += 2 * np.pi
00259 return ang
00260
00261
00262
00263
00264
00265
00266
00267
00268
00269
00270
00271 def create_JTG(self, arm, pos_arr, dur_arr, stamp=None, vel_arr=None, acc_arr=None):
00272
00273 vel_arr = [[0.]*7]*len(pos_arr)
00274 acc_arr = [[0.]*7]*len(pos_arr)
00275
00276
00277
00278 def get_vel_acc(q_arr, d_arr):
00279 vel_arr = [[0.]*7]
00280 acc_arr = [[0.]*7]
00281 for i in range(1, len(q_arr)):
00282 vel, acc = [], []
00283 for j in range(7):
00284 vel += [(q_arr[i][j] - q_arr[i-1][j]) / d_arr[i]]
00285 acc += [(vel[j] - vel_arr[i-1][j]) / d_arr[i]]
00286 vel_arr += [vel]
00287 acc_arr += [acc]
00288 print vel, acc
00289 return vel_arr, acc_arr
00290
00291 if arm != 1:
00292 arm = 0
00293 jtg = JointTrajectoryGoal()
00294 if stamp is None:
00295 stamp = rospy.Time.now()
00296 else:
00297 jtg.trajectory.header.stamp = stamp
00298
00299 if len(pos_arr) > 1 and (vel_arr is None or acc_arr is None):
00300 v_arr, a_arr = get_vel_acc(pos_arr, dur_arr)
00301 if vel_arr is None:
00302 vel_arr = v_arr
00303 if acc_arr is None:
00304 acc_arr = a_arr
00305
00306 jtg.trajectory.joint_names = self.joint_names_list[arm]
00307 for i in range(len(pos_arr)):
00308 if pos_arr[i] is None or type(pos_arr[i]) is types.NoneType:
00309 continue
00310 jtp = JointTrajectoryPoint()
00311 jtp.positions = pos_arr[i]
00312 if vel_arr is None:
00313 vel = [0.] * 7
00314 else:
00315 vel = vel_arr[i]
00316 jtp.velocities = vel
00317 if acc_arr is None:
00318 acc = [0.] * 7
00319 else:
00320 acc = acc_arr[i]
00321 jtp.accelerations = acc
00322 jtp.time_from_start = rospy.Duration(dur_arr[i])
00323 jtg.trajectory.points.append(jtp)
00324 return jtg
00325
00326
00327
00328
00329
00330
00331
00332 def execute_trajectory(self, arm, jtg):
00333 if self.cur_traj[arm] is not None or self.cur_traj_timer[arm] is not None:
00334 log("Arm is currently executing trajectory")
00335 if rospy.is_shutdown():
00336 sys.exit()
00337 return
00338 print "Yo 2"
00339
00340
00341
00342 self.cur_traj_pos[arm] = 0
00343
00344
00345 min_init_time = rospy.Time.now().to_sec() + 2 * self.send_delay
00346 if jtg.trajectory.header.stamp.to_nsec() < min_init_time:
00347 jtg.trajectory.header.stamp = rospy.Time(rospy.Time.now().to_sec(),
00348 2 * self.send_delay)
00349
00350 print "dfsdfsfd", jtg
00351 jtg.trajectory.header.stamp = rospy.Time.now()
00352 self.joint_action_client[arm].send_goal(jtg)
00353 return
00354
00355
00356 call_time = ((jtg.trajectory.header.stamp.to_nsec() - self.send_delay -
00357 rospy.Time.now().to_nsec()) / 1000000000.)
00358 self.cur_traj_timer[arm] = Timer(call_time, self._exec_traj, [arm])
00359 self.cur_traj_timer[arm].start()
00360
00361
00362
00363
00364 def _exec_traj(self, arm):
00365 jtg = self.cur_traj[arm]
00366 i = self.cur_traj_pos[arm]
00367 beg_time = jtg.trajectory.header.stamp.to_nsec()
00368
00369
00370 cur_exec_time = rospy.Time(jtg.trajectory.header.stamp.to_sec() +
00371 jtg.trajectory.points[i].time_from_start.to_sec())
00372
00373
00374 if i == 0:
00375 last_time_from = 0
00376 else:
00377 last_time_from = jtg.trajectory.points[i-1].time_from_start.to_sec()
00378 cur_dur = jtg.trajectory.points[i].time_from_start.to_sec() - last_time_from
00379 cur_jtg = self.create_JTG(arm, [jtg.trajectory.points[i].positions],
00380 [cur_dur],
00381 cur_exec_time,
00382 [jtg.trajectory.points[i].velocities],
00383 [jtg.trajectory.points[i].accelerations])
00384
00385 print "cur_jtg", cur_jtg
00386 self.joint_action_client[arm].send_goal(cur_jtg)
00387
00388 self.cur_traj_pos[arm] += 1
00389 if self.cur_traj_pos[arm] == len(jtg.trajectory.points):
00390
00391 self.cur_traj[arm] = None
00392 self.cur_traj_timer[arm] = None
00393 else:
00394
00395 next_exec_time = beg_time + jtg.trajectory.points[i+1].time_from_start.to_nsec()
00396 print "diff times", next_exec_time / 1000000000. - cur_exec_time.to_sec() - cur_dur
00397 call_time = ((next_exec_time - self.send_delay -
00398 rospy.Time.now().to_nsec()) / 1000000000.)
00399 self.cur_traj_timer[arm] = Timer(call_time, self._exec_traj, [arm])
00400 self.cur_traj_timer[arm].start()
00401
00402
00403
00404
00405
00406 def stop_trajectory(self, arm):
00407 self.cur_traj_timer[arm].cancel()
00408 self.cur_traj[arm] = None
00409 self.cur_traj_timer[arm] = None
00410
00411
00412
00413
00414
00415
00416
00417
00418 def set_joint_angles(self, arm, q, duration=1., start_time=0.):
00419 self.set_joint_angles_traj(arm, [q], [duration], start_time)
00420
00421
00422
00423
00424
00425
00426
00427
00428
00429 def set_joint_angles_traj(self, arm, q_arr, dur_arr, start_time=0.):
00430 if arm != 1:
00431 arm = 0
00432 jtg = self.create_JTG(arm, q_arr, dur_arr)
00433 cur_time = rospy.Time.now().to_sec() + 2 * self.send_delay / 1000000000.
00434 jtg.trajectory.header.stamp = rospy.Duration(start_time + cur_time)
00435 self.execute_trajectory(arm, jtg)
00436
00437
00438
00439
00440
00441
00442 def is_arm_in_motion(self, arm):
00443
00444
00445
00446 state = self.joint_action_client[arm].get_state()
00447 return state == GoalStatus.PENDING or state == GoalStatus.ACTIVE
00448
00449
00450
00451
00452
00453
00454 def wait_for_arm_completion(self, arm, hz=0.01):
00455 while self.is_arm_in_motion(arm) and not rospy.is_shutdown():
00456 rospy.sleep(hz)
00457
00458
00459
00460
00461
00462
00463
00464
00465
00466 def transform_in_frame(self, pos, quat, off_point):
00467 pos = make_column(pos)
00468 quat = make_list(quat)
00469 invquatmat = np.mat(tftrans.quaternion_matrix(quat))
00470 invquatmat[0:3,3] = pos
00471 trans = np.matrix([off_point[0],off_point[1],off_point[2],1.]).T
00472 transpos = invquatmat * trans
00473 return np.resize(transpos, (3, 1))
00474
00475
00476
00477
00478
00479
00480
00481 def FK(self, arm, q):
00482 if rospy.is_shutdown():
00483 sys.exit()
00484 if arm != 1:
00485 arm = 0
00486 fk_req = GetPositionFKRequest()
00487 fk_req.header.frame_id = 'torso_lift_link'
00488 if arm == 0:
00489 fk_req.fk_link_names.append('r_wrist_roll_link')
00490 else:
00491 fk_req.fk_link_names.append('l_wrist_roll_link')
00492 fk_req.robot_state.joint_state.name = self.joint_names_list[arm]
00493 fk_req.robot_state.joint_state.position = q
00494
00495 fk_resp = GetPositionFKResponse()
00496 fk_resp = self.fk_srv[arm].call(fk_req)
00497 if fk_resp.error_code.val == fk_resp.error_code.SUCCESS:
00498 x = fk_resp.pose_stamped[0].pose.position.x
00499 y = fk_resp.pose_stamped[0].pose.position.y
00500 z = fk_resp.pose_stamped[0].pose.position.z
00501 q1 = fk_resp.pose_stamped[0].pose.orientation.x
00502 q2 = fk_resp.pose_stamped[0].pose.orientation.y
00503 q3 = fk_resp.pose_stamped[0].pose.orientation.z
00504 q4 = fk_resp.pose_stamped[0].pose.orientation.w
00505 quat = [q1,q2,q3,q4]
00506
00507
00508 ret1 = self.transform_in_frame([x,y,z], quat, self.off_point)
00509 ret2 = np.matrix(quat).T
00510 else:
00511 rospy.logerr('Forward kinematics failed')
00512 ret1, ret2 = None, None
00513
00514 return ret1, ret2
00515
00516
00517
00518
00519
00520
00521
00522
00523
00524 def IK(self, arm, p, rot, q_guess):
00525 if arm != 1:
00526 arm = 0
00527
00528 p = make_column(p)
00529
00530 if rot.shape == (3, 3):
00531 quat = np.matrix(tr.matrix_to_quaternion(rot)).T
00532 elif rot.shape == (4, 1):
00533 quat = make_column(rot)
00534 else:
00535 rospy.logerr('Inverse kinematics failed (bad rotation)')
00536 return None
00537
00538
00539 neg_off = [-self.off_point[0],-self.off_point[1],-self.off_point[2]]
00540 transpos = self.transform_in_frame(p, quat, neg_off)
00541
00542 ik_req = GetPositionIKRequest()
00543 ik_req.timeout = rospy.Duration(5.)
00544 if arm == 0:
00545 ik_req.ik_request.ik_link_name = 'r_wrist_roll_link'
00546 else:
00547 ik_req.ik_request.ik_link_name = 'l_wrist_roll_link'
00548 ik_req.ik_request.pose_stamped.header.frame_id = 'torso_lift_link'
00549
00550 ik_req.ik_request.pose_stamped.pose.position.x = transpos[0]
00551 ik_req.ik_request.pose_stamped.pose.position.y = transpos[1]
00552 ik_req.ik_request.pose_stamped.pose.position.z = transpos[2]
00553
00554 ik_req.ik_request.pose_stamped.pose.orientation.x = quat[0]
00555 ik_req.ik_request.pose_stamped.pose.orientation.y = quat[1]
00556 ik_req.ik_request.pose_stamped.pose.orientation.z = quat[2]
00557 ik_req.ik_request.pose_stamped.pose.orientation.w = quat[3]
00558
00559 ik_req.ik_request.ik_seed_state.joint_state.position = q_guess
00560 ik_req.ik_request.ik_seed_state.joint_state.name = self.joint_names_list[arm]
00561
00562 ik_resp = self.ik_srv[arm].call(ik_req)
00563 if ik_resp.error_code.val == ik_resp.error_code.SUCCESS:
00564 ret = list(ik_resp.solution.joint_state.position)
00565 else:
00566 rospy.logerr('Inverse kinematics failed')
00567 ret = None
00568
00569 return ret
00570
00571
00572
00573
00574
00575
00576
00577 def can_move_arm(self, arm, pos, rot):
00578 begq = self.get_joint_angles(arm)
00579 endq = self.IK(arm, pos, rot, begq)
00580 if endq is None:
00581 return False
00582 else:
00583 return True
00584
00585
00586
00587
00588
00589
00590
00591
00592 def move_arm(self, arm, pos, begq=None , rot=None, dur=4.0):
00593 if begq is None:
00594 begq = self.get_joint_angles(arm)
00595 if rot is None:
00596 temp, rot = self.FK(arm, begq)
00597 endq = self.IK(arm, pos, rot, begq)
00598 if endq is None:
00599 return False
00600 self.set_joint_angles(arm, endq, dur)
00601 return True
00602
00603 def cancel_trajectory(self, arm):
00604 self.joint_action_client[arm].cancel_all_goals()
00605
00606
00607
00608
00609
00610
00611
00612
00613
00614
00615
00616
00617
00618 def move_arm_trajectory(self, arm, pos_arr, dur=1.,
00619 rot_arr=None, dur_arr=None, freeze_wrist=False):
00620 if dur_arr is None:
00621 dur_arr = np.linspace(0., dur, len(pos_arr) + 1)[1:]
00622
00623 initq = self.get_joint_angles(arm)
00624 curq = initq
00625 q_arr = []
00626 fails, trys = 0, 0
00627 if rot_arr is None:
00628 temp, rot = self.FK(arm, curq)
00629
00630 for i in range(len(pos_arr)):
00631 if rot_arr is None:
00632 cur_rot = rot
00633 else:
00634 cur_rot = rot_arr[i]
00635 nextq = self.IK(arm, pos_arr[i], cur_rot, curq)
00636 q_arr += [nextq]
00637 if nextq is None:
00638 fails += 1
00639 trys += 1
00640 if not nextq is None:
00641 curq = nextq
00642 log("IK Accuracy: %d/%d (%f)" % (trys-fails, trys, (trys-fails)/float(trys)))
00643
00644 if freeze_wrist:
00645 for i in range(len(q_arr)):
00646 if not q_arr[i] is None:
00647 q_arr[i] = (q_arr[i][0], q_arr[i][1], q_arr[i][2], q_arr[i][3],
00648 q_arr[i][4], q_arr[i][5], initq[6])
00649
00650 self.set_joint_angles_traj(arm, q_arr, dur_arr)
00651
00652
00653
00654
00655
00656
00657 def end_effector_pos(self, arm):
00658 q = self.get_joint_angles(arm)
00659 return self.FK(arm, q)
00660
00661
00662
00663
00664
00665
00666 def get_joint_angles(self, arm):
00667 if arm != 1:
00668 arm = 0
00669 self.arm_state_lock[arm].acquire()
00670 q = self.arm_angles[arm]
00671 self.arm_state_lock[arm].release()
00672 return q
00673
00674
00675
00676 def get_wrist_force(self, arm, bias = True, base_frame = False):
00677 if arm != 0:
00678 rospy.logerr('Unsupported arm: %d'%arm)
00679 raise RuntimeError('Unimplemented function')
00680
00681 f = self.r_arm_ftc.read(without_bias = not bias)
00682 f = f[0:3, :]
00683 if base_frame:
00684 trans, quat = self.tf_lstnr.lookupTransform('/torso_lift_link',
00685 '/ft2', rospy.Time(0))
00686 rot = tr.quaternion_to_matrix(quat)
00687 f = rot * f
00688 return -f
00689
00690 def bias_wrist_ft(self, arm):
00691 if arm != 0:
00692 rospy.logerr('Unsupported arm: %d'%arm)
00693 raise RuntimeError('Unimplemented function')
00694 self.r_arm_ftc.bias()
00695
00696
00697 def get_ee_jtt(self, arm):
00698 if arm == 0:
00699 return self.r_ee_pos, self.r_ee_rot
00700 else:
00701 return self.l_ee_pos, self.l_ee_rot
00702
00703 def get_cep_jtt(self, arm, hook_tip = False):
00704 if arm == 0:
00705 if hook_tip:
00706 return self.r_cep_pos_hooktip, self.r_cep_rot
00707 else:
00708 return self.r_cep_pos, self.r_cep_rot
00709 else:
00710 return self.l_cep_pos, self.l_cep_rot
00711
00712
00713 def set_cep_jtt(self, arm, p, rot=None):
00714 if arm != 1:
00715 arm = 0
00716 ps = PoseStamped()
00717 ps.header.stamp = rospy.rostime.get_rostime()
00718 ps.header.frame_id = 'torso_lift_link'
00719
00720 ps.pose.position.x = p[0,0]
00721 ps.pose.position.y = p[1,0]
00722 ps.pose.position.z = p[2,0]
00723
00724 if rot == None:
00725 if arm == 0:
00726 rot = self.r_cep_rot
00727 else:
00728 rot = self.l_cep_rot
00729
00730 quat = tr.matrix_to_quaternion(rot)
00731 ps.pose.orientation.x = quat[0]
00732 ps.pose.orientation.y = quat[1]
00733 ps.pose.orientation.z = quat[2]
00734 ps.pose.orientation.w = quat[3]
00735 if arm == 0:
00736 self.r_arm_cart_pub.publish(ps)
00737 else:
00738 self.l_arm_cart_pub.publish(ps)
00739
00740
00741 def go_cep_jtt(self, arm, p):
00742 step_size = 0.01
00743 sleep_time = 0.1
00744 cep_p, cep_rot = self.get_cep_jtt(arm)
00745 unit_vec = (p-cep_p)
00746 unit_vec = unit_vec / np.linalg.norm(unit_vec)
00747 while np.linalg.norm(p-cep_p) > step_size:
00748 cep_p += unit_vec * step_size
00749 self.set_cep_jtt(arm, cep_p)
00750 rospy.sleep(sleep_time)
00751 self.set_cep_jtt(arm, p)
00752 rospy.sleep(sleep_time)
00753
00754
00755
00756
00757
00758
00759
00760
00761
00762
00763
00764 def move_gripper(self, arm, amount=0.08, effort = 15):
00765 self.gripper_action_client[arm].send_goal(Pr2GripperCommandGoal(Pr2GripperCommand(position=amount, max_effort = effort)))
00766
00767
00768
00769
00770
00771 def open_gripper(self, arm):
00772 self.move_gripper(arm, 0.08, -1)
00773
00774
00775
00776
00777
00778 def close_gripper(self, arm, effort = 15):
00779 self.move_gripper(arm, 0.0, effort)
00780
00781
00782
00783
00784
00785
00786
00787
00788
00789
00790
00791
00792 def smooth_linear_arm_trajectory(self, arm, dist, dir=(0.,0.,-1.), max_jerk=0.25,
00793 delta=0.005, dur=None,
00794 freeze_wrist=True, is_grasp_biased=True):
00795
00796
00797
00798 def _smooth_traj_pos(t, k, T):
00799 return -k / T**3 * np.sin(T * t) + k / T**2 * t
00800
00801 def _smooth_traj_vel(t, k, T):
00802 return -k / T**2 * np.cos(T * t) + k / T**2
00803
00804 def _smooth_traj_acc(t, k, T):
00805 return k / T * np.sin(T * t)
00806
00807
00808 def _smooth_traj_time(l, k):
00809 return np.power(4 * np.pi**2 * l / k, 1./3.)
00810
00811 def _interpolate_traj(traj, k, T, num=10, begt=0., endt=1.):
00812 return [traj(t,k,T) for t in np.linspace(begt, endt, num)]
00813
00814
00815
00816
00817 traj_vec = [x/np.sqrt(np.vdot(dir,dir)) for x in dir]
00818
00819 num_steps = dist / delta
00820
00821 trajt = _smooth_traj_time(dist, max_jerk)
00822 period = 2. * np.pi / trajt
00823
00824
00825
00826 traj_pos_mult = _interpolate_traj(_smooth_traj_pos, max_jerk, period,
00827 num_steps, 0., trajt)
00828
00829
00830
00831
00832
00833
00834 cart_pos_traj = [(a*traj_vec[0],a*traj_vec[1],a*traj_vec[2]) for a in traj_pos_mult]
00835
00836
00837
00838 cur_pos, cur_rot = self.FK(arm, self.get_joint_angles(arm))
00839
00840 arm_traj = [(ct[0] + cur_pos[0], ct[1] + cur_pos[1],
00841 ct[2] + cur_pos[2]) for ct in cart_pos_traj]
00842 if dur is None:
00843 dur = trajt
00844
00845 if is_grasp_biased:
00846 self.grasp_biased_move_arm_trajectory(arm, arm_traj, dur,
00847 freeze_wrist=freeze_wrist)
00848 else:
00849 self.move_arm_trajectory(arm, arm_traj, dur, freeze_wrist=freeze_wrist)
00850
00851
00852 return dur
00853
00854 def bias_guess(self, q, joints_bias, bias_radius):
00855 if bias_radius == 0.0:
00856 return q
00857 max_angs = np.array([.69, 1.33, 0.79, 0.0, 1000000.0, 0.0, 1000000.0])
00858 min_angs = np.array([-2.27, -.54, -3.9, -2.34, -1000000.0, -2.15, -1000000.0])
00859 q_off = bias_radius * np.array(joints_bias) / np.linalg.norm(joints_bias)
00860 angs = np.array(q) + q_off
00861 for i in range(7):
00862 if angs[i] > max_angs[i]:
00863 angs[i] = max_angs[i]
00864 elif angs[i] < min_angs[i]:
00865 angs[i] = min_angs[i]
00866 return angs.tolist()
00867
00868
00869
00870 def grasp_biased_IK(self, arm, pos, rot, q_guess, joints_bias=[0.]*7, bias_radius=0., num_iters=5):
00871 angs = q_guess
00872 for i in range(num_iters):
00873 angs = self.IK(arm, pos, rot, angs)
00874 angs = self.bias_guess(angs, joints_bias, bias_radius)
00875 return self.IK(arm, pos, rot, angs)
00876
00877
00878
00879 def grasp_biased_move_arm(self, arm, pos, rot=None, dur=4.0, num_iters=20):
00880 if rot is None:
00881 temp, rot = self.FK(arm, begq)
00882 angs = self.get_joint_angles(arm)
00883 angs = self.grasp_biased_IK(arm, pos, rot, angs)
00884 self.set_joint_angles(arm, angs, dur)
00885
00886
00887
00888
00889
00890
00891
00892
00893
00894
00895
00896
00897
00898 def grasp_biased_move_arm_trajectory(self, arm, pos_arr, dur=1.,
00899 rot_arr=None, dur_arr=None, freeze_wrist=False):
00900 bias_vec = np.array([0., -1., -1., 0., 0., 1., 0.])
00901 q_radius = 0.012
00902 q_off = q_radius * bias_vec / np.linalg.norm(bias_vec)
00903
00904 if dur_arr is None:
00905 dur_arr = np.linspace(0., dur, len(pos_arr) + 1)[1:]
00906
00907 initq = self.get_joint_angles(arm)
00908 curq = initq
00909 q_arr = []
00910 fails, trys = 0, 0
00911 if rot_arr is None:
00912 temp, rot = self.FK(arm, curq)
00913
00914 for i in range(len(pos_arr)):
00915 if rot_arr is None:
00916 cur_rot = rot
00917 else:
00918 cur_rot = rot_arr[i]
00919 q_guess = (np.array(curq) + q_off).tolist()
00920 nextq = self.IK(arm, pos_arr[i], cur_rot, q_guess)
00921 q_arr += [nextq]
00922 if nextq is None:
00923 fails += 1
00924 trys += 1
00925 if not nextq is None:
00926 curq = nextq
00927 log("IK Accuracy: %d/%d (%f)" % (trys-fails, trys, (trys-fails)/float(trys)))
00928
00929 if freeze_wrist:
00930 for i in range(len(q_arr)):
00931 if not q_arr[i] is None:
00932 q_arr[i] = (q_arr[i][0], q_arr[i][1], q_arr[i][2], q_arr[i][3],
00933 q_arr[i][4], q_arr[i][5], initq[6])
00934
00935 self.set_joint_angles_traj(arm, q_arr, dur_arr)
00936
00937 if __name__ == '__main__':
00938 rospy.init_node(node_name, anonymous = True)
00939 log("Node initialized")
00940
00941 pr2_arm = PR2Arms()
00942
00943 if False:
00944 q = [0, 0, 0, 0, 0, 0, 0]
00945 pr2_arm.set_jointangles('right_arm', q)
00946 ee_pos = simparm.FK('right_arm', q)
00947 log('FK result:' + ee_pos.A1)
00948
00949 ee_pos[0,0] -= 0.1
00950 q_ik = pr2_arm.IK('right_arm', ee_pos, tr.Rx(0.), q)
00951 log('q_ik:' + [math.degrees(a) for a in q_ik])
00952
00953 rospy.spin()
00954
00955 if False:
00956 p = np.matrix([0.9, -0.3, -0.15]).T
00957
00958 rot = tr.Rx(math.radians(90.))
00959 pr2_arm.set_cartesian('right_arm', p, rot)
00960
00961
00962
00963
00964
00965
00966
00967
00968
00969
00970 r_arm, l_arm = 0, 1
00971
00972
00973
00974
00975
00976
00977
00978
00979 raw_input('Hit ENTER to move')
00980 p1 = np.matrix([0.91, -0.22, -0.05]).T
00981 pr2_arm.go_cep_jtt(r_arm, p1)
00982
00983
00984
00985 raw_input('Hit ENTER to move')
00986 p2 = np.matrix([0.70, -0.22, -0.05]).T
00987 pr2_arm.go_cep_jtt(r_arm, p2)
00988
00989
00990
00991
00992
00993
00994