Remove TimingConstraint and simplify edge evaluator interface

swri-robotics · Oct 5, 2020 · 8da5322 · 8da5322
1 parent dea08d6
commit 8da5322
Show file tree

Hide file tree

Showing 13 changed files with 34 additions and 90 deletions.
diff --git a/descartes_light/include/descartes_light/descartes_light.h b/descartes_light/include/descartes_light/descartes_light.h
@@ -38,12 +38,10 @@ class Solver
   Solver(const std::size_t dof);
 
   bool build(const std::vector<typename PositionSampler<FloatType>::Ptr>& trajectory,
-             const std::vector<descartes_core::TimingConstraint<FloatType>>& times,
              const std::vector<typename EdgeEvaluator<FloatType>::Ptr>& edge_eval,
              int num_threads = getMaxThreads());
 
   bool build(const std::vector<typename PositionSampler<FloatType>::Ptr>& trajectory,
-             const std::vector<descartes_core::TimingConstraint<FloatType>>& times,
              typename EdgeEvaluator<FloatType>::Ptr edge_eval,
              int num_threads = getMaxThreads());
 

diff --git a/descartes_light/include/descartes_light/impl/descartes_light.hpp b/descartes_light/include/descartes_light/impl/descartes_light.hpp
@@ -69,7 +69,6 @@ Solver<FloatType>::Solver(const std::size_t dof) : graph_{ dof }
 
 template <typename FloatType>
 bool Solver<FloatType>::build(const std::vector<typename PositionSampler<FloatType>::Ptr>& trajectory,
-                              const std::vector<typename descartes_core::TimingConstraint<FloatType>>& times,
                               const std::vector<typename EdgeEvaluator<FloatType>::Ptr>& edge_eval,
                               int num_threads)
 {
@@ -87,7 +86,6 @@ bool Solver<FloatType>::build(const std::vector<typename PositionSampler<FloatTy
     if (trajectory[static_cast<size_t>(i)]->sample(vertex_data))
     {
       graph_.getRung(static_cast<size_t>(i)).data = std::move(vertex_data);
-      graph_.getRung(static_cast<size_t>(i)).timing = times[static_cast<size_t>(i)];
     }
     else
     {
@@ -147,12 +145,11 @@ bool Solver<FloatType>::build(const std::vector<typename PositionSampler<FloatTy
 
 template <typename FloatType>
 bool Solver<FloatType>::build(const std::vector<typename PositionSampler<FloatType>::Ptr>& trajectory,
-                              const std::vector<typename descartes_core::TimingConstraint<FloatType>>& times,
                               typename EdgeEvaluator<FloatType>::Ptr edge_eval,
                               int num_threads)
 {
   std::vector<typename EdgeEvaluator<FloatType>::Ptr> evaluators(trajectory.size() - 1, edge_eval);
-  return build(trajectory, times, evaluators, num_threads);
+  return build(trajectory, evaluators, num_threads);
 }
 
 template <typename FloatType>

diff --git a/descartes_light/include/descartes_light/impl/ladder_graph.hpp b/descartes_light/include/descartes_light/impl/ladder_graph.hpp
@@ -127,12 +127,10 @@ void LadderGraph<FloatType>::assignEdges(const std::size_t rung,
 template <typename FloatType>
 void LadderGraph<FloatType>::assignRung(const std::size_t index,
                                         descartes_core::TrajectoryID id,
-                                        descartes_core::TimingConstraint<FloatType> time,
                                         const std::vector<std::vector<FloatType>>& sols)
 {
   Rung& r = getRung(index);
   r.id = id;
-  r.timing = time;
   r.data.reserve(sols.size() * dof_);
   for (const auto& sol : sols)
   {

diff --git a/descartes_light/include/descartes_light/interface/edge_evaluator.h b/descartes_light/include/descartes_light/interface/edge_evaluator.h
@@ -36,19 +36,14 @@ class EdgeEvaluator
   EdgeEvaluator(std::size_t dof) : dof_(dof) {}
   virtual ~EdgeEvaluator() {}
 
-  virtual bool evaluate(const Rung_<FloatType>& from,
-                        const Rung_<FloatType>& to,
-                        std::vector<typename LadderGraph<FloatType>::EdgeList>& edge_lists)
+  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_;
 
-    // Allocate if needed because of Compound Evaluator
-    if (edge_lists.size() == 0)
-      edge_lists.resize(n_start);
-
-    assert(edge_lists.size() == n_start);
-
+    edge_lists.resize(n_start);
     for (std::size_t i = 0; i < n_start; ++i)
     {
       const auto* start_vertex = from.data.data() + dof_ * i;
@@ -57,7 +52,7 @@ class EdgeEvaluator
         const FloatType* end_vertex = to.data.data() + dof_ * j;
 
         // Consider the edge:
-        std::pair<bool, FloatType> results = considerEdge(from, start_vertex, to, end_vertex);
+        std::pair<bool, FloatType> results = considerEdge(start_vertex, end_vertex);
         if (results.first)
           edge_lists[i].emplace_back(results.second, j);
       }
@@ -72,17 +67,12 @@ class EdgeEvaluator
 
   /**
    * @brief Determines whether the edge between two vertices is valid and, if so, its cost.
-   * @param from The rung associated with the start state
    * @param start The start state of the edge
-   * @param to The rung associated with the end state
    * @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 Rung_<FloatType>& from,
-                                                  const FloatType* start,
-                                                  const Rung_<FloatType>& to,
-                                                  const FloatType* end) = 0;
+  virtual std::pair<bool, FloatType> considerEdge(const FloatType* start, const FloatType* end) = 0;
 
 protected:
   std::size_t dof_;

diff --git a/descartes_light/include/descartes_light/ladder_graph.h b/descartes_light/include/descartes_light/ladder_graph.h
@@ -30,18 +30,6 @@ DESCARTES_IGNORE_WARNINGS_POP
 namespace descartes_core
 {
 using TrajectoryID = std::size_t;
-
-template <typename FloatType>
-struct TimingConstraint
-{
-  TimingConstraint() : upper(0.0) {}
-  TimingConstraint(FloatType time) : upper(time) {}
-  FloatType upper;
-};
-
-using TimingConstraintF = TimingConstraint<float>;
-using TimingConstraintD = TimingConstraint<double>;
-
 }  // namespace descartes_core
 
 namespace descartes_light
@@ -61,9 +49,8 @@ struct Rung_
   using Edge = Edge_<FloatType>;
   using EdgeList = std::vector<Edge>;
 
-  descartes_core::TrajectoryID id;                     // corresponds to user's input ID
-  descartes_core::TimingConstraint<FloatType> timing;  // user input timing
-  std::vector<FloatType> data;                         // joint values stored in one contiguous array
+  descartes_core::TrajectoryID id;  // corresponds to user's input ID
+  std::vector<FloatType> data;      // joint values stored in one contiguous array
   std::vector<EdgeList> edges;
 };
 
@@ -152,7 +139,6 @@ class LadderGraph
    */
   void assignRung(const std::size_t index,
                   descartes_core::TrajectoryID id,
-                  descartes_core::TimingConstraint<FloatType> time,
                   const std::vector<std::vector<FloatType>>& sols);
 
   void removeRung(const std::size_t index);

diff --git a/descartes_samplers/include/descartes_samplers/evaluators/compound_edge_evaluator.h b/descartes_samplers/include/descartes_samplers/evaluators/compound_edge_evaluator.h
@@ -29,10 +29,7 @@ class CompoundEdgeEvaluator : public EdgeEvaluator<FloatType>
 public:
   CompoundEdgeEvaluator(int dof);
 
-  std::pair<bool, FloatType> considerEdge(const Rung_<FloatType>& from,
-                                          const FloatType* start,
-                                          const Rung_<FloatType>& to,
-                                          const FloatType* end) override;
+  std::pair<bool, FloatType> considerEdge(const FloatType* start, const FloatType* end) override;
 
   std::vector<typename EdgeEvaluator<FloatType>::Ptr> evaluators;
 };

diff --git a/descartes_samplers/include/descartes_samplers/evaluators/distance_edge_evaluator.h b/descartes_samplers/include/descartes_samplers/evaluators/distance_edge_evaluator.h
@@ -27,15 +27,12 @@ template <typename FloatType>
 class DistanceEdgeEvaluator : public EdgeEvaluator<FloatType>
 {
 public:
-  DistanceEdgeEvaluator(const std::vector<FloatType>& velocity_limits);
+  DistanceEdgeEvaluator(const std::vector<FloatType>& velocity_limits, FloatType dt);
 
-  std::pair<bool, FloatType> considerEdge(const Rung_<FloatType>& from,
-                                          const FloatType* start,
-                                          const Rung_<FloatType>& to,
-                                          const FloatType* end) override;
+  std::pair<bool, FloatType> considerEdge(const FloatType* start, const FloatType* end) override;
 
 protected:
-  std::vector<FloatType> velocity_limits_;
+  std::vector<FloatType> joint_distance_threshold_;
 };
 
 using DistanceEdgeEvaluatorF = DistanceEdgeEvaluator<float>;

diff --git a/descartes_samplers/include/descartes_samplers/evaluators/euclidean_distance_edge_evaluator.h b/descartes_samplers/include/descartes_samplers/evaluators/euclidean_distance_edge_evaluator.h
@@ -29,10 +29,7 @@ class EuclideanDistanceEdgeEvaluator : public EdgeEvaluator<FloatType>
 public:
   EuclideanDistanceEdgeEvaluator(int dof);
 
-  std::pair<bool, FloatType> considerEdge(const Rung_<FloatType>& from,
-                                          const FloatType* start,
-                                          const Rung_<FloatType>& to,
-                                          const FloatType* end) override;
+  std::pair<bool, FloatType> considerEdge(const FloatType* start, const FloatType* end) override;
 };
 
 using EuclideanDistanceEdgeEvaluatorF = EuclideanDistanceEdgeEvaluator<float>;

diff --git a/...samplers/include/descartes_samplers/evaluators/gantry_euclidean_distance_edge_evaluator.h b/...samplers/include/descartes_samplers/evaluators/gantry_euclidean_distance_edge_evaluator.h
@@ -29,10 +29,7 @@ class GantryEuclideanDistanceEdgeEvaluator : public EdgeEvaluator<FloatType>
 public:
   GantryEuclideanDistanceEdgeEvaluator(int dof);
 
-  std::pair<bool, FloatType> considerEdge(const Rung_<FloatType>& from,
-                                          const FloatType* start,
-                                          const Rung_<FloatType>& to,
-                                          const FloatType* end) override;
+  std::pair<bool, FloatType> considerEdge(const FloatType* start, const FloatType* end) override;
 };
 
 using GantryEuclideanDistanceEdgeEvaluatorF = GantryEuclideanDistanceEdgeEvaluator<float>;

diff --git a/descartes_samplers/include/descartes_samplers/evaluators/impl/compound_edge_evaluator.hpp b/descartes_samplers/include/descartes_samplers/evaluators/impl/compound_edge_evaluator.hpp
@@ -29,15 +29,12 @@ CompoundEdgeEvaluator<FloatType>::CompoundEdgeEvaluator(int dof)
 }
 
 template <typename FloatType>
-std::pair<bool, FloatType> CompoundEdgeEvaluator<FloatType>::considerEdge(const Rung_<FloatType>& from,
-                                                                          const FloatType* start,
-                                                                          const Rung_<FloatType>& to,
-                                                                          const FloatType* end)
+std::pair<bool, FloatType> CompoundEdgeEvaluator<FloatType>::considerEdge(const FloatType* start, const FloatType* end)
 {
   FloatType cost = 0.0;
   for (auto& evaluator : evaluators)
   {
-    auto results = evaluator->considerEdge(from, start, to, end);
+    auto results = evaluator->considerEdge(start, end);
     if (!results.first)
       return std::make_pair(false, cost);
 

diff --git a/descartes_samplers/include/descartes_samplers/evaluators/impl/distance_edge_evaluator.hpp b/descartes_samplers/include/descartes_samplers/evaluators/impl/distance_edge_evaluator.hpp
@@ -28,41 +28,35 @@ DESCARTES_IGNORE_WARNINGS_POP
 namespace descartes_light
 {
 template <typename FloatType>
-DistanceEdgeEvaluator<FloatType>::DistanceEdgeEvaluator(const std::vector<FloatType>& velocity_limits)
-  : EdgeEvaluator<FloatType>(velocity_limits.size()), velocity_limits_(velocity_limits)
+DistanceEdgeEvaluator<FloatType>::DistanceEdgeEvaluator(const std::vector<FloatType>& velocity_limits, FloatType dt)
+  : EdgeEvaluator<FloatType>(velocity_limits.size())
 {
-}
-
-template <typename FloatType>
-std::pair<bool, FloatType> DistanceEdgeEvaluator<FloatType>::considerEdge(const Rung_<FloatType>&,
-                                                                          const FloatType* start,
-                                                                          const Rung_<FloatType>& to,
-                                                                          const FloatType* end)
-{
-  // Compute thresholds
-  const auto dt = to.timing;
-
-  std::vector<FloatType> joint_distance_threshold(velocity_limits_.size());
-  std::transform(velocity_limits_.begin(),
-                 velocity_limits_.end(),
-                 joint_distance_threshold.begin(),
+  joint_distance_threshold_.resize(velocity_limits.size());
+  std::vector<FloatType> joint_distance_threshold(velocity_limits.size());
+  std::transform(velocity_limits.begin(),
+                 velocity_limits.end(),
+                 joint_distance_threshold_.begin(),
                  [dt](const FloatType& vel_limit) -> FloatType {
-                   if (dt.upper != static_cast<FloatType>(0.0))
+                   if (dt != static_cast<FloatType>(0.0))
                    {
                      const static FloatType safety_factor = static_cast<FloatType>(0.9);
-                     return dt.upper * vel_limit * safety_factor;
+                     return dt * vel_limit * safety_factor;
                    }
                    else
                    {
                      return std::numeric_limits<FloatType>::max();
                    }
                  });
+}
 
+template <typename FloatType>
+std::pair<bool, FloatType> DistanceEdgeEvaluator<FloatType>::considerEdge(const FloatType* start, const FloatType* end)
+{
   FloatType cost = static_cast<FloatType>(0.0);
-  for (std::size_t i = 0; i < joint_distance_threshold.size(); ++i)
+  for (std::size_t i = 0; i < joint_distance_threshold_.size(); ++i)
   {
     FloatType step = end[i] - start[i];
-    if (std::abs(step) > joint_distance_threshold[i])
+    if (std::abs(step) > joint_distance_threshold_[i])
       return std::make_pair(false, cost);
 
     cost += std::pow(step, FloatType(2));

diff --git a/...samplers/include/descartes_samplers/evaluators/impl/euclidean_distance_edge_evaluator.hpp b/...samplers/include/descartes_samplers/evaluators/impl/euclidean_distance_edge_evaluator.hpp
@@ -34,9 +34,7 @@ EuclideanDistanceEdgeEvaluator<FloatType>::EuclideanDistanceEdgeEvaluator(int do
 }
 
 template <typename FloatType>
-std::pair<bool, FloatType> EuclideanDistanceEdgeEvaluator<FloatType>::considerEdge(const Rung_<FloatType>&,
-                                                                                   const FloatType* start,
-                                                                                   const Rung_<FloatType>&,
+std::pair<bool, FloatType> EuclideanDistanceEdgeEvaluator<FloatType>::considerEdge(const FloatType* start,
                                                                                    const FloatType* end)
 {
   FloatType cost = 0.0;

diff --git a/...s/include/descartes_samplers/evaluators/impl/gantry_euclidean_distance_edge_evaluator.hpp b/...s/include/descartes_samplers/evaluators/impl/gantry_euclidean_distance_edge_evaluator.hpp
@@ -34,9 +34,7 @@ GantryEuclideanDistanceEdgeEvaluator<FloatType>::GantryEuclideanDistanceEdgeEval
 }
 
 template <typename FloatType>
-std::pair<bool, FloatType> GantryEuclideanDistanceEdgeEvaluator<FloatType>::considerEdge(const Rung_<FloatType>&,
-                                                                                         const FloatType* start,
-                                                                                         const Rung_<FloatType>&,
+std::pair<bool, FloatType> GantryEuclideanDistanceEdgeEvaluator<FloatType>::considerEdge(const FloatType* start,
                                                                                          const FloatType* end)
 {
   FloatType cost = 0.0;