The code used in this tutorial is available!

Code can be found at industrial_training repository in doc folder. Use kinetic-devel branch.

Introduction to Descartes Path Planning

In this exercise, we will use what was learned in the previous exercises by creating a Descartes planner to create a robot path.

Motivation

MoveIt! is a framework meant primarily for performing "free-space" motion where the objective is to move a robot from point A to point B and you don't particularly care about how that gets done. These types of problems are only a subset of frequently performed tasks. Imagine any manufacturing ''process'' like welding or painting. You very much care about where that tool is pointing the entire time the robot is at work.

This tutorial introduces you to Descartes, a ''Cartesian'' motion planner meant for moving a robot along some process path. It's only one of a number of ways to solve this kind of problem, but it's got some neat properties:

  • It's deterministic and globally optimum (to a certain search resolution).
  • It can search redundant degrees of freedom in your problem (say you have 7 robot joints or you have a process where the tool's Z-axis rotation doesn't matter).

Reference Example

Descartes Tutorial

Scan-N-Plan Application: Problem Statement

In this exercise, you will add a new node to your Scan-N-Plan application, based on a reference template, that:

  1. Takes the nominal pose of the marker as input through a ROS service.
  2. Produces a joint trajectory that commands the robot to trace the perimeter of the marker (as if it is dispensing adhesive).

Scan-N-Plan Application: Guidance

In the interest of time, we've included a file, descartes_node.cpp, that:

  1. Defines a new node & accompanying class for our Cartesian path planning.
  2. Defines the actual service and initializes the Descartes library.
  3. Provides the high level work flow (see planPath function).

Left to you are the details of:

  1. Defining a series of Cartesian poses that comprise a robot “path”.
  2. Translating those paths into something Descartes can understand.

Setup workspace

  1. Clone the Descartes repository into your workspace src/ directory.

    cd ~/catkin_ws/src
git clone -b kinetic-devel https://github.com/ros-industrial-consortium/descartes.git

  1. Copy over the ur5_demo_descartes package into your workspace src/ directory.

    cp -r ~/industrial_training/exercises/4.1/src/ur5_demo_descartes .

  2. Copy over the descartes_node_unfinished.cpp into your core package's src/ folder and rename it descartes_node.cpp.

    cp ~/industrial_training/exercises/4.1/src/descartes_node_unfinished.cpp myworkcell_core/src/descartes_node.cpp

  3. Add dependencies for the following packages in the CMakeLists.txt & package.xml files. As in previous exercises, there are typically 2 lines to add to each file for each dependency package.

    • ur5_demo_descartes
    • descartes_trajectory
    • descartes_planner
    • descartes_utilities

  4. Create rules in the workcell_core package's CMakeLists.txt to build a new node called descartes_node. As in previous exercises, add these lines near similar lines in the template file (not as a block as shown below).

    add_executable(descartes_node src/descartes_node.cpp)
add_dependencies(descartes_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(descartes_node ${catkin_LIBRARIES})

Complete Descartes Node

We will create a Service interface to execute the Descartes planning algorithm.

  1. Define a new service named PlanCartesianPath.srv in the workcell_core package's srv/ directory. This service takes the central target position and computes a joint trajectory to trace the target edges.

    # request
geometry_msgs/Pose pose

---

# response
trajectory_msgs/JointTrajectory trajectory

  2. Add the newly-created service file to the add_service_file() rule in the package's CMakeLists.txt.

  3. Since our new service references a message type from another package, we'll need to add that other package (trajectory_msgs) as a dependency in the workcell_core CMakeLists.txt (3 lines) and package.xml (2 lines) files.

  4. Review descartes_node.cpp to understand the code structure. In particular, the planPath method outlines the main sequence of steps.

  5. Search for the TODO commands in the Descartes node file and expand on those areas:

    1. In makeToolPoses, generate the remaining 3 sides of a path tracing the outside of our "AR Marker" target part.
    2. In makeDescartesTrajectory, convert the path you created into a Descartes Trajectory, one point at a time.
      • Don't forget to transform each nominal point by the specified reference pose: ref * point
    3. In makeTolerancedCartesianPoint, create a new AxialSymmetricPt from the given pose.
      • See here for more documentation on this point type
      • Allow the point to be symmetric about the Z-axis (AxialSymmetricPt::Z_AXIS), with an increment of 90 degrees (PI/2 radians)

Update Workcell Node

With the Descartes node completed, we now want to invoke its logic by adding a new ServiceClient to the primary workcell node. The result of this service is a joint trajectory that we must then execute on the robot. This can be accomplished in many ways; here we will call the JointTrajectoryAction directly.

  1. In myworkcell_node.cpp, add include statements for the following headers:

    #include <actionlib/client/simple_action_client.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <myworkcell_core/PlanCartesianPath.h>

    You do not need to add new dependenies for these libraries/messages, because they are pulled in transitively from moveit.

  2. In your ScanNPlan class, add new private member variables: a ServiceClient for the PlanCartesianPath service and an action client for FollowJointTrajectoryAction:

    ros::ServiceClient cartesian_client_;
actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> ac_;

  3. Initialize these new objects in your constructor. Note that the action client has to be initialized in what is called the initializer list.

    ScanNPlan(ros::NodeHandle& nh) : ac_("joint_trajectory_action", true)
{
  // ... code
  cartesian_client_ = nh.serviceClient<myworkcell_core::PlanCartesianPath>("plan_path");
}

  4. At the end of the start() function, create a new Cartesian service and make the service request:

    // Plan cartesian path
myworkcell_core::PlanCartesianPath cartesian_srv;
cartesian_srv.request.pose = move_target;
if (!cartesian_client_.call(cartesian_srv))
{
  ROS_ERROR("Could not plan for path");
  return;
}

  5. Continue adding the following lines, to execute that path by sending an action directly to the action server (bypassing MoveIt):

    // Execute descartes-planned path directly (bypassing MoveIt)
ROS_INFO("Got cart path, executing");
control_msgs::FollowJointTrajectoryGoal goal;
goal.trajectory = cartesian_srv.response.trajectory;
ac_.sendGoal(goal);
ac_.waitForResult();
ROS_INFO("Done");

Test Full Application

  1. Create a new setup.launch file (in workcell_support package) that brings up everything except your workcell_node:

    <include file="$(find myworkcell_moveit_config)/launch/myworkcell_planning_execution.launch"/>
<node name="fake_ar_publisher" pkg="fake_ar_publisher" type="fake_ar_publisher_node" />
<node name="vision_node" type="vision_node" pkg="myworkcell_core" output="screen"/>
<node name="descartes_node" type="descartes_node" pkg="myworkcell_core" output="screen"/>

  2. Run the new setup file, then your main workcell node:

    roslaunch myworkcell_support setup.launch
rosrun myworkcell_core myworkcell_node

    It's difficult to see what's happening with the rviz planning-loop always running. Disable this loop animation in rviz (Displays -> Planned Path -> Loop Animation), then rerun myworkcell_node.

Hints and Help

Hints:

  • The path we define in makeToolPoses() is relative to some known reference point on the part you are working with. So a tool pose of (0, 0, 0) would be exactly at the reference point, and not at the origin of the world coordinate system.
  • In makeDescartesTrajectorty(...) we need to convert the relative tool poses into world coordinates using the “ref” pose.
  • In makeTolerancedCartesianPoint(...) consider the following documentation for specific implementations of common joint trajectory points:

Solutions:

  • Add the rest of the Cartesian path

    Eigen::Vector3d bot_right (-half_side, -half_side, 0);
Eigen::Vector3d top_right (half_side, -half_side, 0);

    auto segment2 = makeLine(bot_left, bot_right, step_size);
auto segment3 = makeLine(bot_right, top_right, step_size);
auto segment4 = makeLine(top_right, top_left, step_size);

    path.insert(path.end(), segment2.begin(), segment2.end());
path.insert(path.end(), segment3.begin(), segment3.end());
path.insert(path.end(), segment4.begin(), segment4.end());

  • Make a Descartes "Cartesian" point with some kind of constraints,

    descartes_core::TrajectoryPtPtr pt = makeTolerancedCartesianPoint(ref * point);

Open Source Feedback

See something that needs improvement? Please open a pull request on this GitHub page