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

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

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:

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

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