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__