joint-motion-subspace-base.hpp

//
// Copyright (c) 2015-2020 CNRS INRIA
// Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
 
#ifndef __pinocchio_multibody_constraint_base_hpp__
#define __pinocchio_multibody_constraint_base_hpp__
 
#include "pinocchio/macros.hpp"
#include "pinocchio/spatial/fwd.hpp"
#include "pinocchio/spatial/motion.hpp"
#include "pinocchio/spatial/act-on-set.hpp"
 
#include <boost/static_assert.hpp>
 
// S   : v   \in M^6              -> v_J \in lie(Q) ~= R^nv
// S^T : f_J \in lie(Q)^* ~= R^nv -> f    \in F^6
 
#define PINOCCHIO_CONSTRAINT_TYPEDEF_GENERIC(DERIVED, TYPENAME)                                    \
  typedef TYPENAME traits<DERIVED>::Scalar Scalar;                                                 \
  typedef TYPENAME traits<DERIVED>::JointMotion JointMotion;                                       \
  typedef TYPENAME traits<DERIVED>::JointForce JointForce;                                         \
  typedef TYPENAME traits<DERIVED>::DenseBase DenseBase;                                           \
  typedef TYPENAME traits<DERIVED>::MatrixReturnType MatrixReturnType;                             \
  typedef TYPENAME traits<DERIVED>::ConstMatrixReturnType ConstMatrixReturnType;                   \
  enum                                                                                             \
  {                                                                                                \
    LINEAR = traits<DERIVED>::LINEAR,                                                              \
    ANGULAR = traits<DERIVED>::ANGULAR                                                             \
  };                                                                                               \
  enum                                                                                             \
  {                                                                                                \
    Options = traits<DERIVED>::Options                                                             \
  };
 
#define PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(DERIVED)                                                  \
  PINOCCHIO_CONSTRAINT_TYPEDEF_GENERIC(DERIVED, typename)
#define PINOCCHIO_CONSTRAINT_TYPEDEF(DERIVED)                                                      \
  PINOCCHIO_CONSTRAINT_TYPEDEF_GENERIC(DERIVED, PINOCCHIO_EMPTY_ARG)
 
namespace pinocchio
{
 
  template<class ConstraintDerived, typename Force>
  struct ConstraintForceOp
  {
    typedef ReturnTypeNotDefined ReturnType;
  };
 
  template<class ConstraintDerived, typename ForceSet>
  struct ConstraintForceSetOp
  {
    typedef ReturnTypeNotDefined ReturnType;
  };
 
  template<class Derived>
  class JointMotionSubspaceBase : public NumericalBase<Derived>
  {
  protected:
    PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(Derived)
 
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    Derived & derived()
    {
      return *static_cast<Derived *>(this);
    }
    const Derived & derived() const
    {
      return *static_cast<const Derived *>(this);
    }
 
    template<typename VectorLike>
    JointMotion operator*(const Eigen::MatrixBase<VectorLike> & vj) const
    {
      return derived().__mult__(vj);
    }
 
    MatrixReturnType matrix()
    {
      return derived().matrix_impl();
    }
    ConstMatrixReturnType matrix() const
    {
      return derived().matrix_impl();
    }
 
    int nv() const
    {
      return derived().nv_impl();
    }
 
    static int rows()
    {
      return 6;
    }
    int cols() const
    {
      return nv();
    }
 
    template<class OtherDerived>
    bool isApprox(
      const JointMotionSubspaceBase<OtherDerived> & other,
      const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
    {
      return matrix().isApprox(other.matrix(), prec);
    }
 
    void disp(std::ostream & os) const
    {
      derived().disp_impl(os);
    }
    friend std::ostream & operator<<(std::ostream & os, const JointMotionSubspaceBase<Derived> & X)
    {
      X.disp(os);
      return os;
    }
 
    typename SE3GroupAction<Derived>::ReturnType se3Action(const SE3Tpl<Scalar, Options> & m) const
    {
      return derived().se3Action(m);
    }
 
    typename SE3GroupAction<Derived>::ReturnType
    se3ActionInverse(const SE3Tpl<Scalar, Options> & m) const
    {
      return derived().se3ActionInverse(m);
    }
 
    template<typename MotionDerived>
    typename MotionAlgebraAction<Derived, MotionDerived>::ReturnType
    motionAction(const MotionDense<MotionDerived> & v) const
    {
      return derived().motionAction(v);
    }
 
    bool operator==(const JointMotionSubspaceBase<Derived> & other) const
    {
      return derived().isEqual(other.derived());
    }
 
  }; // class JointMotionSubspaceBase
 
  template<typename Scalar, int Options, typename ConstraintDerived>
  typename MultiplicationOp<InertiaTpl<Scalar, Options>, ConstraintDerived>::ReturnType operator*(
    const InertiaTpl<Scalar, Options> & Y,
    const JointMotionSubspaceBase<ConstraintDerived> & constraint)
  {
    return impl::LhsMultiplicationOp<InertiaTpl<Scalar, Options>, ConstraintDerived>::run(
      Y, constraint.derived());
  }
 
  template<typename MatrixDerived, typename ConstraintDerived>
  typename MultiplicationOp<Eigen::MatrixBase<MatrixDerived>, ConstraintDerived>::ReturnType
  operator*(
    const Eigen::MatrixBase<MatrixDerived> & Y,
    const JointMotionSubspaceBase<ConstraintDerived> & constraint)
  {
    return impl::LhsMultiplicationOp<Eigen::MatrixBase<MatrixDerived>, ConstraintDerived>::run(
      Y.derived(), constraint.derived());
  }
 
  namespace details
  {
    template<typename Constraint>
    struct StDiagonalMatrixSOperation
    {
      typedef typename traits<Constraint>::StDiagonalMatrixSOperationReturnType ReturnType;
      typedef typename traits<Constraint>::ReducedSquaredMatrix ReducedSquaredMatrix;
 
      static ReturnType run(const JointMotionSubspaceBase<Constraint> & /*constraint*/)
      {
        return ReducedSquaredMatrix::Identity(Constraint::NV, Constraint::NV);
      }
    };
  } // namespace details
 
  template<class ConstraintDerived>
  struct JointMotionSubspaceTransposeBase
  {
    typedef typename traits<ConstraintDerived>::StDiagonalMatrixSOperationReturnType
      StDiagonalMatrixSOperationReturnType;
  };
 
  template<class ConstraintDerived>
  typename JointMotionSubspaceTransposeBase<ConstraintDerived>::StDiagonalMatrixSOperationReturnType
  operator*(
    const JointMotionSubspaceTransposeBase<ConstraintDerived> & /*S_transpose*/,
    const JointMotionSubspaceBase<ConstraintDerived> & S)
  {
    return details::StDiagonalMatrixSOperation<ConstraintDerived>::run(S.derived());
  }
 
} // namespace pinocchio
 
#endif // ifndef __pinocchio_multibody_constraint_base_hpp__