def append_pose_to_move_group_goal(goal_to_append=None, goal_pose=Pose(), link_name=None):
    """ Appends a pose to the given move_group goal, returns it appended
        Goals for now are for the same link TODO: let it be for different links"""
    if goal_to_append == None:
        rospy.logerr("append_pose_to_move_group_goal needs a goal!")
        return
    goal_to_append = MoveGroupGoal()
    goal_c = Constraints()
    position_c = PositionConstraint()
    position_c.header = goal_to_append.request.goal_constraints[0].header
    position_c.link_name = goal_to_append.request.goal_constraints[0].link_name
    position_c.constraint_region.primitives.append(SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01]))
    position_c.constraint_region.primitive_poses.append(goal_pose)
    position_c.weight = 1.0
    goal_c.position_constraints.append(position_c)
    orientation_c = OrientationConstraint()
    orientation_c.header = goal_to_append.request.goal_constraints[0].header
    orientation_c.link_name = goal_to_append.request.goal_constraints[0].link_name
    orientation_c.orientation = goal_pose.orientation
    orientation_c.absolute_x_axis_tolerance = 0.01
    orientation_c.absolute_y_axis_tolerance = 0.01
    orientation_c.absolute_z_axis_tolerance = 0.01
    orientation_c.weight = 1.0
    goal_c.orientation_constraints.append(orientation_c)
    goal_to_append.request.goal_constraints.append(goal_c)
    return goal_to_append
Ejemplo n.º 2
0
    def create_move_group_pose_goal(self, goal_pose=Pose(), group="manipulator", end_link_name=None, plan_only=True):

        header = Header()
        header.frame_id = 'torso_base_link'
        header.stamp = rospy.Time.now()

        moveit_goal = MoveGroupGoal()
        goal_c = Constraints()
        position_c = PositionConstraint()
        position_c.header = header
        if end_link_name != None: 
            position_c.link_name = end_link_name
        position_c.constraint_region.primitives.append(SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01])) 
        position_c.constraint_region.primitive_poses.append(goal_pose)
        position_c.weight = 1.0
        goal_c.position_constraints.append(position_c)
        orientation_c = OrientationConstraint()
        orientation_c.header = header
        if end_link_name != None:
            orientation_c.link_name = end_link_name
        orientation_c.orientation = goal_pose.orientation
        orientation_c.absolute_x_axis_tolerance = 0.01 
        orientation_c.absolute_y_axis_tolerance = 0.01
        orientation_c.absolute_z_axis_tolerance = 0.01
        orientation_c.weight = 1.0
        goal_c.orientation_constraints.append(orientation_c)
        moveit_goal.request.goal_constraints.append(goal_c)
        moveit_goal.request.num_planning_attempts = 5 
        moveit_goal.request.allowed_planning_time = 5.0
        moveit_goal.planning_options.plan_only = plan_only
        moveit_goal.planning_options.planning_scene_diff.is_diff = True # Necessary
        moveit_goal.request.group_name = group
    
        return moveit_goal
def append_traj_to_move_group_goal(goal_to_append=None, goal_pose=Pose(), link_name=None):
    """ Appends a trajectory_point to the given move_group goal, returns it appended"""
    if goal_to_append == None:
        rospy.logerr("append_trajectory_point_to_move_group_goal needs a goal!")
        return
    #goal_to_append = MoveGroupGoal()
    #rospy.logwarn("goal_to_append is: \n" + str(goal_to_append))
    traj_c = TrajectoryConstraints()
    goal_c = Constraints()
    goal_c.name = "traj_constraint"
    # Position constraint
    position_c = PositionConstraint()
    position_c.header = goal_to_append.request.goal_constraints[0].position_constraints[0].header
    position_c.link_name = goal_to_append.request.goal_constraints[0].position_constraints[0].link_name if link_name == None else link_name
    position_c.constraint_region.primitives.append(SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01]))
    position_c.constraint_region.primitive_poses.append(goal_pose)
    position_c.weight = 2.0
    goal_c.position_constraints.append(position_c)
    # Orientation constraint
    orientation_c = OrientationConstraint()
    orientation_c.header = goal_to_append.request.goal_constraints[0].position_constraints[0].header
    orientation_c.link_name = goal_to_append.request.goal_constraints[0].position_constraints[0].link_name if link_name == None else link_name
    orientation_c.orientation = goal_pose.orientation
    orientation_c.absolute_x_axis_tolerance = 0.01
    orientation_c.absolute_y_axis_tolerance = 0.01
    orientation_c.absolute_z_axis_tolerance = 0.01
    orientation_c.weight = 1.0
    goal_c.orientation_constraints.append(orientation_c)
    
    traj_c.constraints.append(goal_c)
    goal_to_append.request.trajectory_constraints = traj_c
    
    return goal_to_append
def createPositionConstraint():
    position_constraints = []
    p = PositionConstraint()
    p.header.frame_id = 'base_link'
    p.link_name = 'arm_right_tool_link'
    p.target_point_offset.x = 0.5
    p.target_point_offset.y = -0.5
    p.target_point_offset.z = 1.0
    p.weight = 1.0
    position_constraints.append(p)
    return position_constraints
Ejemplo n.º 5
0
 def execute(self, userdata):
     # Prepare the position
     goal_pose = Pose()
     goal_pose.position = userdata.manip_goal_pose
     # Set the rotation of the tool link, all 0 means for the right hand palm looking left straight
     # roll = rotation over X, pitch = rotation over Y, yaw = rotation over Z
     quat = quaternion_from_euler(0.0, 0.0, 0.0) # roll, pitch, yaw
     goal_pose.orientation = Quaternion(*quat.tolist())
     
     header = Header()
     header.frame_id = 'base_link'
     header.stamp = rospy.Time.now()
     
     userdata.move_it_goal = MoveGroupGoal()
     goal_c = Constraints()
     position_c = PositionConstraint()
     position_c.header = header
     
     if userdata.manip_end_link != None: # For some groups the end_link_name can be deduced, but better add it manually
         position_c.link_name = userdata.manip_end_link
         
     # how big is the area where the end effector can be
     position_c.constraint_region.primitives.append(SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01])) 
     position_c.constraint_region.primitive_poses.append(goal_pose)
     
     position_c.weight = 1.0
     
     goal_c.position_constraints.append(position_c)
     
     orientation_c = OrientationConstraint()
     orientation_c.header = header
     if userdata.manip_end_link != None:
         orientation_c.link_name = userdata.manip_end_link
     orientation_c.orientation = goal_pose.orientation
     
     # Tolerances, MoveIt! by default uses 0.001 which may be too low sometimes
     orientation_c.absolute_x_axis_tolerance = 0.01
     orientation_c.absolute_y_axis_tolerance = 0.01
     orientation_c.absolute_z_axis_tolerance = 0.01
     
     orientation_c.weight = 1.0
     
     goal_c.orientation_constraints.append(orientation_c)
     
     userdata.move_it_goal.request.goal_constraints.append(goal_c)
     userdata.move_it_goal.request.num_planning_attempts = 5 # The number of times this plan is to be computed. Shortest solution will be reported.
     userdata.move_it_goal.request.allowed_planning_time = 5.0
     userdata.move_it_goal.planning_options.plan_only = False # True: Plan-Only..... False : Plan + execute
     userdata.move_it_goal.planning_options.planning_scene_diff.is_diff = True # Necessary
     userdata.move_it_goal.request.group_name = userdata.manip_group
     
     return 'succeeded'
Ejemplo n.º 6
0
def _to_pose_constraint(pose,
                        reference_frame,
                        link_name,
                        position_tolerance=_DEFAULT_POSITION_TOLERANCE):
    """Returns an position constraint suitable for ActionGoal's."""
    pos_con = PositionConstraint()
    pos_con.header.frame_id = reference_frame
    pos_con.link_name = link_name
    pos_con.constraint_region.primitive_poses.append(pose)
    pos_con.weight = 1

    region = shape_msgs.SolidPrimitive()
    region.type = shape_msgs.SolidPrimitive.SPHERE
    region.dimensions.append(position_tolerance)

    pos_con.constraint_region.primitives.append(region)

    return pos_con
Ejemplo n.º 7
0
def create_move_group_pose_goal(goal_pose=Pose(), group="right_arm_torso", end_link_name=None, plan_only=True):
    """ Creates a move_group goal based on pose.
    @arg group string representing the move_group group to use
    @arg end_link_name string representing the ending link to use
    @arg goal_pose Pose() representing the goal pose
    @arg plan_only bool to for only planning or planning and executing
    @returns MoveGroupGoal with the data given on the arguments"""
    
    header = Header()
    header.frame_id = 'base_link'
    header.stamp = rospy.Time.now()
    # We are filling in the MoveGroupGoal a MotionPlanRequest and a PlanningOptions message
    # http://docs.ros.org/hydro/api/moveit_msgs/html/msg/MotionPlanRequest.html
    # http://docs.ros.org/hydro/api/moveit_msgs/html/msg/PlanningOptions.html
    moveit_goal = MoveGroupGoal()
    goal_c = Constraints()
    position_c = PositionConstraint()
    position_c.header = header
    if end_link_name != None: # For some groups the end_link_name can be deduced, but better add it manually
        position_c.link_name = end_link_name
    position_c.constraint_region.primitives.append(SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01])) # how big is the area where the end effector can be
    position_c.constraint_region.primitive_poses.append(goal_pose)
    position_c.weight = 1.0
    goal_c.position_constraints.append(position_c)
    orientation_c = OrientationConstraint()
    orientation_c.header = header
    if end_link_name != None:
        orientation_c.link_name = end_link_name
    orientation_c.orientation = goal_pose.orientation
    orientation_c.absolute_x_axis_tolerance = 0.01 # Tolerances, MoveIt! by default uses 0.001 which may be too low sometimes
    orientation_c.absolute_y_axis_tolerance = 0.01
    orientation_c.absolute_z_axis_tolerance = 0.01
    orientation_c.weight = 1.0
    goal_c.orientation_constraints.append(orientation_c)
    moveit_goal.request.goal_constraints.append(goal_c)
    moveit_goal.request.num_planning_attempts = 1 # The number of times this plan is to be computed. Shortest solution will be reported.
    moveit_goal.request.allowed_planning_time = 5.0
    moveit_goal.planning_options.plan_only = plan_only
    moveit_goal.planning_options.planning_scene_diff.is_diff = True # Necessary
    moveit_goal.request.group_name = group
    
    return moveit_goal
Ejemplo n.º 8
0
def create_move_group_pose_goal(goal_pose=Pose(), group="right_arm_torso", end_link_name=None, plan_only=True):
    """ Creates a move_group goal based on pose.
    @arg group string representing the move_group group to use
    @arg end_link_name string representing the ending link to use
    @arg goal_pose Pose() representing the goal pose
    @arg plan_only bool to for only planning or planning and executing
    @returns MoveGroupGoal with the data given on the arguments"""
    
    header = Header()
    header.frame_id = 'base_link'
    header.stamp = rospy.Time.now()
    # We are filling in the MoveGroupGoal a MotionPlanRequest and a PlanningOptions message
    # http://docs.ros.org/hydro/api/moveit_msgs/html/msg/MotionPlanRequest.html
    # http://docs.ros.org/hydro/api/moveit_msgs/html/msg/PlanningOptions.html
    moveit_goal = MoveGroupGoal()
    goal_c = Constraints()
    position_c = PositionConstraint()
    position_c.header = header
    if end_link_name != None: # For some groups the end_link_name can be deduced, but better add it manually
        position_c.link_name = end_link_name
    position_c.constraint_region.primitives.append(SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01])) # how big is the area where the end effector can be
    position_c.constraint_region.primitive_poses.append(goal_pose)
    position_c.weight = 1.0
    goal_c.position_constraints.append(position_c)
    orientation_c = OrientationConstraint()
    orientation_c.header = header
    if end_link_name != None:
        orientation_c.link_name = end_link_name
    orientation_c.orientation = goal_pose.orientation
    orientation_c.absolute_x_axis_tolerance = 0.01 # Tolerances, MoveIt! by default uses 0.001 which may be too low sometimes
    orientation_c.absolute_y_axis_tolerance = 0.01
    orientation_c.absolute_z_axis_tolerance = 0.01
    orientation_c.weight = 1.0
    goal_c.orientation_constraints.append(orientation_c)
    moveit_goal.request.goal_constraints.append(goal_c)
    moveit_goal.request.num_planning_attempts = 5 # The number of times this plan is to be computed. Shortest solution will be reported.
    moveit_goal.request.allowed_planning_time = 5.0
    moveit_goal.planning_options.plan_only = plan_only
    moveit_goal.planning_options.planning_scene_diff.is_diff = True # Necessary
    moveit_goal.request.group_name = group
    
    return moveit_goal
def append_traj_to_move_group_goal(goal_to_append=None,
                                   goal_pose=Pose(),
                                   link_name=None):
    """ Appends a trajectory_point to the given move_group goal, returns it appended"""
    if goal_to_append == None:
        rospy.logerr(
            "append_trajectory_point_to_move_group_goal needs a goal!")
        return
    #goal_to_append = MoveGroupGoal()
    #rospy.logwarn("goal_to_append is: \n" + str(goal_to_append))
    traj_c = TrajectoryConstraints()
    goal_c = Constraints()
    goal_c.name = "traj_constraint"
    # Position constraint
    position_c = PositionConstraint()
    position_c.header = goal_to_append.request.goal_constraints[
        0].position_constraints[0].header
    position_c.link_name = goal_to_append.request.goal_constraints[
        0].position_constraints[0].link_name if link_name == None else link_name
    position_c.constraint_region.primitives.append(
        SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01]))
    position_c.constraint_region.primitive_poses.append(goal_pose)
    position_c.weight = 2.0
    goal_c.position_constraints.append(position_c)
    # Orientation constraint
    orientation_c = OrientationConstraint()
    orientation_c.header = goal_to_append.request.goal_constraints[
        0].position_constraints[0].header
    orientation_c.link_name = goal_to_append.request.goal_constraints[
        0].position_constraints[0].link_name if link_name == None else link_name
    orientation_c.orientation = goal_pose.orientation
    orientation_c.absolute_x_axis_tolerance = 0.01
    orientation_c.absolute_y_axis_tolerance = 0.01
    orientation_c.absolute_z_axis_tolerance = 0.01
    orientation_c.weight = 1.0
    goal_c.orientation_constraints.append(orientation_c)

    traj_c.constraints.append(goal_c)
    goal_to_append.request.trajectory_constraints = traj_c

    return goal_to_append
Ejemplo n.º 10
0
    def create_move_group_pose_goal(self,
                                    goal_pose=Pose(),
                                    group="right_arm",
                                    end_link_name="arm_right_tool_link",
                                    plan_only=True):
        """ Creates a move_group goal based on pose.
        @arg group string representing the move_group group to use
        @arg end_link_name string representing the ending link to use
        @arg goal_pose Pose() representing the goal pose"""

        # Specifying the header makes the planning fail...
        header = Header()
        header.frame_id = 'base_link'
        header.stamp = rospy.Time.now()
        moveit_goal = MoveGroupGoal()
        goal_c = Constraints()
        position_c = PositionConstraint()
        position_c.header = header
        position_c.link_name = end_link_name
        position_c.constraint_region.primitives.append(
            SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01]))
        position_c.constraint_region.primitive_poses.append(goal_pose)
        position_c.weight = 1.0
        goal_c.position_constraints.append(position_c)
        orientation_c = OrientationConstraint()
        orientation_c.header = header
        orientation_c.link_name = end_link_name
        orientation_c.orientation = goal_pose.orientation
        orientation_c.absolute_x_axis_tolerance = 0.01
        orientation_c.absolute_y_axis_tolerance = 0.01
        orientation_c.absolute_z_axis_tolerance = 0.01
        orientation_c.weight = 1.0
        goal_c.orientation_constraints.append(orientation_c)
        moveit_goal.request.goal_constraints.append(goal_c)
        moveit_goal.request.num_planning_attempts = 3
        moveit_goal.request.allowed_planning_time = 1.0
        moveit_goal.planning_options.plan_only = plan_only
        moveit_goal.planning_options.planning_scene_diff.is_diff = True
        moveit_goal.request.group_name = group

        return moveit_goal
Ejemplo n.º 11
0
    def create_move_group_pose_goal(self,
                                    goal_pose=Pose(),
                                    group="manipulator",
                                    end_link_name=None,
                                    plan_only=True):

        header = Header()
        header.frame_id = 'base_link'
        header.stamp = rospy.Time.now()

        moveit_goal = MoveGroupGoal()
        goal_c = Constraints()
        position_c = PositionConstraint()
        position_c.header = header
        if end_link_name != None:
            position_c.link_name = end_link_name
        position_c.constraint_region.primitives.append(
            SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.1]))
        position_c.constraint_region.primitive_poses.append(goal_pose)
        position_c.weight = 1.0
        goal_c.position_constraints.append(position_c)
        orientation_c = OrientationConstraint()
        orientation_c.header = header
        if end_link_name != None:
            orientation_c.link_name = end_link_name
        orientation_c.orientation = goal_pose.orientation
        orientation_c.absolute_x_axis_tolerance = 0.01
        orientation_c.absolute_y_axis_tolerance = 0.01
        orientation_c.absolute_z_axis_tolerance = 0.01
        orientation_c.weight = 0.01
        goal_c.orientation_constraints.append(orientation_c)
        moveit_goal.request.goal_constraints.append(goal_c)
        moveit_goal.request.num_planning_attempts = 5
        moveit_goal.request.allowed_planning_time = 5.0
        moveit_goal.planning_options.plan_only = plan_only
        moveit_goal.planning_options.planning_scene_diff.is_diff = True  # Necessary
        moveit_goal.request.group_name = group

        return moveit_goal
    def create_move_group_pose_goal(
        self, goal_pose=Pose(), group="left_arm", end_link_name="arm_left_tool_link", plan_only=True
    ):
        """ Creates a move_group goal based on pose.
        @arg group string representing the move_group group to use
        @arg end_link_name string representing the ending link to use
        @arg goal_pose Pose() representing the goal pose"""

        # Specifying the header makes the planning fail...
        header = Header()
        header.frame_id = "base_link"
        header.stamp = rospy.Time.now()
        moveit_goal = MoveGroupGoal()
        goal_c = Constraints()
        position_c = PositionConstraint()
        position_c.header = header
        position_c.link_name = end_link_name
        position_c.constraint_region.primitives.append(SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01]))
        position_c.constraint_region.primitive_poses.append(goal_pose)
        position_c.weight = 1.0
        goal_c.position_constraints.append(position_c)
        orientation_c = OrientationConstraint()
        orientation_c.header = header
        orientation_c.link_name = end_link_name
        orientation_c.orientation = goal_pose.orientation
        orientation_c.absolute_x_axis_tolerance = 0.01
        orientation_c.absolute_y_axis_tolerance = 0.01
        orientation_c.absolute_z_axis_tolerance = 0.01
        orientation_c.weight = 1.0
        goal_c.orientation_constraints.append(orientation_c)
        moveit_goal.request.goal_constraints.append(goal_c)
        moveit_goal.request.num_planning_attempts = 3
        moveit_goal.request.allowed_planning_time = 1.0
        moveit_goal.planning_options.plan_only = plan_only
        moveit_goal.planning_options.planning_scene_diff.is_diff = True
        moveit_goal.request.group_name = group

        return moveit_goal
def make_default_action_goal(object, pose, orientation=True):
    action_goal = MoveGroupGoal()
    action_goal.request.group_name = object._move_group
    action_goal.request.start_state.is_diff = True
    action_goal.request.planner_id = "PRMkConfigDefault"

    goal_constraint = Constraints()

    goal_constraint.position_constraints.append(PositionConstraint())
    goal_constraint.position_constraints[
        0].header.frame_id = object.planning_frame
    goal_constraint.position_constraints[0].link_name = object.eef_link
    bounding_volume = BoundingVolume()
    solid_primitive = SolidPrimitive()
    solid_primitive.dimensions = [object._tolerance * object._tolerance]
    solid_primitive.type = solid_primitive.SPHERE
    bounding_volume.primitives.append(solid_primitive)
    bounding_volume.primitive_poses.append(pose)
    goal_constraint.position_constraints[0].constraint_region = bounding_volume
    goal_constraint.position_constraints[0].weight = 1.0
    if orientation is not False:
        orientation_constraint = OrientationConstraint()
        orientation_constraint.header.frame_id = object.planning_frame
        orientation_constraint.orientation = pose.orientation
        orientation_constraint.link_name = object.eef_link
        orientation_constraint.absolute_x_axis_tolerance = object._tolerance
        orientation_constraint.absolute_y_axis_tolerance = object._tolerance
        orientation_constraint.absolute_z_axis_tolerance = object._tolerance
        orientation_constraint.weight = 1.0
        goal_constraint.orientation_constraints.append(orientation_constraint)
    action_goal.request.goal_constraints.append(goal_constraint)
    action_goal.request.num_planning_attempts = 5
    action_goal.request.allowed_planning_time = 5
    action_goal.request.workspace_parameters.header.frame_id = object.planning_frame
    action_goal.request.workspace_parameters.min_corner.x = -20
    action_goal.request.workspace_parameters.min_corner.y = -20
    action_goal.request.workspace_parameters.min_corner.z = -20
    action_goal.request.workspace_parameters.max_corner.x = 20
    action_goal.request.workspace_parameters.max_corner.y = 20
    action_goal.request.workspace_parameters.min_corner.z = 20
    action_goal.planning_options.look_around = False
    action_goal.planning_options.replan = True
    return action_goal
Ejemplo n.º 14
0
    def motionPlanToPose(self,
                         pose,
                         tolerance=0.01,
                         wait=True,
                         **kwargs):
        '''
        Move the arm to set of joint position goals

        :param joints: joint names for which the target position
                is specified.
        :param positions: target joint positions
        :param tolerance: allowed tolerance in the final joint positions.
        :param wait: if enabled, makes the fuctions wait until the
            target joint position is reached

        :type joints: list of string element type
        :type positions: list of float element type
        :type tolerance: float
        :type wait: bool
        '''

        # Check arguments
        supported_args = ("max_velocity_scaling_factor",
                          "max_acceleration_scaling_factor",
                          "planner_id",
                          "planning_scene_diff",
                          "planning_time",
                          "plan_only",
                          "start_state")
        for arg in kwargs.keys():
            if not arg in supported_args:
                rospy.loginfo("motionPlanToPose: unsupported argument: %s",
                              arg)

        # Create goal
        g = MotionPlanRequest()
        
        # 1. fill in request workspace_parameters

        # 2. fill in request start_state
        try:
            g.start_state = kwargs["start_state"]
        except KeyError:
            g.start_state.is_diff = True

        # 3. fill in request goal_constraints
        c1 = Constraints()

        c1.position_constraints.append(PositionConstraint())
        c1.position_constraints[0].header.frame_id = self._fixed_frame
        c1.position_constraints[0].link_name = self._gripper_frame
        # c1.position_constraints[0].target_point_offset
        b = BoundingVolume()
        s = SolidPrimitive()
        s.dimensions = [tolerance]
        s.type = s.SPHERE
        b.primitives.append(s)
        b.primitive_poses.append(pose)
        c1.position_constraints[0].constraint_region = b
        c1.position_constraints[0].weight = 1.0

        c1.orientation_constraints.append(OrientationConstraint())
        c1.orientation_constraints[0].header.frame_id = self._fixed_frame
        c1.orientation_constraints[0].orientation = pose.orientation
        c1.orientation_constraints[0].link_name = self._gripper_frame
        c1.orientation_constraints[0].absolute_x_axis_tolerance = tolerance
        c1.orientation_constraints[0].absolute_y_axis_tolerance = tolerance
        c1.orientation_constraints[0].absolute_z_axis_tolerance = tolerance
        c1.orientation_constraints[0].weight = 1.0

        g.goal_constraints.append(c1)

        # 4. fill in request path constraints

        # 5. fill in request trajectory constraints

        # 6. fill in request planner id
        try:
            g.planner_id = kwargs["planner_id"]
        except KeyError:
            if self.planner_id:
                g.planner_id = self.planner_id

        # 7. fill in request group name
        g.group_name = self._group

        # 8. fill in request number of planning attempts
        try:
            g.num_planning_attempts = kwargs["num_attempts"]
        except KeyError:
            g.num_planning_attempts = 1

        # 9. fill in request allowed planning time
        try:
            g.allowed_planning_time = kwargs["planning_time"]
        except KeyError:
            g.allowed_planning_time = self.planning_time

        # 10. Fill in velocity scaling factor
        try:
            g.max_velocity_scaling_factor = kwargs["max_velocity_scaling_factor"]
        except KeyError:
            pass  # do not fill in at all

         # 11. Fill in acceleration scaling factor
        try:
            g.max_velocity_scaling_factor = kwargs["max_acceleration_scaling_factor"]
        except KeyError:
            pass  # do not fill in at all

        result = self._mp_service(g)
        traj = result.motion_plan_response.trajectory.joint_trajectory.points
        if len(traj) < 1:
            rospy.logwarn('No motion plan found.')
            return None
        return traj
Ejemplo n.º 15
0
    def moveToPose(self,
                   pose_stamped,
                   gripper_frame,
                   tolerance=0.01,
                   wait=True,
                   **kwargs):
        # Check arguments
        supported_args = ("max_velocity_scaling_factor",
                          "planner_id",
                          "planning_time",
                          "plan_only",
                          "start_state")
        for arg in kwargs.keys():
            if not arg in supported_args:
                rospy.loginfo("moveToJointPosition: unsupported argument: %s",
                              arg)

        # Create goal
        g = MoveGroupGoal()
        pose_transformed = self._listener.transformPose(self._fixed_frame, pose_stamped)

        # 1. fill in request workspace_parameters

        # 2. fill in request start_state
        try:
            g.request.start_state = kwargs["start_state"]
        except KeyError:
            g.request.start_state.is_diff = True

        # 3. fill in request goal_constraints
        c1 = Constraints()

        c1.position_constraints.append(PositionConstraint())
        c1.position_constraints[0].header.frame_id = self._fixed_frame
        c1.position_constraints[0].link_name = gripper_frame
        b = BoundingVolume()
        s = SolidPrimitive()
        s.dimensions = [tolerance * tolerance]
        s.type = s.SPHERE
        b.primitives.append(s)
        b.primitive_poses.append(pose_transformed.pose)
        c1.position_constraints[0].constraint_region = b
        c1.position_constraints[0].weight = 1.0

        c1.orientation_constraints.append(OrientationConstraint())
        c1.orientation_constraints[0].header.frame_id = self._fixed_frame
        c1.orientation_constraints[0].orientation = pose_transformed.pose.orientation
        c1.orientation_constraints[0].link_name = gripper_frame
        c1.orientation_constraints[0].absolute_x_axis_tolerance = tolerance
        c1.orientation_constraints[0].absolute_y_axis_tolerance = tolerance
        c1.orientation_constraints[0].absolute_z_axis_tolerance = tolerance
        c1.orientation_constraints[0].weight = 1.0

        g.request.goal_constraints.append(c1)

        # 4. fill in request path constraints

        # 5. fill in request trajectory constraints

        # 6. fill in request planner id
        try:
            g.request.planner_id = kwargs["planner_id"]
        except KeyError:
            if self.planner_id:
                g.request.planner_id = self.planner_id

        # 7. fill in request group name
        g.request.group_name = self._group

        # 8. fill in request number of planning attempts
        try:
            g.request.num_planning_attempts = kwargs["num_attempts"]
        except KeyError:
            g.request.num_planning_attempts = 1

        # 9. fill in request allowed planning time
        try:
            g.request.allowed_planning_time = kwargs["planning_time"]
        except KeyError:
            g.request.allowed_planning_time = self.planning_time

        # Fill in velocity scaling factor
        try:
            g.request.max_velocity_scaling_factor = kwargs["max_velocity_scaling_factor"]
        except KeyError:
            pass  # do not fill in at all

        # 10. fill in planning options diff
        g.planning_options.planning_scene_diff.is_diff = True
        g.planning_options.planning_scene_diff.robot_state.is_diff = True

        # 11. fill in planning options plan only
        try:
            g.planning_options.plan_only = kwargs["plan_only"]
        except KeyError:
            g.planning_options.plan_only = self.plan_only

        # 12. fill in other planning options
        g.planning_options.look_around = False
        g.planning_options.replan = False

        # 13. send goal
        self._action.send_goal(g)
        if wait:
            self._action.wait_for_result()
            return self._action.get_result()
        else:
            return None
Ejemplo n.º 16
0
    def left_arm_go_to_pose_goal(self):
        # 设置动作对象变量,此处为arm
        left_arm = self.left_arm

        # 获取当前末端执行器位置姿态
        pose_goal = left_arm.get_current_pose().pose

        # 限制末端夹具运动
        left_joint_const = JointConstraint()
        left_joint_const.joint_name = "gripper_l_joint_r"
        if Leftfinger > -32:
            left_joint_const.position = 0.024
        else:
            left_joint_const.position = 0
        left_joint_const.weight = 1.0

        # 限制末端位移
        left_position_const = PositionConstraint()
        left_position_const.header = Header()
        left_position_const.link_name = "gripper_l_joint_r"
        left_position_const.target_point_offset.x = 0.5
        left_position_const.target_point_offset.y = -0.5
        left_position_const.target_point_offset.z = 1.0
        left_position_const.weight = 1.0

        # # 限制1轴转动
        left_joint1_const = JointConstraint()
        left_joint1_const.joint_name = "yumi_joint_1_l"
        left_joint1_const.position = 0
        left_joint1_const.tolerance_above = 1.76
        left_joint1_const.tolerance_below = 0
        left_position_const.weight = 1.0

        # 限制2轴转动
        left_joint2_const = JointConstraint()
        left_joint2_const.joint_name = "yumi_joint_2_l"
        left_joint2_const.position = 0
        left_joint2_const.tolerance_above = 0
        left_joint2_const.tolerance_below = 150
        left_joint2_const.weight = 1.0

        # 限制3轴转动
        left_joint3_const = JointConstraint()
        left_joint3_const.joint_name = "yumi_joint_3_l"
        left_joint3_const.position = 0
        left_joint3_const.tolerance_above = 35
        left_joint3_const.tolerance_below = 55
        left_joint3_const.weight = 1.0

        # 限制4轴转动
        left_joint4_const = JointConstraint()
        left_joint4_const.joint_name = "yumi_joint_4_l"
        left_joint4_const.position = 0
        left_joint4_const.tolerance_above = 60
        left_joint4_const.tolerance_below = 75
        left_joint4_const.weight = 1.0

        # 限制5轴转动
        right_joint5_const = JointConstraint()
        right_joint5_const.joint_name = "yumi_joint_5_l"
        right_joint5_const.position = 40
        right_joint5_const.tolerance_above = 50
        right_joint5_const.tolerance_below = 20
        right_joint5_const.weight = 1.0

        # 限制6轴转动
        left_joint6_const = JointConstraint()
        left_joint6_const.joint_name = "yumi_joint_6_l"
        left_joint6_const.position = 0
        left_joint6_const.tolerance_above = 10
        left_joint6_const.tolerance_below = 35
        left_joint6_const.weight = 1.0

        # 限制7轴转动
        left_joint7_const = JointConstraint()
        left_joint7_const.joint_name = "yumi_joint_7_l"
        left_joint7_const.position = -10
        left_joint7_const.tolerance_above = 0
        left_joint7_const.tolerance_below = 160
        left_joint7_const.weight = 1.0

        # 限制末端位移
        left_position_const = PositionConstraint()
        left_position_const.header = Header()
        left_position_const.link_name = "gripper_l_joint_r"
        left_position_const.target_point_offset.x = 0.5
        left_position_const.target_point_offset.y = 0.25
        left_position_const.target_point_offset.z = 0.5
        left_position_const.weight = 1.0

        # 添加末端姿态约束:
        left_orientation_const = OrientationConstraint()
        left_orientation_const.header = Header()
        left_orientation_const.orientation = pose_goal.orientation
        left_orientation_const.link_name = "gripper_l_finger_r"
        left_orientation_const.absolute_x_axis_tolerance = 0.5
        left_orientation_const.absolute_y_axis_tolerance = 0.25
        left_orientation_const.absolute_z_axis_tolerance = 0.5
        left_orientation_const.weight = 1

        # 施加全约束
        consts = Constraints()
        consts.joint_constraints = [left_joint_const]
        # consts.orientation_constraints = [left_orientation_const]
        # consts.position_constraints = [left_position_const]
        left_arm.set_path_constraints(consts)

        # 设置动作对象目标位置姿态
        pose_goal.orientation.x = Left_Qux
        pose_goal.orientation.y = Left_Quy
        pose_goal.orientation.z = Left_Quz
        pose_goal.orientation.w = Left_Quw
        pose_goal.position.x = (Neurondata[11] - 0.05) * 1.48 + 0.053
        pose_goal.position.y = (Neurondata[9] - 0.18) * 1.48 + 0.12
        pose_goal.position.z = (Neurondata[10] - 0.53) * 1.48 + 0.47
        left_arm.set_pose_target(pose_goal)
        print "End effector pose %s" % pose_goal

        # # 设置动作对象目标位置姿态
        # pose_goal.orientation.x = pose_goal.orientation.x
        # pose_goal.orientation.y = pose_goal.orientation.y
        # pose_goal.orientation.z = pose_goal.orientation.z
        # pose_goal.orientation.w = pose_goal.orientation.w
        # pose_goal.position.x = pose_goal.position.x
        # pose_goal.position.y = pose_goal.position.y - 0.01
        # pose_goal.position.z = pose_goal.position.z
        # left_arm.set_pose_target(pose_goal)
        # print "End effector pose %s" % pose_goal

        # 规划和输出动作
        traj = left_arm.plan()
        left_arm.execute(traj, wait=False)
        # 动作完成后清除目标信息
        left_arm.clear_pose_targets()
        # 清除路径约束
        left_arm.clear_path_constraints()
        # 确保没有剩余未完成动作在执行
        left_arm.stop()
Ejemplo n.º 17
0
    def right_arm_go_to_pose_goal(self):
        # 设置动作对象变量,此处为arm
        right_arm = self.right_arm

        # 获取当前末端执行器位置姿态
        pose_goal = right_arm.get_current_pose().pose

        # 限制末端夹具运动
        right_joint_const = JointConstraint()
        right_joint_const.joint_name = "gripper_r_joint_r"
        if Rightfinger > -55:
            right_joint_const.position = 0.024
        else:
            right_joint_const.position = 0
        right_joint_const.weight = 1.0

        # 限制1轴转动
        right_joint1_const = JointConstraint()
        right_joint1_const.joint_name = "yumi_joint_1_r"
        right_joint1_const.position = 0
        right_joint1_const.tolerance_above = 120
        right_joint1_const.tolerance_below = 0
        right_joint1_const.weight = 1.0

        # 限制2轴转动
        right_joint2_const = JointConstraint()
        right_joint2_const.joint_name = "yumi_joint_2_r"
        right_joint2_const.position = 0
        right_joint2_const.tolerance_above = 0
        right_joint2_const.tolerance_below = 150
        right_joint2_const.weight = 1.0

        # 限制3轴转动
        right_joint3_const = JointConstraint()
        right_joint3_const.joint_name = "yumi_joint_3_r"
        right_joint3_const.position = 0
        right_joint3_const.tolerance_above = 35
        right_joint3_const.tolerance_below = 55
        right_joint3_const.weight = 1.0

        # 限制4轴转动
        right_joint4_const = JointConstraint()
        right_joint4_const.joint_name = "yumi_joint_4_r"
        right_joint4_const.position = 0
        right_joint4_const.tolerance_above = 60
        right_joint4_const.tolerance_below = 75
        right_joint4_const.weight = 1.0

        # 限制5轴转动
        right_joint5_const = JointConstraint()
        right_joint5_const.joint_name = "yumi_joint_5_r"
        right_joint5_const.position = 40
        right_joint5_const.tolerance_above = 50
        right_joint5_const.tolerance_below = 20
        right_joint5_const.weight = 1.0

        # 限制6轴转动
        right_joint6_const = JointConstraint()
        right_joint6_const.joint_name = "yumi_joint_6_r"
        right_joint6_const.position = 0
        right_joint6_const.tolerance_above = 10
        right_joint6_const.tolerance_below = 35
        right_joint6_const.weight = 1.0

        # 限制7轴转动
        right_joint7_const = JointConstraint()
        right_joint7_const.joint_name = "yumi_joint_7_r"
        right_joint7_const.position = -10
        right_joint7_const.tolerance_above = 0
        right_joint7_const.tolerance_below = 160
        right_joint7_const.weight = 1.0

        # 限制末端位移
        right_position_const = PositionConstraint()
        right_position_const.header = Header()
        right_position_const.link_name = "gripper_r_joint_r"
        right_position_const.target_point_offset.x = 0.5
        right_position_const.target_point_offset.y = -0.5
        right_position_const.target_point_offset.z = 1.0
        right_position_const.weight = 1.0

        # 添加末端姿态约束:
        right_orientation_const = OrientationConstraint()
        right_orientation_const.header = Header()
        right_orientation_const.orientation = pose_goal.orientation
        right_orientation_const.link_name = "gripper_r_finger_r"
        right_orientation_const.absolute_x_axis_tolerance = 0.50
        right_orientation_const.absolute_y_axis_tolerance = 0.25
        right_orientation_const.absolute_z_axis_tolerance = 0.50
        right_orientation_const.weight = 100

        # 施加全约束
        consts = Constraints()
        consts.joint_constraints = [right_joint_const]
        # consts.orientation_constraints = [right_orientation_const]
        # consts.position_constraints = [right_position_const]
        right_arm.set_path_constraints(consts)

        # 设置动作对象目标位置姿态
        pose_goal.orientation.x = Right_Qux
        pose_goal.orientation.y = Right_Quy
        pose_goal.orientation.z = Right_Quz
        pose_goal.orientation.w = Right_Quw
        pose_goal.position.x = (Neurondata[5] - 0.05) * 1.48 + 0.053
        pose_goal.position.y = (Neurondata[3] + 0.18) * 1.48 - 0.12
        pose_goal.position.z = (Neurondata[4] - 0.53) * 1.48 + 0.47
        right_arm.set_pose_target(pose_goal)
        print "End effector pose %s" % pose_goal

        # # 设置动作对象目标位置姿态
        # pose_goal.orientation.x = 0.1644
        # pose_goal.orientation.y = 0.3111
        # pose_goal.orientation.z = 0.9086
        # pose_goal.orientation.w = 0.2247
        # pose_goal.position.x = (Neurondata[5]-0.05)*1.48+0.053
        # pose_goal.position.y = (Neurondata[3]+0.18)*1.48-0.12
        # pose_goal.position.z = (Neurondata[4]-0.53)*1.48+0.47
        # right_arm.set_pose_target(pose_goal)
        # print "End effector pose %s" % pose_goal

        # # 设置动作对象目标位置姿态
        # pose_goal.orientation.x = pose_goal.orientation.x
        # pose_goal.orientation.y = pose_goal.orientation.y
        # pose_goal.orientation.z = pose_goal.orientation.z
        # pose_goal.orientation.w = pose_goal.orientation.w
        # pose_goal.position.x = pose_goal.position.x
        # pose_goal.position.y = pose_goal.position.y - 0.01
        # pose_goal.position.z = pose_goal.position.z
        # right_arm.set_pose_target(pose_goal)
        # print "End effector pose %s" % pose_goal

        # 规划和输出动作
        traj = right_arm.plan()
        right_arm.execute(traj, wait=False)
        # 动作完成后清除目标信息
        right_arm.clear_pose_targets()
        # 清除路径约束
        right_arm.clear_path_constraints()
        # 确保没有剩余未完成动作在执行
        right_arm.stop()
Ejemplo n.º 18
0
    def build(self, tf_timeout=rospy.Duration(5.0)):
        """Builds the goal message.

        To set a pose or joint goal, call set_pose_goal or set_joint_goal
        before calling build. To add a path orientation constraint, call
        add_path_orientation_constraint first.

        Args:
            tf_timeout: rospy.Duration. How long to wait for a TF message. Only
                used with pose goals.

        Returns: moveit_msgs/MoveGroupGoal
        """
        goal = MoveGroupGoal()

        # Set start state
        goal.request.start_state = copy.deepcopy(self.start_state)

        # Set goal constraint
        if self._pose_goal is not None:
            self._tf_listener.waitForTransform(self._pose_goal.header.frame_id,
                                               self.fixed_frame,
                                               rospy.Time.now(), tf_timeout)
            try:
                pose_transformed = self._tf_listener.transformPose(
                    self.fixed_frame, self._pose_goal)
            except (tf.LookupException, tf.ConnectivityException):
                return None
            c1 = Constraints()
            c1.position_constraints.append(PositionConstraint())
            c1.position_constraints[0].header.frame_id = self.fixed_frame
            c1.position_constraints[0].link_name = self.gripper_frame
            b = BoundingVolume()
            s = SolidPrimitive()
            s.dimensions = [self.tolerance * self.tolerance]
            s.type = s.SPHERE
            b.primitives.append(s)
            b.primitive_poses.append(self._pose_goal.pose)
            c1.position_constraints[0].constraint_region = b
            c1.position_constraints[0].weight = 1.0

            c1.orientation_constraints.append(OrientationConstraint())
            c1.orientation_constraints[0].header.frame_id = self.fixed_frame
            c1.orientation_constraints[
                0].orientation = pose_transformed.pose.orientation
            c1.orientation_constraints[0].link_name = self.gripper_frame
            c1.orientation_constraints[
                0].absolute_x_axis_tolerance = self.tolerance
            c1.orientation_constraints[
                0].absolute_y_axis_tolerance = self.tolerance
            c1.orientation_constraints[
                0].absolute_z_axis_tolerance = self.tolerance
            c1.orientation_constraints[0].weight = 1.0

            goal.request.goal_constraints.append(c1)
        elif self._joint_names is not None:
            c1 = Constraints()
            for i in range(len(self._joint_names)):
                c1.joint_constraints.append(JointConstraint())
                c1.joint_constraints[i].joint_name = self._joint_names[i]
                c1.joint_constraints[i].position = self._joint_positions[i]
                c1.joint_constraints[i].tolerance_above = self.tolerance
                c1.joint_constraints[i].tolerance_below = self.tolerance
                c1.joint_constraints[i].weight = 1.0
            goal.request.goal_constraints.append(c1)

        # Set path constraints
        goal.request.path_constraints.orientation_constraints = self._orientation_contraints 

        # Set trajectory constraints

        # Set planner ID (name of motion planner to use)
        goal.request.planner_id = self.planner_id

        # Set group name
        goal.request.group_name = self.group_name

        # Set planning attempts
        goal.request.num_planning_attempts = self.num_planning_attempts

        # Set planning time
        goal.request.allowed_planning_time = self.allowed_planning_time

        # Set scaling factors
        goal.request.max_acceleration_scaling_factor = self.max_acceleration_scaling_factor
        goal.request.max_velocity_scaling_factor = self.max_velocity_scaling_factor

        # Set planning scene diff
        goal.planning_options.planning_scene_diff = copy.deepcopy(
            self.planning_scene_diff)

        # Set is plan only
        goal.planning_options.plan_only = self.plan_only

        # Set look around
        goal.planning_options.look_around = self.look_around

        # Set replanning options
        goal.planning_options.replan = self.replan
        goal.planning_options.replan_attempts = self.replan_attempts
        goal.planning_options.replan_delay = self.replan_delay

        return goal
def deliver_sample(move_arm, args):
    """
  :type move_arm: class 'moveit_commander.move_group.MoveGroupCommander'
  :type args: List[bool, float, float, float]
  """
    move_arm.set_planner_id("RRTstar")

    x_delivery = args[1]
    y_delivery = args[2]
    z_delivery = args[3]

    # after sample collect
    mypi = 3.14159
    d2r = mypi / 180
    r2d = 180 / mypi

    goal_pose = move_arm.get_current_pose().pose
    # position was found from rviz tool
    goal_pose.position.x = x_delivery
    goal_pose.position.y = y_delivery
    goal_pose.position.z = z_delivery

    r = -179
    p = -20
    y = -90

    q = quaternion_from_euler(r * d2r, p * d2r, y * d2r)
    goal_pose.orientation = Quaternion(q[0], q[1], q[2], q[3])

    move_arm.set_pose_target(goal_pose)

    plan = move_arm.plan()

    if len(plan.joint_trajectory.points) == 0:  # If no plan found, abort
        return False

    plan = move_arm.go(wait=True)
    move_arm.stop()

    # rotate scoop to deliver sample at current location...

    # adding position constraint on the solution so that the tip doesnot diverge to get to the solution.
    pos_constraint = PositionConstraint()
    pos_constraint.header.frame_id = "base_link"
    pos_constraint.link_name = "l_scoop"
    pos_constraint.target_point_offset.x = 0.1
    pos_constraint.target_point_offset.y = 0.1
    # rotate scoop to deliver sample at current location begin
    pos_constraint.target_point_offset.z = 0.1
    pos_constraint.constraint_region.primitives.append(
        SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01]))
    pos_constraint.weight = 1

    # using euler angles for own verification..

    r = +180
    p = 90  # 45 worked get
    y = -90
    q = quaternion_from_euler(r * d2r, p * d2r, y * d2r)
    goal_pose = move_arm.get_current_pose().pose
    rotation = (goal_pose.orientation.x, goal_pose.orientation.y,
                goal_pose.orientation.z, goal_pose.orientation.w)
    euler_angle = euler_from_quaternion(rotation)

    goal_pose.orientation = Quaternion(q[0], q[1], q[2], q[3])
    move_arm.set_pose_target(goal_pose)
    plan = move_arm.plan()

    if len(plan.joint_trajectory.points) == 0:  # If no plan found, abort
        return False

    plan = move_arm.go(wait=True)
    move_arm.stop()
    move_arm.clear_pose_targets()

    move_arm.set_planner_id("RRTconnect")

    return True
    def on_enter(self, userdata):
        self._planning_failed = False
        self._control_failed = False
        self._success = False

        # TODO check userdata
        # if not isinstance(userdata.pose, PoseStamped):
        #Logger.logwarn('userdata.pose must be geomery_msgs.msg.PoseStamped. `%s` received' % str(type(userdata.pose)))
        #self._planning_failed = True
        #return
        check_type('pose', 'geometry_msgs/PoseStamped', userdata.pose)

        # request planing and execution
        action_goal = MoveGroupGoal()
        # set palnning options
        action_goal.planning_options.plan_only = self._plan_only
        action_goal.planning_options.replan = False
        #		action_goal.planning_options.planning_scene_diff.is_diff = True
        #		action_goal.planning_options.planning_scene_diff.robot_state.is_diff = True
        # setup request
        action_goal.request.group_name = self._move_group
        action_goal.request.num_planning_attempts = 3
        action_goal.request.allowed_planning_time = 1.0
        action_goal.request.max_velocity_scaling_factor = 1.0
        action_goal.request.max_acceleration_scaling_factor = 1.0
        # start pose
        action_goal.request.start_state.is_diff = True
        # pose constraint
        goal_constraint = Constraints(name='')
        # set target position
        constraint = PositionConstraint()
        constraint.header = Header(frame_id=userdata.pose.header.frame_id)
        constraint.link_name = self._end_effector
        constraint.constraint_region = BoundingVolume()
        constraint.constraint_region.primitives = [
            SolidPrimitive(type=SolidPrimitive.SPHERE,
                           dimensions=[self._position_tolerance])
        ]
        constraint.constraint_region.primitive_poses = [
            Pose(position=userdata.pose.pose.position)
        ]
        constraint.weight = 1.0
        goal_constraint.position_constraints.append(constraint)
        # set target orientation
        constraint = OrientationConstraint()
        constraint.header = Header(frame_id=userdata.pose.header.frame_id)
        constraint.link_name = self._end_effector
        constraint.orientation = userdata.pose.pose.orientation
        constraint.absolute_x_axis_tolerance = self._orientation_tolerance
        constraint.absolute_y_axis_tolerance = self._orientation_tolerance
        constraint.absolute_z_axis_tolerance = self._orientation_tolerance
        constraint.weight = 0.1
        goal_constraint.orientation_constraints.append(constraint)
        # add goal_constraint
        action_goal.request.goal_constraints.append(goal_constraint)
        try:
            self._client.send_goal('move_group', action_goal)
        except Exception as e:
            Logger.logwarn(
                'MoveitToPose: Failed to send MoveGroupAction goal for group: %s\n%s'
                % (self._move_group, str(e)))
            self._planning_failed = True
Ejemplo n.º 21
0
    def dip_incrementally(self, z_pose):
        # time limited and incremental movement
        group = self.group

        position_constraint = PositionConstraint()
        position_constraint.target_point_offset.x = 0.1
        position_constraint.target_point_offset.y = 0.1
        position_constraint.target_point_offset.z = 0.3
        position_constraint.weight = 0.75
        position_constraint.link_name = group.get_end_effector_link()
        position_constraint.header.frame_id = group.get_planning_frame()
        constraint = Constraints()
        constraint.position_constraints.append(position_constraint)
        group.set_path_constraints(constraint)

        current_pose = group.get_current_pose().pose
        current_orientation = group.get_current_pose().pose.orientation
        curr_q = [
            current_orientation.x, current_orientation.y,
            current_orientation.z, current_orientation.w
        ]
        allow_replanning = False
        planning_time = 5

        print "Now performing dip"
        tolerance = 0.01
        time_limit = 120
        start_time = time.time()
        offset_x = 0.0
        target_x = current_pose.position.x - offset_x
        status = self.go_to_pose_goal(curr_q[0],
                                      curr_q[1],
                                      curr_q[2],
                                      curr_q[3],
                                      target_x,
                                      current_pose.position.y,
                                      current_pose.position.z,
                                      allow_replanning,
                                      planning_time,
                                      thresh=tolerance)

        while (current_pose.position.z - z_pose) > 2 * tolerance and (
                time.time() - start_time) < time_limit:
            status = self.go_to_pose_goal(curr_q[0],
                                          curr_q[1],
                                          curr_q[2],
                                          curr_q[3],
                                          target_x,
                                          current_pose.position.y,
                                          current_pose.position.z - 0.01,
                                          allow_replanning,
                                          planning_time,
                                          thresh=tolerance)
            current_pose = group.get_current_pose().pose
            rospy.sleep(0.1)

        group.clear_path_constraints()
        rospy.sleep(0.05)

        dip = (current_pose.position.z - z_pose) < 2 * tolerance
        print("dip_incrementally: dip successful? " + str(dip))

        return dip
    def right_arm_go_to_pose_goal(self):
        # 设置动作对象变量,此处为arm
        right_arm = self.right_arm

        # 获取当前末端执行器位置姿态
        pose_goal = right_arm.get_current_pose().pose

        # 限制末端夹具运动
        right_joint_const = JointConstraint()
        right_joint_const.joint_name = "gripper_r_joint_r"
        right_joint_const.position = 0
        right_joint_const.weight = 1.0

        # 限制1轴转动
        right_joint1_const = JointConstraint()
        right_joint1_const.joint_name = "yumi_joint_1_r"
        right_joint1_const.position = 0
        right_joint1_const.tolerance_above = 120
        right_joint1_const.tolerance_below = 0
        right_joint1_const.weight = 1.0

        # 限制2轴转动
        right_joint2_const = JointConstraint()
        right_joint2_const.joint_name = "yumi_joint_2_r"
        right_joint2_const.position = 0
        right_joint2_const.tolerance_above = 0
        right_joint2_const.tolerance_below = 150
        right_joint2_const.weight = 1.0

        # 限制3轴转动
        right_joint3_const = JointConstraint()
        right_joint3_const.joint_name = "yumi_joint_3_r"
        right_joint3_const.position = 0
        right_joint3_const.tolerance_above = 35
        right_joint3_const.tolerance_below = 55
        right_joint3_const.weight = 1.0

        # 限制4轴转动
        right_joint4_const = JointConstraint()
        right_joint4_const.joint_name = "yumi_joint_4_r"
        right_joint4_const.position = 0
        right_joint4_const.tolerance_above = 60
        right_joint4_const.tolerance_below = 75
        right_joint4_const.weight = 1.0

        # 限制5轴转动
        right_joint5_const = JointConstraint()
        right_joint5_const.joint_name = "yumi_joint_5_r"
        right_joint5_const.position = 40
        right_joint5_const.tolerance_above = 50
        right_joint5_const.tolerance_below = 20
        right_joint5_const.weight = 1.0

        # 限制6轴转动
        right_joint6_const = JointConstraint()
        right_joint6_const.joint_name = "yumi_joint_6_r"
        right_joint6_const.position = 0
        right_joint6_const.tolerance_above = 10
        right_joint6_const.tolerance_below = 35
        right_joint6_const.weight = 1.0

        # 限制7轴转动
        right_joint7_const = JointConstraint()
        right_joint7_const.joint_name = "yumi_joint_7_r"
        right_joint7_const.position = -10
        right_joint7_const.tolerance_above = 0
        right_joint7_const.tolerance_below = 160
        right_joint7_const.weight = 1.0

        # 限制末端位移
        right_position_const = PositionConstraint()
        right_position_const.header = Header()
        right_position_const.link_name = "gripper_r_joint_r"
        right_position_const.target_point_offset.x = 0.5
        right_position_const.target_point_offset.y = -0.5
        right_position_const.target_point_offset.z = 1.0
        right_position_const.weight = 1.0

        # 添加末端姿态约束:
        right_orientation_const = OrientationConstraint()
        right_orientation_const.header = Header()
        right_orientation_const.orientation = pose_goal.orientation
        right_orientation_const.link_name = "gripper_r_finger_r"
        right_orientation_const.absolute_x_axis_tolerance = 0.50
        right_orientation_const.absolute_y_axis_tolerance = 0.25
        right_orientation_const.absolute_z_axis_tolerance = 0.50
        right_orientation_const.weight = 100

        # 施加全约束
        consts = Constraints()
        consts.joint_constraints = [right_joint_const]
        # consts.orientation_constraints = [right_orientation_const]
        # consts.position_constraints = [right_position_const]
        right_arm.set_path_constraints(consts)

        # # 设置动作对象目标位置姿态
        # pose_goal.orientation.x = Right_Qux
        # pose_goal.orientation.y = Right_Quy
        # pose_goal.orientation.z = Right_Quz
        # pose_goal.orientation.w = Right_Quw
        # pose_goal.position.x = (Neurondata[5]-0.05)*1.48+0.053
        # pose_goal.position.y = (Neurondata[3]+0.18)*1.48-0.12
        # pose_goal.position.z = (Neurondata[4]-0.53)*1.48+0.47
        # right_arm.set_pose_target(pose_goal)
        # print "End effector pose %s" % pose_goal
        global n
        # 设置动作对象目标位置姿态
        if n == 1:
            pose_goal.orientation.x = -0.00825
            pose_goal.orientation.y = 0.03556
            pose_goal.orientation.z = 0.79932
            pose_goal.orientation.w = 0.5998
            pose_goal.position.x = 0.54425
            pose_goal.position.y = -0.2208
            pose_goal.position.z = 0.14822
        elif n == 2:
            pose_goal.orientation.x = -0.00507
            pose_goal.orientation.y = -0.012593
            pose_goal.orientation.z = 0.98844
            pose_goal.orientation.w = 0.15101
            pose_goal.position.x = 0.55198
            pose_goal.position.y = -0.06859
            pose_goal.position.z = 0.17909
        elif n == 3:
            pose_goal.orientation.x = -0.05578
            pose_goal.orientation.y = 0.025377
            pose_goal.orientation.z = 0.99365
            pose_goal.orientation.w = -0.095267
            pose_goal.position.x = 0.36478
            pose_goal.position.y = -0.05781
            pose_goal.position.z = 0.15907
            n = 0

        right_arm.set_pose_target(pose_goal)
        print "End effector pose %s" % pose_goal

        # 规划和输出动作
        traj = right_arm.plan()
        right_arm.execute(traj, wait=False)
        # 动作完成后清除目标信息
        right_arm.clear_pose_targets()
        # 清除路径约束
        right_arm.clear_path_constraints()
        # 确保没有剩余未完成动作在执行
        right_arm.stop()
Ejemplo n.º 23
0
print('eef_link:', eef_link)
# ('eef_link:', 'panda_link8')

tolerance = 0.01

pose_goal = Pose()
pose_goal.orientation.w = 1.0
pose_goal.position.x = 1.0
pose_goal.position.y = 0.15
pose_goal.position.z = 0.4

action_goal = MoveGroupGoal()
action_goal.request.group_name = move_group

goal_constraint = Constraints()
goal_constraint.position_constraints.append(PositionConstraint())
goal_constraint.position_constraints[0].header.frame_id = planning_frame
goal_constraint.position_constraints[0].link_name = eef_link

solid_primitive = SolidPrimitive()
solid_primitive.dimensions = [tolerance * tolerance]
solid_primitive.type = solid_primitive.SPHERE

bounding_volume = BoundingVolume()
bounding_volume.primitives.append(solid_primitive)
bounding_volume.primitive_poses.append(pose_goal)
goal_constraint.position_constraints[0].constraint_region = bounding_volume

goal_constraint.orientation_constraints.append(OrientationConstraint())
goal_constraint.orientation_constraints[0].header.frame_id = planning_frame
goal_constraint.orientation_constraints[0].link_name = eef_link
Ejemplo n.º 24
0
    def moveToPose(self,
                   pose,
                   tolerance=0.01,
                   wait=True,
                   **kwargs):

        '''
        Move the arm, based on a goal pose_stamped for the end effector.

        :param pose: target pose to which we want to move
                            specified link to
        :param tolerance: allowed tolerance in the final joint positions.
        :param wait: if enabled, makes the fuctions wait until the
            target joint position is reached

        :type pose_stamped: ros message object of type PoseStamped
        :type gripper_frame: string
        :type tolerance: float
        :type wait: bool
        '''

        # Check arguments
        supported_args = ("max_velocity_scaling_factor",
                          "planner_id",
                          "planning_time",
                          "plan_only",
                          "start_state")
        for arg in kwargs.keys():
            if not arg in supported_args:
                rospy.loginfo("moveToPose: unsupported argument: %s",
                              arg)

        # Create goal
        g = MoveGroupGoal()

        # 1. fill in request workspace_parameters

        # 2. fill in request start_state
        try:
            g.request.start_state = kwargs["start_state"]
        except KeyError:
            g.request.start_state.is_diff = True

        # 3. fill in request goal_constraints
        c1 = Constraints()

        c1.position_constraints.append(PositionConstraint())
        c1.position_constraints[0].header.frame_id = self._fixed_frame
        c1.position_constraints[0].link_name = self._gripper_frame
        # c1.position_constraints[0].target_point_offset
        b = BoundingVolume()
        s = SolidPrimitive()
        s.dimensions = [tolerance]
        s.type = s.SPHERE
        b.primitives.append(s)
        b.primitive_poses.append(pose)
        c1.position_constraints[0].constraint_region = b
        c1.position_constraints[0].weight = 1.0

        c1.orientation_constraints.append(OrientationConstraint())
        c1.orientation_constraints[0].header.frame_id = self._fixed_frame
        c1.orientation_constraints[0].orientation = pose.orientation
        c1.orientation_constraints[0].link_name = self._gripper_frame
        c1.orientation_constraints[0].absolute_x_axis_tolerance = tolerance
        c1.orientation_constraints[0].absolute_y_axis_tolerance = tolerance
        c1.orientation_constraints[0].absolute_z_axis_tolerance = tolerance
        c1.orientation_constraints[0].weight = 1.0

        g.request.goal_constraints.append(c1)

        # 4. fill in request path constraints

        # 5. fill in request trajectory constraints

        # 6. fill in request planner id
        try:
            g.request.planner_id = kwargs["planner_id"]
        except KeyError:
            if self.planner_id:
                g.request.planner_id = self.planner_id

        # 7. fill in request group name
        g.request.group_name = self._group

        # 8. fill in request number of planning attempts
        try:
            g.request.num_planning_attempts = kwargs["num_attempts"]
        except KeyError:
            g.request.num_planning_attempts = 1

        # 9. fill in request allowed planning time
        try:
            g.request.allowed_planning_time = kwargs["planning_time"]
        except KeyError:
            g.request.allowed_planning_time = self.planning_time

        # Fill in velocity scaling factor
        try:
            g.request.max_velocity_scaling_factor = kwargs["max_velocity_scaling_factor"]
        except KeyError:
            pass  # do not fill in at all

        # 10. fill in planning options diff
        g.planning_options.planning_scene_diff.is_diff = True
        g.planning_options.planning_scene_diff.robot_state.is_diff = True

        # 11. fill in planning options plan only
        try:
            g.planning_options.plan_only = kwargs["plan_only"]
        except KeyError:
            g.planning_options.plan_only = self.plan_only

        # 12. fill in other planning options
        g.planning_options.look_around = False
        g.planning_options.replan = False

        # 13. send goal
        self._action.send_goal(g)
        if wait:
            self._action.wait_for_result()
            result = self._action.get_result()
            return processResult(result)
        else:
            rospy.loginfo('Failed while waiting for action result.')
            return False
def make_default_action_goal(object, pose, orientation=True):
    action_goal = MoveGroupGoal()
    action_goal.request.group_name = object._move_group
    action_goal.request.start_state.is_diff = True
    action_goal.request.planner_id = "RRTkConfigDefault"  # PRMkConfigDefault, PRMstarkConfigDefault, RRTkConfigDefault, RRTstarkConfigDefault

    goal_constraint = Constraints()

    goal_constraint.position_constraints.append(PositionConstraint())
    goal_constraint.position_constraints[
        0].header.frame_id = object.planning_frame
    goal_constraint.position_constraints[0].link_name = object.eef_link
    bounding_volume = BoundingVolume()
    solid_primitive = SolidPrimitive()
    solid_primitive.dimensions = [object._tolerance]
    solid_primitive.type = solid_primitive.SPHERE
    bounding_volume.primitives.append(solid_primitive)

    # if object.selected_object_name[:8] == "postcard":
    #     p_update = pose
    #     p_update.position.z = 0.75
    #     bounding_volume.primitive_poses.append(p_update)
    #elif pose.position.z <= 0.1: #地面に当たらないように
    # if pose.position.z <= 0.1: #地面に当たらないように
    #     p_update = pose
    #     p_update.position.z = 0.13
    #     bounding_volume.primitive_poses.append(p_update)
    # else:
    bounding_volume.primitive_poses.append(pose)

    goal_constraint.position_constraints[0].constraint_region = bounding_volume
    goal_constraint.position_constraints[0].weight = 1.0

    if orientation is not False:
        orientation_constraint = OrientationConstraint()
        orientation_constraint.header.frame_id = object.planning_frame
        if pose.position.z <= 0.2:  # Stage 2 (Grasp)
            rospy.loginfo("Planning num:0-0 (Grasp)")
            orientation_constraint.orientation.x = 0.675
            orientation_constraint.orientation.y = 0.737
            orientation_constraint.orientation.z = 0.018
            orientation_constraint.orientation.w = -0.019
            #goal_constraint.position_constraints[0].constraint_region.primitive_poses[0].position.z -= 0.01 # Stage1

        # elif object.selected_object_name[:8] == "postcard":
        # #elif pose.position.z < 0.02: # Stage 1
        #     object.
        #     orientation_constraint.orientation.x = 0.0#0.904
        #     orientation_constraint.orientation.y = 0.0#-0.166
        #     orientation_constraint.orientation.z = -0.676#-0.068
        #     orientation_constraint.orientation.w = 0.736#0.387
        #     #orientation_constraint.link_name = "hand_palm_link"
        elif object.count_num == 1:  # Stage 2 (Sofa)
            rospy.loginfo("Planning num:1 (Put on sofa)")
            orientation_constraint.orientation.x = -0.019
            orientation_constraint.orientation.y = -0.707
            orientation_constraint.orientation.z = -0.202
            orientation_constraint.orientation.w = 0.706
        elif object.count_num == 2:  # Stage 2 (Work desk, Shelf)
            rospy.loginfo("Planning num:2 (Put on shelf or desk)")
            orientation_constraint.orientation.x = 0.51
            orientation_constraint.orientation.y = -0.489
            orientation_constraint.orientation.z = 0.509
            orientation_constraint.orientation.w = 0.489
        elif object.count_num == 3:  # Stage 2 (White table)
            rospy.loginfo("Planning num:3 (Put on table)")
            orientation_constraint.orientation.x = 0.51
            orientation_constraint.orientation.y = 0.489
            orientation_constraint.orientation.z = 0.501
            orientation_constraint.orientation.w = -0.497
        else:
            rospy.loginfo("Planning num:0-1 (Put)")
            orientation_constraint.orientation = pose.orientation
        orientation_constraint.link_name = object.eef_link
        orientation_constraint.absolute_x_axis_tolerance = object._tolerance * 100
        orientation_constraint.absolute_y_axis_tolerance = object._tolerance * 100
        orientation_constraint.absolute_z_axis_tolerance = object._tolerance * 100
        orientation_constraint.weight = 1.0
        goal_constraint.orientation_constraints.append(orientation_constraint)

    action_goal.request.goal_constraints.append(goal_constraint)
    action_goal.request.num_planning_attempts = 5
    action_goal.request.allowed_planning_time = 10.0
    action_goal.request.workspace_parameters.header.frame_id = object.planning_frame
    action_goal.request.workspace_parameters.min_corner.y = -5.0  #-20.0
    action_goal.request.workspace_parameters.min_corner.x = -5.0  #-20.0
    action_goal.request.workspace_parameters.min_corner.z = -5.0  #-20.0
    action_goal.request.workspace_parameters.max_corner.x = 5.0  #20.0
    action_goal.request.workspace_parameters.max_corner.y = 5.0  #20.0
    action_goal.request.workspace_parameters.min_corner.z = 5.0  #20.0
    action_goal.planning_options.look_around = False
    action_goal.planning_options.replan = True
    action_goal.planning_options.replan_attempts = 5
    action_goal.planning_options.replan_delay = 10.0
    return action_goal
Ejemplo n.º 26
0
def deliver_sample(move_arm, robot, moveit_fk, args):
    """
    :type move_arm: class 'moveit_commander.move_group.MoveGroupCommander'
    :type args: List[bool, float, float, float]
    """
    move_arm.set_planner_id("RRTstar")
    robot_state = robot.get_current_state()
    move_arm.set_start_state(robot_state)
    x_delivery = args.delivery.x
    y_delivery = args.delivery.y
    z_delivery = args.delivery.z

    # after sample collect
    mypi = 3.14159
    d2r = mypi / 180
    r2d = 180 / mypi

    goal_pose = move_arm.get_current_pose().pose
    # position was found from rviz tool
    goal_pose.position.x = x_delivery
    goal_pose.position.y = y_delivery
    goal_pose.position.z = z_delivery

    r = -179
    p = -20
    y = -90

    q = quaternion_from_euler(r * d2r, p * d2r, y * d2r)
    goal_pose.orientation = Quaternion(q[0], q[1], q[2], q[3])

    move_arm.set_pose_target(goal_pose)

    _, plan_a, _, _ = move_arm.plan()

    if len(plan_a.joint_trajectory.points) == 0:  # If no plan found, abort
        return False

    # rotate scoop to deliver sample at current location...

    # adding position constraint on the solution so that the tip doesnot diverge to get to the solution.
    pos_constraint = PositionConstraint()
    pos_constraint.header.frame_id = "base_link"
    pos_constraint.link_name = "l_scoop"
    pos_constraint.target_point_offset.x = 0.1
    pos_constraint.target_point_offset.y = 0.1
    # rotate scoop to deliver sample at current location begin
    pos_constraint.target_point_offset.z = 0.1
    pos_constraint.constraint_region.primitives.append(
        SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01]))
    pos_constraint.weight = 1

    # using euler angles for own verification..

    r = +180
    p = 90  # 45 worked get
    y = -90
    q = quaternion_from_euler(r * d2r, p * d2r, y * d2r)

    cs, start_state, goal_pose = calculate_joint_state_end_pose_from_plan_arm(
        robot, plan_a, move_arm, moveit_fk)

    move_arm.set_start_state(cs)

    goal_pose.orientation = Quaternion(q[0], q[1], q[2], q[3])

    move_arm.set_pose_target(goal_pose)
    _, plan_b, _, _ = move_arm.plan()

    if len(plan_b.joint_trajectory.points
           ) == 0:  # If no plan found, send the previous plan only
        return plan_a

    deliver_sample_traj = cascade_plans(plan_a, plan_b)

    # move_arm.set_planner_id("RRTconnect")

    return deliver_sample_traj
Ejemplo n.º 27
0
    def liftgripper(self, target_z=0.10):
        # approx centers of onions at 0.82, width of onion is 0.038 m. table is at 0.78
        # length of gripper is 0.163 m The gripper should not go lower than
        # (height_z of table w.r.t base+gripper-height/2+tolerance) = 0.78-0.93+0.08+0.01=-0.24
        # pnp._limb.endpoint_pose returns {'position': (x, y, z), 'orientation': (x, y, z, w)}
        # moving from z=-.02 to z=-0.1

        group = self.group
        while self.target_location_x == -100:
            rospy.sleep(0.05)

        current_pose = group.get_current_pose().pose
        current_orientation = group.get_current_pose().pose.orientation
        curr_q = [
            current_orientation.x, current_orientation.y,
            current_orientation.z, current_orientation.w
        ]

        position_constraint = PositionConstraint()
        position_constraint.target_point_offset.x = 0.1
        position_constraint.target_point_offset.y = 0.2
        position_constraint.target_point_offset.z = 0.3
        position_constraint.weight = 0.8
        position_constraint.link_name = group.get_end_effector_link()
        position_constraint.header.frame_id = group.get_planning_frame()
        orientation_constraint = OrientationConstraint()
        orientation_constraint.orientation = Quaternion(x=curr_q[0],
                                                        y=curr_q[1],
                                                        z=curr_q[2],
                                                        w=curr_q[3])
        orientation_constraint.absolute_x_axis_tolerance = 0.1
        orientation_constraint.absolute_y_axis_tolerance = 0.1
        orientation_constraint.absolute_z_axis_tolerance = 0.1
        orientation_constraint.weight = 0.2
        orientation_constraint.link_name = group.get_end_effector_link()
        orientation_constraint.header.frame_id = group.get_planning_frame()

        constraint = Constraints()
        constraint.orientation_constraints.append(orientation_constraint)
        constraint.position_constraints.append(position_constraint)
        group.set_path_constraints(constraint)

        allow_replanning = True
        planning_time = 5
        threshold = 0.02
        time_limit = 120
        start_time = time.time()
        target_x = current_pose.position.x
        target_y = current_pose.position.y
        delta_z = (target_z - current_pose.position.z) / 4

        print "Attempting to lift gripper"
        i = 1
        # if (delta_z < threshold):
        #     return True
        # while (current_pose.position.z < target_z) and ((target_z - current_pose.position.z) > 3*threshold) and ((time.time()-start_time) < time_limit):
        # status = self.go_to_pose_goal(curr_q[0], curr_q[1], curr_q[2], curr_q[3],
        #             target_x, target_y, current_pose.position.z + i*delta_z,
        #             allow_replanning, planning_time, thresh=threshold)
        # rospy.sleep(0.1)
        # current_pose = group.get_current_pose().pose
        # i += 1

        status = self.go_to_pose_goal(curr_q[0],
                                      curr_q[1],
                                      curr_q[2],
                                      curr_q[3],
                                      target_x,
                                      target_y,
                                      target_z,
                                      allow_replanning,
                                      planning_time,
                                      thresh=threshold)

        rospy.sleep(0.25)
        print "Successfully lifted gripper to z: ", current_pose.position.z
        group.clear_path_constraints()

        # return ((target_z-current_pose.position.z) < 3*threshold or (current_pose.position.z > target_z))
        return True
    def flip(cls):
        '''
        rotate gripper 180 degree about y
        :return:
        '''

        # return ManipulatorActions.flip_kinova()

        mg = ManipulatorActions.get_move_group()
        ManipulatorActions.with_planning_scene()
        pose = mg.get_current_pose()
        pose.pose.position.z = 0.4
        quat = [
            pose.pose.orientation.x, pose.pose.orientation.y,
            pose.pose.orientation.z, pose.pose.orientation.w
        ]
        current_euler = euler_from_quaternion(quat)
        pose.pose.orientation = ManipulatorActions.getGraspOrientation(
            0, 100, 0, return_msg=True)
        mg.set_pose_target(pose)

        from moveit_msgs.msg import Constraints, PositionConstraint

        eef_on_line = PositionConstraint()
        eef_on_line.header.frame_id = 'base_link'
        eef_on_line.link_name = mg.get_end_effector_link()
        eef_on_line.target_point_offset.x = 0.0
        eef_on_line.target_point_offset.y = 0.0
        eef_on_line.target_point_offset.z = 0.0

        eef_on_line.weight = 1

        tolerance = 0.2
        line = SolidPrimitive()
        line.type = SolidPrimitive.SPHERE
        line.dimensions.append(0.5)
        # line.dimensions.append(2.1)
        # line.dimensions.append(2.1)

        eef_on_line.constraint_region.primitives.append(line)
        box_pose = mg.get_current_pose().pose
        box_pose.orientation = Quaternion()
        box_pose.orientation.w = 1
        eef_on_line.constraint_region.primitive_poses.append(box_pose)
        eef_on_line.header.frame_id = mg.get_planning_frame()

        flip_ct = Constraints()
        flip_ct.position_constraints.append(eef_on_line)
        # mg.set_path_constraints(flip_ct)

        goal_tol = mg.get_goal_position_tolerance()
        goal_tol_rot = mg.get_goal_orientation_tolerance()

        mg.set_goal_position_tolerance(0.3)
        rospy.sleep(0.5)
        mg.set_goal_orientation_tolerance(0.3)
        mg.set_planning_time(10.0)
        rospy.sleep(0.5)

        plan = mg.plan()
        mg.set_goal_position_tolerance(goal_tol)
        mg.set_goal_orientation_tolerance(goal_tol_rot)
        mg.set_planning_time(5.0)

        if len(plan.joint_trajectory.points) > 1:
            mg.execute(plan)
            return True, 'flip successful'
        else:
            return False, 'flip failed'
        pass