diff --git a/include/hpp/manipulation/steering-method/cross-state-optimization.hh b/include/hpp/manipulation/steering-method/cross-state-optimization.hh
index ae6d0fac87406f1f0eccfb2cab7ec4373a861084..1939ce22e4e65f9688a1b20694c03c3a2bb2dc66 100644
--- a/include/hpp/manipulation/steering-method/cross-state-optimization.hh
+++ b/include/hpp/manipulation/steering-method/cross-state-optimization.hh
@@ -60,14 +60,20 @@ namespace hpp {
}
private:
- struct Data;
+ struct GraphSearchData;
+ struct OptimizationData;
/// Step 1 of the algorithm
/// \return whether the max depth was reached.
- bool findTransitions (Data& data) const;
+ bool findTransitions (GraphSearchData& data) const;
/// Step 2 of the algorithm
- graph::Edges_t getTransitionList (Data& data, const std::size_t& i) const;
+ 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;
/// A pointer to the problem
const Problem& problem_;
diff --git a/src/steering-method/cross-state-optimization.cc b/src/steering-method/cross-state-optimization.cc
index e126c9c53a2238537a948f404eac1ce7021a03c9..b7e18fea8845af2e0518687b8ac979cb71034459 100644
--- a/src/steering-method/cross-state-optimization.cc
+++ b/src/steering-method/cross-state-optimization.cc
@@ -20,13 +20,26 @@
#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)
@@ -52,8 +65,8 @@ namespace hpp {
return shPtr;
}
- struct CrossStateOptimization::Data {
- typedef graph::StatePtr_t StatePtr_t;
+ struct CrossStateOptimization::GraphSearchData
+ {
StatePtr_t s1, s2;
// Datas for findNextTransitions
@@ -99,8 +112,7 @@ namespace hpp {
// Insert state to if necessary
StateMap_t::iterator next = parent1.insert (
StateMap_t::value_type(
- transition->to(),
- Data::state_with_depths_t ()
+ transition->to(), state_with_depths_t ()
)).first;
next->second.push_back (
@@ -110,43 +122,13 @@ namespace hpp {
}
};
- core::PathPtr_t CrossStateOptimization::impl_compute (
- ConfigurationIn_t q1, ConfigurationIn_t q2) const
- {
- const Graph& graph = *problem_.constraintGraph ();
- Data d;
- d.s1 = graph.getState (q1);
- d.s2 = graph.getState (q2);
- d.maxDepth = 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()) {
- std::cout << "Solution " << idxSol << ": ";
- for (std::size_t j = 0; j < transitions.size(); ++j)
- std::cout << transitions[j]->name() << ", ";
- std::cout << std::endl;
- ++idxSol;
- transitions = getTransitionList(d, idxSol);
- }
- maxDepthReached = findTransitions (d);
- }
-
- return core::PathPtr_t ();
- }
-
- bool CrossStateOptimization::findTransitions (Data& d) const
+ bool CrossStateOptimization::findTransitions (GraphSearchData& d) const
{
while (! d.queue1.empty())
{
- Data::state_with_depth_ptr_t _state = d.queue1.front();
+ GraphSearchData::state_with_depth_ptr_t _state = d.queue1.front();
- const Data::state_with_depth& parent = d.getParent(_state);
+ const GraphSearchData::state_with_depth& parent = d.getParent(_state);
if (parent.l >= d.maxDepth) return true;
d.queue1.pop();
@@ -173,14 +155,14 @@ namespace hpp {
}
Edges_t CrossStateOptimization::getTransitionList (
- Data& d, const std::size_t& i) const
+ GraphSearchData& d, const std::size_t& i) const
{
assert (d.parent1.find (d.s2) != d.parent1.end());
- const Data::state_with_depths_t& roots = d.parent1[d.s2];
+ const GraphSearchData::state_with_depths_t& roots = d.parent1[d.s2];
Edges_t transitions;
if (i >= roots.size()) return transitions;
- const Data::state_with_depth* current = &roots[i];
+ const GraphSearchData::state_with_depth* current = &roots[i];
transitions.resize (current->l);
while (current->e) {
assert (current->l > 0);
@@ -189,6 +171,414 @@ namespace hpp {
}
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()) {
+ std::cout << "Trying solution " << idxSol << ": ";
+ for (std::size_t j = 0; j < transitions.size(); ++j)
+ std::cout << transitions[j]->name() << ", ";
+ std::cout << std::endl;
+
+ 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) {
+ std::cout << e.what() << std::endl;
+ ok = false;
+ }
+
+ if (ok && optData.solve()) {
+ try {
+ core::PathPtr_t path = buildPath (optData, transitions);
+ if (path) return path;
+ } catch (const std::runtime_error& e) {
+ hppDout (warning, "Could not build path from solution "
+ << pinocchio::displayConfig(optData.q));
+ }
+ std::cout << "Failed to build" << std::endl;
+ } else {
+ std::cout << "Failed to solve" << std::endl;
+ }
+
+ ++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