-
Notifications
You must be signed in to change notification settings - Fork 946
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Simultaneous trajectory execution #3243
base: master
Are you sure you want to change the base?
Conversation
Codecov ReportBase: 61.95% // Head: 62.12% // Increases project coverage by
Additional details and impacted files@@ Coverage Diff @@
## master #3243 +/- ##
==========================================
+ Coverage 61.95% 62.12% +0.18%
==========================================
Files 380 381 +1
Lines 33597 34097 +500
==========================================
+ Hits 20811 21180 +369
- Misses 12786 12917 +131
Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here. ☔ View full report at Codecov. |
Hi, |
Hi @Amin173 Thank you for checking this new feature. This PR introduces a new layer of collision checking right before the execution of a trajectory which is set ON/OFF based on the Dynamic Reconfigure parameter |
I plan on making an attempt at porting this to ROS2 for my own selfish needs. Stay tuned. Thanks for your awesome hard work on this @cambel, will email you if I run into issues. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
minor finds.
...execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h
Outdated
Show resolved
Hide resolved
moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp
Outdated
Show resolved
Hide resolved
b8e29da
to
95b2dda
Compare
95b2dda
to
ac077aa
Compare
ac077aa
to
f489b66
Compare
a520e8d
to
c8e68fe
Compare
std::unique_lock<std::mutex> ulock(execution_complete_mutex_); | ||
active_trajectories_.push_back(trajectory_id); | ||
} | ||
|
||
if (blocking) |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@cambel there is no case for blocking==false
, right? We were thinking about removing this blocking flag completely in MoveIt2.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Hi @AndyZe
For this new feature, blocking==false
is used to send multiple trajectories to be simultaneously executed without waiting for each one to finish before executing the next one, as in this demo:
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
OK, thanks for the reply.
Shouldn't there be an else()
here? Like this:
if (blocking)
{
...
}
else
{
// Still need to execute the trajectory somehow, right?
}
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Thank you, I was missing the case where simultaneous execution mode is OFF and blocking==false
.
Attempt to cancel the execution of only the trajectory related to the `GoalHandle`
@simonschmeisser code review
Explicitly defaulted move constructor is implicitly deleted mutex
Given a TrajectoryID and a new trajectory. Allow replacing the active trajectory for the new trajectory.
- Missing the case where simultaneous execution mode is OFF and `blocking` is false
c8e68fe
to
dea7b37
Compare
e1b0ec1
to
fdc3b1c
Compare
Dear all, Are there any plans to merge this PR? This feature being missing limits using MoveIt in any setup with 2+, independent robots. Those are a lot of cases, and I think this would benefit a lot of users. Also, can we assume that any feature developed for |
Main PR for GSoC2022 project Simultaneous Trajectory Execution (STE) #3156.
With this PR, MoveIt now allows simultaneous execution of trajectories, as long as, each trajectory uses a different set of controllers. For example, in a dual-arm environment, each arm can execute a different set of trajectories without needing to wait for the other arm to finish moving or manually synchronizing the motion of both arms into a single trajectory. Optionally, a collision check is performed right before the execution of new trajectories to prevent collisions with active trajectories.
Example use cases:
MoveIt Motion Planning Rviz
plugin.A test environment can be found here.
Feature description
Event-based execution system: Events related to the execution of trajectories are pushed to a queue where they are processed sequentially.
When a new trajectory is pushed, it is immediately validated, by checking that the required controllers are active and not in use and that there are no collisions with active trajectories or the current state of the planning scene. Then, the trajectory is sent for execution to the corresponding controllers.
The execution of each trajectory part will result in the event
EXECUTION_COMPLETED
being triggered. It marks the completion of the execution from the controller's side regardless of the status (SUCCEEDED, ABORTED, ...). If the status of the execution for a trajectory part is SUCCEEDED, we wait until all other parts are completed successfully. If the status of the trajectory is not successful, all other trajectory parts are canceled.The execution of a trajectory can result in the event
EXECUTION_TIMEOUT
being triggered. This occurs when the trajectory execution duration monitor is enabled and the trajectory takes longer to execute than expected. When triggered, all trajectory parts for this trajectory are canceled.When a specific trajectory is canceled, the event
EXECUTION_CANCELLATION_REQUEST
is triggered.Interdependent set of trajectories: The user can define a set of trajectories to be executed in strictly sequential order. In such cases, all the required controllers for the set of trajectories would be locked so that no other trajectory can use them. Example use case: In a picking task, the user would send a trajectory for the robot arm to get into a grasping pose and a trajectory for the gripper to close after reaching the grasping pose (two separate trajectories). After the execution of this set of trajectories starts, new trajectories that attempt to use the gripper would be rejected.
Presentation: https://docs.google.com/presentation/d/11hsWSR18X-YGj1ugzUKmdQeB3_EAymgB/edit#slide=id.p1
Setup
The simultaneous trajectory execution feature can be enabled or disabled through the dynamic reconfigure parameter
/move_group/trajectory_execution/enable_simultaneous_execution
.Optionally, the extra layer of collision checking, done right before the execution of trajectories, can be enabled through the dynamic reconfigure parameter
/move_group/trajectory_execution/enable_collision_checking
.The TrajectoryExecutionManager API remains the same for execution in blocking-mode:
TrajectoryExecutionManager::push()
.TrajectoryExecutionManager::execute()
.TrajectoryExecutionManager::stopExecution()
.For execution in simultaneous-mode:
TrajectoryExecutionManager::push()
will be automatically executed. If the trajectory is pushed and executed successfully, a TrajectoryID will be returned.TrajectoryExecutionManager::execute()
method will do nothing.TrajectoryExecutionManager::push(vector<Trajectory>)
.TrajectoryExecutionManager::stopExecution(TrajectoryID)
.Changes to the API
TrajectoryExecutionManager::push()
method optionally accepts a callback that is called when the trajectory execution is completed or aborted.TrajectoryExecutionManager::push(vector<Trajectory>)
method optionally accepts an additional callback that is called after each trajectory "segment" is completed successfully.MoveItCPP
In simultaneous-mode, when multiple trajectories are executed in non-blocking mode
MoveItCpp::execute(group_name, robot_trajectory, blocking=false)
, each one can be executed simultaneously, if they are valid.Move Group Action Servers
To allow simultaneous execution of trajectories through the
move group interfaces
theSimpleActionServers
MoveActionCapabilityServer
andExecuteTrajectoryActionServer
were change toActionServers
. Note: TheSimpleActionClient
seem to work fine with theActionServer
.Code review
This PR was previously tracked on cambel#3
Dependencies
This PR depends on:
Limitations
MoveGroupActionServer
andMoveGroupSequenceActionServer
useplan_execution
which currently does not have an interface for canceling a specific trajectory sent to theTrajectoryExecutionManager
. So, for now, canceling aGoal
on these servers would cancel everything.Checklist