## For instruction on writing tutorials
## http://www.ros.org/wiki/WritingTutorials
####################################
##FILL ME IN
####################################
## for a custom note with links:
## note =
## for the canned note of "This tutorial assumes that you have completed the previous tutorials:" just add the links
## note.0=
## descriptive title for the tutorial
## title = Joint Trajectory Controller
## multi-line description to be displayed in search
## description = Example of how to move TIAGo's arm using a joint trajectory action.
## the next tutorial description (optional)
## next =
## links to next tutorial (optional)
## next.0.link=[[Robots/TIAGo/Tutorials/motions/rqt_joint|Moving individual joints]]
## next.1.link=
## what level user is this tutorial for
## level= BeginnerCategory
## keywords = joint, trajectory controller
####################################
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 <<IncludeCSTemplate(TutorialCSHeaderTemplate)>>

{{attachment:traj_control2.png||align="right", width="180"}}
<<TableOfContents(2)>>

= Purpose =

This tutorial shows how to use the [[http://wiki.ros.org/joint_trajectory_controller|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 [[Robots/TIAGo/Tutorials/Installation/TiagoSimulation|Tutorials Installation]] Section. 

= Available interfaces =

There are two mechanisms for sending trajectories to the controller, as written in [[http://wiki.ros.org/joint_trajectory_controller|joint_trajectory_controller]]. By means of the '''action interface''' or the '''topic interface'''. Both use the <<MsgLink(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:

{{{
#!clearsilver CS/NodeAPI
sub {
  no_header= True
  0.name= /torso_controller/command
  0.type= trajectory_msgs/JointTrajectory
  0.desc= Topic interface to move the torso lifter 
  1.name= /torso_controller/follow_joint_trajectory
  1.type= control_msgs/FollowJointTrajectoryAction
  1.desc= Action interface to move the torso lifter
}
}}}

== Head controller ==

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

{{{
#!clearsilver CS/NodeAPI
sub {
  no_header= True
  0.name= /head_controller/command
  0.type= trajectory_msgs/JointTrajectory
  0.desc= Topic interface to move the head
  1.name= /head_controller/follow_joint_trajectory
  1.type= control_msgs/FollowJointTrajectoryAction
  1.desc= Action interface to move the head
}
}}}

== Arm controller ==

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

{{{
#!clearsilver CS/NodeAPI
sub {
  no_header= True
  0.name= /arm_controller/command
  0.type= trajectory_msgs/JointTrajectory
  0.desc= Topic interface to move the arm 
  1.name= /arm_controller/follow_joint_trajectory
  1.type= control_msgs/FollowJointTrajectoryAction
  1.desc= Action interface to move the arm
}
}}}

== Hey5 hand controller ==

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

{{{
#!clearsilver CS/NodeAPI
sub {
  no_header= True
  0.name= /hand_controller/command
  0.type= trajectory_msgs/JointTrajectory
  0.desc= Topic interface to move the 3 motors of the Hey5 hand.
  1.name= /hand_controller/follow_joint_trajectory
  1.type= control_msgs/FollowJointTrajectory Action
  1.desc= 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:

{{{
#!clearsilver CS/NodeAPI
sub {
  no_header= True
  0.name= /parallel_gripper_controller/command
  0.type= trajectory_msgs/JointTrajectory
  0.desc= Topic interface to move the parallel gripper virtual joint, i.e. to control the separation of the fingers.
  1.name= /parallel_gripper_controller/follow_joint_trajectory 
  1.type= control_msgs/FollowJointTrajectoryAction
  1.desc= Action interface to control the separation of the fingers of the gripper
  2.name= /gripper_controller/command
  2.type= trajectory_msgs/JointTrajectory
  2.desc= 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.

{{attachment:gazebo_empty_world.jpg||align="center", width="600"}}

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:

|| {{attachment:traj_control1.png||width="233"}} || {{attachment:traj_control2.png||width="233"}} || {{attachment:traj_control3.png||width="233"}} ||

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

 {{{
#!cplusplus block=joint_trajectory_controller
// C++ standard headers
#include <exception>
#include <string>

// Boost headers
#include <boost/shared_ptr.hpp>

// ROS headers
#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <ros/topic.h>


// Our Action interface type for moving TIAGo's head, provided as a typedef for convenience
typedef actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> arm_control_client;
typedef boost::shared_ptr< arm_control_client>  arm_control_client_Ptr;


// Create a ROS action client to move TIAGo's arm
void createArmClient(arm_control_client_Ptr& actionClient)
{
  ROS_INFO("Creating action client to arm controller ...");

  actionClient.reset( new arm_control_client("/arm_controller/follow_joint_trajectory") );

  int iterations = 0, max_iterations = 3;
  // Wait for arm controller action server to come up
  while( !actionClient->waitForServer(ros::Duration(2.0)) && ros::ok() && iterations < max_iterations )
  {
    ROS_DEBUG("Waiting for the arm_controller_action server to come up");
    ++iterations;
  }

  if ( iterations == max_iterations )
    throw std::runtime_error("Error in createArmClient: arm controller action server not available");
}


// Generates a simple trajectory with two waypoints to move TIAGo's arm 
void waypoints_arm_goal(control_msgs::FollowJointTrajectoryGoal& goal)
{
  // The joint names, which apply to all waypoints
  goal.trajectory.joint_names.push_back("arm_1_joint");
  goal.trajectory.joint_names.push_back("arm_2_joint");
  goal.trajectory.joint_names.push_back("arm_3_joint");
  goal.trajectory.joint_names.push_back("arm_4_joint");
  goal.trajectory.joint_names.push_back("arm_5_joint");
  goal.trajectory.joint_names.push_back("arm_6_joint");
  goal.trajectory.joint_names.push_back("arm_7_joint");

  // Two waypoints in this goal trajectory
  goal.trajectory.points.resize(2);

  // First trajectory point
  // Positions
  int index = 0;
  goal.trajectory.points[index].positions.resize(7);
  goal.trajectory.points[index].positions[0] = 0.2;
  goal.trajectory.points[index].positions[1] = 0.0;
  goal.trajectory.points[index].positions[2] = -1.5;
  goal.trajectory.points[index].positions[3] = 1.94;
  goal.trajectory.points[index].positions[4] = -1.57;
  goal.trajectory.points[index].positions[5] = -0.5;
  goal.trajectory.points[index].positions[6] = 0.0;
  // Velocities
  goal.trajectory.points[index].velocities.resize(7);
  for (int j = 0; j < 7; ++j)
  {
    goal.trajectory.points[index].velocities[j] = 1.0;
  }
  // To be reached 2 second after starting along the trajectory
  goal.trajectory.points[index].time_from_start = ros::Duration(2.0);

  // Second trajectory point
  // Positions
  index += 1;
  goal.trajectory.points[index].positions.resize(7);
  goal.trajectory.points[index].positions[0] = 2.5;
  goal.trajectory.points[index].positions[1] = 0.2;
  goal.trajectory.points[index].positions[2] = -2.1;
  goal.trajectory.points[index].positions[3] = 1.9;
  goal.trajectory.points[index].positions[4] = 1.0;
  goal.trajectory.points[index].positions[5] = -0.5;
  goal.trajectory.points[index].positions[6] = 0.0;
  // Velocities
  goal.trajectory.points[index].velocities.resize(7);
  for (int j = 0; j < 7; ++j)
  {
    goal.trajectory.points[index].velocities[j] = 0.0;
  }
  // To be reached 4 seconds after starting along the trajectory
  goal.trajectory.points[index].time_from_start = ros::Duration(4.0);
}


// Entry point
int main(int argc, char** argv)
{
  // Init the ROS node
  ros::init(argc, argv, "run_traj_control");

  ROS_INFO("Starting run_traj_control application ...");
 
  // Precondition: Valid clock
  ros::NodeHandle nh;
  if (!ros::Time::waitForValid(ros::WallDuration(10.0))) // NOTE: Important when using simulated clock
  {
    ROS_FATAL("Timed-out waiting for valid time.");
    return EXIT_FAILURE;
  }

  // Create an arm controller action client to move the TIAGo's arm
  arm_control_client_Ptr ArmClient;
  createArmClient(ArmClient);

  // Generates the goal for the TIAGo's arm
  control_msgs::FollowJointTrajectoryGoal arm_goal;
  waypoints_arm_goal(arm_goal);

  // Sends the command to start the given trajectory 1s from now
  arm_goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(1.0);
  ArmClient->sendGoal(arm_goal);

  // Wait for trajectory execution
  while(!(ArmClient->getState().isDone()) && ros::ok())
  {
    ros::Duration(4).sleep(); // sleep for four seconds
  }

  return EXIT_SUCCESS;
}
 }}}

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

<<CodeRef(joint_trajectory_controller,70,70)>> 

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"

<<CodeRef(joint_trajectory_controller,122,122)>> 

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

 {{{
/arm_controller/follow_joint_trajectory
 }}}

## AUTOGENERATED DO NOT DELETE
## TutorialCategory
## FILL IN THE STACK TUTORIAL CATEGORY HERE