Commit d2a24eb9 authored by Aamr El Kazdadi's avatar Aamr El Kazdadi Committed by Joseph Mirabel
Browse files

Integrate(q, v) jacobians with respect to q and v

parent 171e1486
......@@ -166,6 +166,22 @@ namespace hpp {
/// \param[out] J the Jacobian of y
void Jintegrate (vectorIn_t v, matrixOut_t J) const;
/// Compute the Jacobians of the integration operation
/// with respect to q.
/// Given \f$ y = x + v \f$,
///
/// \param[in] Jq the Jacobian of x
/// \param[out] J the Jacobian of y
void dIntegrate_dq (LiegroupElement q, vectorIn_t v, matrixOut_t Jq) const;
/// Compute the Jacobians of the integration operation
/// with respect to v.
/// Given \f$ y = x + v \f$,
///
/// \param[in] Jv the Jacobian of v
/// \param[out] J the Jacobian of y
void dIntegrate_dv (LiegroupElement q, vectorIn_t v, matrixOut_t Jv) const;
/// Compute the Jacobian of the difference operation.
/// Given \f$ v = q1 - q0 \f$,
///
......
// 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_DINTEGRATE_VISITOR_HH
# define HPP_PINOCCHIO_SRC_DINTEGRATE_VISITOR_HH
# include <hpp/pinocchio/liegroup/vector-space.hh>
namespace hpp {
namespace pinocchio {
namespace liegroupType {
struct dIntegrateVisitor_dq : public boost::static_visitor <>
{
dIntegrateVisitor_dq (LiegroupElement q, vectorIn_t& v, matrixOut_t& Jq, size_type& row, size_type& configRow)
: q_(q), v_ (v), Jq_ (Jq), row_ (row), configRow_ (configRow) {}
template <typename LgT> void operator () (const LgT& lg)
{
typename LgT::JacobianMatrix_t JqInt (lg.nv(), lg.nv());
typename LgT::JacobianMatrix_t JvInt (lg.nv(), lg.nv());
lg.Jintegrate (q_.vector().segment<LgT::NQ>(configRow_, lg.nq()), v_.segment<LgT::NV>(row_, lg.nv()), JqInt, JvInt);
Jq_.middleRows<LgT::NV> (row_, lg.nv()).applyOnTheLeft (JqInt);
row_ += lg.nv();
configRow_ += lg.nq();
}
template <int N, bool rot>
void operator () (const liegroup::VectorSpaceOperation<N,rot>& lg)
{
row_ += lg.nv();
configRow_ += lg.nq();
}
LiegroupElement q_;
vectorIn_t& v_;
matrixOut_t& Jq_;
size_type& row_;
size_type& configRow_;
}; // struct dIntegrateVisitor_dq
struct dIntegrateVisitor_dv : public boost::static_visitor <>
{
dIntegrateVisitor_dv (LiegroupElement q, vectorIn_t& v, matrixOut_t& Jv, size_type& row, size_type& configRow)
: q_(q), v_ (v), Jv_ (Jv), row_ (row), configRow_ (configRow) {}
template <typename LgT> void operator () (const LgT& lg)
{
typename LgT::JacobianMatrix_t JqInt (lg.nv(), lg.nv());
typename LgT::JacobianMatrix_t JvInt (lg.nv(), lg.nv());
lg.Jintegrate (q_.vector().segment<LgT::NQ>(configRow_, lg.nq()), v_.segment<LgT::NV>(row_, lg.nv()), JqInt, JvInt);
Jv_.middleRows<LgT::NV> (row_, lg.nv()).applyOnTheLeft (JvInt);
row_ += lg.nv();
configRow_ += lg.nq();
}
template <int N, bool rot>
void operator () (const liegroup::VectorSpaceOperation<N,rot>& lg)
{
row_ += lg.nv();
configRow_ += lg.nq();
}
LiegroupElement q_;
vectorIn_t& v_;
matrixOut_t& Jv_;
size_type& row_;
size_type& configRow_;
}; // struct dIntegrateVisitor_dv
} // namespace liegroupType
} // namespace pinocchio
} // namespace hpp
#endif // HPP_PINOCCHIO_SRC_JINTEGRATE_VISITOR_HH
......@@ -30,7 +30,9 @@ namespace hpp {
template <typename LgT> void operator () (const LgT& lg)
{
typename LgT::JacobianMatrix_t Jint (lg.nv(), lg.nv());
lg.Jintegrate (v_.segment<LgT::NV>(row_, lg.nv()), Jint);
typename LgT::JacobianMatrix_t Jv (lg.nv(), lg.nv());
LiegroupElement q(lg.neutral());
lg.Jintegrate (q.vector(), v_.segment<LgT::NV>(row_, lg.nv()), Jint, Jv);
J_.middleRows<LgT::NV> (row_, lg.nv()).applyOnTheLeft (Jint);
row_ += lg.nv();
}
......
......@@ -18,7 +18,7 @@
#include <hpp/pinocchio/liegroup-space.hh>
#include "../src/comparison.hh"
#include "../src/size-visitor.hh"
#include "../src/jintegrate-visitor.hh"
#include "../src/dintegrate-visitor.hh"
#include "../src/jdifference-visitor.hh"
namespace hpp {
......@@ -137,6 +137,36 @@ namespace hpp {
assert (row == nv());
}
void LiegroupSpace::dIntegrate_dq (LiegroupElement q, vectorIn_t v, matrixOut_t Jq) const
{
assert (q.size() == nq());
assert (v.size() == nv());
assert (Jq.rows() == nv());
size_type row = 0;
size_type configRow = 0;
liegroupType::dIntegrateVisitor_dq jiv (q, v, Jq, row, configRow);
for (std::size_t i = 0; i < liegroupTypes_.size (); ++i) {
boost::apply_visitor (jiv, liegroupTypes_ [i]);
}
assert (row == nv());
assert (configRow == nq());
}
void LiegroupSpace::dIntegrate_dv (LiegroupElement q, vectorIn_t v, matrixOut_t Jv) const
{
assert (q.size() == nq());
assert (v.size() == nv());
assert (Jv.rows() == nv());
size_type row = 0;
size_type configRow = 0;
liegroupType::dIntegrateVisitor_dv jiv (q, v, Jv, row, configRow);
for (std::size_t i = 0; i < liegroupTypes_.size (); ++i) {
boost::apply_visitor (jiv, liegroupTypes_ [i]);
}
assert (row == nv());
assert (configRow == nq());
}
template <bool ApplyOnTheLeft>
void LiegroupSpace::Jdifference (vectorIn_t q0, vectorIn_t q1, matrixOut_t J0, matrixOut_t J1) const
{
......
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