reactive_grasp.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import roslib; roslib.load_manifest("sr_move_arm")
00004 import rospy
00005 
00006 import tf
00007 
00008 import scipy, time
00009 from object_manipulator.convert_functions import *
00010 from object_manipulation_msgs.msg import GripperTranslation, ManipulationPhase
00011 from sr_robot_msgs.srv import which_fingers_are_touching
00012 from trajectory_msgs.msg import JointTrajectory
00013 from actionlib import SimpleActionClient
00014 #from motion_planning_msgs.msg import *
00015 from arm_navigation_msgs.msg import *
00016 #from geometric_shapes_msgs.msg import Shape
00017 from actionlib_msgs.msg import GoalStatus
00018 
00019 from shadowhand_ros import ShadowHand_ROS
00020 
00021 ##abort exception
00022 class Aborted(Exception): pass
00023 
00024 class ReactiveGrasper(object):
00025     """
00026     reactive/guarded movement and grasping
00027     """
00028     def __init__(self, which_arm = 'r', slip_detection = False):
00029         self.using_slip_detection = slip_detection
00030         self.whicharm = which_arm
00031 
00032         self.mypub = rospy.Publisher("/command",JointTrajectory)
00033 
00034         #force to use when closing hard
00035         self.close_force = 100
00036         if self.using_slip_detection:
00037             self.close_force = 10
00038 
00039         #has the hand been closed hard recently?
00040         self._closed_hard = 0
00041 
00042         #collision name of support surface
00043         self._table_name = "table"
00044 
00045         #root and tip name for listening
00046         #to the current palm pose
00047         self.tf_listener = tf.TransformListener()
00048         self.root_name = "world"
00049         self.tip_name = "palm"
00050 
00051         #for taking photos of each step
00052         self._photo = 0
00053 
00054         #broadcast the current phase of the manipulation
00055         self._phase_pub = rospy.Publisher('/reactive_manipulation_phase', ManipulationPhase)
00056 
00057         #send targets to the arm / hand
00058         self.sr_lib   = ShadowHand_ROS()
00059         self.move_arm_client = None
00060 
00061         if which_arm == 'r':
00062             self.move_arm_client = SimpleActionClient( "/move_right_arm", MoveArmAction )
00063         else:
00064             self.move_arm_client = SimpleActionClient( "/move_left_arm", MoveArmAction )
00065         self.move_arm_client.wait_for_server()
00066 
00067         #client to the tactile sensor manager.
00068         rospy.loginfo("Waiting for service which_fingers_are_touching")
00069         rospy.wait_for_service('which_fingers_are_touching')
00070         rospy.loginfo("OK service which_fingers_are_touching found.")
00071         self.which_fingers_are_touching_client = rospy.ServiceProxy('which_fingers_are_touching', which_fingers_are_touching)
00072 
00073         #load tactile thresholds
00074         self.light_touch_thresholds = rospy.get_param('light_touch_thresholds', [100.,100.,100.,100.,0.0])
00075         rospy.loginfo("light and grasp threashold are :")
00076         rospy.loginfo(self.light_touch_thresholds)
00077         self.grasp_touch_thresholds = rospy.get_param('grasp_touch_thresholds', [117.,117.,113.,111.,0.0])
00078         rospy.loginfo(self.grasp_touch_thresholds)
00079 
00080         #dictionary for ManipulationPhase
00081         self.manipulation_phase_dict = {}
00082         for element in dir(ManipulationPhase):
00083             if element[0].isupper():
00084                 self.manipulation_phase_dict[eval('ManipulationPhase.'+element)] = element
00085 
00086         #dictionary of return values for reactive approach
00087         self.reactive_approach_result_dict = {"success":0, "within goal threshold":1, "both fingers touching":2, "ran out of tries":3,
00088                                               "touching table":4, "saw palm contact":5, "grasp infeasible":6}
00089 
00090         #dictionary of return values for reactive_grasp
00091         self.reactive_grasp_result_dict = {"success":0, "ran out of grasp tries":1, "ran out of approach tries":2,
00092                                            "aborted":3, "grasp infeasible":4}
00093 
00094         rospy.logdebug("done with ReactiveGrasper init for the %s arm"%self.whicharm)
00095 
00096     def broadcast_phase(self, phase):
00097         """
00098         broadcast the current manipulation phase (of type ManipulationPhase)
00099         """
00100         rospy.loginfo("broadcasting reactive phase %s"%self.manipulation_phase_dict[phase])
00101         self._phase_pub.publish(phase)
00102 
00103 
00104     def reactive_grasp(self, approach_pose, grasp_pose, joint_path = None, pregrasp_posture = None, grasp_posture = None, side_step = .015, back_step = .03,
00105                        approach_num_tries = 10, goal_pos_thres = 0.01, grasp_num_tries = 4,
00106                        forward_step = 0.03, object_name = "points", table_name = "table",
00107                        grasp_adjust_x_step = .02, grasp_adjust_z_step = .015, grasp_adjust_num_tries = 3):
00108         self._table_name = table_name
00109 
00110         self.check_preempt()
00111 
00112         #if approach_pose is not specified, use the current pose of the hand
00113         if approach_pose == None:
00114             (current_trans,current_rot) = self.tf_listener.lookupTransform(self.root_name, self.tip_name, rospy.Time(0))
00115             approach_pose = create_pose_stamped(current_trans+current_rot)
00116 
00117         self.check_preempt()
00118 
00119         #compute (unit) approach direction in base_link frame
00120         approach_dir = self.compute_approach_dir(approach_pose, grasp_pose)
00121 
00122         num_tries = 0
00123         current_goal_pose = grasp_pose
00124         while 1:
00125             if approach_num_tries:
00126                 #try the approach, unless the reactive approach is disabled (approach_num_tries is 0)
00127                 approach_result = self.reactive_approach(approach_dir, current_goal_pose, joint_path,
00128                                                          side_step, back_step,
00129                                                          approach_num_tries, goal_pos_thres)
00130                 #quit if the approach ran out of tries (couldn't get around the object)
00131                 if approach_result == self.reactive_approach_result_dict["ran out of tries"]:
00132                     return self.reactive_grasp_result_dict["ran out of approach tries"]
00133                 elif approach_result == self.reactive_approach_result_dict["grasp infeasible"]:
00134                     return self.reactive_grasp_result_dict["grasp infeasible"]
00135             else:
00136                 #reactive approach is disabled; just move to the goal
00137                 self.broadcast_phase(ManipulationPhase.MOVING_TO_GRASP)
00138                 rospy.loginfo("reactive approach is disabled, moving to the goal")
00139                 self.move_cartesian_step(grasp_pose, timeout = 10.0, settling_time = 5.0, blocking = 1)
00140 
00141             #go to the pregrasp posture
00142             rospy.loginfo("Going to pregrasp posture.")
00143             self.open_and_reset_fingertips(pregrasp_posture, reset = 1)
00144             #then start closing the hand compliantly
00145             rospy.loginfo("starting compliant_close")
00146             self.compliant_close(grasp_posture, pregrasp_posture)
00147 
00148             self.check_preempt()
00149 
00150             #check if the grasp is okay
00151             if self.check_good_grasp():
00152                 rospy.loginfo("Grasp successfully achieved")
00153                 return self.reactive_grasp_result_dict["success"]
00154 
00155             #if the grasp is not okay
00156             num_tries += 1
00157             if num_tries < grasp_num_tries:
00158                 rospy.logwarn("Failed to grasp: trying once more")
00159                 #move the goal forward along the approach direction by forward_step
00160                 move_forward_vect = approach_dir*forward_step
00161                 current_goal_pose = self.return_rel_pose(move_forward_vect, self.root_name, current_goal_pose)
00162 
00163                 joint_path = None
00164 
00165                 #print the new goal
00166                 (goal_pos, goal_rot) = pose_stamped_to_lists(self.tf_listener, current_goal_pose, self.root_name)
00167                 rospy.loginfo("trying approach again with new goal, pos: "+pplist(goal_pos)+" rot: "+pplist(goal_rot))
00168 
00169                 #open the gripper back up
00170                 self.open_and_reset_fingertips(pregrasp_posture, reset = 1)
00171 
00172             else:
00173                 rospy.loginfo("ran out of grasp tries!")
00174                 return self.reactive_grasp_result_dict["ran out of grasp tries"]
00175 
00176 
00177     def reactive_approach(self, approach_dir, current_goal_pose, joint_path = None,
00178                           side_step = .015, back_step = .03,
00179                           num_tries = 10, goal_pos_thres = 0.01):
00180         """
00181         Stops and goes back a little in case of a contact.
00182         """
00183 
00184         rospy.loginfo("Executing a reactive approach")
00185         self.broadcast_phase(ManipulationPhase.MOVING_TO_GRASP)
00186 
00187         #check the position of the palm compared to the position of the object
00188         #adjust if necessary
00189 
00190         current_sleep_time = rospy.Duration(0.0)
00191         last_time = rospy.Duration(0.0)
00192         #follow the joint_path if one is given
00193         if joint_path != None:
00194             self.check_preempt()
00195                     
00196             self.mypub.publish(joint_path)
00197             rospy.logerr("Sent a trajectory !")
00198             rospy.sleep(10)
00199             rospy.logerr("Trajectory should be finished !") 
00200             '''joint_names = joint_path.joint_names
00201             countTime = 5
00202 
00203             for trajectory_step in joint_path.points:
00204                 sendupdate_msg = []
00205 
00206                 for (joint_name, joint_target) in zip(joint_names, trajectory_step.positions):
00207                     # convert targets to degrees
00208                     sendupdate_msg.append(joint(joint_name = joint_name, joint_target = joint_target * 57.325))
00209 
00210                 self.sr_arm_target_pub.publish(sendupdate(len(sendupdate_msg), sendupdate_msg) )
00211                 self.sr_hand_target_pub.publish(sendupdate(len(sendupdate_msg), sendupdate_msg) )
00212 
00213                 current_sleep_time = trajectory_step.time_from_start - last_time
00214                 rospy.sleep( countTime ) # current_sleep_time )
00215                 countTime=max(countTime-1,0.5)
00216                 last_time = trajectory_step.time_from_start
00217         else:
00218             #if no joint_path is given, go to the current_goal_pose
00219             self.command_cartesian( current_goal_pose )'''
00220 
00221         return self.reactive_approach_result_dict["success"]
00222 
00223 
00224     def reactive_lift(self, goal):
00225         """
00226         Lifts the object while monitoring for contacts.
00227         """
00228         #follow the joint_path if one is given
00229         joint_path = goal.trajectory
00230         last_time = rospy.Duration(0.0)
00231         current_sleep_time = rospy.Duration(0.0)
00232 
00233         if joint_path != None:
00234             self.check_preempt()
00235             joint_names = joint_path.joint_names
00236 
00237             for trajectory_step in joint_path.points:
00238                 sendupdate_dict = {}
00239 
00240                 for (joint_name, joint_target) in zip(joint_names, trajectory_step.positions):
00241                     # convert targets to degrees
00242                     sendupdate_dict[joint_name] = joint_target * 57.325
00243 
00244                 self.sr_lib.sendupdate_arm_from_dict(sendupdate_dict)
00245                 self.sr_lib.sendupdate_from_dict(sendupdate_dict)
00246                 
00247                 current_sleep_time = trajectory_step.time_from_start - last_time
00248                 rospy.sleep( current_sleep_time )
00249                 last_time = trajectory_step.time_from_start
00250 
00251         else:
00252             #if no joint_path is given, go to the current_goal_pose
00253             rospy.logerr("TODO: Implement this. Get current pose, add the lift and set this as the command_cartesian target.")
00254             self.command_cartesian( goal.lift )
00255 
00256         return self.reactive_approach_result_dict["success"]
00257 
00258     def reactive_place(self, goal):
00259         """
00260         Lifts the object while monitoring for contacts.
00261         """
00262         #follow the joint_path if one is given
00263         joint_path = goal.trajectory
00264         last_time = rospy.Duration(0.0)
00265         current_sleep_time = rospy.Duration(0.0)
00266 
00267         if joint_path != None:
00268             self.check_preempt()
00269             joint_names = joint_path.joint_names
00270 
00271             for trajectory_step in joint_path.points:
00272                 sendupdate_dict = {}
00273 
00274                 for (joint_name, joint_target) in zip(joint_names, trajectory_step.positions):
00275                     # convert targets to degrees
00276                     sendupdate_dict[joint_name] = joint_target * 57.325
00277 
00278                 self.sr_lib.sendupdate_arm_from_dict(sendupdate_dict)
00279                 self.sr_lib.sendupdate_from_dict(sendupdate_dict)
00280 
00281                 current_sleep_time = trajectory_step.time_from_start - last_time
00282                 rospy.sleep( current_sleep_time )
00283                 last_time = trajectory_step.time_from_start
00284 
00285         else:
00286             #if no joint_path is given, try to go to the final_place_pose directly
00287             self.command_cartesian( goal.final_place_pose )
00288 
00289         return self.reactive_approach_result_dict["success"]
00290 
00291 
00292     def compliant_close(self, grasp_posture = None, pregrasp_posture = None):
00293         """
00294         Close compliantly. We're going to interpolate from pregrasp to grasp, sending
00295         data each iteration_time. We stop the fingers when they come in contact with something
00296         using the feedback from the tactile sensors.
00297 
00298         @grasp_posture: the posture of the hand for the grasp - joint names and positions
00299         @pregrasp_posture: the posture of the hand when doing the approach. We assume
00300                            that the grasp and pregrasp have the same joint order.
00301         @nb_steps: the number of steps used in the interpolation.
00302         @iteration_time: how long do we wait between 2 steps.
00303         """
00304         rospy.loginfo("Closing the hand compliantly")
00305         self.check_preempt()
00306         self.broadcast_phase(ManipulationPhase.CLOSING)
00307         use_slip_controller_to_close = self.using_slip_detection
00308 
00309         if grasp_posture == None:
00310             rospy.logerr("No grasp posture given, aborting")
00311             return
00312 
00313         if pregrasp_posture == None:
00314             rospy.logerr("No pregrasp posture given, aborting - FIXME")
00315             #doesn't need to abort here, could read current position from the hand and take
00316             #those as the pregrasp.
00317             return
00318 
00319         #TODO: good compliant close - someone from HANDLE?
00320         #   Counter-move the arm to keep the touching fingers in-place while closing?
00321         joint_names = grasp_posture.name
00322         grasp_targets = grasp_posture.position
00323         pregrasp_targets = pregrasp_posture.position
00324         #QUESTION: Should we make sure grasp and pregrasp have the same order?
00325 
00326         current_targets = self.close_until_force(joint_names, pregrasp_targets, grasp_targets, self.light_touch_thresholds)
00327 
00328         #now that all the fingers are touching the object, we're going to close
00329         #with a stronger grasp.
00330         rospy.sleep(1)
00331         rospy.loginfo("Now closing with stronger grasp.")
00332 
00333         current_targets = self.close_until_force(joint_names, current_targets, grasp_targets, self.grasp_touch_thresholds, "All the fingers are now grasping the object strongly.")
00334 
00335         rospy.logwarn("Waiting a little after closing the hand")
00336         rospy.sleep(1)
00337 
00338     def close_until_force(self, joint_names, pregrasp, grasp, forces_threshold, success_msg = "All the fingers are now touching the object.", nb_steps = 20, iteration_time=0.2):
00339         """
00340         Closes the hand from pregrasp to grasp
00341         until the given forces are reached.
00342 
00343         @return returns the positions reached when the forces were reached.
00344         """
00345         current_targets = []
00346         for pos in pregrasp:
00347             current_targets.append(pos)
00348 
00349         #loop on all the iteration steps. The target for a given finger
00350         # is :
00351         #      target = pregrasp + (grasp - pregrasp) * (i / TOTAL_NB_OF_STEPS)
00352         fingers_touching = [0,0,0,0,0]
00353 
00354         for i_step in range(0, nb_steps + 1):
00355             sendupdate_dict = {}
00356             fingers_touch_index = {"FF":0, "MF":1, "RF":2, "LF":3, "TH":4}
00357             for (index, joint_name, grasp_target, pregrasp_target) in zip(range(0,len(joint_names)),joint_names, grasp, pregrasp):
00358                 # only update the finger that are not touching
00359                 # takes the first 2 letters of the names (= finger name)
00360                 # and check the index from the dict defined before the for loop.
00361                 # return -1 if the finger is not in the dict
00362                 touch_index = fingers_touch_index.get(joint_name[:2], -1)
00363                 if touch_index == -1:
00364                     touching = 0
00365                 else:
00366                     touching = fingers_touching[touch_index]
00367                 if touching == 0:
00368                     joint_target = pregrasp_target + float(grasp_target - pregrasp_target)*(float(i_step) / float(nb_steps) )
00369                     myjoint_target = joint_target * 180.0 / math.pi
00370                     current_targets[index] = joint_target
00371                     sendupdate_dict[joint_name] = myjoint_target
00372                     rospy.logdebug("["+joint_name+"]: (p/g/t) = "+str(pregrasp_target)+"/"+str(grasp_target)+"/"+str(myjoint_target) + " ("+
00373                                    str(float(i_step) / float(nb_steps))+"%)")
00374 
00375             self.sr_lib.sendupdate_from_dict(sendupdate_dict)
00376             rospy.sleep(iteration_time)
00377             #check which fingers are touching
00378             which_fingers_are_touching_response = []
00379             try:
00380                 which_fingers_are_touching_response = self.which_fingers_are_touching_client(forces_threshold)
00381             except rospy.ServiceException, e:
00382                 rospy.logerr("Couldn't call the service which_fingers_are_touching")
00383 
00384             #check if all the fingers are touching
00385             fingers_touching = which_fingers_are_touching_response.touch_forces
00386             all_fingers_touching = True
00387 
00388             #we don't check the thumb as the dummy tactile sensors are not
00389             # working properly for the thumb
00390             for value in fingers_touching[:4]:
00391                 if value == 0.0:
00392                     all_fingers_touching = False
00393                     break
00394             #stop when all the fingers are touching the object
00395             if all_fingers_touching:
00396                 rospy.loginfo(success_msg)
00397                 break
00398 
00399         return current_targets
00400 
00401     def check_good_grasp(self):
00402         """
00403         Check if we have a good grasp on the object.
00404         """
00405         rospy.logerr("TODO: really check if we have a good grasp - shake the object maybe?")
00406 
00407         return True
00408 
00409     def compute_approach_dir(self, approach_pose, grasp_pose):
00410         """
00411         compute (unit) approach direction in base_link frame from an approach_pose and grasp_pose (returns scipy array)
00412         """
00413         approach_vect = self.compute_approach_vect(approach_pose, grasp_pose)
00414         approach_dir = approach_vect/scipy.dot(approach_vect, approach_vect)**.5
00415         return approach_dir
00416 
00417     def compute_approach_vect(self, approach_pose, grasp_pose):
00418         """
00419         compute approach vector in base_link frame from an approach_pose and grasp_pose (returns scipy array)
00420         """
00421         (approach_pos, approach_rot) = pose_stamped_to_lists(self.tf_listener, approach_pose, self.root_name)
00422         (grasp_pos, grasp_rot) = pose_stamped_to_lists(self.tf_listener, grasp_pose, self.root_name)
00423         approach_vect = scipy.array(grasp_pos) - scipy.array(approach_pos)
00424         return approach_vect
00425 
00426     def return_rel_pose(self, vector, frame, start_pose = None, orthogonal_to_vect = None, orthogonal_to_vect_frame = 'world'):
00427         """
00428         convert a relative vector in frame to a pose in the base_link frame
00429         if start_pose is not specified, uses current pose of the wrist
00430         if orthogonal_to_vect and orthogonal_to_vect_frame are specified, first convert vector to be orthogonal to orthogonal_to_vect
00431         """
00432 
00433         #if start_pose is not specified, use the current Cartesian pose of the wrist
00434         if start_pose == None:
00435             (start_trans, start_rot) =  pose_stamped_to_lists(self.tf_listener, self.tip_name, self.root_name)
00436         else:
00437             start_pose.header.stamp = rospy.Time(0)
00438             (start_trans, start_rot) = pose_stamped_to_lists(self.tf_listener, start_pose, self.root_name)
00439 
00440         #convert the vector in frame to base_link frame
00441         if frame != self.root_name:
00442             vector3_stamped = create_vector3_stamped(vector, frame)
00443             base_link_vector = vector3_stamped_to_list(self.tf_listener, vector3_stamped, self.root_name)
00444         else:
00445             base_link_vector = vector
00446 
00447         #if orthogonal_to_vect and orthogonal_to_vect_frame are specified, make the vector orthogonal to that vector
00448         if orthogonal_to_vect != None:
00449             #first convert it to the base_link frame if it's not already
00450             if orthogonal_to_vect_frame != self.root_name:
00451                 ortho_vector3_stamped = create_vector3_stamped(orthogonal_to_vect, orthogonal_to_vect_frame)
00452                 ortho_base_link_vector = vector3_stamped_to_list(self.tf_listener, ortho_vector3_stamped, self.root_name)
00453             else:
00454                 ortho_base_link_vector = orthogonal_to_vect
00455 
00456             #now project the desired vector onto the orthogonal vector and subtract out that component
00457             base_link_vector = self.make_orthogonal(base_link_vector, ortho_base_link_vector)
00458 
00459         #add the desired vector to the position
00460         new_trans = [x+y for (x,y) in zip(start_trans, base_link_vector)]
00461         #create a new poseStamped
00462         pose_stamped = create_pose_stamped(new_trans+start_rot)
00463 
00464         return pose_stamped
00465 
00466     def open_and_reset_fingertips(self, pregrasp_posture, reset = 0):
00467         """
00468         Open the hand
00469         """
00470         joint_names = pregrasp_posture.name
00471         joint_targets = pregrasp_posture.position
00472         sendupdate_dict = {}
00473 
00474         for (joint_name, joint_target) in zip(joint_names, joint_targets):
00475             myjoint_target=joint_target * 180 / math.pi
00476             sendupdate_dict[joint_name] = myjoint_target
00477 
00478         self.sr_lib.sendupdate_from_dict(sendupdate_dict)
00479 
00480     def command_cartesian(self, pose, frame_id='world'):
00481         """
00482         Tries to go to the given pose with the arm.
00483         """
00484         goalA = MoveArmGoal()
00485         if self.whicharm == "r":
00486             goalA.motion_plan_request.group_name = "right_arm"
00487         else:
00488             goalA.motion_plan_request.group_name = "left_arm"
00489 
00490         goalA.motion_plan_request.num_planning_attempts = 1
00491         goalA.motion_plan_request.planner_id = ""
00492         goalA.motion_plan_request.allowed_planning_time = rospy.Duration(5.0)
00493 
00494 
00495         pc = PositionConstraint()
00496         pc.header.stamp = rospy.Time.now()
00497         pc.header.frame_id = self.root_name
00498         pc.link_name = self.tip_name
00499         pc.position = pose.pose.position
00500 
00501         pc.constraint_region_shape.type = Shape.BOX
00502         pc.constraint_region_shape.dimensions = [0.02, 0.02, 0.02]
00503         pc.constraint_region_orientation.w = 1.0
00504 
00505         goalA.motion_plan_request.goal_constraints.position_constraints.append(pc)
00506 
00507         oc = OrientationConstraint()
00508         oc.header.stamp = rospy.Time.now()
00509         oc.header.frame_id = self.root_name
00510         oc.link_name = self.tip_name
00511         oc.orientation =  pose.pose.orientation
00512 
00513         oc.absolute_roll_tolerance = 0.04
00514         oc.absolute_pitch_tolerance = 0.04
00515         oc.absolute_yaw_tolerance = 0.04
00516         oc.weight = 1.
00517 
00518         goalA.motion_plan_request.goal_constraints.orientation_constraints.append(oc)
00519 
00520         self.move_arm_client.send_goal(goalA)
00521         finished_within_time = False
00522         finished_within_time = self.move_arm_client.wait_for_result()
00523 
00524         if not finished_within_time:
00525             self.move_arm_client.cancel_goal()
00526             rospy.logout('Timed out achieving goal A')
00527         else:
00528             state = self.move_arm_client.get_state()
00529             if state == GoalStatus.SUCCEEDED:
00530                 rospy.logout('Action finished with SUCCESS')
00531             else:
00532                 rospy.logout('Action failed')
00533 
00534     def check_preempt(self):
00535         """
00536         Needs to be overloaded
00537         """
00538         pass


sr_move_arm
Author(s): Ugo Cupcic
autogenerated on Fri Jan 3 2014 12:07:38