Commit b467be95 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Add LiegroupSpace::interpolate.

parent f3a1c7c7
......@@ -258,6 +258,17 @@ namespace hpp {
template <DerivativeProduct side>
void dDifference_dq1 (vectorIn_t q0, vectorIn_t q1, matrixOut_t J1) const;
/// Interpolate between two elements of the Lie group.
/// This is equivalent to \f$ q_0 \oplus u*(q_1 \ominus q_0) \f$.
/// \param q0, q1 two elements
/// \param u in [0,1] position along the interpolation: q0 for u=0,
/// q1 for u=1
/// \retval result interpolated configuration
void interpolate (vectorIn_t q0, vectorIn_t q1, value_type u,
vectorOut_t result) const;
/// Return name of Lie group
std::string name () const;
......@@ -20,6 +20,7 @@
#include "../src/size-visitor.hh"
#include "../src/dintegrate-visitor.hh"
#include "../src/jdifference-visitor.hh"
#include "../src/visitor/interpolate.hh"
namespace hpp {
namespace pinocchio {
......@@ -253,6 +254,19 @@ namespace hpp {
template void LiegroupSpace::dDifference_dq1<DerivativeTimesInput> (vectorIn_t, vectorIn_t, matrixOut_t) const;
template void LiegroupSpace::dDifference_dq1<InputTimesDerivative> (vectorIn_t, vectorIn_t, matrixOut_t) const;
void LiegroupSpace::interpolate (vectorIn_t q0, vectorIn_t q1, value_type u,
vectorOut_t r) const
assert (q0.size() == nq());
assert (q1.size() == nq());
assert (r .size() == nq());
liegroupType::visitor::Interpolate iv (q0, q1, u, r);
for (LiegroupTypes::const_iterator it = liegroupTypes ().begin ();
it != liegroupTypes ().end (); ++it)
boost::apply_visitor (iv, *it);
struct NameVisitor : public boost::static_visitor <>
template <typename LgT1> void operator () (const LgT1& lg1)
// Copyright (c) 2020, CNRS
// Authors: Joseph Mirabel
// This file is part of hpp-pinocchio.
// hpp-pinocchio 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-pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-pinocchio. If not, see <>.
namespace hpp {
namespace pinocchio {
namespace liegroupType {
namespace visitor {
/// Interpolate
struct Interpolate : public boost::static_visitor <>
Interpolate (const vectorIn_t& e1, const vectorIn_t& e2,
value_type u,
vectorOut_t& er) :
e1_ (e1), e2_ (e2), u_ (u), er_ (er),
iq_(0), iv_(0)
template <typename LiegroupType> void operator () (LiegroupType& op)
op.interpolate (
e1_.segment<LiegroupType::NQ>(iq_, op.nq()),
e2_.segment<LiegroupType::NQ>(iq_, op.nq()),
er_.segment<LiegroupType::NQ>(iq_, op.nq()));
iq_ += op.nq();
iv_ += op.nv();
const vectorIn_t& e1_;
const vectorIn_t& e2_;
value_type u_;
vectorOut_t& er_;
size_type iq_, iv_;
}; // struct SubstractionVisitor
} // namespace visitor
} // namespace liegroupType
} // namespace pinocchio
} // namespace hpp
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment