jttask_utils.py
Go to the documentation of this file.
00001 #!/usr/bin/python
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         #### SERVICES ####
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         #### Action Clients ####
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         #### SUBSCRIBERS ####
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      #   self.ft_wrench = WrenchStamped()
00070      #   self.force_stopped = False
00071      #   self.ft_z_thresh = -2
00072      #   self.ft_mag_thresh = 5
00073      #   rospy.Subscriber('ft_data_pm_adjusted', WrenchStamped, self.get_ft_state)
00074         
00075         #### PUBLISHERS ####
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         #### STATIC DATA ####
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):#WORKING, TESTED
00095         self.curr_state[0] = ps
00096 
00097     def get_r_state(self, ps): #WORKING, TESTED
00098         self.curr_state[1] = ps
00099 
00100   #  def get_ft_state(self, ws):
00101   #      self.ft_wrench = ws
00102   #      self.ft_mag = math.sqrt(ws.wrench.force.x**2 + ws.wrench.force.y**2 + ws.wrench.force.z**2)
00103   #      if ws.wrench.force.z < self.ft_z_thresh:
00104   #          self.force_stopped = True
00105   #         # rospy.logwarn("Z force threshold exceeded")
00106   #      if self.ft_mag > self.ft_mag_thresh:
00107   #          self.force_stopped = True
00108   #          rospy.logwarn("Total force threshold exceeded")
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)) #Hand frame roll (hand roll)
00113         q_p = transformations.quaternion_about_axis(pt.y, (0,1,0)) #Hand frame pitch (wrist flex)
00114         q_h = transformations.quaternion_multiply(q_r, q_p)
00115         q_f = transformations.quaternion_about_axis(pt.y, (1,0,0)) #Forearm frame rot (forearm roll)
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         #TODO: Add Action Goal Sender to Move along trajectory!!!!!!!!!!!!!!!!
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)) #Hand frame roll (hand roll)
00139         q_p = transformations.quaternion_about_axis(-pt.y, (0,1,0)) #Hand frame pitch (wrist flex)
00140         q_h = transformations.quaternion_multiply(q_r, q_p)
00141         q_f = transformations.quaternion_about_axis(-pt.y, (1,0,0)) #Forearm frame rot (forearm roll)
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         #TODO: Add Action Goal Sender to Move along trajectory!!!!!!!!!!!!!!!!
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 ): # WORKING, TESTED TODO: SLOW TRANSITION (if possible)
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         ##!!!!!!!!!!!!  MUST BALANCE SEND RATE WITH SPACING IN 'BUILD_TRAJECTORY' FOR CONTROL OF VELOCITY !!!!!!!!!!!!
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         ##!!!!!!!!!!!!  MUST BALANCE SEND RATE WITH SPACING IN 'BUILD_TRAJECTORY' FOR CONTROL OF VELOCITY !!!!!!!!!!!!
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): #WORKING, TESTED
00220     ##!!!!!!!!!!!!  MUST BALANCE SPACING WITH SEND RATE IN 'SEND_TRAJ' FOR CONTROL OF VELOCITY !!!!!!!!!!!!
00221         if start is None: # if given one pose, use current position as start
00222             start = self.curr_state[arm]
00223 
00224         dist = self.calc_dist(start,finish,arm=arm)     #Total distance to travel
00225         if steps is None:
00226             steps = int(math.ceil(dist/space))
00227         fracs = np.linspace(0, 1, steps)   #A list of fractional positions along course
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         #rospy.loginfo("Planning straight-line path, please wait")
00247         #self.wt_log_out.publish(data="Planning straight-line path, please wait")
00248         return poses
00249         
00250     def pose_frame_move(self, pose, x, y=0, z=0, arm=0): # FINISHED, UNTESTED
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): #FINISHED, UNTESTED
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         #### TEST STATE GRABBING ####
00299         print "Left Current Pose:"
00300         print self.curr_state[0]
00301         print "Right Current Pose:"
00302         print self.curr_state[1]
00303         
00304         #### TEST FORCE STATE GRABBING ####
00305         print "Current Force Wrench:"
00306         print self.ft_wrench
00307         print "Current Force Magnitude:"
00308         print self.ft_mag
00309 
00310         #### TEST LEFT ARM GOAL SENDING ####
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         #### TEST RIGHT ARM GOAL SENDING
00318         #r_pose = PoseStamped()
00319         #r_pose.header.frame_id = 'torso_lift_link'
00320         #r_pose.pose.position = Point(0.6, -0.3, 0.1)
00321         #r_pose.pose.orientation = Quaternion(1,0,0,0)
00322         #raw_input("send right arm goal")
00323         #self.goal_pub[1].publish(r_pose)
00324         
00325         #### TEST POSE SETTING ####
00326        # raw_input("Left Elbow Up")
00327        # self.send_posture('elbowup',0)
00328        # raw_input("Right Elbow Up")
00329        # self.send_posture('elbowup',1)
00330        # raw_input("Left Elbow Down")
00331        # self.send_posture('elbowdown',0)
00332         #raw_input("Right Elbow Down")
00333         #self.send_posture('elbowdown',1)
00334         #raw_input("Both Postures Off")
00335         #self.send_posture(arm=0)
00336         #self.send_posture(arm=1)
00337         #print "Postures adjusted"
00338 
00339         #### TEST TRAJECTORY MOTION ####
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         #l_pose2 = self.pose_frame_move(self.curr_state[0], -0.1, arm=0)
00346         traj = self.build_trajectory(l_pose2)
00347         self.send_traj_to_contact(traj)
00348 
00349         #r_pose2 = PoseStamped()
00350         #r_pose2.header.frame_id = 'torso_lift_link'
00351         #r_pose2.pose.position = Point(0.8, -0.15, -0.3)
00352         #r_pose2.pose.orientation = Quaternion(0,0.5,0.5,0)
00353         #raw_input("Right trajectory")
00354         #r_pose2 = self.pose_frame_move(self.curr_state[1], -0.1, arm=1)
00355         #traj = self.build_trajectory(r_pose2, arm=1)
00356         #self.send_traj(traj,1)
00357 
00358         #### RECONFIRM POSITION ####
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()


assistive_teleop
Author(s): Phillip M. Grice, Advisor: Prof. Charlie Kemp, Lab: The Healthcare Robotoics Lab at Georgia Tech.
autogenerated on Wed Nov 27 2013 11:35:34