Tutorial Cartesian Task Planning

Tutorial: Cartesian Task Planning

This tutorial shows how to plan cartesian motions considering multiple start IK configuration, in order to achieve as much of the trajectory as possible.

Cartesian Task Planner Class

In the invite_utils package, on the cartesian_task_planner.h header file, you will find a naive implementation of a task planner which search for multiple robot configurations to start the desired trajectory, and selects the one that allows the robot to make as much of the trajectory as possible.

Note: The algorithm is robot agnostic, meaning it sould work for any robot chain with the appropiate configuration.

In the video below youll see the initial 20 robot configuration considered (very fast), and the plan for the approach motion to the selected start configuration, followed by the cartesian trajectory, displayed in orange.

interaction with a naive implementation of a task planner.

How to import it?

Executable

The invite_beginner_tutorials/src/cartesian_task_planning.cpp has an example on how to use the task planning algorithm with the CSDA10F robot. In order to run it, run first the simulation of the CSDA10F:

roslaunch invite_motoman_moveit_config demo.launch

And then run the tutorial example

rosrun invite_beginner_tutorials cartesian_task_planning 

In summary you need to do the following:

Import class

#include <invite_utils/cartesian_task_planner.h>

Create instance of cartesian task planner.

invite_utils::CartesianTaskPlanner task_planner;

Create your cartesian trajectory

std::vector<geometry_msgs::Pose> waypoints;
...

Use cartesian task planner class method planCartesianMotionTask

// Percentage of cartesian path achieved by the best robot start configuration
float motion_achievable;

// Max robot start (IK) configurations to consider.
int max_ik = 20; 

// Objects allowed to be touched by the end effector links during cartesian motion
std::vector<std::string> allowed_touch_objects; 

// Array that will contain two plans
//      1. Approach plan to first pose of cartesian motion.
//      2. Cartesian motion.
std::vector<MoveGroupInterface::Plan> motion_plans; 

// Plan cartesian task !
motion_achievable = task_planner.planCartesianMotionTask( move_group, 
                                                          waypoints,
                                                          motion_plans,
                                                          allowed_touch_objects,
                                                          max_ik);

// Perform approach and cartesian path motion.
if (motion_achievable > 0.7) {
  move_group.execute( motion_plans[0] ); // Approach motion.
  
  ros::Duration(0.1).sleep();

  move_group.execute( motion_plans[1] ); // Cartesian motion.
}