joint-motion-subspace-base.hpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2015-2020 CNRS INRIA
3 // Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4 //
5 
6 #ifndef __pinocchio_multibody_constraint_base_hpp__
7 #define __pinocchio_multibody_constraint_base_hpp__
8 
9 #include "pinocchio/macros.hpp"
13 
14 #include <boost/static_assert.hpp>
15 
16 // S : v \in M^6 -> v_J \in lie(Q) ~= R^nv
17 // S^T : f_J \in lie(Q)^* ~= R^nv -> f \in F^6
18 
19 #define PINOCCHIO_CONSTRAINT_TYPEDEF_GENERIC(DERIVED, TYPENAME) \
20  typedef TYPENAME traits<DERIVED>::Scalar Scalar; \
21  typedef TYPENAME traits<DERIVED>::JointMotion JointMotion; \
22  typedef TYPENAME traits<DERIVED>::JointForce JointForce; \
23  typedef TYPENAME traits<DERIVED>::DenseBase DenseBase; \
24  typedef TYPENAME traits<DERIVED>::MatrixReturnType MatrixReturnType; \
25  typedef TYPENAME traits<DERIVED>::ConstMatrixReturnType ConstMatrixReturnType; \
26  enum \
27  { \
28  LINEAR = traits<DERIVED>::LINEAR, \
29  ANGULAR = traits<DERIVED>::ANGULAR \
30  }; \
31  enum \
32  { \
33  Options = traits<DERIVED>::Options \
34  };
35 
36 #define PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(DERIVED) \
37  PINOCCHIO_CONSTRAINT_TYPEDEF_GENERIC(DERIVED, typename)
38 #define PINOCCHIO_CONSTRAINT_TYPEDEF(DERIVED) \
39  PINOCCHIO_CONSTRAINT_TYPEDEF_GENERIC(DERIVED, PINOCCHIO_EMPTY_ARG)
40 
41 namespace pinocchio
42 {
43 
45  template<class ConstraintDerived, typename Force>
47  {
48  typedef ReturnTypeNotDefined ReturnType;
49  };
50 
52  template<class ConstraintDerived, typename ForceSet>
54  {
55  typedef ReturnTypeNotDefined ReturnType;
56  };
57 
58  template<class Derived>
59  class JointMotionSubspaceBase : public NumericalBase<Derived>
60  {
61  protected:
63 
64  public:
65  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
66 
67  Derived & derived()
68  {
69  return *static_cast<Derived *>(this);
70  }
71  const Derived & derived() const
72  {
73  return *static_cast<const Derived *>(this);
74  }
75 
76  template<typename VectorLike>
77  JointMotion operator*(const Eigen::MatrixBase<VectorLike> & vj) const
78  {
79  return derived().__mult__(vj);
80  }
81 
82  MatrixReturnType matrix()
83  {
84  return derived().matrix_impl();
85  }
86  ConstMatrixReturnType matrix() const
87  {
88  return derived().matrix_impl();
89  }
90 
91  int nv() const
92  {
93  return derived().nv_impl();
94  }
95 
96  static int rows()
97  {
98  return 6;
99  }
100  int cols() const
101  {
102  return nv();
103  }
104 
105  template<class OtherDerived>
106  bool isApprox(
108  const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
109  {
110  return matrix().isApprox(other.matrix(), prec);
111  }
112 
113  void disp(std::ostream & os) const
114  {
115  derived().disp_impl(os);
116  }
117  friend std::ostream & operator<<(std::ostream & os, const JointMotionSubspaceBase<Derived> & X)
118  {
119  X.disp(os);
120  return os;
121  }
122 
124  {
125  return derived().se3Action(m);
126  }
127 
130  {
131  return derived().se3ActionInverse(m);
132  }
133 
134  template<typename MotionDerived>
137  {
138  return derived().motionAction(v);
139  }
140 
142  {
143  return derived().isEqual(other.derived());
144  }
145 
146  }; // class JointMotionSubspaceBase
147 
149  template<typename Scalar, int Options, typename ConstraintDerived>
150  typename MultiplicationOp<InertiaTpl<Scalar, Options>, ConstraintDerived>::ReturnType operator*(
151  const InertiaTpl<Scalar, Options> & Y,
153  {
155  Y, constraint.derived());
156  }
157 
159  template<typename MatrixDerived, typename ConstraintDerived>
160  typename MultiplicationOp<Eigen::MatrixBase<MatrixDerived>, ConstraintDerived>::ReturnType
162  const Eigen::MatrixBase<MatrixDerived> & Y,
164  {
166  Y.derived(), constraint.derived());
167  }
168 
169  namespace details
170  {
171  template<typename Constraint>
173  {
176 
177  static ReturnType run(const JointMotionSubspaceBase<Constraint> & /*constraint*/)
178  {
179  return ReducedSquaredMatrix::Identity(Constraint::NV, Constraint::NV);
180  }
181  };
182  } // namespace details
183 
184  template<class ConstraintDerived>
186  {
189  };
190 
191  template<class ConstraintDerived>
196  {
198  }
199 
200 } // namespace pinocchio
201 
202 #endif // ifndef __pinocchio_multibody_constraint_base_hpp__
pinocchio::InertiaTpl< Scalar, Options >
pinocchio::JointMotionSubspaceBase::matrix
ConstMatrixReturnType matrix() const
Definition: joint-motion-subspace-base.hpp:86
act-on-set.hpp
fwd.hpp
pinocchio::JointMotionSubspaceBase::nv
int nv() const
Definition: joint-motion-subspace-base.hpp:91
pinocchio::NumericalBase
Definition: fwd.hpp:89
pinocchio::MultiplicationOp
Forward declaration of the multiplication operation return type. Should be overloaded,...
Definition: binary-op.hpp:15
test-cpp2pybind11.m
m
Definition: test-cpp2pybind11.py:25
pinocchio::details::StDiagonalMatrixSOperation::run
static ReturnType run(const JointMotionSubspaceBase< Constraint > &)
Definition: joint-motion-subspace-base.hpp:177
pinocchio::JointMotionSubspaceBase::motionAction
MotionAlgebraAction< Derived, MotionDerived >::ReturnType motionAction(const MotionDense< MotionDerived > &v) const
Definition: joint-motion-subspace-base.hpp:136
macros.hpp
omniidl_be_python_with_docstring.run
def run(tree, args)
Definition: cmake/hpp/idl/omniidl_be_python_with_docstring.py:140
pinocchio::ConstraintForceOp
Return type of the Constraint::Transpose * Force operation.
Definition: joint-motion-subspace-base.hpp:46
pinocchio::JointMotionSubspaceBase
Definition: joint-motion-subspace-base.hpp:59
pinocchio::SE3Tpl< Scalar, Options >
pinocchio::SE3GroupAction::ReturnType
D ReturnType
Definition: spatial/se3.hpp:41
pinocchio::JointMotionSubspaceBase::rows
static int rows()
Definition: joint-motion-subspace-base.hpp:96
pinocchio::NumericalBase< JointMotionSubspaceTpl< _Dim, _Scalar, _Options > >::Scalar
traits< JointMotionSubspaceTpl< _Dim, _Scalar, _Options > >::Scalar Scalar
Definition: fwd.hpp:91
pinocchio::MotionDense
Definition: context/casadi.hpp:36
pinocchio::ConstraintForceOp::ReturnType
ReturnTypeNotDefined ReturnType
Definition: joint-motion-subspace-base.hpp:48
pinocchio::JointMotionSubspaceTransposeBase::StDiagonalMatrixSOperationReturnType
traits< ConstraintDerived >::StDiagonalMatrixSOperationReturnType StDiagonalMatrixSOperationReturnType
Definition: joint-motion-subspace-base.hpp:188
pinocchio::details::StDiagonalMatrixSOperation::ReturnType
traits< Constraint >::StDiagonalMatrixSOperationReturnType ReturnType
Definition: joint-motion-subspace-base.hpp:174
motion.hpp
pinocchio::operator*
TridiagonalSymmetricMatrixApplyOnTheLeftReturnType< LhsMatrixType, TridiagonalSymmetricMatrixTpl< S, O > > operator*(const Eigen::MatrixBase< LhsMatrixType > &lhs, const TridiagonalSymmetricMatrixTpl< S, O > &rhs)
Definition: math/tridiagonal-matrix.hpp:319
pinocchio::impl::LhsMultiplicationOp
Definition: binary-op.hpp:20
pinocchio::JointMotionSubspaceBase::operator<<
friend std::ostream & operator<<(std::ostream &os, const JointMotionSubspaceBase< Derived > &X)
Definition: joint-motion-subspace-base.hpp:117
pinocchio::JointMotionSubspaceBase::isApprox
bool isApprox(const JointMotionSubspaceBase< OtherDerived > &other, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision()) const
Definition: joint-motion-subspace-base.hpp:106
pinocchio::MotionAlgebraAction::ReturnType
D ReturnType
Definition: spatial/motion.hpp:47
pinocchio::JointMotionSubspaceBase::matrix
MatrixReturnType matrix()
Definition: joint-motion-subspace-base.hpp:82
pinocchio::JointMotionSubspaceBase::disp
void disp(std::ostream &os) const
Definition: joint-motion-subspace-base.hpp:113
pinocchio::ConstraintForceSetOp::ReturnType
ReturnTypeNotDefined ReturnType
Definition: joint-motion-subspace-base.hpp:55
simulation-contact-dynamics.S
S
Definition: simulation-contact-dynamics.py:80
details
pinocchio::JointMotionSubspaceBase::derived
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Derived & derived()
Definition: joint-motion-subspace-base.hpp:67
pinocchio::JointMotionSubspaceBase::se3Action
SE3GroupAction< Derived >::ReturnType se3Action(const SE3Tpl< Scalar, Options > &m) const
Definition: joint-motion-subspace-base.hpp:123
pinocchio::JointMotionSubspaceBase::operator*
JointMotion operator*(const Eigen::MatrixBase< VectorLike > &vj) const
Definition: joint-motion-subspace-base.hpp:77
pinocchio::v
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
Definition: joint-configuration.hpp:1084
pinocchio::ConstraintForceSetOp
Return type of the Constraint::Transpose * ForceSet operation.
Definition: joint-motion-subspace-base.hpp:53
pinocchio::details::StDiagonalMatrixSOperation
Definition: joint-motion-subspace-base.hpp:172
pinocchio::JointMotionSubspaceBase::se3ActionInverse
SE3GroupAction< Derived >::ReturnType se3ActionInverse(const SE3Tpl< Scalar, Options > &m) const
Definition: joint-motion-subspace-base.hpp:129
pinocchio::JointMotionSubspaceBase::cols
int cols() const
Definition: joint-motion-subspace-base.hpp:100
PINOCCHIO_CONSTRAINT_TYPEDEF_TPL
#define PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(DERIVED)
Definition: joint-motion-subspace-base.hpp:36
pinocchio::details::StDiagonalMatrixSOperation::ReducedSquaredMatrix
traits< Constraint >::ReducedSquaredMatrix ReducedSquaredMatrix
Definition: joint-motion-subspace-base.hpp:175
Y
Y
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:71
pinocchio::JointMotionSubspaceBase::derived
const Derived & derived() const
Definition: joint-motion-subspace-base.hpp:71
pinocchio::JointMotionSubspaceTransposeBase
Definition: joint-motion-subspace-base.hpp:185
dcrba.NV
NV
Definition: dcrba.py:536
X
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27
pinocchio::JointMotionSubspaceBase::operator==
bool operator==(const JointMotionSubspaceBase< Derived > &other) const
Definition: joint-motion-subspace-base.hpp:141


pinocchio
Author(s):
autogenerated on Sun Dec 22 2024 03:41:10