示例#1
0
    def attach_object_to_gripper(self, arm_name, object_id):
        '''
        Attaches an object to the robot's end effector.  

        This does NOT "snap" the object to the end effector.
        Rather, now when the robot moves, the object is assumed to remain stationary with respect to the robot's
        end effector instead of the world.  Collisions will be checked between the object and the world as the 
        object moves, but collisions between the object and the end effector will be ignored.  The link the object
        is attached to and the links with which collisions are ignored are defined by the hand description
        (see hand_description.py).

        **Args:**
        
            **arm_name (string):** The arm ('left_arm' or 'right_arm') to attach the object ot

            **object_id (string):** The ID of the object to attach
        '''
        obj = AttachedCollisionObject()
        obj.link_name = self.hands[arm_name].attach_link
        obj.object.operation.operation = obj.object.operation.ATTACH_AND_REMOVE_AS_OBJECT
        obj.object.header.stamp = rospy.Time.now()
        obj.object.header.frame_id = 'base_link'
        obj.object.id = object_id
        obj.touch_links = self.hands[arm_name].touch_links
        self._publish(obj, self._attached_object_pub)
    def attach_object_to_gripper(self, arm_name, object_id):
        '''
        Attaches an object to the robot's end effector.  

        This does NOT "snap" the object to the end effector.
        Rather, now when the robot moves, the object is assumed to remain stationary with respect to the robot's
        end effector instead of the world.  Collisions will be checked between the object and the world as the 
        object moves, but collisions between the object and the end effector will be ignored.  The link the object
        is attached to and the links with which collisions are ignored are defined by the hand description
        (see hand_description.py).

        **Args:**
        
            **arm_name (string):** The arm ('left_arm' or 'right_arm') to attach the object ot

            **object_id (string):** The ID of the object to attach
        '''
        obj = AttachedCollisionObject()
        obj.link_name = self.hands[arm_name].attach_link
        obj.object.operation.operation = obj.object.operation.ATTACH_AND_REMOVE_AS_OBJECT
        obj.object.header.stamp = rospy.Time.now()
        obj.object.header.frame_id = 'base_link'
        obj.object.id = object_id
        obj.touch_links = self.hands[arm_name].touch_links
        self._publish(obj, self._attached_object_pub)
    def detach_all_objects_from_gripper(self, whicharm='r'):

        rospy.loginfo("detaching all objects from gripper %s" % whicharm)
        obj = AttachedCollisionObject()
        obj.object.header.stamp = rospy.Time.now()
        obj.object.header.frame_id = "base_link"
        obj.link_name = whicharm + "_gripper_r_finger_tip_link"
        obj.object.operation.operation = CollisionObjectOperation.REMOVE
        self.attached_object_pub.publish(obj)
    def detach_all_objects_from_gripper(self, whicharm = 'r'):

        rospy.loginfo("detaching all objects from gripper %s"%whicharm)
        obj = AttachedCollisionObject()
        obj.object.header.stamp = rospy.Time.now()
        obj.object.header.frame_id = "base_link"
        obj.link_name = whicharm+"_gripper_r_finger_tip_link"
        obj.object.operation.operation = CollisionObjectOperation.REMOVE
        self.attached_object_pub.publish(obj)
示例#5
0
 def reset_attached_objects(self):
     '''
     Removes all collision objects that are currently attached to the robot.
     '''
     #and all attached objects
     reset_attached_objects = AttachedCollisionObject()
     reset_attached_objects.link_name = 'all'
     reset_attached_objects.object.header.frame_id = 'base_link'
     reset_attached_objects.object.header.stamp = rospy.Time.now()
     reset_attached_objects.object = self._get_reset_object()
     self._publish(reset_attached_objects, self._attached_object_pub)
     rospy.logdebug('Attached object reset')
    def attach_object_to_gripper(self, object_name, whicharm = 'r'):

        obj = AttachedCollisionObject()
        
        obj.link_name = whicharm+"_gripper_r_finger_tip_link"
        obj.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT
        obj.object.header.stamp = rospy.Time.now()
        obj.object.header.frame_id = "base_link"
        obj.object.id = object_name
        touch_links = ["_gripper_palm_link", "_gripper_r_finger_tip_link", "_gripper_l_finger_tip_link", "_gripper_l_finger_link", "_gripper_r_finger_link"]
        obj.touch_links = [whicharm+link for link in touch_links]
        self.attached_object_pub.publish(obj)
 def reset_attached_objects(self):
     '''
     Removes all collision objects that are currently attached to the robot.
     '''
     #and all attached objects
     reset_attached_objects = AttachedCollisionObject()
     reset_attached_objects.link_name = 'all'
     reset_attached_objects.object.header.frame_id = 'base_link'
     reset_attached_objects.object.header.stamp = rospy.Time.now()
     reset_attached_objects.object = self._get_reset_object()
     self._publish(reset_attached_objects, self._attached_object_pub)
     rospy.logdebug('Attached object reset')
    def detach_object(self, arm_name, object_id):
        '''
        Detaches a single object from the arm and removes it from the collision space entirely.  

        The object must have been attached in the world or planning scene diff previously.  This removes the object 
        from the planning scene even if it currently exists in the world.

        **Args:**

            **arm_name (string):** The arm ('left_arm' or 'right_arm') from which to detach the object
            
            **object_id (string):** The ID of the object to detach

        **Returns:**
            False if the object was not previously attached in the world or the diff and therefore cannot be 
            detached; True otherwise.
        '''
        obj = AttachedCollisionObject()
        obj.link_name = self.hands[arm_name].attach_link
        #did the object ever exist in the world?
        #if so, we need to continually remove it after detaching
        #but it will always be in 
        #corresponding collision object if it exists
        
        for co in self.current_diff.planning_scene_diff.collision_objects:
           if co.id == object_id:
               #if the planning scene originally added this object, we could
               #just remove it from the list of collision objects but we have
               #no way of knowing that at this point
               co.operation.operation = co.operation.REMOVE
               for p in range(len(co.poses)):
                   co.poses[p] = self.transform_pose(self.world_frame, co.header.frame_id, co.poses[p])
               co.header.frame_id = self.world_frame
               break
        for ao in self.current_diff.planning_scene_diff.attached_collision_objects:
            if ao.object.id == object_id:
                rospy.loginfo('Object was attached by the planning scene interface')
                self.current_diff.planning_scene_diff.attached_collision_objects.remove(ao)
                self.send_diff()
                return True
        rospy.loginfo('Object was not attached by the planning scene interface')
        aos = self.attached_collision_objects()
        for ao in aos:
            if ao.object.id == object_id:
                obj.object = ao.object
                obj.object.operation.operation = obj.object.operation.REMOVE
                self.current_diff.planning_scene_diff.attached_collision_objects.append(obj)
                self.send_diff()
                return True
            
        rospy.logwarn('Object '+object_id+' not attached to gripper')
        return False
    def detach_object(self, arm_name, object_id):
        '''
        Detaches a single object from the arm and removes it from the collision space entirely.  

        The object must have been attached in the world or planning scene diff previously.  This removes the object 
        from the planning scene even if it currently exists in the world.

        **Args:**

            **arm_name (string):** The arm ('left_arm' or 'right_arm') from which to detach the object
            
            **object_id (string):** The ID of the object to detach

        **Returns:**
            False if the object was not previously attached in the world or the diff and therefore cannot be 
            detached; True otherwise.
        '''
        obj = AttachedCollisionObject()
        obj.link_name = self.hands[arm_name].attach_link
        #did the object ever exist in the world?
        #if so, we need to continually remove it after detaching
        #but it will always be in 
        #corresponding collision object if it exists
        
        for co in self.current_diff.planning_scene_diff.collision_objects:
           if co.id == object_id:
               #if the planning scene originally added this object, we could
               #just remove it from the list of collision objects but we have
               #no way of knowing that at this point
               co.operation.operation = co.operation.REMOVE
               for p in range(len(co.poses)):
                   co.poses[p] = self.transform_pose(self.world_frame, co.header.frame_id, co.poses[p])
               co.header.frame_id = self.world_frame
               break
        for ao in self.current_diff.planning_scene_diff.attached_collision_objects:
            if ao.object.id == object_id:
                rospy.loginfo('Object was attached by the planning scene interface')
                self.current_diff.planning_scene_diff.attached_collision_objects.remove(ao)
                self.send_diff()
                return True
        rospy.loginfo('Object was not attached by the planning scene interface')
        aos = self.attached_collision_objects()
        for ao in aos:
            if ao.object.id == object_id:
                obj.object = ao.object
                obj.object.operation.operation = obj.object.operation.REMOVE
                self.current_diff.planning_scene_diff.attached_collision_objects.append(obj)
                self.send_diff()
                return True
            
        rospy.logwarn('Object '+object_id+' not attached to gripper')
        return False
    def detach_all_objects_from_gripper(self, arm_name):
        '''
        Detaches all objects and removes them from the collision space entirely.

        **Args:**

            **arm_name (string):** The arm ('left_arm' or 'right_arm') from which to detach all objects.
        '''
        obj = AttachedCollisionObject()
        obj.object.header.stamp = rospy.Time.now()
        obj.object.header.frame_id = 'base_link'
        obj.link_name = self.hands[arm_name].attach_link
        obj.object.id = 'all'
        obj.object.operation.operation = obj.object.operation.REMOVE
        self._publish(obj, self._attached_object_pub)
示例#11
0
    def detach_all_objects_from_gripper(self, arm_name):
        '''
        Detaches all objects and removes them from the collision space entirely.

        **Args:**

            **arm_name (string):** The arm ('left_arm' or 'right_arm') from which to detach all objects.
        '''
        obj = AttachedCollisionObject()
        obj.object.header.stamp = rospy.Time.now()
        obj.object.header.frame_id = 'base_link'
        obj.link_name = self.hands[arm_name].attach_link
        obj.object.id = 'all'
        obj.object.operation.operation = obj.object.operation.REMOVE
        self._publish(obj, self._attached_object_pub)
    def detach_and_add_back_objects_attached_to_gripper(self, whicharm = 'r', object_collision_name = None):

        rospy.loginfo("detaching all objects from gripper %s and adding them back to the collision map"%whicharm)

        if object_collision_name == None:
            rospy.loginfo("need to specify the object name!  Detaching and not adding back object.")
            self.detach_all_objects_from_gripper(whicharm)
            return

        obj = AttachedCollisionObject()
        obj.object.header.stamp = rospy.Time.now()
        obj.object.header.frame_id = "base_link"
        obj.link_name = whicharm+"_gripper_r_finger_tip_link"
        obj.object.id = object_collision_name
        obj.object.operation.operation = CollisionObjectOperation.DETACH_AND_ADD_AS_OBJECT
        self.attached_object_pub.publish(obj)
    def attach_object_to_gripper(self, object_name, whicharm='r'):

        obj = AttachedCollisionObject()

        obj.link_name = whicharm + "_gripper_r_finger_tip_link"
        obj.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT
        obj.object.header.stamp = rospy.Time.now()
        obj.object.header.frame_id = "base_link"
        obj.object.id = object_name
        touch_links = [
            "_gripper_palm_link", "_gripper_r_finger_tip_link",
            "_gripper_l_finger_tip_link", "_gripper_l_finger_link",
            "_gripper_r_finger_link"
        ]
        obj.touch_links = [whicharm + link for link in touch_links]
        self.attached_object_pub.publish(obj)
    def drop_object(self, goal):
        if self.action_server.is_preempt_requested():
            rospy.loginfo('%s: preempted' % ACTION_NAME)
            self.action_server.set_preempted()

        # check that we have something in hand before dropping it
        grasp_status = self.get_grasp_status_srv()

        # if the object is still in hand after lift is done we are good
        if not grasp_status.is_hand_occupied:
            rospy.logerr('%s: gripper empty, nothing to drop' % ACTION_NAME)
            self.action_server.set_aborted()
            return

        # record sound padded by 0.5 seconds from start and 1.5 seconds from the end
        self.start_audio_recording_srv(InfomaxAction.DROP, goal.category_id)
        rospy.sleep(0.5)
        self.open_gripper()
        rospy.sleep(1.5)

        # check if gripper actually opened first
        sound_msg = None
        grasp_status = self.get_grasp_status_srv()

        # if there something in the gripper - drop failed
        if grasp_status.is_hand_occupied:
            self.stop_audio_recording_srv(False)
        else:
            sound_msg = self.stop_audio_recording_srv(True)

        # delete the object that we just dropped, we don't really know where it will land
        obj = AttachedCollisionObject()
        obj.object.header.stamp = rospy.Time.now()
        obj.object.header.frame_id = GRIPPER_LINK_FRAME
        obj.object.operation.operation = CollisionObjectOperation.REMOVE
        obj.object.id = goal.collision_object_name
        obj.link_name = GRIPPER_LINK_FRAME
        obj.touch_links = GRIPPER_LINKS

        self.attached_object_pub.publish(obj)

        if sound_msg:
            self.action_server.set_succeeded(
                DropObjectResult(sound_msg.recorded_sound))
        else:
            self.action_server.set_aborted()
    def detach_object(self, arm_name, object_id):
        '''
        Detaches a single object from the arm and removes it from the collision space entirely.

        **Args:**

            **arm_name (string):** The arm ('left_arm' or 'right_arm') from which to detach the object

            **object_id (string):** The ID of the object to detach
        '''
        obj = AttachedCollisionObject()
        obj.object.header.stamp = rospy.Time.now()
        obj.object.header.frame_id = 'base_link'
        obj.link_name = self.hands[arm_name].attach_link
        obj.object.id = object_id
        obj.object.operation.operation = obj.object.operation.REMOVE
        self._publish(obj, self._attached_object_pub)
示例#16
0
    def detach_object(self, arm_name, object_id):
        '''
        Detaches a single object from the arm and removes it from the collision space entirely.

        **Args:**

            **arm_name (string):** The arm ('left_arm' or 'right_arm') from which to detach the object

            **object_id (string):** The ID of the object to detach
        '''
        obj = AttachedCollisionObject()
        obj.object.header.stamp = rospy.Time.now()
        obj.object.header.frame_id = 'base_link'
        obj.link_name = self.hands[arm_name].attach_link
        obj.object.id = object_id
        obj.object.operation.operation = obj.object.operation.REMOVE
        self._publish(obj, self._attached_object_pub)
 def drop_object(self, goal):
     if self.action_server.is_preempt_requested():
         rospy.loginfo('%s: preempted' % ACTION_NAME)
         self.action_server.set_preempted()
         
     # check that we have something in hand before dropping it
     grasp_status = self.get_grasp_status_srv()
     
     # if the object is still in hand after lift is done we are good
     if not grasp_status.is_hand_occupied:
         rospy.logerr('%s: gripper empty, nothing to drop' % ACTION_NAME)
         self.action_server.set_aborted()
         return
         
     # record sound padded by 0.5 seconds from start and 1.5 seconds from the end
     self.start_audio_recording_srv(InfomaxAction.DROP, goal.category_id)
     rospy.sleep(0.5)
     self.open_gripper()
     rospy.sleep(1.5)
     
     # check if gripper actually opened first
     sound_msg = None
     grasp_status = self.get_grasp_status_srv()
     
     # if there something in the gripper - drop failed
     if grasp_status.is_hand_occupied:
         self.stop_audio_recording_srv(False)
     else:
         sound_msg = self.stop_audio_recording_srv(True)
         
     # delete the object that we just dropped, we don't really know where it will land
     obj = AttachedCollisionObject()
     obj.object.header.stamp = rospy.Time.now()
     obj.object.header.frame_id = GRIPPER_LINK_FRAME
     obj.object.operation.operation = CollisionObjectOperation.REMOVE
     obj.object.id = goal.collision_object_name
     obj.link_name = GRIPPER_LINK_FRAME
     obj.touch_links = GRIPPER_LINKS
     
     self.attached_object_pub.publish(obj)
     
     if sound_msg:
         self.action_server.set_succeeded(DropObjectResult(sound_msg.recorded_sound))
     else:
         self.action_server.set_aborted()
    def detach_and_add_back_attached_object(self, arm_name, object_id):
        '''
        Detaches a single object from the gripper and adds it back to the world at its current location.  

        From here on, it is assumed that the object remains stationary in the world.

        **Args:**

            **arm_name (string):** The arm ('right_arm' or 'left_arm') from which to detach the object

            **object_id (string):** The ID of the object to detach
        '''
        obj = AttachedCollisionObject()
        obj.object.header.stamp = rospy.Time.now()
        obj.object.header.frame_id = 'base_link'
        obj.link_name = self.hands[arm_name].attach_link
        obj.object.id = object_id
        obj.object.operation.operation = obj.object.operation.DETACH_AND_ADD_AS_OBJECT
        self._publish(obj, self._attached_object_pub)
示例#19
0
    def detach_and_add_back_attached_object(self, arm_name, object_id):
        '''
        Detaches a single object from the gripper and adds it back to the world at its current location.  

        From here on, it is assumed that the object remains stationary in the world.

        **Args:**

            **arm_name (string):** The arm ('right_arm' or 'left_arm') from which to detach the object

            **object_id (string):** The ID of the object to detach
        '''
        obj = AttachedCollisionObject()
        obj.object.header.stamp = rospy.Time.now()
        obj.object.header.frame_id = 'base_link'
        obj.link_name = self.hands[arm_name].attach_link
        obj.object.id = object_id
        obj.object.operation.operation = obj.object.operation.DETACH_AND_ADD_AS_OBJECT
        self._publish(obj, self._attached_object_pub)
    def detach_and_add_back_objects_attached_to_gripper(
            self, whicharm='r', object_collision_name=None):

        rospy.loginfo(
            "detaching all objects from gripper %s and adding them back to the collision map"
            % whicharm)

        if object_collision_name == None:
            rospy.loginfo(
                "need to specify the object name!  Detaching and not adding back object."
            )
            self.detach_all_objects_from_gripper(whicharm)
            return

        obj = AttachedCollisionObject()
        obj.object.header.stamp = rospy.Time.now()
        obj.object.header.frame_id = "base_link"
        obj.link_name = whicharm + "_gripper_r_finger_tip_link"
        obj.object.id = object_collision_name
        obj.object.operation.operation = CollisionObjectOperation.DETACH_AND_ADD_AS_OBJECT
        self.attached_object_pub.publish(obj)
def get_virtual_gloves():
    r_glove = AttachedCollisionObject()
    r_glove.object.header.stamp = rospy.get_rostime()
    r_glove.object.header.frame_id = '/r_gripper_palm_link'
    r_glove.link_name = 'r_gripper_palm_link'

    r_glove.object.id = 'r_gripper_glove'
    r_glove.object.operation.operation = CollisionObjectOperation.ADD
    
    glove_shape = Shape()
    glove_shape.type = Shape.BOX
    glove_shape.dimensions = [ 0.25, 0.18, 0.1 ]
    glove_pose = Pose()
    glove_pose.orientation.w = 1
    glove_pose.position.x = 0.1
    
    # Pose will be zero

    r_glove.touch_links = ['r_end_effector',
                           'r_wrist_roll_link',
                           'r_wrist_flex_link',
                           'r_forearm_link']

    r_glove.object.shapes.append(glove_shape)
    r_glove.object.poses.append(glove_pose)

    l_glove = copy.deepcopy(r_glove)
    l_glove.object.id = 'l_gripper_glove'
    l_glove.object.header.frame_id = '/l_gripper_palm_link'
    l_glove.link_name = 'l_gripper_palm_link'
    l_glove.touch_links = ['l_end_effector',
                           'l_wrist_roll_link',
                           'l_wrist_flex_link',
                           'l_forearm_link']

    return r_glove, l_glove
    def reset_collision_map(self):

        #remove all objects
        reset_object = CollisionObject()
        reset_object.operation.operation = CollisionObjectOperation.REMOVE
        reset_object.header.frame_id = "base_link"
        reset_object.header.stamp = rospy.Time.now()
        reset_object.id = "all"
        self.object_in_map_pub.publish(reset_object)

        #and all attached objects
        reset_attached_objects = AttachedCollisionObject()
        reset_attached_objects.link_name = "all"
        reset_attached_objects.object.header.frame_id = "base_link"
        reset_attached_objects.object.header.stamp = rospy.Time.now()
        reset_attached_objects.object = reset_object
        self.attached_object_pub.publish(reset_attached_objects)

        #and clear the octomap
        self.clear_octomap()

        rospy.loginfo("collision objects reset")
        self.object_in_map_current_id = 0.
        return 1
    def reset_collision_map(self):

        #remove all objects
        reset_object = CollisionObject()
        reset_object.operation.operation = CollisionObjectOperation.REMOVE
        reset_object.header.frame_id = "base_link"
        reset_object.header.stamp = rospy.Time.now()
        reset_object.id = "all"
        self.object_in_map_pub.publish(reset_object)

        #and all attached objects
        reset_attached_objects = AttachedCollisionObject()
        reset_attached_objects.link_name = "all"
        reset_attached_objects.object.header.frame_id = "base_link"
        reset_attached_objects.object.header.stamp = rospy.Time.now()
        reset_attached_objects.object = reset_object
        self.attached_object_pub.publish(reset_attached_objects)

        #and clear the octomap
        self.clear_octomap()

        rospy.loginfo("collision objects reset")
        self.object_in_map_current_id = 0.
        return 1
示例#24
0
 rospy.sleep(1)
 
 pg = GraspHandPostureExecutionGoal()
 pg.goal = GraspHandPostureExecutionGoal.GRASP
 
 posture_controller.send_goal(pg)
 posture_controller.wait_for_result()
 
 rospy.sleep(1)
 
 obj = AttachedCollisionObject()
 obj.object.header.stamp = rospy.Time.now()
 obj.object.header.frame_id = 'L7_wrist_roll_link'#'base_link'
 obj.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT
 obj.object.id = coll_map_res.collision_object_names[0]
 obj.link_name = 'L7_wrist_roll_link'
 obj.touch_links = ['L7_wrist_roll_link', 'L8_left_finger_link', 'L9_right_finger_link']
 
 attached_object_pub.publish(obj)
 
 rospy.sleep(1)
 
 ordered_collision_operations = OrderedCollisionOperations()
 collision_operation = CollisionOperation()
 collision_operation.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS
 collision_operation.object2 = coll_map_res.collision_support_surface_name
 collision_operation.operation = CollisionOperation.DISABLE
 ordered_collision_operations.collision_operations = [collision_operation]
 
 if not move_arm_to_grasping_joint_pose(joint_names, joint_positions, allowed_contacts, gripper_paddings, ordered_collision_operations):
     exit(1)
示例#25
0
    def place(self,place_goal):

        placeresult = PlaceResult()
        target_pose_to_execute_ = PoseStamped()
        #for location, check path from approach pose to release pose first and then check motion to approach pose
        motion_plan_res=GetMotionPlanResponse()
        object_to_attach = place_goal.collision_object_name
        # get current hand pose
        self.listener.waitForTransform('/world', '/palm', rospy.Time(), rospy.Duration(1.0))
        try:
            (trans,rot) = self.listener.lookupTransform('/world', '/palm', rospy.Time())
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            rospy.logerr("Cannot get current palm pose")
            placeresult.manipulation_result.value = ManipulationResult.ERROR
            return placeresult

        current_pose_= PoseStamped()
        current_pose_.header.frame_id = "/world"
        current_pose_.pose.position = Point(trans[0],trans[1],trans[2])
        current_pose_.pose.orientation = Quaternion(rot[0],rot[1],rot[2],rot[3])

        # for each place location
        for index, target_pose_ in enumerate(place_goal.place_locations):

            #compute straight trajectory to approach distance
            target_approach_pose_= PoseStamped()
            target_approach_pose_.header.frame_id = "/world"
            target_approach_pose_.pose.position = Point(target_pose_.pose.position.x,target_pose_.pose.position.y,target_pose_.pose.position.z+place_goal.approach.desired_distance)
            target_approach_pose_.pose.orientation = Quaternion(target_pose_.pose.orientation.x,target_pose_.pose.orientation.y,target_pose_.pose.orientation.z,target_pose_.pose.orientation.w) #keep same orientation for the first test

            # for distance from 0 (release_pose) to desired_approach distance (approach_pose) test IK/Collision and save result
            # decompose this in X steps depending on distance to do and max speed
            interpolated_motion_plan_res = self.plan.get_interpolated_ik_motion_plan(target_approach_pose_, target_pose_, False)

            # check the result (depending on number of steps etc...)
            if (interpolated_motion_plan_res.error_code.val == interpolated_motion_plan_res.error_code.SUCCESS):
                number_of_interpolated_steps=0
                # check if one approach trajectory is feasible
                for interpolation_index, traj_error_code in enumerate(interpolated_motion_plan_res.trajectory_error_codes):
                    if traj_error_code.val!=1:
                        rospy.logerr("One unfeasible approach-phase step found at "+str(interpolation_index)+ "with val " + str(traj_error_code.val))
                        break
                    else:
                        number_of_interpolated_steps=interpolation_index

                # if trajectory is feasible then plan reach motion to approach pose
                if number_of_interpolated_steps+1==len(interpolated_motion_plan_res.trajectory.joint_trajectory.points):
                    rospy.loginfo("Place pose number "+str(index)+" approach is possible, checking motion plan to approach pose")
                    #print interpolated_motion_plan_res

                    # check and plan motion to this approach pose
                    motion_plan_res = self.plan.plan_arm_motion( place_goal.arm_name, "jointspace" ,target_approach_pose_)#,object_to_attach)

                    #if this approach pose is successful do not test others
                    if (motion_plan_res.error_code.val == motion_plan_res.error_code.SUCCESS):
                        rospy.loginfo("Place pose number "+str(index)+" is possible, executing it")
                        # copy the pose to execute for the following steps
                        target_pose_to_execute_ = copy.deepcopy(target_pose_)
                        break
                else:
                    rospy.logerr("Place pose number "+str(index)+" approach is impossible")
                    #print interpolated_motion_plan_res
            else:
                rospy.logerr("Place pose number "+str(index)+" approach is impossible")
                #print interpolated_motion_plan_res

        # execution part
        if (motion_plan_res.error_code.val == motion_plan_res.error_code.SUCCESS):
            #go there
            # filter the trajectory
            filtered_traj = self.filter_traj_(motion_plan_res)

            self.display_traj_( filtered_traj )

            # reach approach pose
            if self.send_traj_( filtered_traj )<0:
                #QMessageBox.warning(self, "Warning",
                #    "Reach trajectory execution failed: ")
                placeresult.manipulation_result.value = ManipulationResult.FAILED
                return placeresult
            time.sleep(self.simdelay) # TODO use actionlib here

            # approach
            if self.send_traj_( interpolated_motion_plan_res.trajectory.joint_trajectory )<0:
                #QMessageBox.warning(self, "Warning",
                #    "Approach trajectory execution failed: ")
                placeresult.manipulation_result.value = ManipulationResult.FAILED
                return placeresult
            time.sleep(self.simdelay) # TODO use actionlib here

            #put hand in pre-grasp posture (to gently release)
            if self.pre_grasp_exec(place_goal.grasp)<0:
                #QMessageBox.warning(self, "Warning",
                #    "Release action failed: ")
                placeresult.manipulation_result.value = ManipulationResult.FAILED
                return placeresult
            time.sleep(self.simdelay) # TODO use actionlib here
            #detach the object from the hand
            rospy.loginfo("Now we detach the attached object")
            att_object = AttachedCollisionObject()
            att_object.link_name = "palm"
            att_object.object.id = object_to_attach
            att_object.object.operation.operation = CollisionObjectOperation.DETACH_AND_ADD_AS_OBJECT
            att_object.object.header.frame_id = "palm"
            att_object.object.header.stamp = rospy.Time.now()
            object = Shape()
            object.type = Shape.CYLINDER
            object.dimensions.append(.03)
            object.dimensions.append(0.1)
            pose = Pose()
            pose.position.x = 0.0
            pose.position.y = -0.06
            pose.position.z = 0.06
            pose.orientation.x = 0
            pose.orientation.y = 0
            pose.orientation.z = 0
            pose.orientation.w = 1
            att_object.object.shapes.append(object)
            att_object.object.poses.append(pose);
            self.att_object_in_map_pub_.publish(att_object)
            rospy.loginfo("Attached object to be detached published")


        else:
            rospy.logerr("None of the place pose tested is possible")
            placeresult.manipulation_result.value = ManipulationResult.UNFEASIBLE
            return placeresult
        placeresult.manipulation_result.value = ManipulationResult.SUCCESS
        placeresult.place_location= target_pose_to_execute_
        return placeresult
    def setUp(self):

        self.tf = TransformListener()

        self.move_arm_action_client = actionlib.SimpleActionClient(
            "move_right_arm", MoveArmAction)

        att_pub = rospy.Publisher('attached_collision_object',
                                  AttachedCollisionObject)
        obj_pub = rospy.Publisher('collision_object', CollisionObject)

        rospy.init_node('test_motion_execution_buffer')

        #let everything settle down
        rospy.sleep(5.)

        obj1 = CollisionObject()

        obj1.header.stamp = rospy.Time.now() - rospy.Duration(.1)
        obj1.header.frame_id = "base_footprint"
        obj1.id = "obj2"
        obj1.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
        obj1.shapes = [Shape() for _ in range(3)]
        obj1.poses = [Pose() for _ in range(3)]

        obj1.shapes[0].type = Shape.BOX
        obj1.shapes[0].dimensions = [float() for _ in range(3)]
        obj1.shapes[0].dimensions[0] = .5
        obj1.shapes[0].dimensions[1] = 1.0
        obj1.shapes[0].dimensions[2] = .2
        obj1.poses[0].position.x = .95
        obj1.poses[0].position.y = -.25
        obj1.poses[0].position.z = .62
        obj1.poses[0].orientation.x = 0
        obj1.poses[0].orientation.y = 0
        obj1.poses[0].orientation.z = 0
        obj1.poses[0].orientation.w = 1

        obj1.shapes[2].type = Shape.BOX
        obj1.shapes[2].dimensions = [float() for _ in range(3)]
        obj1.shapes[2].dimensions[0] = .5
        obj1.shapes[2].dimensions[1] = .1
        obj1.shapes[2].dimensions[2] = 1.0
        obj1.poses[2].position.x = .95
        obj1.poses[2].position.y = -.14
        obj1.poses[2].position.z = 1.2
        obj1.poses[2].orientation.x = 0
        obj1.poses[2].orientation.y = 0
        obj1.poses[2].orientation.z = 0
        obj1.poses[2].orientation.w = 1

        obj1.shapes[1].type = Shape.BOX
        obj1.shapes[1].dimensions = [float() for _ in range(3)]
        obj1.shapes[1].dimensions[0] = .5
        obj1.shapes[1].dimensions[1] = .1
        obj1.shapes[1].dimensions[2] = 1.0
        obj1.poses[1].position.x = .95
        obj1.poses[1].position.y = .12
        obj1.poses[1].position.z = 1.2
        obj1.poses[1].orientation.x = 0
        obj1.poses[1].orientation.y = 0
        obj1.poses[1].orientation.z = 0
        obj1.poses[1].orientation.w = 1

        obj_pub.publish(obj1)

        att_object = AttachedCollisionObject()
        att_object.object.header.stamp = rospy.Time.now()
        att_object.object.header.frame_id = "r_gripper_r_finger_tip_link"
        att_object.link_name = "r_gripper_r_finger_tip_link"
        att_object.object.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
        att_object.object = CollisionObject()

        att_object.object.header.stamp = rospy.Time.now()
        att_object.object.header.frame_id = "r_gripper_r_finger_tip_link"
        att_object.object.id = "pole"
        att_object.object.shapes = [Shape() for _ in range(1)]
        att_object.object.shapes[0].type = Shape.CYLINDER
        att_object.object.shapes[0].dimensions = [float() for _ in range(2)]
        att_object.object.shapes[0].dimensions[0] = .02
        att_object.object.shapes[0].dimensions[1] = .1
        att_object.object.poses = [Pose() for _ in range(1)]
        att_object.object.poses[0].position.x = -.02
        att_object.object.poses[0].position.y = .04
        att_object.object.poses[0].position.z = 0
        att_object.object.poses[0].orientation.x = 0
        att_object.object.poses[0].orientation.y = 0
        att_object.object.poses[0].orientation.z = 0
        att_object.object.poses[0].orientation.w = 1

        att_pub.publish(att_object)

        rospy.sleep(5.0)
示例#27
0
    def pick(self,pickup_goal):
        #prepare result
        pickresult = PickupResult()

        #get grasps for the object
        # fill up a grasp planning request
        grasp_planning_req = GraspPlanningRequest()
        grasp_planning_req.arm_name = pickup_goal.arm_name
        grasp_planning_req.target = pickup_goal.target
        object_to_attach = pickup_goal.collision_object_name
        # call grasp planning service
        grasp_planning_res = self.grasp_planning_service_.call(grasp_planning_req)
        #print grasp_planning_res
        # process grasp planning result
        if (grasp_planning_res.error_code.value != grasp_planning_res.error_code.SUCCESS):
            rospy.logerr("No grasp found for this object, we will generate some, but only when the node is ready for that !")
            pickresult.manipulation_result.value = ManipulationResult.UNFEASIBLE
            return pickresult
        else:
            rospy.loginfo("Got "+ str(len(grasp_planning_res.grasps)) +" grasps for this object")

        # for each grasp, generate rotational symmetric grasps around the object (this is already in the DB for the CokeCan but should be removed and done online)

        #for each grasp, check path from pre-grasp pose to grasp pose first and then check motion to pre-grasp pose
        motion_plan_res=GetMotionPlanResponse()
        grasp_to_execute_=Grasp()
        for index, grasp in enumerate(grasp_planning_res.grasps):
            # extract grasp_pose
            grasp_pose_ = PoseStamped()
            grasp_pose_.header.frame_id = "/world";
            grasp_pose_.pose = grasp.grasp_pose

            grasp_pose_.pose.position.y=grasp_pose_.pose.position.y-0.01 #-0.01 in sim, +0.03 in real #cheating here

            # copy the grasp_pose as a pre-grasp_pose
            pre_grasp_pose_ = copy.deepcopy(grasp_pose_)

            # add desired_approach_distance along the approach vector. above the object to plan pre-grasp pose

            # currently add this to Z because approach vector needs to be computed somehow first (TODO)
            pre_grasp_pose_.pose.position.z = pre_grasp_pose_.pose.position.z + 0.05

            # for distance from 0 (grasp_pose) to desired_approach distance (pre_grasp_pose) test IK/Collision and save result
            # decompose this in X steps depending on distance to do and max speed
            interpolated_motion_plan_res = self.plan.get_interpolated_ik_motion_plan(pre_grasp_pose_, grasp_pose_, False)

            # check the result (depending on number of steps etc...)
            if (interpolated_motion_plan_res.error_code.val == interpolated_motion_plan_res.error_code.SUCCESS):
                number_of_interpolated_steps=0
                # check if one approach trajectory is feasible
                for interpolation_index, traj_error_code in enumerate(interpolated_motion_plan_res.trajectory_error_codes):
                    if traj_error_code.val!=1:
                        rospy.logerr("One unfeasible approach-phase step found at "+str(interpolation_index)+ "with val " + str(traj_error_code.val))
                        break
                    else:
                        number_of_interpolated_steps=interpolation_index

                # if trajectory is feasible then plan reach motion to pre-grasp pose
                if number_of_interpolated_steps+1==len(interpolated_motion_plan_res.trajectory.joint_trajectory.points):
                    rospy.loginfo("Grasp number "+str(index)+" approach is possible, checking motion plan to pre-grasp")
                    #print interpolated_motion_plan_res

                    # check and plan motion to this pre_grasp_pose
                    motion_plan_res = self.plan.plan_arm_motion( pickup_goal.arm_name, "jointspace", pre_grasp_pose_ )

                    #if this pre-grasp pose is successful do not test others
                    if (motion_plan_res.error_code.val == motion_plan_res.error_code.SUCCESS):
                        rospy.loginfo("Grasp number "+str(index)+" is possible, executing it")
                        # copy the grasp to execute for the following steps
                        grasp_to_execute_ = copy.deepcopy(grasp)
                        break
                else:
                    rospy.logerr("Grasp number "+str(index)+" approach is impossible")
                    #print interpolated_motion_plan_res
            else:
                rospy.logerr("Grasp number "+str(index)+" approach is impossible")
                #print interpolated_motion_plan_res
        # execution part
        if (motion_plan_res.error_code.val == motion_plan_res.error_code.SUCCESS):
            #put hand in pre-grasp posture
            if self.pre_grasp_exec(grasp_to_execute_)<0:
                #QMessageBox.warning(self, "Warning",
                #    "Pre-grasp action failed: ")
                pickresult.manipulation_result.value = ManipulationResult.FAILED
                return pickresult

            #go there
            # filter the trajectory
            filtered_traj = self.filter_traj_(motion_plan_res)

            self.display_traj_( filtered_traj )

            # reach pregrasp pose
            if self.send_traj_( filtered_traj )<0:
                #QMessageBox.warning(self, "Warning",
                #    "Reach trajectory execution failed: ")
                pickresult.manipulation_result.value = ManipulationResult.FAILED
                return pickresult
            #time.sleep(20) # TODO use actionlib here
            time.sleep(self.simdelay) # TODO use actionlib here

            # approach
            if self.send_traj_( interpolated_motion_plan_res.trajectory.joint_trajectory )<0:
                #QMessageBox.warning(self, "Warning",
                #    "Approach trajectory execution failed: ")
                pickresult.manipulation_result.value = ManipulationResult.FAILED
                return pickresult
            time.sleep(self.simdelay) # TODO use actionlib here

            #grasp
            if self.grasp_exec(grasp_to_execute_)<0:
                #QMessageBox.warning(self, "Warning",
                #    "Grasp action failed: ")
                pickresult.manipulation_result.value = ManipulationResult.FAILED
                return pickresult
            time.sleep(self.simdelay) # TODO use actionlib here

            #attach the collision object to the hand (should be cleaned-up)
            rospy.loginfo("Now we attach the object")

            att_object = AttachedCollisionObject()
            att_object.link_name = "palm"
            att_object.object.id = object_to_attach
            att_object.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT
            att_object.object.header.frame_id = "palm"
            att_object.object.header.stamp = rospy.Time.now()
            object = Shape()
            object.type = Shape.CYLINDER
            object.dimensions.append(.03)
            object.dimensions.append(0.1)
            pose = Pose()
            pose.position.x = 0.0
            pose.position.y = -0.06
            pose.position.z = 0.06
            pose.orientation.x = 0
            pose.orientation.y = 0
            pose.orientation.z = 0
            pose.orientation.w = 1
            att_object.object.shapes.append(object)
            att_object.object.poses.append(pose);
            att_object.touch_links= ["ffdistal","mfdistal","rfdistal","lfdistal","thdistal","ffmiddle","mfmiddle","rfmiddle","lfmiddle","thmiddle","ffproximal","mfproximal","rfproximal","lfproximal","thproximal","palm","lfmetacarpal","thbase"]
            self.att_object_in_map_pub_.publish(att_object)
            rospy.loginfo("Attach object published")
        else:
            rospy.logerr("None of the grasps tested is possible")
            pickresult.manipulation_result.value = ManipulationResult.UNFEASIBLE
            return pickresult
        pickresult.manipulation_result.value = ManipulationResult.SUCCESS
        pickresult.grasp= grasp_to_execute_
        return pickresult
    def push_object(self, goal):
        if self.action_server.is_preempt_requested():
            rospy.loginfo('%s: preempted' % ACTION_NAME)
            self.action_server.set_preempted()

        collision_object_name = goal.collision_object_name
        collision_support_surface_name = goal.collision_support_surface_name

        current_state = rospy.wait_for_message('l_arm_controller/state',
                                               FollowJointTrajectoryFeedback)
        start_angles = current_state.actual.positions

        full_state = rospy.wait_for_message('/joint_states', JointState)

        req = GetPositionFKRequest()
        req.header.frame_id = 'base_link'
        req.fk_link_names = [GRIPPER_LINK_FRAME]
        req.robot_state.joint_state = full_state

        if not self.set_planning_scene_diff_client():
            rospy.logerr('%s: failed to set planning scene diff' % ACTION_NAME)
            self.action_server.set_aborted()
            return

        pose = self.get_fk_srv(req).pose_stamped[0].pose

        frame_id = 'base_link'

        approachpos = [pose.position.x, pose.position.y, pose.position.z]
        approachquat = [
            pose.orientation.x, pose.orientation.y, pose.orientation.z,
            pose.orientation.w
        ]

        push_distance = 0.40
        grasppos = [
            pose.position.x, pose.position.y - push_distance, pose.position.z
        ]
        graspquat = [0.00000, 0.00000, 0.70711, -0.70711]

        # attach object to gripper, they will move together
        obj = AttachedCollisionObject()
        obj.object.header.stamp = rospy.Time.now()
        obj.object.header.frame_id = GRIPPER_LINK_FRAME
        obj.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT
        obj.object.id = collision_object_name
        obj.link_name = GRIPPER_LINK_FRAME
        obj.touch_links = GRIPPER_LINKS

        self.attached_object_pub.publish(obj)

        # disable collisions between attached object and table
        collision_operation1 = CollisionOperation()
        collision_operation1.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS
        collision_operation1.object2 = collision_support_surface_name
        collision_operation1.operation = CollisionOperation.DISABLE

        collision_operation2 = CollisionOperation()
        collision_operation2.object1 = collision_support_surface_name
        collision_operation2.object2 = GRIPPER_GROUP_NAME
        collision_operation2.operation = CollisionOperation.DISABLE
        collision_operation2.penetration_distance = 0.02

        ordered_collision_operations = OrderedCollisionOperations()
        ordered_collision_operations.collision_operations = [
            collision_operation1, collision_operation2
        ]

        res = self.check_cartesian_path_lists(
            approachpos,
            approachquat,
            grasppos,
            graspquat,
            start_angles,
            frame=frame_id,
            ordered_collision_operations=ordered_collision_operations)

        error_code_dict = {
            ArmNavigationErrorCodes.SUCCESS: 0,
            ArmNavigationErrorCodes.COLLISION_CONSTRAINTS_VIOLATED: 1,
            ArmNavigationErrorCodes.PATH_CONSTRAINTS_VIOLATED: 2,
            ArmNavigationErrorCodes.JOINT_LIMITS_VIOLATED: 3,
            ArmNavigationErrorCodes.PLANNING_FAILED: 4
        }

        trajectory_len = len(res.trajectory.joint_trajectory.points)
        trajectory = [
            res.trajectory.joint_trajectory.points[i].positions
            for i in range(trajectory_len)
        ]
        vels = [
            res.trajectory.joint_trajectory.points[i].velocities
            for i in range(trajectory_len)
        ]
        times = [
            res.trajectory.joint_trajectory.points[i].time_from_start
            for i in range(trajectory_len)
        ]
        error_codes = [
            error_code_dict[error_code.val]
            for error_code in res.trajectory_error_codes
        ]

        # if even one code is not 0, abort
        if sum(error_codes) != 0:
            for ind in range(len(trajectory)):
                rospy.loginfo("error code " + str(error_codes[ind]) +
                              " pos : " + self.pplist(trajectory[ind]))

            rospy.loginfo("")

            for ind in range(len(trajectory)):
                rospy.loginfo("time: " + "%5.3f  " % times[ind].to_sec() +
                              "vels: " + self.pplist(vels[ind]))

            rospy.logerr('%s: attempted push failed' % ACTION_NAME)
            self.action_server.set_aborted()
            return

        req = FilterJointTrajectoryRequest()
        req.trajectory = res.trajectory.joint_trajectory
        req.trajectory.points = req.trajectory.points[
            1:]  # skip zero velocity point
        req.allowed_time = rospy.Duration(2.0)

        filt_res = self.trajectory_filter_srv(req)

        goal = FollowJointTrajectoryGoal()
        goal.trajectory = filt_res.trajectory
        goal.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.1)

        self.start_audio_recording_srv(InfomaxAction.PUSH, goal.category_id)
        rospy.sleep(0.5)

        self.trajectory_controller.send_goal(goal)
        self.trajectory_controller.wait_for_result()

        state = self.trajectory_controller.get_state()

        if state == GoalStatus.SUCCEEDED:
            rospy.sleep(0.5)
            sound_msg = self.stop_audio_recording_srv(True)
            self.action_server.set_succeeded(
                PushObjectResult(sound_msg.recorded_sound))
            return

        rospy.logerr('%s: attempted push failed' % ACTION_NAME)
        self.stop_audio_recording_srv(False)
        self.action_server.set_aborted()
    def put_down_at(self, goal):
        if self.action_server.is_preempt_requested():
            rospy.loginfo('%s: preempted' % ACTION_NAME)
            self.action_server.set_preempted()

        bbox_center = self.find_cluster_bounding_box_center(
            goal.graspable_object.cluster)
        if not bbox_center: self.action_server.set_aborted()

        goal.target_location.z = bbox_center.z
        x_dist = goal.target_location.x - bbox_center.x
        y_dist = goal.target_location.y - bbox_center.y

        target = goal.graspable_object
        target.cluster.points = [
            Point32(p.x + x_dist, p.y + y_dist, p.z)
            for p in target.cluster.points
        ]

        collision_object_name = goal.collision_object_name
        collision_support_surface_name = goal.collision_support_surface_name

        ik_solution = self.find_grasp_pose(target, collision_object_name,
                                           collision_support_surface_name)

        if ik_solution:
            # disable collisions between gripper and target object
            ordered_collision_operations = OrderedCollisionOperations()
            collision_operation = CollisionOperation()
            collision_operation.object1 = collision_object_name
            collision_operation.object2 = GRIPPER_GROUP_NAME
            collision_operation.operation = CollisionOperation.DISABLE

            # disable collisions between gripper and table
            collision_operation2 = CollisionOperation()
            collision_operation2.object1 = collision_support_surface_name
            collision_operation2.object2 = GRIPPER_GROUP_NAME
            collision_operation2.operation = CollisionOperation.DISABLE
            collision_operation2.penetration_distance = 0.01

            # disable collisions between arm and table
            collision_operation3 = CollisionOperation()
            collision_operation3.object1 = collision_support_surface_name
            collision_operation3.object2 = ARM_GROUP_NAME
            collision_operation3.operation = CollisionOperation.DISABLE
            collision_operation3.penetration_distance = 0.01

            ordered_collision_operations.collision_operations = [
                collision_operation, collision_operation2, collision_operation3
            ]

            # set gripper padding to 0
            gripper_paddings = [LinkPadding(l, 0.0) for l in GRIPPER_LINKS]
            gripper_paddings.extend([LinkPadding(l, 0.0) for l in ARM_LINKS])

            # move into pre-grasp pose
            if not move_arm_joint_goal(
                    self.move_arm_client,
                    ik_solution.joint_state.name,
                    ik_solution.joint_state.position,
                    link_padding=gripper_paddings,
                    collision_operations=ordered_collision_operations):
                rospy.logerr('%s: attempted grasp failed' % ACTION_NAME)
                self.action_server.set_aborted()
                return

            # record put down sound with 0.5 second padding before and after
            self.start_audio_recording_srv(InfomaxAction.GRASP,
                                           goal.category_id)
            rospy.sleep(0.5)
            self.open_gripper()
            rospy.sleep(0.5)
            sound_msg = self.stop_audio_recording_srv(True)

            obj = AttachedCollisionObject()
            obj.object.header.stamp = rospy.Time.now()
            obj.object.header.frame_id = GRIPPER_LINK_FRAME
            obj.object.operation.operation = CollisionObjectOperation.DETACH_AND_ADD_AS_OBJECT
            obj.object.id = collision_object_name
            obj.link_name = GRIPPER_LINK_FRAME
            obj.touch_links = GRIPPER_LINKS

            self.attached_object_pub.publish(obj)
            self.action_server.set_succeeded(
                PutDownAtResult(sound_msg.recorded_sound))
            return

        self.stop_audio_recording_srv(False)
        rospy.logerr('%s: attempted put down failed' % ACTION_NAME)
        self.action_server.set_aborted()
示例#30
0
    def lift_object(self, goal):
        if self.action_server.is_preempt_requested():
            rospy.loginfo('%s: preempted' % ACTION_NAME)
            self.action_server.set_preempted()

        collision_support_surface_name = goal.collision_support_surface_name

        # disable collisions between grasped object and table
        collision_operation1 = CollisionOperation()
        collision_operation1.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS
        collision_operation1.object2 = collision_support_surface_name
        collision_operation1.operation = CollisionOperation.DISABLE

        # disable collisions between gripper and table
        collision_operation2 = CollisionOperation()
        collision_operation2.object1 = GRIPPER_GROUP_NAME
        collision_operation2.object2 = collision_support_surface_name
        collision_operation2.operation = CollisionOperation.DISABLE

        ordered_collision_operations = OrderedCollisionOperations()
        ordered_collision_operations.collision_operations = [
            collision_operation1, collision_operation2
        ]

        gripper_paddings = [LinkPadding(l, 0.0) for l in GRIPPER_LINKS]

        # this is a hack to make arm lift an object faster
        obj = AttachedCollisionObject()
        obj.object.header.stamp = rospy.Time.now()
        obj.object.header.frame_id = GRIPPER_LINK_FRAME
        obj.object.operation.operation = CollisionObjectOperation.REMOVE
        obj.object.id = collision_support_surface_name
        obj.link_name = GRIPPER_LINK_FRAME
        obj.touch_links = GRIPPER_LINKS
        self.attached_object_pub.publish(obj)

        current_state = rospy.wait_for_message('l_arm_controller/state',
                                               FollowJointTrajectoryFeedback)
        joint_names = current_state.joint_names
        joint_positions = current_state.actual.positions
        start_angles = [
            joint_positions[joint_names.index(jn)] for jn in ARM_JOINTS
        ]
        start_angles[0] = start_angles[0] - 0.3  # move shoulder up a bit

        if not move_arm_joint_goal(
                self.move_arm_client,
                ARM_JOINTS,
                start_angles,
                link_padding=gripper_paddings,
                collision_operations=ordered_collision_operations):
            self.action_server.set_aborted()
            return

        self.start_audio_recording_srv(InfomaxAction.LIFT, goal.category_id)

        if move_arm_joint_goal(
                self.move_arm_client,
                ARM_JOINTS,
                LIFT_POSITION,
                link_padding=gripper_paddings,
                collision_operations=ordered_collision_operations):
            # check if are still holding an object after lift is done
            grasp_status = self.get_grasp_status_srv()

            # if the object is still in hand after lift is done we are good
            if grasp_status.is_hand_occupied:
                sound_msg = self.stop_audio_recording_srv(True)
                self.action_server.set_succeeded(
                    LiftObjectResult(sound_msg.recorded_sound))
                return

        self.stop_audio_recording_srv(False)
        rospy.logerr('%s: attempted lift failed' % ACTION_NAME)
        self.action_server.set_aborted()
        return
    def setUp(self):

        self.tf = TransformListener()
        
        self.move_arm_action_client = actionlib.SimpleActionClient("move_right_arm", MoveArmAction)



        att_pub = rospy.Publisher('attached_collision_object', AttachedCollisionObject)
        obj_pub = rospy.Publisher('collision_object',CollisionObject)
        
        rospy.init_node('test_motion_execution_buffer')
        
        #let everything settle down
        rospy.sleep(5.)
        
        obj1 = CollisionObject()
    
        obj1.header.stamp = rospy.Time.now()-rospy.Duration(.1)
        obj1.header.frame_id = "base_footprint"
        obj1.id = "obj2";
        obj1.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
        obj1.shapes = [Shape() for _ in range(3)]
        obj1.poses = [Pose() for _ in range(3)]

        obj1.shapes[0].type = Shape.BOX
        obj1.shapes[0].dimensions = [float() for _ in range(3)]
        obj1.shapes[0].dimensions[0] = .5
        obj1.shapes[0].dimensions[1] = 1.0
        obj1.shapes[0].dimensions[2] = .2
        obj1.poses[0].position.x = .95
        obj1.poses[0].position.y = -.25
        obj1.poses[0].position.z = .62
        obj1.poses[0].orientation.x = 0
        obj1.poses[0].orientation.y = 0
        obj1.poses[0].orientation.z = 0
        obj1.poses[0].orientation.w = 1

        obj1.shapes[2].type = Shape.BOX
        obj1.shapes[2].dimensions = [float() for _ in range(3)]
        obj1.shapes[2].dimensions[0] = .5
        obj1.shapes[2].dimensions[1] = .1
        obj1.shapes[2].dimensions[2] = 1.0
        obj1.poses[2].position.x = .95
        obj1.poses[2].position.y = -.14
        obj1.poses[2].position.z = 1.2
        obj1.poses[2].orientation.x = 0
        obj1.poses[2].orientation.y = 0
        obj1.poses[2].orientation.z = 0
        obj1.poses[2].orientation.w = 1

        obj1.shapes[1].type = Shape.BOX
        obj1.shapes[1].dimensions = [float() for _ in range(3)]
        obj1.shapes[1].dimensions[0] = .5
        obj1.shapes[1].dimensions[1] = .1
        obj1.shapes[1].dimensions[2] = 1.0
        obj1.poses[1].position.x = .95
        obj1.poses[1].position.y = .12
        obj1.poses[1].position.z = 1.2
        obj1.poses[1].orientation.x = 0
        obj1.poses[1].orientation.y = 0
        obj1.poses[1].orientation.z = 0
        obj1.poses[1].orientation.w = 1
        
        obj_pub.publish(obj1)

        att_object = AttachedCollisionObject()
        att_object.object.header.stamp = rospy.Time.now()
        att_object.object.header.frame_id = "r_gripper_r_finger_tip_link"
        att_object.link_name = "r_gripper_r_finger_tip_link"
        att_object.object.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
        att_object.object = CollisionObject();

        att_object.object.header.stamp = rospy.Time.now()
        att_object.object.header.frame_id = "r_gripper_r_finger_tip_link"
        att_object.object.id = "pole"
        att_object.object.shapes = [Shape() for _ in range(1)]
        att_object.object.shapes[0].type = Shape.CYLINDER
        att_object.object.shapes[0].dimensions = [float() for _ in range(2)]
        att_object.object.shapes[0].dimensions[0] = .02
        att_object.object.shapes[0].dimensions[1] = .1
        att_object.object.poses = [Pose() for _ in range(1)]
        att_object.object.poses[0].position.x = -.02
        att_object.object.poses[0].position.y = .04
        att_object.object.poses[0].position.z = 0
        att_object.object.poses[0].orientation.x = 0
        att_object.object.poses[0].orientation.y = 0
        att_object.object.poses[0].orientation.z = 0
        att_object.object.poses[0].orientation.w = 1

        att_pub.publish(att_object)

        rospy.sleep(5.0)        
    def grasp_object(self, goal):
        if self.action_server.is_preempt_requested():
            rospy.loginfo('%s: preempted' % ACTION_NAME)
            self.action_server.set_preempted()

        target = goal.graspable_object
        collision_object_name = goal.collision_object_name
        collision_support_surface_name = goal.collision_support_surface_name

        ik_solution = self.find_grasp_pose(target, collision_object_name,
                                           collision_support_surface_name)

        if ik_solution:
            # disable collisions between gripper and target object
            ordered_collision_operations = OrderedCollisionOperations()
            collision_operation1 = CollisionOperation()
            collision_operation1.object1 = collision_object_name
            collision_operation1.object2 = GRIPPER_GROUP_NAME
            collision_operation1.operation = CollisionOperation.DISABLE

            collision_operation2 = CollisionOperation()
            collision_operation2.object1 = collision_support_surface_name
            collision_operation2.object2 = GRIPPER_GROUP_NAME
            collision_operation2.operation = CollisionOperation.DISABLE
            #            collision_operation2.penetration_distance = 0.02

            ordered_collision_operations.collision_operations = [
                collision_operation1, collision_operation2
            ]

            # set gripper padding to 0
            gripper_paddings = [LinkPadding(l, 0.0) for l in GRIPPER_LINKS]
            gripper_paddings.extend([LinkPadding(l, 0.0) for l in ARM_LINKS])

            self.open_gripper()

            # move into pre-grasp pose
            if not move_arm_joint_goal(
                    self.move_arm_client,
                    ik_solution.joint_state.name,
                    ik_solution.joint_state.position,
                    link_padding=gripper_paddings,
                    collision_operations=ordered_collision_operations):
                rospy.logerr('%s: attempted grasp failed' % ACTION_NAME)
                self.action_server.set_aborted()
                return

            # record grasping sound with 0.5 second padding before and after
            self.start_audio_recording_srv(InfomaxAction.GRASP,
                                           goal.category_id)
            rospy.sleep(0.5)
            grasp_successful = self.close_gripper()
            rospy.sleep(0.5)

            # if grasp was successful, attach it to the gripper
            if grasp_successful:
                sound_msg = self.stop_audio_recording_srv(True)

                obj = AttachedCollisionObject()
                obj.object.header.stamp = rospy.Time.now()
                obj.object.header.frame_id = GRIPPER_LINK_FRAME
                obj.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT
                obj.object.id = collision_object_name
                obj.link_name = GRIPPER_LINK_FRAME
                obj.touch_links = GRIPPER_LINKS

                self.attached_object_pub.publish(obj)
                self.action_server.set_succeeded(
                    GraspObjectResult(sound_msg.recorded_sound))
                return

        self.stop_audio_recording_srv(False)
        rospy.logerr('%s: attempted grasp failed' % ACTION_NAME)
        self.action_server.set_aborted()
    def grasp_object(self, goal):
        if self.action_server.is_preempt_requested():
            rospy.loginfo('%s: preempted' % ACTION_NAME)
            self.action_server.set_preempted()
            
        target = goal.graspable_object
        collision_object_name = goal.collision_object_name
        collision_support_surface_name = goal.collision_support_surface_name
        
        ik_solution = self.find_grasp_pose(target, collision_object_name, collision_support_surface_name)
        
        if ik_solution:
            # disable collisions between gripper and target object
            ordered_collision_operations = OrderedCollisionOperations()
            collision_operation1 = CollisionOperation()
            collision_operation1.object1 = collision_object_name
            collision_operation1.object2 = GRIPPER_GROUP_NAME
            collision_operation1.operation = CollisionOperation.DISABLE
            
            collision_operation2 = CollisionOperation()
            collision_operation2.object1 = collision_support_surface_name
            collision_operation2.object2 = GRIPPER_GROUP_NAME
            collision_operation2.operation = CollisionOperation.DISABLE
#            collision_operation2.penetration_distance = 0.02
            
            ordered_collision_operations.collision_operations = [collision_operation1, collision_operation2]
            
            # set gripper padding to 0
            gripper_paddings = [LinkPadding(l,0.0) for l in GRIPPER_LINKS]
            gripper_paddings.extend([LinkPadding(l,0.0) for l in ARM_LINKS])
            
            self.open_gripper()
            
            # move into pre-grasp pose
            if not move_arm_joint_goal(self.move_arm_client,
                                       ik_solution.joint_state.name,
                                       ik_solution.joint_state.position,
                                       link_padding=gripper_paddings,
                                       collision_operations=ordered_collision_operations):
                rospy.logerr('%s: attempted grasp failed' % ACTION_NAME)
                self.action_server.set_aborted()
                return
                
            # record grasping sound with 0.5 second padding before and after
            self.start_audio_recording_srv(InfomaxAction.GRASP, goal.category_id)
            rospy.sleep(0.5)
            grasp_successful = self.close_gripper()
            rospy.sleep(0.5)
            
            # if grasp was successful, attach it to the gripper
            if grasp_successful:
                sound_msg = self.stop_audio_recording_srv(True)
                
                obj = AttachedCollisionObject()
                obj.object.header.stamp = rospy.Time.now()
                obj.object.header.frame_id = GRIPPER_LINK_FRAME
                obj.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT
                obj.object.id = collision_object_name
                obj.link_name = GRIPPER_LINK_FRAME
                obj.touch_links = GRIPPER_LINKS
                
                self.attached_object_pub.publish(obj)
                self.action_server.set_succeeded(GraspObjectResult(sound_msg.recorded_sound))
                return 
                
        self.stop_audio_recording_srv(False)
        rospy.logerr('%s: attempted grasp failed' % ACTION_NAME)
        self.action_server.set_aborted()
 def place_object(self, goal):
     if self.action_server.is_preempt_requested():
         rospy.loginfo('%s: preempted' % ACTION_NAME)
         self.action_server.set_preempted()
         
     collision_object_name = goal.collision_object_name
     collision_support_surface_name = goal.collision_support_surface_name
     
     # check that we have something in hand before placing it
     grasp_status = self.get_grasp_status_srv()
     
     # if the object is still in hand after lift is done we are good
     if not grasp_status.is_hand_occupied:
         rospy.logerr('%s: gripper empty, nothing to place' % ACTION_NAME)
         self.action_server.set_aborted()
         return
         
     gripper_paddings = [LinkPadding(l,0.0) for l in GRIPPER_LINKS]
     
     # disable collisions between attached object and table
     collision_operation1 = CollisionOperation()
     collision_operation1.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS
     collision_operation1.object2 = collision_support_surface_name
     collision_operation1.operation = CollisionOperation.DISABLE
     
     # disable collisions between gripper and table
     collision_operation2 = CollisionOperation()
     collision_operation2.object1 = collision_support_surface_name
     collision_operation2.object2 = GRIPPER_GROUP_NAME
     collision_operation2.operation = CollisionOperation.DISABLE
     collision_operation2.penetration_distance = 0.01
     
     # disable collisions between arm and table
     collision_operation3 = CollisionOperation()
     collision_operation3.object1 = collision_support_surface_name
     collision_operation3.object2 = ARM_GROUP_NAME
     collision_operation3.operation = CollisionOperation.DISABLE
     collision_operation3.penetration_distance = 0.01
     
     ordered_collision_operations = OrderedCollisionOperations()
     ordered_collision_operations.collision_operations = [collision_operation1, collision_operation2, collision_operation3]
     
     self.start_audio_recording_srv(InfomaxAction.PLACE, goal.category_id)
     
     if move_arm_joint_goal(self.move_arm_client,
                            ARM_JOINTS,
                            PLACE_POSITION,
                            link_padding=gripper_paddings,
                            collision_operations=ordered_collision_operations):
         sound_msg = None
         grasp_status = self.get_grasp_status_srv()
         
         self.open_gripper()
         rospy.sleep(0.5)
         
         # if after lowering arm gripper is still holding an object life's good
         if grasp_status.is_hand_occupied:
             sound_msg = self.stop_audio_recording_srv(True)
         else:
             self.stop_audio_recording_srv(False)
             
         obj = AttachedCollisionObject()
         obj.object.header.stamp = rospy.Time.now()
         obj.object.header.frame_id = GRIPPER_LINK_FRAME
         obj.object.operation.operation = CollisionObjectOperation.DETACH_AND_ADD_AS_OBJECT
         obj.object.id = collision_object_name
         obj.link_name = GRIPPER_LINK_FRAME
         obj.touch_links = GRIPPER_LINKS
         self.attached_object_pub.publish(obj)
         
         if sound_msg:
             self.action_server.set_succeeded(PlaceObjectResult(sound_msg.recorded_sound))
             return
         else:
             self.action_server.set_aborted()
             return
             
     self.stop_audio_recording_srv(False)
     rospy.logerr('%s: attempted place failed' % ACTION_NAME)
     self.action_server.set_aborted()
    def place_object(self, goal):
        if self.action_server.is_preempt_requested():
            rospy.loginfo('%s: preempted' % ACTION_NAME)
            self.action_server.set_preempted()

        collision_object_name = goal.collision_object_name
        collision_support_surface_name = goal.collision_support_surface_name

        # check that we have something in hand before placing it
        grasp_status = self.get_grasp_status_srv()

        # if the object is still in hand after lift is done we are good
        if not grasp_status.is_hand_occupied:
            rospy.logerr('%s: gripper empty, nothing to place' % ACTION_NAME)
            self.action_server.set_aborted()
            return

        gripper_paddings = [LinkPadding(l, 0.0) for l in GRIPPER_LINKS]

        # disable collisions between attached object and table
        collision_operation1 = CollisionOperation()
        collision_operation1.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS
        collision_operation1.object2 = collision_support_surface_name
        collision_operation1.operation = CollisionOperation.DISABLE

        # disable collisions between gripper and table
        collision_operation2 = CollisionOperation()
        collision_operation2.object1 = collision_support_surface_name
        collision_operation2.object2 = GRIPPER_GROUP_NAME
        collision_operation2.operation = CollisionOperation.DISABLE
        collision_operation2.penetration_distance = 0.01

        # disable collisions between arm and table
        collision_operation3 = CollisionOperation()
        collision_operation3.object1 = collision_support_surface_name
        collision_operation3.object2 = ARM_GROUP_NAME
        collision_operation3.operation = CollisionOperation.DISABLE
        collision_operation3.penetration_distance = 0.01

        ordered_collision_operations = OrderedCollisionOperations()
        ordered_collision_operations.collision_operations = [
            collision_operation1, collision_operation2, collision_operation3
        ]

        self.start_audio_recording_srv(InfomaxAction.PLACE, goal.category_id)

        if move_arm_joint_goal(
                self.move_arm_client,
                ARM_JOINTS,
                PLACE_POSITION,
                link_padding=gripper_paddings,
                collision_operations=ordered_collision_operations):
            sound_msg = None
            grasp_status = self.get_grasp_status_srv()

            self.open_gripper()
            rospy.sleep(0.5)

            # if after lowering arm gripper is still holding an object life's good
            if grasp_status.is_hand_occupied:
                sound_msg = self.stop_audio_recording_srv(True)
            else:
                self.stop_audio_recording_srv(False)

            obj = AttachedCollisionObject()
            obj.object.header.stamp = rospy.Time.now()
            obj.object.header.frame_id = GRIPPER_LINK_FRAME
            obj.object.operation.operation = CollisionObjectOperation.DETACH_AND_ADD_AS_OBJECT
            obj.object.id = collision_object_name
            obj.link_name = GRIPPER_LINK_FRAME
            obj.touch_links = GRIPPER_LINKS
            self.attached_object_pub.publish(obj)

            if sound_msg:
                self.action_server.set_succeeded(
                    PlaceObjectResult(sound_msg.recorded_sound))
                return
            else:
                self.action_server.set_aborted()
                return

        self.stop_audio_recording_srv(False)
        rospy.logerr('%s: attempted place failed' % ACTION_NAME)
        self.action_server.set_aborted()
    def push_object(self, goal):
        if self.action_server.is_preempt_requested():
            rospy.loginfo("%s: preempted" % ACTION_NAME)
            self.action_server.set_preempted()

        collision_object_name = goal.collision_object_name
        collision_support_surface_name = goal.collision_support_surface_name

        current_state = rospy.wait_for_message("l_arm_controller/state", FollowJointTrajectoryFeedback)
        start_angles = current_state.actual.positions

        full_state = rospy.wait_for_message("/joint_states", JointState)

        req = GetPositionFKRequest()
        req.header.frame_id = "base_link"
        req.fk_link_names = [GRIPPER_LINK_FRAME]
        req.robot_state.joint_state = full_state

        if not self.set_planning_scene_diff_client():
            rospy.logerr("%s: failed to set planning scene diff" % ACTION_NAME)
            self.action_server.set_aborted()
            return

        pose = self.get_fk_srv(req).pose_stamped[0].pose

        frame_id = "base_link"

        approachpos = [pose.position.x, pose.position.y, pose.position.z]
        approachquat = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]

        push_distance = 0.40
        grasppos = [pose.position.x, pose.position.y - push_distance, pose.position.z]
        graspquat = [0.00000, 0.00000, 0.70711, -0.70711]

        # attach object to gripper, they will move together
        obj = AttachedCollisionObject()
        obj.object.header.stamp = rospy.Time.now()
        obj.object.header.frame_id = GRIPPER_LINK_FRAME
        obj.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT
        obj.object.id = collision_object_name
        obj.link_name = GRIPPER_LINK_FRAME
        obj.touch_links = GRIPPER_LINKS

        self.attached_object_pub.publish(obj)

        # disable collisions between attached object and table
        collision_operation1 = CollisionOperation()
        collision_operation1.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS
        collision_operation1.object2 = collision_support_surface_name
        collision_operation1.operation = CollisionOperation.DISABLE

        collision_operation2 = CollisionOperation()
        collision_operation2.object1 = collision_support_surface_name
        collision_operation2.object2 = GRIPPER_GROUP_NAME
        collision_operation2.operation = CollisionOperation.DISABLE
        collision_operation2.penetration_distance = 0.02

        ordered_collision_operations = OrderedCollisionOperations()
        ordered_collision_operations.collision_operations = [collision_operation1, collision_operation2]

        res = self.check_cartesian_path_lists(
            approachpos,
            approachquat,
            grasppos,
            graspquat,
            start_angles,
            frame=frame_id,
            ordered_collision_operations=ordered_collision_operations,
        )

        error_code_dict = {
            ArmNavigationErrorCodes.SUCCESS: 0,
            ArmNavigationErrorCodes.COLLISION_CONSTRAINTS_VIOLATED: 1,
            ArmNavigationErrorCodes.PATH_CONSTRAINTS_VIOLATED: 2,
            ArmNavigationErrorCodes.JOINT_LIMITS_VIOLATED: 3,
            ArmNavigationErrorCodes.PLANNING_FAILED: 4,
        }

        trajectory_len = len(res.trajectory.joint_trajectory.points)
        trajectory = [res.trajectory.joint_trajectory.points[i].positions for i in range(trajectory_len)]
        vels = [res.trajectory.joint_trajectory.points[i].velocities for i in range(trajectory_len)]
        times = [res.trajectory.joint_trajectory.points[i].time_from_start for i in range(trajectory_len)]
        error_codes = [error_code_dict[error_code.val] for error_code in res.trajectory_error_codes]

        # if even one code is not 0, abort
        if sum(error_codes) != 0:
            for ind in range(len(trajectory)):
                rospy.loginfo("error code " + str(error_codes[ind]) + " pos : " + self.pplist(trajectory[ind]))

            rospy.loginfo("")

            for ind in range(len(trajectory)):
                rospy.loginfo("time: " + "%5.3f  " % times[ind].to_sec() + "vels: " + self.pplist(vels[ind]))

            rospy.logerr("%s: attempted push failed" % ACTION_NAME)
            self.action_server.set_aborted()
            return

        req = FilterJointTrajectoryRequest()
        req.trajectory = res.trajectory.joint_trajectory
        req.trajectory.points = req.trajectory.points[1:]  # skip zero velocity point
        req.allowed_time = rospy.Duration(2.0)

        filt_res = self.trajectory_filter_srv(req)

        goal = FollowJointTrajectoryGoal()
        goal.trajectory = filt_res.trajectory
        goal.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.1)

        self.start_audio_recording_srv(InfomaxAction.PUSH, goal.category_id)
        rospy.sleep(0.5)

        self.trajectory_controller.send_goal(goal)
        self.trajectory_controller.wait_for_result()

        state = self.trajectory_controller.get_state()

        if state == GoalStatus.SUCCEEDED:
            rospy.sleep(0.5)
            sound_msg = self.stop_audio_recording_srv(True)
            self.action_server.set_succeeded(PushObjectResult(sound_msg.recorded_sound))
            return

        rospy.logerr("%s: attempted push failed" % ACTION_NAME)
        self.stop_audio_recording_srv(False)
        self.action_server.set_aborted()
    def attach_object_to_gripper(self, arm_name, object_id):
        '''
        Attaches an object to the robot's end effector.  

        The object must already exist in the planning scene 
        (although it does not need to exist in the current state of the world).  This does NOT "snap" the object to 
        the end effector. Rather, now when the robot moves, the object is assumed to remain stationary with respect 
        to the robot's end effector instead of the world.  Collisions will be checked between the object and the 
        world as the object moves, but collisions between the object and the end effector will be ignored.  
        The link the object is attached to and the links with which collisions are ignored are defined by the hand 
        description (see hand_description.py).

        The object will be attached to the robot according to the current state of the robot and object in the 
        planning scene as maintained by this class (i.e. with any prior changes you made to the diff).

        To undo this function, you can use detach_object and add_object but you must remember the position of the
        object where it was originally attached!  If you call detach_and_add_back_attached_object, it will add
        the object back at its current location in the planning scene, NOT at the location at which it was 
        originally attached.

        The planning scene doesn't support ATTACH_AND_REMOVE so we do those two operations simultaneously, passing 
        an attached object with operation ADD and a collision object with operation REMOVE.  

        **Args:**
        
            **arm_name (string):** The arm ('left_arm' or 'right_arm') to attach the object to

            **object_id (string):** The ID of the object to attach

        **Returns:**
            False if the object did not previously exist in the world or planning scene diff and therefore cannot be 
            attached; True otherwise.
        '''
        #Note: It is important to send the full collision object (not just the id) in the attached collision object.
        #This was quite complicated to figure out the first time.
        obj = AttachedCollisionObject()
        obj.link_name = self.hands[arm_name].attach_link
        obj.touch_links = self.hands[arm_name].touch_links
        diff_obj = None
        for co in self.current_diff.planning_scene_diff.collision_objects:
            if co.id == object_id:
                rospy.loginfo('Object was added to or modified in planning scene.')
                diff_obj = co
                break
        if not diff_obj:
            rospy.loginfo('Object was not previously added to or modified in planning scene.')
            diff_obj = self.collision_object(object_id)
            if not diff_obj:
                rospy.logerr('Cannot attach object '+object_id+'.  This object does not exist')
                return False
            self.current_diff.planning_scene_diff.collision_objects.append(diff_obj)
        self.current_diff.planning_scene_diff.attached_collision_objects.append(obj)

        #convert it into the frame of the hand - it remains stationary with respect to this
        for p in range(len(diff_obj.poses)):
            diff_obj.poses[p] = self.transform_pose(obj.link_name, diff_obj.header.frame_id, diff_obj.poses[p])
        diff_obj.header.frame_id = obj.link_name
        diff_obj.operation.operation = diff_obj.operation.REMOVE
        obj.object = copy.deepcopy(diff_obj)
        obj.object.operation.operation = obj.object.operation.ADD
        self.send_diff()
        return True
    def attach_object_to_gripper(self, arm_name, object_id):
        '''
        Attaches an object to the robot's end effector.  

        The object must already exist in the planning scene 
        (although it does not need to exist in the current state of the world).  This does NOT "snap" the object to 
        the end effector. Rather, now when the robot moves, the object is assumed to remain stationary with respect 
        to the robot's end effector instead of the world.  Collisions will be checked between the object and the 
        world as the object moves, but collisions between the object and the end effector will be ignored.  
        The link the object is attached to and the links with which collisions are ignored are defined by the hand 
        description (see hand_description.py).

        The object will be attached to the robot according to the current state of the robot and object in the 
        planning scene as maintained by this class (i.e. with any prior changes you made to the diff).

        To undo this function, you can use detach_object and add_object but you must remember the position of the
        object where it was originally attached!  If you call detach_and_add_back_attached_object, it will add
        the object back at its current location in the planning scene, NOT at the location at which it was 
        originally attached.

        The planning scene doesn't support ATTACH_AND_REMOVE so we do those two operations simultaneously, passing 
        an attached object with operation ADD and a collision object with operation REMOVE.  

        **Args:**
        
            **arm_name (string):** The arm ('left_arm' or 'right_arm') to attach the object to

            **object_id (string):** The ID of the object to attach

        **Returns:**
            False if the object did not previously exist in the world or planning scene diff and therefore cannot be 
            attached; True otherwise.
        '''
        #Note: It is important to send the full collision object (not just the id) in the attached collision object.
        #This was quite complicated to figure out the first time.
        obj = AttachedCollisionObject()
        obj.link_name = self.hands[arm_name].attach_link
        obj.touch_links = self.hands[arm_name].touch_links
        diff_obj = None
        for co in self.current_diff.planning_scene_diff.collision_objects:
            if co.id == object_id:
                rospy.loginfo('Object was added to or modified in planning scene.')
                diff_obj = co
                break
        if not diff_obj:
            rospy.loginfo('Object was not previously added to or modified in planning scene.')
            diff_obj = self.collision_object(object_id)
            if not diff_obj:
                rospy.logerr('Cannot attach object '+object_id+'.  This object does not exist')
                return False
            self.current_diff.planning_scene_diff.collision_objects.append(diff_obj)
        self.current_diff.planning_scene_diff.attached_collision_objects.append(obj)

        #convert it into the frame of the hand - it remains stationary with respect to this
        for p in range(len(diff_obj.poses)):
            diff_obj.poses[p] = self.transform_pose(obj.link_name, diff_obj.header.frame_id, diff_obj.poses[p])
        diff_obj.header.frame_id = obj.link_name
        diff_obj.operation.operation = diff_obj.operation.REMOVE
        obj.object = copy.deepcopy(diff_obj)
        obj.object.operation.operation = obj.object.operation.ADD
        self.send_diff()
        return True
示例#39
0
def test_add_convert_objects():

    obj_pub = rospy.Publisher('collision_object',CollisionObject)
    att_pub = rospy.Publisher('attached_collision_object',AttachedCollisionObject)

    rospy.init_node('test_collision_objects')

    #let everything settle down
    rospy.sleep(5.)
      
    obj1 = CollisionObject()
    
    obj1.header.stamp = rospy.Time.now()
    obj1.header.frame_id = "base_link"
    obj1.id = "obj1";
    obj1.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
    obj1.shapes = [Shape() for _ in range(1)]
    obj1.shapes[0].type = Shape.BOX
    obj1.shapes[0].dimensions = [float() for _ in range(3)]
    obj1.shapes[0].dimensions[0] = .1
    obj1.shapes[0].dimensions[1] = .1
    obj1.shapes[0].dimensions[2] = .75
    obj1.poses = [Pose() for _ in range(1)]
    obj1.poses[0].position.x = .6
    obj1.poses[0].position.y = -.6
    obj1.poses[0].position.z = .375
    obj1.poses[0].orientation.x = 0
    obj1.poses[0].orientation.y = 0
    obj1.poses[0].orientation.z = 0
    obj1.poses[0].orientation.w = 1

    att_obj = AttachedCollisionObject()
    att_obj.link_name = "r_gripper_palm_link"
    att_obj.touch_links = ['r_gripper_palm_link', 'r_gripper_r_finger_link', 'r_gripper_l_finger_link',
                           'r_gripper_r_finger_tip_link', 'r_gripper_l_finger_tip_link', 'r_wrist_roll_link', 'r_wrist_flex_link', 'r_forearm_link', 'r_gripper_motor_accelerometer_link']
    obj2 = CollisionObject()
    
    obj2.header.stamp = rospy.Time.now()
    obj2.header.frame_id = "r_gripper_palm_link"
    obj2.id = "obj2";
    obj2.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
    obj2.shapes = [Shape() for _ in range(1)]
    obj2.shapes[0].type = Shape.CYLINDER
    obj2.shapes[0].dimensions = [float() for _ in range(2)]
    obj2.shapes[0].dimensions[0] = .025
    obj2.shapes[0].dimensions[1] = .5
    obj2.poses = [Pose() for _ in range(1)]
    obj2.poses[0].position.x = .12
    obj2.poses[0].position.y = 0
    obj2.poses[0].position.z = 0
    obj2.poses[0].orientation.x = 0
    obj2.poses[0].orientation.y = 0
    obj2.poses[0].orientation.z = 0
    obj2.poses[0].orientation.w = 1
    att_obj.object = obj2
    r = rospy.Rate(.1)

    while(True):
        obj1.header.stamp = rospy.Time.now()
        obj_pub.publish(obj1)
        att_obj.object.header.stamp = rospy.Time.now()
        att_pub.publish(att_obj)
        r.sleep()
    def lift_object(self, goal):
        if self.action_server.is_preempt_requested():
            rospy.loginfo("%s: preempted" % ACTION_NAME)
            self.action_server.set_preempted()

        collision_support_surface_name = goal.collision_support_surface_name

        # disable collisions between grasped object and table
        collision_operation1 = CollisionOperation()
        collision_operation1.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS
        collision_operation1.object2 = collision_support_surface_name
        collision_operation1.operation = CollisionOperation.DISABLE

        # disable collisions between gripper and table
        collision_operation2 = CollisionOperation()
        collision_operation2.object1 = GRIPPER_GROUP_NAME
        collision_operation2.object2 = collision_support_surface_name
        collision_operation2.operation = CollisionOperation.DISABLE

        ordered_collision_operations = OrderedCollisionOperations()
        ordered_collision_operations.collision_operations = [collision_operation1, collision_operation2]

        gripper_paddings = [LinkPadding(l, 0.0) for l in GRIPPER_LINKS]

        # this is a hack to make arm lift an object faster
        obj = AttachedCollisionObject()
        obj.object.header.stamp = rospy.Time.now()
        obj.object.header.frame_id = GRIPPER_LINK_FRAME
        obj.object.operation.operation = CollisionObjectOperation.REMOVE
        obj.object.id = collision_support_surface_name
        obj.link_name = GRIPPER_LINK_FRAME
        obj.touch_links = GRIPPER_LINKS
        self.attached_object_pub.publish(obj)

        current_state = rospy.wait_for_message("l_arm_controller/state", FollowJointTrajectoryFeedback)
        joint_names = current_state.joint_names
        joint_positions = current_state.actual.positions
        start_angles = [joint_positions[joint_names.index(jn)] for jn in ARM_JOINTS]
        start_angles[0] = start_angles[0] - 0.3  # move shoulder up a bit

        if not move_arm_joint_goal(
            self.move_arm_client,
            ARM_JOINTS,
            start_angles,
            link_padding=gripper_paddings,
            collision_operations=ordered_collision_operations,
        ):
            self.action_server.set_aborted()
            return

        self.start_audio_recording_srv(InfomaxAction.LIFT, goal.category_id)

        if move_arm_joint_goal(
            self.move_arm_client,
            ARM_JOINTS,
            LIFT_POSITION,
            link_padding=gripper_paddings,
            collision_operations=ordered_collision_operations,
        ):
            # check if are still holding an object after lift is done
            grasp_status = self.get_grasp_status_srv()

            # if the object is still in hand after lift is done we are good
            if grasp_status.is_hand_occupied:
                sound_msg = self.stop_audio_recording_srv(True)
                self.action_server.set_succeeded(LiftObjectResult(sound_msg.recorded_sound))
                return

        self.stop_audio_recording_srv(False)
        rospy.logerr("%s: attempted lift failed" % ACTION_NAME)
        self.action_server.set_aborted()
        return
 def put_down_at(self, goal):
     if self.action_server.is_preempt_requested():
         rospy.loginfo('%s: preempted' % ACTION_NAME)
         self.action_server.set_preempted()
         
     bbox_center = self.find_cluster_bounding_box_center(goal.graspable_object.cluster)
     if not bbox_center: self.action_server.set_aborted()
     
     goal.target_location.z = bbox_center.z
     x_dist = goal.target_location.x - bbox_center.x
     y_dist = goal.target_location.y - bbox_center.y
     
     target = goal.graspable_object
     target.cluster.points = [Point32(p.x+x_dist, p.y+y_dist, p.z) for p in target.cluster.points]
     
     collision_object_name = goal.collision_object_name
     collision_support_surface_name = goal.collision_support_surface_name
     
     ik_solution = self.find_grasp_pose(target, collision_object_name, collision_support_surface_name)
     
     if ik_solution:
         # disable collisions between gripper and target object
         ordered_collision_operations = OrderedCollisionOperations()
         collision_operation = CollisionOperation()
         collision_operation.object1 = collision_object_name
         collision_operation.object2 = GRIPPER_GROUP_NAME
         collision_operation.operation = CollisionOperation.DISABLE
         
         # disable collisions between gripper and table
         collision_operation2 = CollisionOperation()
         collision_operation2.object1 = collision_support_surface_name
         collision_operation2.object2 = GRIPPER_GROUP_NAME
         collision_operation2.operation = CollisionOperation.DISABLE
         collision_operation2.penetration_distance = 0.01
         
         # disable collisions between arm and table
         collision_operation3 = CollisionOperation()
         collision_operation3.object1 = collision_support_surface_name
         collision_operation3.object2 = ARM_GROUP_NAME
         collision_operation3.operation = CollisionOperation.DISABLE
         collision_operation3.penetration_distance = 0.01
         
         ordered_collision_operations.collision_operations = [collision_operation, collision_operation2, collision_operation3]
         
         # set gripper padding to 0
         gripper_paddings = [LinkPadding(l,0.0) for l in GRIPPER_LINKS]
         gripper_paddings.extend([LinkPadding(l,0.0) for l in ARM_LINKS])
         
         # move into pre-grasp pose
         if not move_arm_joint_goal(self.move_arm_client,
                                    ik_solution.joint_state.name,
                                    ik_solution.joint_state.position,
                                    link_padding=gripper_paddings,
                                    collision_operations=ordered_collision_operations):
             rospy.logerr('%s: attempted grasp failed' % ACTION_NAME)
             self.action_server.set_aborted()
             return
             
         # record put down sound with 0.5 second padding before and after
         self.start_audio_recording_srv(InfomaxAction.GRASP, goal.category_id)
         rospy.sleep(0.5)
         self.open_gripper()
         rospy.sleep(0.5)
         sound_msg = self.stop_audio_recording_srv(True)
         
         obj = AttachedCollisionObject()
         obj.object.header.stamp = rospy.Time.now()
         obj.object.header.frame_id = GRIPPER_LINK_FRAME
         obj.object.operation.operation = CollisionObjectOperation.DETACH_AND_ADD_AS_OBJECT
         obj.object.id = collision_object_name
         obj.link_name = GRIPPER_LINK_FRAME
         obj.touch_links = GRIPPER_LINKS
         
         self.attached_object_pub.publish(obj)
         self.action_server.set_succeeded(PutDownAtResult(sound_msg.recorded_sound))
         return
         
     self.stop_audio_recording_srv(False)
     rospy.logerr('%s: attempted put down failed' % ACTION_NAME)
     self.action_server.set_aborted()