Author: Alessandro Di Fava < alessandro.difava@pal-robotics.com >

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

Source: https://github.com/pal-robotics/tiago_dual_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

Description: How to reach a given joint space configuration using motion planning based on MoveIt!

Keywords: Motion planning, joint states, forward kinematics

Tutorial Level: INTERMEDIATE

Next Tutorial: Planning in cartesian space

Contents

Purpose
Pre-requisites
Execution

Purpose

This tutorial shows how to use MoveIt! in order to bring the arm_left_torso or the arm_right_torso 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_dual_public_ws
source ./devel/setup.bash

Launching the simulation

In the first console launch the following simulation

roslaunch tiago_dual_gazebo tiago_dual_gazebo.launch public_sim:=true end_effector_left:=pal-gripper end_effector_right:=pal-gripper

Gazebo will show up with TIAGo++ with a 6-axis force/torque sensor in each wrist and the parallel gripper.

tiago++_gazebo_grippers.jpg

Wait until TIAGo++ has tucked its arms. 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 both the arms groups:

torso_joint: 0.15
arm_left_1_joint/arm_right_1_joint: 1.5
arm_left_2_joint/arm_right_2_joint: 0.58
arm_left_3_joint/arm_right_3_joint: 0.06
arm_left_4_joint/arm_right_4_joint: 1.0
arm_left_5_joint/arm_right_5_joint: -1.70
arm_left_6_joint/arm_right_6_joint: 0.0
arm_left_7_joint/arm_right_7_joint: 0.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_dual_arm_torso_fk included in tiago_dual_moveit_tutorial package and has to be called as follows for the left arm

rosrun tiago_dual_moveit_tutorial plan_dual_arm_torso_fk left 0.15 1.5 0.58 0.06 1.0 -1.70 0.0 0.0

and as follows for the right arm

rosrun tiago_dual_moveit_tutorial plan_dual_arm_torso_fk right 0.15 1.5 0.58 0.06 1.0 -1.70 0.0 0.0

An example of plan executed by the node for the left arm 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_dual_arm_torso_fk");
  15 
  16   if ( argc < 10 )
  17   {
  18     ROS_INFO(" ");
  19     ROS_INFO("\tUsage:");
  20     ROS_INFO(" ");
  21     ROS_INFO("\trosrun tiago_moveit_tutorial plan_dual_arm_torso_fk [left|right] 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::string arm_name = argv[1];
  29   std::map<std::string, double> target_position;
  30 
  31   target_position["torso_lift_joint"] = atof(argv[2]);
  32   std::string target_joint = "arm_" + arm_name + "_1_joint";
  33   target_position[target_joint] = atof(argv[3]);
  34   target_joint = "arm_" + arm_name + "_2_joint";
  35   target_position[target_joint] = atof(argv[4]);
  36   target_joint = "arm_" + arm_name + "_3_joint";
  37   target_position[target_joint] = atof(argv[5]);
  38   target_joint = "arm_" + arm_name + "_4_joint";
  39   target_position[target_joint] = atof(argv[6]);
  40   target_joint = "arm_" + arm_name + "_5_joint";
  41   target_position[target_joint] = atof(argv[7]);
  42   target_joint = "arm_" + arm_name + "_6_joint";
  43   target_position[target_joint] = atof(argv[8]);
  44   target_joint = "arm_" + arm_name + "_7_joint";
  45   target_position[target_joint] = atof(argv[9]);
  46 
  47   ros::NodeHandle nh;
  48   ros::AsyncSpinner spinner(1);
  49   spinner.start();
  50 
  51   std::vector<std::string> torso_arm_joint_names;
  52   //select group of joints
  53   std::string moveit_group = "arm_" + arm_name + "_torso";
  54   moveit::planning_interface::MoveGroupInterface group_arm_torso(moveit_group);
  55   //choose your preferred planner
  56   group_arm_torso.setPlannerId("SBLkConfigDefault");
  57 
  58   torso_arm_joint_names = group_arm_torso.getJoints();
  59 
  60   group_arm_torso.setStartStateToCurrentState();
  61   group_arm_torso.setMaxVelocityScalingFactor(1.0);
  62 
  63   for (unsigned int i = 0; i < torso_arm_joint_names.size(); ++i)
  64     if ( target_position.count(torso_arm_joint_names[i]) > 0 )
  65     {
  66       ROS_INFO_STREAM("\t" << torso_arm_joint_names[i] << " goal position: " << target_position[torso_arm_joint_names[i]]);
  67       group_arm_torso.setJointValueTarget(torso_arm_joint_names[i], target_position[torso_arm_joint_names[i]]);
  68     }
  69 
  70   moveit::planning_interface::MoveGroupInterface::Plan my_plan;
  71   group_arm_torso.setPlanningTime(5.0);
  72   bool success = bool(group_arm_torso.plan(my_plan));
  73 
  74   if ( !success )
  75     throw std::runtime_error("No plan found");
  76 
  77   ROS_INFO_STREAM("Plan found in " << my_plan.planning_time_ << " seconds");
  78 
  79   // Execute the plan
  80   ros::Time start = ros::Time::now();
  81 
  82   moveit::planning_interface::MoveItErrorCode e = group_arm_torso.move();
  83   if (!bool(e))
  84     throw std::runtime_error("Error executing plan");
  85 
  86   ROS_INFO_STREAM("Motion duration: " << (ros::Time::now() - start).toSec());
  87 
  88   spinner.stop();
  89 
  90   return EXIT_SUCCESS;
  91 }

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

  82   moveit::planning_interface::MoveItErrorCode e = group_arm_torso.move();

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

/arm_left_controller/follow_joint_trajectory/goal
/arm_right_controller/follow_joint_trajectory/goal
/torso_controller/follow_joint_trajectory/goal

Note also the following line of code

  53   std::string moveit_group = "arm_" + arm_name + "_torso";

Here we are selecting the group of arm and torso related to the correspondent left or right arm torso group. There is also the possibility to select the both_arms_torso group where you can control arm_right, arm_left and torso contemporaneously.

ROS 2 Documentation

Wiki

Page

User