## 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 = Planning in joint space
## multi-line description to be displayed in search
## description = How to reach a given joint space configuration using motion planning based on [[http://moveit.ros.org|MoveIt!]]
## the next tutorial description (optional)
## next =
## links to next tutorial (optional)
## next.0.link=[[Robots/TIAGo++/Tutorials/MoveIt/Planning_cartesian_space|Planning in cartesian space]]
## next.1.link=
## what level user is this tutorial for
## level= IntermediateCategory
## keywords = Motion planning, joint states, forward kinematics
####################################
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 
<<IncludeCSTemplate(TutorialCSHeaderTemplate)>>

{{attachment:planning_joint_title.jpg||align="right", width="160"}}
<<TableOfContents(2)>>

= Purpose =

This tutorial shows how to use [[http://moveit.ros.org|MoveIt!]] in order to bring the arm_left_torso or the arm_right_torso group of joints of TIAGo++ to a desired joint space configuration ensuring joint limit avoidance and self-collision avoidance. The 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 [[Robots/TIAGo++/Tutorials/Installation/Tiago++Simulation|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.

{{attachment:tiago++_gazebo_grippers.jpg||align="center", width="600"}}

Wait until TIAGo++ has tucked its arms. Then you may proceed with the next steps.

== Launching the nodes ==

Now we are going to run an example that will bring TIAGo++ to the following joint space configuration of both the arms groups:

 {{{
torso_joint: 0.15
arm_left_1_joint/arm_right_1_joint: 1.5
arm_left_2_joint/arm_right_2_joint: 0.58
arm_left_3_joint/arm_right_3_joint: 0.06
arm_left_4_joint/arm_right_4_joint: 1.0
arm_left_5_joint/arm_right_5_joint: -1.70
arm_left_6_joint/arm_right_6_joint: 0.0
arm_left_7_joint/arm_right_7_joint: 0.0
 }}}

Note that if we try to set the above kinematic configuration moving individual joints we will end up probably in a self-collision. The node that will take care to find a plan, i.e. a sequence of movements, to reach such a kinematic configuration is '''plan_dual_arm_torso_fk''' 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_fk left 0.15 1.5 0.58 0.06 1.0 -1.70 0.0 0.0
 }}}

and as follows for the right arm

 {{{
rosrun tiago_dual_moveit_tutorial plan_dual_arm_torso_fk right 0.15 1.5 0.58 0.06 1.0 -1.70 0.0 0.0
 }}}

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

|| {{attachment:plan_joints_1.jpg||width="233"}} || {{attachment:plan_joints_2.jpg||width="233"}} || {{attachment:plan_joints_3.jpg||width="233"}} ||

== Inspecting the code ==

The code to implement such a node able to plan in joint space is shown below. Note that the key parts of the code are:

 * Choose a group of joints
 * Choose a planner
 * Set initial and desired joint state
 * Give time to find a plan
 * Execute the plan if found

 {{{
#!cplusplus block=planning_joint_space
// ROS headers
#include <ros/ros.h>

// MoveIt! headers
#include <moveit/move_group_interface/move_group_interface.h>

// Std C++ headers
#include <string>
#include <vector>
#include <map>

int main(int argc, char** argv)
{
  ros::init(argc, argv, "plan_dual_arm_torso_fk");

  if ( argc < 10 )
  {
    ROS_INFO(" ");
    ROS_INFO("\tUsage:");
    ROS_INFO(" ");
    ROS_INFO("\trosrun tiago_moveit_tutorial plan_dual_arm_torso_fk [left|right] torso_lift arm_1 arm_2 arm_3 arm_4 arm_5 arm_6 arm_7");
    ROS_INFO(" ");
    ROS_INFO("\twhere the list of arguments are the target values for the given joints");
    ROS_INFO(" ");
    return EXIT_FAILURE;
  }

  std::string arm_name = argv[1];
  std::map<std::string, double> target_position;

  target_position["torso_lift_joint"] = atof(argv[2]);
  std::string target_joint = "arm_" + arm_name + "_1_joint";
  target_position[target_joint] = atof(argv[3]);
  target_joint = "arm_" + arm_name + "_2_joint";
  target_position[target_joint] = atof(argv[4]);
  target_joint = "arm_" + arm_name + "_3_joint";
  target_position[target_joint] = atof(argv[5]);
  target_joint = "arm_" + arm_name + "_4_joint";
  target_position[target_joint] = atof(argv[6]);
  target_joint = "arm_" + arm_name + "_5_joint";
  target_position[target_joint] = atof(argv[7]);
  target_joint = "arm_" + arm_name + "_6_joint";
  target_position[target_joint] = atof(argv[8]);
  target_joint = "arm_" + arm_name + "_7_joint";
  target_position[target_joint] = atof(argv[9]);

  ros::NodeHandle nh;
  ros::AsyncSpinner spinner(1);
  spinner.start();

  std::vector<std::string> torso_arm_joint_names;
  //select group of joints
  std::string moveit_group = "arm_" + arm_name + "_torso";
  moveit::planning_interface::MoveGroupInterface group_arm_torso(moveit_group);
  //choose your preferred planner
  group_arm_torso.setPlannerId("SBLkConfigDefault");

  torso_arm_joint_names = group_arm_torso.getJoints();

  group_arm_torso.setStartStateToCurrentState();
  group_arm_torso.setMaxVelocityScalingFactor(1.0);

  for (unsigned int i = 0; i < torso_arm_joint_names.size(); ++i)
    if ( target_position.count(torso_arm_joint_names[i]) > 0 )
    {
      ROS_INFO_STREAM("\t" << torso_arm_joint_names[i] << " goal position: " << target_position[torso_arm_joint_names[i]]);
      group_arm_torso.setJointValueTarget(torso_arm_joint_names[i], target_position[torso_arm_joint_names[i]]);
    }

  moveit::planning_interface::MoveGroupInterface::Plan my_plan;
  group_arm_torso.setPlanningTime(5.0);
  bool success = bool(group_arm_torso.plan(my_plan));

  if ( !success )
    throw std::runtime_error("No plan found");

  ROS_INFO_STREAM("Plan found in " << my_plan.planning_time_ << " seconds");

  // Execute the plan
  ros::Time start = ros::Time::now();

  moveit::planning_interface::MoveItErrorCode e = group_arm_torso.move();
  if (!bool(e))
    throw std::runtime_error("Error executing plan");

  ROS_INFO_STREAM("Motion duration: " << (ros::Time::now() - start).toSec());

  spinner.stop();

  return EXIT_SUCCESS;
}
 }}}

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

<<CodeRef(planning_joint_space,82,82)>> 

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

<<CodeRef(planning_joint_space,53,53)>>

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.

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