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

Description: UseMoveIt! to plan a joint trajectory in order to reach a given pose in cartesian space

Keywords: Motion planning, cartesian space, inverse kinematics

Tutorial Level: INTERMEDIATE

Next Tutorial: Pick and place

tiago_planning_ik_title.jpg

Purpose

This tutorial shows how to use MoveIt! in order to move the end-effector frame of TIAGo to a desired pose in cartesian space. An 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_planning_ik_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's end-effector frame, i.e. arm_tool_link, to the following cartesian space configuration with respect to /base_footprint:

  • x: 0.4
    y: -0.3
    z: 0.26
    Roll: -0.011
    Pitch: 1.57
    Yaw: 0.037

which corresponds to the following pose as shown in rviz:

tiago_goal_pose_planning_ik.jpg

In order to safely reach such a cartesian goal the node plan_arm_torso_ik included in tiago_moveit_tutorial package and has to be called as follows

  • rosrun tiago_moveit_tutorial plan_arm_torso_ik 0.4 -0.3 0.26 -0.011, 1.57, 0.037

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

tiago_planning_ik_example.jpg

Note that the final pose of /arm_tool_link is the desired one:

tiago_reached_pose_planning_ik_rviz.jpg

Inspecting the code

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

  • Choose a group of joints
  • Choose a planner and define the reference frame, i.e. /base_footprint in this case
  • Set desired pose of /arm_tool_link encoded in a geometry_msgs::PoseStamped

  • 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_ik");
      15 
      16   if ( argc < 7 )
      17   {
      18     ROS_INFO(" ");
      19     ROS_INFO("\tUsage:");
      20     ROS_INFO(" ");
      21     ROS_INFO("\trosrun tiago_moveit_tutorial plan_arm_torso_ik  x y z  r p y");
      22     ROS_INFO(" ");
      23     ROS_INFO("\twhere the list of arguments specify the target pose of /arm_tool_link expressed in /base_footprint");
      24     ROS_INFO(" ");
      25     return EXIT_FAILURE;
      26   }
      27 
      28   geometry_msgs::PoseStamped goal_pose;
      29   goal_pose.header.frame_id = "base_footprint";
      30   goal_pose.pose.position.x = atof(argv[1]);
      31   goal_pose.pose.position.y = atof(argv[2]);
      32   goal_pose.pose.position.z = atof(argv[3]);
      33   goal_pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(atof(argv[4]), atof(argv[5]), atof(argv[6]));
      34 
      35   ros::NodeHandle nh;
      36   ros::AsyncSpinner spinner(1);
      37   spinner.start();
      38 
      39   std::vector<std::string> torso_arm_joint_names;
      40   //select group of joints
      41   moveit::planning_interface::MoveGroup group_arm_torso("arm_torso");
      42   //choose your preferred planner
      43   group_arm_torso.setPlannerId("SBLkConfigDefault");
      44   group_arm_torso.setPoseReferenceFrame("base_footprint");
      45   group_arm_torso.setPoseTarget(goal_pose);
      46 
      47   ROS_INFO_STREAM("Planning to move " <<
      48                   group_arm_torso.getEndEffectorLink() << " to a target pose expressed in " <<
      49                   group_arm_torso.getPlanningFrame());
      50 
      51   group_arm_torso.setStartStateToCurrentState();
      52   group_arm_torso.setMaxVelocityScalingFactor(1.0);
      53 
      54 
      55   moveit::planning_interface::MoveGroup::Plan my_plan;
      56   //set maximum time to find a plan
      57   group_arm_torso.setPlanningTime(5.0);
      58   bool success = group_arm_torso.plan(my_plan);
      59 
      60   if ( !success )
      61     throw std::runtime_error("No plan found");
      62 
      63   ROS_INFO_STREAM("Plan found in " << my_plan.planning_time_ << " seconds");
      64 
      65   // Execute the plan
      66   ros::Time start = ros::Time::now();
      67 
      68   group_arm_torso.move();
      69 
      70   ROS_INFO_STREAM("Motion duration: " << (ros::Time::now() - start).toSec());
      71 
      72   spinner.stop();
      73 
      74   return EXIT_SUCCESS;
      75 }
    

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

  68   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_cartesian_space (last edited 2017-03-17 15:52:10 by AlessandroDiFava)