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

Maintainer: Jordi Pages < jordi.pages@pal-robotics.com >, Alessandro Di Fava < alessandro.difava@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.

Joint Trajectory Controller

Purpose

This tutorial shows how to use the joint_trajectory_controller to move the arm of TIAGo. The action can be used to give a trajectory to the arm expressed in several waypoints.

Pre-requisites

First make sure that the tutorials are properly installed along with the TIAGo simulation, as shown in the Tutorials Installation Section.

Available interfaces

There are two mechanisms for sending trajectories to the controller, as written in joint_trajectory_controller. By means of the action interface or the topic interface. Both use the trajectory_msgs/JointTrajectory message to specify trajectories.

For the TIAGo the following interfaces are available: torso control, head control, arm control. The following interfaces are used.

Torso controller

The torso prismatic joint can be controlled with the following joint trajectory interfaces:

/torso_controller/command (trajectory_msgs/JointTrajectory)

• Topic interface to move the torso lifter

• Action interface to move the torso lifter

Head controller

The 2 joints of the head can be controlled defining trajectories using the following interfaces:

/head_controller/command (trajectory_msgs/JointTrajectory)

• Topic interface to move the head

• Action interface to move the head

Arm controller

Trajectories for the 7 joints of the arm can be controlled using the following interfaces:

/arm_controller/command (trajectory_msgs/JointTrajectory)

• Topic interface to move the arm

• Action interface to move the arm

Hey5 hand controller

For the titanium version of TIAGo the hand control interface is also available:

/hand_controller/command (trajectory_msgs/JointTrajectory)

• Topic interface to move the 3 motors of the Hey5 hand.

• Action interface to move the 3 motors of the Hey5 hand.

Gripper controller

For the steel version of TIAGo the gripper control interface is available:

/parallel_gripper_controller/command (trajectory_msgs/JointTrajectory)

• Topic interface to move the parallel gripper virtual joint, i.e. to control the separation of the fingers.

• Action interface to control the separation of the fingers of the gripper

• Topic interface to move the 2 motors of the gripper, i.e. each finger, individually.

Execution

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 for example the following simulation

• roslaunch tiago_gazebo tiago_gazebo.launch public_sim:=true end_effector:=pal-gripper world:=empty

Gazebo will show up with TIAGo in an empty world.

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 with two waypoints that will bring TIAGO to the following joint space configurations of the arm. First waypoint:

• arm_1_joint: 0.2
  arm_2_joint: 0.0
  arm_3_joint: -1.5
  arm_4_joint: 1.94
  arm_5_joint: -1.57
  arm_6_joint: -0.5
  arm_7_joint: 0.0

Second waypoint:

• arm_1_joint: 2.5
  arm_2_joint: 0.2
  arm_3_joint: -2.1
  arm_4_joint: 1.9
  arm_5_joint: 1.0
  arm_6_joint: -0.5
  arm_7_joint: 0.0

The node that will take care to execute the set of waypoints to reach such a kinematic configuration is run_traj_control included in tiago_trajectory_controller package and has to be called as follows

• rosrun tiago_trajectory_controller run_traj_control

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

Inspecting the code

The code to implement such a node is shown below. Note that the key parts of the code are:

• Create an arm controller action client
• Wait for arm controller action server to come up
• Generates a simple trajectory with two waypoints (goal)
• Sends the command to start the given trajectory
• Give time for trajectory execution

     1 // C++ standard headers
     2 #include <exception>
     3 #include <string>
     4 
     5 // Boost headers
     6 #include <boost/shared_ptr.hpp>
     7 
     8 // ROS headers
     9 #include <ros/ros.h>
    10 #include <actionlib/client/simple_action_client.h>
    11 #include <control_msgs/FollowJointTrajectoryAction.h>
    12 #include <ros/topic.h>
    13 
    14 
    15 // Our Action interface type for moving TIAGo's head, provided as a typedef for convenience
    16 typedef actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> arm_control_client;
    17 typedef boost::shared_ptr< arm_control_client>  arm_control_client_Ptr;
    18 
    19 
    20 // Create a ROS action client to move TIAGo's arm
    21 void createArmClient(arm_control_client_Ptr& actionClient)
    22 {
    23   ROS_INFO("Creating action client to arm controller ...");
    24 
    25   actionClient.reset( new arm_control_client("/arm_controller/follow_joint_trajectory") );
    26 
    27   int iterations = 0, max_iterations = 3;
    28   // Wait for arm controller action server to come up
    29   while( !actionClient->waitForServer(ros::Duration(2.0)) && ros::ok() && iterations < max_iterations )
    30   {
    31     ROS_DEBUG("Waiting for the arm_controller_action server to come up");
    32     ++iterations;
    33   }
    34 
    35   if ( iterations == max_iterations )
    36     throw std::runtime_error("Error in createArmClient: arm controller action server not available");
    37 }
    38 
    39 
    40 // Generates a simple trajectory with two waypoints to move TIAGo's arm 
    41 void waypoints_arm_goal(control_msgs::FollowJointTrajectoryGoal& goal)
    42 {
    43   // The joint names, which apply to all waypoints
    44   goal.trajectory.joint_names.push_back("arm_1_joint");
    45   goal.trajectory.joint_names.push_back("arm_2_joint");
    46   goal.trajectory.joint_names.push_back("arm_3_joint");
    47   goal.trajectory.joint_names.push_back("arm_4_joint");
    48   goal.trajectory.joint_names.push_back("arm_5_joint");
    49   goal.trajectory.joint_names.push_back("arm_6_joint");
    50   goal.trajectory.joint_names.push_back("arm_7_joint");
    51 
    52   // Two waypoints in this goal trajectory
    53   goal.trajectory.points.resize(2);
    54 
    55   // First trajectory point
    56   // Positions
    57   int index = 0;
    58   goal.trajectory.points[index].positions.resize(7);
    59   goal.trajectory.points[index].positions[0] = 0.2;
    60   goal.trajectory.points[index].positions[1] = 0.0;
    61   goal.trajectory.points[index].positions[2] = -1.5;
    62   goal.trajectory.points[index].positions[3] = 1.94;
    63   goal.trajectory.points[index].positions[4] = -1.57;
    64   goal.trajectory.points[index].positions[5] = -0.5;
    65   goal.trajectory.points[index].positions[6] = 0.0;
    66   // Velocities
    67   goal.trajectory.points[index].velocities.resize(7);
    68   for (int j = 0; j < 7; ++j)
    69   {
    70     goal.trajectory.points[index].velocities[j] = 1.0;
    71   }
    72   // To be reached 2 second after starting along the trajectory
    73   goal.trajectory.points[index].time_from_start = ros::Duration(2.0);
    74 
    75   // Second trajectory point
    76   // Positions
    77   index += 1;
    78   goal.trajectory.points[index].positions.resize(7);
    79   goal.trajectory.points[index].positions[0] = 2.5;
    80   goal.trajectory.points[index].positions[1] = 0.2;
    81   goal.trajectory.points[index].positions[2] = -2.1;
    82   goal.trajectory.points[index].positions[3] = 1.9;
    83   goal.trajectory.points[index].positions[4] = 1.0;
    84   goal.trajectory.points[index].positions[5] = -0.5;
    85   goal.trajectory.points[index].positions[6] = 0.0;
    86   // Velocities
    87   goal.trajectory.points[index].velocities.resize(7);
    88   for (int j = 0; j < 7; ++j)
    89   {
    90     goal.trajectory.points[index].velocities[j] = 0.0;
    91   }
    92   // To be reached 4 seconds after starting along the trajectory
    93   goal.trajectory.points[index].time_from_start = ros::Duration(4.0);
    94 }
    95 
    96 
    97 // Entry point
    98 int main(int argc, char** argv)
    99 {
   100   // Init the ROS node
   101   ros::init(argc, argv, "run_traj_control");
   102 
   103   ROS_INFO("Starting run_traj_control application ...");
   104  
   105   // Precondition: Valid clock
   106   ros::NodeHandle nh;
   107   if (!ros::Time::waitForValid(ros::WallDuration(10.0))) // NOTE: Important when using simulated clock
   108   {
   109     ROS_FATAL("Timed-out waiting for valid time.");
   110     return EXIT_FAILURE;
   111   }
   112 
   113   // Create an arm controller action client to move the TIAGo's arm
   114   arm_control_client_Ptr ArmClient;
   115   createArmClient(ArmClient);
   116 
   117   // Generates the goal for the TIAGo's arm
   118   control_msgs::FollowJointTrajectoryGoal arm_goal;
   119   waypoints_arm_goal(arm_goal);
   120 
   121   // Sends the command to start the given trajectory 1s from now
   122   arm_goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(1.0);
   123   ArmClient->sendGoal(arm_goal);
   124 
   125   // Wait for trajectory execution
   126   while(!(ArmClient->getState().isDone()) && ros::ok())
   127   {
   128     ros::Duration(4).sleep(); // sleep for four seconds
   129   }
   130 
   131   return EXIT_SUCCESS;
   132 }
  

Note that for the first waypoint, for each joint in the trajectory, we use a velocity of 1.0 m/s. It specifies the velocity of the trajectory when reaching the waypoint. We obtain it with the following line of code

  70     goal.trajectory.points[index].velocities[j] = 1.0;

As we want the robot to move smoothly when reaching the first target waypoint. By setting the velocity to 0.0 m/s will cause a step-wise movement, where the robot will stop in each waypoint. We set the velocity to 0.0 m/s just for the last waypoint.

The joint_trajectory_controller will map the position+velocity trajectory to position commands through a PID loop.

Joint trajectory messages allow to specify the time at which the trajectory should start executing by means of the header timestamp. In the following line of code, the node will start the given trajectory 1s from "now"

 122   arm_goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(1.0);

The required control commands are sent to the arm controller through the topic of their action interfaces:

• /arm_controller/follow_joint_trajectory