Simultaneous trajectory execution

CMakeLists.txt

@@ -74,3 +74,4 @@ add_subdirectory(doc/collision_environments)
 add_subdirectory(doc/visualizing_collisions)
 add_subdirectory(doc/bullet_collision_checker)
 add_subdirectory(doc/mesh_filter)
+add_subdirectory(doc/simultaneous_trajectory_execution)

doc/controller_configuration/src/moveit_controller_manager_example.cpp

@@ -49,9 +49,13 @@ class ExampleControllerHandle : public moveit_controller_manager::MoveItControll
  {
  }
 
- bool sendTrajectory(const moveit_msgs::RobotTrajectory& /*t*/) 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;
  }
 

doc/simultaneous_trajectory_execution/CMakeLists.txt

@@ -0,0 +1,5 @@
+add_executable(simultaneous_trajectory_execution_tutorial src/simultaneous_trajectory_execution_tutorial.cpp)
+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})

doc/simultaneous_trajectory_execution/launch/simultaneous_trajectory_execution_tutorial.launch

@@ -0,0 +1,10 @@
+<launch>
+
+ <node name="simultaneous_trajectory_execution_tutorial"
+ pkg="moveit_tutorials"
+ type="simultaneous_trajectory_execution_tutorial"
+ respawn="false"
+ output="screen">
+ </node>
+
+</launch>

doc/simultaneous_trajectory_execution/simultaneous_trajectory_execution_tutorial.rst

@@ -0,0 +1,46 @@
+Simultaneous Trajectory Execution
+==================================
+
+Introduction
+------------
+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.
+
+
+The following GIF shows a simple example of simultaneous execution of trajectories through the **Rviz Motion Planning** 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>`_.
+
+(Optional) Setup
+---------------
+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
+----------------
+Open two shells. In the first shell start RViz and wait for everything to finish loading: ::
+
+ roslaunch moveit_resources_dual_panda_moveit_config demo.launch
+
+In the second shell, run the launch file for this demo: ::
+
+ roslaunch moveit_tutorials simultaneous_trajectory_execution_tutorial.launch
+
+Expected Output
+---------------
+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
+---------------
+The entire code can be seen :codedir:`here in the MoveIt GitHub project<simultaneous_trajectory_execution>`.
+
+.. tutorial-formatter:: ./src/simultaneous_trajectory_execution_tutorial.cpp
+
+The launch file
+---------------
+The entire launch file is :codedir:`here <simultaneous_trajectory_execution/launch/simultaneous_trajectory_execution_tutorial.launch>` on GitHub. All the code in this tutorial can be compiled and run from the moveit_tutorials package.

doc/simultaneous_trajectory_execution/src/simultaneous_trajectory_execution_tutorial.cpp

@@ -0,0 +1,87 @@
+/* Author: Cristian C. Beltran-Hernandez */
+
+#include <ros/ros.h>
+
+#include <memory>
+
+#include <geometry_msgs/PointStamped.h>
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <stdlib.h>
+
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "simultaneous_trajectory_execution_move_group");
+
+ // ROS spinning must be running for the MoveGroupInterface to get information
+ // about the robot's state. One way to do this is to start an AsyncSpinner
+ // beforehand.
+ ros::AsyncSpinner spinner(1);
+ spinner.start();
+
+ // BEGIN_TUTORIAL
+ //
+ // Setup
+ // ^^^^^
+ // Let's start by creating planning groups for each robot arm.
+ // The panda dual arm environment has two planning groups defined as `panda_1` and `panda_2`
+ moveit::planning_interface::MoveGroupInterface panda_1_group("panda_1");
+ moveit::planning_interface::MoveGroupInterface panda_2_group("panda_2");
+
+ // Now, let's define a target pose for `panda_1`
+ geometry_msgs::PoseStamped panda_1_target_pose;
+ panda_1_target_pose.header.frame_id = "base";
+ panda_1_target_pose.pose.position.x = 0.450;
+ panda_1_target_pose.pose.position.y = -0.50;
+ panda_1_target_pose.pose.position.z = 1.600;
+ panda_1_target_pose.pose.orientation.x = 0.993436;
+ panda_1_target_pose.pose.orientation.y = 3.5161e-05;
+ panda_1_target_pose.pose.orientation.z = 0.114386;
+ panda_1_target_pose.pose.orientation.w = 2.77577e-05;
+
+ // And one for `panda_2`
+ geometry_msgs::PoseStamped panda_2_target_pose;
+ panda_2_target_pose.header.frame_id = "base";
+ panda_2_target_pose.pose.position.x = 0.450;
+ panda_2_target_pose.pose.position.y = 0.40;
+ panda_2_target_pose.pose.position.z = 1.600;
+ panda_2_target_pose.pose.orientation.x = 0.993434;
+ panda_2_target_pose.pose.orientation.y = -7.54803e-06;
+ panda_2_target_pose.pose.orientation.z = 0.114403;
+ panda_2_target_pose.pose.orientation.w = 3.67256e-05;
+
+ // Planning
+ // ^^^^^^^^
+ // Let's plan a trajectory for `panda_1` using the previously defined target pose.
+ panda_1_group.clearPoseTargets();
+ panda_1_group.setStartStateToCurrentState();
+ panda_1_group.setPoseTarget(panda_1_target_pose);
+
+ moveit::planning_interface::MoveGroupInterface::Plan panda_1_plan;
+ bool success1 = (panda_1_group.plan(panda_1_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
+ if (!success1)
+ {
+ ROS_INFO("Plan with Panda 1 did not succeeded");
+ }
+
+ // Same for `panda_2`.
+ panda_2_group.clearPoseTargets();
+ panda_2_group.setStartStateToCurrentState();
+ panda_2_group.setPoseTarget(panda_2_target_pose);
+
+ moveit::planning_interface::MoveGroupInterface::Plan panda_2_plan;
+ bool success2 = (panda_2_group.plan(panda_2_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
+ if (!success2)
+ {
+ ROS_INFO("Plan with Panda 2 did not succeeded");
+ }
+
+ // Simultaneous Execution
+ // ^^^^^^^^^^^^^^^^^^^^^^
+ // Finally, let's execute both plans asynchronously to have them run simultaneously.
+ panda_1_group.asyncExecute(panda_1_plan);
+ panda_2_group.asyncExecute(panda_2_plan);
+ // END_TUTORIAL
+
+ ros::shutdown();
+ return 0;
+}

index.rst

@@ -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
 -----------