How to stop an ongoing trajectory execution in moveit2?

MoveIt
, ,
1

Hi all,

I am a beginner to ros2 humble and moveit2 and navigating myself into getting more experienced with the moveit2 framework. I am currently working on path planning using UR10e robot arm.

I am using
ROS_DISTRO: HUMBLE
UBUNTU: 22.04
Moveit2: built from source

I used the moveit2 C++ APIs to execute my path planning sequence. Upon receiving a signal from an external source, I am able to start and finish my path planning and achieve desirable results.

I am using the “scaled_joint_trajectory_controller” provided by UR which is similar to the “joint_trajectory_controller” and is of the same type “FollowJointTrajectory”.

My current objective is to immediately stop active trajectory execution in moveit2 using moveit2’s C++ APIs and prevent any upcoming trajectory execution upon receiving a signal from this external source. The desired outcome is that until this signal is released, move_group will not execute any trajectories and halt any active trajectory execution.

I tried the below strategies

  1. using asyncExecute() in one thread and using move_group->stop() in another thread to halt the trajectory.
  2. using “ros2 service call” to stop controllers.
  3. having a separate ros2 action client node that sends a zero velocity trajectory to the action server “scaled_joint_trajectory_controller/follow_joint_trajectory”.

Observed outcomes for the above strategies

  1. The move_group->stop() function is not being called at the right time. It’s only being called after the asyncExecute() function completes its execution. Which provides no value at all even though both the functions are run in separate threads.
  2. Using this approach, I was able to abruptly stop my active trajectory execution, but I was not able to regain control over my controllers, even though I deactivate and then reactivate my controllers. I got this error message from ros2_control_node stating that “goal_handle attempted invalid transition from state EXECUTING with event CANCELED”.
  3. This approach gave me chaotic results as I had trajectories from path planning node conflicting with trajectories from this action client node. So, I am forced to drop this strategy.

I am really curious on stopping an active trajectory execution in moveit2 and kindly request your support and this community’s support for my learning. I hope I had provided as much information to the best of my abilities and please let me know if you require any further information.

I hope this is solvable and I look forward to an engaging discussion.

2

I had the same issue when trying to implement a soft stop for the FollowJointTrajectory controller in ManyMove. As I understand, there is no stop command for the FJT, but if you send a new trajectory it preempts the previous one.
If it can be of any help, I solved by creating an action server that manages sending the trajectory to the FJT controller instead of using the controller’s action server itself. This way I can manage what to do on cancel: I send a single-point trajectory to the same FJT controller. The single-point trajectory uses as a point the current robot’s position, and as time_from_start a settable value.
This way you’d obtain a “spring-back” motion on stop.

It’s not on-par with the controlled stop obtained in industrial robots, but at least ends on a point of the trajectory. An alternative would be to create a trajectory that is composed by a segment of the remaining part of the original trajectory to implement an “on path” stop, but that’d be more complicated. Moreover I noticed that, when a trajectory is preempted, the transition to the new trajectory is not that smooth: you get a slight bump probably caused by a sudden acceleration spike.

I don’t know if this applies to your scenario, please mind I’m not an expert myself.

You can find the code in ManyMove on Github, in the manymove_planner repo, in the *_planner.cpp and in *action_server.cpp files