pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
joint-motion-subspace-base.hpp
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"
10#include "pinocchio/spatial/fwd.hpp"
11#include "pinocchio/spatial/motion.hpp"
12#include "pinocchio/spatial/act-on-set.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
41namespace pinocchio
42{
43
45 template<class ConstraintDerived, typename Force>
47 {
49 };
50
52 template<class ConstraintDerived, typename ForceSet>
57
58 template<class Derived>
59 class JointMotionSubspaceBase : public NumericalBase<Derived>
60 {
61 protected:
62 PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(Derived)
63
64 public:
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
123 typename SE3GroupAction<Derived>::ReturnType se3Action(const SE3Tpl<Scalar, Options> & m) const
124 {
125 return derived().se3Action(m);
126 }
127
128 typename SE3GroupAction<Derived>::ReturnType
129 se3ActionInverse(const SE3Tpl<Scalar, Options> & m) const
130 {
131 return derived().se3ActionInverse(m);
132 }
133
134 template<typename MotionDerived>
135 typename MotionAlgebraAction<Derived, MotionDerived>::ReturnType
136 motionAction(const MotionDense<MotionDerived> & v) const
137 {
138 return derived().motionAction(v);
139 }
140
141 bool operator==(const JointMotionSubspaceBase<Derived> & other) const
142 {
143 return derived().isEqual(other.derived());
144 }
145
146 }; // class JointMotionSubspaceBase
147
149 template<typename Scalar, int Options, typename ConstraintDerived>
153 {
155 Y, constraint.derived());
156 }
157
159 template<typename MatrixDerived, typename ConstraintDerived>
160 typename MultiplicationOp<Eigen::MatrixBase<MatrixDerived>, ConstraintDerived>::ReturnType
161 operator*(
162 const Eigen::MatrixBase<MatrixDerived> & Y,
164 {
166 Y.derived(), constraint.derived());
167 }
168
169 namespace details
170 {
171 template<typename Constraint>
173 {
175 typedef typename traits<Constraint>::ReducedSquaredMatrix ReducedSquaredMatrix;
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 {
188 StDiagonalMatrixSOperationReturnType;
189 };
190
191 template<class ConstraintDerived>
192 typename JointMotionSubspaceTransposeBase<ConstraintDerived>::StDiagonalMatrixSOperationReturnType
193 operator*(
196 {
198 }
199
200} // namespace pinocchio
201
202#endif // ifndef __pinocchio_multibody_constraint_base_hpp__
Main pinocchio namespace.
Definition treeview.dox:11
Return type of the Constraint::Transpose * Force operation.
Return type of the Constraint::Transpose * ForceSet operation.
Common traits structure to fully define base classes for CRTP.
Definition fwd.hpp:72