cartesian-product.hpp

Go to the documentation of this file.

//
// Copyright (c) 2016-2020 CNRS CNRS
//

#ifndef __pinocchio_multibody_liegroup_cartesian_product_operation_hpp__
#define __pinocchio_multibody_liegroup_cartesian_product_operation_hpp__

#include <pinocchio/multibody/liegroup/liegroup-base.hpp>

namespace pinocchio
{
  template<int dim1, int dim2>
  struct eval_set_dim
  {
    enum { value = dim1 + dim2 };
  };
  
  template<int dim>
  struct eval_set_dim<dim,Eigen::Dynamic>
  {
    enum { value = Eigen::Dynamic };
  };

  template<int dim>
  struct eval_set_dim<Eigen::Dynamic,dim>
  {
    enum { value = Eigen::Dynamic };
  };
  
  template<typename LieGroup1, typename LieGroup2>
  struct CartesianProductOperation;
  
  template<typename LieGroup1, typename LieGroup2>
  struct traits<CartesianProductOperation<LieGroup1, LieGroup2> > {
    typedef typename traits<LieGroup1>::Scalar Scalar;
    enum {
      Options = traits<LieGroup1>::Options,
      NQ = eval_set_dim<LieGroup1::NQ,LieGroup2::NQ>::value,
      NV = eval_set_dim<LieGroup1::NV,LieGroup2::NV>::value
    };
  };

  template<typename LieGroup1, typename LieGroup2>
  struct CartesianProductOperation : public LieGroupBase <CartesianProductOperation<LieGroup1, LieGroup2> >
  {
    PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(CartesianProductOperation);

    CartesianProductOperation () : lg1 (), lg2 ()
    {
    }
    // Get dimension of Lie Group vector representation
    //
    // For instance, for SO(3), the dimension of the vector representation is
    // 4 (quaternion) while the dimension of the tangent space is 3.
    Index nq () const
    {
      return lg1.nq () + lg2.nq ();
    }
    
    // Get dimension of Lie Group tangent space
    Index nv () const
    {
      return lg1.nv () + lg2.nv ();
    }

    ConfigVector_t neutral () const
    {
      ConfigVector_t n;
      n.resize (nq ());
      Qo1(n) = lg1.neutral ();
      Qo2(n) = lg2.neutral ();
      return n;
    }

    std::string name () const
    {
      std::ostringstream oss; oss << lg1.name () << "*" << lg2.name ();
      return oss.str ();
    }

    template <class ConfigL_t, class ConfigR_t, class Tangent_t>
    void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
                         const Eigen::MatrixBase<ConfigR_t> & q1,
                         const Eigen::MatrixBase<Tangent_t> & d) const
    {
      lg1.difference(Q1(q0), Q1(q1), Vo1(d));
      lg2.difference(Q2(q0), Q2(q1), Vo2(d));
    }

    template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
    void dDifference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
                          const Eigen::MatrixBase<ConfigR_t> & q1,
                          const Eigen::MatrixBase<JacobianOut_t> & J) const
    {
      J12(J).setZero();
      J21(J).setZero();

      lg1.template dDifference<arg> (Q1(q0), Q1(q1), J11(J));
      lg2.template dDifference<arg> (Q2(q0), Q2(q1), J22(J));
    }

    template <class ConfigIn_t, class Velocity_t, class ConfigOut_t>
    void integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q,
                        const Eigen::MatrixBase<Velocity_t> & v,
                        const Eigen::MatrixBase<ConfigOut_t> & qout) const
    {
      lg1.integrate(Q1(q), V1(v), Qo1(qout));
      lg2.integrate(Q2(q), V2(v), Qo2(qout));
    }
    
    template <class Config_t, class Jacobian_t>
    void integrateCoeffWiseJacobian_impl(const Eigen::MatrixBase<Config_t> & q,
                                         const Eigen::MatrixBase<Jacobian_t> & J) const
    {
      assert(J.rows() == nq() && J.cols() == nv() && "J is not of the right dimension");
      Jacobian_t & J_ = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J);
      J_.topRightCorner(lg1.nq(),lg2.nv()).setZero();
      J_.bottomLeftCorner(lg2.nq(),lg1.nv()).setZero();
      
      lg1.integrateCoeffWiseJacobian(Q1(q),
                                      J_.topLeftCorner(lg1.nq(),lg1.nv()));
      lg2.integrateCoeffWiseJacobian(Q2(q), J_.bottomRightCorner(lg2.nq(),lg2.nv()));
    }

    template <class Config_t, class Tangent_t, class JacobianOut_t>
    void dIntegrate_dq_impl(const Eigen::MatrixBase<Config_t > & q,
                            const Eigen::MatrixBase<Tangent_t> & v,
                            const Eigen::MatrixBase<JacobianOut_t> & J,
                            const AssignmentOperatorType op=SETTO) const
    {
      switch(op)
        {
        case SETTO:
          J12(J).setZero();
          J21(J).setZero();
          // fallthrough
        case ADDTO:
        case RMTO:
          lg1.dIntegrate_dq(Q1(q), V1(v), J11(J),op);
          lg2.dIntegrate_dq(Q2(q), V2(v), J22(J),op);
          break;
        default:
          assert(false && "Wrong Op requesed value");
          break;
        }      
    }

    template <class Config_t, class Tangent_t, class JacobianOut_t>
    void dIntegrate_dv_impl(const Eigen::MatrixBase<Config_t > & q,
                            const Eigen::MatrixBase<Tangent_t> & v,
                            const Eigen::MatrixBase<JacobianOut_t> & J,
                            const AssignmentOperatorType op=SETTO) const
    {
      switch(op)
        {
        case SETTO:
          J12(J).setZero();
          J21(J).setZero();
          // fallthrough
        case ADDTO:
        case RMTO:
          lg1.dIntegrate_dv(Q1(q), V1(v), J11(J),op);
          lg2.dIntegrate_dv(Q2(q), V2(v), J22(J),op);
          break;
        default:
          assert(false && "Wrong Op requesed value");
          break;
        }
    }

    template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
    void 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
    {
      JacobianOut_t& Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out);
      JacobianOut_t& Jin = PINOCCHIO_EIGEN_CONST_CAST(JacobianIn_t,J_in);
      lg1.dIntegrateTransport_dq(Q1(q), V1(v), Jin.template topRows<LieGroup1::NV>(),Jout.template topRows<LieGroup1::NV>());
      lg2.dIntegrateTransport_dq(Q2(q), V2(v), Jin.template bottomRows<LieGroup2::NV>(),Jout.template bottomRows<LieGroup2::NV>());
    }

    template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
    void 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
    {
      JacobianOut_t& Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out);
      JacobianOut_t& Jin = PINOCCHIO_EIGEN_CONST_CAST(JacobianIn_t,J_in);
      lg1.dIntegrateTransport_dv(Q1(q), V1(v), Jin.template topRows<LieGroup1::NV>(),Jout.template topRows<LieGroup1::NV>());
      lg2.dIntegrateTransport_dv(Q2(q), V2(v), Jin.template bottomRows<LieGroup2::NV>(),Jout.template bottomRows<LieGroup2::NV>());
    }
    

    template <class Config_t, class Tangent_t, class Jacobian_t>
    void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & q,
                                     const Eigen::MatrixBase<Tangent_t> & v,
                                     const Eigen::MatrixBase<Jacobian_t> & Jin) const
    {
      Jacobian_t& J = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,Jin);
      lg1.dIntegrateTransport_dq(Q1(q), V1(v), J.template topRows<LieGroup1::NV>());
      lg2.dIntegrateTransport_dq(Q2(q), V2(v), J.template bottomRows<LieGroup2::NV>());
    }

    template <class Config_t, class Tangent_t, class Jacobian_t>
    void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & q,
                                     const Eigen::MatrixBase<Tangent_t> & v,
                                     const Eigen::MatrixBase<Jacobian_t> & Jin) const
    {
      Jacobian_t& J = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,Jin);
      lg1.dIntegrateTransport_dv(Q1(q), V1(v), J.template topRows<LieGroup1::NV>());
      lg2.dIntegrateTransport_dv(Q2(q), V2(v), J.template bottomRows<LieGroup2::NV>());
    }

    template <class ConfigL_t, class ConfigR_t>
    Scalar squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
                                const Eigen::MatrixBase<ConfigR_t> & q1) const
    {
      return lg1.squaredDistance(Q1(q0), Q1(q1))
        +    lg2.squaredDistance(Q2(q0), Q2(q1));
    }
    
    template <class Config_t>
    void normalize_impl (const Eigen::MatrixBase<Config_t>& qout) const
    {
      lg1.normalize(Qo1(qout));
      lg2.normalize(Qo2(qout));
    }

    template <class Config_t>
    bool isNormalized_impl (const Eigen::MatrixBase<Config_t>& qin, const Scalar& prec) const
    {
      return lg1.isNormalized(Qo1(qin), prec) && lg2.isNormalized(Qo2(qin), prec);
    }

    template <class Config_t>
    void random_impl (const Eigen::MatrixBase<Config_t>& qout) const
    {
      lg1.random(Qo1(qout));
      lg2.random(Qo2(qout));
    }

    template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
    void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower,
                                  const Eigen::MatrixBase<ConfigR_t> & upper,
                                  const Eigen::MatrixBase<ConfigOut_t> & qout)
      const
    {
      lg1.randomConfiguration(Q1(lower), Q1(upper), Qo1(qout));
      lg2.randomConfiguration(Q2(lower), Q2(upper), Qo2(qout));
    }

    template <class ConfigL_t, class ConfigR_t>
    bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
                                  const Eigen::MatrixBase<ConfigR_t> & q1,
                                  const Scalar & prec) const
    {
      return lg1.isSameConfiguration(Q1(q0), Q1(q1), prec)
        &&   lg2.isSameConfiguration(Q2(q0), Q2(q1), prec);
    }

    bool isEqual_impl (const CartesianProductOperation& other) const
    {
      return lg1 == other.lg1 && lg2 == other.lg2;
    }

    LieGroup1 lg1;
    LieGroup2 lg2;

  private:
    // VectorSpaceOperationTpl<-1> within CartesianProductOperation will not work
    // if Eigen version is lower than 3.2.1
#if EIGEN_VERSION_AT_LEAST(3,2,1)
# define REMOVE_IF_EIGEN_TOO_LOW(x) x
#else
# define REMOVE_IF_EIGEN_TOO_LOW(x)
#endif

    template <typename Config > typename Config ::template ConstFixedSegmentReturnType<LieGroup1::NQ>::Type Q1 (const Eigen::MatrixBase<Config >& q) const { return q.derived().template head<LieGroup1::NQ>(REMOVE_IF_EIGEN_TOO_LOW(lg1.nq())); }
    template <typename Config > typename Config ::template ConstFixedSegmentReturnType<LieGroup2::NQ>::Type Q2 (const Eigen::MatrixBase<Config >& q) const { return q.derived().template tail<LieGroup2::NQ>(REMOVE_IF_EIGEN_TOO_LOW(lg2.nq())); }
    template <typename Tangent> typename Tangent::template ConstFixedSegmentReturnType<LieGroup1::NV>::Type V1 (const Eigen::MatrixBase<Tangent>& v) const { return v.derived().template head<LieGroup1::NV>(REMOVE_IF_EIGEN_TOO_LOW(lg1.nv())); }
    template <typename Tangent> typename Tangent::template ConstFixedSegmentReturnType<LieGroup2::NV>::Type V2 (const Eigen::MatrixBase<Tangent>& v) const { return v.derived().template tail<LieGroup2::NV>(REMOVE_IF_EIGEN_TOO_LOW(lg2.nv())); }

    template <typename Config > typename Config ::template      FixedSegmentReturnType<LieGroup1::NQ>::Type Qo1 (const Eigen::MatrixBase<Config >& q) const { return PINOCCHIO_EIGEN_CONST_CAST(Config,q).template head<LieGroup1::NQ>(REMOVE_IF_EIGEN_TOO_LOW(lg1.nq())); }
    template <typename Config > typename Config ::template      FixedSegmentReturnType<LieGroup2::NQ>::Type Qo2 (const Eigen::MatrixBase<Config >& q) const { return PINOCCHIO_EIGEN_CONST_CAST(Config,q).template tail<LieGroup2::NQ>(REMOVE_IF_EIGEN_TOO_LOW(lg2.nq())); }
    template <typename Tangent> typename Tangent::template      FixedSegmentReturnType<LieGroup1::NV>::Type Vo1 (const Eigen::MatrixBase<Tangent>& v) const { return PINOCCHIO_EIGEN_CONST_CAST(Tangent,v).template head<LieGroup1::NV>(REMOVE_IF_EIGEN_TOO_LOW(lg1.nv())); }
    template <typename Tangent> typename Tangent::template      FixedSegmentReturnType<LieGroup2::NV>::Type Vo2 (const Eigen::MatrixBase<Tangent>& v) const { return PINOCCHIO_EIGEN_CONST_CAST(Tangent,v).template tail<LieGroup2::NV>(REMOVE_IF_EIGEN_TOO_LOW(lg2.nv())); }

    template <typename Jac> Eigen::Block<Jac, LieGroup1::NV, LieGroup1::NV> J11 (const Eigen::MatrixBase<Jac>& J) const { return PINOCCHIO_EIGEN_CONST_CAST(Jac,J).template     topLeftCorner<LieGroup1::NV, LieGroup1::NV>(lg1.nv(),lg1.nv()); }
    template <typename Jac> Eigen::Block<Jac, LieGroup1::NV, LieGroup2::NV> J12 (const Eigen::MatrixBase<Jac>& J) const { return PINOCCHIO_EIGEN_CONST_CAST(Jac,J).template    topRightCorner<LieGroup1::NV, LieGroup2::NV>(lg1.nv(),lg2.nv()); }
    template <typename Jac> Eigen::Block<Jac, LieGroup2::NV, LieGroup1::NV> J21 (const Eigen::MatrixBase<Jac>& J) const { return PINOCCHIO_EIGEN_CONST_CAST(Jac,J).template  bottomLeftCorner<LieGroup2::NV, LieGroup1::NV>(lg2.nv(),lg1.nv()); }
    template <typename Jac> Eigen::Block<Jac, LieGroup2::NV, LieGroup2::NV> J22 (const Eigen::MatrixBase<Jac>& J) const { return PINOCCHIO_EIGEN_CONST_CAST(Jac,J).template bottomRightCorner<LieGroup2::NV, LieGroup2::NV>(lg2.nv(),lg2.nv()); }
#undef REMOVE_IF_EIGEN_TOO_LOW

  }; // struct CartesianProductOperation

} // namespace pinocchio

#endif // ifndef __pinocchio_multibody_liegroup_cartesian_product_operation_hpp__