From 00612069cbd17348792f801aacd4e2ec06f7158a Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Wed, 23 Aug 2017 14:49:06 +0200
Subject: [PATCH] Add Graph::initialize and fix const-correctness of
constructor of constraints
---
include/hpp/manipulation/graph/edge.hh | 52 ++++------------
.../hpp/manipulation/graph/graph-component.hh | 4 ++
include/hpp/manipulation/graph/graph.hh | 2 +
.../hpp/manipulation/graph/state-selector.hh | 2 +
include/hpp/manipulation/graph/state.hh | 10 ++-
include/hpp/manipulation/problem-solver.hh | 3 +
src/graph/edge.cc | 62 +++++++++----------
src/graph/graph.cc | 8 +++
src/graph/state.cc | 34 +++++-----
src/problem-solver.cc | 8 +++
10 files changed, 89 insertions(+), 96 deletions(-)
diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh
index 06f485aa..6d608645 100644
--- a/include/hpp/manipulation/graph/edge.hh
+++ b/include/hpp/manipulation/graph/edge.hh
@@ -29,32 +29,6 @@
namespace hpp {
namespace manipulation {
namespace graph {
- /// \cond
- /// Cache mechanism that enable const-correctness of member functions.
- template <typename C>
- class HPP_MANIPULATION_LOCAL Cache
- {
- public:
- void set (const C& c)
- {
- c_ = c;
- }
-
- operator bool() const
- {
- return (bool)c_;
- }
-
- const C& get () const
- {
- return c_;
- }
-
- private:
- C c_;
- };
- /// \endcond
-
/// \addtogroup constraint_graph
/// \{
@@ -141,13 +115,13 @@ namespace hpp {
/// Get steering method associated to the edge.
const core::SteeringMethodPtr_t& steeringMethod () const
{
- return steeringMethod_->get();
+ return steeringMethod_;
}
/// Get path validation associated to the edge.
const core::PathValidationPtr_t& pathValidation () const
{
- return pathValidation_->get();
+ return pathValidation_;
}
const RelativeMotion::matrix_type& relativeMotion () const
@@ -190,9 +164,11 @@ namespace hpp {
/// \return The initialized constraint.
ConstraintSetPtr_t pathConstraint() const;
- virtual ConstraintSetPtr_t buildConfigConstraint() const;
+ virtual ConstraintSetPtr_t buildConfigConstraint();
+
+ virtual ConstraintSetPtr_t buildPathConstraint();
- virtual ConstraintSetPtr_t buildPathConstraint() const;
+ virtual void initialize ();
/// Print the object in a stream.
virtual std::ostream& print (std::ostream& os) const;
@@ -200,16 +176,12 @@ namespace hpp {
bool isShort_;
private:
- typedef Cache < ConstraintSetPtr_t > Constraint_t;
- typedef Cache < core::SteeringMethodPtr_t > SteeringMethod_t;
- typedef Cache < core::PathValidationPtr_t > PathValidation_t;
-
/// See pathConstraint member function.
- Constraint_t* pathConstraints_;
+ ConstraintSetPtr_t pathConstraints_;
/// Constraint ensuring that a q_proj will be in to_ and in the
/// same leaf of to_ as the configuration used for initialization.
- Constraint_t* configConstraints_;
+ ConstraintSetPtr_t configConstraints_;
/// The two ends of the transition.
StateWkPtr_t from_, to_;
@@ -218,11 +190,11 @@ namespace hpp {
StateWkPtr_t state_;
/// Steering method used to create paths associated to the edge
- SteeringMethod_t* steeringMethod_;
+ core::SteeringMethodPtr_t steeringMethod_;
/// Path validation associated to the edge
mutable RelativeMotion::matrix_type relMotion_;
- PathValidation_t* pathValidation_;
+ core::PathValidationPtr_t pathValidation_;
/// Weak pointer to itself.
EdgeWkPtr_t wkPtr_;
@@ -334,7 +306,7 @@ namespace hpp {
virtual bool applyConstraints (core::NodePtr_t n_offset, ConfigurationOut_t q) const;
- virtual ConstraintSetPtr_t buildConfigConstraint() const;
+ virtual ConstraintSetPtr_t buildConfigConstraint();
void buildHistogram ();
@@ -383,6 +355,8 @@ namespace hpp {
/// Populate DrawingAttributes tooltip
virtual void populateTooltip (dot::Tooltip& tp) const;
+ virtual void initialize ();
+
private:
bool applyConstraintsWithOffset (ConfigurationIn_t qoffset,
ConfigurationIn_t qlevelset, ConfigurationOut_t q) const;
diff --git a/include/hpp/manipulation/graph/graph-component.hh b/include/hpp/manipulation/graph/graph-component.hh
index 8c2e2ab9..785f8c63 100644
--- a/include/hpp/manipulation/graph/graph-component.hh
+++ b/include/hpp/manipulation/graph/graph-component.hh
@@ -121,6 +121,8 @@ namespace hpp {
/// Populate DrawingAttributes tooltip
virtual void populateTooltip (dot::Tooltip& tp) const;
+ virtual void initialize () = 0;
+
private:
/// Name of the component.
std::string name_;
@@ -128,6 +130,8 @@ namespace hpp {
GraphComponentWkPtr_t wkPtr_;
/// ID of the component (index in components vector).
std::size_t id_;
+
+ friend class Graph;
};
std::ostream& operator<< (std::ostream& os, const GraphComponent& graphComp);
diff --git a/include/hpp/manipulation/graph/graph.hh b/include/hpp/manipulation/graph/graph.hh
index b6827f90..4fb0d973 100644
--- a/include/hpp/manipulation/graph/graph.hh
+++ b/include/hpp/manipulation/graph/graph.hh
@@ -214,6 +214,8 @@ namespace hpp {
/// Print the component in DOT language.
virtual std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;
+ virtual void initialize ();
+
protected:
/// Initialization of the object.
void init (const GraphWkPtr_t& weak, DevicePtr_t robot);
diff --git a/include/hpp/manipulation/graph/state-selector.hh b/include/hpp/manipulation/graph/state-selector.hh
index fa560dc9..09f72b80 100644
--- a/include/hpp/manipulation/graph/state-selector.hh
+++ b/include/hpp/manipulation/graph/state-selector.hh
@@ -84,6 +84,8 @@ namespace hpp {
WeighedStates_t orderedStates_;
States_t waypoints_;
+ virtual void initialize () {};
+
private:
/// Weak pointer to itself.
StateSelectorPtr_t wkPtr_;
diff --git a/include/hpp/manipulation/graph/state.hh b/include/hpp/manipulation/graph/state.hh
index 5bb71b0f..5136965f 100644
--- a/include/hpp/manipulation/graph/state.hh
+++ b/include/hpp/manipulation/graph/state.hh
@@ -104,7 +104,10 @@ namespace hpp {
Weight_t getWeight (const EdgePtr_t&edge);
/// Constraint to project onto this state.
- ConstraintSetPtr_t configConstraint() const;
+ ConstraintSetPtr_t configConstraint() const
+ {
+ return configConstraints_;
+ }
/// Add core::NumericalConstraint to the component.
virtual void addNumericalConstraintForPath (const NumericalConstraintPtr_t& nm,
@@ -156,6 +159,8 @@ namespace hpp {
virtual void populateTooltip (dot::Tooltip& tp) const;
+ virtual void initialize ();
+
private:
/// List of possible motions from this state (i.e. the outgoing
/// vertices).
@@ -163,8 +168,7 @@ namespace hpp {
std::vector <EdgePtr_t> hiddenNeighbors_;
/// Set of constraints to be statisfied.
- typedef Cache < ConstraintSetPtr_t > Constraint_t;
- Constraint_t* configConstraints_;
+ ConstraintSetPtr_t configConstraints_;
/// Stores the numerical constraints for path.
NumericalConstraints_t numericalConstraintsForPath_;
diff --git a/include/hpp/manipulation/problem-solver.hh b/include/hpp/manipulation/problem-solver.hh
index e763ac2f..4af6c282 100644
--- a/include/hpp/manipulation/problem-solver.hh
+++ b/include/hpp/manipulation/problem-solver.hh
@@ -71,6 +71,9 @@ namespace hpp {
/// Get the constraint graph
graph::GraphPtr_t constraintGraph () const;
+
+ /// Should be called before any call on the graph is made.
+ void initConstraintGraph ();
/// \}
/// Create placement constraint
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index 711f686a..13a8cc85 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -38,19 +38,14 @@ namespace hpp {
namespace graph {
Edge::Edge (const std::string& name) :
GraphComponent (name), isShort_ (false),
- pathConstraints_ (new Constraint_t()),
- configConstraints_ (new Constraint_t()),
- steeringMethod_ (new SteeringMethod_t()),
- pathValidation_ (new PathValidation_t())
+ pathConstraints_ (),
+ configConstraints_ (),
+ steeringMethod_ (),
+ pathValidation_ ()
{}
Edge::~Edge ()
- {
- if (pathConstraints_ ) delete pathConstraints_;
- if (configConstraints_) delete configConstraints_;
- if (steeringMethod_ ) delete steeringMethod_;
- if (pathValidation_ ) delete pathValidation_;
- }
+ {}
StatePtr_t Edge::to () const
{
@@ -159,6 +154,12 @@ namespace hpp {
state_ = to;
}
+ void Edge::initialize ()
+ {
+ configConstraints_ = buildConfigConstraint ();
+ pathConstraints_ = buildPathConstraint ();
+ }
+
std::ostream& Edge::print (std::ostream& os) const
{
os << "| | |-- ";
@@ -181,13 +182,10 @@ namespace hpp {
ConstraintSetPtr_t Edge::configConstraint() const
{
- if (!*configConstraints_) {
- configConstraints_->set (buildConfigConstraint ());
- }
- return configConstraints_->get ();
+ return configConstraints_;
}
- ConstraintSetPtr_t Edge::buildConfigConstraint() const
+ ConstraintSetPtr_t Edge::buildConfigConstraint()
{
std::string n = "(" + name () + ")";
GraphPtr_t g = graph_.lock ();
@@ -216,14 +214,10 @@ namespace hpp {
ConstraintSetPtr_t Edge::pathConstraint() const
{
- if (!*pathConstraints_) {
- ConstraintSetPtr_t pathConstraints (buildPathConstraint ());
- pathConstraints_->set (pathConstraints);
- }
- return pathConstraints_->get ();
+ return pathConstraints_;
}
- ConstraintSetPtr_t Edge::buildPathConstraint() const
+ ConstraintSetPtr_t Edge::buildPathConstraint()
{
std::string n = "(" + name () + ")";
GraphPtr_t g = graph_.lock ();
@@ -244,16 +238,15 @@ namespace hpp {
// Build steering method
const ProblemPtr_t& problem (g->problem());
- steeringMethod_->set(problem->steeringMethod()
- ->innerSteeringMethod()->copy());
- steeringMethod_->get()->constraints (constraint);
+ steeringMethod_ = problem->steeringMethod()->innerSteeringMethod()->copy();
+ steeringMethod_->constraints (constraint);
// Build path validation and relative motion matrix
// TODO this path validation will not contain obstacles added after
// its creation.
- pathValidation_->set(problem->pathValidationFactory ());
+ pathValidation_ = problem->pathValidationFactory ();
relMotion_ = RelativeMotion::matrix (g->robot());
RelativeMotion::fromConstraint (relMotion_, g->robot(), constraint);
- pathValidation_->get()->filterCollisionPairs (relMotion_);
+ pathValidation_->filterCollisionPairs (relMotion_);
return constraint;
}
@@ -273,12 +266,7 @@ namespace hpp {
const
{
using pinocchio::displayConfig;
- core::SteeringMethodPtr_t sm (steeringMethod_->get());
- if (!sm) {
- buildPathConstraint ();
- }
- sm = (steeringMethod_->get());
- if (!sm) {
+ if (!steeringMethod_) {
std::ostringstream oss;
oss << "No steering method set in edge " << name () << ".";
throw std::runtime_error (oss.str ().c_str ());
@@ -287,7 +275,7 @@ namespace hpp {
constraints->configProjector ()->rightHandSideFromConfig(q1);
if (constraints->isSatisfied (q1)) {
if (constraints->isSatisfied (q2)) {
- path = (*sm) (q1, q2);
+ path = (*steeringMethod_) (q1, q2);
return (bool)path;
} else {
hppDout(info, "q2 = " << displayConfig (q2)
@@ -686,7 +674,13 @@ namespace hpp {
g->insertHistogram (hist_);
}
- ConstraintSetPtr_t LevelSetEdge::buildConfigConstraint() const
+ void LevelSetEdge::initialize ()
+ {
+ Edge::initialize();
+ buildHistogram ();
+ }
+
+ ConstraintSetPtr_t LevelSetEdge::buildConfigConstraint()
{
std::string n = "(" + name () + ")";
GraphPtr_t g = graph_.lock ();
diff --git a/src/graph/graph.cc b/src/graph/graph.cc
index 68e4dc7b..b85a09d7 100644
--- a/src/graph/graph.cc
+++ b/src/graph/graph.cc
@@ -47,6 +47,14 @@ namespace hpp {
);
}
+ void Graph::initialize ()
+ {
+ hists_.clear ();
+ assert(components_.size() >= 1 && components_[0].lock() == wkPtr_.lock());
+ for (std::size_t i = 1; i < components_.size(); ++i)
+ components_[i].lock()->initialize();
+ }
+
StateSelectorPtr_t Graph::createNodeSelector (const std::string& name)
{
stateSelector_ = StateSelector::create (name);
diff --git a/src/graph/state.cc b/src/graph/state.cc
index 1819c2f6..4a082e91 100644
--- a/src/graph/state.cc
+++ b/src/graph/state.cc
@@ -27,14 +27,12 @@ namespace hpp {
namespace manipulation {
namespace graph {
State::State (const std::string& name) :
- GraphComponent (name), configConstraints_ (new Constraint_t()),
+ GraphComponent (name), configConstraints_ (),
isWaypoint_ (false)
{}
State::~State ()
- {
- if (configConstraints_) delete configConstraints_;
- }
+ {}
StatePtr_t State::create (const std::string& name)
{
@@ -107,23 +105,19 @@ namespace hpp {
return os;
}
- ConstraintSetPtr_t State::configConstraint() const
+ void State::initialize()
{
- if (!*configConstraints_) {
- std::string n = "(" + name () + ")";
- GraphPtr_t g = graph_.lock ();
- ConstraintSetPtr_t constraint = ConstraintSet::create (g->robot (), "Set " + n);
-
- ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "proj " + n, g->errorThreshold(), g->maxIterations());
- g->insertNumericalConstraints (proj);
- insertNumericalConstraints (proj);
- constraint->addConstraint (proj);
-
- g->insertLockedJoints (proj);
- insertLockedJoints (proj);
- configConstraints_->set (constraint);
- }
- return configConstraints_->get ();
+ std::string n = "(" + name () + ")";
+ GraphPtr_t g = graph_.lock ();
+ configConstraints_ = ConstraintSet::create (g->robot (), "Set " + n);
+
+ ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "proj " + n, g->errorThreshold(), g->maxIterations());
+ g->insertNumericalConstraints (proj);
+ insertNumericalConstraints (proj);
+ configConstraints_->addConstraint (proj);
+
+ g->insertLockedJoints (proj);
+ insertLockedJoints (proj);
}
void State::updateWeight (const EdgePtr_t& e, const Weight_t& w)
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index c921308e..9d69e3f2 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -184,6 +184,14 @@ namespace hpp {
return constraintGraph_;
}
+ void ProblemSolver::initConstraintGraph ()
+ {
+ if (!constraintGraph_)
+ throw std::runtime_error ("The graph is not defined.");
+ initSteeringMethod();
+ constraintGraph_->initialize();
+ }
+
void ProblemSolver::createPlacementConstraint
(const std::string& name, const StringList_t& surface1,
const StringList_t& surface2, const value_type& margin)
--
GitLab