Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/pinocchio/multibody/liegroup/cartesian-product-variant.hxx	Lines:	104	177	58.8 %
Date:	2024-01-23 21:41:47	Branches:	65	218	29.8 %


//
// Copyright (c) 2020 CNRS
//

#ifndef __pinocchio_cartesian_product_variant_hxx__
#define __pinocchio_cartesian_product_variant_hxx__

#include "pinocchio/multibody/liegroup/liegroup-variant-visitors.hpp"

namespace pinocchio
{

template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
void CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
append(const LieGroupGeneric & lg)
{
  liegroups.push_back(lg);
  const Index lg_nq = ::pinocchio::nq(lg); lg_nqs.push_back(lg_nq); m_nq += lg_nq;
  const Index lg_nv = ::pinocchio::nv(lg); lg_nvs.push_back(lg_nv); m_nv += lg_nv;

  if(liegroups.size() > 1)
    m_name += " x ";
  m_name += ::pinocchio::name(lg);

  m_neutral.conservativeResize(m_nq);
  m_neutral.tail(lg_nq) = ::pinocchio::neutral(lg);

}

template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
template <class ConfigL_t, class ConfigR_t, class Tangent_t>
void CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
                const Eigen::MatrixBase<ConfigR_t> & q1,
                const Eigen::MatrixBase<Tangent_t> & d) const
{
  Index id_q = 0, id_v = 0;
  for(size_t k = 0; k < liegroups.size(); ++k)
  {
    const Index & nq = lg_nqs[k];
    const Index & nv = lg_nvs[k];
    ::pinocchio::difference(liegroups[k],
                     q0.segment(id_q,nq),
                     q1.segment(id_q,nq),
                     PINOCCHIO_EIGEN_CONST_CAST(Tangent_t, d).segment(id_v,nv));

    id_q += nq; id_v += nv;
  }
}

template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
void CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
dDifference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
                 const Eigen::MatrixBase<ConfigR_t> & q1,
                 const Eigen::MatrixBase<JacobianOut_t> & J) const
{
  PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J).setZero();
  Index id_q = 0, id_v = 0;
  for(size_t k = 0; k < liegroups.size(); ++k)
  {
    const Index & nq = lg_nqs[k];
    const Index & nv = lg_nvs[k];
    ::pinocchio::dDifference(liegroups[k],
                     q0.segment(id_q,nq),
                     q1.segment(id_q,nq),
                     PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J).block(id_v,id_v,nv,nv),
                     arg);

    id_q += nq; id_v += nv;
  }
}

template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
void CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
dDifference_product_impl(const ConfigL_t & q0,
                         const ConfigR_t & q1,
                         const JacobianIn_t & Jin,
                         JacobianOut_t & Jout,
                         bool dDifferenceOnTheLeft,
                         const AssignmentOperatorType op) const
{
  Index id_q = 0, id_v = 0;
  for(size_t k = 0; k < liegroups.size(); ++k)
  {
    const Index & nq = lg_nqs[k];
    const Index & nv = lg_nvs[k];
    if (dDifferenceOnTheLeft)
      ::pinocchio::dDifference<arg>(liegroups[k],
                       q0.segment(id_q,nq),
                       q1.segment(id_q,nq),
                       SELF,
                       Jin.middleRows(id_v,nv),
                       Jout.middleRows(id_v,nv),
                       op);
    else
      ::pinocchio::dDifference<arg>(liegroups[k],
                       q0.segment(id_q,nq),
                       q1.segment(id_q,nq),
                       Jin.middleCols(id_v,nv),
                       SELF,
                       Jout.middleCols(id_v,nv),
                       op);

    id_q += nq; id_v += nv;
  }
}

template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
template <class ConfigIn_t, class Velocity_t, class ConfigOut_t>
void CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q,
               const Eigen::MatrixBase<Velocity_t> & v,
               const Eigen::MatrixBase<ConfigOut_t> & qout) const
{
  ConfigOut_t & qout_ = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout);
  Index id_q = 0, id_v = 0;
  for(size_t k = 0; k < liegroups.size(); ++k)
  {
    const Index & nq = lg_nqs[k];
    const Index & nv = lg_nvs[k];
    ::pinocchio::integrate(liegroups[k],
                     q.segment(id_q,nq),
                     v.segment(id_v,nv),
                     qout_.segment(id_q,nq));

    id_q += nq; id_v += nv;
  }
}

template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
template <class Config_t, class Jacobian_t>
void CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
integrateCoeffWiseJacobian_impl(const Eigen::MatrixBase<Config_t> & q,
                                const Eigen::MatrixBase<Jacobian_t> & J) const
{
  PINOCCHIO_UNUSED_VARIABLE(q);
  PINOCCHIO_UNUSED_VARIABLE(J);
// not implemented yet      assert(J.rows() == nq() && J.cols() == nv() && "J is not of the right dimension");
// not implemented yet      Jacobian_t & J_ = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J);
// not implemented yet      J_.topRightCorner(lg1.nq(),lg2.nv()).setZero();
// not implemented yet      J_.bottomLeftCorner(lg2.nq(),lg1.nv()).setZero();
// not implemented yet
// not implemented yet      lg1.integrateCoeffWiseJacobian(Q1(q),
// not implemented yet                                      J_.topLeftCorner(lg1.nq(),lg1.nv()));
// not implemented yet      lg2.integrateCoeffWiseJacobian(Q2(q), J_.bottomRightCorner(lg2.nq(),lg2.nv()));
}

template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
template <class Config_t, class Tangent_t, class JacobianOut_t>
void CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
dIntegrate_dq_impl(const Eigen::MatrixBase<Config_t > & q,
                   const Eigen::MatrixBase<Tangent_t> & v,
                   const Eigen::MatrixBase<JacobianOut_t> & J,
                   const AssignmentOperatorType op) const
{
  if (op == SETTO)
    PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J).setZero();
  Index id_q = 0, id_v = 0;
  for(size_t k = 0; k < liegroups.size(); ++k)
  {
    const Index & nq = lg_nqs[k];
    const Index & nv = lg_nvs[k];
    ::pinocchio::dIntegrate(liegroups[k],
                     q.segment(id_q,nq),
                     v.segment(id_v,nv),
                     PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J).block(id_v,id_v,nv,nv),
                     ARG0,
                     op);

    id_q += nq; id_v += nv;
  }
}

template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
template <class Config_t, class Tangent_t, class JacobianOut_t>
void CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
dIntegrate_dv_impl(const Eigen::MatrixBase<Config_t > & q,
                   const Eigen::MatrixBase<Tangent_t> & v,
                   const Eigen::MatrixBase<JacobianOut_t> & J,
                   const AssignmentOperatorType op) const
{
  if (op == SETTO)
    PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J).setZero();
  Index id_q = 0, id_v = 0;
  for(size_t k = 0; k < liegroups.size(); ++k)
  {
    const Index & nq = lg_nqs[k];
    const Index & nv = lg_nvs[k];
    ::pinocchio::dIntegrate(liegroups[k],
                     q.segment(id_q,nq),
                     v.segment(id_v,nv),
                     PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J).block(id_v,id_v,nv,nv),
                     ARG1,
                     op);

    id_q += nq; id_v += nv;
  }
}

template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
dIntegrate_product_impl(const Config_t & q,
                        const Tangent_t & v,
                        const JacobianIn_t & Jin,
                        JacobianOut_t & Jout,
                        bool dIntegrateOnTheLeft,
                        const ArgumentPosition arg,
                        const AssignmentOperatorType op) const
{
  Index id_q = 0, id_v = 0;
  for(size_t k = 0; k < liegroups.size(); ++k)
  {
    const Index & nq = lg_nqs[k];
    const Index & nv = lg_nvs[k];
    if(dIntegrateOnTheLeft)
      ::pinocchio::dIntegrate(liegroups[k],
                       q.segment(id_q,nq),
                       v.segment(id_v,nv),
                       SELF,
                       Jin.middleRows(id_v,nv),
                       Jout.middleRows(id_v,nv),
                       arg, op);
    else
      ::pinocchio::dIntegrate(liegroups[k],
                       q.segment(id_q,nq),
                       v.segment(id_v,nv),
                       Jin.middleCols(id_v,nv),
                       SELF,
                       Jout.middleCols(id_v,nv),
                       arg, op);

    id_q += nq; id_v += nv;
  }
}

template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & q,
                            const Eigen::MatrixBase<Tangent_t> & v,
                            const Eigen::MatrixBase<JacobianIn_t> & J_in,
                            const Eigen::MatrixBase<JacobianOut_t> & J_out) const
{
  Index id_q = 0, id_v = 0;
  for(size_t k = 0; k < liegroups.size(); ++k)
  {
    const Index & nq = lg_nqs[k];
    const Index & nv = lg_nvs[k];
    ::pinocchio::dIntegrateTransport(liegroups[k],
                     q.segment(id_q,nq),
                     v.segment(id_v,nv),
                     J_in.middleRows(id_v,nv),
                     PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out).middleRows(id_v,nv),
                     ARG0);

    id_q += nq; id_v += nv;
  }
}

template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & q,
                            const Eigen::MatrixBase<Tangent_t> & v,
                            const Eigen::MatrixBase<JacobianIn_t> & J_in,
                            const Eigen::MatrixBase<JacobianOut_t> & J_out) const
{
  Index id_q = 0, id_v = 0;
  for(size_t k = 0; k < liegroups.size(); ++k)
  {
    const Index & nq = lg_nqs[k];
    const Index & nv = lg_nvs[k];
    ::pinocchio::dIntegrateTransport(liegroups[k],
                     q.segment(id_q,nq),
                     v.segment(id_v,nv),
                     J_in.middleRows(id_v,nv),
                     PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out).middleRows(id_v,nv),
                     ARG1);

    id_q += nq; id_v += nv;
  }
}

template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
template <class Config_t, class Tangent_t, class JacobianOut_t>
void CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & q,
                            const Eigen::MatrixBase<Tangent_t> & v,
                            const Eigen::MatrixBase<JacobianOut_t> & J) const
{
  Index id_q = 0, id_v = 0;
  for(size_t k = 0; k < liegroups.size(); ++k)
  {
    const Index & nq = lg_nqs[k];
    const Index & nv = lg_nvs[k];
    ::pinocchio::dIntegrateTransport(liegroups[k],
                     q.segment(id_q,nq),
                     v.segment(id_v,nv),
                     PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J).middleRows(id_v,nv),
                     ARG0);

    id_q += nq; id_v += nv;
  }
}

template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
template <class Config_t, class Tangent_t, class JacobianOut_t>
void CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & q,
                            const Eigen::MatrixBase<Tangent_t> & v,
                            const Eigen::MatrixBase<JacobianOut_t> & J) const
{
  Index id_q = 0, id_v = 0;
  for(size_t k = 0; k < liegroups.size(); ++k)
  {
    const Index & nq = lg_nqs[k];
    const Index & nv = lg_nvs[k];
    ::pinocchio::dIntegrateTransport(liegroups[k],
                     q.segment(id_q,nq),
                     v.segment(id_v,nv),
                     PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J).middleRows(id_v,nv),
                     ARG1);

    id_q += nq; id_v += nv;
  }
}

template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
template <class ConfigL_t, class ConfigR_t>
typename CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::Scalar
CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
                     const Eigen::MatrixBase<ConfigR_t> & q1) const
{
  Scalar d2 = 0;
  Index id_q = 0;
  for(size_t k = 0; k < liegroups.size(); ++k)
  {
    const Index & nq = lg_nqs[k];
    d2 += ::pinocchio::squaredDistance(liegroups[k],
        q0.segment(id_q,nq),
        q1.segment(id_q,nq));
    id_q += nq;
  }
  return d2;
}

template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
template <class Config_t>
void CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
normalize_impl (const Eigen::MatrixBase<Config_t>& qout) const
{
  Index id_q = 0;
  for(size_t k = 0; k < liegroups.size(); ++k)
  {
    const Index & nq = lg_nqs[k];
    ::pinocchio::normalize(liegroups[k],
        PINOCCHIO_EIGEN_CONST_CAST(Config_t, qout).segment(id_q,nq));
    id_q += nq;
  }
}

template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
template <class Config_t>
bool CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
isNormalized_impl (const Eigen::MatrixBase<Config_t>& qin,
                   const Scalar& prec) const
{
  Index id_q = 0;
  for(size_t k = 0; k < liegroups.size(); ++k)
  {
    const Index nq = lg_nqs[k];
    const bool res_k = ::pinocchio::isNormalized(liegroups[k],
        PINOCCHIO_EIGEN_CONST_CAST(Config_t, qin).segment(id_q,nq), prec);
    if(!res_k)
      return false;
    id_q += nq;
  }
  return true;
}

template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
template <class Config_t>
void CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
random_impl (const Eigen::MatrixBase<Config_t>& qout) const
{
  Index id_q = 0;
  for(size_t k = 0; k < liegroups.size(); ++k)
  {
    const Index & nq = lg_nqs[k];
    ::pinocchio::random(liegroups[k],
        PINOCCHIO_EIGEN_CONST_CAST(Config_t, qout).segment(id_q,nq));
    id_q += nq;
  }
}

template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
void CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower,
                         const Eigen::MatrixBase<ConfigR_t> & upper,
                         const Eigen::MatrixBase<ConfigOut_t> & qout) const
{
  Index id_q = 0;
  for(size_t k = 0; k < liegroups.size(); ++k)
  {
    const Index & nq = lg_nqs[k];
    ::pinocchio::randomConfiguration(liegroups[k],
                     lower.segment(id_q,nq),
                     upper.segment(id_q,nq),
                     PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t, qout).segment(id_q,nq));

    id_q += nq;
  }
}

template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
template <class ConfigL_t, class ConfigR_t>
bool CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
                         const Eigen::MatrixBase<ConfigR_t> & q1,
                         const Scalar & prec) const
{
  Index id_q = 0;
  for(size_t k = 0; k < liegroups.size(); ++k)
  {
    const Index & nq = lg_nqs[k];
    if (!::pinocchio::isSameConfiguration(liegroups[k],
                                          q0.segment(id_q,nq),
                                          q1.segment(id_q,nq),
                                          prec))
      return false;

    id_q += nq;
  }
  return true;
}

template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>
CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
operator* (const CartesianProductOperationVariantTpl& other) const
{
  CartesianProductOperationVariantTpl res;

  res.liegroups.reserve(liegroups.size() + other.liegroups.size());
  res.liegroups.insert(res.liegroups.end(), liegroups.begin(), liegroups.end());
  res.liegroups.insert(res.liegroups.end(), other.liegroups.begin(), other.liegroups.end());

  res.lg_nqs.reserve(lg_nqs.size() + other.lg_nqs.size());
  res.lg_nqs.insert(res.lg_nqs.end(), lg_nqs.begin(), lg_nqs.end());
  res.lg_nqs.insert(res.lg_nqs.end(), other.lg_nqs.begin(), other.lg_nqs.end());

  res.lg_nvs.reserve(lg_nvs.size() + other.lg_nvs.size());
  res.lg_nvs.insert(res.lg_nvs.end(), lg_nvs.begin(), lg_nvs.end());
  res.lg_nvs.insert(res.lg_nvs.end(), other.lg_nvs.begin(), other.lg_nvs.end());

  res.m_nq = m_nq + other.m_nq;
  res.m_nv = m_nv + other.m_nv;

  if(liegroups.size() > 0)
    res.m_name = m_name;
  if(other.liegroups.size() > 0) {
    if (liegroups.size() > 0)
      res.m_name += " x ";
    res.m_name += other.m_name;
  }

  res.m_neutral.resize(res.m_nq);
  res.m_neutral.head(m_nq) = m_neutral;
  res.m_neutral.tail(other.m_nq) = other.m_neutral;

  return res;
}

template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>&
CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
operator*= (const CartesianProductOperationVariantTpl& other)
{
  liegroups.insert(liegroups.end(), other.liegroups.begin(), other.liegroups.end());

  lg_nqs.insert(lg_nqs.end(), other.lg_nqs.begin(), other.lg_nqs.end());
  lg_nvs.insert(lg_nvs.end(), other.lg_nvs.begin(), other.lg_nvs.end());

  m_nq += other.m_nq;
  m_nv += other.m_nv;

  if(other.liegroups.size() > 0) {
    if (liegroups.size())
      m_name += " x ";
    m_name += other.m_name;
  }

  m_neutral.conservativeResize(m_nq);
  m_neutral.tail(other.m_nq) = other.m_neutral;

  return *this;
}

template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
bool CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
isEqual_impl (const CartesianProductOperationVariantTpl& other) const
{
  if (liegroups.size() != other.liegroups.size())
    return false;
  for(size_t k = 0; k < liegroups.size(); ++k)
    if (liegroups[k].isDifferent_impl(other.liegroups[k]))
      return false;
  return true;
}

template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
template <typename LieGroup1, typename LieGroup2>
bool CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
isEqual(const CartesianProductOperation<LieGroup1, LieGroup2> & other) const
{
  if (liegroups.size() != 2)
    return false;
  if (liegroups[0].isDifferent_impl(LieGroupGeneric(other.lg1)))
    return false;
  if (liegroups[1].isDifferent_impl(LieGroupGeneric(other.lg2)))
    return false;
  return true;
}

} // namespace pinocchio

#endif // ifndef __pinocchio_cartesian_product_variant_hxx__


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2020 CNRS
3			//
4
5			#ifndef __pinocchio_cartesian_product_variant_hxx__
6			#define __pinocchio_cartesian_product_variant_hxx__
7
8			#include "pinocchio/multibody/liegroup/liegroup-variant-visitors.hpp"
9
10			namespace pinocchio
11			{
12
13			template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
14		20	void CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
15			append(const LieGroupGeneric & lg)
16			{
17	✓✗	20	liegroups.push_back(lg);
18	✓✗✓✗	20	const Index lg_nq = ::pinocchio::nq(lg); lg_nqs.push_back(lg_nq); m_nq += lg_nq;
19	✓✗✓✗	20	const Index lg_nv = ::pinocchio::nv(lg); lg_nvs.push_back(lg_nv); m_nv += lg_nv;
20
21	✓✓	20	if(liegroups.size() > 1)
22	✓✗	2	m_name += " x ";
23	✓✗✓✗	20	m_name += ::pinocchio::name(lg);
24
25	✓✗	20	m_neutral.conservativeResize(m_nq);
26	✓✗✓✗ ✓✗	20	m_neutral.tail(lg_nq) = ::pinocchio::neutral(lg);
27
28		20	}
29
30			template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
31			template <class ConfigL_t, class ConfigR_t, class Tangent_t>
32		18	void CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
33			difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
34			const Eigen::MatrixBase<ConfigR_t> & q1,
35			const Eigen::MatrixBase<Tangent_t> & d) const
36			{
37		18	Index id_q = 0, id_v = 0;
38	✓✓	38	for(size_t k = 0; k < liegroups.size(); ++k)
39			{
40		20	const Index & nq = lg_nqs[k];
41		20	const Index & nv = lg_nvs[k];
42	✓✗✓✗ ✓✗	20	::pinocchio::difference(liegroups[k],
43			q0.segment(id_q,nq),
44			q1.segment(id_q,nq),
45		20	PINOCCHIO_EIGEN_CONST_CAST(Tangent_t, d).segment(id_v,nv));
46
47		20	id_q += nq; id_v += nv;
48			}
49		18	}
50
51			template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
52			template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
53		36	void CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
54			dDifference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
55			const Eigen::MatrixBase<ConfigR_t> & q1,
56			const Eigen::MatrixBase<JacobianOut_t> & J) const
57			{
58		36	PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J).setZero();
59		36	Index id_q = 0, id_v = 0;
60	✓✓	76	for(size_t k = 0; k < liegroups.size(); ++k)
61			{
62		40	const Index & nq = lg_nqs[k];
63		40	const Index & nv = lg_nvs[k];
64	✓✗✓✗ ✓✗	40	::pinocchio::dDifference(liegroups[k],
65			q0.segment(id_q,nq),
66			q1.segment(id_q,nq),
67		40	PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J).block(id_v,id_v,nv,nv),
68			arg);
69
70		40	id_q += nq; id_v += nv;
71			}
72		36	}
73
74			template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
75			template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
76			void CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
77			dDifference_product_impl(const ConfigL_t & q0,
78			const ConfigR_t & q1,
79			const JacobianIn_t & Jin,
80			JacobianOut_t & Jout,
81			bool dDifferenceOnTheLeft,
82			const AssignmentOperatorType op) const
83			{
84			Index id_q = 0, id_v = 0;
85			for(size_t k = 0; k < liegroups.size(); ++k)
86			{
87			const Index & nq = lg_nqs[k];
88			const Index & nv = lg_nvs[k];
89			if (dDifferenceOnTheLeft)
90			::pinocchio::dDifference<arg>(liegroups[k],
91			q0.segment(id_q,nq),
92			q1.segment(id_q,nq),
93			SELF,
94			Jin.middleRows(id_v,nv),
95			Jout.middleRows(id_v,nv),
96			op);
97			else
98			::pinocchio::dDifference<arg>(liegroups[k],
99			q0.segment(id_q,nq),
100			q1.segment(id_q,nq),
101			Jin.middleCols(id_v,nv),
102			SELF,
103			Jout.middleCols(id_v,nv),
104			op);
105
106			id_q += nq; id_v += nv;
107			}
108			}
109
110			template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
111			template <class ConfigIn_t, class Velocity_t, class ConfigOut_t>
112		18	void CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
113			integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q,
114			const Eigen::MatrixBase<Velocity_t> & v,
115			const Eigen::MatrixBase<ConfigOut_t> & qout) const
116			{
117		18	ConfigOut_t & qout_ = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout);
118		18	Index id_q = 0, id_v = 0;
119	✓✓	38	for(size_t k = 0; k < liegroups.size(); ++k)
120			{
121		20	const Index & nq = lg_nqs[k];
122		20	const Index & nv = lg_nvs[k];
123	✓✗✓✗ ✓✗	20	::pinocchio::integrate(liegroups[k],
124			q.segment(id_q,nq),
125			v.segment(id_v,nv),
126			qout_.segment(id_q,nq));
127
128		20	id_q += nq; id_v += nv;
129			}
130		18	}
131
132			template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
133			template <class Config_t, class Jacobian_t>
134			void CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
135			integrateCoeffWiseJacobian_impl(const Eigen::MatrixBase<Config_t> & q,
136			const Eigen::MatrixBase<Jacobian_t> & J) const
137			{
138			PINOCCHIO_UNUSED_VARIABLE(q);
139			PINOCCHIO_UNUSED_VARIABLE(J);
140			// not implemented yet assert(J.rows() == nq() && J.cols() == nv() && "J is not of the right dimension");
141			// not implemented yet Jacobian_t & J_ = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J);
142			// not implemented yet J_.topRightCorner(lg1.nq(),lg2.nv()).setZero();
143			// not implemented yet J_.bottomLeftCorner(lg2.nq(),lg1.nv()).setZero();
144			// not implemented yet
145			// not implemented yet lg1.integrateCoeffWiseJacobian(Q1(q),
146			// not implemented yet J_.topLeftCorner(lg1.nq(),lg1.nv()));
147			// not implemented yet lg2.integrateCoeffWiseJacobian(Q2(q), J_.bottomRightCorner(lg2.nq(),lg2.nv()));
148			}
149
150			template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
151			template <class Config_t, class Tangent_t, class JacobianOut_t>
152		18	void CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
153			dIntegrate_dq_impl(const Eigen::MatrixBase<Config_t > & q,
154			const Eigen::MatrixBase<Tangent_t> & v,
155			const Eigen::MatrixBase<JacobianOut_t> & J,
156			const AssignmentOperatorType op) const
157			{
158	✓✗	18	if (op == SETTO)
159		18	PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J).setZero();
160		18	Index id_q = 0, id_v = 0;
161	✓✓	38	for(size_t k = 0; k < liegroups.size(); ++k)
162			{
163		20	const Index & nq = lg_nqs[k];
164		20	const Index & nv = lg_nvs[k];
165	✓✗✓✗ ✓✗	20	::pinocchio::dIntegrate(liegroups[k],
166			q.segment(id_q,nq),
167			v.segment(id_v,nv),
168		20	PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J).block(id_v,id_v,nv,nv),
169			ARG0,
170			op);
171
172		20	id_q += nq; id_v += nv;
173			}
174		18	}
175
176			template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
177			template <class Config_t, class Tangent_t, class JacobianOut_t>
178		18	void CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
179			dIntegrate_dv_impl(const Eigen::MatrixBase<Config_t > & q,
180			const Eigen::MatrixBase<Tangent_t> & v,
181			const Eigen::MatrixBase<JacobianOut_t> & J,
182			const AssignmentOperatorType op) const
183			{
184	✓✗	18	if (op == SETTO)
185		18	PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J).setZero();
186		18	Index id_q = 0, id_v = 0;
187	✓✓	38	for(size_t k = 0; k < liegroups.size(); ++k)
188			{
189		20	const Index & nq = lg_nqs[k];
190		20	const Index & nv = lg_nvs[k];
191	✓✗✓✗ ✓✗	20	::pinocchio::dIntegrate(liegroups[k],
192			q.segment(id_q,nq),
193			v.segment(id_v,nv),
194		20	PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J).block(id_v,id_v,nv,nv),
195			ARG1,
196			op);
197
198		20	id_q += nq; id_v += nv;
199			}
200		18	}
201
202			template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
203			template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
204			void CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
205			dIntegrate_product_impl(const Config_t & q,
206			const Tangent_t & v,
207			const JacobianIn_t & Jin,
208			JacobianOut_t & Jout,
209			bool dIntegrateOnTheLeft,
210			const ArgumentPosition arg,
211			const AssignmentOperatorType op) const
212			{
213			Index id_q = 0, id_v = 0;
214			for(size_t k = 0; k < liegroups.size(); ++k)
215			{
216			const Index & nq = lg_nqs[k];
217			const Index & nv = lg_nvs[k];
218			if(dIntegrateOnTheLeft)
219			::pinocchio::dIntegrate(liegroups[k],
220			q.segment(id_q,nq),
221			v.segment(id_v,nv),
222			SELF,
223			Jin.middleRows(id_v,nv),
224			Jout.middleRows(id_v,nv),
225			arg, op);
226			else
227			::pinocchio::dIntegrate(liegroups[k],
228			q.segment(id_q,nq),
229			v.segment(id_v,nv),
230			Jin.middleCols(id_v,nv),
231			SELF,
232			Jout.middleCols(id_v,nv),
233			arg, op);
234
235			id_q += nq; id_v += nv;
236			}
237			}
238
239			template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
240			template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
241			void CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
242			dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & q,
243			const Eigen::MatrixBase<Tangent_t> & v,
244			const Eigen::MatrixBase<JacobianIn_t> & J_in,
245			const Eigen::MatrixBase<JacobianOut_t> & J_out) const
246			{
247			Index id_q = 0, id_v = 0;
248			for(size_t k = 0; k < liegroups.size(); ++k)
249			{
250			const Index & nq = lg_nqs[k];
251			const Index & nv = lg_nvs[k];
252			::pinocchio::dIntegrateTransport(liegroups[k],
253			q.segment(id_q,nq),
254			v.segment(id_v,nv),
255			J_in.middleRows(id_v,nv),
256			PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out).middleRows(id_v,nv),
257			ARG0);
258
259			id_q += nq; id_v += nv;
260			}
261			}
262
263			template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
264			template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
265			void CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
266			dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & q,
267			const Eigen::MatrixBase<Tangent_t> & v,
268			const Eigen::MatrixBase<JacobianIn_t> & J_in,
269			const Eigen::MatrixBase<JacobianOut_t> & J_out) const
270			{
271			Index id_q = 0, id_v = 0;
272			for(size_t k = 0; k < liegroups.size(); ++k)
273			{
274			const Index & nq = lg_nqs[k];
275			const Index & nv = lg_nvs[k];
276			::pinocchio::dIntegrateTransport(liegroups[k],
277			q.segment(id_q,nq),
278			v.segment(id_v,nv),
279			J_in.middleRows(id_v,nv),
280			PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out).middleRows(id_v,nv),
281			ARG1);
282
283			id_q += nq; id_v += nv;
284			}
285			}
286
287			template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
288			template <class Config_t, class Tangent_t, class JacobianOut_t>
289			void CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
290			dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & q,
291			const Eigen::MatrixBase<Tangent_t> & v,
292			const Eigen::MatrixBase<JacobianOut_t> & J) const
293			{
294			Index id_q = 0, id_v = 0;
295			for(size_t k = 0; k < liegroups.size(); ++k)
296			{
297			const Index & nq = lg_nqs[k];
298			const Index & nv = lg_nvs[k];
299			::pinocchio::dIntegrateTransport(liegroups[k],
300			q.segment(id_q,nq),
301			v.segment(id_v,nv),
302			PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J).middleRows(id_v,nv),
303			ARG0);
304
305			id_q += nq; id_v += nv;
306			}
307			}
308
309			template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
310			template <class Config_t, class Tangent_t, class JacobianOut_t>
311			void CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
312			dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & q,
313			const Eigen::MatrixBase<Tangent_t> & v,
314			const Eigen::MatrixBase<JacobianOut_t> & J) const
315			{
316			Index id_q = 0, id_v = 0;
317			for(size_t k = 0; k < liegroups.size(); ++k)
318			{
319			const Index & nq = lg_nqs[k];
320			const Index & nv = lg_nvs[k];
321			::pinocchio::dIntegrateTransport(liegroups[k],
322			q.segment(id_q,nq),
323			v.segment(id_v,nv),
324			PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J).middleRows(id_v,nv),
325			ARG1);
326
327			id_q += nq; id_v += nv;
328			}
329			}
330
331			template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
332			template <class ConfigL_t, class ConfigR_t>
333			typename CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::Scalar
334		36	CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
335			squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
336			const Eigen::MatrixBase<ConfigR_t> & q1) const
337			{
338		36	Scalar d2 = 0;
339		36	Index id_q = 0;
340	✓✓	76	for(size_t k = 0; k < liegroups.size(); ++k)
341			{
342		40	const Index & nq = lg_nqs[k];
343	✓✗✓✗	40	d2 += ::pinocchio::squaredDistance(liegroups[k],
344			q0.segment(id_q,nq),
345			q1.segment(id_q,nq));
346		40	id_q += nq;
347			}
348		36	return d2;
349			}
350
351			template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
352			template <class Config_t>
353		18	void CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
354			normalize_impl (const Eigen::MatrixBase<Config_t>& qout) const
355			{
356		18	Index id_q = 0;
357	✓✓	38	for(size_t k = 0; k < liegroups.size(); ++k)
358			{
359		20	const Index & nq = lg_nqs[k];
360	✓✗	20	::pinocchio::normalize(liegroups[k],
361		20	PINOCCHIO_EIGEN_CONST_CAST(Config_t, qout).segment(id_q,nq));
362		20	id_q += nq;
363			}
364		18	}
365
366			template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
367			template <class Config_t>
368			bool CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
369			isNormalized_impl (const Eigen::MatrixBase<Config_t>& qin,
370			const Scalar& prec) const
371			{
372			Index id_q = 0;
373			for(size_t k = 0; k < liegroups.size(); ++k)
374			{
375			const Index nq = lg_nqs[k];
376			const bool res_k = ::pinocchio::isNormalized(liegroups[k],
377			PINOCCHIO_EIGEN_CONST_CAST(Config_t, qin).segment(id_q,nq), prec);
378			if(!res_k)
379			return false;
380			id_q += nq;
381			}
382			return true;
383			}
384
385			template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
386			template <class Config_t>
387		18	void CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
388			random_impl (const Eigen::MatrixBase<Config_t>& qout) const
389			{
390		18	Index id_q = 0;
391	✓✓	38	for(size_t k = 0; k < liegroups.size(); ++k)
392			{
393		20	const Index & nq = lg_nqs[k];
394	✓✗	20	::pinocchio::random(liegroups[k],
395		20	PINOCCHIO_EIGEN_CONST_CAST(Config_t, qout).segment(id_q,nq));
396		20	id_q += nq;
397			}
398		18	}
399
400			template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
401			template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
402		18	void CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
403			randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower,
404			const Eigen::MatrixBase<ConfigR_t> & upper,
405			const Eigen::MatrixBase<ConfigOut_t> & qout) const
406			{
407		18	Index id_q = 0;
408	✓✓	38	for(size_t k = 0; k < liegroups.size(); ++k)
409			{
410		20	const Index & nq = lg_nqs[k];
411	✓✗✓✗ ✓✗	20	::pinocchio::randomConfiguration(liegroups[k],
412			lower.segment(id_q,nq),
413			upper.segment(id_q,nq),
414		20	PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t, qout).segment(id_q,nq));
415
416		20	id_q += nq;
417			}
418		18	}
419
420			template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
421			template <class ConfigL_t, class ConfigR_t>
422		18	bool CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
423			isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
424			const Eigen::MatrixBase<ConfigR_t> & q1,
425			const Scalar & prec) const
426			{
427		18	Index id_q = 0;
428	✓✓	38	for(size_t k = 0; k < liegroups.size(); ++k)
429			{
430		20	const Index & nq = lg_nqs[k];
431	✓✗✓✗ ✗✓	20	if (!::pinocchio::isSameConfiguration(liegroups[k],
432			q0.segment(id_q,nq),
433			q1.segment(id_q,nq),
434			prec))
435			return false;
436
437		20	id_q += nq;
438			}
439		18	return true;
440			}
441
442			template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
443			CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>
444			CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
445			operator* (const CartesianProductOperationVariantTpl& other) const
446			{
447			CartesianProductOperationVariantTpl res;
448
449			res.liegroups.reserve(liegroups.size() + other.liegroups.size());
450			res.liegroups.insert(res.liegroups.end(), liegroups.begin(), liegroups.end());
451			res.liegroups.insert(res.liegroups.end(), other.liegroups.begin(), other.liegroups.end());
452
453			res.lg_nqs.reserve(lg_nqs.size() + other.lg_nqs.size());
454			res.lg_nqs.insert(res.lg_nqs.end(), lg_nqs.begin(), lg_nqs.end());
455			res.lg_nqs.insert(res.lg_nqs.end(), other.lg_nqs.begin(), other.lg_nqs.end());
456
457			res.lg_nvs.reserve(lg_nvs.size() + other.lg_nvs.size());
458			res.lg_nvs.insert(res.lg_nvs.end(), lg_nvs.begin(), lg_nvs.end());
459			res.lg_nvs.insert(res.lg_nvs.end(), other.lg_nvs.begin(), other.lg_nvs.end());
460
461			res.m_nq = m_nq + other.m_nq;
462			res.m_nv = m_nv + other.m_nv;
463
464			if(liegroups.size() > 0)
465			res.m_name = m_name;
466			if(other.liegroups.size() > 0) {
467			if (liegroups.size() > 0)
468			res.m_name += " x ";
469			res.m_name += other.m_name;
470			}
471
472			res.m_neutral.resize(res.m_nq);
473			res.m_neutral.head(m_nq) = m_neutral;
474			res.m_neutral.tail(other.m_nq) = other.m_neutral;
475
476			return res;
477			}
478
479			template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
480			CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>&
481			CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
482			operator*= (const CartesianProductOperationVariantTpl& other)
483			{
484			liegroups.insert(liegroups.end(), other.liegroups.begin(), other.liegroups.end());
485
486			lg_nqs.insert(lg_nqs.end(), other.lg_nqs.begin(), other.lg_nqs.end());
487			lg_nvs.insert(lg_nvs.end(), other.lg_nvs.begin(), other.lg_nvs.end());
488
489			m_nq += other.m_nq;
490			m_nv += other.m_nv;
491
492			if(other.liegroups.size() > 0) {
493			if (liegroups.size())
494			m_name += " x ";
495			m_name += other.m_name;
496			}
497
498			m_neutral.conservativeResize(m_nq);
499			m_neutral.tail(other.m_nq) = other.m_neutral;
500
501			return *this;
502			}
503
504			template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
505		9	bool CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
506			isEqual_impl (const CartesianProductOperationVariantTpl& other) const
507			{
508	✗✓	9	if (liegroups.size() != other.liegroups.size())
509			return false;
510	✓✓	19	for(size_t k = 0; k < liegroups.size(); ++k)
511	✗✓	10	if (liegroups[k].isDifferent_impl(other.liegroups[k]))
512			return false;
513		9	return true;
514			}
515
516			template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
517			template <typename LieGroup1, typename LieGroup2>
518			bool CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
519			isEqual(const CartesianProductOperation<LieGroup1, LieGroup2> & other) const
520			{
521			if (liegroups.size() != 2)
522			return false;
523			if (liegroups[0].isDifferent_impl(LieGroupGeneric(other.lg1)))
524			return false;
525			if (liegroups[1].isDifferent_impl(LieGroupGeneric(other.lg2)))
526			return false;
527			return true;
528			}
529
530			} // namespace pinocchio
531
532			#endif // ifndef __pinocchio_cartesian_product_variant_hxx__