diff --git a/include/hpp/manipulation/fwd.hh b/include/hpp/manipulation/fwd.hh
index 2a1ea85dcf4ce4ea75c7c7e7f43cd80327b525d3..b3159434410000a6e40cd51e92a483878b2b0d0e 100644
--- a/include/hpp/manipulation/fwd.hh
+++ b/include/hpp/manipulation/fwd.hh
@@ -100,6 +100,8 @@ namespace hpp {
typedef std::vector <ObjectPtr_t> Objects_t;
typedef core::Constraint Constraint;
typedef core::ConstraintPtr_t ConstraintPtr_t;
+ typedef constraints::Explicit Explicit;
+ typedef constraints::ExplicitPtr_t ExplicitPtr_t;
typedef constraints::ImplicitPtr_t ImplicitPtr_t;
typedef constraints::LockedJoint LockedJoint;
typedef constraints::LockedJointPtr_t LockedJointPtr_t;
@@ -112,6 +114,7 @@ namespace hpp {
typedef core::ConfigurationShooter ConfigurationShooter;
typedef core::ConfigurationShooterPtr_t ConfigurationShooterPtr_t;
typedef core::ValidationReport ValidationReport;
+ typedef core::NumericalConstraints_t NumericalConstraints_t;
typedef core::PathValidationPtr_t PathValidationPtr_t;
typedef core::PathValidationReportPtr_t PathValidationReportPtr_t;
typedef core::matrix_t matrix_t;
diff --git a/include/hpp/manipulation/steering-method/cross-state-optimization.hh b/include/hpp/manipulation/steering-method/cross-state-optimization.hh
index 8b555508fab9dcdbaa1025881e7a490043fd38b3..74905076999c3a9db6141df5a255c9c4b3d88833 100644
--- a/include/hpp/manipulation/steering-method/cross-state-optimization.hh
+++ b/include/hpp/manipulation/steering-method/cross-state-optimization.hh
@@ -1,5 +1,6 @@
// Copyright (c) 2017, Joseph Mirabel
-// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
+// Authors: Joseph Mirabel (joseph.mirabel@laas.fr),
+// Florent Lamiraux (florent.lamiraux@laas.fr)
//
// This file is part of hpp-manipulation.
// hpp-manipulation is free software: you can redistribute it
@@ -18,6 +19,7 @@
# define HPP_MANIPULATION_STEERING_METHOD_CROSS_STATE_OPTIMIZATION_HH
# include <hpp/core/steering-method.hh>
+# include <hpp/core/config-projector.hh>
# include <hpp/manipulation/config.hh>
# include <hpp/manipulation/fwd.hh>
@@ -36,7 +38,7 @@ namespace hpp {
/// #### Methodology
///
/// Given two configuration \f$ (q_1,q_2) \f$, this class formulates and
- /// solves the problem as follows.
+ /// solves the problem as follows.
/// - Compute the corresponding states \f$ (s_1, s_2) \f$.
/// - For a each path \f$ (e_0, ... e_n) \f$ between \f$ (s_1, s_2) \f$
/// in the constraint graph, do:
@@ -57,6 +59,8 @@ namespace hpp {
public SteeringMethod
{
public:
+ struct OptimizationData;
+
static CrossStateOptimizationPtr_t create (const Problem& problem);
/// \warning core::Problem will be casted to Problem
@@ -71,12 +75,16 @@ namespace hpp {
protected:
CrossStateOptimization (const Problem& problem) :
- SteeringMethod (problem)
- {}
+ SteeringMethod (problem),
+ sameRightHandSide_ ()
+ {
+ gatherGraphConstraints ();
+ }
CrossStateOptimization (const CrossStateOptimization& other) :
- SteeringMethod (other),
- weak_ ()
+ SteeringMethod (other), constraints_ (other.constraints_),
+ index_ (other.index_), sameRightHandSide_
+ (other.sameRightHandSide_), weak_ ()
{}
core::PathPtr_t impl_compute (ConfigurationIn_t q1, ConfigurationIn_t q2) const;
@@ -88,8 +96,11 @@ namespace hpp {
}
private:
+ typedef constraints::solver::BySubstitution Solver_t;
struct GraphSearchData;
- struct OptimizationData;
+
+ /// Gather constraints of all edges
+ void gatherGraphConstraints ();
/// Step 1 of the algorithm
/// \return whether the max depth was reached.
@@ -99,10 +110,28 @@ namespace hpp {
graph::Edges_t getTransitionList (GraphSearchData& data, const std::size_t& i) const;
/// Step 3 of the algorithm
- void buildOptimizationProblem (OptimizationData& d, const graph::Edges_t& edges) const;
+ bool buildOptimizationProblem
+ (OptimizationData& d, const graph::Edges_t& transitions) const;
+
+ /// Step 4 of the algorithm
+ bool solveOptimizationProblem (OptimizationData& d) const;
+
+ bool checkConstantRightHandSide (OptimizationData& d,
+ size_type index) const;
core::PathVectorPtr_t buildPath (OptimizationData& d, const graph::Edges_t& edges) const;
+ bool contains (const Solver_t& solver, const ImplicitPtr_t& c) const;
+
+ /// Vector of parameterizable edge numerical constraints
+ NumericalConstraints_t constraints_;
+ /// Map of indexes in constraints_
+ std::map < std::string, std::size_t > index_;
+
+ /// associative map that stores pairs of constraints of the form
+ /// (constraint, constraint/hold)
+ std::map <ImplicitPtr_t, ImplicitPtr_t> sameRightHandSide_;
+
/// Weak pointer to itself
CrossStateOptimizationWkPtr_t weak_;
}; // class CrossStateOptimization
diff --git a/src/steering-method/cross-state-optimization.cc b/src/steering-method/cross-state-optimization.cc
index 850392f90152496c79659140ff95237f1ec2d5ca..f07cbe776a68691e620c39dd735878eacc57b1d1 100644
--- a/src/steering-method/cross-state-optimization.cc
+++ b/src/steering-method/cross-state-optimization.cc
@@ -1,5 +1,6 @@
// Copyright (c) 2017, Joseph Mirabel
-// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
+// Authors: Joseph Mirabel (joseph.mirabel@laas.fr),
+// Florent Lamiraux (florent.lamiraux@laas.fr)
//
// This file is part of hpp-manipulation.
// hpp-manipulation is free software: you can redistribute it
@@ -39,8 +40,6 @@
#include <hpp/manipulation/graph/edge.hh>
#include <hpp/manipulation/graph/state.hh>
-#include <../src/steering-method/cross-state-optimization/function.cc>
-
namespace hpp {
namespace manipulation {
namespace steeringMethod {
@@ -142,6 +141,64 @@ namespace hpp {
}
};
+ void CrossStateOptimization::gatherGraphConstraints ()
+ {
+ typedef graph::Edge Edge;
+ typedef graph::EdgePtr_t EdgePtr_t;
+ typedef graph::GraphPtr_t GraphPtr_t;
+ typedef constraints::solver::BySubstitution Solver_t;
+
+ std::map <ImplicitPtr_t, ImplicitPtr_t> constraintCopy, constraintOrig;
+ ImplicitPtr_t copy;
+ GraphPtr_t cg (problem_.constraintGraph ());
+ const ConstraintsAndComplements_t& cac
+ (cg->constraintsAndComplements ());
+ for (std::size_t i = 0; i < cg->nbComponents (); ++i) {
+ EdgePtr_t edge (HPP_DYNAMIC_PTR_CAST (Edge, cg->get (i).lock ()));
+ if (edge) {
+ const Solver_t& solver (edge->pathConstraint ()->
+ configProjector ()->solver ());
+ const NumericalConstraints_t& constraints
+ (solver.numericalConstraints ());
+ for (NumericalConstraints_t::const_iterator it
+ (constraints.begin ()); it != constraints.end (); ++it) {
+ if ((*it)->parameterSize () > 0) {
+ const std::string& name ((*it)->function ().name ());
+ if (index_.find (name) == index_.end ()) {
+ // constraint is not in map, add it
+ index_ [name] = constraints_.size ();
+ copy = (*it)->copy ();
+ constraintCopy [*it] = copy;
+ constraintOrig [copy] = *it;
+ // Check whether constraint is equivalent to a previous one
+ for (NumericalConstraints_t::const_iterator it1
+ (constraints_.begin ()); it1 != constraints_.end ();
+ ++it1) {
+ for (ConstraintsAndComplements_t::const_iterator it2
+ (cac.begin ()); it2 != cac.end (); ++it2) {
+ if (((constraintOrig [*it1] == it2->complement) &&
+ (*it == it2->both)) ||
+ ((constraintOrig [*it1] == it2->both) &&
+ (*it == it2->complement))) {
+ assert (sameRightHandSide_.count (*it1) == 0);
+ assert (sameRightHandSide_.count (copy) == 0);
+ sameRightHandSide_ [*it1] = copy;
+ sameRightHandSide_ [copy] = *it1;
+ }
+ }
+ }
+ constraints_.push_back (copy);
+ hppDout (info, "Adding constraint \"" << name << "\"");
+ hppDout (info, "Edge \"" << edge->name () << "\"");
+ hppDout (info, "parameter size: " << (*it)->parameterSize ());
+
+ }
+ }
+ }
+ }
+ }
+ }
+
bool CrossStateOptimization::findTransitions (GraphSearchData& d) const
{
while (! d.queue1.empty())
@@ -209,452 +266,280 @@ namespace hpp {
return transitions;
}
+ namespace internal {
+ bool saturate (const core::DevicePtr_t& robot, vectorIn_t q,
+ vectorOut_t qSat, pinocchio::ArrayXb& saturatedDof)
+ {
+ qSat = q;
+ return hpp::pinocchio::saturate (robot, qSat, saturatedDof);
+ }
+ } // namespace internal
+
struct CrossStateOptimization::OptimizationData
{
+ typedef constraints::solver::HierarchicalIterative::Saturation_t
+ Saturation_t;
+ enum RightHandSideStatus_t {
+ // Constraint is not in solver for this waypoint
+ ABSENT,
+ // right hand side of constraint for this waypoint is equal to
+ // right hand side for previous waypoint
+ EQUAL_TO_PREVIOUS,
+ // right hand side of constraint for this waypoint is equal to
+ // right hand side for initial configuration
+ EQUAL_TO_INIT,
+ // right hand side of constraint for this waypoint is equal to
+ // right hand side for goal configuration
+ EQUAL_TO_GOAL
+ }; // enum RightHandSideStatus_t
const std::size_t N, nq, nv;
- constraints::solver::BySubstitution solver;
+ std::vector <Solver_t> solvers;
+ // Waypoints lying in each intermediate state
+ matrix_t waypoint;
Configuration_t q1, q2;
- vector_t q;
core::DevicePtr_t robot;
- typedef std::vector<States_t> StatesPerConf_t;
- StatesPerConf_t statesPerConf_;
- struct RightHandSideSetter {
- ImplicitPtr_t impF;
- size_type expFidx;
- Configuration_t* qrhs;
- vector_t rhs;
- RightHandSideSetter () : qrhs (NULL) {}
- // TODO delete this constructor
- RightHandSideSetter (ImplicitPtr_t _impF, size_type _expFidx,
- Configuration_t* _qrhs)
- : impF(_impF), expFidx(_expFidx), qrhs (_qrhs) {}
- RightHandSideSetter (ImplicitPtr_t _impF, size_type _expFidx,
- vector_t _rhs)
- : impF(_impF), expFidx(_expFidx), qrhs (NULL), rhs (_rhs) {}
- void apply(constraints::solver::BySubstitution& s)
- {
- if (expFidx >= 0) {
- if (qrhs != NULL) s.explicitConstraintSet().rightHandSideFromInput
- (expFidx, *qrhs);
- else s.explicitConstraintSet().rightHandSide
- (expFidx, rhs);
- } else {
- if (qrhs != NULL) s.rightHandSideFromConfig (impF, *qrhs);
- else s.rightHandSide (impF, rhs);
- }
- }
- };
- typedef std::vector<RightHandSideSetter> RightHandSideSetters_t;
- RightHandSideSetters_t rhsSetters_;
-
- OptimizationData (const std::size_t& _N, const core::DevicePtr_t _robot)
- : N (_N), nq (_robot->configSize()), nv (_robot->numberDof()),
- solver (_robot->configSpace () ^ N), robot (_robot),
- statesPerConf_ (N)
+ // Matrix specifying for each constraint and each waypoint how
+ // the right hand side is initialized in the solver.
+ Eigen::Matrix < vector_t, Eigen::Dynamic, Eigen::Dynamic > M_rhs;
+ Eigen::Matrix < RightHandSideStatus_t, Eigen::Dynamic, Eigen::Dynamic >
+ M_status;
+
+ OptimizationData (const core::DevicePtr_t _robot,
+ const Configuration_t& _q1,
+ const Configuration_t& _q2,
+ const Edges_t& transitions
+ ) :
+ N (transitions.size () - 1), nq (_robot->configSize()),
+ nv (_robot->numberDof()), solvers (N, _robot->configSpace ()),
+ waypoint (nq, N), q1 (_q1), q2 (_q2), robot (_robot), M_rhs (),
+ M_status ()
{
- solver.saturation (boost::bind(&OptimizationData::_saturate , this,
- _1, _2, _3));
+ assert (transitions.size () > 0);
}
+ };
- void addGraphConstraints (const graph::GraphPtr_t& graph)
- {
- for (std::size_t i = 0; i < N; ++i) {
- _add (graph->lockedJoints(), i);
- _add (graph->numericalConstraints(), i);
- }
+ bool CrossStateOptimization::checkConstantRightHandSide
+ (OptimizationData& d, size_type index) const
+ {
+ const ImplicitPtr_t c (constraints_ [index]);
+ c->rightHandSideFromConfig (d.q1);
+ vector_t rhsInit (c->rightHandSide ());
+ c->rightHandSideFromConfig (d.q2);
+ vector_t rhsGoal (c->rightHandSide ());
+ // Check that right hand sides are close to each other
+ value_type eps (problem_.constraintGraph ()->errorThreshold ());
+ value_type eps2 (eps * eps);
+ if ((rhsGoal - rhsInit).squaredNorm () > eps2) {
+ return false;
}
-
- void addConstraints (const StatePtr_t& state, const std::size_t& i)
- {
- bool alreadyAdded = (
- std::find (statesPerConf_[i].begin(), statesPerConf_[i].end(), state)
- != statesPerConf_[i].end());
- if (alreadyAdded) return;
- _add (state->lockedJoints(), i);
- _add (state->numericalConstraints(), i);
- statesPerConf_[i].push_back(state);
+ // Matrix of solver right hand sides
+ for (size_type j=0; j<d.M_rhs.cols (); ++j) {
+ d.M_rhs (index, j) = rhsInit;
}
+ return true;
+ }
- void addConstraints (const EdgePtr_t& trans, const std::size_t& i)
- {
- const LockedJoints_t& ljs = trans->lockedJoints();
- for (LockedJoints_t::const_iterator _lj = ljs.begin();
- _lj != ljs.end(); ++_lj) {
- LockedJointPtr_t lj (*_lj);
- std::ostringstream os;
- os << lj->jointName() << " | " << i << " -> " << (i+1);
- DifferentiableFunctionPtr_t f, f_implicit;
- ImplicitPtr_t c_implicit;
- // i = Input, o = Output,
- // c = Config, v = Velocity
- RowBlockIndices ic, oc, ov;
- ColBlockIndices iv;
- ComparisonTypes_t cts;
- vector_t rhs;
- if (i == 0) {
- f = lj->explicitFunction();
- ic = _row(lj->inputConf() , 0);
- oc = _row(lj->outputConf() , 0);
- iv = _col(lj->inputVelocity() , 0);
- ov = _row(lj->outputVelocity(), 0);
- cts = lj->comparisonType();
- lj->rightHandSideFromConfig (q1);
- rhs = lj->rightHandSide();
- } else if (i == N) {
- f = lj->explicitFunction();
- // Currently, this function is mandatory because if the same
- // joint is locked all along the path, then, one of the LockedJoint
- // has to be treated implicitely.
- // TODO it would be smarter to detect this case beforehand. If the
- // chain in broken in the middle, an explicit formulation exists
- // (by inverting the equality in the next else section) and we
- // miss it.
- f_implicit = _stateFunction (lj->functionPtr(), N-1);
- ic = _row(lj->inputConf() , 0);
- oc = _row(lj->outputConf() , (N-1) * nq);
- iv = _col(lj->inputVelocity() , 0);
- ov = _row(lj->outputVelocity(), (N-1) * nv);
- cts = lj->comparisonType();
- lj->rightHandSideFromConfig (q2);
- rhs = lj->rightHandSide();
- } else {
- f = constraints::Identity::create (lj->configSpace(), os.str());
- ic = _row(lj->outputConf() , (i - 1) * nq);
- oc = _row(lj->outputConf() , i * nq);
- iv = _col(lj->outputVelocity(), (i - 1) * nv);
- ov = _row(lj->outputVelocity(), i * nv);
- cts = ComparisonTypes_t (lj->numberDof(), constraints::EqualToZero);
- }
-
- // It is important to use the index of the function since the same
- // function may be added several times on different part.
- constraints::ExplicitPtr_t ec
- (constraints::Explicit::create (solver.configSpace (), f,
- ic.indices (), oc.indices (),
- iv.indices (), ov.indices (),
- cts));
- size_type expFidx = solver.explicitConstraintSet().add (ec);
- if (expFidx < 0) {
- if (f_implicit) {
- c_implicit = constraints::Implicit::create (f_implicit, cts);
- solver.add (c_implicit);
- } else {
- HPP_THROW (std::invalid_argument,
- "Could not add locked joint " << lj->jointName() <<
- " of transition " << trans->name() << " at id " << i);
- }
+ void displayRhsMatrix (const Eigen::Matrix < vector_t, Eigen::Dynamic,
+ Eigen::Dynamic >& m,
+ const NumericalConstraints_t& constraints)
+ {
+ std::ostringstream oss; oss.precision (5);
+ oss << "\\documentclass[12pt,landscape]{article}" << std::endl;
+ oss << "\\usepackage[a3paper]{geometry}" << std::endl;
+ oss << "\\begin {document}" << std::endl;
+ oss << "\\begin {tabular}{";
+ for (size_type j=0; j<m.cols () + 1; ++j)
+ oss << "c";
+ oss << "}" << std::endl;
+ for (size_type i=0; i<m.rows (); ++i) {
+ oss << constraints [i]->function ().name () << " & ";
+ for (size_type j=0; j<m.cols (); ++j) {
+ oss << "$\\left(\\begin{array}{c}" << std::endl;
+ for (size_type k=0; k<m (i,j).size (); ++k) {
+ oss << m (i,j) [k] << "\\\\" << std::endl;
}
-
- // Setting the right hand side must be done later
- if (rhs.size() > 0) {
- assert (c_implicit);
- rhsSetters_.push_back
- (RightHandSideSetter(c_implicit, expFidx, rhs));
+ oss << "\\end{array}\\right)$" << std::endl;
+ if (j < m.cols () - 1) {
+ oss << " & " << std::endl;
}
- f_implicit.reset();
- c_implicit.reset();
}
+ oss << "\\\\" << std::endl;
+ }
+ oss << "\\end{tabular}" << std::endl;
+ oss << "\\end {document}" << std::endl;
+ hppDout (info, oss.str ());
+ }
- // TODO handle numerical constraints
- using namespace ::hpp::core;
- constraints::ExplicitPtr_t enc;
- const NumericalConstraints_t& ncs = trans->numericalConstraints();
- for (NumericalConstraints_t::const_iterator _nc = ncs.begin();
- _nc != ncs.end(); ++_nc) {
- constraints::ImplicitPtr_t nc (*_nc);
- enc = HPP_DYNAMIC_PTR_CAST (constraints::Explicit, nc);
-
- DifferentiableFunctionPtr_t f, ef;
- constraints::ImplicitPtr_t constraint;
- // i = Input, o = Output,
- // c = Config, v = Velocity
- // Configuration_t* qrhs;
- RowBlockIndices ic, oc, ov;
- ColBlockIndices iv;
- ComparisonTypes_t cts;
- vector_t rhs;
- if (i == 0) {
- f = _stateFunction(nc->functionPtr(), 0);
- // if (enc) {
- // ef = enc->explicitFunction();
- // ic = _row(enc->inputConf() , 0);
- // oc = _row(enc->outputConf() , 0);
- // iv = _col(enc->inputVelocity() , 0);
- // ov = _row(enc->outputVelocity(), 0);
- // }
- // cts = ComparisonTypes_t (enc->outputDerivativeSize(), constraints::Equality);
- cts = nc->comparisonType ();
- nc->rightHandSideFromConfig (q1);
- rhs = nc->rightHandSide();
- // qrhs = &q1;
- } else if (i == N) {
- f = _stateFunction(nc->functionPtr(), N - 1);
- cts = nc->comparisonType ();
- nc->rightHandSideFromConfig (q2);
- rhs = nc->rightHandSide();
- // qrhs = &q2;
- } else {
- f = _edgeFunction (nc->functionPtr(), i - 1, i);
- cts = ComparisonTypes_t (f->outputSize(), constraints::EqualToZero);
- // qrhs = NULL;
- }
- constraint = constraints::Implicit::create (f, cts);
- size_type expFidx = -1;
- // if (ef) {
- // constraints::ExplicitPtr_t ec
- // (constraints::Explicit::create (solver.configSpace (), ef,
- // ic.indices (), oc.indices (),
- // iv.indices (), ov.indices (),
- // cts));
- // expFidx = solver.explicitConstraintSet().add (ec);
- // }
- if (expFidx < 0) solver.add (constraint);
-
- // TODO This must be done later...
- // if (qrhs != NULL) {
- if (rhs.size() > 0) {
- // solver.rightHandSideFromConfig (f, ef, rhs);
- rhsSetters_.push_back (RightHandSideSetter
- (constraint, expFidx, rhs));
+ void displayStatusMatrix (const Eigen::Matrix < CrossStateOptimization::OptimizationData::RightHandSideStatus_t, Eigen::Dynamic, Eigen::Dynamic >& m,
+ const NumericalConstraints_t& constraints)
+ {
+ std::ostringstream oss; oss.precision (5);
+ oss << "\\documentclass[12pt,landscape]{article}" << std::endl;
+ oss << "\\usepackage[a3paper]{geometry}" << std::endl;
+ oss << "\\begin {document}" << std::endl;
+ oss << "\\begin {tabular}{";
+ for (size_type j=0; j<m.cols () + 1; ++j)
+ oss << "c";
+ oss << "}" << std::endl;
+ for (size_type i=0; i<m.rows (); ++i) {
+ oss << constraints [i]->function ().name () << " & ";
+ for (size_type j=0; j<m.cols (); ++j) {
+ oss << m (i,j);
+ if (j < m.cols () - 1) {
+ oss << " & " << std::endl;
}
}
+ oss << "\\\\" << std::endl;
}
+ oss << "\\end{tabular}" << std::endl;
+ oss << "\\end {document}" << std::endl;
+ hppDout (info, oss.str ());
+ }
- void setRightHandSide ()
- {
- for (std::size_t i = 0; i < rhsSetters_.size(); ++i)
- rhsSetters_[i].apply(solver);
- }
-
- bool solve ()
- {
- if (N == 0) return true;
- constraints::HierarchicalIterativeSolver::Status status =
- solver.solve <constraints::solver::lineSearch::FixedSequence> (q);
- bool success = (status == constraints::HierarchicalIterativeSolver::SUCCESS);
- if (!success) {
- hppDout (warning, "Projection failed with status " << status <<
- ". Configuration after projection is\n"
- << pinocchio::displayConfig(q));
- }
- return success;
+ bool CrossStateOptimization::contains
+ (const Solver_t& solver, const ImplicitPtr_t& c) const
+ {
+ if (solver.contains (c)) return true;
+ std::map <ImplicitPtr_t, ImplicitPtr_t>::const_iterator it
+ (sameRightHandSide_.find (c));
+ if ((it != sameRightHandSide_.end () &&
+ solver.contains (it->second))) {
+ return true;
}
+ return false;
+ }
- void _add (const NumericalConstraints_t& ncs, const std::size_t& i)
- {
- using namespace ::hpp::core;
- constraints::ExplicitPtr_t enc;
- for (NumericalConstraints_t::const_iterator _nc = ncs.begin();
- _nc != ncs.end(); ++_nc) {
- constraints::ImplicitPtr_t nc (*_nc);
- enc = HPP_DYNAMIC_PTR_CAST (constraints::Explicit, nc);
- bool added = false;
- if (enc) {
- added = solver.explicitConstraintSet().add
- (explicitConstraintOnWaypoint (enc, i));
+ bool CrossStateOptimization::buildOptimizationProblem
+ (OptimizationData& d, const graph::Edges_t& transitions) const
+ {
+ if (d.N == 0) return true;
+ d.M_status.resize (constraints_.size (), d.N);
+ d.M_status.fill (OptimizationData::ABSENT);
+ d.M_rhs.resize (constraints_.size (), d.N);
+ d.M_rhs.fill (vector_t ());
+ size_type index = 0;
+ // Loop over constraints
+ for (NumericalConstraints_t::const_iterator it (constraints_.begin ());
+ it != constraints_.end (); ++it) {
+ const ImplicitPtr_t& c (*it);
+ // Loop forward over waypoints to determine right hand sides equal
+ // to initial configuration
+ for (std::size_t j = 0; j < d.N; ++j) {
+ // Get transition solver
+ const Solver_t& trSolver
+ (transitions [j]->pathConstraint ()->configProjector ()->
+ solver ());
+ if (contains (trSolver, c)) {
+ if ((j==0) || d.M_status (index, j-1) ==
+ OptimizationData::EQUAL_TO_INIT) {
+ d.M_status (index, j) = OptimizationData::EQUAL_TO_INIT;
+ } else {
+ d.M_status (index, j) = OptimizationData::EQUAL_TO_PREVIOUS;
+ }
}
- if (!added)
- solver.add (implicitConstraintOnWayPoint (nc, i));
- }
- }
- void _add (const LockedJoints_t& ljs, const std::size_t i)
- {
- for (LockedJoints_t::const_iterator _lj = ljs.begin();
- _lj != ljs.end(); ++_lj) {
- LockedJointPtr_t lj (*_lj);
- size_type expFidx = solver.explicitConstraintSet().add
- (explicitConstraintOnWaypoint (lj, i));
- if (expFidx < 0)
- throw std::invalid_argument ("Could not add locked joint " + lj->jointName());
-
- // This must be done later
- rhsSetters_.push_back
- (RightHandSideSetter(constraints::Implicit::create
- (DifferentiableFunctionPtr_t()),
- expFidx,
- lj->rightHandSide()));
- // solver.rightHandSide (DifferentiableFunctionPtr_t(),
- // lj->explicitFunction(),
- // lj->rightHandSide());
}
- }
-
- /// Apply constraint on a give waypoint
- /// \param constaint explicit constraint defined on the configuration
- /// space,
- /// \param i rank of the waypoint in the vector of parameters.
- constraints::ExplicitPtr_t explicitConstraintOnWaypoint
- (const constraints::ExplicitPtr_t& constraint,
- std::size_t i)
- {
- assert (i < N);
- // size_type nq (constraint->explicitFunction ().inputSize ()),
- // nv (constraint->function ().inputDerrivativeSize ());
- segments_t ic (constraint->inputConf ()),
- oc (constraint->outputConf ()),
- iv (constraint->inputVelocity ()),
- ov (constraint->outputVelocity ());
- // TODO: implicit formulation should be built from input constraint
- return constraints::Explicit::create
- (solver.configSpace (), constraint->functionPtr (),
- _row (ic, i*nq).indices (), _row (oc, i*nq).indices (),
- _col (iv, i*nv).indices (), _col (ov, i*nv).indices (),
- constraint->comparisonType ());
- }
-
- RowBlockIndices _row (segments_t s, const std::size_t& shift)
- {
- for (std::size_t j = 0; j < s.size(); ++j) s[j].first += shift;
- return RowBlockIndices (s);
- }
- ColBlockIndices _col (segments_t s, const std::size_t& shift)
- {
- for (std::size_t j = 0; j < s.size(); ++j) s[j].first += shift;
- return ColBlockIndices (s);
- }
- constraints::ImplicitPtr_t implicitConstraintOnWayPoint
- (const constraints::ImplicitPtr_t& constraint, std::size_t i)
- {
- assert (i < N);
- ImplicitPtr_t res (constraints::Implicit::create
- (StateFunction::Ptr_t
- (new StateFunction (constraint->functionPtr (),
- N * nq, N * nv,
- segment_t (i * nq, nq),
- segment_t (i * nv, nv))),
- constraint->comparisonType ()));
- return res;
- }
- StateFunction::Ptr_t _stateFunction(const DifferentiableFunctionPtr_t inner, const std::size_t i)
- {
- assert (i < N);
- return StateFunction::Ptr_t (new StateFunction (inner, N * nq, N * nv,
- segment_t (i * nq, nq), segment_t (i * nv, nv)
- ));
- }
- EdgeFunction::Ptr_t _edgeFunction(const DifferentiableFunctionPtr_t inner,
- const std::size_t iL, const std::size_t iR)
- {
- assert (iL < N && iR < N);
- return EdgeFunction::Ptr_t (new EdgeFunction (inner, N * nq, N * nv,
- segment_t (iL * nq, nq), segment_t (iL * nv, nv),
- segment_t (iR * nq, nq), segment_t (iR * nv, nv)));
- }
- void _integrate (vectorIn_t qin, vectorIn_t v, vectorOut_t qout)
- {
- for (std::size_t i = 0, iq = 0, iv = 0; i < N; ++i, iq += nq, iv += nv)
- pinocchio::integrate (robot,
- qin.segment(iq,nq),
- v.segment(iv,nv),
- qout.segment(iq,nq));
- }
- bool _saturate (vectorIn_t q, vectorOut_t qSat, Eigen::VectorXi& sat)
- {
- bool ret = false;
- const pinocchio::Model& model = robot->model();
-
- for (std::size_t i = 1; i < model.joints.size(); ++i) {
- const size_type jnq = model.joints[i].nq();
- const size_type jnv = model.joints[i].nv();
- const size_type jiq = model.joints[i].idx_q();
- const size_type jiv = model.joints[i].idx_v();
- for (std::size_t k = 0; k < N; ++k) {
- const size_type idx_q = k * nq + jiq;
- const size_type idx_v = k * nv + jiv;
- for (size_type j = 0; j < jnq; ++j) {
- const size_type iq = idx_q + j;
- const size_type iv = idx_v + std::min(j,jnv-1);
- if (q[iq] >= model.upperPositionLimit[jiq + j]) {
- qSat [iq] = model.upperPositionLimit[jiq + j];
- sat[iv] = 1;
- ret = true;
- } else if (q[iq] <= model.lowerPositionLimit[jiq + j]) {
- qSat [iq] = model.lowerPositionLimit[jiq + j];
- sat[iv] = -1;
- ret = true;
- } else
- qSat [iq] = q [iq];
- sat[iv] = 0;
+ // Loop backward over waypoints to determine right hand sides equal
+ // to initial configuration
+ for (size_type j = d.N-1; j > 0; --j) {
+ // Get transition solver
+ const Solver_t& trSolver
+ (transitions [(std::size_t)j+1]->pathConstraint ()->
+ configProjector ()->solver ());
+ if (contains (trSolver, c)) {
+ if ((j==(size_type) d.N-1) || d.M_status (index, j+1) ==
+ OptimizationData::EQUAL_TO_GOAL) {
+ // If constraint right hand side is already equal to
+ // initial config, check that right hand side is equal
+ // for init and goal configs.
+ if (d.M_status (index, j) ==
+ OptimizationData::EQUAL_TO_INIT) {
+ if (checkConstantRightHandSide (d, index)) {
+ // stop for this constraint
+ break;
+ } else {
+ // Right hand side of constraint should be equal along the
+ // whole path but is different at init and at goal configs.
+ return false;
+ }
+ } else {
+ d.M_status (index, j) = OptimizationData::EQUAL_TO_GOAL;
+ }
}
}
}
-
- const hpp::pinocchio::ExtraConfigSpace& ecs = robot->extraConfigSpace();
- const size_type& d = ecs.dimension();
-
- for (size_type k = 0; k < d; ++k) {
- for (std::size_t j = 0; j < N; ++j) {
- const size_type iq = j * nq + model.nq + k;
- const size_type iv = j * nv + model.nv + k;
- if (q[iq] >= ecs.upper(k)) {
- sat[iv] = 1;
- ret = true;
- } else if (q[iq] <= ecs.lower(k)) {
- sat[iv] = -1;
- ret = true;
- } else
- sat[iv] = 0;
+ ++index;
+ } // for (NumericalConstraints_t::const_iterator it
+ displayStatusMatrix (d.M_status, constraints_);
+ graph::GraphPtr_t cg (problem_.constraintGraph ());
+ // Fill solvers with graph, node and edge constraints
+ for (std::size_t j = 0; j < d.N; ++j) {
+ graph::StatePtr_t state (transitions [(std::size_t)j]->to ());
+ // initialize solver with state constraints
+ d.solvers [(std::size_t)j] = state->configConstraint ()->
+ configProjector ()->solver ();
+ // Add graph constraints
+ const NumericalConstraints_t c (cg->numericalConstraints ());
+ for (NumericalConstraints_t::const_iterator it (c.begin ());
+ it != c.end (); ++it) {
+ d.solvers [(std::size_t)j].add (*it);
+ }
+ // Add edge constraints
+ for (std::size_t i=0; i<constraints_.size (); ++i) {
+ if (d.M_status (i, j) != OptimizationData::ABSENT) {
+ d.solvers [(std::size_t)j].add (constraints_ [i]);
}
}
- return ret;
}
- };
+ return true;
+ }
- void CrossStateOptimization::buildOptimizationProblem (
- OptimizationData& d, const Edges_t& transitions) const
+ bool CrossStateOptimization::solveOptimizationProblem
+ (OptimizationData& d) const
{
- if (d.N == 0) return;
- size_type maxIter = problem_.getParameter ("CrossStateOptimization/maxIteration").intValue();
- value_type thr = problem_.getParameter ("CrossStateOptimization/errorThreshold").floatValue();
- d.solver.maxIterations (maxIter);
- d.solver.errorThreshold (thr);
-
- // Add graph constraints (decoupled)
- d.addGraphConstraints (problem_.constraintGraph());
-
- std::size_t i = 0;
- for (Edges_t::const_iterator _t = transitions.begin();
- _t != transitions.end(); ++_t)
- {
- const EdgePtr_t& t = *_t;
- bool first = (i == 0);
- bool last = (i == d.N);
-
- // Add state constraints (decoupled)
- if (!last ) d.addConstraints (t->to() , i);
- if (!last ) d.addConstraints (t->state(), i);
- if (!first) d.addConstraints (t->from() , i - 1);
- if (!first) d.addConstraints (t->state(), i - 1);
-
- // Add transition constraints (coupled)
- d.addConstraints (t, i);
-
- ++i;
- }
-
- d.solver.explicitConstraintSetHasChanged();
- d.setRightHandSide();
-
- hppDout (info, "Solver informations\n" << d.solver);
-
- // Initial guess
- std::vector<size_type> ks;
- size_type K = 0;
- ks.resize(d.N);
- for (std::size_t i = 0; i < d.N + 1; ++i) {
- if (!transitions[i]->isShort()) ++K;
- if (i < d.N) ks[i] = K;
- }
- if (K==0) {
- ++K;
- for (std::size_t i = d.N/2; i < d.N; ++i)
- ks[i] = 1;
- }
- d.q.resize (d.N * d.nq);
- for (std::size_t i = 0; i < d.N; ++i) {
- value_type u = value_type(ks[i]) / value_type(K);
- pinocchio::interpolate (d.robot, d.q1, d.q2, u, d.q.segment (i * d.nq, d.nq));
+ // Iterate on waypoint solvers, for each of them
+ // 1. initialize right hand side,
+ // 2. solve.
+ for (std::size_t j=0; j<d.solvers.size (); ++j) {
+ Solver_t& solver (d.solvers [j]);
+ for (std::size_t i=0; i<constraints_.size (); ++i) {
+ const ImplicitPtr_t& c (constraints_ [i]);
+ switch (d.M_status ((size_type)i, (size_type)j)) {
+ case OptimizationData::EQUAL_TO_PREVIOUS:
+ assert (j != 0);
+ solver.rightHandSideFromConfig (c, d.waypoint.col (j-1));
+ break;
+ case OptimizationData::EQUAL_TO_INIT:
+ solver.rightHandSideFromConfig (c, d.q1);
+ break;
+ case OptimizationData::EQUAL_TO_GOAL:
+ solver.rightHandSideFromConfig (c, d.q2);
+ break;
+ case OptimizationData::ABSENT:
+ default:
+ ;
+ }
+ }
+ if (j==0) d.waypoint.col (j) = d.q1;
+ else d.waypoint.col (j) = d.waypoint.col (j-1);
+ Solver_t::Status status = solver.solve
+ (d.waypoint.col (j),
+ constraints::solver::lineSearch::Backtracking ());
+
+ switch (status) {
+ case Solver_t::ERROR_INCREASED:
+ hppDout (info, "error increased.");
+ return false;
+ case Solver_t::MAX_ITERATION_REACHED:
+ hppDout (info, "max iteration reached.");
+ return false;
+ case Solver_t::INFEASIBLE:
+ hppDout (info, "infeasible.");
+ return false;
+ case Solver_t::SUCCESS:
+ hppDout (info, "success.");
+ }
}
+ return true;
}
core::PathVectorPtr_t CrossStateOptimization::buildPath (
@@ -680,17 +565,15 @@ namespace hpp {
if (first && last)
status = t->build (path, d.q1, d.q2);
else if (first)
- status = t->build (path, d.q1, d.q.head (d.nq));
+ status = t->build (path, d.q1, d.waypoint.col (i));
else if (last)
- status = t->build (path, d.q.tail (d.nq), d.q2);
+ status = t->build (path, d.waypoint.col (i-1), d.q2);
else {
- std::size_t j = (i-1) * d.nq;
- status = t->build (path, d.q.segment (j, d.nq), d.q.segment (j + d.nq, d.nq));
+ status = t->build (path, d.waypoint.col (i-1), d.waypoint.col (i));
}
if (!status || !path) {
- hppDout (warning, "Could not build path from solution "
- << pinocchio::displayConfig(d.q));
+ hppDout (warning, "Could not build path from solution ");
return PathVectorPtr_t();
}
pv->appendPath(path);
@@ -726,26 +609,16 @@ namespace hpp {
hppDout (info, ss.str());
#endif // HPP_DEBUG
- OptimizationData optData (transitions.size() - 1, problem().robot());
- optData.q1 = q1;
- optData.q2 = q2;
- bool ok = true;
- try {
- buildOptimizationProblem (optData, transitions);
- } catch (const std::invalid_argument& e) {
- hppDout (info, e.what ());
- ok = false;
- }
-
- if (ok && optData.solve()) {
- core::PathPtr_t path = buildPath (optData, transitions);
- if (path) return path;
- hppDout (info, "Failed to build path from solution: "
- << pinocchio::displayConfig(optData.q));
- } else {
- hppDout (info, "Failed to solve");
+ OptimizationData optData (problem().robot(), q1, q2, transitions);
+ if (buildOptimizationProblem (optData, transitions)) {
+ if (solveOptimizationProblem (optData)) {
+ core::PathPtr_t path = buildPath (optData, transitions);
+ if (path) return path;
+ hppDout (info, "Failed to build path from solution: ");
+ } else {
+ hppDout (info, "Failed to solve");
+ }
}
-
++idxSol;
transitions = getTransitionList(d, idxSol);
}
diff --git a/src/steering-method/cross-state-optimization/function.cc b/src/steering-method/cross-state-optimization/function.cc
deleted file mode 100644
index 10443ef1030de854a4f4b9cb0aaeb202857f5fb2..0000000000000000000000000000000000000000
--- a/src/steering-method/cross-state-optimization/function.cc
+++ /dev/null
@@ -1,144 +0,0 @@
-// Copyright (c) 2017, Joseph Mirabel
-// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
-//
-// This file is part of hpp-manipulation.
-// hpp-manipulation is free software: you can redistribute it
-// and/or modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation, either version
-// 3 of the License, or (at your option) any later version.
-//
-// hpp-manipulation is distributed in the hope that it will be
-// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
-// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
-// General Lesser Public License for more details. You should have
-// received a copy of the GNU Lesser General Public License along with
-// hpp-manipulation. If not, see <http://www.gnu.org/licenses/>.
-
-#include <hpp/constraints/differentiable-function.hh>
-
-#include <hpp/manipulation/config.hh>
-
-namespace hpp {
- namespace manipulation {
- namespace steeringMethod {
- typedef core::segment_t segment_t;
- namespace {
- std::string toStr (const segment_t& s)
- {
- std::ostringstream os;
- os << "[ " << s.first << ", " << s.first + s.second << " ]";
- return os.str();
- }
- }
-
- /// Apply the constraint on a subspace of the input space.
- /// i.e.: \f$ f (q_0, ... , q_n) = f_{inner} (q_k) \f$
- class HPP_MANIPULATION_LOCAL StateFunction :
- public constraints::DifferentiableFunction
- {
- public:
- typedef boost::shared_ptr<StateFunction> Ptr_t;
- StateFunction (const DifferentiableFunctionPtr_t& inner,
- const size_type& nArgs, const size_type& nDers,
- const segment_t& inArgs, const segment_t& inDers) :
- DifferentiableFunction (nArgs, nDers,
- inner->outputSpace(), inner->name() + " | " + toStr(inArgs)),
- inner_ (inner), sa_ (inArgs), sd_ (inDers)
- {
- activeParameters_.setConstant(false);
- activeParameters_.segment(sa_.first, sa_.second)
- = inner_->activeParameters();
-
- activeDerivativeParameters_.setConstant(false);
- activeDerivativeParameters_.segment(sd_.first, sd_.second)
- = inner_->activeDerivativeParameters();
- }
-
- protected:
- void impl_compute (pinocchio::LiegroupElementRef y, vectorIn_t arg) const
- {
- inner_->value(y, arg.segment (sa_.first, sa_.second));
- }
-
- void impl_jacobian (matrixOut_t J, vectorIn_t arg) const
- {
- inner_->jacobian(J.middleCols (sd_.first, sd_.second),
- arg.segment (sa_.first, sa_.second));
- }
-
- std::ostream& print (std::ostream& os) const
- {
- constraints::DifferentiableFunction::print(os);
- return os << incindent << iendl << *inner_ << decindent;
- }
-
- DifferentiableFunctionPtr_t inner_;
- const segment_t sa_, sd_;
- }; // class Function
-
- /// Compute the difference between the value of the function in two points.
- /// i.e.: \f$ f (q_0, ... , q_n) = f_{inner} (q_{left}) - f_{inner} (q_{right}) \f$
- class HPP_MANIPULATION_LOCAL EdgeFunction :
- public constraints::DifferentiableFunction
- {
- public:
- typedef boost::shared_ptr<EdgeFunction> Ptr_t;
- EdgeFunction (const DifferentiableFunctionPtr_t& inner,
- const size_type& nArgs, const size_type& nDers,
- const segment_t& lInArgs, const segment_t& lInDers,
- const segment_t& rInArgs, const segment_t& rInDers) :
- DifferentiableFunction (nArgs, nDers,
- LiegroupSpace::Rn(inner->outputSpace()->nv()),
- inner->name() + " | " + toStr(lInArgs) + " - " + toStr(rInArgs)),
- inner_ (inner),
- lsa_ (lInArgs), lsd_ (lInDers),
- rsa_ (rInArgs), rsd_ (rInDers),
- l_ (inner->outputSpace()), r_ (inner->outputSpace())
- {
- activeParameters_.setConstant(false);
- activeParameters_.segment(lsa_.first, lsa_.second)
- = inner_->activeParameters();
- activeParameters_.segment(rsa_.first, rsa_.second)
- = inner_->activeParameters();
-
- activeDerivativeParameters_.setConstant(false);
- activeDerivativeParameters_.segment(lsd_.first, lsd_.second)
- = inner_->activeDerivativeParameters();
- activeDerivativeParameters_.segment(rsd_.first, rsd_.second)
- = inner_->activeDerivativeParameters();
- }
-
- protected:
- void impl_compute (pinocchio::LiegroupElementRef y, vectorIn_t arg) const
- {
- inner_->value(l_, arg.segment (lsa_.first, lsa_.second));
- inner_->value(r_, arg.segment (rsa_.first, rsa_.second));
- y.vector() = l_ - r_;
- }
-
- void impl_jacobian (matrixOut_t J, vectorIn_t arg) const
- {
- inner_->jacobian(
- J.middleCols (lsd_.first, lsd_.second),
- arg.segment (lsa_.first, lsa_.second));
- inner_->jacobian(
- J.middleCols (rsd_.first, rsd_.second),
- arg.segment (rsa_.first, rsa_.second));
- J.middleCols (rsd_.first, rsd_.second) *= -1;
- }
-
- std::ostream& print (std::ostream& os) const
- {
- constraints::DifferentiableFunction::print(os);
- return os << incindent << iendl << *inner_ << decindent;
- }
-
- DifferentiableFunctionPtr_t inner_;
- const segment_t lsa_, lsd_;
- const segment_t rsa_, rsd_;
-
- mutable LiegroupElement l_, r_;
- }; // class Function
- } // namespace steeringMethod
- } // namespace manipulation
-} // namespace hpp