arm_intermediary.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 import sys
00003 import math
00004 import numpy as np
00005 
00006 import roslib; roslib.load_manifest('assistive_teleop')
00007 import rospy
00008 import actionlib
00009 from geometry_msgs.msg  import PoseStamped, Point
00010 from trajectory_msgs.msg import JointTrajectoryPoint
00011 from std_msgs.msg import String, Float32, Bool
00012 from tf import TransformListener, transformations
00013 from pr2_controllers_msgs.msg import Pr2GripperCommand
00014 
00015 from pixel_2_3d.srv import Pixel23d
00016 from pr2_arms import PR2Arm_Planning
00017 from pr2_gripper import PR2Gripper
00018 import pose_utils as pu
00019 
00020 class ArmIntermediary():
00021     def __init__(self, arm):
00022         self.arm = arm
00023         self.tfl = TransformListener()
00024         self.pr2_arm = PR2Arm_Planning(self.arm, self.tfl)
00025         self.pr2_gripper = PR2Gripper(self.arm)
00026 
00027         rospy.loginfo('Waiting for Pixel_2_3d Service')
00028         try:
00029             rospy.wait_for_service('/pixel_2_3d', 7.0)
00030             self.p23d_proxy = rospy.ServiceProxy('/pixel_2_3d', Pixel23d, True)
00031             rospy.loginfo("Found pixel_2_3d service")
00032         except:
00033             rospy.logwarn("Pixel_2_3d Service Not available")
00034 
00035         #Low-level motion requests: pass directly to pr2_arm
00036         rospy.Subscriber("wt_"+self.arm+"_arm_pose_commands", Point,
00037                          self.torso_frame_move)
00038         rospy.Subscriber("wt_"+self.arm+"_arm_angle_commands", JointTrajectoryPoint,
00039                          self.pr2_arm.send_traj_point)
00040 
00041         #More complex motion scripts, built here using pr2_arm functions 
00042         rospy.Subscriber("norm_approach_"+self.arm, PoseStamped, self.norm_approach)
00043         rospy.Subscriber("wt_grasp_"+self.arm+"_goal", PoseStamped, self.grasp)
00044         rospy.Subscriber("wt_wipe_"+self.arm+"_goals", PoseStamped, self.wipe)
00045         rospy.Subscriber("wt_swipe_"+self.arm+"_goals", PoseStamped, self.swipe)
00046         rospy.Subscriber("wt_lin_move_"+self.arm, Float32, self.hand_move)
00047         rospy.Subscriber("wt_adjust_elbow_"+self.arm, Float32, 
00048                         self.pr2_arm.adjust_elbow)
00049         rospy.Subscriber('wt_surf_wipe_right_points', Point, 
00050                         self.prep_surf_wipe)
00051         rospy.Subscriber("wt_poke_"+self.arm+"_point", PoseStamped, self.poke)
00052         rospy.Subscriber(rospy.get_name()+"/log_out", String, self.repub_log)
00053         rospy.Subscriber("wt_"+self.arm[0]+"_gripper_commands",
00054                         Pr2GripperCommand, self.gripper_pos)
00055         rospy.Subscriber("wt_"+self.arm[0]+"_gripper_grab_commands",
00056                         Bool, self.gripper_grab)
00057         rospy.Subscriber("wt_"+self.arm[0]+"_gripper_release_commands",
00058                         Bool, self.gripper_release)
00059 
00060         self.wt_log_out = rospy.Publisher("wt_log_out", String)
00061 
00062         self.wipe_started = False
00063         self.surf_wipe_started = False
00064         self.wipe_ends = [PoseStamped(), PoseStamped()]
00065 
00066     def repub_log(self, msg):
00067         self.wt_log_out.publish(msg)
00068 
00069     def gripper_pos(self, msg):
00070         self.pr2_gripper.gripper_action(msg.position, msg.max_effort)
00071 
00072     def gripper_grab(self, msg):
00073         self.pr2_gripper.grab()
00074 
00075     def gripper_release(self, msg):
00076         self.pr2_gripper.release()
00077 
00078     def torso_frame_move(self, msg):
00079         """Do linear motion relative to torso frame."""
00080         goal = self.pr2_arm.curr_pose()
00081         goal.pose.position.x += msg.x
00082         goal.pose.position.y += msg.y
00083         goal.pose.position.z += msg.z
00084         self.pr2_arm.blind_move(goal)
00085 
00086     def hand_move(self, f32):
00087         """Do linear motion relative to hand frame."""
00088         hfm_pose = self.pr2_arm.hand_frame_move(f32.data)
00089         self.pr2_arm.blind_move(hfm_pose)
00090 
00091     def norm_approach(self, pose):
00092         """ Safe move normal to surface pose at given distance."""
00093         appr = pu.find_approach(pose, 0.25)
00094         self.pr2_arm.move_arm_to(appr)
00095         
00096     def grasp(self, ps):
00097         """Do simple grasp: Normal approch, open, advance, close, retreat."""
00098         rospy.loginfo("Initiating Grasp Sequence")
00099         self.wt_log_out.publish(data="Initiating Grasp Sequence")
00100         approach = pose_utils.find_approach(ps, 0.15)
00101         rospy.loginfo("approach: \r\n %s" %approach)
00102         at_appr = self.pr2_arm.move_arm_to(approach)
00103         rospy.loginfo("arrived at approach: %s" %at_appr)
00104         if at_appr:
00105             opened = self.pr2_arm.gripper(0.09)
00106             if opened:
00107                 rospy.loginfo("making linear approach")
00108                 hfm_pose = pose_utils.find_approach(ps, -0.02) 
00109                 self.pr2_arm.blind_move(hfm_pose)
00110                 self.pr2_arm.wait_for_stop()
00111                 closed = self.pr2_arm.gripper(0.0)
00112                 if not closed:
00113                     rospy.loginfo("Couldn't close completely:\
00114                                     Grasp likely successful")
00115                 hfm_pose = self.pr2_arm.hand_frame_move(-0.20) 
00116                 self.pr2_arm.blind_move(hfm_pose)
00117         else:
00118             pass
00119 
00120     def prep_surf_wipe(self, point):
00121         pixel_u = point.x
00122         pixel_v = point.y
00123         test_pose = self.p23d_proxy(pixel_u, pixel_v).pixel3d
00124         test_pose = pose_utils.find_approach(test_pose, 0)
00125         (reachable, ik_goal) = self.pr2_arm.full_ik_check(test_pose)
00126         if reachable:
00127             if not self.surf_wipe_started:
00128                 start_pose = test_pose
00129                 self.surf_wipe_start = [pixel_u, pixel_v, start_pose]
00130                 self.surf_wipe_started = True
00131                 rospy.loginfo("Received valid starting position for wiping\
00132                                 action")
00133                 self.wt_log_out.publish(data="Received valid starting position\
00134                                             for wiping action")
00135                 return #Return after 1st point, wait for second
00136             else:
00137                 rospy.loginfo("Received valid ending position for wiping\
00138                                 action")
00139                 self.wt_log_out.publish(data="Received valid ending position\
00140                                             for wiping action")
00141                 self.surf_wipe_started = False
00142         else:
00143             rospy.loginfo("Cannot reach wipe position, please try another")
00144             self.wt_log_out.publish(data="Cannot reach wipe position,\
00145                                             please try another")
00146             return #Return on invalid point, wait for another
00147         
00148         dist = self.pr2_arm.calc_dist(self.surf_wipe_start[2],test_pose)
00149         print 'dist', dist
00150         num_points = dist/0.02
00151         print 'num_points', num_points
00152         us = np.round(np.linspace(self.surf_wipe_start[0], pixel_u, num_points))
00153         vs = np.round(np.linspace(self.surf_wipe_start[1], pixel_v, num_points))
00154         surf_points = [PoseStamped() for i in xrange(len(us))]
00155         print "Surface Points", [us,vs]
00156         for i in xrange(len(us)):
00157             pose = self.p23d_proxy(us[i],vs[i]).pixel3d
00158             surf_points[i] = pose_utils.find_approach(pose,0)
00159             print i+1, '/', len(us)
00160 
00161         self.pr2_arm.blind_move(surf_points[0])
00162         self.pr2_arm.wait_for_stop()
00163         for pose in surf_points:
00164             self.pr2_arm.blind_move(pose,2.5)
00165             rospy.sleep(2)
00166         self.pr2_arm.hand_frame_move(-0.1)       
00167 
00168     def poke(self, ps):
00169         appr = pose_utils.find_approach(ps,0.15)
00170         prepared = self.pr2_arm.move_arm_to(appr)
00171         if prepared:
00172             pt1 = self.pr2_arm.hand_frame_move(0.155)
00173             self.pr2_arm.blind_move(pt1)
00174             self.pr2_arm.wait_for_stop()
00175             pt2 = self.pr2_arm.hand_frame_move(-0.155)
00176             self.pr2_arm.blind_move(pt2)
00177 
00178     def swipe(self, ps):
00179         traj = self.prep_wipe(ps)
00180         if traj is not None:
00181             self.wipe_move(traj, 1)
00182 
00183     def wipe(self, ps):
00184         traj = self.prep_wipe(ps)
00185         if traj is not None:
00186             self.wipe_move(traj, 4)
00187 
00188     def prep_wipe(self, ps):
00189         #print "Prep Wipe Received: %s" %pa
00190         print "Updating frame to: %s \r\n" %ps
00191         if not self.wipe_started:
00192             self.wipe_appr_seed = ps
00193             self.wipe_ends[0] = pose_utils.find_approach(ps, 0)
00194             print "wipe_end[0]: %s" %self.wipe_ends[0]
00195             (reachable, ik_goal) = self.pr2_arm.full_ik_check(self.wipe_ends[0])
00196             if not reachable:
00197                 rospy.loginfo("Cannot find approach for initial wipe position,\
00198                                 please try another")
00199                 self.wt_log_out.publish(data="Cannot find approach for initial\
00200                                             wipe position, please try another")
00201                 return None
00202             else:
00203                 self.wipe_started = True
00204                 rospy.loginfo("Received starting position for wiping action")
00205                 self.wt_log_out.publish(data="Received starting position for\
00206                                                 wiping action")
00207                 return None
00208         else:
00209             self.wipe_ends[1] = pose_utils.find_approach(ps, 0)
00210             self.wipe_ends.reverse()
00211             (reachable, ik_goal) = self.pr2_arm.full_ik_check(self.wipe_ends[1])
00212             if not reachable:
00213                 rospy.loginfo("Cannot find approach for final wipe position,\
00214                                 please try another")
00215                 self.wt_log_out.publish(data="Cannot find approach for final\
00216                                             wipe position, please try another")
00217                 return None
00218             else:
00219                 rospy.loginfo("Received End position for wiping action")
00220                 self.wt_log_out.publish(data="Received End position for wiping\
00221                                             action")
00222 
00223                 self.wipe_ends[1].header.stamp = rospy.Time.now()
00224                 self.tfl.waitForTransform(self.wipe_ends[1].header.frame_id,
00225                                           'rh_utility_frame',
00226                                           rospy.Time.now(),
00227                                           rospy.Duration(3.0))
00228                 fin_in_start = self.tfl.transformPose('rh_utility_frame',
00229                                                         self.wipe_ends[1])
00230                 
00231                 ang = math.atan2(-fin_in_start.pose.position.z,
00232                                     -fin_in_start.pose.position.y)+(math.pi/2)
00233                 q_st_rot = transformations.quaternion_about_axis(ang, (1,0,0))
00234                 q_st_new = transformations.quaternion_multiply(
00235                                     [self.wipe_ends[0].pose.orientation.x,
00236                                      self.wipe_ends[0].pose.orientation.y,
00237                                      self.wipe_ends[0].pose.orientation.z,
00238                                      self.wipe_ends[0].pose.orientation.w],
00239                                      q_st_rot)
00240                 self.wipe_ends[0].pose.orientation = Quaternion(*q_st_new)
00241                 self.wipe_ends[0].header.stamp = rospy.Time.now()
00242                 self.tfl.waitForTransform(self.wipe_ends[0].header.frame_id,
00243                                             'rh_utility_frame',
00244                                             rospy.Time.now(),
00245                                             rospy.Duration(3.0))
00246                 start_in_fin = self.tfl.transformPose('rh_utility_frame',
00247                                                         self.wipe_ends[0])
00248                 ang = math.atan2(start_in_fin.pose.position.z,
00249                                 start_in_fin.pose.position.y)+(math.pi/2)
00250                 
00251                 q_st_rot = transformations.quaternion_about_axis(ang, (1,0,0))
00252                 q_st_new = transformations.quaternion_multiply(
00253                                         [self.wipe_ends[1].pose.orientation.x,
00254                                          self.wipe_ends[1].pose.orientation.y,
00255                                          self.wipe_ends[1].pose.orientation.z,
00256                                          self.wipe_ends[1].pose.orientation.w],
00257                                          q_st_rot)
00258                 self.wipe_ends[1].pose.orientation = Quaternion(*q_st_new)
00259                 
00260                 appr = pose_utils.find_approach(self.wipe_appr_seed, 0.15)
00261                 appr.pose.orientation = self.wipe_ends[1].pose.orientation
00262                 prepared = self.pr2_arm.move_arm_to(appr)
00263                 if prepared:
00264                     self.pr2_arm.blind_move(self.wipe_ends[1])
00265                     traj = self.pr2_arm.build_trajectory(self.wipe_ends[1],
00266                                                          self.wipe_ends[0])
00267                     wipe_traj = self.pr2_arm.build_follow_trajectory(traj)
00268                     self.pr2_arm.wait_for_stop()
00269                     self.wipe_started = False
00270                     return wipe_traj
00271                     #self.wipe(wipe_traj)
00272                 else:
00273                     rospy.loginfo("Failure reaching start point,\
00274                                     please try again")
00275                     self.wt_log_out.publish(data="Failure reaching start\
00276                                                     point, please try again")
00277     
00278     def wipe_move(self, traj_goal, passes=4):
00279         times = []
00280         for i in range(len(traj_goal.trajectory.points)):
00281             times.append(traj_goal.trajectory.points[i].time_from_start)
00282         count = 0
00283         while count < passes:
00284             traj_goal.trajectory.points.reverse()
00285             for i in range(len(times)):
00286                 traj_goal.trajectory.points[i].time_from_start = times[i]
00287             traj_goal.trajectory.header.stamp = rospy.Time.now()
00288             assert traj_goal.trajectory.points[0] != []
00289             self.pr2_arm.r_arm_follow_traj_client.send_goal(traj_goal)
00290             self.pr2_arm.r_arm_follow_traj_client.wait_for_result(
00291                                                     rospy.Duration(20))
00292             rospy.sleep(0.5)# Pause at end of swipe
00293             count += 1
00294         
00295         rospy.loginfo("Done Wiping")
00296         self.wt_log_out.publish(data="Done Wiping")
00297         hfm_pose = self.pr2_arm.hand_frame_move(-0.15)
00298         self.pr2_arm.blind_move(hfm_pose)
00299 
00300 if __name__ == '__main__':
00301     arm = rospy.myargv(argv=sys.argv)[1]
00302     rospy.init_node('wt_'+arm+'_arm_intermediary')
00303     ai = ArmIntermediary(arm)
00304     while not rospy.is_shutdown():
00305         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