diff --git a/descartes_light/include/descartes_light/descartes_light.h b/descartes_light/include/descartes_light/descartes_light.h index aa40c584..9af4e248 100644 --- 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::Ptr>& trajectory, - const std::vector>& times, const std::vector::Ptr>& edge_eval, int num_threads = getMaxThreads()); bool build(const std::vector::Ptr>& trajectory, - const std::vector>& times, typename EdgeEvaluator::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 index f0391f42..6338c348 100644 --- 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::Solver(const std::size_t dof) : graph_{ dof } template bool Solver::build(const std::vector::Ptr>& trajectory, - const std::vector>& times, const std::vector::Ptr>& edge_eval, int num_threads) { @@ -87,7 +86,6 @@ bool Solver::build(const std::vector(i)]->sample(vertex_data)) { graph_.getRung(static_cast(i)).data = std::move(vertex_data); - graph_.getRung(static_cast(i)).timing = times[static_cast(i)]; } else { @@ -147,12 +145,11 @@ bool Solver::build(const std::vector bool Solver::build(const std::vector::Ptr>& trajectory, - const std::vector>& times, typename EdgeEvaluator::Ptr edge_eval, int num_threads) { std::vector::Ptr> evaluators(trajectory.size() - 1, edge_eval); - return build(trajectory, times, evaluators, num_threads); + return build(trajectory, evaluators, num_threads); } template diff --git a/descartes_light/include/descartes_light/impl/ladder_graph.hpp b/descartes_light/include/descartes_light/impl/ladder_graph.hpp index b3df8cde..c0251004 100644 --- 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::assignEdges(const std::size_t rung, template void LadderGraph::assignRung(const std::size_t index, descartes_core::TrajectoryID id, - descartes_core::TimingConstraint time, const std::vector>& 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 index 1bb070d3..30197b10 100644 --- 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_& from, - const Rung_& to, - std::vector::EdgeList>& edge_lists) + bool evaluate(const Rung_& from, + const Rung_& to, + std::vector::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 results = considerEdge(from, start_vertex, to, end_vertex); + std::pair 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 if edge is valid, false otherwise. Cost to move from the first vertex to * the next */ - virtual std::pair considerEdge(const Rung_& from, - const FloatType* start, - const Rung_& to, - const FloatType* end) = 0; + virtual std::pair 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 index 47a67844..55437432 100644 --- 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 -struct TimingConstraint -{ - TimingConstraint() : upper(0.0) {} - TimingConstraint(FloatType time) : upper(time) {} - FloatType upper; -}; - -using TimingConstraintF = TimingConstraint; -using TimingConstraintD = TimingConstraint; - } // namespace descartes_core namespace descartes_light @@ -61,9 +49,8 @@ struct Rung_ using Edge = Edge_; using EdgeList = std::vector; - descartes_core::TrajectoryID id; // corresponds to user's input ID - descartes_core::TimingConstraint timing; // user input timing - std::vector data; // joint values stored in one contiguous array + descartes_core::TrajectoryID id; // corresponds to user's input ID + std::vector data; // joint values stored in one contiguous array std::vector edges; }; @@ -152,7 +139,6 @@ class LadderGraph */ void assignRung(const std::size_t index, descartes_core::TrajectoryID id, - descartes_core::TimingConstraint time, const std::vector>& 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 index c1cb5c8b..9438ff76 100644 --- 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 public: CompoundEdgeEvaluator(int dof); - std::pair considerEdge(const Rung_& from, - const FloatType* start, - const Rung_& to, - const FloatType* end) override; + std::pair considerEdge(const FloatType* start, const FloatType* end) override; std::vector::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 index c65d9482..acd5272d 100644 --- 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 class DistanceEdgeEvaluator : public EdgeEvaluator { public: - DistanceEdgeEvaluator(const std::vector& velocity_limits); + DistanceEdgeEvaluator(const std::vector& velocity_limits, FloatType dt); - std::pair considerEdge(const Rung_& from, - const FloatType* start, - const Rung_& to, - const FloatType* end) override; + std::pair considerEdge(const FloatType* start, const FloatType* end) override; protected: - std::vector velocity_limits_; + std::vector joint_distance_threshold_; }; using DistanceEdgeEvaluatorF = DistanceEdgeEvaluator; 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 index 61a02c34..7f22c1a9 100644 --- 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 public: EuclideanDistanceEdgeEvaluator(int dof); - std::pair considerEdge(const Rung_& from, - const FloatType* start, - const Rung_& to, - const FloatType* end) override; + std::pair considerEdge(const FloatType* start, const FloatType* end) override; }; using EuclideanDistanceEdgeEvaluatorF = EuclideanDistanceEdgeEvaluator; diff --git a/descartes_samplers/include/descartes_samplers/evaluators/gantry_euclidean_distance_edge_evaluator.h b/descartes_samplers/include/descartes_samplers/evaluators/gantry_euclidean_distance_edge_evaluator.h index bad64135..e854d159 100644 --- a/descartes_samplers/include/descartes_samplers/evaluators/gantry_euclidean_distance_edge_evaluator.h +++ b/descartes_samplers/include/descartes_samplers/evaluators/gantry_euclidean_distance_edge_evaluator.h @@ -29,10 +29,7 @@ class GantryEuclideanDistanceEdgeEvaluator : public EdgeEvaluator public: GantryEuclideanDistanceEdgeEvaluator(int dof); - std::pair considerEdge(const Rung_& from, - const FloatType* start, - const Rung_& to, - const FloatType* end) override; + std::pair considerEdge(const FloatType* start, const FloatType* end) override; }; using GantryEuclideanDistanceEdgeEvaluatorF = GantryEuclideanDistanceEdgeEvaluator; 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 index 25fcbfac..7e638831 100644 --- 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::CompoundEdgeEvaluator(int dof) } template -std::pair CompoundEdgeEvaluator::considerEdge(const Rung_& from, - const FloatType* start, - const Rung_& to, - const FloatType* end) +std::pair CompoundEdgeEvaluator::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 index 6c11818a..1a067474 100644 --- 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 -DistanceEdgeEvaluator::DistanceEdgeEvaluator(const std::vector& velocity_limits) - : EdgeEvaluator(velocity_limits.size()), velocity_limits_(velocity_limits) +DistanceEdgeEvaluator::DistanceEdgeEvaluator(const std::vector& velocity_limits, FloatType dt) + : EdgeEvaluator(velocity_limits.size()) { -} - -template -std::pair DistanceEdgeEvaluator::considerEdge(const Rung_&, - const FloatType* start, - const Rung_& to, - const FloatType* end) -{ - // Compute thresholds - const auto dt = to.timing; - - std::vector 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 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(0.0)) + if (dt != static_cast(0.0)) { const static FloatType safety_factor = static_cast(0.9); - return dt.upper * vel_limit * safety_factor; + return dt * vel_limit * safety_factor; } else { return std::numeric_limits::max(); } }); +} +template +std::pair DistanceEdgeEvaluator::considerEdge(const FloatType* start, const FloatType* end) +{ FloatType cost = static_cast(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/descartes_samplers/include/descartes_samplers/evaluators/impl/euclidean_distance_edge_evaluator.hpp b/descartes_samplers/include/descartes_samplers/evaluators/impl/euclidean_distance_edge_evaluator.hpp index 347b9e5a..55fd7c59 100644 --- a/descartes_samplers/include/descartes_samplers/evaluators/impl/euclidean_distance_edge_evaluator.hpp +++ b/descartes_samplers/include/descartes_samplers/evaluators/impl/euclidean_distance_edge_evaluator.hpp @@ -34,9 +34,7 @@ EuclideanDistanceEdgeEvaluator::EuclideanDistanceEdgeEvaluator(int do } template -std::pair EuclideanDistanceEdgeEvaluator::considerEdge(const Rung_&, - const FloatType* start, - const Rung_&, +std::pair EuclideanDistanceEdgeEvaluator::considerEdge(const FloatType* start, const FloatType* end) { FloatType cost = 0.0; diff --git a/descartes_samplers/include/descartes_samplers/evaluators/impl/gantry_euclidean_distance_edge_evaluator.hpp b/descartes_samplers/include/descartes_samplers/evaluators/impl/gantry_euclidean_distance_edge_evaluator.hpp index 1b335835..17dcfac5 100644 --- a/descartes_samplers/include/descartes_samplers/evaluators/impl/gantry_euclidean_distance_edge_evaluator.hpp +++ b/descartes_samplers/include/descartes_samplers/evaluators/impl/gantry_euclidean_distance_edge_evaluator.hpp @@ -34,9 +34,7 @@ GantryEuclideanDistanceEdgeEvaluator::GantryEuclideanDistanceEdgeEval } template -std::pair GantryEuclideanDistanceEdgeEvaluator::considerEdge(const Rung_&, - const FloatType* start, - const Rung_&, +std::pair GantryEuclideanDistanceEdgeEvaluator::considerEdge(const FloatType* start, const FloatType* end) { FloatType cost = 0.0;