diff --git a/include/hpp/manipulation/graph/graph-component.hh b/include/hpp/manipulation/graph/graph-component.hh
index 29832a7c65bb18831e3fd9f6298e04ce2626430c..e95cab93a1dc6107cf67ace2df34e45bfd9d9692 100644
--- a/include/hpp/manipulation/graph/graph-component.hh
+++ b/include/hpp/manipulation/graph/graph-component.hh
@@ -102,13 +102,28 @@ namespace hpp {
/// \deprecated call invalidate instead
void setDirty() HPP_MANIPULATION_DEPRECATED;
- protected:
+ /// Set whether hierachical constraints are solved level by level
+ /// \sa hpp::constraints::solver::HierarchicalIterative
+ void solveLevelByLevel(bool solveLevelByLevel)
+ {
+ solveLevelByLevel_ = solveLevelByLevel;
+ }
+
+ /// Get whether hierachical constraints are solved level by level
+ /// \sa hpp::constraints::solver::HierarchicalIterative
+ bool solveLevelByLevel() const
+ {
+ return solveLevelByLevel_;
+ }
+
+ protected:
/// Initialize the component
void init (const GraphComponentWkPtr_t& weak);
GraphComponent(const std::string& name) : isInit_(false)
, name_ (name)
, id_(-1)
+ , solveLevelByLevel_(false)
{}
/// Stores the numerical constraints.
@@ -138,7 +153,8 @@ namespace hpp {
GraphComponentWkPtr_t wkPtr_;
/// ID of the component (index in components vector).
std::size_t id_;
-
+ /// Whether the constraints are solved level by level
+ bool solveLevelByLevel_;
friend class Graph;
};
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index 91aaa628c1e9e21aabb5fdd66c60fb3ecea8d861..ae78615a0a56df8f36ea3590c6773e93feb2ca38 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -40,6 +40,7 @@
#include <hpp/constraints/differentiable-function.hh>
#include <hpp/constraints/locked-joint.hh>
+#include <hpp/constraints/solver/by-substitution.hh>
#include "hpp/manipulation/device.hh"
#include "hpp/manipulation/problem.hh"
@@ -258,6 +259,14 @@ namespace hpp {
for (const auto& _nc : nc)
proj->add (_nc);
+ // Insert numerical costs
+ nc.clear();
+ for (const auto& gc : components)
+ nc.insert(nc.end(), gc->numericalCosts().begin(),
+ gc->numericalCosts().end ());
+ for (const auto& _nc : nc)
+ proj->add (_nc, 1);
+
}
ConstraintSetPtr_t Edge::buildConfigConstraint()
@@ -273,6 +282,7 @@ namespace hpp {
ConstraintSetPtr_t constraint = ConstraintSet::create (g->robot (), "Set " + n);
ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "proj_" + n, g->errorThreshold(), g->maxIterations());
+ proj->solver().solveLevelByLevel(this->solveLevelByLevel());
std::vector <GraphComponentPtr_t> components;
components.push_back (g);
components.push_back (wkPtr_.lock ());
@@ -307,6 +317,7 @@ namespace hpp {
ConfigProjectorPtr_t proj = ConfigProjector::create
(g->robot(), "proj_" + n, .5*g->errorThreshold(), g->maxIterations());
+ proj->solver().solveLevelByLevel(this->solveLevelByLevel());
std::vector <GraphComponentPtr_t> components;
components.push_back (g);
components.push_back (wkPtr_.lock ());
diff --git a/src/graph/state.cc b/src/graph/state.cc
index 8f6af9369663f3946c630c756bc03b2260dfbe46..46d40e40aacd097e1b71f1eadbc132cc284bdf2e 100644
--- a/src/graph/state.cc
+++ b/src/graph/state.cc
@@ -29,6 +29,7 @@
#include "hpp/manipulation/graph/state.hh"
#include <hpp/constraints/differentiable-function.hh>
+#include <hpp/constraints/solver/by-substitution.hh>
#include "hpp/manipulation/device.hh"
#include "hpp/manipulation/graph/edge.hh"
@@ -126,6 +127,7 @@ namespace hpp {
configConstraints_ = ConstraintSet::create (g->robot (), "Set " + n);
ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "proj " + n, g->errorThreshold(), g->maxIterations());
+ proj->solver().solveLevelByLevel(this->solveLevelByLevel());
g->insertNumericalConstraints (proj);
insertNumericalConstraints (proj);
configConstraints_->addConstraint (proj);
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index dc271a8c70b401bde4c3d690d9ab9a06cdb4a3a5..d5c5a2915f705b48db648d20d1c812f983c8ab96 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -161,8 +161,6 @@ namespace hpp {
pathProjectors.add ("RecursiveHermite",
createPathProjector <core::pathProjector::RecursiveHermite>);
- steeringMethods.add ("Graph-SteeringMethodStraight",
- steeringMethod::Graph::create <core::steeringMethod::Straight>);
steeringMethods.add ("Graph-Straight",
steeringMethod::Graph::create <core::steeringMethod::Straight>);
steeringMethods.add ("Graph-Hermite",
@@ -184,7 +182,7 @@ namespace hpp {
steeringMethods.add ("EndEffectorTrajectory", steeringMethod::EndEffectorTrajectory::create);
pathPlannerType ("M-RRT");
- steeringMethodType ("Graph-SteeringMethodStraight");
+ steeringMethodType ("Graph-Straight");
}
ProblemSolverPtr_t ProblemSolver::create ()
@@ -244,6 +242,7 @@ namespace hpp {
(const std::string& name, const StringList_t& surface1,
const StringList_t& surface2, const value_type& margin)
{
+ bool explicit_(true);
if (!robot_) throw std::runtime_error ("No robot loaded");
JointAndShapes_t floorSurfaces, objectSurfaces, l;
for (StringList_t::const_iterator it1 = surface1.begin ();
@@ -251,7 +250,19 @@ namespace hpp {
if (!robot_->jointAndShapes.has (*it1))
throw std::runtime_error ("First list of triangles not found.");
l = robot_->jointAndShapes.get (*it1);
- objectSurfaces.insert(objectSurfaces.end(), l.begin(), l.end());
+ for(auto js : l){
+ JointPtr_t j(js.first);
+ // If one robot contact surface is not on a freeflying object,
+ // The contact constraint cannot be expressed by an explicit
+ // constraint.
+ if ((j->parentJoint()) ||
+ ((*j->configurationSpace()!=*pinocchio::LiegroupSpace::SE3()) &&
+ (*j->configurationSpace()!=*pinocchio::LiegroupSpace::R3xSO3())))
+ {
+ explicit_ = false;
+ }
+ objectSurfaces.insert(objectSurfaces.end(), l.begin(), l.end());
+ }
}
for (StringList_t::const_iterator it2 = surface2.begin ();
@@ -266,28 +277,45 @@ namespace hpp {
floorSurfaces.insert(floorSurfaces.end(), l.begin(), l.end());
}
- typedef hpp::constraints::explicit_::ConvexShapeContact Constraint_t;
- Constraint_t::Constraints_t constraints
- (Constraint_t::createConstraintAndComplement
- (name, robot_, floorSurfaces, objectSurfaces, margin));
-
- addNumericalConstraint(std::get<0>(constraints)->function().name(),
- std::get<0>(constraints));
- addNumericalConstraint(std::get<1>(constraints)->function().name(),
- std::get<1>(constraints));
- addNumericalConstraint(std::get<2>(constraints)->function().name(),
- std::get<2>(constraints));
- // Set security margin to contact constraint
- assert(HPP_DYNAMIC_PTR_CAST(constraints::ConvexShapeContact,
- std::get<0>(constraints)->functionPtr()));
- constraints::ConvexShapeContactPtr_t contactFunction
- (HPP_STATIC_PTR_CAST(constraints::ConvexShapeContact,
- std::get<0>(constraints)->functionPtr()));
- contactFunction->setNormalMargin(margin);
- constraintsAndComplements.push_back (
- ConstraintAndComplement_t (std::get<0>(constraints),
- std::get<1>(constraints),
- std::get<2>(constraints)));
+ if (explicit_){
+ typedef hpp::constraints::explicit_::ConvexShapeContact Constraint_t;
+ Constraint_t::Constraints_t constraints
+ (Constraint_t::createConstraintAndComplement
+ (name, robot_, floorSurfaces, objectSurfaces, margin));
+
+ addNumericalConstraint(std::get<0>(constraints)->function().name(),
+ std::get<0>(constraints));
+ addNumericalConstraint(std::get<1>(constraints)->function().name(),
+ std::get<1>(constraints));
+ addNumericalConstraint(std::get<2>(constraints)->function().name(),
+ std::get<2>(constraints));
+ // Set security margin to contact constraint
+ assert(HPP_DYNAMIC_PTR_CAST(constraints::ConvexShapeContact,
+ std::get<0>(constraints)->functionPtr()));
+ constraints::ConvexShapeContactPtr_t contactFunction
+ (HPP_STATIC_PTR_CAST(constraints::ConvexShapeContact,
+ std::get<0>(constraints)->functionPtr()));
+ contactFunction->setNormalMargin(margin);
+ constraintsAndComplements.push_back (
+ ConstraintAndComplement_t (std::get<0>(constraints),
+ std::get<1>(constraints),
+ std::get<2>(constraints)));
+ } else{
+ using constraints::ComparisonTypes_t;
+ using constraints::Implicit;
+ using constraints::Equality;
+ using constraints::EqualToZero;
+ std::pair<hpp::constraints::ConvexShapeContactPtr_t,
+ hpp::constraints::ConvexShapeContactComplementPtr_t>
+ functions(constraints::ConvexShapeContactComplement::createPair
+ (name, robot_, floorSurfaces, objectSurfaces));
+ functions.first->setNormalMargin(margin);
+ addNumericalConstraint(functions.first->name(),
+ Implicit::create(functions.first, 5*EqualToZero));
+ addNumericalConstraint(functions.second->name(),
+ Implicit::create(functions.second, ComparisonTypes_t
+ (functions.second->outputSize(), Equality)));
+ }
}
void ProblemSolver::createPrePlacementConstraint