diff --git a/CMakeLists.txt b/CMakeLists.txt index 427a56194..80879b48b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -58,6 +58,7 @@ add_subdirectory(doc/motion_planning_api) add_subdirectory(doc/motion_planning_pipeline) add_subdirectory(doc/move_group_interface) add_subdirectory(doc/move_group_python_interface) +add_subdirectory(doc/multi_group) add_subdirectory(doc/perception_pipeline) add_subdirectory(doc/pick_place) add_subdirectory(doc/planning) diff --git a/doc/multi_group/CMakeLists.txt b/doc/multi_group/CMakeLists.txt new file mode 100644 index 000000000..82db9f896 --- /dev/null +++ b/doc/multi_group/CMakeLists.txt @@ -0,0 +1,5 @@ +add_executable(multi_group_tutorial src/multi_group_tutorial.cpp) +target_link_libraries(multi_group_tutorial ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +install(TARGETS multi_group_tutorial DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + +install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) \ No newline at end of file diff --git a/doc/multi_group/all_groups.png b/doc/multi_group/all_groups.png new file mode 100644 index 000000000..1720abbeb Binary files /dev/null and b/doc/multi_group/all_groups.png differ diff --git a/doc/multi_group/arm1.png b/doc/multi_group/arm1.png new file mode 100644 index 000000000..799c17ce3 Binary files /dev/null and b/doc/multi_group/arm1.png differ diff --git a/doc/multi_group/arm2.png b/doc/multi_group/arm2.png new file mode 100644 index 000000000..508d3bac4 Binary files /dev/null and b/doc/multi_group/arm2.png differ diff --git a/doc/multi_group/arm_dual.gif b/doc/multi_group/arm_dual.gif new file mode 100644 index 000000000..034e62216 Binary files /dev/null and b/doc/multi_group/arm_dual.gif differ diff --git a/doc/multi_group/arm_joint.png b/doc/multi_group/arm_joint.png new file mode 100644 index 000000000..efc4715ff Binary files /dev/null and b/doc/multi_group/arm_joint.png differ diff --git a/doc/multi_group/end_effectors.png b/doc/multi_group/end_effectors.png new file mode 100644 index 000000000..b903a9f0d Binary files /dev/null and b/doc/multi_group/end_effectors.png differ diff --git a/doc/multi_group/home_pose.png b/doc/multi_group/home_pose.png new file mode 100644 index 000000000..11c9dc794 Binary files /dev/null and b/doc/multi_group/home_pose.png differ diff --git a/doc/multi_group/multi_arm_tutorial.gif b/doc/multi_group/multi_arm_tutorial.gif new file mode 100644 index 000000000..ab8d9ec24 Binary files /dev/null and b/doc/multi_group/multi_arm_tutorial.gif differ diff --git a/doc/multi_group/multi_group_tutorial.rst b/doc/multi_group/multi_group_tutorial.rst new file mode 100644 index 000000000..d5cf33eef --- /dev/null +++ b/doc/multi_group/multi_group_tutorial.rst @@ -0,0 +1,111 @@ +Run multiple robot arms +=============================== +.. image:: multi_arm_tutorial.gif + :width: 500px + + +For running multiple robot arms in MoveIt, the following points need to be kept in mind : + 1. All robot arms should be defined in the same URDF. + 2. Each arm should have its own planning_group. + 3. If arms need to move in sync, they can be defined in a combined planning_group. + +But before starting with multiple arms, the concept of planning groups, there usage and significance of multiple planning groups for multiple end effectors +needs to be understood clearly. kindly go through `this `_ answer if you are a beginner/new to these concepts. + +Robot model used +---------------- +This tutorial will use the two panda arms for demonstration. The urdf of the two panda arms can be seen `here `_. + +Configuring the robot through the setup assistant +--------------------------------------------------- + +For moving the robot arms individually, we need to create two different planning groups for each arm and then plan for the two groups separately. Refer to `MoveIt setup assistant tutorial `_ +if you do not know the GUI based method for setting up your robot configuration for MoveIt. For synchronous movement, refer further down in the tutorial. + +.. image:: arm1.png + :width: 700px + +Subsequently add the joints of each arm into their respective groups. + +.. image:: arm_joint.png + :width: 700px + +Moving the two arms in sync +----------------------------- + +First of all define the individual arms and their end-effectors in respective groups as shown - + +.. image:: all_groups.png + :width: 900px + +Now define the end effectors - + +.. image:: end_effectors.png + :width: 800px + +Then combine all the planning groups into a common planning group, in this case - "both_bots" in the srdf file : :: + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +We can then define the initial pose of the combined group - + +.. image:: home_pose.png + :width: 700px + +Code for execution +------------------------------------------------------- +.. tutorial-formatter:: ./src/multi_group_tutorial.cpp + +Moving multiple arms asynchronously +-------------------------------------- + +Currently we can not plan for two separate trajectories for two different arms and execute them separately using the ``MoveGroupInterface``. +The workaround to this is to take the planned trajectories for left and right arm and send them as a goal to the action server of the robot arm .The problem however, +is the fact that there is no collision checking between the two trajectories, since the plans were generated without any knowledge of the other arms movement. +Refer to `this answer `_ for more details. \ No newline at end of file diff --git a/doc/multi_group/src/multi_group_tutorial.cpp b/doc/multi_group/src/multi_group_tutorial.cpp new file mode 100644 index 000000000..11131c163 --- /dev/null +++ b/doc/multi_group/src/multi_group_tutorial.cpp @@ -0,0 +1,149 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, Utkarsh Rai + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Utkarsh Rai nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Utkarsh Rai */ + +#include "ros/ros.h" +#include + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "multi_group_tutorial"); + ros::NodeHandle nh("~"); + + ros::AsyncSpinner spinner(1); + spinner.start(); + + std::string movegroup_name, ee1_link, ee2_link; + int counter = 1; + // BEGIN_TUTORIAL + // Setup + // ^^^^^ + // + // Setting up the dynamic parameters. Last arg is the default value. You can assign these from a launch file. + // The parameters ee1_link and ee2_link are used to specify the end effector for the two different arms. + // The parameter movegroup_name specifies the planning group. + // We have included both the arms under a combined planning group "both_bots" for moving the two arms in sync, as discussed above. + nh.param("move_group", movegroup_name, "both_bots"); + nh.param("ee_link1", ee1_link, "panda_1_link8" ); + nh.param("ee_link2", ee2_link, "panda_2_link8" ); + + // We choose the rate at which this node should run. For our case it's 0.2 Hz(5 seconds) + double ros_rate; + nh.param("ros_rate", ros_rate, 0.2); // 0.2 Hz = 5 seconds + ros::Rate* loop_rate_ = new ros::Rate(ros_rate); + + ros::Duration(1.0).sleep(); // sleep 1 second + + // The :planning_interface:`MoveGroupInterface` class is setup for the + // combined planning group of the arms. + moveit::planning_interface::MoveGroupInterface move_group(movegroup_name); + moveit::planning_interface::MoveGroupInterface::Plan my_plan; + + // Configure planner + move_group.setPlanningTime(0.5); + move_group.setPlannerId("RRTConnectkConfigDefault"); + move_group.setEndEffectorLink(ee1_link); + moveit::planning_interface::MoveItErrorCode success_plan = moveit_msgs::MoveItErrorCodes::FAILURE, + motion_done = moveit_msgs::MoveItErrorCodes::FAILURE; + + // Motion Planning + // ^^^^^^^^^^^^^^^^^^ + // Send the arms to the home position, if they are not there yet. + move_group.setStartStateToCurrentState(); + move_group.setNamedTarget("home_all"); + success_plan = move_group.plan(my_plan); + + if (success_plan == moveit_msgs::MoveItErrorCodes::SUCCESS) { + motion_done = move_group.execute(my_plan); + } + else + { + ROS_WARN("Something went wrong moving the robots to home position."); + } + + ros::Duration(2.0).sleep(); // 2 seconds + + // Specify the target pose and orientation of the end-effectors. + + geometry_msgs::PoseStamped target1, target2; + + target1.pose.position.x = 0.8; + target1.pose.position.y = 0.3; + target1.pose.position.z = 0.7; + target1.pose.orientation.w = 1; + target1.pose.orientation.x = 0; + target1.pose.orientation.y = 1; + target1.pose.orientation.z = 0; + + target2.pose.position.x = 0.6; + target2.pose.position.y = -0.3; + target2.pose.position.z = 0.7; + target2.pose.orientation.w = 1; + target2.pose.orientation.x = 0; + target2.pose.orientation.y = 1; + target2.pose.orientation.z = 0; + + // The ``setPoseTarget(target1, ee1_link)`` sets the target pose for the corresponding end-effector (ee1_link in this case). + // We alternate between the two target positions for the end-effectors in each iteration. + + while (ros::ok()) { + + if(counter == 0) { + move_group.setPoseTarget(target1, ee1_link); + move_group.setPoseTarget(target2, ee2_link); + + } else { + move_group.setPoseTarget(target2, ee1_link); + move_group.setPoseTarget(target1, ee2_link); + } + + success_plan = move_group.plan(my_plan); + + if (success_plan == moveit_msgs::MoveItErrorCodes::SUCCESS) { + motion_done = move_group.execute(my_plan); + } + if (motion_done) { + loop_rate_->sleep(); + } + + counter = (counter +1 ) % 2; + } + + ros::shutdown(); + + // END_TUTORIAL + + return 0; +} \ No newline at end of file diff --git a/index.rst b/index.rst index 0f152d6b2..f220e772d 100644 --- a/index.rst +++ b/index.rst @@ -51,6 +51,7 @@ Building more complex applications with MoveIt often requires developers to dig doc/subframes/subframes_tutorial doc/moveit_cpp/moveitcpp_tutorial doc/bullet_collision_checker/bullet_collision_checker + doc/multi_group/multi_group_tutorial Integration with a New Robot ----------------------------