The Descartes maintainers should be updated in each package.xml.

Dan Solomon should be removed (but kept as an author). Please add @jrgnicho, @shaun-edwards, and @Jmeyer1292.

Maintainers get an email when the build system detects a failure.

Valid solutions for the sampled cartesian points can not be produced.

The straightforward nature of the core Descartes algorithm contained in planning_graph.cpp lends itself very well to doing parallel computations. For example, the bulk of the computational cost in generating the graph is in calculating IKs where each point is considered on its own. This applies to edge calculations and even searching the graph from multiple starting points.

Descartes could benefit massively from splitting up the graph generation step into threads. This imposes the interesting requirement that all classes derived from TrajectoryPt and RobotModel must be thread-safe. This is currently not the case with the moveit state adapter because RobotModel is not thread safe.

I'm not sure what the right solution is. The easiest solution would be to copy the users RobotModel into each thread doing computation but that would require adding a clone() function to Descartes' RobotModel interface.

We could also create an alternate interface that inherits from the current but adds the clone method. This would be fed to an alternate planning algorithm that takes "ThreadSafeRobotModel" for example. This might involve a lot of code duplication.

The destructor of the PlanningGraph may attempt to delete an unassigned non null pointer if no points were ever added to the planner.

This library uses Eigen which has known issues on 32bit systems. These issues can be addressed by compiler switches, but these are not present.

This issue is a result of discussions for PR #13 & #15.

At a high level a trajectory_pt should be independent of solvers and samplers. Two key parts of the discussion are copied here for context:

@shaun-edwards

The main rationale for making a trajectory_pt look like both a cartesian and joint point is to allow planners to operate on trajectories of mixed types. In order for this to work, a point needs to be able to calculate it's own IK/FK (and sample for that matter).

@dpsolomon

Each TrajectoryPt should contain a pointer to a sampler class, and implement a bool sample(vector<JointPoint>&, int n) method that simply calls the sampler with that point. The samplershould be able to incrementally sample according to its own discretization (or maybe discretization could also be passed as an arguement?). Repeated calls to `sample(.., 1) will return single-item vector, sample(.., 0) will return a vector of all joint solutions according to the discretization.
The initial implementation should just call sample(.., 1) on each point, and the samplers should return the nominal joint solution. My though was that subsequent calls to sample() return results farther from the nominal until the tolerance is all used up.

TLDR: I discovered issues with how pruning and Djikstras_shortest_paths was being used. Optimal path planning, NOT IK calculations, (using aggressive joint delta checks) has speed up by 2000-5000x in my limited testing.

I have been working on making Descartes lately in my work and made some improvements. I discovered a few issues and have developed a fixes for them. I'm making this issue to reference in my pull-request (coming this weekend) and to share with you all the exciting results. This is related to issue #21 and issue #48.

The two most expensive components of Descartes are the IK solutions and the graph searching to find an ideal solutions. IK is O(n). Searching is something bigger like O(n^2). With IKFast, the IK can be done very quickly but it was taking minutes to solve for the optimal path with several hundred points.

The long planning time can be abridged by fixing the start and end joint positions, but you have to know those positions and it sacrifices optimality.

The reason the planner is so slow right now is primarily two fold:

1. There are checks for large joint changes and on that condition edges are given huge weights when in fact they should simply be removed. Not adding an edge led to strange problems where the graph search would solve paths that were only several points long. This turned out to be an issue with the findStartVertices and findEndVertices which were actually checking all of the vertices for the absence of in-edges and out-edges respectively. Naturally if we start pruning edges then outliers in the middle of the users trajectory will start showing up as start or end points using that logic. Fixing that allows Dijkstra's search to search MUCH faster and still fail elegantly.
2. The boost algorithm being used (http://www.boost.org/doc/libs/1_57_0/libs/graph/doc/dijkstra_shortest_paths.html) calculates the shortest path from a source vertex to EVERY other vertex. Currently we are running the algorithm for every start point to every end point (n^2 ops) when all we need to do is run the algorithm from every start point (n operations). Fixing this obviously means we get N times speed boosts where N is the number of end point discretizations.

Using C++11's steady clock from the chrono library, I took some poor man's measurements using the simulated objects in Godel (the smallest face closest to the robot and the biggest face) and got the following timing (all builds were with release flags):

Time w/o Pruning
6092 joint solutions -> 10467 ms
39948 joint solutions -> 211514 ms

Time w/ Pruning
6092 joint solutions -> 37 ms
39948 joint solutions -> 561 ms

Time w/ Pruning and Boost improvement
6084 joint solutions -> 5 ms
39948 joint solutions -> 41 ms

I pruned edges with single joint motions exceeding 10 degrees between points. I've also developed a system of time paramiterization with the help of @shaun-edwards and @jrgnicho which I will push this weekend as well.

Would y'all prefer these improvements seperate from the time pull request or together?

A GUI friendly class for holding an array of trajectory points should have an interface similar to the ViaPointContainter class in the amos repo here

As per title. See descartes_core/CMakeLists.txt.

Not sure if this is by design, but it is considered bad practice to set the CMake build type from within a CMakeLists.txt file. Recommended is to do it at configuration time (ie: catkin_make -DCMAKE_BUILD_TYPE=...).

The console_bridge methods like logInform and logDebug do not support streams. As a result, some additional code is required when a stream is desired (mostly due to convenience and ease of use).

For example, the following snippet

std::stringstream msg;
msg << "Found first solution on " << sample_iter << " iteration, joint: " << joint_pose;
logDebug(msg.str().c_str());

This code requires constructing a stream and converting several objects to strings, even in the case that DEBUG message are not enabled.

ROS Console and it's stream versions provide several advantages:

• Logging messages can be remove at run-time, see here.
• One line initialization of log messages. This requires implementing the << stream operator for most objects, but this is helpful in general (i.e. for testing).
• ROS_*_STREAM arguments (i.e. string conversion) are not evaluated (more than likely) according to this.

Cartesian and Joint trajectory point types do not define equality. This makes testing difficult. Testing equality of these types is non-trivial given their complexity and class hieararchy. See here for an example implementation. Note, it does appear that most approaches for implementing the == operator are meant to protect from bugs in development, not necessarily functional issues.

Currently, all source/include files are include in the descartes_trajectory_planning package. I would like to propose a new package hierarchy to minimize dependencies and keep things better organized.

• descartes (repo)
  • descartes_core - All abstract and generalize-able classes, common utilities. Basic implementations can be stored here until interfaces become defined (I'm thinking about the planner here). This would basically be a rename of descartes_trajectory_planning.
  • descartes_moveit - This package would introduce a dependency on moveit. Wrappers and class implementations that are moveit specific will be stored here.
  • descartes_gui - future - Common GUI tools for editing/managing paths, trajectory planning tools, etc..
  • descartes_planners - future - Specific implementation of planners

NOTE - we should also change namespaces to be identical to package names (this is the ROS convention).

The current structures within Descartes are all prefixed with process. However, the data types do not include any process data.

I'd like to propose that the process modifier be removed from the data types and file names and replaced with more generic types such as waypoint and trajectory. Higher level classes will certainly contain process data, at which point, the prefix would seem more appropriate.

The getIK method does not verify that the joint solution is within the allowed joint limits.

The Descartes wiki page does not include auto-generated content because a meta-package does not exist. An example meta-package can be found in industrial_core.

The discretization of the cartesian poses in the getCartesianPoses() method does not take into account the tool_pt_ and tool_base_ members when performing this computation. The correct way is already implemented in the getJointPoses() method.

Paths returned by the graph planner sometimes contain large joint changes between consecutive points

The CartTrajectoryPt::getClosestJointPose(...) methods needs to be implemented in order to address the sparse planner implementation discussed in issue #37.

All pkgs in the descartes repository add the -std=gnu++0x compiler option somewhere in their CMakeLists.txt (descartes_core fi). Dependant pkgs also need to set this flag, in order to be able to use headers from descartes (godel_path_planning fi).

It is customary in such cases (in catkin projects) for the providing pkg to export any necessary cflags by exporting a pkgname_CFLAGS or pkgname_DEFINITIONS variable (with a preference for the latter, it seems) in a .cmake snippet that is passed to dependants using the CFG_EXTRAS argument to the catkin_package(..) call.

All pkgs in descartes should use this mechanism to export the required -std=gnu++0x.

Related: afaict, the -std=gnu++0x option could actually be GCC specific and should ideally be guarded by a check for CMAKE_COMPILER_IS_GNUCXX (although apparently CLang is compatible with GCC command line options, so it should work there).

The MoveitStateAdapter does not perform any collision checking. This capability needs to be implemented so that only IK and FK collision free solutions are returned.

The current approach for initializing a trajectory point with the data of another point is to use the default copy constructor. This operation copies every member into the new object including the id. Thus, in order to prevent issues with duplicate ids a clone method should be provided. Then, the cloned point will be initialized to the same data as the originating point but will be assigned a new unique id.

The current interface (and implementations) for descartes path planners defines an initialize method which takes as an argument a reference to a shared pointer to a const robot model. This implies that the interface might make changes to the shared pointer itself. It also prevents users from initializing a shared pointer to a robot model and then simply passing it into the constructor (as that requires the compiler to make two levels of indirection to ConstPtr and then to ConstPtr&).

The fix is to simply take the shared pointer by value and use std::move (since we're in c++11 land now) to get it all the way down the chain to planning graph.

The function/method parameter ordering does not conform to the ROS style guide. According to the guide, the parameters must be inputs first, outputs last. Inputs should be passed as const references/points and outputs as non-const.

Affected files:

• trajectory_pt.h
• cart_trajectory_pt.h
• joint_trajectory_pt.h

The current branch hydro-devel shows that the wobj_base_ is checked against the last element of the link names array "back()" . Should it be compared against the robot base instead or the first element in the link names array "front()"?

See here and PR #14.

The constructor for the MoveitStateAdapter requires the robot tool and object frames (i.e. strings) be identified. See here.

@dpsolomon commented the following:

The design of the tool/wobj base was to allow planning with either a pure
robot moveit package, or a custom one. In order to support that strategy, I
think what was missing is a frame_id for each of the base frames.

I would rather see the frame id's added to the adaptor, and an identity
frame used for the bases. That should help avoid future problems expanding
the usage.

The calls to actually get ik should always solve between two known frames.
Moveit::robotstate does some sort of automatic check and correction for
difference between the planning frames and ik solver frames

The purpose of this issue is to continue the discussion.

The current MoveIt KinematicsBase class does not have a method to return multiple joint solutions for a single Cartesian point. The Descartes graph style path planners require such a method. To make use of MoveIt IK solutions in Descartes, we will need to add such a method.

There was some previous discussion on this.

Please coordinate with the @ros-planning/moveit_core team to ensure this addition is accepted.

Line 139
Shouldn't it be SparsePlanner?

Trajectory point IDs are not currently populated on construction. The IDs shall uniquely identify a point. This means they can be used as keys to a map and to link other data/object to trajectory points.

A simple IK search was implemented in PR #14. The search is a slow implementation leveraging the existing IK functionality in moveit. A better/faster search implementation will be needed, since this method is called multiple times in the building of the planning graph.

The getClosestJointPose function of the cartesian trajectory point does FK on a set of joint values and then checks to see if that solution is within the tolerances of the toleranced-frame "wobj_pt_".

Cartesian point, however, specifies 4 possible frames to fully define the position of the robot end-effector and this method only takes into account one of them. If someone specifies the other transformations as other than identity (say they use AxialSymmetricPt) then this method will fail.

I'm not sure what the correct math is, but we need to find a way to compute the over-all tolerances in the world frame based on the four transformations (two of which are nominal and two of which are toleranced).

See: https://github.com/ros-industrial-consortium/descartes/blob/hydro-devel/descartes_trajectory/src/cart_trajectory_pt.cpp#L276-L289

The trajectory_pt object contains a transition pointer. To simplify things, I propose to remove the transition pointer and replace it with velocity and timing tolerances AT the trajectory point (acceleration could be added later). These are somewhat redundant since all are dependent on the profile used between points. To avoid this ambiguity/confusion, the profiles can be assumed to be well behaved and the density of the points ensures this.

A higher level path abstraction similar to gcode/robot code should be developed (in which case transition types are required), but for now we should assume dense trajectories.

This is the issue related to the discussion started in PR #6; in order to avoid repeating myself below I'm pasting the comment I made regarding this issue:

I see that there are several methods that take a moveit::core::RobotState object in order to perform actions specific to a particular robot model. It is my understanding that this RobotState object holds one or more "planning groups" and most of its IK and FK related methods require knowing about the group http://moveit.ros.org/doxygen/classmoveit_1_1core_1_1RobotState.html . Thus, our current interface methods won't be of much use as they are since "planning groups" can not be specified with them. One easy solution is to expand each method's argument list so that they accept planning group names or moveit::core::JointModelGroups objects. Another option is to pass the RobotState and planning group name in the constructor of any specialized class that inherits from these interfaces and remove the RobotState object from the methods' argument list since it would be redundant to have it there. The second option is more appealing to me because it would make our interfaces more flexible by not being tied up to any particular robot model implementation (the moveit RobotState as of now). One good reason for keeping our interface classes agnostic is those cases where multiple joint configurations exist for a single end-effector pose. The moveit RobotState only returns one solution (at least I haven't found a way to get more than one) even if more are available. Ideally, we'd handle this by implementing a custom ik solver or robot model and save a reference to it in the specialized TrajectoryPt implementation. This probably requires further discussion and should be addressed in future PRs.

Another suggested solution by @shaun-edwards is to implement a RobotModel interface class that will be used as input for the TrajectoryPt interface methods instead of the moveit::core::RobotState as it is the case now. This interface could then be specialized to handle the unique requirements of a path planner , such as returning multiple joint configurations for a tool pose or it could even contain an instance of the moveit RobotState and it's corresponding planning group. Hopefully, I captured the original idea correctly (@shaun-edwards correct me if I'm wrong). I'm on board with this approach
Something else that stood out to me were the constraint members used by the ToleranceFrame structures. These constraints are also part of the moveit library and required a moveit::core::RobotModel instance (http://moveit.ros.org/doxygen/classmoveit_1_1core_1_1RobotModel.html). If we decide to use our own RobotModel interface then we might have to define our own constraint structures which do not depend on moveit's RobotModel class

Create a node that services the moveit_msgs::GetCartesianPath and uses a descartes path planner

The current (working) graph search algorithm is slow when a trajectory with 1000's of points is desired. Such trajectories could be common, especially when trying to control positions with high accuracy or when performing linear motion with a manipulator. Preliminary tests indicate that graph construction is the expensive(time) operation.

For example, assume the following: 1000 Cartesian trajectory points specified, with a free yaw rotation (at 1 degree resolution - required for smooth motion), on a standard 6 DOF industrial robot, w/o collision checking. The number of IK calls (approximately the same as number of nodes) in the graph (i.e. that must be constructed) is:

1000 Cart pts * 360 yaw samples/Cart pt * 10 IK solutions (assuming KDL) = 3.6M (IK solver calls)

Current tests have been executed without yaw sampling and take several seconds to load. Even a sparse yaw sampling (which isn't good for motion smoothness) will result in very long graph construction times.

An obvious improvement to the existing algorithm is to use IKFast instead of KDL. In the past we have seen a 10x faster solve time with IKFast. Our results may be better given IKFast doesn't need to "search" for multiple IK solutions. It can calculated them all directly.

While this is an obvious change, it will not solve the main problem: The current planning algorithm does not scale with time. Furthermore, simple planning problems take as long as to solve as more complex ones.

The process point/waypoint structure doesn't capture nominal values. A waypoint will likely have a desired value, with some error bound or tolerance. When at all possible, the nominal value should be held. The tolerance zone should only be used when the process requires/allows for it.

See http://jenkins.ros.org/job/ros-hydro-descartes-trajectory_sourcedeb/

The base class for TrajectoryPt defines a virtual method cloneTo:

virtual void cloneTo(TrajectoryPt &clone) const
  {
    clone = *this;
    clone.setID(TrajectoryID::make_id());
  }

This method is unimplemented in all of the child classes and it will not do the 'expected' thing. What I believe it will do is 'slice' the 'this' variable and only copy the information that it is aware of in the base class itself.

The fix is probably to create a clone function that returns an entirely new TrajectoryPtPtr with the appropriate underlying type. The implementation will probably look something like:

virtual TrajectoyPtPtr clone() const
{
// add cloning info for timing and ids
return new SomeTypeOfPt(*this);
}

Or it could by a cloneTo method. The difficulty there is memory management. You'd probably have to take a shared pointer by reference and call it's reset function.

The path planner tests need to be improved/refactored:

• The dense_planner & sparse_planner tests have similar code (i.e. multiple definitions of ThreeDOFRobot) and similar tests.
  • The ThreeDOFRobot class should be extracted into it's own source/header file
  • A typed test approach, similar to robot_model_test should be investigated. See type parameterized tests for gtest documentation.
• Tests should be added to track planner performance and make certain that a major regression does not take place.
• Tests that exercise unsolvable paths (i.e. exceeding joint velocity limits) should also be added.
• Stress tests (i.e. large number of points, unconstrained paths, etc..) should also be added (no test should take more than 10 minutes to complete).

When we first considered using A* to search the graph, we assumed it would not make much impact due to the fact that at every Cartesian point, we would need to essentially expand all the sampled configuration positions (neighbors in the graph). But, now that we have implemented a limit on the maximum joint velocity, thereby limiting the maximum difference in joint positions between points, the number of nodes to expand will be limited. So, should we consider A* again?

Update: to be more clear, I am considering the case where the graph is not fully populated up front. Nodes are added as they are explored, thereby reducing the number of IK and UUID calls.

The current waypoint types are specific to Cartesian waypoints. While the majority of a path is expected to be this type, joint waypoints, and other cooler waypoints we don't even know about, should be supported. To support both joint and Cartesian waypoints, a common base class/interface is proposed. Specific joint and Cartesian implementations will be created, while still allowing for new, custom types as well.

A key requirement of trajectory following is that the robot maintain a path velocity. The path velocity and timing must be captured in the based data types. In addition, when/where this data is populated still is TBD. This data will also be used to determine if adjacent joint nodes in the planning graph are connected. If the joint velocity required to move between joint points is too high, then the joint points are not connected.

While playing with Descartes to do Cartesian motion through free space, I was generating some cartesian points with several hundred to tens of thousands of joint solutions (using IKFast). I noticed that the IK process was taking a while but the actual joint solutions came available very quickly.

I did some profiling and it seems that a single IK solution takes ~5 microseconds to calculate (using IKFast). Creating a JointTrajectoryPt, putting it into a std::list, and inserting it into the joint solutions std::map takes 1 millisecond per point. As you can imagine this adds up quickly.

We pay 100x more in data structure costs than the raw calculations. I need to look into it more, but I imagine that the majority of the cost comes from cache misses associated with std::list and map as well as the cost of doing map lookup and insertion for every pt.

I'm not sure what the right solution is, but it might require significant changes to PlanningGraph. It probably means doing away with the extra map of uuids to joint solutions. If we ever find that PlanningGraph is too slow, I believe this is the best approach to fixing that.

In order to use RobotModel implementations in a generalized way then each Implementation should be made into a plugin that can be loaded at run time. Currently this won't be possible due to a limitation that prevents libraries containing the plugins from being loaded dynamically if they are also linked statically. The descartes_core package where the RobotModel interface resides suffers from this drawback since it also contains the implementation for the various trajectory point classes which are linked statically. One easy solution would be to isolate the RobotModel class into its own package. The nav_core of the navigation repo is a good example of this.

To make the planner accessible to others, we need to write some actual documentation on:

1. What problems the planner solves
2. Software architecture
3. A typical Descartes workflow (trajectory creation, planning, to ROS, etc...)
4. Examples for common use cases

I propose that all of the get* methods in "path_planner_base.h" be marked const. I think it is reasonable for users to expect that retrieving a plan not change the state of the graph for subsiquent requests and be able to use const correctness in their own code.

Specifically, the following methods should be changed:
virtual void getConfig(PlannerConfig& config) const = 0;
virtual bool getPath(std::vector& path) const = 0;
virtual int getErrorCode() const = 0;
virtual bool getErrorMessage(int error_code, std::string& msg) const = 0;

The current descartes planner (PlanningGraph, SparsePlanner) only operate on dense trajectories. Thus an Interpolator would produce intermediate points between the sparse waypoints and then pass the interpolated dense trajectory to a planner. In addition, it should allow for several interpolation strategies (cartesian, joint, etc) and tolerances (waypoint zone) for each waypoint.

In CartesianPt::getClosestJointPose ,FK is called with the seed pose and the resulting x,y,z, rx, ry,rz values are check against those in the point to find the closest cartesian pose. Then IK is called for the closest cartesian pose in order to find the corresponding joints, however when IK fails the method does not return a joint pose. This behavior seems lacking since in theory there should always be a closest joint pose. A solution for this would consist of getting all the valid joint poses and for the point and comparing against the seed to find the closest pose in joint space.

This is a minor quibble, but the current RobotModel initialize method takes the robot description string by value instead of by reference. This appears to be a simple typo, but I wanted to check.

https://github.com/ros-industrial-consortium/descartes/blob/hydro-devel/descartes_core/include/descartes_core/robot_model.h#L102

The unit test covering the number of discretizations of a given sample space currently fails.

Value of: NUM_SAMPLED_ORIENT
  Actual: 216
Expected: solutions.size()
Which is: 124
/home/jon/ros/godel_ws/src/descartes/descartes_trajectory/test/cart_trajectory_pt.cpp:90: Failure
Value of: NUM_SAMPLED_ORIENT
  Actual: 216
Expected: joint_solutions.size()
Which is: 124
[ INFO] [1426606593.117965884]: Testing fuzzy both point
/home/jon/ros/godel_ws/src/descartes/descartes_trajectory/test/cart_trajectory_pt.cpp:94: Failure
Value of: NUM_SAMPLED_BOTH
  Actual: 287496
Expected: solutions.size()
Which is: 165044
/home/jon/ros/godel_ws/src/descartes/descartes_trajectory/test/cart_trajectory_pt.cpp:96: Failure
Value of: NUM_SAMPLED_BOTH
  Actual: 287496
Expected: joint_solutions.size()
Which is: 165044
[  FAILED  ] CartTrajPt.getPoses (233 ms)

The fix in PR #96 introduced a dependency on the boost::atomic, see here. This library doesn't appear to exist in the ROS Hydro boost verions, 1.46.1, see not here.