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 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 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)
    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 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)
    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 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 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
 gripper_paddings = [LinkPadding(l,0.0) for l in ('L9_right_finger_link', 'L8_left_finger_link')]
 
 if not move_arm_to_grasping_joint_pose(ik_solution.joint_state.name, ik_solution.joint_state.position, allowed_contacts, gripper_paddings):
     exit(1)
     
 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
    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 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 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 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 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 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()