From 665655560872b213131a1cea012b5c7fc3649cc6 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Thu, 24 Jul 2014 20:57:18 +0200
Subject: [PATCH] Add member function pathConstraint and configConstraint
---
include/hpp/manipulation/graph/node.hh | 3 +++
src/graph/edge.cc | 37 ++++++++++++++++++++++++++
src/graph/node.cc | 15 +++++++++++
3 files changed, 55 insertions(+)
diff --git a/include/hpp/manipulation/graph/node.hh b/include/hpp/manipulation/graph/node.hh
index 1ff71f24..7e21f243 100644
--- a/include/hpp/manipulation/graph/node.hh
+++ b/include/hpp/manipulation/graph/node.hh
@@ -71,6 +71,9 @@ namespace hpp {
/// Print the object in a stream.
std::ostream& print (std::ostream& os) const;
+ /// Constraint to project onto this node.
+ ConstraintPtr_t configConstraint();
+
protected:
/// Initialize the object.
void init (const NodeWkPtr_t& self);
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index 98ffd756..36384640 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -43,6 +43,43 @@ namespace hpp {
return os;
}
+ ConstraintPtr_t Edge::configConstraint(ConfigurationIn_t config)
+ {
+ if (!configConstraints_) {
+ NodePtr_t to = to_.lock();
+ if (!to)
+ HPP_THROW_EXCEPTION (Bad_function_call, "Edge does not have a destination.");
+ ConstraintSetPtr_t configConst = buildConstraintSet (graph_, name () + "-cfg");
+ insertListIn <LockedDofs_t> (lockedDofConstraints_, configConst);
+ insertListIn <LockedDofs_t> (to->lockedDofConstraints(), configConst);
+ DifferentiableFunctions_t toNumConst = to->numericalConstraints();
+ if (numericalConstraints_.size() > 0 || toNumConst.size() > 0) {
+ ConfigProjectorPtr_t cp = buildConfigProjector (graph_, name () + "cfgproj");
+ insertListIn <DifferentiableFunctions_t> (numericalConstraints_, cp);
+ insertListIn <DifferentiableFunctions_t> (toNumConst, cp);
+ configConst->addConstraint (HPP_DYNAMIC_PTR_CAST(Constraint, cp));
+ }
+ configConstraints_ = configConst;
+ }
+ configConstraints_->setLeafParameterFromConfig (config);
+ return configConstraints_;
+ }
+
+ ConstraintPtr_t Edge::pathConstraint(ConfigurationIn_t config)
+ {
+ if (!pathConstraints_) {
+ ConstraintSetPtr_t pathConst = buildConstraintSet (graph_, name () + "-pathconstraint");
+ insertListIn <LockedDofs_t> (lockedDofConstraints_, pathConst);
+ if (numericalConstraints_.size () > 0) {
+ ConfigProjectorPtr_t cp = buildConfigProjector (graph_, name () + "pathproj");
+ insertListIn <DifferentiableFunctions_t> (numericalConstraints_, cp);
+ pathConst->addConstraint (HPP_DYNAMIC_PTR_CAST(Constraint, cp));
+ }
+ pathConstraints_ = pathConst;
+ }
+ pathConstraints_->setLeafParameterFromConfig (config);
+ return pathConstraints_;
+ }
} // namespace graph
} // namespace manipulation
} // namespace hpp
diff --git a/src/graph/node.cc b/src/graph/node.cc
index 732585ec..70a205d5 100644
--- a/src/graph/node.cc
+++ b/src/graph/node.cc
@@ -58,6 +58,21 @@ namespace hpp {
os << *(*it);
return os;
}
+
+ ConstraintPtr_t Node::configConstraint()
+ {
+ if (!configConstraints_) {
+ ConstraintSetPtr_t configConst = buildConstraintSet (graph_, name () + "-cfgconstraint");
+ insertListIn <LockedDofs_t> (lockedDofConstraints_, configConst);
+ if (numericalConstraints_.size () > 0) {
+ ConfigProjectorPtr_t cp = buildConfigProjector (graph_, name () + "cfgproj");
+ insertListIn <DifferentiableFunctions_t> (numericalConstraints_, cp);
+ configConst->addConstraint (HPP_DYNAMIC_PTR_CAST(Constraint, cp));
+ }
+ configConstraints_ = configConst;
+ }
+ return configConstraints_;
+ }
} // namespace graph
} // namespace manipulation
} // namespace hpp
--
GitLab