Author: Jordi Pages < jordi.pages@pal-robotics.com >

Maintainer: Jordi Pages < jordi.pages@pal-robotics.com >

Support: tiago-support@pal-robotics.com

Source: https://github.com/pal-robotics/tiago_tutorials.git

Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Planning in joint space

Purpose

This tutorial shows how to use MoveIt! in order to bring the torso-arm group of joints of TIAGo to a desired joint space configuration ensuring joint limit avoidance and self-collision avoidance. The example is given in C++.

Pre-requisites

First make sure that the tutorials are properly installed along with the TIAGo simulation, as shown in the Tutorials Installation Section.

Execution

First open two consoles and source the public simulation workspace as follows:

• cd /tiago_public_ws/
  source ./devel/setup.bash

Launching the simulation

In the first console launch the following simulation

• roslaunch tiago_gazebo tiago_gazebo.launch public_sim:=true end_effector:=pal-gripper

Gazebo will show up with TIAGo.

Wait until TIAGo has tucked its arm. Then you may proceed with the next steps.

Launching the nodes

Now we are going to run an example that will bring TIAGO to the following joint space configuration of the torso-arm group:

• torso_joint: 0
  arm_1_joint: 2.7
  arm_2_joint: 0.2
  arm_3_joint: -2.1
  arm_4_joint: 2.0
  arm_5_joint: 1.0
  arm_6_joint:-0.8
  arm_7_joint: 0

Note that if we try to set the above kinematic configuration moving individual joints we will end up probably in a self-collision. The node that will take care to find a plan, i.e. a sequence of movements, to reach such a kinematic configuration is plan_arm_torso_fk included in tiago_moveit_tutorial package and has to be called as follows

• rosrun tiago_moveit_tutorial plan_arm_torso_fk  0 2.7 0.2 -2.1 2.0 1.0 -0.8 0

An example of plan executed by the node is depicted in the following sequence of images:

Inspecting the code

The code to implement such a node able to plan in joint space is shown below. Note that the key parts of the code are:

• Choose a group of joints
• Choose a planner
• Set initial and desired joint state
• Give time to find a plan
• Execute the plan if found

     1 // ROS headers
     2 #include <ros/ros.h>
     3 
     4 // MoveIt! headers
     5 #include <moveit/move_group_interface/move_group_interface.h>
     6 
     7 // Std C++ headers
     8 #include <string>
     9 #include <vector>
    10 #include <map>
    11 
    12 int main(int argc, char** argv)
    13 {
    14   ros::init(argc, argv, "plan_arm_torso_fk");
    15 
    16   if ( argc < 9 )
    17   {
    18     ROS_INFO(" ");
    19     ROS_INFO("\tUsage:");
    20     ROS_INFO(" ");
    21     ROS_INFO("\trosrun tiago_moveit_tutorial plan_arm_torso_fk torso_lift arm_1 arm_2 arm_3 arm_4 arm_5 arm_6 arm_7");
    22     ROS_INFO(" ");
    23     ROS_INFO("\twhere the list of arguments are the target values for the given joints");
    24     ROS_INFO(" ");
    25     return EXIT_FAILURE;
    26   }
    27 
    28   std::map<std::string, double> target_position;
    29 
    30   target_position["torso_lift_joint"] = atof(argv[1]);
    31   target_position["arm_1_joint"] = atof(argv[2]);
    32   target_position["arm_2_joint"] = atof(argv[3]);
    33   target_position["arm_3_joint"] = atof(argv[4]);
    34   target_position["arm_4_joint"] = atof(argv[5]);
    35   target_position["arm_5_joint"] = atof(argv[6]);
    36   target_position["arm_6_joint"] = atof(argv[7]);
    37   target_position["arm_7_joint"] = atof(argv[8]);
    38 
    39   ros::NodeHandle nh;
    40   ros::AsyncSpinner spinner(1);
    41   spinner.start();
    42 
    43   std::vector<std::string> torso_arm_joint_names;
    44   //select group of joints
    45   moveit::planning_interface::MoveGroup group_arm_torso("arm_torso");
    46   //choose your preferred planner
    47   group_arm_torso.setPlannerId("SBLkConfigDefault");
    48 
    49   torso_arm_joint_names = group_arm_torso.getJoints();
    50 
    51   group_arm_torso.setStartStateToCurrentState();
    52   group_arm_torso.setMaxVelocityScalingFactor(1.0);
    53 
    54   for (unsigned int i = 0; i < torso_arm_joint_names.size(); ++i)
    55     if ( target_position.count(torso_arm_joint_names[i]) > 0 )
    56     {
    57       ROS_INFO_STREAM("\t" << torso_arm_joint_names[i] << " goal position: " << target_position[torso_arm_joint_names[i]]);
    58       group_arm_torso.setJointValueTarget(torso_arm_joint_names[i], target_position[torso_arm_joint_names[i]]);
    59     }
    60 
    61   moveit::planning_interface::MoveGroup::Plan my_plan;
    62   group_arm_torso.setPlanningTime(5.0);
    63   bool success = group_arm_torso.plan(my_plan);
    64 
    65   if ( !success )
    66     throw std::runtime_error("No plan found");
    67 
    68   ROS_INFO_STREAM("Plan found in " << my_plan.planning_time_ << " seconds");
    69 
    70   // Execute the plan
    71   ros::Time start = ros::Time::now();
    72 
    73   group_arm_torso.move();
    74 
    75   ROS_INFO_STREAM("Motion duration: " << (ros::Time::now() - start).toSec());
    76 
    77   spinner.stop();
    78 
    79   return EXIT_SUCCESS;
    80 }
  

Note that when a plan is found and is executed with the following line of code

  73   group_arm_torso.move();

The required control commands are sent to the arm and torso controllers through their Action interfaces:

• /arm_controller/follow_joint_trajectory/goal
  /torso_controller/follow_joint_trajectory/goal