diff --git a/CMakeLists.txt b/CMakeLists.txt
index 2295f4fd5e59af1cad64a7cfd1c30a6501953f02..edfe0750adfd6516e1115effbb2f90ba1cbb2563 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -107,6 +107,9 @@ SET (${PROJECT_NAME}_HEADERS
include/hpp/manipulation/path-optimization/spline-gradient-based.hh
include/hpp/manipulation/problem-target/state.hh
+
+ include/hpp/manipulation/steering-method/cross-state-optimization.hh
+ include/hpp/manipulation/steering-method/fwd.hh
)
ADD_SUBDIRECTORY(src)
diff --git a/include/hpp/manipulation/fwd.hh b/include/hpp/manipulation/fwd.hh
index a7432aa41d7c7bf69bc528785dddc0a5bcb60610..360ce8e9ae01d5d5e96810237cf04e6f59e9fcd1 100644
--- a/include/hpp/manipulation/fwd.hh
+++ b/include/hpp/manipulation/fwd.hh
@@ -86,7 +86,9 @@ namespace hpp {
typedef boost::shared_ptr < SymbolicPlanner > SymbolicPlannerPtr_t;
HPP_PREDEF_CLASS (GraphPathValidation);
typedef boost::shared_ptr < GraphPathValidation > GraphPathValidationPtr_t;
+ HPP_PREDEF_CLASS (SteeringMethod);
HPP_PREDEF_CLASS (GraphSteeringMethod);
+ typedef boost::shared_ptr < SteeringMethod > SteeringMethodPtr_t;
typedef boost::shared_ptr < GraphSteeringMethod > GraphSteeringMethodPtr_t;
typedef core::PathOptimizer PathOptimizer;
typedef core::PathOptimizerPtr_t PathOptimizerPtr_t;
@@ -125,8 +127,6 @@ namespace hpp {
typedef core::vector3_t vector3_t;
typedef core::matrix3_t matrix3_t;
- typedef std::list < NumericalConstraintPtr_t > NumericalConstraints_t;
-
typedef core::Shape_t Shape_t;
typedef core::JointAndShape_t JointAndShape_t;
typedef core::JointAndShapes_t JointAndShapes_t;
diff --git a/include/hpp/manipulation/graph-steering-method.hh b/include/hpp/manipulation/graph-steering-method.hh
index 42d96c6b6ece8f833431317f6685c75dea20b320..d21d7a46184d737c5c77a5ede6e303c35754a6c7 100644
--- a/include/hpp/manipulation/graph-steering-method.hh
+++ b/include/hpp/manipulation/graph-steering-method.hh
@@ -27,11 +27,41 @@
namespace hpp {
namespace manipulation {
- using core::SteeringMethod;
using core::PathPtr_t;
/// \addtogroup steering_method
/// \{
+ class HPP_MANIPULATION_DLLAPI SteeringMethod : public core::SteeringMethod
+ {
+ public:
+ const core::SteeringMethodPtr_t& innerSteeringMethod () const
+ {
+ return steeringMethod_;
+ }
+
+ void innerSteeringMethod (const core::SteeringMethodPtr_t& sm)
+ {
+ steeringMethod_ = sm;
+ }
+
+ protected:
+ /// Constructor
+ SteeringMethod (const Problem& problem);
+
+ /// Copy constructor
+ SteeringMethod (const SteeringMethod& other);
+
+ void init (SteeringMethodWkPtr_t weak)
+ {
+ core::SteeringMethod::init (weak);
+ }
+
+ /// A pointer to the manipulation problem
+ const Problem& problem_;
+ /// The encapsulated steering method
+ core::SteeringMethodPtr_t steeringMethod_;
+ };
+
class HPP_MANIPULATION_DLLAPI GraphSteeringMethod : public SteeringMethod
{
typedef core::SteeringMethodBuilder_t SteeringMethodBuilder_t;
@@ -59,16 +89,6 @@ namespace hpp {
return createCopy (weak_.lock ());
}
- const core::SteeringMethodPtr_t& innerSteeringMethod () const
- {
- return steeringMethod_;
- }
-
- void innerSteeringMethod (const core::SteeringMethodPtr_t& sm)
- {
- steeringMethod_ = sm;
- }
-
protected:
/// Constructor
GraphSteeringMethod (const Problem& problem);
@@ -80,17 +100,13 @@ namespace hpp {
void init (GraphSteeringMethodWkPtr_t weak)
{
- core::SteeringMethod::init (weak);
+ SteeringMethod::init (weak);
weak_ = weak;
}
private:
- /// A pointer to the problem
- const Problem& problem_;
/// Weak pointer to itself
GraphSteeringMethodWkPtr_t weak_;
- /// The encapsulated steering method
- core::SteeringMethodPtr_t steeringMethod_;
};
template <typename T>
diff --git a/include/hpp/manipulation/problem.hh b/include/hpp/manipulation/problem.hh
index 6b4b05ef3b13c26e010f6ea4dceba1a8fcb9925d..c7c318b5db32f81ed8300fe833845b1ad5cda92a 100644
--- a/include/hpp/manipulation/problem.hh
+++ b/include/hpp/manipulation/problem.hh
@@ -64,7 +64,7 @@ namespace hpp {
void pathValidation (const PathValidationPtr_t& pathValidation);
/// Get the steering method as a GraphSteeringMethod
- GraphSteeringMethodPtr_t steeringMethod () const;
+ SteeringMethodPtr_t steeringMethod () const;
/// Build a new path validation
/// \note Current obstacles are added to the created object.
diff --git a/include/hpp/manipulation/steering-method/cross-state-optimization.hh b/include/hpp/manipulation/steering-method/cross-state-optimization.hh
new file mode 100644
index 0000000000000000000000000000000000000000..14293f6128a0730d85c4a18fe4381c709a94aa81
--- /dev/null
+++ b/include/hpp/manipulation/steering-method/cross-state-optimization.hh
@@ -0,0 +1,84 @@
+// 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/>.
+
+#ifndef HPP_MANIPULATION_STEERING_METHOD_CROSS_STATE_OPTIMIZATION_HH
+# define HPP_MANIPULATION_STEERING_METHOD_CROSS_STATE_OPTIMIZATION_HH
+
+# include <hpp/core/steering-method.hh>
+
+# include <hpp/manipulation/config.hh>
+# include <hpp/manipulation/fwd.hh>
+# include <hpp/manipulation/problem.hh>
+# include <hpp/manipulation/steering-method/fwd.hh>
+# include <hpp/manipulation/graph-steering-method.hh>
+
+namespace hpp {
+ namespace manipulation {
+ namespace steeringMethod {
+ class HPP_MANIPULATION_DLLAPI CrossStateOptimization :
+ public SteeringMethod
+ {
+ public:
+ static CrossStateOptimizationPtr_t create (const Problem& problem);
+
+ /// \warning core::Problem will be casted to Problem
+ static CrossStateOptimizationPtr_t createFromCore
+ (const core::Problem& problem);
+
+ core::SteeringMethodPtr_t copy () const;
+
+ protected:
+ CrossStateOptimization (const Problem& problem) :
+ SteeringMethod (problem)
+ {}
+
+ CrossStateOptimization (const CrossStateOptimization& other) :
+ SteeringMethod (other),
+ weak_ ()
+ {}
+
+ core::PathPtr_t impl_compute (ConfigurationIn_t q1, ConfigurationIn_t q2) const;
+
+ void init (CrossStateOptimizationWkPtr_t weak)
+ {
+ SteeringMethod::init (weak);
+ weak_ = weak;
+ }
+
+ private:
+ struct GraphSearchData;
+ struct OptimizationData;
+
+ /// Step 1 of the algorithm
+ /// \return whether the max depth was reached.
+ bool findTransitions (GraphSearchData& data) const;
+
+ /// Step 2 of the algorithm
+ 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;
+
+ core::PathVectorPtr_t buildPath (OptimizationData& d, const graph::Edges_t& edges) const;
+
+ /// Weak pointer to itself
+ CrossStateOptimizationWkPtr_t weak_;
+ }; // class CrossStateOptimization
+ } // namespace steeringMethod
+ } // namespace manipulation
+} // namespace hpp
+
+#endif // HPP_MANIPULATION_STEERING_METHOD_CROSS_STATE_OPTIMIZATION_HH
diff --git a/include/hpp/manipulation/steering-method/fwd.hh b/include/hpp/manipulation/steering-method/fwd.hh
new file mode 100644
index 0000000000000000000000000000000000000000..3b245f6bf4d74a71de57110f81f8078aaba51dd2
--- /dev/null
+++ b/include/hpp/manipulation/steering-method/fwd.hh
@@ -0,0 +1,35 @@
+//
+// Copyright (c) 2017 CNRS
+// Authors: Joseph Mirabel
+//
+//
+// 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/>.
+
+#ifndef HPP_MANIPULATION_STEERING_METHOD_FWD_HH
+# define HPP_MANIPULATION_STEERING_METHOD_FWD_HH
+
+# include <map>
+# include <hpp/core/fwd.hh>
+
+namespace hpp {
+ namespace manipulation {
+ namespace steeringMethod {
+ HPP_PREDEF_CLASS (CrossStateOptimization);
+ typedef boost::shared_ptr < CrossStateOptimization > CrossStateOptimizationPtr_t;
+ } // namespace steeringMethod
+ } // namespace manipulation
+} // namespace hpp
+
+#endif // HPP_MANIPULATION_STEERING_METHOD_FWD_HH
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index b7810d4d4b1b6ea42302740183bd045d6b5fdc9e..66d6b9eed3168cbd25eb1d8faac2a71c5e1eece2 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -53,6 +53,8 @@ SET(SOURCES
path-optimization/spline-gradient-based.cc
problem-target/state.cc
+
+ steering-method/cross-state-optimization.cc
)
IF(HPP_WHOLEBODY_STEP_FOUND)
diff --git a/src/graph-steering-method.cc b/src/graph-steering-method.cc
index 8d7004bf8c24cc01d981917a0428385c20220315..446388814d27f4f33ca2ac072b2dd4d8cef5c01e 100644
--- a/src/graph-steering-method.cc
+++ b/src/graph-steering-method.cc
@@ -27,10 +27,22 @@
namespace hpp {
namespace manipulation {
+ SteeringMethod::SteeringMethod (const Problem& problem) :
+ core::SteeringMethod (problem), problem_ (problem),
+ steeringMethod_ (core::SteeringMethodStraight::create (problem))
+ {
+ }
+
+ SteeringMethod::SteeringMethod (const SteeringMethod& other):
+ core::SteeringMethod (other), problem_ (other.problem_), steeringMethod_
+ (other.steeringMethod_)
+ {
+ }
+
GraphSteeringMethodPtr_t GraphSteeringMethod::create
(const core::Problem& problem)
{
- dynamic_cast <const Problem&> (problem);
+ HPP_STATIC_CAST_REF_CHECK (const Problem, problem);
const Problem& p = static_cast <const Problem&> (problem);
return create (p);
}
@@ -54,14 +66,12 @@ namespace hpp {
}
GraphSteeringMethod::GraphSteeringMethod (const Problem& problem) :
- SteeringMethod (problem), problem_ (problem), weak_ (),
- steeringMethod_ (core::SteeringMethodStraight::create (problem))
+ SteeringMethod (problem), weak_ ()
{
}
GraphSteeringMethod::GraphSteeringMethod (const GraphSteeringMethod& other):
- SteeringMethod (other), problem_ (other.problem_), steeringMethod_
- (other.steeringMethod_)
+ SteeringMethod (other)
{
}
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index 324bf8246a86f33256c48c10a820f46329d456d6..543de2fe8068390290c06954db7a0cfae1a09a3f 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -54,6 +54,7 @@
#include "hpp/manipulation/path-optimization/keypoints.hh"
#include "hpp/manipulation/path-optimization/spline-gradient-based.hh"
#include "hpp/manipulation/problem-target/state.hh"
+#include "hpp/manipulation/steering-method/cross-state-optimization.hh"
#if HPP_MANIPULATION_HAS_WHOLEBODY_STEP
#include <hpp/wholebody-step/small-steps.hh>
@@ -144,6 +145,8 @@ namespace hpp {
GraphSteeringMethod::create <core::steeringMethod::Straight>);
parent_t::add <SteeringMethodBuilder_t> ("Graph-Hermite",
GraphSteeringMethod::create <core::steeringMethod::Hermite>);
+ parent_t::add <SteeringMethodBuilder_t> ("CrossStateOptimization",
+ steeringMethod::CrossStateOptimization::createFromCore);
parent_t::add <PathOptimizerBuilder_t> ("KeypointsShortcut",
pathOptimization::Keypoints::create);
diff --git a/src/problem.cc b/src/problem.cc
index 4658f6bbf3af973e8c2f8979f54d5523723fe1b8..78a1b72cad15825c164815fb4ee90e9a14811d27 100644
--- a/src/problem.cc
+++ b/src/problem.cc
@@ -68,9 +68,9 @@ namespace hpp {
return pv;
}
- GraphSteeringMethodPtr_t Problem::steeringMethod () const
+ SteeringMethodPtr_t Problem::steeringMethod () const
{
- return HPP_DYNAMIC_PTR_CAST (GraphSteeringMethod,
+ return HPP_DYNAMIC_PTR_CAST (SteeringMethod,
Parent::steeringMethod());
}
} // namespace manipulation
diff --git a/src/steering-method/cross-state-optimization.cc b/src/steering-method/cross-state-optimization.cc
new file mode 100644
index 0000000000000000000000000000000000000000..7d5ccb9dbcd14ca553ac876877a745ae0f5613e1
--- /dev/null
+++ b/src/steering-method/cross-state-optimization.cc
@@ -0,0 +1,593 @@
+// 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/manipulation/steering-method/cross-state-optimization.hh>
+
+#include <map>
+#include <queue>
+#include <vector>
+
+#include <boost/bind.hpp>
+
+#include <hpp/util/exception-factory.hh>
+
+#include <hpp/pinocchio/configuration.hh>
+
+#include <hpp/core/explicit-numerical-constraint.hh>
+#include <hpp/core/path-vector.hh>
+
+#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 {
+ using namespace graph;
+ using Eigen::RowBlockIndices;
+ using Eigen::ColBlockIndices;
+
+ CrossStateOptimizationPtr_t CrossStateOptimization::create (
+ const Problem& problem)
+ {
+ CrossStateOptimizationPtr_t shPtr (new CrossStateOptimization (problem));
+ shPtr->init(shPtr);
+ return shPtr;
+ }
+
+ CrossStateOptimizationPtr_t CrossStateOptimization::createFromCore (
+ const core::Problem& problem)
+ {
+ HPP_STATIC_CAST_REF_CHECK (const Problem, problem);
+ const Problem& p = static_cast <const Problem&> (problem);
+ return create (p);
+ }
+
+ core::SteeringMethodPtr_t CrossStateOptimization::copy () const
+ {
+ CrossStateOptimization* ptr = new CrossStateOptimization (*this);
+ CrossStateOptimizationPtr_t shPtr (ptr);
+ ptr->init (shPtr);
+ return shPtr;
+ }
+
+ struct CrossStateOptimization::GraphSearchData
+ {
+ StatePtr_t s1, s2;
+
+ // Datas for findNextTransitions
+ struct state_with_depth {
+ StatePtr_t s;
+ EdgePtr_t e;
+ std::size_t l; // depth to root
+ std::size_t i; // index in parent state_with_depths_t
+ inline state_with_depth () : s(), e(), l(0), i (0) {}
+ inline state_with_depth (EdgePtr_t _e, std::size_t _l, std::size_t _i)
+ : s(_e->from()), e(_e), l(_l), i (_i) {}
+ };
+ typedef std::vector<state_with_depth> state_with_depths_t;
+ typedef std::map<StatePtr_t,state_with_depths_t> StateMap_t;
+ /// std::size_t is the index in state_with_depths_t at StateMap_t::iterator
+ typedef std::pair<StateMap_t::iterator, std::size_t> state_with_depth_ptr_t;
+ typedef std::queue<state_with_depth_ptr_t> Queue_t;
+ typedef std::set<EdgePtr_t> VisitedEdge_t;
+ std::size_t maxDepth;
+ StateMap_t parent1; // TODO, parent2;
+ Queue_t queue1;
+ VisitedEdge_t visitedEdge_;
+
+ const state_with_depth& getParent(const state_with_depth_ptr_t& _p) const
+ {
+ const state_with_depths_t& parents = _p.first->second;
+ return parents[_p.second];
+ }
+
+ state_with_depth_ptr_t addInitState()
+ {
+ StateMap_t::iterator next =
+ parent1.insert(StateMap_t::value_type(s1, state_with_depths_t(1))).first;
+ return state_with_depth_ptr_t (next, 0);
+ }
+
+ state_with_depth_ptr_t addParent(
+ const state_with_depth_ptr_t& _p,
+ const EdgePtr_t& transition)
+ {
+ const state_with_depths_t& parents = _p.first->second;
+ const state_with_depth& from = parents[_p.second];
+
+ // Insert state to if necessary
+ StateMap_t::iterator next = parent1.insert (
+ StateMap_t::value_type(
+ transition->to(), state_with_depths_t ()
+ )).first;
+
+ next->second.push_back (
+ state_with_depth(transition, from.l + 1, _p.second));
+
+ return state_with_depth_ptr_t (next, next->second.size()-1);
+ }
+ };
+
+ bool CrossStateOptimization::findTransitions (GraphSearchData& d) const
+ {
+ while (! d.queue1.empty())
+ {
+ GraphSearchData::state_with_depth_ptr_t _state = d.queue1.front();
+
+ const GraphSearchData::state_with_depth& parent = d.getParent(_state);
+ if (parent.l >= d.maxDepth) return true;
+ d.queue1.pop();
+
+ bool done = false;
+
+ const Neighbors_t& neighbors = _state.first->first->neighbors();
+ for (Neighbors_t::const_iterator _n = neighbors.begin();
+ _n != neighbors.end(); ++_n) {
+ EdgePtr_t transition = _n->second;
+
+ // Avoid identical consecutive transition
+ if (transition == parent.e) continue;
+
+ // If transition has already been visited, continue
+ // if (d.visitedEdge_.count (transition) == 1) continue;
+
+ // TODO
+ // If (transition->to() == d.s2) check if this list is feasible.
+ // - If a constraint with non-constant right hand side is present
+ // in all transitions, then the rhs from d.q1 and d.q2 should be
+ // equal
+
+ // Insert parent
+ d.queue1.push (
+ d.addParent (_state, transition)
+ );
+
+ done = done || (transition->to() == d.s2);
+ }
+ if (done) break;
+ }
+ return false;
+ }
+
+ Edges_t CrossStateOptimization::getTransitionList (
+ GraphSearchData& d, const std::size_t& i) const
+ {
+ assert (d.parent1.find (d.s2) != d.parent1.end());
+ const GraphSearchData::state_with_depths_t& roots = d.parent1[d.s2];
+ Edges_t transitions;
+ if (i >= roots.size()) return transitions;
+
+ const GraphSearchData::state_with_depth* current = &roots[i];
+ transitions.resize (current->l);
+ while (current->e) {
+ assert (current->l > 0);
+ transitions[current->l-1] = current->e;
+ current = &d.parent1[current->s][current->i];
+ }
+ return transitions;
+ }
+
+ struct CrossStateOptimization::OptimizationData
+ {
+ const std::size_t N, nq, nv;
+ constraints::HybridSolver solver;
+ Configuration_t q1, q2;
+ vector_t q;
+ core::DevicePtr_t robot;
+ typedef std::vector<States_t> StatesPerConf_t;
+ StatesPerConf_t statesPerConf_;
+ struct RightHandSideSetter {
+ DifferentiableFunctionPtr_t impF, expF;
+ Configuration_t* qrhs;
+ vector_t rhs;
+ RightHandSideSetter () : qrhs (NULL) {}
+ RightHandSideSetter (DifferentiableFunctionPtr_t _impF, DifferentiableFunctionPtr_t _expF, Configuration_t* _qrhs)
+ : impF(_impF), expF(_expF), qrhs (_qrhs) {}
+ RightHandSideSetter (DifferentiableFunctionPtr_t _impF, DifferentiableFunctionPtr_t _expF, vector_t _rhs)
+ : impF(_impF), expF(_expF), qrhs (NULL), rhs (_rhs) {}
+ void apply(constraints::HybridSolver& s)
+ {
+ if (qrhs != NULL) s.rightHandSideFromInput (impF, expF, *qrhs);
+ else s.rightHandSide (impF, expF, 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 (N * nq, N * nv), robot (_robot), statesPerConf_ (N)
+ {
+ solver.integration (boost::bind(&OptimizationData::_integrate, this, _1, _2, _3));
+ }
+
+ void addGraphConstraints (const GraphPtr_t& graph)
+ {
+ for (std::size_t i = 0; i < N; ++i) {
+ _add (graph->lockedJoints(), i);
+ _add (graph->numericalConstraints(), i);
+ }
+ }
+
+ 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);
+ }
+
+ 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;
+ // i = Input, o = Output,
+ // c = Config, v = Velocity
+ RowBlockIndices ic, oc, ov;
+ ColBlockIndices iv;
+ ComparisonTypes_t cts;
+ Configuration_t* qrhs;
+ if (i == 0) {
+ f = lj->function();
+ ic = _row(lj->inputConf() , 0);
+ oc = _row(lj->outputConf() , 0);
+ iv = _col(lj->inputVelocity() , 0);
+ ov = _row(lj->outputVelocity(), 0);
+ cts = ComparisonTypes_t (lj->numberDof(), constraints::Equality);
+ qrhs = &q1;
+ } else if (i == N) {
+ f = lj->function();
+ 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 = ComparisonTypes_t (lj->numberDof(), constraints::Equality);
+ qrhs = &q2;
+ } else {
+ f = Identity::Ptr_t (new Identity (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);
+ qrhs = NULL;
+ }
+
+ bool added = solver.explicitSolver().add (f, ic, oc, iv, ov, cts);
+ if (!added) {
+ HPP_THROW (std::invalid_argument,
+ "Could not add locked joint " << lj->jointName() <<
+ " of transition " << trans->name() << " at id " << i);
+ }
+
+ // This must be done later
+ if (qrhs != NULL)
+ rhsSetters_.push_back (RightHandSideSetter(
+ DifferentiableFunctionPtr_t(), f, qrhs));
+ // solver.rightHandSideFromInput (DifferentiableFunctionPtr_t(), f, *qrhs);
+ }
+
+ // TODO handle numerical constraints
+ using namespace ::hpp::core;
+ ExplicitNumericalConstraintPtr_t enc;
+ const NumericalConstraints_t& ncs = trans->numericalConstraints();
+ for (NumericalConstraints_t::const_iterator _nc = ncs.begin();
+ _nc != ncs.end(); ++_nc) {
+ NumericalConstraintPtr_t nc (*_nc);
+ enc = HPP_DYNAMIC_PTR_CAST (ExplicitNumericalConstraint, nc);
+
+ DifferentiableFunctionPtr_t f, ef;
+ // 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;
+ }
+
+ bool added = false;
+ if (ef) added = solver.explicitSolver().add (ef, ic, oc, iv, ov, cts);
+ if (!added) solver.add (f, 0, cts);
+
+ // TODO This must be done later...
+ // if (qrhs != NULL) {
+ if (rhs.size() > 0) {
+ // solver.rightHandSideFromInput (f, ef, rhs);
+ rhsSetters_.push_back (RightHandSideSetter(f, ef, rhs));
+ }
+ }
+ }
+
+ 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::lineSearch::FixedSequence> (q);
+ bool success = (status == constraints::HierarchicalIterativeSolver::SUCCESS);
+ if (!success) {
+ hppDout (warning, "Solution from projection "
+ << pinocchio::displayConfig(q));
+ }
+ return success;
+ }
+
+ void _add (const NumericalConstraints_t& ncs, const std::size_t& i)
+ {
+ using namespace ::hpp::core;
+ ExplicitNumericalConstraintPtr_t enc;
+ for (NumericalConstraints_t::const_iterator _nc = ncs.begin();
+ _nc != ncs.end(); ++_nc) {
+ NumericalConstraintPtr_t nc (*_nc);
+ enc = HPP_DYNAMIC_PTR_CAST (ExplicitNumericalConstraint, nc);
+ bool added = false;
+ if (enc) {
+ added = solver.explicitSolver().add (enc->explicitFunction(),
+ _row(enc->inputConf() , i * nq),
+ _row(enc->outputConf() , i * nq),
+ _col(enc->inputVelocity() , i * nv),
+ _row(enc->outputVelocity(), i * nv),
+ enc->comparisonType ());
+ }
+ if (!added)
+ solver.add (_stateFunction(nc->functionPtr(), i), 0, nc->comparisonType());
+ }
+ }
+ 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);
+ bool added = solver.explicitSolver().add (
+ lj->function(),
+ _row(lj->inputConf() , i * nq),
+ _row(lj->outputConf() , i * nq),
+ _col(lj->inputVelocity() , i * nv),
+ _row(lj->outputVelocity(), i * nv),
+ lj->comparisonType ());
+ if (!added)
+ throw std::invalid_argument ("Could not add locked joint " + lj->jointName());
+
+ // This must be done later
+ rhsSetters_.push_back (RightHandSideSetter(
+ DifferentiableFunctionPtr_t(),
+ lj->function(),
+ lj->rightHandSide()));
+ // solver.rightHandSide (DifferentiableFunctionPtr_t(),
+ // lj->function(),
+ // lj->rightHandSide());
+ }
+ }
+
+ 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);
+ }
+ 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));
+ }
+ };
+
+ void CrossStateOptimization::buildOptimizationProblem (
+ OptimizationData& d, const Edges_t& transitions) const
+ {
+ if (d.N == 0) return;
+ d.solver.maxIterations (60);
+ d.solver.errorThreshold (1e-4);
+
+ // 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.explicitSolverHasChanged();
+ d.setRightHandSide();
+
+ hppDout (info, "Solver informations\n" << d.solver);
+
+ // Initial guess
+ d.q.resize (d.N * d.nq);
+ for (std::size_t i = 0; i < d.N; ++i) {
+ pinocchio::interpolate (d.robot, d.q1, d.q2,
+ value_type(i+1) / value_type(d.N+1),
+ d.q.segment (i * d.nq, d.nq));
+ }
+ }
+
+ core::PathVectorPtr_t CrossStateOptimization::buildPath (
+ OptimizationData& d, const Edges_t& transitions) const
+ {
+ using core::PathVector;
+ using core::PathVectorPtr_t;
+
+ const core::DevicePtr_t& robot = problem().robot();
+ PathVectorPtr_t pv = PathVector::create (
+ robot->configSize(), robot->numberDof());
+ core::PathPtr_t path;
+
+ 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);
+
+ bool status;
+ 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));
+ else if (last)
+ status = t->build (path, d.q.tail (d.nq), 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));
+ }
+
+ if (!status || !path) {
+ hppDout (warning, "Could not build path from solution "
+ << pinocchio::displayConfig(d.q));
+ return PathVectorPtr_t();
+ }
+ pv->appendPath(path);
+
+ ++i;
+ }
+ return pv;
+ }
+
+ core::PathPtr_t CrossStateOptimization::impl_compute (
+ ConfigurationIn_t q1, ConfigurationIn_t q2) const
+ {
+ const Graph& graph = *problem_.constraintGraph ();
+ GraphSearchData d;
+ d.s1 = graph.getState (q1);
+ d.s2 = graph.getState (q2);
+ // d.maxDepth = 2;
+ d.maxDepth = problem_.getParameter ("CrossStateOptimization/maxDepth", size_type(2));
+
+ // Find
+ d.queue1.push (d.addInitState());
+ std::size_t idxSol = (d.s1 == d.s2 ? 1 : 0);
+ bool maxDepthReached = findTransitions (d);
+
+ while (!maxDepthReached) {
+ Edges_t transitions = getTransitionList (d, idxSol);
+ while (! transitions.empty()) {
+#ifdef HPP_DEBUG
+ std::ostringstream ss;
+ ss << "Trying solution " << idxSol << ": ";
+ for (std::size_t j = 0; j < transitions.size(); ++j)
+ ss << transitions[j]->name() << ", ";
+ 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");
+ }
+
+ ++idxSol;
+ transitions = getTransitionList(d, idxSol);
+ }
+ maxDepthReached = findTransitions (d);
+ }
+
+ return core::PathPtr_t ();
+ }
+ } // namespace steeringMethod
+ } // namespace manipulation
+} // namespace hpp
diff --git a/src/steering-method/cross-state-optimization/function.cc b/src/steering-method/cross-state-optimization/function.cc
new file mode 100644
index 0000000000000000000000000000000000000000..a536f0e1db50ac42af581291d1c08d791ba389ae
--- /dev/null
+++ b/src/steering-method/cross-state-optimization/function.cc
@@ -0,0 +1,161 @@
+// 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();
+ }
+ }
+
+ 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 (LiegroupElement& 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
+
+ class HPP_MANIPULATION_LOCAL Identity :
+ public constraints::DifferentiableFunction
+ {
+ public:
+ typedef boost::shared_ptr<Identity> Ptr_t;
+
+ Identity (const LiegroupSpacePtr_t space, const std::string& name) :
+ DifferentiableFunction (space->nq(), space->nv(), space, name) {}
+
+ protected:
+ void impl_compute (LiegroupElement& y, vectorIn_t arg) const
+ {
+ y.vector() = arg;
+ }
+
+ void impl_jacobian (matrixOut_t J, vectorIn_t) const
+ {
+ J.setIdentity();
+ }
+ }; // class Function
+
+ 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 (LiegroupElement& 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