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

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

Support: tiago-support@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 cartesian space

Purpose

This tutorial shows how to use MoveIt! in order to move the end-effectors 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_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.

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 left end-effector frame, i.e. arm_left_tool_link, to the following cartesian space configuration with respect to /base_footprint:

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

And an example that will bring TIAGo++'s right end-effector frame, i.e. arm_right_tool_link, to the following cartesian space configuration with respect to /base_footprint:

• x: 0.4
  y: -0.3
  z: 0.28
  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_dual_arm_torso_ik 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_ik left 0.4 0.3 0.28 -0.011 1.57 0.037

After going to the home position:

rosrun play_motion run_motion home

Can also be run as follows for the right arm

• rosrun tiago_dual_moveit_tutorial plan_dual_arm_torso_ik right 0.4 -0.3 0.28 -0.011 1.57 0.037

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

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

Here, instead, an example of plan executed by the node for the right arm is shown in the following sequence of images:

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_[left/right]_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 #include <tf/transform_broadcaster.h>
     7 
     8 // Std C++ headers
     9 #include <string>
    10 #include <vector>
    11 #include <map>
    12 
    13 int main(int argc, char** argv)
    14 {
    15   ros::init(argc, argv, "plan_dual_arm_torso_ik");
    16 
    17   if ( argc < 8 )
    18   {
    19     ROS_INFO(" ");
    20     ROS_INFO("\tUsage:");
    21     ROS_INFO(" ");
    22     ROS_INFO("\trosrun tiago_moveit_tutorial plan_dual_arm_torso_ik [left|right] x y z  r p y");
    23     ROS_INFO(" ");
    24     ROS_INFO("\twhere the list of arguments specify the target pose of /arm_[left|right]_tool_link expressed in /base_footprint");
    25     ROS_INFO(" ");
    26     return EXIT_FAILURE;
    27   }
    28 
    29   std::string arm_name = argv[1];
    30   geometry_msgs::PoseStamped goal_pose;
    31   goal_pose.header.frame_id = "base_footprint";
    32   goal_pose.pose.position.x = atof(argv[2]);
    33   goal_pose.pose.position.y = atof(argv[3]);
    34   goal_pose.pose.position.z = atof(argv[4]);
    35   goal_pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(atof(argv[5]), atof(argv[6]), atof(argv[7]));
    36 
    37   ros::NodeHandle nh;
    38   ros::AsyncSpinner spinner(1);
    39   spinner.start();
    40 
    41   std::vector<std::string> torso_arm_joint_names;
    42   //select group of joints
    43   std::string moveit_group = "arm_" + arm_name + "_torso";
    44   moveit::planning_interface::MoveGroupInterface group_arm_torso(moveit_group);
    45   //choose your preferred planner
    46   group_arm_torso.setPlannerId("SBLkConfigDefault");
    47   group_arm_torso.setPoseReferenceFrame("base_footprint");
    48   group_arm_torso.setPoseTarget(goal_pose);
    49 
    50   ROS_INFO_STREAM("Planning to move " <<
    51                   group_arm_torso.getEndEffectorLink() << " to a target pose expressed in " <<
    52                   group_arm_torso.getPlanningFrame());
    53 
    54   group_arm_torso.setStartStateToCurrentState();
    55   group_arm_torso.setMaxVelocityScalingFactor(1.0);
    56 
    57 
    58   moveit::planning_interface::MoveGroupInterface::Plan my_plan;
    59   //set maximum time to find a plan
    60   group_arm_torso.setPlanningTime(5.0);
    61   bool success = bool(group_arm_torso.plan(my_plan));
    62 
    63   if ( !success )
    64     throw std::runtime_error("No plan found");
    65 
    66   ROS_INFO_STREAM("Plan found in " << my_plan.planning_time_ << " seconds");
    67 
    68   // Execute the plan
    69   ros::Time start = ros::Time::now();
    70 
    71 
    72   moveit::planning_interface::MoveItErrorCode e = group_arm_torso.move();
    73   if (!bool(e))
    74     throw std::runtime_error("Error executing plan");
    75 
    76   ROS_INFO_STREAM("Motion duration: " << (ros::Time::now() - start).toSec());
    77 
    78   spinner.stop();
    79 
    80   return EXIT_SUCCESS;
    81 }
  

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

  72   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

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