From 5c68e0347794174c2986b361c691220db8f27888 Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Wed, 26 Jun 2024 08:26:52 +0000
Subject: [PATCH] [TransitionPlanner] Add a method to validate a configuration
with selected edge.
---
.../path-planner/transition-planner.hh | 8 +++++
src/path-planner/transition-planner.cc | 33 +++++++++++++------
2 files changed, 31 insertions(+), 10 deletions(-)
diff --git a/include/hpp/manipulation/path-planner/transition-planner.hh b/include/hpp/manipulation/path-planner/transition-planner.hh
index c7e558f..b5b3dc1 100644
--- a/include/hpp/manipulation/path-planner/transition-planner.hh
+++ b/include/hpp/manipulation/path-planner/transition-planner.hh
@@ -31,6 +31,7 @@
#include <hpp/core/path-planner.hh>
#include <hpp/manipulation/fwd.hh>
+#include <hpp/manipulation/graph/fwd.hh>
namespace hpp {
namespace manipulation {
@@ -110,6 +111,11 @@ class HPP_MANIPULATION_DLLAPI TransitionPlanner : public core::PathPlanner {
/// continuity.
PathPtr_t directPath(ConfigurationIn_t q1, ConfigurationIn_t q2,
bool validate, bool& success, std::string& status);
+ /// Validate a configuration with the path validation of an edge.
+ /// \param q configuration to validate,
+ /// \param id index of the edge in the constraint graph.
+ bool validateConfiguration(ConfigurationIn_t q, std::size_t id,
+ core::ValidationReportPtr_t& report) const;
/// Optimize path using the selected path optimizers
/// \param path input path
/// \return optimized path
@@ -156,6 +162,8 @@ class HPP_MANIPULATION_DLLAPI TransitionPlanner : public core::PathPlanner {
void init(TransitionPlannerWkPtr_t weak);
private:
+ /// Get pointer to edge from an id
+ graph::EdgePtr_t getEdgeOrThrow(std::size_t id) const;
/// Pointer to the problem of the inner planner
core::ProblemPtr_t innerProblem_;
/// Pointer to the inner path planner
diff --git a/src/path-planner/transition-planner.cc b/src/path-planner/transition-planner.cc
index dca4dd7..b3bad15 100644
--- a/src/path-planner/transition-planner.cc
+++ b/src/path-planner/transition-planner.cc
@@ -145,6 +145,13 @@ core::PathPtr_t TransitionPlanner::directPath(ConfigurationIn_t q1,
return validPart;
}
+bool TransitionPlanner::validateConfiguration(ConfigurationIn_t q, std::size_t id,
+ core::ValidationReportPtr_t& report) const
+{
+ graph::EdgePtr_t edge(getEdgeOrThrow(id));
+ return edge->pathValidation()->validate(q, report);
+}
+
core::PathVectorPtr_t TransitionPlanner::optimizePath(const PathPtr_t& path) {
PathVectorPtr_t pv(HPP_DYNAMIC_PTR_CAST(PathVector, path));
if (!pv) {
@@ -164,16 +171,7 @@ core::PathVectorPtr_t TransitionPlanner::timeParameterization(
}
void TransitionPlanner::setEdge(std::size_t id) {
- ProblemConstPtr_t p(HPP_DYNAMIC_PTR_CAST(const Problem, problem()));
- assert(p);
- graph::GraphComponentPtr_t comp(p->constraintGraph()->get(id).lock());
- graph::EdgePtr_t edge(HPP_DYNAMIC_PTR_CAST(graph::Edge, comp));
- if (!edge) {
- std::ostringstream os;
- os << "hpp::manipulation::pathPlanner::TransitionPlanner::setEdge: index "
- << id << " does not correspond to any edge of the constraint graph.";
- throw std::logic_error(os.str().c_str());
- }
+ graph::EdgePtr_t edge(getEdgeOrThrow(id));
innerProblem_->constraints(edge->pathConstraint());
innerProblem_->pathValidation(edge->pathValidation());
innerProblem_->steeringMethod(edge->steeringMethod());
@@ -243,6 +241,21 @@ void TransitionPlanner::init(TransitionPlannerWkPtr_t weak) {
weakPtr_ = weak;
}
+graph::EdgePtr_t TransitionPlanner::getEdgeOrThrow(std::size_t id) const
+{
+ ProblemConstPtr_t p(HPP_DYNAMIC_PTR_CAST(const Problem, problem()));
+ assert(p);
+ graph::GraphComponentPtr_t comp(p->constraintGraph()->get(id).lock());
+ graph::EdgePtr_t edge(HPP_DYNAMIC_PTR_CAST(graph::Edge, comp));
+ if (!edge) {
+ std::ostringstream os;
+ os << "hpp::manipulation::pathPlanner::TransitionPlanner::setEdge: index "
+ << id << " does not correspond to any edge of the constraint graph.";
+ throw std::logic_error(os.str().c_str());
+ }
+ return edge;
+}
+
} // namespace pathPlanner
} // namespace manipulation
} // namespace hpp
--
GitLab