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

Remove LiegroupSpace::Jintegrate and update to new API in Pinocchio

parent f341eac5
......@@ -159,13 +159,6 @@ namespace hpp {
/// Return exponential of a tangent vector
LiegroupElement exp (vectorIn_t v) const;
/// Compute the Jacobian of the integration operation.
/// Given \f$ y = x + v \f$,
///
/// \param[in] J the Jacobian of x
/// \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$,
......
......@@ -30,8 +30,7 @@ namespace hpp {
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);
lg.dIntegrate_dq (q_.vector().segment<LgT::NQ>(configRow_, lg.nq()), v_.segment<LgT::NV>(row_, lg.nv()), JqInt);
Jq_.middleRows<LgT::NV> (row_, lg.nv()).applyOnTheLeft (JqInt);
row_ += lg.nv();
configRow_ += lg.nq();
......@@ -57,9 +56,8 @@ namespace hpp {
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);
lg.dIntegrate_dv (q_.vector().segment<LgT::NQ>(configRow_, lg.nq()), v_.segment<LgT::NV>(row_, lg.nv()), JvInt);
Jv_.middleRows<LgT::NV> (row_, lg.nv()).applyOnTheLeft (JvInt);
row_ += lg.nv();
configRow_ += lg.nq();
......
// 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_JINTEGRATE_VISITOR_HH
# define HPP_PINOCCHIO_SRC_JINTEGRATE_VISITOR_HH
# include <hpp/pinocchio/liegroup/vector-space.hh>
namespace hpp {
namespace pinocchio {
namespace liegroupType {
struct JintegrateVisitor : public boost::static_visitor <>
{
JintegrateVisitor (vectorIn_t& v, matrixOut_t& J, size_type& row)
: v_ (v), J_ (J), row_ (row) {}
template <typename LgT> void operator () (const LgT& lg)
{
typename LgT::JacobianMatrix_t Jint (lg.nv(), lg.nv());
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();
}
template <int N, bool rot>
void operator () (const liegroup::VectorSpaceOperation<N,rot>& lg)
{
row_ += lg.nv();
}
vectorIn_t& v_;
matrixOut_t& J_;
size_type& row_;
}; // struct JintegrateVisitor
} // namespace liegroupType
} // namespace pinocchio
} // namespace hpp
#endif // HPP_PINOCCHIO_SRC_JINTEGRATE_VISITOR_HH
......@@ -125,18 +125,6 @@ namespace hpp {
return neutral () + v;
}
void LiegroupSpace::Jintegrate (vectorIn_t v, matrixOut_t J) const
{
assert (v.size() == nv());
assert (J.rows() == nv());
size_type row = 0;
liegroupType::JintegrateVisitor jiv (v, J, row);
for (std::size_t i = 0; i < liegroupTypes_.size (); ++i) {
boost::apply_visitor (jiv, liegroupTypes_ [i]);
}
assert (row == nv());
}
void LiegroupSpace::dIntegrate_dq (LiegroupElement q, vectorIn_t v, matrixOut_t Jq) const
{
assert (q.size() == nq());
......
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