From 73a7c938317126b64fa9a9b5189756cab54f4d28 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Thu, 17 Dec 2020 09:57:13 +0100
Subject: [PATCH] Remove usage of passive dofs + clean code.
---
include/hpp/manipulation/graph/state.hh | 9 +--
src/graph/edge.cc | 101 ++++++++----------------
src/graph/graph-component.cc | 15 +---
src/handle.cc | 2 -
src/problem-solver.cc | 2 +-
src/steering-method/graph.cc | 2 +-
6 files changed, 42 insertions(+), 89 deletions(-)
diff --git a/include/hpp/manipulation/graph/state.hh b/include/hpp/manipulation/graph/state.hh
index 01fc6649..7970bf67 100644
--- a/include/hpp/manipulation/graph/state.hh
+++ b/include/hpp/manipulation/graph/state.hh
@@ -139,13 +139,8 @@ namespace hpp {
/// \return true is at least one ImplicitPtr_t was inserted.
bool insertNumericalConstraintsForPath (ConfigProjectorPtr_t& proj) const
{
- assert (numericalConstraintsForPath_.size () == passiveDofsForPath_.size ());
- IntervalsContainer_t::const_iterator itpdofs = passiveDofsForPath_.begin ();
- for (NumericalConstraints_t::const_iterator it = numericalConstraintsForPath_.begin();
- it != numericalConstraintsForPath_.end(); it++) {
- proj->add (*it, *itpdofs);
- itpdofs++;
- }
+ for (const auto& nc : numericalConstraintsForPath_)
+ proj->add (nc);
return !numericalConstraintsForPath_.empty ();
}
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index 6012044b..7d840521 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -214,61 +214,43 @@ namespace hpp {
const GraphPtr_t& graph)
{
NumericalConstraints_t nc;
- std::vector <segments_t> pdof;
- for (std::vector <GraphComponentPtr_t>::const_iterator it
- (components.begin ()); it != components.end (); ++it) {
- nc.insert (nc.end (), (*it)->numericalConstraints ().begin (),
- (*it)->numericalConstraints ().end ());
- pdof.insert (pdof.end (), (*it)->passiveDofs ().begin (),
- (*it)->passiveDofs ().end ());
- }
- assert (nc.size () == pdof.size ());
- NumericalConstraints_t::iterator itnc1, itnc2;
- std::vector <segments_t>::iterator itpdof1, itpdof2;
+ for (const auto& gc : components)
+ nc.insert(nc.end(), gc->numericalConstraints().begin(),
+ gc->numericalConstraints ().end ());
// Remove duplicate constraints
- for (itnc1 = nc.begin(), itpdof1 = pdof.begin(); itnc1 != nc.end(); ++itnc1, ++itpdof1) {
- itnc2 = itnc1; ++itnc2;
- itpdof2 = itpdof1; ++itpdof2;
- while (itnc2 != nc.end()) {
- if (*itnc1 == *itnc2) {
- itnc2 = nc.erase (itnc2);
- itpdof2 = pdof.erase (itpdof2);
- } else {
- ++itnc2;
- ++itpdof2;
- }
- }
- }
+ auto end = nc.end();
+ for (auto it = nc.begin(); it != end; ++it)
+ end = std::remove(std::next(it), end, *it);
+ nc.erase(end, nc.end());
+
+ NumericalConstraints_t::iterator itnc1, itnc2;
+
+ itnc2 = nc.end();
+ for (itnc1 = nc.begin(); itnc1 != itnc2; ++itnc1)
+ itnc2 = std::remove(std::next(itnc1), itnc2, *itnc1);
+ nc.erase(itnc2, nc.end());
// Look for complement
- for (itnc1 = nc.begin(), itpdof1 = pdof.begin(); itnc1 != nc.end(); ++itnc1, ++itpdof1) {
- itnc2 = itnc1; ++itnc2;
- itpdof2 = itpdof1; ++itpdof2;
+ for (itnc1 = nc.begin(); itnc1 != nc.end(); ++itnc1) {
+ const auto& c1 = *itnc1;
+ itnc2 = std::next(itnc1);
constraints::ImplicitPtr_t combination;
- while (itnc2 != nc.end()) {
+ itnc2 = std::find_if(std::next(itnc1), nc.end(),
+ [&c1, &combination, &graph](const auto& c2) -> bool {
+ assert (c1 != c2);
+ return graph->isComplement (c1, c2, combination)
+ || graph->isComplement (c2, c1, combination);
+ });
+ if (itnc2 != nc.end()) {
assert (*itnc1 != *itnc2);
- if ( graph->isComplement (*itnc1, *itnc2, combination)
- || graph->isComplement (*itnc2, *itnc1, combination)) {
- // Replace constraint by combination of both and remove
- // complement.
- *itnc1 = combination;
- nc.erase (itnc2);
- pdof.erase (itpdof2);
- break;
- } else {
- ++itnc2;
- ++itpdof2;
- }
+ *itnc1 = combination;
+ nc.erase (itnc2);
}
}
- NumericalConstraints_t::iterator itnc (nc.begin ());
- std::vector <segments_t>::iterator itpdof (pdof.begin ());
- while (itnc != nc.end ()) {
- proj->add (*itnc, *itpdof);
- ++itnc; ++itpdof;
- }
+ for (const auto& _nc : nc)
+ proj->add (_nc);
}
ConstraintSetPtr_t Edge::buildConfigConstraint()
@@ -741,13 +723,8 @@ namespace hpp {
ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "projParam_" + n, g->errorThreshold(), g->maxIterations());
- IntervalsContainer_t::const_iterator itpdof = paramPassiveDofs_.begin ();
- for (NumericalConstraints_t::const_iterator it = paramNumericalConstraints_.begin ();
- it != paramNumericalConstraints_.end (); ++it) {
- proj->add (*it, *itpdof);
- ++itpdof;
- }
- assert (itpdof == paramPassiveDofs_.end ());
+ for (const auto& nc : paramNumericalConstraints_)
+ proj->add (nc);
param->addConstraint (proj);
param->edge (wkPtr_.lock ());
@@ -763,13 +740,8 @@ namespace hpp {
ConstraintSetPtr_t cond = ConstraintSet::create (g->robot (), "Set " + n);
proj = ConfigProjector::create(g->robot(), "projCond_" + n, g->errorThreshold(), g->maxIterations());
- itpdof = condPassiveDofs_.begin ();
- for (NumericalConstraints_t::const_iterator it = condNumericalConstraints_.begin ();
- it != condNumericalConstraints_.end (); ++it) {
- proj->add (*it, *itpdof);
- ++itpdof;
- }
- assert (itpdof == condPassiveDofs_.end ());
+ for (const auto& nc : condNumericalConstraints_)
+ proj->add (nc);
f.condition (cond);
cond->addConstraint (proj);
@@ -816,13 +788,8 @@ namespace hpp {
// - the state in which the transition lies if different
g->insertNumericalConstraints (proj);
- IntervalsContainer_t::const_iterator itpdof = paramPassiveDofs_.begin ();
- for (NumericalConstraints_t::const_iterator it = paramNumericalConstraints_.begin ();
- it != paramNumericalConstraints_.end (); ++it) {
- proj->add (*it, *itpdof);
- ++itpdof;
- }
- assert (itpdof == paramPassiveDofs_.end ());
+ for (const auto& nc : paramNumericalConstraints_)
+ proj->add (nc);
insertNumericalConstraints (proj);
stateTo ()->insertNumericalConstraints (proj);
diff --git a/src/graph/graph-component.cc b/src/graph/graph-component.cc
index 0900acc1..50ef1a79 100644
--- a/src/graph/graph-component.cc
+++ b/src/graph/graph-component.cc
@@ -85,17 +85,10 @@ namespace hpp {
bool GraphComponent::insertNumericalConstraints (ConfigProjectorPtr_t& proj) const
{
- IntervalsContainer_t::const_iterator itpdof = passiveDofs_.begin ();
- for (NumericalConstraints_t::const_iterator it = numericalConstraints_.begin();
- it != numericalConstraints_.end(); ++it) {
- proj->add (*it, *itpdof);
- ++itpdof;
- }
- assert (itpdof == passiveDofs_.end ());
- for (NumericalConstraints_t::const_iterator it = numericalCosts_.begin();
- it != numericalCosts_.end(); ++it) {
- proj->add (*it, 1);
- }
+ for (const auto& nc : numericalConstraints_)
+ proj->add (nc);
+ for (const auto& nc : numericalCosts_)
+ proj->add (nc, 1);
return !numericalConstraints_.empty ();
}
diff --git a/src/handle.cc b/src/handle.cc
index 945e5847..16f2e2c0 100644
--- a/src/handle.cc
+++ b/src/handle.cc
@@ -27,7 +27,6 @@
#include <hpp/pinocchio/joint-collection.hh>
#include <hpp/pinocchio/gripper.hh>
-#include <hpp/constraints/generic-transformation.hh>
#include <hpp/constraints/implicit.hh>
#include <hpp/constraints/explicit/relative-pose.hh>
#include <hpp/constraints/generic-transformation.hh>
@@ -162,7 +161,6 @@ namespace hpp {
ImplicitPtr_t Handle::createGraspAndComplement
(const GripperPtr_t& gripper, std::string n) const
{
- using core::ExplicitRelativeTransformation;
if (n.empty()) {
n = gripper->name() + "_holds_" + name();
}
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index 86df217b..b7bc3f03 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -156,7 +156,7 @@ namespace hpp {
createPathProjector <core::pathProjector::RecursiveHermite>);
steeringMethods.add ("Graph-SteeringMethodStraight",
- steeringMethod::Graph::create <core::SteeringMethodStraight>);
+ steeringMethod::Graph::create <core::steeringMethod::Straight>);
steeringMethods.add ("Graph-Straight",
steeringMethod::Graph::create <core::steeringMethod::Straight>);
steeringMethods.add ("Graph-Hermite",
diff --git a/src/steering-method/graph.cc b/src/steering-method/graph.cc
index f7d778ca..48cefdf7 100644
--- a/src/steering-method/graph.cc
+++ b/src/steering-method/graph.cc
@@ -29,7 +29,7 @@ namespace hpp {
namespace manipulation {
SteeringMethod::SteeringMethod (const ProblemConstPtr_t& problem) :
core::SteeringMethod (problem), problem_ (problem),
- steeringMethod_ (core::SteeringMethodStraight::create (problem))
+ steeringMethod_ (core::steeringMethod::Straight::create (problem))
{
}
--
GitLab