def motionPlanToPose()

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

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

    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