Draft: Simultaneous execution of trajectories #2810

cambel · 2021-08-13T05:52:50Z

@felixvd and I have been working on this PR

Description

This draft PR proposes a method to allow the execution of multiple controllers in several independent trajectory plans at the same time.
So far, MoveIt only supports the execution of one trajectory at a time. If a second trajectory is sent for execution, the first trajectory is aborted and the second one is attempted instead.

There are two main reasons for this limitation. First, all the action servers are of the type SimpleActionServer which can only serve one goal at a time and immediately reject any old goal as a new one is received. Second, the TrajectoryExecutionManager only supports the execution of one trajectory at a time by default, aborting any previous trajectory as a new one is pushed.

The first issue is addressed by converting the SimpleActionServers into (General) ActionServers. This allows the server to handle multiple goals at the same time.
The second issue is addressed by implementing a scheduling procedure to execute trajectories simultaneously as long as the following conditions are met:

The controllers needed for executing the trajectory are not being used.
The trajectory is not in collision with any active trajectory.
The trajectory is not in collision with the current state. This is assumed in the current trajectory execution scheme as it is expected that the planning covers this validation. For most cases, it works fine, but for simultaneous execution of trajectories, that assumption does not hold anymore, so an extra validation is needed.
The start state of the trajectory matched that of the current state of the robot.

If the trajectory does not meet the conditions mentioned above, the trajectory is stored in a backlog and periodically checked. The trajectories in the backlog and newly received trajectories are evaluated and executed in a strict FIFO order.

Our use case: Simultaneous control of robots with different sets of instructions that do not have/need to be synchronized in any particular way. See this simple equip/unequip routine where both robots can follow their independent set of instructions without waiting for the other one.

equip.unequip.tool.mp4

There are some issues pending to be fixed:

Re-enable feedback for the action servers.
Simultaneous planning seems to crash MoveIt/move_group. It is a non-deterministic bug that we are still trying to understand. I am trying to debug here. The problem is not uncommon, it happens during the planning, as we do plan_only and then execute the plans. It even happens with only one robot planning at a time. The crash happens when sending a solution plan to the goal_handle (of the ActionServer), for whatever reason the executed trajectory of the action response (MoveGroupResult) is corrupted because something cannot be accessed Cannot access memory at address.
Unless I am mistaken, the execute_trajectory_service also needs to be changed over to simultaneous execution
There are no tests for the new behavior yet

Comments and contributions are highly appreciated!

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

welcome · 2021-08-13T05:52:52Z

Thanks for helping in improving MoveIt and open source robotics!

felixvd · 2021-08-13T07:35:39Z

Fixes #2287

@JafarAbdi @henningkayser @tylerjw This is the project I mentioned in the call yesterday and the maintainer meeting. Some issues remain, but we have been running this for a few weeks and it should be ready for testing and feedback from a fresh set of eyes. There are infrequent crashes, probably related to thread safety in move_action.cpp. It would be great if someone could have a look.

Some more notes:

We renamed the functions and variables in TrajectoryExecutionManager to specify their behavior (blocking vs simultaneous).
The class originally had two quasi-redundant behaviors. The logic of the blocking queue could probably be removed now, but there are some calls in plan_execution.cpp and plan_with_sensing.cpp and I am not too familiar with those files. I'd like a second opinion. @v4hn ?
We added the group_name to RobotTrajectory because we need to know it for tracking the active joints in the TEM class.

Since the video above does not do this PR justice (slow PlanningScene update rate and ugly CollisionObjects 💩), I want to add some more to drive home the point how sweet and deserving of enthusiastic reviews this is:

Two arms rearranging an objects in their own part of the workspace independently, each planning in their own thread in peace, simultaneously, without a care in the world

simultaneous-rearrange.mp4

The right arm aligns the screw holes of the bearing housing (using vision) while the left arm picks the tool and a screw from the feeder in the back

bearing-align.mp4

The right arm inserts the shaft while the left arm picks screws and fastens the bearing. Note how close the robots pass each other and how suicidal this would be without the TrajectoryExecutionMonitor ensuring that the trajectories don't collide.

shaft-insert-3.mp4

Anyway, I hope we can get the kinks ironed out soon. I think this PR is in everyone's interest. Thanks!

PS: I originally thought that the collision check between two trajectories could be done more efficiently with gpu-voxels, so we wasted a bit of time on trying that. That was absolutely premature optimization though, the performance as is seems perfectly fine.

henningkayser

@cambel @felixvd Whoa, this is a lot. Great work so far! I really like the general idea of having an ongoing loop in the TEM that keeps track of any active trajectories and controllers. By validating incoming trajectories with all active instances in the beginning it should be possible to ensure we are at least commanding collision-free trajectories (fingers crossed that the controllers are actually doing what we command ;)). I haven't fully understood all of the new logic yet, I'll follow up with more in-depth and structural feedback. I also still need to make up my mind about how concurrent actions could be handled in general.

So far, my biggest concern is that collision checking between trajectories is inherently unsafe if we do it the way it's implemented right now. Self collisions have reduced padding by default and the resolution of trajectory waypoints is usually not sufficient to really guarantee that different groups don't run into each other.
As already mentioned in one of the comments, I think that we need to:

Increase padding for self collisions (= collisions between different groups)
Ensure a minimal joint distance resolution similar to longest_valid_fragment_section
Use a "continuous" collision check that validates the full segment between two consecutive waypoints. I'm not sure if this is currently even supported for self-collisions, but I imagine that this would be the most secure and efficient solution long-term. This could make the minimal joint distance threshold redundant.

Using a very big padding with a reasonable trajectory resolution should be fine for most use cases, but you could still not really rule out collisions for all cases as small joint state jumps can produce larger Cartesian distances.

Do you have a testing setup that you could share for testing?

henningkayser · 2021-09-07T17:35:10Z

moveit_core/robot_trajectory/src/robot_trajectory.cpp

@@ -231,6 +231,7 @@ void RobotTrajectory::getRobotTrajectoryMsg(moveit_msgs::RobotTrajectory& trajec

  std::vector<const moveit::core::JointModel*> onedof;
  std::vector<const moveit::core::JointModel*> mdof;
+  trajectory.group_name = group_ ? group_->getName() : "";


Could you open a PR to add group_name to the message?

moveit/moveit_msgs#133 for reference

henningkayser · 2021-09-07T17:37:09Z

moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp

      root_node_handle_, "sequence_move_group",
      boost::bind(&MoveGroupSequenceAction::executeSequenceCallback, this, _1), false);
-  move_action_server_->registerPreemptCallback(boost::bind(&MoveGroupSequenceAction::preemptMoveCallback, this));
+  // move_action_server_->registerPreemptCallback(boost::bind(&MoveGroupSequenceAction::preemptMoveCallback, this));


Use registerCancelCallback()?

henningkayser · 2021-09-07T17:43:26Z

moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp

-  move_state_ = state;
-  move_feedback_.state = stateToStr(state);
-  move_action_server_->publishFeedback(move_feedback_);
+  // move_state_ = state;


We probably need to keep track of states for each goal separately and pass the goal handle into this function for publishing feedback

henningkayser · 2021-09-07T17:45:45Z

moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controllers.h

@@ -60,6 +60,8 @@ class BaseFakeController : public moveit_controller_manager::MoveItControllerHan
 protected:
  std::vector<std::string> joints_;
  const ros::Publisher& pub_;
+  moveit_controller_manager::ExecutionStatus status_;
+  std::string name_ = "fake_controllers";


Better set the default with the constructor.

Also, if this is only used for the logger names, I'd prefer adding a constexpr char LOGNAME[] to the source file instead of a member. See code style guidelines

henningkayser · 2021-09-07T17:51:36Z

moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp

-    else
-      move_action_server_->setAborted(action_res, response);
-  }
+    goal_handle.setAborted(action_res, response);


Also here, setCanceled() could replace setPreempted()

henningkayser · 2021-09-08T16:32:14Z