####################################
##FILL ME IN
####################################
## for a custom note with links:
## note = This tutorial is under-construction and does not represent the final product.
## for the canned note of "This tutorial assumes that you have completed the
## previous tutorials:" just add the links
## !note.0.link=
## descriptive title for the tutorial
## title = Getting Started With Descartes
## multi-line description to be displayed in search
## description = An introductory tutorial to the Descartes cartesian path-planner.
## the next tutorial description (optional)
## !next =
## links to next tutorial (optional)
## !next.0.link=
## !next.1.link=
## what level user is this tutorial for
## level= BeginnerCategory
## keywords = Descartes, descartes, path planner
####################################
<<IncludeCSTemplate(TutorialCSHeaderTemplate)>>
<<TOC>>

== Overview ==
In this tutorial, we will be using the Descartes path planner to generate a robot joint trajectory for a given Cartesian path. Source code for this tutorial can currently be found at [[https://github.com/ros-industrial-consortium/descartes_tutorials|this website]].

== Install Descartes ==
=== (RECOMMENDED) Binary Install ===

NOTE: Recommended, but binary is not ready for ROS Indigo ([[https://github.com/ros-industrial-consortium/descartes/issues/194|release requested]]). Following will become available once the request is fulfilled.

{{{
sudo apt-get install ros-indigo-descartes
}}}

=== (Optional) Source Install ===

For Ubuntu:

{{{
sudo apt-get install python-catkin-tools python-rosdep
cd %YOUR_CATKIN_WS%/src
git clone https://github.com/ros-industrial-consortium/descartes.git
cd cd %YOUR_CATKIN_WS%
rosdep install -r -y --from-paths src --ignore-src
catkin build
source devel/setup.bash
}}}

== Using Descartes ==
Using Descartes requires a software developer to create three objects:

 1. A '''robot model''' which will solve forward and inverse kinematics.
 1. A trajectory of '''trajectory points''' to define what the path looks like.
 1. A '''planner''' that will do the work of finding a valid route along the trajectory using the provided model of the robot.

== Source Code ==
What follows is a single file program that defines a simple trajectory, creates a robot model using Moveit, plans for the trajectory, and executes it. At the top are several function declarations the source code for which is detailed in a following section.

{{{
#!cplusplus block=mainblock

// Core ros functionality like ros::init and spin
#include <ros/ros.h>
// ROS Trajectory Action server definition
#include <control_msgs/FollowJointTrajectoryAction.h>
// Means by which we communicate with above action-server
#include <actionlib/client/simple_action_client.h>

// Includes the descartes robot model we will be using
#include <descartes_moveit/moveit_state_adapter.h>
// Includes the descartes trajectory type we will be using
#include <descartes_trajectory/axial_symmetric_pt.h>
#include <descartes_trajectory/cart_trajectory_pt.h>
// Includes the planner we will be using
#include <descartes_planner/dense_planner.h>

typedef std::vector<descartes_core::TrajectoryPtPtr> TrajectoryVec;
typedef TrajectoryVec::const_iterator TrajectoryIter;

/**
 * Generates an completely defined (zero-tolerance) cartesian point from a pose
 */
descartes_core::TrajectoryPtPtr
makeCartesianPoint(const Eigen::Affine3d& pose);

/**
 * Generates a cartesian point with free rotation about the Z axis of the EFF frame
 */
descartes_core::TrajectoryPtPtr
makeTolerancedCartesianPoint(const Eigen::Affine3d& pose);

/**
 * Translates a descartes trajectory to a ROS joint trajectory
 */
trajectory_msgs::JointTrajectory
toROSJointTrajectory(const TrajectoryVec& trajectory,
                     const descartes_core::RobotModel& model,
                     const std::vector<std::string>& joint_names, double time_delay);

/**
 * Sends a ROS trajectory to the robot controller
 */
bool executeTrajectory(const trajectory_msgs::JointTrajectory& trajectory);

int main(int argc, char** argv)
{
  // Initialize ROS
  ros::init(argc, argv, "descartes_tutorial");
  ros::NodeHandle nh;

  // Required for communication with moveit components
  ros::AsyncSpinner spinner (1);
  spinner.start();

  // 1. Define sequence of points
  TrajectoryVec points;
  for (unsigned int i = 0; i < 10; ++i)
  {
    Eigen::Affine3d pose;
    pose = Eigen::Translation3d(0.0, 0.0, 1.0 + 0.05 * i);
    descartes_core::TrajectoryPtPtr pt = makeTolerancedCartesianPoint(pose);
    points.push_back(pt);
  }

  for (unsigned int i = 0; i < 5; ++i)
  {
    Eigen::Affine3d pose;
    pose = Eigen::Translation3d(0.0, 0.04 * i, 1.3);
    descartes_core::TrajectoryPtPtr pt = makeTolerancedCartesianPoint(pose);
    points.push_back(pt);
  }

  // 2. Create a robot model and initialize it
  descartes_core::RobotModelPtr model (new descartes_moveit::MoveitStateAdapter);

  // Name of description on parameter server. Typically just "robot_description".
  const std::string robot_description = "robot_description";

  // name of the kinematic group you defined when running MoveitSetupAssistant
  const std::string group_name = "manipulator";

  // Name of frame in which you are expressing poses.
  // Typically "world_frame" or "base_link".
  const std::string world_frame = "base_link";

  // tool center point frame (name of link associated with tool)
  const std::string tcp_frame = "tool0";

  if (!model->initialize(robot_description, group_name, world_frame, tcp_frame))
  {
    ROS_INFO("Could not initialize robot model");
    return -1;
  }

  // 3. Create a planner and initialize it with our robot model
  descartes_planner::DensePlanner planner;
  planner.initialize(model);

  // 4. Feed the trajectory to the planner
  if (!planner.planPath(points))
  {
    ROS_ERROR("Could not solve for a valid path");
    return -2;
  }

  TrajectoryVec result;
  if (!planner.getPath(result))
  {
    ROS_ERROR("Could not retrieve path");
    return -3;
  }

  // 5. Translate the result into a type that ROS understands
  // Get Joint Names
  std::vector<std::string> names;
  nh.getParam("controller_joint_names", names);
  // Generate a ROS joint trajectory with the result path, robot model,
  // joint names, and a certain time delta between each trajectory point
  trajectory_msgs::JointTrajectory joint_solution =
      toROSJointTrajectory(result, *model, names, 1.0);

  // 6. Send the ROS trajectory to the robot for execution
  if (!executeTrajectory(joint_solution))
  {
    ROS_ERROR("Could not execute trajectory!");
    return -4;
  }

  // Wait till user kills the process (Control-C)
  ROS_INFO("Done!");
  return 0;
}
}}}
=== Discussion ===

<<CodeRef(mainblock,1,7)>>

The first section of headers includes fundamental ROS components needed for printing, defining trajectories, and sending those trajectories. Most robots supported by ROS-Industrial offer a FollowJointTrajectory action interface for both simulation and actual hardware.

<<CodeRef(mainblock,8,14)>>
This second batch of headers includes all of the descartes specific files we'll need to generate trajectories. ''moveit_state_adapter.h'' defines a descartes '''RobotModel''' while the two files from ''descartes_trajectory'' define specific types of '''Trajectory Points'''. Finally ''dense_planner.h'' provides access to a descartes '''Planner'''. See [[descartes]] for more information about the specific components.

<<CodeRef(mainblock,46,52)>>
Before we can use ROS components, we must initialize our node by calling ros::init. I also create a NodeHandle here which will be used to retrieve components from the ROS parameter server later.

Lines 51-52 create a new thread to handle any callbacks that might need processing while we're executing descartes. Initializing MoveIt without the spinner seemed to cause problems.

<<CodeRef(mainblock,54,70)>>
Here we define a series of trajectory points using Eigen transformations. These transformations define an upward motion of the end effector followed by a sideways translation.

The {{{makeTolerancedCartesianPoint}}} function is a helper function for generating Cartesian points whose rotation about the z-axis of the end effector frame is unconstrained. When solving the graph, Descartes will attempt to search for valid IK solutions by essentially rotating the target pose around the z-axis.

The definition of {{{makeTolerancedCartesianPoint}}} is straight forward and will be discussed shortly.

<<CodeRef(mainblock,72,92)>>
In this block of code, we create and initialize a robot model by providing information about the robot we want to control. A prerequisite for using the provided moveit_state_adapter is that one must have run the Moveit Setup Assistant and created at least one kinematic groups prior to running this sample.

Underneath the hood, the code searches the ROS parameter server for a description of the robot (or system containing the robot) from the '''robot_description''' parameter. The '''group_name''' parameter should match the name of the kinematic group you created when running Moveit Setup Assistant. The default ROS-Industrial packages typically use 'manipulator' as their default group name.

The '''world_frame''' parameter specifies the frame in which the poses of positions you pass in are expressed. If you are specifying global positions relative to some predetermined origin, then the name of that origin is the argument to world_frame (it's also the root frame in your URDF). If the points are expressed relative to the robot base or some other fixed frame, you pass in the name of the base link or any other frame you'd like to use.

The '''tcp_frame''', or tool center point frame, is the frame of reference used when calculating forward and inverse kinematics. It will almost always be the last link in your manipulator group (associate with the name '''group_name''').

<<CodeRef(mainblock,94,110)>>
Once you have defined a Robot Model and a trajectory, planning is straightforward. Create the planner of your choice, initialize it with the model that was just created (and initialized itself), call the {{{planPath}}} function while passing in the desired trajectory. This function call does the bulk of the calculations to generate appropriate joint trajectories and could take a while to return depending on the complexity of the plan being analyzed.

The {{{getPath}}} function is used to retrieve the resulting trajectory. To make the planners as generic as possible, it returns the polymorphic type {{{TrajectoryPtPtr}}}, but for both Dense Planner and Sparse Planner the underlying type is {{{JointTrajectoryPt}}}. The joint values themselves are unpacked later in the {{{toROSJointTrajectory}}} function which will be discussed shortly.

Be sure to check that the return value of both the {{{planPath}}} and {{{getPath}}} functions  is ''true'' as they can fail for a variety of reasons including unreachable points and non-smooth joint trajectories.

<<CodeRef(mainblock,112,126)>>
This final code block converts the joint trajectory generated by Descartes to a format that ROS understands. The {{{toROSJointTrajectory}}} helper function performs this task, but requires a few pieces of information:
 1. The descartes joint trajectory solutions
 2. The descartes robot model used to interpret those points
 3. The names of the joints being actively commanded
 4. The time between each joint position

Lines 114-115 retrieve the joint names of the robot in the tutorial from the ROS parameter server.

=== Helper Functions ===

{{{
#!cplusplus block=makepoint
descartes_core::TrajectoryPtPtr makeTolerancedCartesianPoint(const Eigen::Affine3d& pose)
{
  using namespace descartes_core;
  using namespace descartes_trajectory;
  return TrajectoryPtPtr( new AxialSymmetricPt(pose, // Nominal pose
                                               M_PI/2.0, // Search discretization
                                               AxialSymmetricPt::Z_AXIS) ); // Free axis
}
}}}

{{{makeTolerancedCartesianPoint}}} is a small wrapper function around Descartes' {{{AxialSymmetricPt}}} class. This class is itself a specialization of a Cartesian point where the final argument (here {{{AxialSymmetricPt::Z_AXIS}}}) is the axis of the nominal pose about which the tool can rotate freely. Other options include {{{X_AXIS}}} and {{{Y_AXIS}}}.

If you wanted to create a fixed 6D cartesian pose, or if you wanted greater control over the acceptable tolerances, you can create a Cartesian point with equivalent properties.

{{{TrajectoryPtPtr}}} is an alias for a {{{boost::shared_ptr}}} with type {{{descartes_core::TrajectoryPt}}}. Shared pointers are a means of automatically managing object lifetime and can, in many cases, be used just as a normal pointer would be. 

{{{
#!cplusplus block=maketraj

trajectory_msgs::JointTrajectory
toROSJointTrajectory(const TrajectoryVec& trajectory,
                     const descartes_core::RobotModel& model,
                     const std::vector<std::string>& joint_names,
                     double time_delay)
{
  // Fill out information about our trajectory
  trajectory_msgs::JointTrajectory result;
  result.header.stamp = ros::Time::now();
  result.header.frame_id = "world_frame";
  result.joint_names = joint_names;

  // For keeping track of time-so-far in the trajectory
  double time_offset = 0.0;
  // Loop through the trajectory
  for (TrajectoryIter it = trajectory.begin(); it != trajectory.end(); ++it)
  {
    // Find nominal joint solution at this point
    std::vector<double> joints;
    it->get()->getNominalJointPose(std::vector<double>(), model, joints);

    // Fill out a ROS trajectory point
    trajectory_msgs::JointTrajectoryPoint pt;
    pt.positions = joints;
    // velocity, acceleration, and effort are given dummy values
    // we'll let the controller figure them out
    pt.velocities.resize(joints.size(), 0.0);
    pt.accelerations.resize(joints.size(), 0.0);
    pt.effort.resize(joints.size(), 0.0);
    // set the time into the trajectory
    pt.time_from_start = ros::Duration(time_offset);
    // increment time
    time_offset += time_delay;

    result.points.push_back(pt);
  }

  return result;
}

}}}

This function is responsible for converting a descartes trajectory into a ROS {{{trajectory_msgs::JointTrajectory}}}. As with any ROS message type, be sure to fill in the header information including frame_id and time-stamp.

Descartes' planners return a vector of trajectory point pointers, so we must access the underlying data polymorphically. The underlying type returned by both the sparse and dense planner is a joint trajectory point. The {{{getNominalJointPose}}} function can be used to extract the desired joint positions. For this type of point, the first argument to the function is just a dummy one. Next, we resize the velocity, acceleration, and effort fields and set all values to zero so that the controller will not reject our points and will hopefully figure out these values on its own. Finally we keep a running count of the time ''so far'' in the trajectory. ROS trajectory point timings are specified relative to the start of the trajectory, not the previous point.

{{{
#!cplusplus block=execute

bool executeTrajectory(const trajectory_msgs::JointTrajectory& trajectory)
{
  // Create a Follow Joint Trajectory Action Client
  actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> ac("joint_trajectory_action", true);
  if (!ac.waitForServer(ros::Duration(2.0)))
  {
    ROS_ERROR("Could not connect to action server");
    return false;
  }

  control_msgs::FollowJointTrajectoryGoal goal;
  goal.trajectory = trajectory;
  goal.goal_time_tolerance = ros::Duration(1.0);
  
  ac.sendGoal(goal);

  if (ac.waitForResult(goal.trajectory.points[goal.trajectory.points.size()-1].time_from_start + ros::Duration(5)))
  {
    ROS_INFO("Action server reported successful execution");
    return true;
  } else {
    ROS_WARN("Action server could not execute trajectory");
    return false;
  }
}
}}}

=== Build ===

`CMakeLists.txt` in [[https://github.com/ros-industrial-consortium/descartes_tutorials/blob/da65b683cab67a72f88130eba00d95c000184c12/descartes_tutorials/CMakeLists.txt|descartes_tutorials repository]] shows a minimal sample for the code above. Add necessary config to your CMakeLists.txt, then build by `catkin_make` etc.

== Running the Sample ==
To run the code, first make sure you've built the source code by changing to your workspaces home directory and running {{{catkin_make}}}.

Running the node you created in the tutorial first requires you to launch moveit for your desired robot. Included in the tutorial package structure is a launch file, {{{motoman_sim.launch}}} which will launch moveit and a simulator for a Motoman SIA20D robot. Alternatively, one can launch the moveit_planning_execution.launch file with the {{{sim:=true}}} argument for many ROS-Industrial robots including Fanuc, ABB, and other Motoman robots.

In summary:
{{{
roslaunch descartes_tutorials motoman_sim.launch
}}}

and in a separate (sourced) terminal:
{{{
roslaunch descartes_tutorials tutorial1.launch
}}}