Commit db4f0ec8 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Add LiegroupSpace::Jdifference

parent 03a98ae3
......@@ -166,6 +166,17 @@ namespace hpp {
/// \param[out] J the Jacobian of y
void Jintegrate (vectorIn_t v, matrixOut_t J) const;
/// Compute the Jacobian of the difference operation.
/// Given \f$ v = q1 - q0 \f$,
///
/// \param[in] q0,q1
/// \param[in] J0 the Jacobian of q0.
/// \param[in] J1 the Jacobian of q1.
/// \param[out] J0 the Jacobian of v with respect to q0.
/// \param[out] J1 the Jacobian of v with respect to q1.
/// \note to compute only one jacobian, provide for J0 or J1 an empty matrix.
void Jdifference (vectorIn_t q0, vectorIn_t q1, matrixOut_t J0, matrixOut_t J1) const;
/// Return name of Lie group
std::string name () const;
......
// Copyright (c) 2018, CNRS
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// 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
// 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-pinocchio. If not, see <http://www.gnu.org/licenses/>.
#ifndef HPP_PINOCCHIO_SRC_JDIFFERENCE_VISITOR_HH
# define HPP_PINOCCHIO_SRC_JDIFFERENCE_VISITOR_HH
# include <hpp/pinocchio/liegroup/vector-space.hh>
namespace hpp {
namespace pinocchio {
namespace liegroupType {
struct JdifferenceVisitor : public boost::static_visitor <>
{
JdifferenceVisitor (vectorIn_t& q0, vectorIn_t& q1,
matrixOut_t& J0, matrixOut_t& J1)
: q0_ (q0)
, q1_ (q1)
, J0_ (J0)
, J1_ (J1)
, iq_ (0)
, iv_ (0)
{}
template <typename LgT> void operator () (const LgT& lg)
{
typename LgT::JacobianMatrix_t J0int (lg.nv(), lg.nv());
typename LgT::JacobianMatrix_t J1int (lg.nv(), lg.nv());
lg.Jdifference (
q0_.segment<LgT::NQ>(iq_, lg.nq()),
q1_.segment<LgT::NQ>(iq_, lg.nq()),
J0int,
J1int);
if (J0_.size() > 0)
J0_.middleRows<LgT::NV> (iv_, lg.nv()).applyOnTheLeft (J0int);
if (J1_.size() > 0)
J1_.middleRows<LgT::NV> (iv_, lg.nv()).applyOnTheLeft (J1int);
iq_ += lg.nq();
iv_ += lg.nv();
}
template <int N, bool rot>
void operator () (const liegroup::VectorSpaceOperation<N,rot>& lg)
{
iq_ += lg.nq();
iv_ += lg.nv();
}
vectorIn_t & q0_, q1_;
matrixOut_t& J0_, J1_;
size_type iq_, iv_;
}; // struct JdifferenceVisitor
} // namespace liegroupType
} // namespace pinocchio
} // namespace hpp
#endif // HPP_PINOCCHIO_SRC_JDIFFERENCE_VISITOR_HH
......@@ -19,6 +19,7 @@
#include "../src/comparison.hh"
#include "../src/size-visitor.hh"
#include "../src/jintegrate-visitor.hh"
#include "../src/jdifference-visitor.hh"
namespace hpp {
namespace pinocchio {
......@@ -136,6 +137,18 @@ namespace hpp {
assert (row == nv());
}
void LiegroupSpace::Jdifference (vectorIn_t q0, vectorIn_t q1, matrixOut_t J0, matrixOut_t J1) const
{
assert (q0.size() == nq() && q1.size() == nq());
liegroupType::JdifferenceVisitor jdv (q0,q1,J0,J1);
for (std::size_t i = 0; i < liegroupTypes_.size (); ++i) {
boost::apply_visitor (jdv, liegroupTypes_ [i]);
}
assert (jdv.iq_ == nq());
assert (jdv.iv_ == nv());
}
struct NameVisitor : public boost::static_visitor <>
{
template <typename LgT1> void operator () (const LgT1& lg1)
......
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