Skip to content

Commit

Permalink
Fix compatibility with Eigen 3.2.0
Browse files Browse the repository at this point in the history
  • Loading branch information
@jmirabel
jmirabel committed Oct 24, 2018
1 parent bdc5b6f commit 749a76a
Show file tree
Hide file tree
Showing 3 changed files with 64 additions and 12 deletions.
22 changes: 18 additions & 4 deletions src/dintegrate-visitor.hh
View file
Expand Up @@ -19,6 +19,8 @@

# include <hpp/pinocchio/liegroup/vector-space.hh>

# include <../src/eigen_fix.hh>

namespace hpp {
namespace pinocchio {
namespace liegroupType {
Expand All @@ -30,8 +32,14 @@ namespace hpp {
template <typename LgT> void operator () (const LgT& lg)
{
typename LgT::JacobianMatrix_t JqInt (lg.nv(), lg.nv());
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);
lg.dIntegrate_dq (
// q_.vector().segment<LgT::NQ>(configRow_, lg.nq()),
_BLOCK_ACCESSOR_EIGEN(q_.vector(),segment,LgT::NQ, configRow_, lg.nq()),
// v_.segment<LgT::NV>(row_, lg.nv()),
_BLOCK_ACCESSOR_EIGEN(v_,segment,LgT::NV, row_, lg.nv()),
JqInt);
// Jq_.middleRows<LgT::NV> (row_, lg.nv()).applyOnTheLeft (JqInt);
_BLOCK_ACCESSOR_EIGEN(Jq_,middleRows,LgT::NV, row_, lg.nv()).applyOnTheLeft (JqInt);
row_ += lg.nv();
configRow_ += lg.nq();
}
Expand All @@ -57,8 +65,14 @@ namespace hpp {
template <typename LgT> void operator () (const LgT& lg)
{
typename LgT::JacobianMatrix_t JvInt (lg.nv(), lg.nv());
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);
lg.dIntegrate_dv (
// q_.vector().segment<LgT::NQ>(configRow_, lg.nq()),
_BLOCK_ACCESSOR_EIGEN(q_.vector(),segment,LgT::NQ, configRow_, lg.nq()),
// v_.segment<LgT::NV>(row_, lg.nv()),
_BLOCK_ACCESSOR_EIGEN(v_,segment,LgT::NV, row_, lg.nv()),
JvInt);
// Jv_.middleRows<LgT::NV> (row_, lg.nv()).applyOnTheLeft (JvInt);
_BLOCK_ACCESSOR_EIGEN(Jv_,middleRows,LgT::NV, row_, lg.nv()).applyOnTheLeft (JvInt);
row_ += lg.nv();
configRow_ += lg.nq();
}
Expand Down
28 changes: 28 additions & 0 deletions src/eigen_fix.hh
View file
@@ -0,0 +1,28 @@
// 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/>.

// Eigen 3.2.0 has no fixed size segment<>, moddleRows<>, middleCols<> with 2
// arguments. This is not documented in recent eigen either.
// This was introduced in Eigen 3.2.1. See
// - http://eigen.tuxfamily.org/index.php?title=ChangeLog#Eigen_3.2.1 (bug 579)
// - http://eigen.tuxfamily.org/bz/show_bug.cgi?id=579
#if EIGEN_VERSION_AT_LEAST(3,2,1)
# define _BLOCK_ACCESSOR_EIGEN(var,method,tplSize,arg,size) \
var.method<tplSize>(arg,size)
#else
# define _BLOCK_ACCESSOR_EIGEN(var,method,tplSize,arg,size) \
((tplSize==Eigen::Dynamic) ? var.method(arg,size) : var.method<tplSize>(arg))
#endif
26 changes: 18 additions & 8 deletions src/jdifference-visitor.hh
View file
Expand Up @@ -19,6 +19,8 @@

# include <hpp/pinocchio/liegroup/vector-space.hh>

# include <../src/eigen_fix.hh>

namespace hpp {
namespace pinocchio {
namespace liegroupType {
Expand All @@ -41,21 +43,27 @@ namespace hpp {
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()),
// q0_.segment<LgT::NQ>(iq_, lg.nq()),
_BLOCK_ACCESSOR_EIGEN(q0_,segment,LgT::NQ, iq_, lg.nq()),
// q1_.segment<LgT::NQ>(iq_, lg.nq()),
_BLOCK_ACCESSOR_EIGEN(q1_,segment,LgT::NQ, iq_, lg.nq()),
J0int,
J1int);
if (J0_.size() > 0) {
if (ApplyOnTheLeft)
J0_.middleRows<LgT::NV> (iv_, lg.nv()).applyOnTheLeft (J0int);
// J0_.middleRows<LgT::NV> (iv_, lg.nv()).applyOnTheLeft (J0int);
_BLOCK_ACCESSOR_EIGEN(J0_,middleRows,LgT::NV, iv_, lg.nv()).applyOnTheLeft (J0int);
else
J0_.middleCols<LgT::NV> (iv_, lg.nv()).applyOnTheRight (J0int);
// J0_.middleCols<LgT::NV> (iv_, lg.nv()).applyOnTheRight (J0int);
_BLOCK_ACCESSOR_EIGEN(J0_,middleCols,LgT::NV, iv_, lg.nv()).applyOnTheRight (J0int);
}
if (J1_.size() > 0) {
if (ApplyOnTheLeft)
J1_.middleRows<LgT::NV> (iv_, lg.nv()).applyOnTheLeft (J1int);
// J1_.middleRows<LgT::NV> (iv_, lg.nv()).applyOnTheLeft (J1int);
_BLOCK_ACCESSOR_EIGEN(J1_,middleRows,LgT::NV, iv_, lg.nv()).applyOnTheLeft (J1int);
else
J1_.middleCols<LgT::NV> (iv_, lg.nv()).applyOnTheRight (J1int);
// J1_.middleCols<LgT::NV> (iv_, lg.nv()).applyOnTheRight (J1int);
_BLOCK_ACCESSOR_EIGEN(J1_,middleCols,LgT::NV, iv_, lg.nv()).applyOnTheRight (J1int);
}
iq_ += lg.nq();
iv_ += lg.nv();
Expand All @@ -66,9 +74,11 @@ namespace hpp {
{
if (J0_.size() > 0) {
if (ApplyOnTheLeft)
J0_.middleRows<N> (iv_, lg.nv()) *= -1;
// J0_.middleRows<N> (iv_, lg.nv()) *= -1;
_BLOCK_ACCESSOR_EIGEN(J0_,middleRows,N, iv_, lg.nv()) *= -1;
else
J0_.middleCols<N> (iv_, lg.nv()) *= -1;
// J0_.middleCols<N> (iv_, lg.nv()) *= -1;
_BLOCK_ACCESSOR_EIGEN(J0_,middleCols,N, iv_, lg.nv()) *= -1;
}
iq_ += lg.nq();
iv_ += lg.nv();
Expand Down

0 comments on commit 749a76a

Please sign in to comment.