00001
00002
00003 import numpy as np
00004 from copy import deepcopy
00005 import math
00006
00007 import roslib; roslib.load_manifest('assistive_teleop')
00008 import rospy
00009 import actionlib
00010 from geometry_msgs.msg import PoseStamped, Point, Quaternion, WrenchStamped
00011 from std_msgs.msg import Float64MultiArray
00012 from tf import transformations, TransformListener
00013
00014 from assistive_teleop.srv import FrameUpdate, FrameUpdateRequest
00015 from assistive_teleop.msg import FtMoveAction, FtMoveGoal, FtHoldAction, FtHoldGoal
00016
00017 TEST = True
00018
00019 class jt_task_utils():
00020 def __init__(self, tf=None):
00021 if tf is None:
00022 self.tf = TransformListener()
00023 else:
00024 self.tf = tf
00025
00026
00027 rospy.loginfo("Waiting for utility_frame_services")
00028 try:
00029 rospy.wait_for_service('/l_utility_frame_update', 3.0)
00030 rospy.wait_for_service('/r_utility_frame_update', 3.0)
00031 self.update_frame = [rospy.ServiceProxy('/l_utility_frame_update', FrameUpdate),\
00032 rospy.ServiceProxy('/r_utility_frame_update', FrameUpdate)]
00033
00034 rospy.loginfo("Found utility_frame_services")
00035 except:
00036 rospy.logwarn("Left or Right Utility Frame Service Not available")
00037
00038
00039 self.ft_move_client = actionlib.SimpleActionClient('l_cart/ft_move_action', FtMoveAction)
00040 rospy.loginfo("Waiting for l_cart/ft_move_action server")
00041 if self.ft_move_client.wait_for_server(rospy.Duration(3)):
00042 rospy.loginfo("Found l_cart/ft_move_action server")
00043 else:
00044 rospy.logwarn("Cannot find l_cart/ft_move_action server")
00045
00046 self.ft_move_r_client = actionlib.SimpleActionClient('r_cart/ft_move_action', FtMoveAction)
00047 rospy.loginfo("Waiting for r_cart/ft_move_action server")
00048 if self.ft_move_r_client.wait_for_server(rospy.Duration(3)):
00049 rospy.loginfo("Found r_cart/ft_move_action server")
00050 else:
00051 rospy.logwarn("Cannot find r_cart/ft_move_action server")
00052
00053 self.ft_hold_client = actionlib.SimpleActionClient('ft_hold_action', FtHoldAction)
00054 rospy.loginfo("Waiting for ft_hold_action server")
00055 if self.ft_hold_client.wait_for_server(rospy.Duration(3)):
00056 rospy.loginfo("Found ft_hold_action server")
00057 else:
00058 rospy.logwarn("Cannot find ft_hold_action server")
00059
00060
00061 self.curr_state = [PoseStamped(), PoseStamped()]
00062 rospy.Subscriber('/l_cart/state/x', PoseStamped, self.get_l_state)
00063 rospy.Subscriber('/r_cart/state/x', PoseStamped, self.get_r_state)
00064 rospy.Subscriber('/wt_l_wrist_command', Point, self.rot_l_wrist)
00065 rospy.Subscriber('/wt_r_wrist_command', Point, self.rot_r_wrist)
00066 rospy.Subscriber('/wt_left_arm_pose_commands', Point, self.trans_l_hand)
00067 rospy.Subscriber('/wt_right_arm_pose_commands', Point, self.trans_r_hand)
00068
00069
00070
00071
00072
00073
00074
00075
00076 self.goal_pub = [rospy.Publisher('l_cart/command_pose', PoseStamped),\
00077 rospy.Publisher('r_cart/command_pose', PoseStamped)]
00078
00079 self.posture_pub = [rospy.Publisher('l_cart/command_posture', Float64MultiArray),\
00080 rospy.Publisher('r_cart/command_posture', Float64MultiArray)]
00081
00082
00083 self.postures = {
00084 'off': [],
00085 'mantis': [0, 1, 0, -1, 3.14, -1, 3.14],
00086 'elbowupr': [-0.79,0,-1.6, 9999, 9999, 9999, 9999],
00087 'elbowupl': [0.79,0,1.6 , 9999, 9999, 9999, 9999],
00088 'old_elbowupr': [-0.79,0,-1.6, -0.79,3.14, -0.79,5.49],
00089 'old_elbowupl': [0.79,0,1.6, -0.79,3.14, -0.79,5.49],
00090 'elbowdownr': [-0.028262, 1.294634, -0.2578564, -1.549888, -31.27891385, -1.05276449, -1.8127318],
00091 'elbowdownl': [-0.008819572, 1.28348282, 0.2033844, -1.5565279, -0.09634, -1.023502, 1.799089]
00092 }
00093
00094 def get_l_state(self, ps):
00095 self.curr_state[0] = ps
00096
00097 def get_r_state(self, ps):
00098 self.curr_state[1] = ps
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110 def rot_l_wrist(self, pt):
00111 out_pose = deepcopy(self.curr_state[0])
00112 q_r = transformations.quaternion_about_axis(pt.x, (1,0,0))
00113 q_p = transformations.quaternion_about_axis(pt.y, (0,1,0))
00114 q_h = transformations.quaternion_multiply(q_r, q_p)
00115 q_f = transformations.quaternion_about_axis(pt.y, (1,0,0))
00116
00117 if pt.x or pt.y:
00118 self.tf.waitForTransform(out_pose.header.frame_id, 'l_wrist_roll_link', out_pose.header.stamp, rospy.Duration(3.0))
00119 hand_pose = self.tf.transformPose('l_wrist_roll_link', out_pose)
00120 q_hand_pose = (hand_pose.pose.orientation.x, hand_pose.pose.orientation.y, hand_pose.pose.orientation.z, hand_pose.pose.orientation.w)
00121 q_h_rot = transformations.quaternion_multiply(q_h, hand_pose.pose.orientation)
00122 hand_pose.pose.orientation = Quaternion(*q_h_rot)
00123 out_pose = self.tf.transformPose(out_pose.header.frame_id, hand_pose)
00124
00125 if pt.z:
00126 self.tf.waitForTransform(out_pose.header.frame_id, 'l_forearm_roll_link', out_pose.header.stamp, rospy.Duration(3.0))
00127 hand_pose = self.tf.transformPose('l_forearm_roll_link', out_pose)
00128 q_hand_pose = (hand_pose.pose.orientation.x, hand_pose.pose.orientation.y, hand_pose.pose.orientation.z, hand_pose.pose.orientation.w)
00129 q_f_rot = transformations.quaternion_multiply(q_f, hand_pose.pose.orientation)
00130 hand_pose.pose.orientation = Quaternion(*q_f_rot)
00131 out_pose = self.tf.transformPose(out_pose.header.frame_id, hand_pose)
00132
00133 wrist_traj = self.build_trajectory(out_pose, arm=0)
00134
00135
00136 def rot_r_wrist(self, pt):
00137 out_pose = deepcopy(self.curr_state[1])
00138 q_r = transformations.quaternion_about_axis(-pt.x, (1,0,0))
00139 q_p = transformations.quaternion_about_axis(-pt.y, (0,1,0))
00140 q_h = transformations.quaternion_multiply(q_r, q_p)
00141 q_f = transformations.quaternion_about_axis(-pt.y, (1,0,0))
00142
00143 if pt.x or pt.y:
00144 self.tf.waitForTransform(out_pose.header.frame_id, 'r_wrist_roll_link', out_pose.header.stamp, rospy.Duration(3.0))
00145 hand_pose = self.tf.transformPose('r_wrist_roll_link', out_pose)
00146 q_hand_pose = (hand_pose.pose.orientation.x, hand_pose.pose.orientation.y, hand_pose.pose.orientation.z, hand_pose.pose.orientation.w)
00147 q_h_rot = transformations.quaternion_multiply(q_h, hand_pose.pose.orientation)
00148 hand_pose.pose.orientation = Quaternion(*q_h_rot)
00149 out_pose = self.tf.transformPose(out_pose.header.frame_id, hand_pose)
00150
00151 if pt.z:
00152 self.tf.waitForTransform(out_pose.header.frame_id, 'r_forearm_roll_link', out_pose.header.stamp, rospy.Duration(3.0))
00153 hand_pose = self.tf.transformPose('r_forearm_roll_link', out_pose)
00154 q_hand_pose = (hand_pose.pose.orientation.x, hand_pose.pose.orientation.y, hand_pose.pose.orientation.z, hand_pose.pose.orientation.w)
00155 q_f_rot = transformations.quaternion_multiply(q_f, hand_pose.pose.orientation)
00156 hand_pose.pose.orientation = Quaternion(*q_f_rot)
00157 out_pose = self.tf.transformPose(out_pose.header.frame_id, hand_pose)
00158
00159 wrist_traj = self.build_trajectory(out_pose, arm=1)
00160
00161
00162 def trans_l_hand(self, pt):
00163 print "Moving Left Hand with JT Task Controller"
00164 out_pose = PoseStamped()
00165 out_pose.header.frame_id = self.curr_state[0].header.frame_id
00166 out_pose.header.stamp = rospy.Time.now()
00167 out_pose.pose.position.x = self.curr_state[0].pose.position.x + pt.x
00168 out_pose.pose.position.y = self.curr_state[0].pose.position.y + pt.y
00169 out_pose.pose.position.z = self.curr_state[0].pose.position.z + pt.z
00170 out_pose.pose.orientation = self.curr_state[0].pose.orientation
00171 trans_traj = self.build_trajectory(out_pose, arm=0)
00172 self.ft_move_client.send_goal(FtMoveGoal(trans_traj,0., True))
00173 self.ft_move_client.wait_for_result(rospy.Duration(0.025*len(trans_traj)))
00174
00175 def trans_r_hand(self, pt):
00176 out_pose = PoseStamped()
00177 out_pose.header.frame_id = self.curr_state[1].header.frame_id
00178 out_pose.header.stamp = rospy.Time.now()
00179 out_pose.pose.position.x = self.curr_state[1].pose.position.x + pt.x
00180 out_pose.pose.position.y = self.curr_state[1].pose.position.y + pt.y
00181 out_pose.pose.position.z = self.curr_state[1].pose.position.z + pt.z
00182 out_pose.pose.orientation = self.curr_state[1].pose.orientation
00183 trans_traj = self.build_trajectory(out_pose, arm=0)
00184 self.ft_move_client.send_goal(FtMoveGoal(trans_traj,0., True))
00185 self.ft_move_client.wait_for_result(rospy.Duration(0.025*len(trans_traj)))
00186
00187 def send_posture(self, posture='off', arm=0 ):
00188 if 'elbow' in posture:
00189 if arm == 0:
00190 posture = posture + 'l'
00191 elif arm == 1:
00192 posture = posture + 'r'
00193 self.posture_pub[arm].publish(Float64MultiArray(data=self.postures[posture]))
00194
00195 def send_traj(self, poses, arm=0):
00196 send_rate = rospy.Rate(50)
00197
00198 finished = False
00199 count = 0
00200 while not (rospy.is_shutdown() or finished):
00201 self.goal_pub[arm].publish(poses[count])
00202 count += 1
00203 send_rate.sleep()
00204 if count == len(poses):
00205 finished = True
00206
00207 def send_traj_to_contact(self, poses, arm=0):
00208 send_rate = rospy.Rate(20)
00209
00210 finished = False
00211 count = 0
00212 while not (rospy.is_shutdown() or finished or self.force_stopped):
00213 self.goal_pub[arm].publish(poses[count])
00214 count += 1
00215 send_rate.sleep()
00216 if count == len(poses):
00217 finished = True
00218
00219 def build_trajectory(self, finish, start=None, arm=0, space = 0.001, steps=None):
00220
00221 if start is None:
00222 start = self.curr_state[arm]
00223
00224 dist = self.calc_dist(start,finish,arm=arm)
00225 if steps is None:
00226 steps = int(math.ceil(dist/space))
00227 fracs = np.linspace(0, 1, steps)
00228 print "Steps: %s" %steps
00229
00230 poses = [PoseStamped() for i in xrange(steps)]
00231 xs = np.linspace(start.pose.position.x, finish.pose.position.x, steps)
00232 ys = np.linspace(start.pose.position.y, finish.pose.position.y, steps)
00233 zs = np.linspace(start.pose.position.z, finish.pose.position.z, steps)
00234
00235 qs = [start.pose.orientation.x, start.pose.orientation.y,
00236 start.pose.orientation.z, start.pose.orientation.w]
00237 qf = [finish.pose.orientation.x, finish.pose.orientation.y,
00238 finish.pose.orientation.z, finish.pose.orientation.w]
00239
00240 for i,frac in enumerate(fracs):
00241 poses[i].header.stamp = rospy.Time.now()
00242 poses[i].header.frame_id = start.header.frame_id
00243 poses[i].pose.position = Point(xs[i], ys[i], zs[i])
00244 new_q = transformations.quaternion_slerp(qs,qf,frac)
00245 poses[i].pose.orientation = Quaternion(*new_q)
00246
00247
00248 return poses
00249
00250 def pose_frame_move(self, pose, x, y=0, z=0, arm=0):
00251 self.update_frame[arm](pose)
00252 if arm == 0:
00253 frame = 'lh_utility_frame'
00254 elif arm == 1:
00255 frame = 'rh_utility_frame'
00256 pose.header.stamp = rospy.Time.now()
00257 self.tf.waitForTransform(pose.header.frame_id, frame , pose.header.stamp, rospy.Duration(3.0))
00258 framepose = self.tf.transformPose(frame, pose)
00259 framepose.pose.position.x += x
00260 framepose.pose.position.y += y
00261 framepose.pose.position.z += z
00262 self.dist = math.sqrt(x**2+y**2+z**2)
00263 self.tf.waitForTransform(frame, pose.header.frame_id, pose.header.stamp, rospy.Duration(3.0))
00264 return self.tf.transformPose(pose.header.frame_id, framepose)
00265
00266 def calc_dist(self, ps1, ps2=None, arm=0):
00267 if ps2 is None:
00268 ps2 = self.curr_pose()
00269
00270 p1 = ps1.pose.position
00271 p2 = ps2.pose.position
00272 wrist_dist = math.sqrt((p1.x-p2.x)**2 + (p1.y-p2.y)**2 + (p1.z-p2.z)**2)
00273
00274 self.update_frame[arm](ps2)
00275 ps2.header.stamp=rospy.Time(0)
00276 np2 = self.tf.transformPose('lh_utility_frame', ps2)
00277 np2.pose.position.x += 0.21
00278 self.tf.waitForTransform(np2.header.frame_id, 'torso_lift_link', rospy.Time.now(), rospy.Duration(3.0))
00279 p2 = self.tf.transformPose('torso_lift_link', np2)
00280
00281 self.update_frame[arm](ps1)
00282 ps1.header.stamp=rospy.Time(0)
00283 np1 = self.tf.transformPose('lh_utility_frame', ps1)
00284 np1.pose.position.x += 0.21
00285 self.tf.waitForTransform(np1.header.frame_id, 'torso_lift_link', rospy.Time.now(), rospy.Duration(3.0))
00286 p1 = self.tf.transformPose('torso_lift_link', np1)
00287
00288 p1 = p1.pose.position
00289 p2 = p2.pose.position
00290 finger_dist = math.sqrt((p1.x-p2.x)**2 + (p1.y-p2.y)**2 + (p1.z-p2.z)**2)
00291 dist = max(wrist_dist, finger_dist)
00292 print 'Calculated Distance: ', dist
00293 return dist
00294
00295 def test(self):
00296 print "Testing..."
00297 rospy.sleep(1)
00298
00299 print "Left Current Pose:"
00300 print self.curr_state[0]
00301 print "Right Current Pose:"
00302 print self.curr_state[1]
00303
00304
00305 print "Current Force Wrench:"
00306 print self.ft_wrench
00307 print "Current Force Magnitude:"
00308 print self.ft_mag
00309
00310
00311 l_pose = PoseStamped()
00312 l_pose.header.frame_id = 'torso_lift_link'
00313 l_pose.pose.position = Point(0.6, 0.3, 0.1)
00314 l_pose.pose.orientation = Quaternion(1,0,0,0)
00315 raw_input("send left arm goal")
00316 self.goal_pub[0].publish(l_pose)
00317
00318
00319
00320
00321
00322
00323
00324
00325
00326
00327
00328
00329
00330
00331
00332
00333
00334
00335
00336
00337
00338
00339
00340 l_pose2 = PoseStamped()
00341 l_pose2.header.frame_id = 'torso_lift_link'
00342 l_pose2.pose.position = Point(0.8, 0.3, 0.1)
00343 l_pose2.pose.orientation = Quaternion(1,0,0,0)
00344 raw_input("Left trajectory")
00345
00346 traj = self.build_trajectory(l_pose2)
00347 self.send_traj_to_contact(traj)
00348
00349
00350
00351
00352
00353
00354
00355
00356
00357
00358
00359 print "New Left Pose:"
00360 print self.curr_state[0]
00361 print "New Right Pose:"
00362 print self.curr_state[1]
00363
00364
00365 if __name__ == '__main__':
00366 rospy.init_node('jttask_utils_test')
00367 jttu = jt_task_utils()
00368 if TEST:
00369 jttu.test()
00370 else:
00371 while not rospy.is_shutdown():
00372 rospy.spin()