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

Description: Example of how to move TIAGo's arm using a joint trajectory action.

Keywords: joint, trajectory controller

Tutorial Level: BEGINNER

Next Tutorial: Moving individual joints

traj_control2.png

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
/torso_controller/follow_joint_trajectory (control_msgs/FollowJointTrajectoryAction)
  • 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
/head_controller/follow_joint_trajectory (control_msgs/FollowJointTrajectoryAction)
  • 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
/arm_controller/follow_joint_trajectory (control_msgs/FollowJointTrajectoryAction)
  • 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.
/hand_controller/follow_joint_trajectory (control_msgs/FollowJointTrajectory)
  • 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.
/parallel_gripper_controller/follow_joint_trajectory (control_msgs/FollowJointTrajectoryAction)
  • Action interface to control the separation of the fingers of the gripper
/gripper_controller/command (trajectory_msgs/JointTrajectory)
  • 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 robot:=steel world:=empty

Gazebo will show up with TIAGo in an empty world.

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

traj_control1.png

traj_control2.png

traj_control3.png

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

Wiki: Robots/TIAGo/Tutorials/trajectory_controller (last edited 2019-05-22 13:59:18 by SaiKishor)