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 should work for any robot chain with the appropriate configuration.

In the video below you'll 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.

Executable

The invite_beginner_tutorials/src/cartesian_task_planning.cpp has an example on how to use them 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 an instance of cartesian task planner.

invite_utils::CartesianTaskPlanner task_planner;

Create your cartesian trajectory

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

Use the 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.
}