Development Plan

[marip8]

Initial development plan:

• Remove KinematicsInterface, CollisionInterface, and all samplers depending on these interfaces. Prefer to use the single PositionSampler interface instead.
  • Most use of Descartes involves both kinematics-solving and collision checking, both of which are generally packaged together in planning environment such as tesseract or moveit.
  • Instead of defining sampler interfaces like GantrySampler, RobotRailSampler, it seems easier to use a specialized kinematics solver in the context of moveit or tesseract to provide all possible valid kinematics solutions and write a single PositionSampler class
• Create an interface for solving the Descartes graph to support multiple implementations
  • Keep exisiting custom ladder-graph DAG search
  • Add Boost graph library search
  • Add OMPL-style graph discovery searches
• Add unit tests
• Add meaningful documentation to interfaces
• Rename from descartes_light -> descartes_core
• Release debian distribution
• Re-integrate with ros-industrial-consortium/descartes
  • Remove descartes_core and descartes_planner
  • Revise the moveit interface

[Levi-Armstrong]

Bellow are the current interface in descartes light to discuss. Would this interface work for descartes? If not, what would need to be added?

Solver Interface

template <typename FloatType>
class Solver
{
public:
  // Solver(const std::size_t dof); Should change this so it is not required.
  virtual ~Solver() = default;

  virtual  bool build(const std::vector<typename PositionSampler<FloatType>::Ptr>& trajectory,
                              const std::vector<typename EdgeEvaluator<FloatType>::Ptr>& edge_eval,
                              int num_threads = getMaxThreads()) = 0;

 virtual bool build(const std::vector<typename PositionSampler<FloatType>::Ptr>& trajectory,
                            typename EdgeEvaluator<FloatType>::Ptr edge_eval,
                            int num_threads = getMaxThreads()) = 0;

  virtual const std::vector<std::size_t>& getFailedVertices() const = 0;
  virtual const std::vector<std::size_t>& getFailedEdges() const = 0;

  virtual bool search(std::vector<FloatType>& solution) = 0;

  static int getMaxThreads() { return omp_get_max_threads(); }
}

Position Sampler Interface

template <typename FloatType>
class PositionSampler
{
public:
  using Ptr = std::shared_ptr<PositionSampler<FloatType>>;
  using ConstPtr = std::shared_ptr<const PositionSampler<FloatType>>;

  virtual ~PositionSampler() = default;

  virtual bool sample(std::vector<FloatType>& solution_set) = 0;
};

Edge Evaluator Interface

template <typename FloatType>
class EdgeEvaluator
{
public:
  using Ptr = typename std::shared_ptr<EdgeEvaluator<FloatType>>;

  // EdgeEvaluator(std::size_t dof) : dof_(dof) {} Should change this so it is not required
  virtual ~EdgeEvaluator() = default;

  /**
   * @brief Determines whether the edge between two vertices is valid and, if so, its cost.
   * @param start The start state of the edge
   * @param end The end state of the edge
   * @return A pair <True/False, Cost>, True if edge is valid, false otherwise. Cost to move from the first vertex to
   * the next
   */
  virtual std::pair<bool, FloatType> considerEdge(const FloatType* start, const FloatType* end) = 0;

  bool evaluate(const Rung_<FloatType>& from, const Rung_<FloatType>& to, std::vector<typename LadderGraph<FloatType>::EdgeList>& edge_lists)
  {
    const auto n_start = from.data.size() / dof_;
    const auto n_end = to.data.size() / dof_;

    edge_lists.resize(n_start);
    for (std::size_t i = 0; i < n_start; ++i)
    {
      const auto* start_vertex = from.data.data() + dof_ * i;
      for (std::size_t j = 0; j < n_end; ++j)
      {
        const FloatType* end_vertex = to.data.data() + dof_ * j;

        // Consider the edge:
        std::pair<bool, FloatType> results = considerEdge(start_vertex, end_vertex);
        if (results.first)
          edge_lists[i].emplace_back(results.second, j);
      }
    }

    for (const auto& edge_list : edge_lists)
      if (!edge_list.empty())
        return true;

    return false;
  }

protected:
  std::size_t dof_;
};

[marip8]

I think we should change a couple of things:

General Changes

• Leverage exceptions to communicate failure instead of boolean returns
  • SolverInterface::build -> void build(...)
  • SolverInterface::search -> std::vector<FloatType> search()
  • PositionSampler::sample -> std::vector<FloatType> sample()
• Create a DescartesException as the base class of all exceptions thrown in this repository
• Represent trajectories, rungs, position samples, etc. in terms of std::vector<std::vector<FloatType>> (or something similar like Eigen::MatrixXd or Eigen::ArrayXXd) rather than a single vector whose size is a multiple of the robot DOF. I find the semantics of the current interfaces pretty confusing because they return a vector of floats when you would expect them to return an array of joint poses, which themselves are a vector of floats.
• What is the benefit of using float pointers in the interface and forcing each interface implementation to maintain a member that represents the DOF of the system? Why aren't we using std::vector or Eigen::VectorXd instead since it inherently captures the DOF as the size of the vector?

Solver Interface

• Return failed vertices relative to the rung that they were a part of rather than their global index which doesn't mean much
  • std::map<rung_idx, std::vector<vertex_idx>>
• Return failed edges relative to the rung and vertex they were connected with rather than their global index which doesn't mean much
  • std::map<rung_idx, std::map<vertex_idx, std::vector<edge_idx>>>

Edge Evaluator

• Move EdgeEvaluator::evaluate into the LadderGraphDAGSearch class. Rungs in their current defintions are only constructs of the ladder graph approach

[Levi-Armstrong]

Here is a first pass at the updated interface.

New Interface

Solver

template <typename FloatType>
class Solver
{
public:
  Solver(const std::size_t dof);

  virtual bool build(const std::vector<typename WaypointSampler<FloatType>::Ptr>& trajectory,
             const std::vector<typename EdgeEvaluator<FloatType>::Ptr>& edge_eval,
             int num_threads = getMaxThreads()) = 0;

  virtual bool build(const std::vector<typename WaypointSampler<FloatType>::Ptr>& trajectory,
             typename EdgeEvaluator<FloatType>::Ptr edge_eval,
             int num_threads = getMaxThreads()) = 0;

  virtual const std::vector<std::size_t>& getFailedVertices() const = 0;
  virtual const std::vector<std::size_t>& getFailedEdges() const = 0;

  virtual std::vector<Eigen::Matrix<FloatType, Eigen::Dynamic, 1>> search() = 0;

  virtual int getMaxThreads() const = 0;
};

Position/Waypoint Sampler

template <typename FloatType>
class WaypointSampler
{
public:
  virtual ~WaypointSampler() = default;

  virtual std::vector<Eigen::Matrix<FloatType, Eigen::Dynamic, 1>> sample() = 0;
};

Edge Evaluator

template <typename FloatType>
class EdgeEvaluator
{
public:
  using Ptr = typename std::shared_ptr<EdgeEvaluator<FloatType>>;
  virtual ~EdgeEvaluator() = default;

  /**
   * @brief Determines whether the edge between two vertices is valid and, if so, its cost.
   * @param start The start state of the edge
   * @param end The end state of the edge
   * @return A pair <True/False, Cost>, True if edge is valid, false otherwise. Cost to move from the first vertex to
   * the next
   */
  virtual std::pair<bool, FloatType> evaluate(const Eigen::Matrix<FloatType, Eigen::Dynamic, 1>& start,
                                              const Eigen::Matrix<FloatType, Eigen::Dynamic, 1>& end) = 0;
};

[Levi-Armstrong]

Add State Evaluator

namespace descartes_light
{
template <typename FloatType>
class StateCostEvaluator
{
public:
  using Ptr = std::shared_ptr<StateCostEvaluator<FloatType>>;
  using ConstPtr = std::shared_ptr<const StateCostEvaluator<FloatType>>;

  virtual ~StateCostEvaluator() = default;

  virtual std::pair<bool, FloatType> evaluate(const Eigen::Matrix<FloatType, Eigen::Dynamic, 1>& solution) const = 0;
};

using StateCostEvaluatorF = StateCostEvaluator<float>;
using StateCostEvaluatorD = StateCostEvaluator<double>;

}  // namespace descartes_light