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

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

plan_joint_space_title.jpg

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 robot:=steel

Gazebo will show up with TIAGo.

tiago_steel_gazebo.jpg

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:

joint_state_planning_example.jpg

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

Wiki: Robots/TIAGo/Tutorials/MoveIt/Planning_joint_space (last edited 2018-03-02 08:51:51 by VictorLopez)