Tutorial for running multiple groups/arms.

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)

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})

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 <https://answers.ros.org/question/321843/how-is-moveit-planning-group-supposed-to-be-used/>`_ 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 <https://github.com/frankaemika/franka_ros/blob/kinetic-devel/franka_description/robots/dual_panda_example.urdf.xacro>`_.
+
+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 <http://moveit2_tutorials.picknik.ai/doc/setup_assistant/setup_assistant_tutorial.html>`_
+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 : ::
+
+    <group name="arm1">
+        <joint name="virtual_joint" />
+        <joint name="panda_1_joint_base" />
+        <joint name="panda_1_joint1" />
+        <joint name="panda_1_joint2" />
+        <joint name="panda_1_joint3" />
+        <joint name="panda_1_joint4" />
+        <joint name="panda_1_joint5" />
+        <joint name="panda_1_joint6" />
+        <joint name="panda_1_joint7" />
+        <joint name="panda_1_joint8" />
+        <chain base_link="panda_1_link0" tip_link="panda_1_link8" />
+    </group>
+    <group name="arm2">
+        <joint name="panda_2_joint_base" />
+        <joint name="panda_2_joint1" />
+        <joint name="panda_2_joint2" />
+        <joint name="panda_2_joint3" />
+        <joint name="panda_2_joint4" />
+        <joint name="panda_2_joint5" />
+        <joint name="panda_2_joint6" />
+        <joint name="panda_2_joint7" />
+        <joint name="panda_2_joint8" />
+        <chain base_link="panda_2_link0" tip_link="panda_2_link8" />
+    </group>
+    <group name="robot1">
+        <group name="arm1" />
+        <group name="hand1" />
+    </group>
+    <group name="robot2">
+        <group name="arm2" />
+        <group name="hand2" />
+    </group>
+    <group name="hand1">
+        <joint name="panda_1_hand_joint" />
+        <joint name="panda_1_finger_joint1" />
+    </group>
+    <group name="hand2">
+        <joint name="panda_2_hand_joint" />
+        <joint name="panda_2_finger_joint1" />
+    </group>
+    <group name="both_bots">
+        <group name="arm1" />
+        <group name="arm2" />
+        <group name="hand1" />
+        <group name="hand2" />
+    </group>
+
+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 <https://answers.ros.org/question/374907/multirobot-moveit-controller-management/>`_ for more details.

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 <moveit/move_group_interface/move_group_interface.h>
+
+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<std::string>("move_group", movegroup_name, "both_bots");
+  nh.param<std::string>("ee_link1", ee1_link, "panda_1_link8" );
+  nh.param<std::string>("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;
+}

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