From ad65e83b5b3423b978325d22406f650dbd7ad9bd Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Fri, 24 Jan 2020 23:49:02 +0100
Subject: [PATCH] Add cost in GraphComponent.
---
.../hpp/manipulation/graph/graph-component.hh | 8 ++++++++
src/graph/graph-component.cc | 16 ++++++++++++++++
2 files changed, 24 insertions(+)
diff --git a/include/hpp/manipulation/graph/graph-component.hh b/include/hpp/manipulation/graph/graph-component.hh
index e26850b..36cb8d5 100644
--- a/include/hpp/manipulation/graph/graph-component.hh
+++ b/include/hpp/manipulation/graph/graph-component.hh
@@ -57,6 +57,9 @@ namespace hpp {
const ImplicitPtr_t& numConstraint,
const segments_t& passiveDofs = segments_t ());
+ /// Add a cost function Implicit to the component.
+ virtual void addNumericalCost (const ImplicitPtr_t& numCost);
+
/// Reset the numerical constraints stored in the component.
virtual void resetNumericalConstraints ();
@@ -85,6 +88,9 @@ namespace hpp {
/// Get a reference to the NumericalConstraints_t
const NumericalConstraints_t& numericalConstraints() const;
+ /// Get a reference to the NumericalConstraints_t
+ const NumericalConstraints_t& numericalCosts() const;
+
/// Get a reference to the NumericalConstraints_t
const IntervalsContainer_t& passiveDofs() const;
@@ -119,6 +125,8 @@ namespace hpp {
NumericalConstraints_t numericalConstraints_;
/// Stores the passive dofs for each numerical constraints.
IntervalsContainer_t passiveDofs_;
+ /// Stores the numerical costs.
+ NumericalConstraints_t numericalCosts_;
/// List of LockedJoint constraints: \todo to be removed
const LockedJoints_t lockedJoints_;
/// A weak pointer to the parent graph.
diff --git a/src/graph/graph-component.cc b/src/graph/graph-component.cc
index 8b67e2b..1bed006 100644
--- a/src/graph/graph-component.cc
+++ b/src/graph/graph-component.cc
@@ -59,11 +59,18 @@ namespace hpp {
passiveDofs_.push_back (passiveDofs);
}
+ void GraphComponent::addNumericalCost (const ImplicitPtr_t& cost)
+ {
+ isInit_ = false;
+ numericalCosts_.push_back(cost);
+ }
+
void GraphComponent::resetNumericalConstraints ()
{
isInit_ = false;
numericalConstraints_.clear();
passiveDofs_.clear();
+ numericalCosts_.clear();
}
void GraphComponent::addLockedJointConstraint
@@ -85,6 +92,10 @@ namespace hpp {
++itpdof;
}
assert (itpdof == passiveDofs_.end ());
+ for (NumericalConstraints_t::const_iterator it = numericalCosts_.begin();
+ it != numericalCosts_.end(); ++it) {
+ proj->add (*it, 1);
+ }
return !numericalConstraints_.empty ();
}
@@ -93,6 +104,11 @@ namespace hpp {
return numericalConstraints_;
}
+ const NumericalConstraints_t& GraphComponent::numericalCosts() const
+ {
+ return numericalCosts_;
+ }
+
const std::vector <segments_t>& GraphComponent::passiveDofs() const
{
return passiveDofs_;
--
GitLab