Skip to content

Commit

Permalink
Fix tutorial
Browse files Browse the repository at this point in the history
  • Loading branch information
cambel committed Nov 2, 2022
1 parent 378045d commit dec7a70
Show file tree
Hide file tree
Showing 5 changed files with 16 additions and 10 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -49,9 +49,13 @@ class ExampleControllerHandle : public moveit_controller_manager::MoveItControll
{
}

bool sendTrajectory(const moveit_msgs::RobotTrajectory& /*t*/, const ExecutionCompleteCallback& /*cb*/) override
bool sendTrajectory(const moveit_msgs::RobotTrajectory& /*t*/, const ExecutionCompleteCallback& cb) override
{
// do whatever is needed to actually execute this trajectory

// then if there is a callback, return the status of the execution. For example, signal success
if (cb)
cb(moveit_controller_manager::ExecutionStatus::SUCCEEDED);
return true;
}

Expand Down
2 changes: 1 addition & 1 deletion doc/simultaneous_trajectory_execution/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
add_executable(simultaneous_trajectory_execution_tutorial src/simultaneous_trajectory_execution_tutorial.cpp)
target_link_libraries(simultaneous_trajectory_execution_tutorial ${catkin_LIBRARIES} ${Boost_LIBRARIES})
target_link_libraries(simultaneous_trajectory_execution_tutorial ${catkin_LIBRARIES})
install(TARGETS simultaneous_trajectory_execution_tutorial DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})

install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
Original file line number Diff line number Diff line change
Expand Up @@ -3,24 +3,25 @@ Simultaneous Trajectory Execution

Introduction
------------
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 arm into a single trajectory. Optionally, a collision check is performed right before execution of new trajectories to prevent collisions with active trajectories.
MoveIt 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 arm into a single trajectory. Optionally, a collision check is performed right before execution of new trajectories to prevent collisions with active trajectories.


.. only:: html
The following GIF shows a simple example of simultaneous execution of trajectories through the **Rviz Motion Planning** plugin.

.. figure:: simultaneous-execution-rviz.gif
.. only:: html

Simultaneous execution of several trajectories through Rviz plugin.
.. figure:: images/simultaneous-execution-rviz.gif

This tutorial presents how to use the Simultaneous Trajectory Execution feature through the `Move Group C++ Interface <../move_group_interface/move_group_interface_tutorial.html>`_ but it can be similarly used through the `Move Group Python Interface <../move_group_python_interface/move_group_python_interface_tutorial.html>`_ or `MoveIt Cpp <../moveit_cpp/moveitcpp_tutorial.html>`_.

Getting Started
---------------
If you haven't already done so, make sure you've completed the steps in `Getting Started <../getting_started/getting_started.html>`_.

Setup
(Optional) Setup
---------------
The simultaneous trajectory execution feature can be enabled or disabled through the dynamic reconfigure parameter `/move_group/trajectory_execution/enable_simultaneous_execution`.
Optionally, an extra layer of collision checking, done right before execution of trajectories, can be enabled through the dynamic reconfigure parameter `/move_group/trajectory_execution/enable_collision_checking`.
The simultaneous execution feature is active by default. However, through the following dynamic reconfigure parameter, it can be disabled, **/move_group/trajectory_execution/enable_simultaneous_execution**.
Similarly, an extra layer of collision checking, performed right before execution of trajectories has been added to the `TrajectoryExecutionManager`, which can also be disabled through the dynamic reconfigure parameter **/move_group/trajectory_execution/enable_collision_checking**.

Running the code
----------------
Expand All @@ -34,7 +35,7 @@ In the second shell, run the launch file for this demo: ::

Expected Output
---------------
Though, two independent trajectories for two different joint groups have been planned, both can be simultaneously executed.
In a robotic environment with two Franka Panda robot arms, two different trajectories are planned, one for each robot arm. Then both trajectory are simultaneously executed.

The entire code
---------------
Expand Down
1 change: 1 addition & 0 deletions index.rst
Original file line number Diff line number Diff line change
Expand Up @@ -97,6 +97,7 @@ Miscellaneous
doc/benchmarking/benchmarking_tutorial
doc/tests/tests_tutorial
doc/test_debugging/test_debugging_tutorial
doc/simultaneous_trajectory_execution/simultaneous_trajectory_execution_tutorial

Attribution
-----------
Expand Down

0 comments on commit dec7a70

Please sign in to comment.