| Note: This tutorial is under-construction and does not represent the final product.. |
 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Getting Started With Descartes
Description: An introductory tutorial to the Descartes cartesian path-planner.Keywords: Descartes, descartes, path planner
Tutorial Level: BEGINNER
Contents
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 this website.
Install Descartes
(RECOMMENDED) Binary Install
NOTE: Recommended, but binary is not ready for ROS Indigo (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:
A robot model which will solve forward and inverse kinematics.
A trajectory of trajectory points to define what the path looks like.
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.
1 // Core ros functionality like ros::init and spin
2 #include <ros/ros.h>
3 // ROS Trajectory Action server definition
4 #include <control_msgs/FollowJointTrajectoryAction.h>
5 // Means by which we communicate with above action-server
6 #include <actionlib/client/simple_action_client.h>
7
8 // Includes the descartes robot model we will be using
9 #include <descartes_moveit/moveit_state_adapter.h>
10 // Includes the descartes trajectory type we will be using
11 #include <descartes_trajectory/axial_symmetric_pt.h>
12 #include <descartes_trajectory/cart_trajectory_pt.h>
13 // Includes the planner we will be using
14 #include <descartes_planner/dense_planner.h>
15
16 typedef std::vector<descartes_core::TrajectoryPtPtr> TrajectoryVec;
17 typedef TrajectoryVec::const_iterator TrajectoryIter;
18
19 /**
20 * Generates an completely defined (zero-tolerance) cartesian point from a pose
21 */
22 descartes_core::TrajectoryPtPtr
23 makeCartesianPoint(const Eigen::Affine3d& pose);
24
25 /**
26 * Generates a cartesian point with free rotation about the Z axis of the EFF frame
27 */
28 descartes_core::TrajectoryPtPtr
29 makeTolerancedCartesianPoint(const Eigen::Affine3d& pose);
30
31 /**
32 * Translates a descartes trajectory to a ROS joint trajectory
33 */
34 trajectory_msgs::JointTrajectory
35 toROSJointTrajectory(const TrajectoryVec& trajectory,
36 const descartes_core::RobotModel& model,
37 const std::vector<std::string>& joint_names, double time_delay);
38
39 /**
40 * Sends a ROS trajectory to the robot controller
41 */
42 bool executeTrajectory(const trajectory_msgs::JointTrajectory& trajectory);
43
44 int main(int argc, char** argv)
45 {
46 // Initialize ROS
47 ros::init(argc, argv, "descartes_tutorial");
48 ros::NodeHandle nh;
49
50 // Required for communication with moveit components
51 ros::AsyncSpinner spinner (1);
52 spinner.start();
53
54 // 1. Define sequence of points
55 TrajectoryVec points;
56 for (unsigned int i = 0; i < 10; ++i)
57 {
58 Eigen::Affine3d pose;
59 pose = Eigen::Translation3d(0.0, 0.0, 1.0 + 0.05 * i);
60 descartes_core::TrajectoryPtPtr pt = makeTolerancedCartesianPoint(pose);
61 points.push_back(pt);
62 }
63
64 for (unsigned int i = 0; i < 5; ++i)
65 {
66 Eigen::Affine3d pose;
67 pose = Eigen::Translation3d(0.0, 0.04 * i, 1.3);
68 descartes_core::TrajectoryPtPtr pt = makeTolerancedCartesianPoint(pose);
69 points.push_back(pt);
70 }
71
72 // 2. Create a robot model and initialize it
73 descartes_core::RobotModelPtr model (new descartes_moveit::MoveitStateAdapter);
74
75 // Name of description on parameter server. Typically just "robot_description".
76 const std::string robot_description = "robot_description";
77
78 // name of the kinematic group you defined when running MoveitSetupAssistant
79 const std::string group_name = "manipulator";
80
81 // Name of frame in which you are expressing poses.
82 // Typically "world_frame" or "base_link".
83 const std::string world_frame = "base_link";
84
85 // tool center point frame (name of link associated with tool)
86 const std::string tcp_frame = "tool0";
87
88 if (!model->initialize(robot_description, group_name, world_frame, tcp_frame))
89 {
90 ROS_INFO("Could not initialize robot model");
91 return -1;
92 }
93
94 // 3. Create a planner and initialize it with our robot model
95 descartes_planner::DensePlanner planner;
96 planner.initialize(model);
97
98 // 4. Feed the trajectory to the planner
99 if (!planner.planPath(points))
100 {
101 ROS_ERROR("Could not solve for a valid path");
102 return -2;
103 }
104
105 TrajectoryVec result;
106 if (!planner.getPath(result))
107 {
108 ROS_ERROR("Could not retrieve path");
109 return -3;
110 }
111
112 // 5. Translate the result into a type that ROS understands
113 // Get Joint Names
114 std::vector<std::string> names;
115 nh.getParam("controller_joint_names", names);
116 // Generate a ROS joint trajectory with the result path, robot model,
117 // joint names, and a certain time delta between each trajectory point
118 trajectory_msgs::JointTrajectory joint_solution =
119 toROSJointTrajectory(result, *model, names, 1.0);
120
121 // 6. Send the ROS trajectory to the robot for execution
122 if (!executeTrajectory(joint_solution))
123 {
124 ROS_ERROR("Could not execute trajectory!");
125 return -4;
126 }
127
128 // Wait till user kills the process (Control-C)
129 ROS_INFO("Done!");
130 return 0;
131 }
Discussion
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.
8 // Includes the descartes robot model we will be using
9 #include <descartes_moveit/moveit_state_adapter.h>
10 // Includes the descartes trajectory type we will be using
11 #include <descartes_trajectory/axial_symmetric_pt.h>
12 #include <descartes_trajectory/cart_trajectory_pt.h>
13 // Includes the planner we will be using
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.
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.
54 // 1. Define sequence of points
55 TrajectoryVec points;
56 for (unsigned int i = 0; i < 10; ++i)
57 {
58 Eigen::Affine3d pose;
59 pose = Eigen::Translation3d(0.0, 0.0, 1.0 + 0.05 * i);
60 descartes_core::TrajectoryPtPtr pt = makeTolerancedCartesianPoint(pose);
61 points.push_back(pt);
62 }
63
64 for (unsigned int i = 0; i < 5; ++i)
65 {
66 Eigen::Affine3d pose;
67 pose = Eigen::Translation3d(0.0, 0.04 * i, 1.3);
68 descartes_core::TrajectoryPtPtr pt = makeTolerancedCartesianPoint(pose);
69 points.push_back(pt);
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.
72 // 2. Create a robot model and initialize it
73 descartes_core::RobotModelPtr model (new descartes_moveit::MoveitStateAdapter);
74
75 // Name of description on parameter server. Typically just "robot_description".
76 const std::string robot_description = "robot_description";
77
78 // name of the kinematic group you defined when running MoveitSetupAssistant
79 const std::string group_name = "manipulator";
80
81 // Name of frame in which you are expressing poses.
82 // Typically "world_frame" or "base_link".
83 const std::string world_frame = "base_link";
84
85 // tool center point frame (name of link associated with tool)
86 const std::string tcp_frame = "tool0";
87
88 if (!model->initialize(robot_description, group_name, world_frame, tcp_frame))
89 {
90 ROS_INFO("Could not initialize robot model");
91 return -1;
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).
94 // 3. Create a planner and initialize it with our robot model
95 descartes_planner::DensePlanner planner;
96 planner.initialize(model);
97
98 // 4. Feed the trajectory to the planner
99 if (!planner.planPath(points))
100 {
101 ROS_ERROR("Could not solve for a valid path");
102 return -2;
103 }
104
105 TrajectoryVec result;
106 if (!planner.getPath(result))
107 {
108 ROS_ERROR("Could not retrieve path");
109 return -3;
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.
112 // 5. Translate the result into a type that ROS understands
113 // Get Joint Names
114 std::vector<std::string> names;
115 nh.getParam("controller_joint_names", names);
116 // Generate a ROS joint trajectory with the result path, robot model,
117 // joint names, and a certain time delta between each trajectory point
118 trajectory_msgs::JointTrajectory joint_solution =
119 toROSJointTrajectory(result, *model, names, 1.0);
120
121 // 6. Send the ROS trajectory to the robot for execution
122 if (!executeTrajectory(joint_solution))
123 {
124 ROS_ERROR("Could not execute trajectory!");
125 return -4;
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:
- The descartes joint trajectory solutions
- The descartes robot model used to interpret those points
- The names of the joints being actively commanded
- 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
1 descartes_core::TrajectoryPtPtr makeTolerancedCartesianPoint(const Eigen::Affine3d& pose)
2 {
3 using namespace descartes_core;
4 using namespace descartes_trajectory;
5 return TrajectoryPtPtr( new AxialSymmetricPt(pose, // Nominal pose
6 M_PI/2.0, // Search discretization
7 AxialSymmetricPt::Z_AXIS) ); // Free axis
8 }
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.
1 trajectory_msgs::JointTrajectory
2 toROSJointTrajectory(const TrajectoryVec& trajectory,
3 const descartes_core::RobotModel& model,
4 const std::vector<std::string>& joint_names,
5 double time_delay)
6 {
7 // Fill out information about our trajectory
8 trajectory_msgs::JointTrajectory result;
9 result.header.stamp = ros::Time::now();
10 result.header.frame_id = "world_frame";
11 result.joint_names = joint_names;
12
13 // For keeping track of time-so-far in the trajectory
14 double time_offset = 0.0;
15 // Loop through the trajectory
16 for (TrajectoryIter it = trajectory.begin(); it != trajectory.end(); ++it)
17 {
18 // Find nominal joint solution at this point
19 std::vector<double> joints;
20 it->get()->getNominalJointPose(std::vector<double>(), model, joints);
21
22 // Fill out a ROS trajectory point
23 trajectory_msgs::JointTrajectoryPoint pt;
24 pt.positions = joints;
25 // velocity, acceleration, and effort are given dummy values
26 // we'll let the controller figure them out
27 pt.velocities.resize(joints.size(), 0.0);
28 pt.accelerations.resize(joints.size(), 0.0);
29 pt.effort.resize(joints.size(), 0.0);
30 // set the time into the trajectory
31 pt.time_from_start = ros::Duration(time_offset);
32 // increment time
33 time_offset += time_delay;
34
35 result.points.push_back(pt);
36 }
37
38 return result;
39 }
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.
1 bool executeTrajectory(const trajectory_msgs::JointTrajectory& trajectory)
2 {
3 // Create a Follow Joint Trajectory Action Client
4 actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> ac("joint_trajectory_action", true);
5 if (!ac.waitForServer(ros::Duration(2.0)))
6 {
7 ROS_ERROR("Could not connect to action server");
8 return false;
9 }
10
11 control_msgs::FollowJointTrajectoryGoal goal;
12 goal.trajectory = trajectory;
13 goal.goal_time_tolerance = ros::Duration(1.0);
14
15 ac.sendGoal(goal);
16
17 if (ac.waitForResult(goal.trajectory.points[goal.trajectory.points.size()-1].time_from_start + ros::Duration(5)))
18 {
19 ROS_INFO("Action server reported successful execution");
20 return true;
21 } else {
22 ROS_WARN("Action server could not execute trajectory");
23 return false;
24 }
25 }
Build
CMakeLists.txt in 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
