From fec57f7bd5d50d29a1dcf4b78948f55d35c18635 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Tue, 29 Jul 2014 10:17:37 +0200
Subject: [PATCH] Update Graph and Edge interface
---
include/hpp/manipulation/graph/edge.hh | 12 +++++
include/hpp/manipulation/graph/graph.hh | 17 ++++++++
src/graph/graph.cc | 58 +++++++++++++++++++++++++
3 files changed, 87 insertions(+)
diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh
index 54955b5a..35e48215 100644
--- a/include/hpp/manipulation/graph/edge.hh
+++ b/include/hpp/manipulation/graph/edge.hh
@@ -49,6 +49,18 @@ namespace hpp {
/// Print the object in a stream.
std::ostream& print (std::ostream& os) const;
+ /// Get the destination
+ NodePtr_t to () const
+ {
+ return to_.lock();
+ }
+
+ /// Get the origin
+ NodePtr_t from () const
+ {
+ return from_.lock();
+ }
+
protected:
/// Initialization of the object.
void init (const EdgeWkPtr_t& weak, const NodeWkPtr_t& from,
diff --git a/include/hpp/manipulation/graph/graph.hh b/include/hpp/manipulation/graph/graph.hh
index 304d8a53..80dede06 100644
--- a/include/hpp/manipulation/graph/graph.hh
+++ b/include/hpp/manipulation/graph/graph.hh
@@ -161,6 +161,23 @@ namespace hpp {
/// Select randomly outgoing edges of the given nodes.
virtual Edges_t chooseEdge(const Nodes_t& node);
+ /// Constraint to project onto the Nodes_t.
+ /// \param the Nodes_t on which to project.
+ /// \return The initialized projector.
+ virtual ConstraintPtr_t configConstraint (const Nodes_t& nodes);
+
+ /// Constraint to project onto the same leaf as config.
+ /// \param edges a list of edges defining the foliation.
+ /// \param config Configuration that will initialize the projector.
+ /// \return The initialized projector.
+ virtual ConstraintPtr_t configConstraint (const Edges_t& edges, ConfigurationIn_t config);
+
+ /// Constraint to project a path.
+ /// \param edges a list of edges defining the foliation.
+ /// \param config Configuration that will initialize the constraint.
+ /// \return The initialized constraint.
+ virtual ConstraintPtr_t pathConstraint (const Edges_t& edges, ConfigurationIn_t config);
+
/// Return the NodeSelector with the given name if any,
/// NULL pointer if not found.
NodeSelectorPtr_t getNodeSelectorByName (const std::string& name);
diff --git a/src/graph/graph.cc b/src/graph/graph.cc
index 17b2b28d..a15450ac 100644
--- a/src/graph/graph.cc
+++ b/src/graph/graph.cc
@@ -17,6 +17,7 @@
#include <hpp/util/assertion.hh>
#include "hpp/manipulation/graph/node-selector.hh"
+#include "hpp/manipulation/graph/node.hh"
#include "hpp/manipulation/graph/graph.hh"
namespace hpp {
@@ -75,6 +76,63 @@ namespace hpp {
return NodeSelectorPtr_t();
}
+ ConstraintPtr_t Graph::configConstraint (const Nodes_t& nodes)
+ {
+ ConstraintSetPtr_t constraint = ConstraintSet::create (robot (), name ());
+ insertListIn <LockedDofs_t> (lockedDofConstraints (), constraint);
+ for (Nodes_t::const_iterator it = nodes.begin();
+ it != nodes.end(); it++)
+ insertListIn <LockedDofs_t> ((*it)->lockedDofConstraints (), constraint);
+
+ ConfigProjectorPtr_t proj = ConfigProjector::create(robot(), name (), errorThreshold(), maxIterations());
+ for (Nodes_t::const_iterator it = nodes.begin();
+ it != nodes.end(); it++)
+ insertListIn <DifferentiableFunctions_t> ((*it)->numericalConstraints (), proj);
+
+ constraint->addConstraint (HPP_DYNAMIC_PTR_CAST(Constraint, proj));
+ return constraint;
+ }
+
+ ConstraintPtr_t Graph::configConstraint (const Edges_t& edges, ConfigurationIn_t config)
+ {
+ ConstraintSetPtr_t constraint = ConstraintSet::create (robot (), name ());
+ insertListIn <LockedDofs_t> (lockedDofConstraints (), constraint);
+ for (Edges_t::const_iterator it = edges.begin();
+ it != edges.end(); it++) {
+ insertListIn <LockedDofs_t> ((*it)->lockedDofConstraints (), constraint);
+ insertListIn <LockedDofs_t> ((*it)->to()->lockedDofConstraints(), constraint);
+ }
+
+ ConfigProjectorPtr_t proj = ConfigProjector::create(robot(), name (), errorThreshold(), maxIterations());
+ for (Edges_t::const_iterator it = edges.begin();
+ it != edges.end(); it++) {
+ insertListIn <DifferentiableFunctions_t> ((*it)->numericalConstraints (), proj);
+ insertListIn <DifferentiableFunctions_t> ((*it)->to()->numericalConstraints (), proj);
+ }
+
+ constraint->addConstraint (HPP_DYNAMIC_PTR_CAST(Constraint, proj));
+ constraint->offsetFromConfig (config);
+ return constraint;
+ }
+
+ ConstraintPtr_t Graph::pathConstraint (const Edges_t& edges, ConfigurationIn_t config)
+ {
+ ConstraintSetPtr_t constraint = ConstraintSet::create (robot (), name ());
+ insertListIn <LockedDofs_t> (lockedDofConstraints (), constraint);
+ for (Edges_t::const_iterator it = edges.begin();
+ it != edges.end(); it++)
+ insertListIn <LockedDofs_t> ((*it)->lockedDofConstraints (), constraint);
+
+ ConfigProjectorPtr_t proj = ConfigProjector::create(robot(), name (), errorThreshold(), maxIterations());
+ for (Edges_t::const_iterator it = edges.begin();
+ it != edges.end(); it++)
+ insertListIn <DifferentiableFunctions_t> ((*it)->numericalConstraints (), proj);
+
+ constraint->addConstraint (HPP_DYNAMIC_PTR_CAST(Constraint, proj));
+ constraint->offsetFromConfig (config);
+ return constraint;
+ }
+
std::ostream& Graph::print (std::ostream& os) const
{
GraphComponent::print (os) << std::endl;
--
GitLab