From 5aa37d125cb7cfa7f463f0be714883f898f12662 Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Tue, 10 Sep 2019 09:55:13 +0200
Subject: [PATCH] [graph] Handle locked joint as other constraints.
---
include/hpp/manipulation/graph/edge.hh | 9 ---
.../hpp/manipulation/graph/graph-component.hh | 20 ++++--
src/graph/edge.cc | 63 -------------------
src/graph/graph-component.cc | 17 +----
src/graph/helper.cc | 18 +-----
src/graph/state.cc | 3 -
6 files changed, 16 insertions(+), 114 deletions(-)
diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh
index ca229ee..7872241 100644
--- a/include/hpp/manipulation/graph/edge.hh
+++ b/include/hpp/manipulation/graph/edge.hh
@@ -310,15 +310,10 @@ namespace hpp {
void insertParamConstraint (const ImplicitPtr_t& nm,
const segments_t& passiveDofs = segments_t ());
- /// Insert a LockedJoint that parametrizes the foliation
- void insertParamConstraint (const LockedJointPtr_t lockedJoint);
-
/// Insert a numerical constraint that defines the foliation
void insertConditionConstraint (const ImplicitPtr_t& nm,
const segments_t& passiveDofs = segments_t ());
- /// Insert a LockedJoint that defines the foliation
- void insertConditionConstraint (const LockedJointPtr_t lockedJoint);
/// \}
/// Print the object in a stream.
@@ -347,15 +342,11 @@ namespace hpp {
// NumericalConstraints_t
NumericalConstraints_t paramNumericalConstraints_;
IntervalsContainer_t paramPassiveDofs_;
- // LockedJoints_t
- LockedJoints_t paramLockedJoints_;
// Condition
// NumericalConstraints_t
NumericalConstraints_t condNumericalConstraints_;
IntervalsContainer_t condPassiveDofs_;
- // LockedJoints_t
- LockedJoints_t condLockedJoints_;
/// This histogram will be used to find a good level set.
LeafHistogramPtr_t hist_;
diff --git a/include/hpp/manipulation/graph/graph-component.hh b/include/hpp/manipulation/graph/graph-component.hh
index ac3e02a..066a92a 100644
--- a/include/hpp/manipulation/graph/graph-component.hh
+++ b/include/hpp/manipulation/graph/graph-component.hh
@@ -64,11 +64,15 @@ namespace hpp {
virtual void resetNumericalConstraints ();
/// Add core::LockedJoint constraint to the component.
+ /// \deprecated LockedJoint are handled as classical explicit
+ /// constraint
virtual void addLockedJointConstraint
- (const LockedJointPtr_t& constraint);
+ (const LockedJointPtr_t& constraint) HPP_MANIPULATION_DEPRECATED;
+ /// \deprecated LockedJoint are handled as classical explicit
+ /// constraint
/// Reset the locked joint in the component.
- virtual void resetLockedJoints ();
+ virtual void resetLockedJoints () HPP_MANIPULATION_DEPRECATED;
/// Insert the numerical constraints in a ConfigProjector
/// \return true is at least one ImplicitPtr_t was inserted.
@@ -76,7 +80,10 @@ namespace hpp {
/// Insert the LockedJoint constraints in a ConstraintSet
/// \return true is at least one LockedJointPtr_t was inserted.
- bool insertLockedJoints (ConfigProjectorPtr_t& cs) const;
+ /// \deprecated LockedJoint are handled as classical explicit
+ /// constraint
+ bool insertLockedJoints (ConfigProjectorPtr_t& cs) const
+ HPP_MANIPULATION_DEPRECATED;
/// Get a reference to the NumericalConstraints_t
const NumericalConstraints_t& numericalConstraints() const;
@@ -85,7 +92,8 @@ namespace hpp {
const IntervalsContainer_t& passiveDofs() const;
/// Get a reference to the LockedJoints_t
- const LockedJoints_t& lockedJoints () const;
+ const LockedJoints_t& lockedJoints () const
+ HPP_MANIPULATION_DEPRECATED;
/// Set the parent graph.
void parentGraph(const GraphWkPtr_t& parent);
@@ -112,8 +120,8 @@ namespace hpp {
NumericalConstraints_t numericalConstraints_;
/// Stores the passive dofs for each numerical constraints.
IntervalsContainer_t passiveDofs_;
- /// List of LockedJoint constraints
- LockedJoints_t lockedJoints_;
+ /// List of LockedJoint constraints: to be removed
+ const LockedJoints_t lockedJoints_;
/// A weak pointer to the parent graph.
GraphWkPtr_t graph_;
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index ed590f1..e2acc46 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -102,10 +102,6 @@ namespace hpp {
insertNumericalConstraints (proj);
state ()->insertNumericalConstraints (proj);
- g->insertLockedJoints (proj);
- insertLockedJoints (proj);
- state ()->insertLockedJoints (proj);
-
if (wkPtr_.lock() == other) // No intersection to be computed.
return false;
@@ -113,9 +109,6 @@ namespace hpp {
other->insertNumericalConstraints (proj);
if (!stateB_Eq_stateA) other->state()->insertNumericalConstraints (proj);
- other->insertLockedJoints (proj);
- if (!stateB_Eq_stateA) other->state()->insertLockedJoints (proj);
-
return true;
}
@@ -247,13 +240,6 @@ namespace hpp {
ConstraintSetPtr_t constraint = ConstraintSet::create (g->robot (), "Set " + n);
ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "proj_" + n, g->errorThreshold(), g->maxIterations());
- g->insertLockedJoints (proj);
- insertLockedJoints (proj);
- to ()->insertLockedJoints (proj);
- if (state () != to ()) {
- state ()->insertLockedJoints (proj);
- }
-
std::vector <GraphComponentPtr_t> components;
components.push_back (g);
components.push_back (wkPtr_.lock ());
@@ -282,10 +268,6 @@ namespace hpp {
ConstraintSetPtr_t constraint = ConstraintSet::create (g->robot (), "Set " + n);
ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "proj_" + n, g->errorThreshold(), g->maxIterations());
- g->insertLockedJoints (proj);
- insertLockedJoints (proj);
- state ()->insertLockedJoints (proj);
-
std::vector <GraphComponentPtr_t> components;
components.push_back (g);
components.push_back (wkPtr_.lock ());
@@ -549,19 +531,11 @@ namespace hpp {
it != condNumericalConstraints_.end (); ++it) {
tp.addLine ("- " + (*it)->function ().name ());
}
- for (LockedJoints_t::const_iterator it = condLockedJoints_.begin ();
- it != condLockedJoints_.end (); ++it) {
- tp.addLine ("- " + (*it)->jointName ());
- }
tp.addLine ("Foliation parametrization constraints:");
for (NumericalConstraints_t::const_iterator it = paramNumericalConstraints_.begin ();
it != paramNumericalConstraints_.end (); ++it) {
tp.addLine ("- " + (*it)->function ().name ());
}
- for (LockedJoints_t::const_iterator it = paramLockedJoints_.begin ();
- it != paramLockedJoints_.end (); ++it) {
- tp.addLine ("- " + (*it)->jointName ());
- }
}
bool LevelSetEdge::applyConstraints (ConfigurationIn_t qoffset, ConfigurationOut_t q) const
@@ -605,10 +579,6 @@ namespace hpp {
it != paramNumericalConstraints_.end (); ++it) {
cp->rightHandSideFromConfig (*it, qlevelset);
}
- for (LockedJoints_t::const_iterator it = paramLockedJoints_.begin ();
- it != paramLockedJoints_.end (); ++it) {
- cp->rightHandSideFromConfig (*it, qlevelset);
- }
// Eventually, do the projection.
if (isShort_) q = qoffset;
@@ -662,11 +632,6 @@ namespace hpp {
ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "projParam_" + n, g->errorThreshold(), g->maxIterations());
- for (LockedJoints_t::const_iterator it = paramLockedJoints_.begin ();
- it != paramLockedJoints_.end (); ++it) {
- proj->add (*it);
- }
-
IntervalsContainer_t::const_iterator itpdof = paramPassiveDofs_.begin ();
for (NumericalConstraints_t::const_iterator it = paramNumericalConstraints_.begin ();
it != paramNumericalConstraints_.end (); ++it) {
@@ -689,11 +654,6 @@ namespace hpp {
ConstraintSetPtr_t cond = ConstraintSet::create (g->robot (), "Set " + n);
proj = ConfigProjector::create(g->robot(), "projCond_" + n, g->errorThreshold(), g->maxIterations());
- for (LockedJoints_t::const_iterator it = condLockedJoints_.begin ();
- it != condLockedJoints_.end (); ++it) {
- proj->add (*it);
- }
-
itpdof = condPassiveDofs_.begin ();
for (NumericalConstraints_t::const_iterator it = condNumericalConstraints_.begin ();
it != condNumericalConstraints_.end (); ++it) {
@@ -732,17 +692,6 @@ namespace hpp {
ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "proj_" + n, g->errorThreshold(), g->maxIterations());
- g->insertLockedJoints (proj);
- for (LockedJoints_t::const_iterator it = paramLockedJoints_.begin ();
- it != paramLockedJoints_.end (); ++it) {
- proj->add (*it);
- }
- insertLockedJoints (proj);
- to ()->insertLockedJoints (proj);
- if (state () != to ()) {
- state ()->insertLockedJoints (proj);
- }
-
g->insertNumericalConstraints (proj);
IntervalsContainer_t::const_iterator itpdof = paramPassiveDofs_.begin ();
for (NumericalConstraints_t::const_iterator it = paramNumericalConstraints_.begin ();
@@ -772,12 +721,6 @@ namespace hpp {
paramPassiveDofs_.push_back (passiveDofs);
}
- void LevelSetEdge::insertParamConstraint (const LockedJointPtr_t lockedJoint)
- {
- isInit_ = false;
- paramLockedJoints_.push_back (lockedJoint);
- }
-
void LevelSetEdge::insertConditionConstraint
(const constraints::ImplicitPtr_t& nm,
const segments_t& passiveDofs)
@@ -787,12 +730,6 @@ namespace hpp {
condPassiveDofs_.push_back (passiveDofs);
}
- void LevelSetEdge::insertConditionConstraint (const LockedJointPtr_t lockedJoint)
- {
- isInit_ = false;
- condLockedJoints_.push_back (lockedJoint);
- }
-
LevelSetEdge::LevelSetEdge
(const std::string& name) :
Edge (name)
diff --git a/src/graph/graph-component.cc b/src/graph/graph-component.cc
index 485e3f4..8b67e2b 100644
--- a/src/graph/graph-component.cc
+++ b/src/graph/graph-component.cc
@@ -69,14 +69,11 @@ namespace hpp {
void GraphComponent::addLockedJointConstraint
(const LockedJointPtr_t& constraint)
{
- isInit_ = false;
- lockedJoints_.push_back (constraint);
+ addNumericalConstraint (constraint);
}
void GraphComponent::resetLockedJoints ()
{
- isInit_ = false;
- lockedJoints_.clear();
}
bool GraphComponent::insertNumericalConstraints (ConfigProjectorPtr_t& proj) const
@@ -91,14 +88,6 @@ namespace hpp {
return !numericalConstraints_.empty ();
}
- bool GraphComponent::insertLockedJoints (ConfigProjectorPtr_t& cp) const
- {
- for (LockedJoints_t::const_iterator it = lockedJoints_.begin();
- it != lockedJoints_.end(); ++it)
- cp->add (*it);
- return !lockedJoints_.empty ();
- }
-
const NumericalConstraints_t& GraphComponent::numericalConstraints() const
{
return numericalConstraints_;
@@ -150,10 +139,6 @@ namespace hpp {
it != numericalConstraints_.end (); ++it) {
tp.addLine ("- " + (*it)->function ().name ());
}
- for (LockedJoints_t::const_iterator it = lockedJoints_.begin ();
- it != lockedJoints_.end (); ++it) {
- tp.addLine ("- " + (*it)->jointName ());
- }
}
} // namespace graph
} // namespace manipulation
diff --git a/src/graph/helper.cc b/src/graph/helper.cc
index 815edbc..ecd245b 100644
--- a/src/graph/helper.cc
+++ b/src/graph/helper.cc
@@ -90,34 +90,18 @@ namespace hpp {
void FoliatedManifold::addToState (StatePtr_t comp) const
{
nc.addToComp <false> (comp);
- for (LockedJoints_t::const_iterator it = lj.begin ();
- it != lj.end (); ++it)
- if (*it && (*it)->numberDof() > 0)
- comp->addLockedJointConstraint (*it);
nc_path.addToComp <true> (comp);
}
void FoliatedManifold::addToEdge (EdgePtr_t comp) const
{
nc_fol.addToComp <false> (comp);
- for (LockedJoints_t::const_iterator it = lj_fol.begin ();
- it != lj_fol.end (); ++it)
- if (*it && (*it)->numberDof() > 0)
- comp->addLockedJointConstraint (*it);
}
void FoliatedManifold::specifyFoliation (LevelSetEdgePtr_t lse) const
{
nc.specifyFoliation <false> (lse);
- for (LockedJoints_t::const_iterator it = lj.begin ();
- it != lj.end (); ++it)
- if (*it && (*it)->numberDof() > 0)
- lse->insertConditionConstraint (*it);
-
nc_fol.specifyFoliation <true> (lse);
- for (LockedJoints_t::const_iterator it = lj_fol.begin ();
- it != lj_fol.end (); ++it)
- lse->insertParamConstraint (*it);
}
namespace {
@@ -1112,7 +1096,7 @@ namespace hpp {
.segment (oj->rankInConfiguration (),
oj->configSize ()), space);
LockedJointPtr_t lj = core::LockedJoint::create (oj, lge);
- ps->lockedJoints.add ("lock_" + oj->name (), lj);
+ ps->numericalConstraints.add ("lock_" + oj->name (), lj);
objects[i].get<0> ().get<2> ().push_back (lj);
}
++i;
diff --git a/src/graph/state.cc b/src/graph/state.cc
index b8e441c..28b3d06 100644
--- a/src/graph/state.cc
+++ b/src/graph/state.cc
@@ -117,9 +117,6 @@ namespace hpp {
g->insertNumericalConstraints (proj);
insertNumericalConstraints (proj);
configConstraints_->addConstraint (proj);
-
- g->insertLockedJoints (proj);
- insertLockedJoints (proj);
}
void State::updateWeight (const EdgePtr_t& e, const Weight_t& w)
--
GitLab