def moveToPose()

in src/pyrobot/utils/move_group_interface.py [0:0]

• 73 lines of code
• 11 McCabe index (conditional complexity)

    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