00001
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
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
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
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
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
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
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)
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()