Exemple #1
0
    def get_collisions(self):
        '''
        creates list of collisions to be ignored.
        @rtype OrderedCollisionOperations
        @returns list of collisions to be ignored
        '''
        ignore = OrderedCollisionOperations()
        gripper = CollisionOperation()
        gripper.object1 = "l_end_effector"
        gripper.object2 = CollisionOperation.COLLISION_SET_ALL
        gripper.operation = CollisionOperation.DISABLE
        wrist = CollisionOperation()
        wrist.object1 = "l_wrist_roll_link"
        wrist.object2 = CollisionOperation.COLLISION_SET_ALL
        wrist.operation = CollisionOperation.DISABLE
        obj = CollisionOperation()
        obj.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS
        obj.object2 = "table"
        obj.operation = CollisionOperation.DISABLE
        map_obj = CollisionOperation()
        map_obj.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS
        map_obj.object2 = "collision_map"
        map_obj.operation = CollisionOperation.DISABLE
        lfing = CollisionOperation()
        lfing.object1 = "r_gripper_r_finger_tip_link"
        lfing.object2 = "table"
        lfing.operation = CollisionOperation.DISABLE
        rfingtip = CollisionOperation()
        rfingtip.object1 = "r_gripper_l_finger_tip_link"
        rfingtip.object2 = "table"
        rfingtip.operation = CollisionOperation.DISABLE
        rfing = CollisionOperation()
        rfing.object1 = "r_gripper_l_finger_link"
        rfing.object2 = "table"
        rfing.operation = CollisionOperation.DISABLE
        forearm = CollisionOperation()
        forearm.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS
        forearm.object2 = "l_forearm_link"
        forearm.operation = CollisionOperation.DISABLE
        flex = CollisionOperation()
        flex.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS
        flex.object2 = "l_wrist_flex_link"
        flex.operation = CollisionOperation.DISABLE

        ops = []
        op = CollisionOperation()
        op.operation = op.DISABLE
        op.object1 = op.COLLISION_SET_ATTACHED_OBJECTS
        for l in self.my_world.hands['left_arm'].hand_links:
            op.object2 = l
            ops.append(copy.deepcopy(op))

        ignore.collision_operations = [
            gripper, wrist, obj, map_obj, lfing, rfing, rfingtip, forearm, flex
        ] + ops
        return ignore
Exemple #2
0
    def reset_robot(self, tabletop_collision_map_processing_result=None):
        rospy.loginfo('resetting robot')
        ordered_collision_operations = OrderedCollisionOperations()

        if tabletop_collision_map_processing_result:
            closest_index = self.info[0][0]
            collision_object_name = tabletop_collision_map_processing_result.collision_object_names[
                closest_index]
            collision_support_surface_name = tabletop_collision_map_processing_result.collision_support_surface_name

            collision_operation = CollisionOperation()
            collision_operation.object1 = collision_support_surface_name
            collision_operation.object2 = ARM_GROUP_NAME
            collision_operation.operation = CollisionOperation.DISABLE
            collision_operation.penetration_distance = 0.02
            ordered_collision_operations.collision_operations = [
                collision_operation
            ]

            collision_operation = CollisionOperation()
            collision_operation.object1 = collision_support_surface_name
            collision_operation.object2 = GRIPPER_GROUP_NAME
            collision_operation.operation = CollisionOperation.DISABLE
            collision_operation.penetration_distance = 0.02
            ordered_collision_operations.collision_operations.append(
                collision_operation)

            collision_operation = CollisionOperation()
            collision_operation.object1 = collision_object_name
            collision_operation.object2 = GRIPPER_GROUP_NAME
            collision_operation.operation = CollisionOperation.DISABLE
            collision_operation.penetration_distance = 0.02
            ordered_collision_operations.collision_operations.append(
                collision_operation)
        else:
            self.reset_collision_map()

        current_state = find_current_arm_state()
        rospy.loginfo('arm is currently in %s state' % current_state)

        if current_state not in ['READY', 'TUCK']:
            # check if the gripper is empty
            grasp_status = self.get_grasp_status_srv()
            if grasp_status.is_hand_occupied:
                rospy.loginfo(
                    'arm is not in any of the following states %s, opening gripper'
                    % str(['READY', 'TUCK']))
                self.open_gripper()

        if current_state != 'READY' and not self.ready_arm(
                ordered_collision_operations):
            return False
        self.gentle_close_gripper()
        return True
    def remove_ordered_collisions(self, ordered_collisions):
        '''
        Removes the ordered collisions from the current diff.  

        This will ONLY remove ordered collisions by removing 
        them from the diff.  It can be used to exactly undo the effects of add_ordered_collisions, but cannot be
        used to change any collisions that were not set using add_ordered_collisions.  To enable or disable 
        collisions in the scene, use add_ordered_collisions.

        For example, if you used add_ordered_collisions to add a collision operation that removed all collisions with
        the right gripper, passing the same collision operation to this function will reset the collisions to exactly
        what they were before.  It will NOT enable the collisions of the gripper with everything.

        **Args:**
        
            **ordered_collisions (arm_navigation_msgs.msg.OrderedCollisionOperations):** ordered collisions to remove
        '''
        if not ordered_collisions or\
                not ordered_collisions.collision_operations:
            return
        newops = OrderedCollisionOperations()
        for o in self.current_diff.operations.collision_operations:
            doadd = True
            for a in ordered_collisions.collision_operations:
                if o.object1 == a.object1 and o.object2 == a.object2 and\
                        o.operation == a.operation:
                    doadd = False
                    break
            if doadd:
                newops.collision_operations.append(o)
        self.set_collisions(newops)
Exemple #4
0
 def reset_robot(self, tabletop_collision_map_processing_result=None):
     rospy.loginfo('resetting robot')
     ordered_collision_operations = OrderedCollisionOperations()
     
     if tabletop_collision_map_processing_result:
         closest_index = self.info[0][0]
         collision_object_name = tabletop_collision_map_processing_result.collision_object_names[closest_index]
         collision_support_surface_name = tabletop_collision_map_processing_result.collision_support_surface_name
         
         collision_operation = CollisionOperation()
         collision_operation.object1 = collision_support_surface_name
         collision_operation.object2 = ARM_GROUP_NAME
         collision_operation.operation = CollisionOperation.DISABLE
         collision_operation.penetration_distance = 0.02
         ordered_collision_operations.collision_operations = [collision_operation]
         
         collision_operation = CollisionOperation()
         collision_operation.object1 = collision_support_surface_name
         collision_operation.object2 = GRIPPER_GROUP_NAME
         collision_operation.operation = CollisionOperation.DISABLE
         collision_operation.penetration_distance = 0.02
         ordered_collision_operations.collision_operations.append(collision_operation)
         
         collision_operation = CollisionOperation()
         collision_operation.object1 = collision_object_name
         collision_operation.object2 = GRIPPER_GROUP_NAME
         collision_operation.operation = CollisionOperation.DISABLE
         collision_operation.penetration_distance = 0.02
         ordered_collision_operations.collision_operations.append(collision_operation)
     else:
         self.reset_collision_map()
         
     current_state = find_current_arm_state()
     rospy.loginfo('arm is currently in %s state' % current_state)
     
     if current_state not in ['READY', 'TUCK']:
         # check if the gripper is empty
         grasp_status = self.get_grasp_status_srv()
         if grasp_status.is_hand_occupied:
             rospy.loginfo('arm is not in any of the following states %s, opening gripper' % str(['READY', 'TUCK']))
             self.open_gripper()
             
     if current_state != 'READY' and not self.ready_arm(ordered_collision_operations): return False
     self.gentle_close_gripper()
     return True
Exemple #5
0
    def build_collision_operations(self, object1, object2):
        msg = OrderedCollisionOperations()
        collision = CollisionOperation()

        collision.operation = CollisionOperation.DISABLE
        collision.object1 = object1
        collision.object2 = object2
        msg.collision_operations.append(collision)
        return msg
    def ready_arm(self, collision_operations=OrderedCollisionOperations()):
        """
        Moves the arm to the side out of the view of all sensors. In this
        position the arm is ready to perform a grasp action.
        """
        goal = ReadyArmGoal()
        goal.collision_operations = collision_operations
        state = self.ready_arm_client.send_goal_and_wait(goal)

        return state == GoalStatus.SUCCEEDED
    def set_collisions(self, ordered_collisions):
        '''
        Sets the ordered collisions in the scene, removing any ordered collisions you passed previously.  

        If you pass in  None, this removes all ordered collisions.
        
        **Args:**
        
            **ordered_collisions (arm_navigation_msgs.msg.OrderedCollisionOperations):** ordered collisions
        '''
        if not ordered_collisions:
            self.current_diff.operations = OrderedCollisionOperations()
        else:
            self.current_diff.operations = ordered_collisions
        self.send_diff()
        return True
    def execute_trajectories(self, result):
        rospy.loginfo('There are ' + str(len(result.primitive_names)) +
                      ' segments')
        for (i, t) in enumerate(result.primitive_types):
            rospy.loginfo('Executing trajectory ' + str(i) + ' of type ' + t +
                          ' and length ' + str(
                              len(result.primitive_trajectories[i].
                                  joint_trajectory.points)))
            #print 'Trajectory is\n', result.primitive_trajectories[i]
            splits = t.split('-')
            if splits[0] == 'BaseTransit':
                self.execute_base_trajectory(result.primitive_trajectories[i])
                continue
            if splits[0] == 'BaseWarp':
                self.arm_tasks.move_arm_to_side('right_arm')
                self.arm_tasks.move_arm_to_side('left_arm')
                self.execute_warp_trajectory(result.primitive_trajectories[i])
                continue
            if splits[0] == 'ArmTransit':
                self.execute_arm_trajectory(splits[1],
                                            result.primitive_trajectories[i])
                continue
            #if splits[0] == 'Approach':
            #gripper.open()
            if splits[0] == 'Pickup':
                try:
                    self.grippers[splits[1]].close()
                except ActionFailedError:
                    pass
                self.wi.attach_object_to_gripper(splits[1], splits[2])
                #so the trajectory filter doesn't complain
                ops = OrderedCollisionOperations()
                op = CollisionOperation()
                op.operation = op.DISABLE

                op.object2 = splits[2]
                for t in self.wi.hands[splits[1]].touch_links:
                    op.object1 = t
                    ops.collision_operations.append(copy.deepcopy(op))
                self.psi.add_ordered_collisions(ops)
            self.execute_straight_line_arm_trajectory(
                splits[1], result.primitive_trajectories[i])
            if splits[0] == 'Place':
                self.grippers[splits[1]].open()
                self.wi.detach_and_add_back_attached_object(
                    splits[1], splits[2])
                self.psi.reset()
Exemple #9
0
def move_arm_joint_goal(move_arm_client,
                        joint_names,
                        joint_positions,
                        allowed_contacts=[],
                        link_padding=[],
                        collision_operations=OrderedCollisionOperations()):
    goal = MoveArmGoal()
    goal.planner_service_name = 'ompl_planning/plan_kinematic_path'
    goal.motion_plan_request.planner_id = ''
    goal.motion_plan_request.group_name = ARM_GROUP_NAME
    goal.motion_plan_request.num_planning_attempts = 3
    goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.0)
    goal.motion_plan_request.goal_constraints.joint_constraints = [
        JointConstraint(j, p, 0.1, 0.1, 0.0)
        for (j, p) in zip(joint_names, joint_positions)
    ]

    goal.planning_scene_diff.allowed_contacts = allowed_contacts
    goal.planning_scene_diff.link_padding = link_padding
    goal.operations = collision_operations

    move_arm_client.send_goal(goal)
    finished_within_time = move_arm_client.wait_for_result(
        rospy.Duration(200.0))

    if not finished_within_time:
        move_arm_client.cancel_goal()
        rospy.logerr('timed out trying to achieve joint goal')
        return False
    else:
        state = move_arm_client.get_state()

        if state == GoalStatus.SUCCEEDED:
            return True
        else:
            rospy.logerr(
                'failed to achieve joint goal (returned status code %s)' %
                str(state))
            return False
    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 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()
Exemple #13
0
def main():
    print 'Starting!'
    parser = _get_options_parser()
    (options, args) = parser.parse_args()
    print 'pause =', options.pause
    ss = SimSetup(options.world, False)
    world = ss.setup()
    planner = Planner()

    base_poses = get_base_poses(ss.wi, ss.world)
    arm_poses = get_arm_poses(ss.wi, ss.world)
    time = 0

    #note: these should all update the planning scene as we go
    #first plan to initial base position
    stime = planner.plan_base(base_poses.push)
    rospy.loginfo("Initial base move: " + str(stime))
    time += stime
    print 'pause =', options.pause
    if options.pause:
        raw_input()

    #now plan to approach to push
    stime = planner.plan_arm(arm_poses.approach_push)
    rospy.loginfo('Gross move to push: ' + str(stime))
    time += stime
    if options.pause:
        raw_input()

    ops = OrderedCollisionOperations()
    op = CollisionOperation()
    op.operation = op.DISABLE
    op.object1 = world.object.id
    op.object2 = world.object_support_surface
    ops.collision_operations.append(op)
    op = copy.deepcopy(op)
    op.object2 = 'r_end_effector'
    ops.collision_operations.append(op)
    op = copy.deepcopy(op)
    op.object1 = world.object_support_surface
    ops.collision_operations.append(op)

    stime = planner.plan_arm_interpolated(arm_poses.init_push,
                                          ordered_colls=ops)
    rospy.loginfo('Approach to push: ' + str(stime))
    time += stime
    if options.pause:
        raw_input()

    stime = planner.plan_arm_interpolated(arm_poses.final_push,
                                          ordered_colls=ops)
    rospy.loginfo('Push: ' + str(stime))
    time += stime
    co = copy.deepcopy(world.object)
    co.poses[0].position.x -= PUSH_DIST
    ss.wi.add_object(co)
    planner.psi.reset()
    if options.pause:
        raw_input()

    stime = planner.plan_arm_interpolated(arm_poses.retreat_push,
                                          ordered_colls=ops)
    rospy.loginfo('Retreat: ' + str(stime))
    time += stime
    if options.pause:
        raw_input()

    stime = planner.plan_base(base_poses.pick)
    rospy.loginfo('Base move to pick: ' + str(stime))
    time += stime
    if options.pause:
        raw_input()

    stime = planner.plan_arm(arm_poses.approach_pick)
    rospy.loginfo('Gross move to Pick: ' + str(stime))
    time += stime
    if options.pause:
        raw_input()

    stime = planner.plan_arm_interpolated(arm_poses.pick, ordered_colls=ops)
    rospy.loginfo('Approach to pick: ' + str(stime))
    time += stime
    if options.pause:
        raw_input()

    stime = planner.plan_arm_interpolated(arm_poses.lift, ordered_colls=ops)
    rospy.loginfo('Lift: ' + str(stime))
    time += stime
    if options.pause:
        raw_input()

    for g in base_poses.goal:
        stime = planner.plan_base(g)
        rospy.loginfo('Base move to goal: ' + str(stime))
        time += stime
        if options.pause:
            raw_input()

    stime = planner.plan_arm(arm_poses.goal)
    rospy.loginfo('Arm move to goal: ' + str(stime))
    time += stime
    if options.pause:
        raw_input()

    rospy.loginfo('TIME = ' + str(time))
Exemple #14
0
    def run(self):  #(goal, mass, prims, obs):
        search_start = time.time()
        obj_name = sys.argv[1]

        table_height = self.get_table_height()
        self.goal.pose.position.z = table_height + 0.01
        print "goal=" + str(self.goal)

        self.make_marker(self.goal,
                         "goal_pose",
                         mtype=Marker.ARROW,
                         color=[0.0, 1.0, 0.0, 0.8])

        obj = self.get_obj(obj_name)
        shape = self.get_shape(obj)

        obs = self.make_gripper_obs()

        mobj = Mobject(mass, shape, prims)

        pose = PoseStamped()
        pose.header.frame_id = "/torso_lift_link"
        pose.pose.position.x = goal.pose.position.x
        pose.pose.position.y = goal.pose.position.y
        pose.pose.position.z = goal.pose.position.z + 0.02  #.1
        pose.pose.orientation = goal.pose.orientation
        vels = [0.0, 0.0, 0.0]

        #        self.place('right_arm', obj_name, pose)

        print "DIMS=" + str(shape.dimensions)

        (release_pose, wrist_release) = self.get_best_release(mobj, obj, goal)
        #        release_pose = pose
        self.make_marker(release_pose, namespace="release_pose")
        print "release_pose=" + str(release_pose)
        search_end = time.time()

        # block_start = time.time()
        # dof = mobj.find_dof(goal, release_pose, vels)
        # print "DOF="+str(dof.degrees)

        # success = False
        # order = dof.index_order()
        # while not success: # not passive placing
        #     if len(order) != 0.0:
        #         next = order.pop(0)
        #         print "NEXT="+str(next)
        #         success = self.block(obs, next, dof[next], shape, goal)
        #         print "SUCCESS="+str(success)
        #     else:
        #         print "BLOCK FAILED"
        #         break

        # block_end = time.time()
        print "waiting for place:"
        #        raw_input()

        ignore = OrderedCollisionOperations()
        gripper = CollisionOperation()
        gripper.object1 = CollisionOperation.COLLISION_SET_ALL  #"l_end_effector"
        gripper.object2 = CollisionOperation.COLLISION_SET_ALL
        gripper.operation = CollisionOperation.DISABLE
        wrist = CollisionOperation()
        wrist.object1 = "l_wrist_roll_link"
        wrist.object2 = CollisionOperation.COLLISION_SET_ALL
        wrist.operation = CollisionOperation.DISABLE
        obj = CollisionOperation()
        obj.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS
        obj.object2 = "table"  #CollisionOperation.COLLISION_SET_ALL
        obj.operation = CollisionOperation.DISABLE
        ignore.collision_operations = [gripper, wrist, obj]
        self.move("right_arm", wrist_release, ignore)
        #        self.release("right_arm")
        #        self.place('right_arm', obj_name, release_pose)

        search = search_end - search_start
        block = 0.0  #block_end - block_start
        return (search, block)
    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()
Exemple #17
0
 
 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)
     
 rospy.loginfo('Pickup stage has successfully finished. Will place the object now')
 
 ############################################################################
 ####################### PLACE STAGE START HERE #############################
 
 listener = TransformListener()
Exemple #18
0
def move_arm_to_grasping_joint_pose(joint_names, joint_positions, allowed_contacts=[], link_padding=[], collision_operations=OrderedCollisionOperations()):
    move_arm_client = SimpleActionClient('move_left_arm', MoveArmAction)
    move_arm_client.wait_for_server()
    rospy.loginfo('connected to move_left_arm action server')
    
    goal = MoveArmGoal()
    goal.planner_service_name = 'ompl_planning/plan_kinematic_path'
    goal.motion_plan_request.planner_id = ''
    goal.motion_plan_request.group_name = 'left_arm'
    goal.motion_plan_request.num_planning_attempts = 1
    goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.0)
    goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint(j, p, 0.1, 0.1, 0.0) for (j,p) in zip(joint_names,joint_positions)]
    
    goal.motion_plan_request.allowed_contacts = allowed_contacts
    goal.motion_plan_request.link_padding = link_padding
    goal.motion_plan_request.ordered_collision_operations = collision_operations
    
    move_arm_client.send_goal(goal)
    finished_within_time = move_arm_client.wait_for_result(rospy.Duration(200.0))
    
    if not finished_within_time:
        move_arm_client.cancel_goal()
        rospy.loginfo('timed out trying to achieve a joint goal')
    else:
        state = move_arm_client.get_state()
        if state == GoalStatus.SUCCEEDED:
            rospy.loginfo('action finished: %s' % str(state))
            return True
        else:
            rospy.loginfo('action failed: %s' % str(state))
            return False
Exemple #19
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 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 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()
                               graspquat,
                               start_angles,
                               pos_spacing=0.02,
                               collision_aware=1,
                               collision_check_resolution=1,
                               steps_before_abort=-1)

    #print "using current start angles"
    #check_cartesian_path_lists(approachpos, approachquat, grasppos, graspquat, start_angles = [], pos_spacing = 0.02, collision_aware = 1, collision_check_resolution = 1, steps_before_abort = -1)

    print "ignoring collisions with all collision points"
    collision_oper = CollisionOperation(object1 = "points", \
                                        object2 = CollisionOperation.COLLISION_SET_ALL, \
                                        operation = CollisionOperation.DISABLE)
    ordered_collision_operations = OrderedCollisionOperations([
        collision_oper,
    ])
    check_cartesian_path_lists(
        approachpos,
        approachquat,
        grasppos,
        graspquat,
        start_angles,
        pos_spacing=0.02,
        collision_aware=1,
        collision_check_resolution=1,
        steps_before_abort=-1,
        ordered_collision_operations=ordered_collision_operations)

    print "abort early"
    check_cartesian_path_lists(approachpos,
 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 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()