Skip to content
Snippets Groups Projects
Commit 5c68e034 authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

[TransitionPlanner] Add a method to validate a configuration with selected edge.

parent 6eaa68a1
No related branches found
No related tags found
No related merge requests found
......@@ -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
......
......@@ -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
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment