Sequenced motion via MoveIt2

[SUUHOLEE]

Hello,

I am a student conducting research on MED14 in South Korea. First of all, I would like to express my gratitude for sharing the code you developed.

I am currently using the code provided at https://github.com/lbr-stack/lbr_fri_ros2_stack/issues/152 to control a robot. there is an issue with this code where the robot starts from x=0.3, y=0.3, z=0.8, stops at x=0.35, y=0.3, z=0.8, and then resumes to reach x=0.4, y=0.3, z=0.8. However, I would like to know how to modify it so that the robot can move directly from x=0.3, y=0.3, z=0.8 to x=0.4, y=0.3, z=0.8 without stopping at x=0.35, y=0.3, z=0.8.

Additionally, I am curious about how to input the path of the robot's end effector to make it move.

Thank you always.

[mhubii]

hi @SUUHOLEE , yes no problem. Assume you have a list of waypoints

    # List of poses to move the robot to
    poses = [
        Pose(
            position=Point(x=0.3, y=0.3, z=0.8),
            orientation=Quaternion(x=1.0, y=0.0, z=0.0, w=0.0),
        ),
        # Pose(
        #     position=Point(x=0.35, y=0.3, z=0.8),
        #     orientation=Quaternion(x=1.0, y=0.0, z=0.0, w=0.0),
        # ),
        Pose(
            position=Point(x=0.4, y=0.3, z=0.8),
            orientation=Quaternion(x=1.0, y=0.0, z=0.0, w=0.0),
        ),
        # Add more poses as needed
    ]

Just remove / add points as needed (commented out above).

May I ask whether you aim to control the end-effector's position or velocity? Do you want to use Moveit or would something suffice?

[SUUHOLEE]

It seems that my intended purpose wasn't conveyed properly.

In the code provided in https://github.com/lbr-stack/lbr_fri_ros2_stack/issues/152, when there are three points forming a straight line, removing the middle point allows the movement to proceed as desired. However, there is an issue with implementing a waypoint at x=0.35, y=0.3, z=0.8 while moving from x=0.3, y=0.3, z=0.8 to x=0.4, y=0.4, z=0.8. Attempting to incorporate this waypoint with only three points causes the movement to stop at the middle point. ( The point I want to address. )

Thank you

[mhubii]

okay I understand. I'll have to figure this out myself first to be honest. There is some documentation for Moveit 1 here, which was posted by @andyze on ROS Discourse. They mention an action server interface under /sequence_move_group, which I cannot find for ROS 2, however, there is an action for it

from moveit_msgs.action import MoveGroupSequence

so I am guessing this is still possible somehow.

Doing this through the ros2_control Joint Trajectory Controller should be relatively simple, but Moveit has to take collision avoidance into consideration. I'll let you know once I figure it out.

[SUUHOLEE]

Thank you for kindly answering my question. I will also diligently research my question, and if I find out first, I will inform you through this message.

Thank you.

[mhubii]

This issue appears to be related https://github.com/ros-planning/moveit2_tutorials/issues/801

There is updated documentation for the Pilz planner here https://moveit.picknik.ai/main/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.html

[mhubii]

Okay I managed to get the sequence action server up and running at least.

Thank you for adding this issue @SUUHOLEE , very much appreciated. I'll try to provide you with an example as soon as possible.

Here is a quick hint what needs change.

• [ ] Add pilz_industrial_motion_planner additional planner to the pipelines https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/b59922cf6119a88a3cc55148c422b1c6e3c05d08/lbr_bringup/lbr_bringup/launch_mixin.py#L70 e.g.

pipelines=["pilz_industrial_motion_planner", "ompl"],

• [ ] You might need to sudo apt install ros-humble-pilz-industrial-motion-planner
• [ ] Change capabilities parameter (as in https://github.com/ros-planning/moveit2/issues/1248#issuecomment-1350773802) https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/b59922cf6119a88a3cc55148c422b1c6e3c05d08/lbr_bringup/lbr_bringup/launch_mixin.py#L81 e.g.

"capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService",

Running ros2 action list will now display /lbr/sequence_move_group. One should then be able to write an action client and send messages, e.g.:

from moveit_msgs.action import MoveGroupSequence

[SUUHOLEE]

I appreciate the various responses you provided to my previous questions.

While I have studied related topics based on those responses, I found some aspects understandable and others not. If possible, could you show me a simple code for the sequence motion in Med14 when you have the time?

Thank you.

[mhubii]

yes I'll try to provide you an example Python script shortly. In a nutshell:

• Moveit supports sequenced motions using the Pilz motion planner: https://moveit.picknik.ai/main/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.html#sequence-of-multiple-segments
• The changes necessary for using the planner are summarized here: https://github.com/lbr-stack/lbr_fri_ros2_stack/issues/159#issuecomment-1963011900
• They provide an action server to interface the Pillz motion planner
• You can e.g. interface this action server from Python

[SUUHOLEE]

Thank you for always being willing to answer my questions.

Have you had a chance to resolve the issue? I tried looking for relevant documentation and attempted to solve the problem myself, but I found it difficult to find any clues or solutions.

Therefore, I'm reaching out to you again for assistance. Thank you.

[mhubii]

hi @SUUHOLEE , really sorry for the delay. Will have significantly more time next week! Will attempt to close a bunch of related issues then. Hope this is okay.

[SUUHOLEE]

Thank you as always for your kind answers.

Could I possibly get some additional hints or answers on this topic?

I've seen the hints you provided above, but I can't figure out how to solve it. :(

thank you

[mhubii]

okay let me revisit this topic, just got some big chunk of work off my table!

[mhubii]

hi @SUUHOLEE , I added a demo for you in a dev-humble-sequenced-motion branch. Please let me know if this works for you:

https://github.com/lbr-stack/lbr_fri_ros2_stack/tree/dev-humble-sequenced-motion/lbr_demos/lbr_demos_moveit_python

I found that this sometimes doesn't run at first but couldn't fix yet. Might have to run

ros2 run lbr_demos_moveit_python sequenced_motion

A couple of times, MoveIt seems to struggle with inverse kinematics.

This won't be merged in the humble branch just yet as some changes to the launch files are required.

[SUUHOLEE]

Thank you for your response.

After trying to run it as mentioned in the link provided:

https://github.com/lbr-stack/lbr_fri_ros2_stack/tree/dev-humble-sequenced-motion/lbr_demos/lbr_demos_moveit_python

It seems that the inverse kinematics cannot be solved.

Even after several attempts, it appears that MoveIt struggles to solve the inverse kinematics.

What I'm looking for is similar to what is described in the following link, where I want to set multiple points to be passed through sequentially:

https://moveit.picknik.ai/main/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.html#sequence-of-multiple-segments

[mhubii]

The demo I provided uses the Pilz planner from the link you provide.

After some trying, it appears you can adopt the IK timeout (related https://github.com/moveit/moveit/issues/2459), e.g. here

https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/8317e534b2467932a142a923e1c2fcd0625ea5f6/lbr_moveit_config/iiwa7_moveit_config/config/kinematics.yaml#L4

You can e.g. set it to 1.0. The timeout is now increased in the dev-humble-sequenced-motion branch.

Please note how multiple targets are set here (with blend radius etc)

https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/2cdd90aeb85355eddc5d05f2600e00319328cbb0/lbr_demos/lbr_demos_moveit_python/lbr_demos_moveit_python/sequenced_motion.py#L77

Please find below a video of the demo

https://github.com/lbr-stack/lbr_fri_ros2_stack/assets/26366414/e0c99063-4143-4fed-b422-0ce95fe9eef5

Thank you again for raising this issue @SUUHOLEE!

[SUUHOLEE]

Thank you for your prompt response.

I am using the med14 robot, and I will try the method you provided and let you know the results.

Thank you.

[mhubii]

no worries. Sorry for the massive delay!

You can git pull the changes to your local repo and run

ros2 launch lbr_bringup bringup.launch.py model:=med14 moveit:=true

and

ros2 run lbr_demos_moveit_python sequenced_motion

You might encounter solving issues with scene objects.

[SUUHOLEE]

Thank you for providing helpful responses and test code.

Even after modifying kinematics_solver_timeout = 1.0 in lbr_fri_ros2_stack/lbr_moveit_config/iiwa7_moveit_config/config/kinematics.yaml and executing sequenced_motion, I still see the message [sequenced_motion]: Waiting for /lbr/sequenced_move_group..., indicating that the inverse kinematics is not being solved. ( Before executing it on Med14, I tried running it on iiwa7.)

Do you have any idea why this problem might be occurring?

(When I execute the command 'ros2 run lbr_demos_moveit_python sequenced_motion', I get an error saying 'Package 'lbr_demos_moveit_python' not found'. To resolve this, I downloaded the Python code and tried running 'python3 sequenced_motion.py'. Is this causing the error?)

[mhubii]

so you will have to checkout the dev-humble-sequenced-motion branch. Here is a full example:

1. Follow the install instructions

mkdir -p lbr-stack/src && cd lbr-stack
vcs import src --input https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/humble/lbr_fri_ros2_stack/repos.yaml
rosdep install --from-paths src -i -r -y

2. Checkout required branch

cd src/lbr_fri_ros2_stack
git checkout dev-humble-sequenced-motion
cd ../..

3. Build

colcon build --symlink-install --cmake-args -DFRI_CLIENT_VERSION=1.15 --no-warn-unused-cli # replace by your FRI client version

4. Run the demos. Open 2 terminals:

• Terminal 1:

source install/setup.bash
ros2 launch lbr_bringup bringup.launch.py model:=med14 moveit:=true

• Terminal 2:

source install/setup.bash
ros2 run lbr_demos_moveit_python sequenced_motion

Please feel free to keep this issue open, as it is not solved in the humble branch yet. You can find git documentation here: https://git-scm.com/docs/git-checkout

[SUUHOLEE]

It seems that there was an error when using the default humble instead of the dev-humble-sequenced-motion branch.

After testing with the dev-humble-sequenced-motion branch, I confirmed that it runs without errors.

Thank you for always providing helpful and detailed responses.

[mhubii]

awesome! Let's keep this issue open until solved in humble branch. Thank you for all the valuable feedback. Please feel free to open other issues as needed.