def __init__(self, which_arm = 'r', slip_detection = False): self.using_slip_detection = slip_detection self.whicharm = which_arm self.mypub = rospy.Publisher("/command",JointTrajectory) #force to use when closing hard self.close_force = 100 if self.using_slip_detection: self.close_force = 10 #has the hand been closed hard recently? self._closed_hard = 0 #collision name of support surface self._table_name = "table" #root and tip name for listening #to the current palm pose self.tf_listener = tf.TransformListener() self.root_name = "world" self.tip_name = "palm" #for taking photos of each step self._photo = 0 #broadcast the current phase of the manipulation self._phase_pub = rospy.Publisher('/reactive_manipulation_phase', ManipulationPhase) #send targets to the arm / hand self.sr_lib = ShadowHand_ROS() self.move_arm_client = None if which_arm == 'r': self.move_arm_client = SimpleActionClient( "/move_right_arm", MoveArmAction ) else: self.move_arm_client = SimpleActionClient( "/move_left_arm", MoveArmAction ) self.move_arm_client.wait_for_server() #client to the tactile sensor manager. rospy.loginfo("Waiting for service which_fingers_are_touching") rospy.wait_for_service('which_fingers_are_touching') rospy.loginfo("OK service which_fingers_are_touching found.") self.which_fingers_are_touching_client = rospy.ServiceProxy('which_fingers_are_touching', which_fingers_are_touching) #load tactile thresholds self.light_touch_thresholds = rospy.get_param('light_touch_thresholds', [100.,100.,100.,100.,0.0]) rospy.loginfo("light and grasp threashold are :") rospy.loginfo(self.light_touch_thresholds) self.grasp_touch_thresholds = rospy.get_param('grasp_touch_thresholds', [117.,117.,113.,111.,0.0]) rospy.loginfo(self.grasp_touch_thresholds) #dictionary for ManipulationPhase self.manipulation_phase_dict = {} for element in dir(ManipulationPhase): if element[0].isupper(): self.manipulation_phase_dict[eval('ManipulationPhase.'+element)] = element #dictionary of return values for reactive approach self.reactive_approach_result_dict = {"success":0, "within goal threshold":1, "both fingers touching":2, "ran out of tries":3, "touching table":4, "saw palm contact":5, "grasp infeasible":6} #dictionary of return values for reactive_grasp self.reactive_grasp_result_dict = {"success":0, "ran out of grasp tries":1, "ran out of approach tries":2, "aborted":3, "grasp infeasible":4} rospy.logdebug("done with ReactiveGrasper init for the %s arm"%self.whicharm)
class ReactiveGrasper(object): """ reactive/guarded movement and grasping """ def __init__(self, which_arm = 'r', slip_detection = False): self.using_slip_detection = slip_detection self.whicharm = which_arm self.mypub = rospy.Publisher("/command",JointTrajectory) #force to use when closing hard self.close_force = 100 if self.using_slip_detection: self.close_force = 10 #has the hand been closed hard recently? self._closed_hard = 0 #collision name of support surface self._table_name = "table" #root and tip name for listening #to the current palm pose self.tf_listener = tf.TransformListener() self.root_name = "world" self.tip_name = "palm" #for taking photos of each step self._photo = 0 #broadcast the current phase of the manipulation self._phase_pub = rospy.Publisher('/reactive_manipulation_phase', ManipulationPhase) #send targets to the arm / hand self.sr_lib = ShadowHand_ROS() self.move_arm_client = None if which_arm == 'r': self.move_arm_client = SimpleActionClient( "/move_right_arm", MoveArmAction ) else: self.move_arm_client = SimpleActionClient( "/move_left_arm", MoveArmAction ) self.move_arm_client.wait_for_server() #client to the tactile sensor manager. rospy.loginfo("Waiting for service which_fingers_are_touching") rospy.wait_for_service('which_fingers_are_touching') rospy.loginfo("OK service which_fingers_are_touching found.") self.which_fingers_are_touching_client = rospy.ServiceProxy('which_fingers_are_touching', which_fingers_are_touching) #load tactile thresholds self.light_touch_thresholds = rospy.get_param('light_touch_thresholds', [100.,100.,100.,100.,0.0]) rospy.loginfo("light and grasp threashold are :") rospy.loginfo(self.light_touch_thresholds) self.grasp_touch_thresholds = rospy.get_param('grasp_touch_thresholds', [117.,117.,113.,111.,0.0]) rospy.loginfo(self.grasp_touch_thresholds) #dictionary for ManipulationPhase self.manipulation_phase_dict = {} for element in dir(ManipulationPhase): if element[0].isupper(): self.manipulation_phase_dict[eval('ManipulationPhase.'+element)] = element #dictionary of return values for reactive approach self.reactive_approach_result_dict = {"success":0, "within goal threshold":1, "both fingers touching":2, "ran out of tries":3, "touching table":4, "saw palm contact":5, "grasp infeasible":6} #dictionary of return values for reactive_grasp self.reactive_grasp_result_dict = {"success":0, "ran out of grasp tries":1, "ran out of approach tries":2, "aborted":3, "grasp infeasible":4} rospy.logdebug("done with ReactiveGrasper init for the %s arm"%self.whicharm) def broadcast_phase(self, phase): """ broadcast the current manipulation phase (of type ManipulationPhase) """ rospy.loginfo("broadcasting reactive phase %s"%self.manipulation_phase_dict[phase]) self._phase_pub.publish(phase) def reactive_grasp(self, approach_pose, grasp_pose, joint_path = None, pregrasp_posture = None, grasp_posture = None, side_step = .015, back_step = .03, approach_num_tries = 10, goal_pos_thres = 0.01, grasp_num_tries = 4, forward_step = 0.03, object_name = "points", table_name = "table", grasp_adjust_x_step = .02, grasp_adjust_z_step = .015, grasp_adjust_num_tries = 3): self._table_name = table_name self.check_preempt() #if approach_pose is not specified, use the current pose of the hand if approach_pose == None: (current_trans,current_rot) = self.tf_listener.lookupTransform(self.root_name, self.tip_name, rospy.Time(0)) approach_pose = create_pose_stamped(current_trans+current_rot) self.check_preempt() #compute (unit) approach direction in base_link frame approach_dir = self.compute_approach_dir(approach_pose, grasp_pose) num_tries = 0 current_goal_pose = grasp_pose while 1: if approach_num_tries: #try the approach, unless the reactive approach is disabled (approach_num_tries is 0) approach_result = self.reactive_approach(approach_dir, current_goal_pose, joint_path, side_step, back_step, approach_num_tries, goal_pos_thres) #quit if the approach ran out of tries (couldn't get around the object) if approach_result == self.reactive_approach_result_dict["ran out of tries"]: return self.reactive_grasp_result_dict["ran out of approach tries"] elif approach_result == self.reactive_approach_result_dict["grasp infeasible"]: return self.reactive_grasp_result_dict["grasp infeasible"] else: #reactive approach is disabled; just move to the goal self.broadcast_phase(ManipulationPhase.MOVING_TO_GRASP) rospy.loginfo("reactive approach is disabled, moving to the goal") self.move_cartesian_step(grasp_pose, timeout = 10.0, settling_time = 5.0, blocking = 1) #go to the pregrasp posture rospy.loginfo("Going to pregrasp posture.") self.open_and_reset_fingertips(pregrasp_posture, reset = 1) #then start closing the hand compliantly rospy.loginfo("starting compliant_close") self.compliant_close(grasp_posture, pregrasp_posture) self.check_preempt() #check if the grasp is okay if self.check_good_grasp(): rospy.loginfo("Grasp successfully achieved") return self.reactive_grasp_result_dict["success"] #if the grasp is not okay num_tries += 1 if num_tries < grasp_num_tries: rospy.logwarn("Failed to grasp: trying once more") #move the goal forward along the approach direction by forward_step move_forward_vect = approach_dir*forward_step current_goal_pose = self.return_rel_pose(move_forward_vect, self.root_name, current_goal_pose) joint_path = None #print the new goal (goal_pos, goal_rot) = pose_stamped_to_lists(self.tf_listener, current_goal_pose, self.root_name) rospy.loginfo("trying approach again with new goal, pos: "+pplist(goal_pos)+" rot: "+pplist(goal_rot)) #open the gripper back up self.open_and_reset_fingertips(pregrasp_posture, reset = 1) else: rospy.loginfo("ran out of grasp tries!") return self.reactive_grasp_result_dict["ran out of grasp tries"] def reactive_approach(self, approach_dir, current_goal_pose, joint_path = None, side_step = .015, back_step = .03, num_tries = 10, goal_pos_thres = 0.01): """ Stops and goes back a little in case of a contact. """ rospy.loginfo("Executing a reactive approach") self.broadcast_phase(ManipulationPhase.MOVING_TO_GRASP) #check the position of the palm compared to the position of the object #adjust if necessary current_sleep_time = rospy.Duration(0.0) last_time = rospy.Duration(0.0) #follow the joint_path if one is given if joint_path != None: self.check_preempt() self.mypub.publish(joint_path) rospy.logerr("Sent a trajectory !") rospy.sleep(10) rospy.logerr("Trajectory should be finished !") '''joint_names = joint_path.joint_names countTime = 5 for trajectory_step in joint_path.points: sendupdate_msg = [] for (joint_name, joint_target) in zip(joint_names, trajectory_step.positions): # convert targets to degrees sendupdate_msg.append(joint(joint_name = joint_name, joint_target = joint_target * 57.325)) self.sr_arm_target_pub.publish(sendupdate(len(sendupdate_msg), sendupdate_msg) ) self.sr_hand_target_pub.publish(sendupdate(len(sendupdate_msg), sendupdate_msg) ) current_sleep_time = trajectory_step.time_from_start - last_time rospy.sleep( countTime ) # current_sleep_time ) countTime=max(countTime-1,0.5) last_time = trajectory_step.time_from_start else: #if no joint_path is given, go to the current_goal_pose self.command_cartesian( current_goal_pose )''' return self.reactive_approach_result_dict["success"] def reactive_lift(self, goal): """ Lifts the object while monitoring for contacts. """ #follow the joint_path if one is given joint_path = goal.trajectory last_time = rospy.Duration(0.0) current_sleep_time = rospy.Duration(0.0) if joint_path != None: self.check_preempt() joint_names = joint_path.joint_names for trajectory_step in joint_path.points: sendupdate_dict = {} for (joint_name, joint_target) in zip(joint_names, trajectory_step.positions): # convert targets to degrees sendupdate_dict[joint_name] = joint_target * 57.325 self.sr_lib.sendupdate_arm_from_dict(sendupdate_dict) self.sr_lib.sendupdate_from_dict(sendupdate_dict) current_sleep_time = trajectory_step.time_from_start - last_time rospy.sleep( current_sleep_time ) last_time = trajectory_step.time_from_start else: #if no joint_path is given, go to the current_goal_pose rospy.logerr("TODO: Implement this. Get current pose, add the lift and set this as the command_cartesian target.") self.command_cartesian( goal.lift ) return self.reactive_approach_result_dict["success"] def reactive_place(self, goal): """ Lifts the object while monitoring for contacts. """ #follow the joint_path if one is given joint_path = goal.trajectory last_time = rospy.Duration(0.0) current_sleep_time = rospy.Duration(0.0) if joint_path != None: self.check_preempt() joint_names = joint_path.joint_names for trajectory_step in joint_path.points: sendupdate_dict = {} for (joint_name, joint_target) in zip(joint_names, trajectory_step.positions): # convert targets to degrees sendupdate_dict[joint_name] = joint_target * 57.325 self.sr_lib.sendupdate_arm_from_dict(sendupdate_dict) self.sr_lib.sendupdate_from_dict(sendupdate_dict) current_sleep_time = trajectory_step.time_from_start - last_time rospy.sleep( current_sleep_time ) last_time = trajectory_step.time_from_start else: #if no joint_path is given, try to go to the final_place_pose directly self.command_cartesian( goal.final_place_pose ) return self.reactive_approach_result_dict["success"] def compliant_close(self, grasp_posture = None, pregrasp_posture = None): """ Close compliantly. We're going to interpolate from pregrasp to grasp, sending data each iteration_time. We stop the fingers when they come in contact with something using the feedback from the tactile sensors. @grasp_posture: the posture of the hand for the grasp - joint names and positions @pregrasp_posture: the posture of the hand when doing the approach. We assume that the grasp and pregrasp have the same joint order. @nb_steps: the number of steps used in the interpolation. @iteration_time: how long do we wait between 2 steps. """ rospy.loginfo("Closing the hand compliantly") self.check_preempt() self.broadcast_phase(ManipulationPhase.CLOSING) use_slip_controller_to_close = self.using_slip_detection if grasp_posture == None: rospy.logerr("No grasp posture given, aborting") return if pregrasp_posture == None: rospy.logerr("No pregrasp posture given, aborting - FIXME") #doesn't need to abort here, could read current position from the hand and take #those as the pregrasp. return #TODO: good compliant close - someone from HANDLE? # Counter-move the arm to keep the touching fingers in-place while closing? joint_names = grasp_posture.name grasp_targets = grasp_posture.position pregrasp_targets = pregrasp_posture.position #QUESTION: Should we make sure grasp and pregrasp have the same order? current_targets = self.close_until_force(joint_names, pregrasp_targets, grasp_targets, self.light_touch_thresholds) #now that all the fingers are touching the object, we're going to close #with a stronger grasp. rospy.sleep(1) rospy.loginfo("Now closing with stronger grasp.") 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.") rospy.logwarn("Waiting a little after closing the hand") rospy.sleep(1) 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): """ Closes the hand from pregrasp to grasp until the given forces are reached. @return returns the positions reached when the forces were reached. """ current_targets = [] for pos in pregrasp: current_targets.append(pos) #loop on all the iteration steps. The target for a given finger # is : # target = pregrasp + (grasp - pregrasp) * (i / TOTAL_NB_OF_STEPS) fingers_touching = [0,0,0,0,0] for i_step in range(0, nb_steps + 1): sendupdate_dict = {} fingers_touch_index = {"FF":0, "MF":1, "RF":2, "LF":3, "TH":4} for (index, joint_name, grasp_target, pregrasp_target) in zip(range(0,len(joint_names)),joint_names, grasp, pregrasp): # only update the finger that are not touching # takes the first 2 letters of the names (= finger name) # and check the index from the dict defined before the for loop. # return -1 if the finger is not in the dict touch_index = fingers_touch_index.get(joint_name[:2], -1) if touch_index == -1: touching = 0 else: touching = fingers_touching[touch_index] if touching == 0: joint_target = pregrasp_target + float(grasp_target - pregrasp_target)*(float(i_step) / float(nb_steps) ) myjoint_target = joint_target * 180.0 / math.pi current_targets[index] = joint_target sendupdate_dict[joint_name] = myjoint_target rospy.logdebug("["+joint_name+"]: (p/g/t) = "+str(pregrasp_target)+"/"+str(grasp_target)+"/"+str(myjoint_target) + " ("+ str(float(i_step) / float(nb_steps))+"%)") self.sr_lib.sendupdate_from_dict(sendupdate_dict) rospy.sleep(iteration_time) #check which fingers are touching which_fingers_are_touching_response = [] try: which_fingers_are_touching_response = self.which_fingers_are_touching_client(forces_threshold) except rospy.ServiceException, e: rospy.logerr("Couldn't call the service which_fingers_are_touching") #check if all the fingers are touching fingers_touching = which_fingers_are_touching_response.touch_forces all_fingers_touching = True #we don't check the thumb as the dummy tactile sensors are not # working properly for the thumb for value in fingers_touching[:4]: if value == 0.0: all_fingers_touching = False break #stop when all the fingers are touching the object if all_fingers_touching: rospy.loginfo(success_msg) break return current_targets