[moveit_py] add getMotionPlanRequest in planning_component

[patrickKXMD]

Description

A MotionPlanRequest message is required to construct a MoveGroupSequence action, which is needed when using pilz planner.
Add get_motion_plan_request so we can use pilz planner in moveit_py.

Checklist

• Required by CI: Code is auto formatted using clang-format
• Extend the tutorials / documentation reference
• Document API changes relevant to the user in the MIGRATION.md notes
• Create tests, which fail without this PR reference
• Include a screenshot if changing a GUI
• While waiting for someone to review your request, please help review another open pull request to support the maintainers

[peterdavidfagan]

Hi @patrickKXMD,

Thanks for this contribution. Do you have an example for how you wish to specify motion sequence requests from moveit_py? Currently the sequence request is only implemented in MoveGroup, my understanding is that this hasn't been ported to ROS 2 yet (related issue).

With the above being said you should be able to plan motions with PILZ using moveit_py and manually construct a planned sequence through post processing the returned trajectories, I am happy to share an example of this.

[patrickKXMD]

Hi @patrickKXMD,

Thanks for this contribution. Do you have an example for how you wish to specify motion sequence requests from moveit_py? Currently the sequence request is only implemented in MoveGroup, my understanding is that this hasn't been ported to ROS 2 yet (related issue).

With the above being said you should be able to plan motions with PILZ using moveit_py and manually construct a planned sequence through post processing the returned trajectories, I am happy to share an example of this.

Hi @peterdavidfagan, thanks for the comment.
It worked in my project by constructing a MoveGroupSequence.Goal and send it to "/sequence_move_group", so I think MoveGroup is working on ROS 2 right now.

Adding one get_motion_plan_request api is the most simple commit to make it work ... with boilerplate codes of messages constructing and ActionServer handling of course.

So, maybe some nice classes in moveit_py that helps communicating with MoveGroup?
It would always be better to write less code using Python :)

[peterdavidfagan]

Ok thanks for the further details, I'll look to test your changes sometime this week for fetching this message type. It would be nice to also expose similar functionality directly with moveit_py rather than having to interface with the MoveGroup node as the Python API shouldn't depend on MoveGroup in my opinion. This being said if fetching this message type from moveit_py enables your application I think we should expose this method.

It worked in my project by constructing a MoveGroupSequence.Goal and send it to "/sequence_move_group", so I think MoveGroup is working on ROS 2 right now.

Would it be possible to link your code it may be relevant for the aforementioned issue.

[patrickKXMD]

@peterdavidfagan My project is not open-sourced yet, but here is a sample:

1. include move_group.launch.py in your *_moveit_config project or include the result of moveit_config_utils.launches.generate_move_group_launch
2. create a ActionClient in your program.

from moveit_msgs.action import MoveGroupSequence
from rclpy.action import ActionClient

self.__sequence_action = ActionClient(
    node,
    MoveGroupSequence,
    "/sequence_move_group",
)

3. construct your sequence item.

    def __create_sequence_item(
        self,
        index: int,
        ...... # your args
    ) -> MotionSequenceItem:
        # configure your planning_component
        ......
        # get the MotionPlanRequest message.
        req = planning_component.get_motion_plan_request(
            plan_request_parameters=...
        )

        # only first item have start_state
        if index != 0:
            req.start_state = RobotState(is_diff=True)

        return MotionSequenceItem(
            req=req,
            blend_radius=...,
        )

4. get planning result via /sequence_move_group.

    goal = MoveGroupSequence.Goal(
        request=MotionSequenceRequest(items=requests),
        planning_options=PlanningOptions(
            planning_scene_diff=PlanningScene(is_diff=True), plan_only=True
        ),
    )

    result = self.__sequence_action.send_goal(goal).result
    success = result.response.error_code.val == 1
    if success:
        # moveit.msgs.RobotTrajectory
        plan = result.response.planned_trajectories[0]

5. execute with moveit_py

# different from moveit.msgs.RobotTrajectory
from moveit.core.robot_trajectory import RobotTrajectory as RobotTrajectoryPy

def execute(self, plan, robot_model, planning_group):
    robot_trajectory = RobotTrajectoryPy(robot_model)
    robot_trajectory.joint_model_group_name = planning_group
    # fill trajectory message
    with self.scene_monitor.read_only() as scene:
        robot_trajectory.set_robot_trajectory_msg(scene.current_state, plan)

    self.moveit_py.execute(robot_trajectory=robot_trajectory, controllers=[])

[github-actions]

This PR is stale because it has been open for 45 days with no activity. Please tag a maintainer for help on completing this PR, or close it if you think it has become obsolete.

[patrickKXMD]

@peterdavidfagan hi, what do you suggest on this pr?

[peterdavidfagan]

Hi @patrickKXMD, once I clear my task list for the week I will take a look at this, apologies for the delay.

[github-actions]

This PR is stale because it has been open for 45 days with no activity. Please tag a maintainer for help on completing this PR, or close it if you think it has become obsolete.