diff --git a/CMakeLists.txt b/CMakeLists.txt index c394c7c46..a64b09bc8 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/doc/controller_configuration/src/moveit_controller_manager_example.cpp b/doc/controller_configuration/src/moveit_controller_manager_example.cpp index 9f3c2fb9d..da76836a9 100644 --- a/doc/controller_configuration/src/moveit_controller_manager_example.cpp +++ b/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; } diff --git a/doc/simultaneous_trajectory_execution/CMakeLists.txt b/doc/simultaneous_trajectory_execution/CMakeLists.txt new file mode 100644 index 000000000..7c923f824 --- /dev/null +++ b/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}) diff --git a/doc/simultaneous_trajectory_execution/images/simultaneous-execution-rviz.gif b/doc/simultaneous_trajectory_execution/images/simultaneous-execution-rviz.gif new file mode 100644 index 000000000..396ef716b Binary files /dev/null and b/doc/simultaneous_trajectory_execution/images/simultaneous-execution-rviz.gif differ diff --git a/doc/simultaneous_trajectory_execution/launch/simultaneous_trajectory_execution_tutorial.launch b/doc/simultaneous_trajectory_execution/launch/simultaneous_trajectory_execution_tutorial.launch new file mode 100644 index 000000000..2b72a4ce4 --- /dev/null +++ b/doc/simultaneous_trajectory_execution/launch/simultaneous_trajectory_execution_tutorial.launch @@ -0,0 +1,10 @@ + + + + + + diff --git a/doc/simultaneous_trajectory_execution/simultaneous_trajectory_execution_tutorial.rst b/doc/simultaneous_trajectory_execution/simultaneous_trajectory_execution_tutorial.rst new file mode 100644 index 000000000..03c9e7a68 --- /dev/null +++ b/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`. + +.. tutorial-formatter:: ./src/simultaneous_trajectory_execution_tutorial.cpp + +The launch file +--------------- +The entire launch file is :codedir:`here ` on GitHub. All the code in this tutorial can be compiled and run from the moveit_tutorials package. diff --git a/doc/simultaneous_trajectory_execution/src/simultaneous_trajectory_execution_tutorial.cpp b/doc/simultaneous_trajectory_execution/src/simultaneous_trajectory_execution_tutorial.cpp new file mode 100644 index 000000000..6db21e9ea --- /dev/null +++ b/doc/simultaneous_trajectory_execution/src/simultaneous_trajectory_execution_tutorial.cpp @@ -0,0 +1,87 @@ +/* Author: Cristian C. Beltran-Hernandez */ + +#include + +#include + +#include +#include +#include + +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; +} diff --git a/index.rst b/index.rst index f10a6cbc1..be7244485 100644 --- a/index.rst +++ b/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 -----------