Adding new functionalities to MoveIt: End effector's cartesian speed control, synchronized dual-arm cartesian motion, Automatic Tool Changing

[pablomalvido]

In the European Project REMODEL of the H2020 program (https://remodel-project.eu/), we have analyzed the current status of MoveIt. From this study, we have identified some of its main gaps: Automatic Tool Changing, a complete solution for motion planning with cartesian speed control on the robot’s end effectors, and the multi-arm synchronized motion. To address these three gaps, we have developed some functions that can be integrated with MoveIt. The research behind these developments and the analysis of their performance have been published in this paper: https://authors.elsevier.com/sd/article/S0736-5845(23)00035-2. Additionally, a video showing the experimental evaluation of these functions for an SDA10F dual-arm Yaskawa robot (https://www.youtube.com/watch?v=DJXKWWmBquM) and the repository (https://github.com/pablomalvido/Advanced_manipulation_moveit) are publicly available. The developed functions are generic and can be used for any robot compatible with MoveIt with minor modifications (indicated in the journal article). If there is interest, we would be happy to work on making these functions completely hardware-agnostic and adding them to the MoveGroupCommander class. These functions add the following functionalities:

• Motion planning with end effector’s cartesian speed control: This function allows the definition of trajectories with complete control of the path and the speed of the end effector in the cartesian space. The function allows to define different linear and angular target speeds for different sections of the same trajectory, ensuring a smooth speed transition between them. Additionally, the maximum linear and angular cartesian accelerations can be also specified. The calculated motion plans always comply with three constraints: the target speed of each section is never exceeded, the maximum linear and angular accelerations are never exceeded, and all the joint velocities are below their limits.

• Synchronized dual-arm cartesian motion: This function allows the definition of dual-arm trajectories according to 5 synchronization policies: both arms start and finish moving at the same time, both arms move independently (each one at its own target speed), both arms reach a set of waypoints at the same time (the transitions between waypoints are smooth and the arms don’t stop between waypoints), constant distance master-slave (i.e., just the trajectory of the master arm is specified, and the slave arm moves to keep always a constant relative pose with respect to the master arm), identical master-slave (i.e., just the trajectory of the master arm is specified, and the slave arm performs the same cartesian displacements and rotations as the master arm). The function checks possible collisions and retrieves a safe plan. With this function, the user has full control over the path and speed of the arms.

• Automatic tool changing: This class adds the automatic tool changing capability in MoveIt. The class allows to detach and attach tools to the robot’s wrists, considers collisions for the tools, and provides a function to transform the planned trajectories from the robot’s wrist frame to the actuation frame of the current end effector.

[welcome[bot]]

Thanks for reporting an issue. Because we're a volunteer community, providing a pull request with suggested changes is always welcomed.

[gavanderhoorn]

:+1: on the work.

Some comments:

for an SDA10F dual-arm Yaskawa robot

as one of the maintainers of the driver you appear to be using: why did you not use a fork? Disconnecting the history like this makes it rather difficult to merge improvements. It also makes it difficult to compare the base-line driver and your work.

As to MoveIt: same question. If you have a fork, I'd suggest opening a couple of PRs. Manually comparing code in different repositories is not a very efficient use of time (neither yours nor that of the maintainers here).

[pablomalvido]

Thanks for your prompt response and for the comments. You are right, a fork would be the best way to compare code. Initially, our goal was just to develop custom solutions for our robot, as we were not able to perform some manipulations with the current functionalities of MoveIt. After that, we decided to make the functions generic, as we found many issues and questions in forums asking for the functionalities we had developed. Due to this, we extended our custom code instead of starting to work on a fork.

I have just made a fork of the repository and I will start working on it as soon as I have time, as I’m quite busy at the moment. I was analyzing the repository and my suggestion is to do the following for each of the new functions:

• Include the code of the developed function as a new service inside the move_group package (e.g., move_group/src/default_capabilities/cartesian_path_speed_control_service_capability.cpp).

• Add a new function inside the MoveGroupInterface class (in the move_group_interface.cpp file of the moveit_ros_planning_interface package) which calls the service created in the previous point and returns the computed trajectory plan.

• Add a function inside the MoveGroupCommander class (in the move_group.py file of the moveit_commander class) which calls the function created in the previous point. Then, the users will be able to call this function from instances of the MoveGroupCommander class.

Please, let me know if this implementation would be correct.

As I said, this will take some time as our code is in Python and it should be translated to C++. In the meantime, I suggest leaving this issue open. This way, if someone needs to use any of the developed functionalities, they can still check and implement our solution.

[rhaschke]

I will comment on the three contributions individually:

• Motion planning with end effector’s cartesian speed control This sounds like duplication of the Pilz Industrial planner. Please elaborate on the differences. If really different, this work should be integrated as a new planning plugin, I think.
• Synchronized dual-arm cartesian motion This is very interesting for dual arm motion, but very specific. I suggest a new planning plugin for integration as well.
• Automatic tool changing This is a often requested feature and I am very curious how you implemented it.
  • How is the tool specified?
  • Is it possible for the tool to have own degrees of freedom?
  • Note that there is a a GSoC task formulated for this. Please approach @AndyZe.

[AndyZe]

Automatic tool changing This is a often requested feature and I am very curious how you implemented it.

I'm watching this. I think we should (at least!) review what you've done. I'll try to find time to do this

[pablomalvido]

Thanks for your comments. I will reply to these three points:

• Motion planning with end effector’s speed control: I haven’t used Pilz too much, so my experience with it is limited. But I think one of the main differences is the possibility of programming trajectories with multiple target speeds, with smooth transitions between them. For instance, in a trajectory with 5 waypoints (A, B, C, D, and E), we can move from A to C at 50mm/s, from C to D at 80mm/s, and from D to E at 30mm/s, ensuring that the target speed is not exceeded in any section (understanding a section as the path between waypoints with a common target speed (A-C, C-D, or D-E in this case)).

• Synchronized dual-arm cartesian motion: I think a new planning plugin would be a good option. With some work, this could also be extended and define a more generic multi-robot control function.

• Automatic tool changing: The tools are specified as instances of the “EEF” class defined in our repository. This class contains information about the end effector’s model, dimensions, the transformation matrix from its base frame to its actuation frame, etc. Then, the “ATC” class allows to detach and attach tools to the robot’s wrists, considering collisions, and providing a function to transform the planned trajectories from the robot’s wrist frame to the actuation frame of the current end effector. In the version presented in the paper, it was not possible for the tool to have its own degrees of freedom as we were adding them as collision objects. However, the approach has been later extended and now this is possible, as can be seen in this video: https://www.youtube.com/watch?v=59j7i5tLQl4. In this case, the method presented in the paper is used to consider collisions and update the planning frames, but now, each tool is added in the robot_description parameter under different namespaces, and we can modify their parent frames using tf broadcasters (detaching or attaching them to the robot).

If you need more detailed information about any of the functions, you can check the paper (the link is in the first message of the issue), and if you have any questions, don’t hesitate to ask. Please, let me know how to proceed if you are interested in incorporating any of these functions in the MoveIt repository.

[yurirocha15]

Thank you for sharing your work :+1:

(...) However, the approach has been later extended and now this is possible, as can be seen in this video: https://www.youtube.com/watch?v=59j7i5tLQl4. In this case, the method presented in the paper is used to consider collisions and update the planning frames, but now, each tool is added in the robot_description parameter under different namespaces, and we can modify their parent frames using tf broadcasters (detaching or attaching them to the robot).

I am curious if the solution you mentioned for dynamically changing the TF is also open-source (I could not find it in the original work's repo). I am also interested in this feature, but being able to attach/detach tools with moving parts is paramount for my application (and for most of the pick-and-place applications, I believe).

[pablomalvido]

Thank you for your question and curiosity @yurirocha15. The ATC functionality with moving parts is not available yet in the repository. We are currently working on a new version of the code, including this functionality. I’ll let you know when this new version is pushed.