def __init__(self, arm, use_coll_detection=False): self.arm = arm self.use_coll_detection = use_coll_detection self.GRIPPER_POINT = np.array([0.23, 0.0, 0.0]) self.cm = ControllerManager(self.arm) class CollisionState: def __init__(self): self.collided = False def callback(self, msg): if msg.data: self.collided = True self.coll_state = CollisionState() if self.use_coll_detection: rospy.loginfo("Waiting for start_detection") rospy.wait_for_service(self.arm + '_joint_coll_detect/start_detection') self.start_detection = rospy.ServiceProxy(self.arm + '_joint_coll_detect/start_detection', JointDetectionStart, persistent=True) rospy.loginfo("Waiting for stop_detection") rospy.wait_for_service(self.arm + '_joint_coll_detect/stop_detection') self.stop_detection = rospy.ServiceProxy(self.arm + '_joint_coll_detect/stop_detection', std_srvs.srv.Empty, persistent=True) self.stop_detection() rospy.Subscriber(self.arm + "_joint_coll_detect/arm_collision_detected", Bool, self.coll_state.callback) rospy.loginfo("Waiting for arm_moving_wait") rospy.wait_for_service(self.arm + '_arm_moving_server/arm_moving_wait') self.arm_moving_wait = rospy.ServiceProxy(self.arm + '_arm_moving_server/arm_moving_wait', ArmMovingWait, persistent=True) rospy.loginfo("[grasp_manager] GraspBehavior loaded.")
class GraspBehavior(object): def __init__(self, arm, use_coll_detection=False): self.arm = arm self.use_coll_detection = use_coll_detection self.GRIPPER_POINT = np.array([0.23, 0.0, 0.0]) self.cm = ControllerManager(self.arm) class CollisionState: def __init__(self): self.collided = False def callback(self, msg): if msg.data: self.collided = True self.coll_state = CollisionState() if self.use_coll_detection: rospy.loginfo("Waiting for start_detection") rospy.wait_for_service(self.arm + '_joint_coll_detect/start_detection') self.start_detection = rospy.ServiceProxy(self.arm + '_joint_coll_detect/start_detection', JointDetectionStart, persistent=True) rospy.loginfo("Waiting for stop_detection") rospy.wait_for_service(self.arm + '_joint_coll_detect/stop_detection') self.stop_detection = rospy.ServiceProxy(self.arm + '_joint_coll_detect/stop_detection', std_srvs.srv.Empty, persistent=True) self.stop_detection() rospy.Subscriber(self.arm + "_joint_coll_detect/arm_collision_detected", Bool, self.coll_state.callback) rospy.loginfo("Waiting for arm_moving_wait") rospy.wait_for_service(self.arm + '_arm_moving_server/arm_moving_wait') self.arm_moving_wait = rospy.ServiceProxy(self.arm + '_arm_moving_server/arm_moving_wait', ArmMovingWait, persistent=True) rospy.loginfo("[grasp_manager] GraspBehavior loaded.") ################################################################################ # virtual functions to be implemented by specific grasp ## # Move arm to preparatory pose def grasp_preparation_move(self): rospy.logerr("UNIMPLEMENTED!") ## # Move arm to grasp setup, just before approach. def grasp_setup_move(self, params): rospy.logerr("UNIMPLEMENTED!") ## # Execute the grasping arm motion def execute_approach(self, block): rospy.logerr("UNIMPLEMENTED!") ## # Generate a random behavior parameterization for sampling # the workspace of possible grasps def random_generator(self): rospy.logerr("UNIMPLEMENTED!") ## # Attempt to slightly adjust grasp parameters to get a close configuration # which will hopefully find an IK solution. def jiggle_grasp_params(self, grasp_params): return grasp_params ################################################################################ ## # Transforms the given position by the offset position in the given quaternion # rotation frame def transform_in_frame(self, pos, quat, off_point): quatmat = np.mat(quaternion_matrix(quat)) quatmat[0:3,3] = np.mat(pos).T trans = np.matrix([off_point[0],off_point[1],off_point[2],1.]).T transpos = quatmat * trans return transpos.T.A[0,0:3] ## # Returns same gripper rotation normalized to [0, pi) range. def normalize_rot(self, gripper_rot): while gripper_rot >= np.pi: gripper_rot -= np.pi while gripper_rot < 0.: gripper_rot += np.pi return gripper_rot ## # Creates a PoseStamped in the torso_lift_link frame def create_goal_pose(self, x, y, z, gripper_pose): point = [x, y, z] point = self.transform_in_frame(point, gripper_pose, -self.GRIPPER_POINT).tolist() pose = point + gripper_pose.tolist() goal_pose = cf.create_pose_stamped(pose, "torso_lift_link") goal_pose.header.stamp = rospy.Time.now() return goal_pose ## # Returns the position of the end effector def get_end_effector_pos(self): wrist_pose = self.cm.get_current_wrist_pose_stamped("torso_lift_link") p = wrist_pose.pose.position o = wrist_pose.pose.orientation affector_pos = self.transform_in_frame([p.x, p.y, p.z], [o.x, o.y, o.z, o.w], self.GRIPPER_POINT) return affector_pos ## # Takes special care to kill the arm's movement by canceling # the action client and accounting for some edge cases. def kill_arm_movement(self): rospy.loginfo("Killing arm movement") self.cm.joint_action_client.cancel_all_goals() rospy.sleep(0.01) # Needed for lag time in the case that the collision occurs # before the action is dispatched and due to the fact that # the arm doesn't necessarily respond the first time while not self.cm.check_joint_trajectory_done(): self.cm.joint_action_client.cancel_all_goals() rospy.sleep(0.01) if rospy.is_shutdown(): self.cm.joint_action_client.cancel_all_goals() ## # Readys arm for grasp motion. def stage_grasp(self, grasp_params, open_gripper=True): rospy.loginfo("Staging grasp motion") self.grasp_setup_move(grasp_params) if open_gripper: # open gripper rospy.loginfo("Opening gripper") self.cm.command_gripper(1.00, -1.0, False) self.cm.gripper_action_client.wait_for_result(rospy.Duration(2.0)) self.cm.wait_joint_trajectory_done() ## # Performs motion where arm approaches object def grasp_approach_motion(self, coll_detect=False, behavior_name="", sig_level=0.99): # move arm down rospy.loginfo("Moving arm down") startt = rospy.Time.now().to_sec() if coll_detect: self.start_detection(behavior_name, sig_level) result = self.execute_approach(False) if result == "no solution": # break without completing the grasp #self.apm.end_collision_detection() if coll_detect: self.stop_detection() return "no solution" # grasp motion should be successful and moving now # wait until the trajectory is complete. # this loop will not exit until the arm has stopped moving while not self.cm.check_joint_trajectory_done(): if rospy.is_shutdown(): self.kill_arm_movement() return "shutdown" if self.coll_state.collided: self.kill_arm_movement() break rospy.sleep(0.005) if coll_detect: self.stop_detection() if self.coll_state.collided and coll_detect: grasp_result = "Collision" else: grasp_result = "No collision" self.coll_state.collided = False endt = rospy.Time.now().to_sec() rospy.loginfo("Grasp duration: %f", endt - startt) rospy.loginfo("Finished moving arm") return grasp_result ## # Gripper graping action def grasping_action(self): rospy.loginfo("Closing gripper") self.cm.command_gripper(0.0, 30.0, True) rospy.loginfo("Gripper closed") ## # Gripper placing action def placing_action(self): rospy.loginfo("Opening gripper") self.cm.command_gripper(1.00, -1.0, False) rospy.sleep(0.5) rospy.loginfo("Gripper opened") ## # Performs full grasping behavior. # # @param grasp_params A tuple containing the parameters for the behavior. # @param is_place If False, perform a grasp. If True, perform a place. # @param collide Whether or not we should detect for collisions. # @param data_collecting If True, only perform the approach motion and return after that. # @param num_jiggle How many times the parameters should be jiggled before giving up # @param publish_state Callback function which takes a string and publishes the current state # @return grasp_result Result of grasp. def perform_grasp(self, grasp_params, is_place=False, collide=True, behavior_name="", sig_level=0.99, data_collecting=False, num_jiggle=2, publish_state=lambda x: x): self.collide = collide rospy.loginfo("Performing grasp with parameters: " + str(grasp_params)) iters = 0 while not rospy.is_shutdown(): # Move to grasp position publish_state(GraspStates.GRASP_SETUP_MOVE) self.stage_grasp(grasp_params, not is_place) self.arm_moving_wait(True, 3.0) # Wait for the arm to stop moving publish_state(GraspStates.EXECUTE_APPROACH) approach_result = self.grasp_approach_motion(self.use_coll_detection and self.collide, behavior_name=behavior_name, sig_level=sig_level) rospy.sleep(0.1) if approach_result != "no solution": break else: if num_jiggle == 0: rospy.loginfo("Interpolated IK failed, cannot perform grasp for x = %1.2f, y = %1.2f, rot = %1.2f" % (x, y, gripper_rot)) return "IK failure" rospy.loginfo("Interpolated IK failed, jiggling configuration for nearby trajectory") # Jiggle it to see if it works grasp_params = self.jiggle_grasp_params(grasp_params) iters += 1 # Tried too many times if iters >= num_jiggle: rospy.loginfo("Cannot find IK solution") return "IK failure" else: continue if data_collecting: return "Grasp motion success" if approach_result == "Collision": if not is_place: publish_state(GraspStates.GRASP_OBJECT) self.grasping_action() else: self.placing_action() if not is_place: rospy.loginfo("Picking object up") else: rospy.loginfo("Pulling arm away") publish_state(GraspStates.EXECUTE_RETREAT) self.execute_retreat() ################################################################################ # determine return result self.arm_moving_wait(True, 3.0) # Wait for the arm to stop moving if approach_result == "Collision": if not is_place: if self.is_obj_in_gripper(): grasp_result = "Object grasped" else: grasp_result = "Object missed" else: grasp_result = "Object placed" else: if not is_place: grasp_result = "No collision for grasp" else: if self.is_obj_in_gripper(): grasp_result = "No collision for place" else: grasp_result = "Object dropped before place" ################################################################################ rospy.loginfo("Grasp result: %s" % grasp_result) rospy.loginfo("Grasp complete!") return grasp_result ## # Checks to see if anything is in the gripper currently (gripper is not fully closed). def is_obj_in_gripper(self): last_opening = self.cm.get_current_gripper_opening() while not rospy.is_shutdown(): rospy.sleep(0.2) next_opening = self.cm.get_current_gripper_opening() print next_opening if abs(last_opening - next_opening) < 0.001: break last_opening = next_opening return self.cm.get_current_gripper_opening() > 0.01