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-generic.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_generic_hpp__
7#define __pinocchio_multibody_constraint_generic_hpp__
8
9namespace pinocchio
10{
11
12 template<int _Dim, typename _Scalar, int _Options, int _MaxDim>
14 {
15 typedef _Scalar Scalar;
16 enum
17 {
18 LINEAR = 0,
19 ANGULAR = 3,
20 Options = _Options,
21 Dim = _Dim
22 };
23
25 typedef Eigen::Matrix<Scalar, Dim, 1, Options, _MaxDim, 1> JointForce;
26 typedef Eigen::Matrix<Scalar, 6, Dim, Options, 6, _MaxDim> DenseBase;
27 typedef Eigen::Matrix<Scalar, Dim, Dim, Options, _MaxDim, _MaxDim> ReducedSquaredMatrix;
28
29 typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(DenseBase) ConstMatrixReturnType;
30 typedef typename PINOCCHIO_EIGEN_REF_TYPE(DenseBase) MatrixReturnType;
31
32 typedef ReducedSquaredMatrix StDiagonalMatrixSOperationReturnType;
33 }; // traits JointMotionSubspaceTpl
34
35 template<int Dim, typename Scalar, int Options, int _MaxDim>
36 struct SE3GroupAction<JointMotionSubspaceTpl<Dim, Scalar, Options, _MaxDim>>
37 {
38 typedef Eigen::Matrix<Scalar, 6, Dim, Options, 6, _MaxDim> ReturnType;
39 };
40
41 template<int Dim, typename Scalar, int Options, int MaxDim, typename MotionDerived>
42 struct MotionAlgebraAction<JointMotionSubspaceTpl<Dim, Scalar, Options, MaxDim>, MotionDerived>
43 {
44 typedef Eigen::Matrix<Scalar, 6, Dim, Options, 6, MaxDim> ReturnType;
45 };
46
47 template<int Dim, typename Scalar, int Options, int MaxDim, typename ForceDerived>
48 struct ConstraintForceOp<JointMotionSubspaceTpl<Dim, Scalar, Options, MaxDim>, ForceDerived>
49 {
50 typedef
52 typedef Eigen::Matrix<Scalar, Dim, Dim, Options, MaxDim, MaxDim> ReturnType;
53 };
54
55 template<int Dim, typename Scalar, int Options, int MaxDim, typename ForceSet>
56 struct ConstraintForceSetOp<JointMotionSubspaceTpl<Dim, Scalar, Options, MaxDim>, ForceSet>
57 {
58 typedef
60 typedef
62 };
63
64 template<int _Dim, typename _Scalar, int _Options, int _MaxDim>
66 : public JointMotionSubspaceBase<JointMotionSubspaceTpl<_Dim, _Scalar, _Options, _MaxDim>>
67 {
69
71
73 PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(JointMotionSubspaceTpl)
74
75 enum
76 {
77 NV = _Dim
78 };
79
80 constexpr static int MaxNV = NV < 0 ? _MaxDim : NV;
81
82 using Base::nv;
83
84 template<typename D>
85 explicit JointMotionSubspaceTpl(const Eigen::MatrixBase<D> & _S)
86 : S(_S)
87 {
88 // There is currently a bug in Eigen/Core/util/StaticAssert.h in the use of the full namespace
89 // path
90 // TODO
92 }
93
95 : S()
96 {
99 }
100
101 // It is only valid for dynamics size
102 explicit JointMotionSubspaceTpl(const int dim)
103 : S(6, dim)
104 {
107 assert(_MaxDim < 0 || dim <= _MaxDim);
108 }
109
110 // It is only valid for dynamics size
111 template<int D, int MD>
113 : S(subspace.matrix())
114 {
117 assert(_MaxDim < 0 || subspace.matrix().cols() <= _MaxDim);
118 }
119
120 static JointMotionSubspaceTpl Zero(const int dim)
121 {
122 return JointMotionSubspaceTpl(dim);
123 }
124
125 template<typename VectorLike>
126 JointMotion __mult__(const Eigen::MatrixBase<VectorLike> & vj) const
127 {
128 return JointMotion(S * vj);
129 }
130
131 struct Transpose : JointMotionSubspaceTransposeBase<JointMotionSubspaceTpl>
132 {
133 const JointMotionSubspaceTpl & ref;
135 : ref(ref)
136 {
137 }
138
139 template<typename ForceDerived>
141 operator*(const ForceDense<ForceDerived> & f) const
142 {
143 return (ref.S.transpose() * f.toVector()).eval();
144 }
145
146 template<typename ForceSet>
148 operator*(const Eigen::MatrixBase<ForceSet> & F)
149 {
150 return ref.S.transpose() * F.derived();
151 }
152 };
153
154 Transpose transpose() const
155 {
156 return Transpose(*this);
157 }
158
159 MatrixReturnType matrix_impl()
160 {
161 return S;
162 }
163 ConstMatrixReturnType matrix_impl() const
164 {
165 return S;
166 }
167
168 int nv_impl() const
169 {
170 return (int)S.cols();
171 }
172
173 template<typename S2, int O2>
174 friend typename JointMotionSubspaceTpl<_Dim, _Scalar, _Options, _MaxDim>::DenseBase
175 operator*(const InertiaTpl<S2, O2> & Y, const JointMotionSubspaceTpl & S)
176 {
177 typedef typename JointMotionSubspaceTpl::DenseBase ReturnType;
178 ReturnType res(6, S.nv());
179 motionSet::inertiaAction(Y, S.S, res);
180 return res;
181 }
182
183 template<typename S2, int O2>
184 friend Eigen::Matrix<_Scalar, 6, _Dim, _Options, 6, _MaxDim>
185 operator*(const Eigen::Matrix<S2, 6, 6, O2> & Ymatrix, const JointMotionSubspaceTpl & S)
186 {
187 typedef Eigen::Matrix<_Scalar, 6, _Dim, _Options, 6, _MaxDim> ReturnType;
188 return ReturnType(Ymatrix * S.matrix());
189 }
190
191 DenseBase se3Action(const SE3Tpl<Scalar, Options> & m) const
192 {
193 DenseBase res(6, nv());
194 motionSet::se3Action(m, S, res);
195 return res;
196 }
197
198 DenseBase se3ActionInverse(const SE3Tpl<Scalar, Options> & m) const
199 {
200 DenseBase res(6, nv());
202 return res;
203 }
204
205 template<typename MotionDerived>
206 DenseBase motionAction(const MotionDense<MotionDerived> & v) const
207 {
208 DenseBase res(6, nv());
209 motionSet::motionAction(v, S, res);
210 return res;
211 }
212
213 void disp_impl(std::ostream & os) const
214 {
215 os << "S =\n" << S << std::endl;
216 }
217
218 bool isEqual(const JointMotionSubspaceTpl & other) const
219 {
220 return S == other.S;
221 }
222
223 protected:
224 DenseBase S;
225 }; // class JointMotionSubspaceTpl
226
227 namespace details
228 {
229 template<int Dim, typename Scalar, int Options, int MaxDim>
230 struct StDiagonalMatrixSOperation<JointMotionSubspaceTpl<Dim, Scalar, Options, MaxDim>>
231 {
234
235 static ReturnType run(const JointMotionSubspaceBase<Constraint> & constraint)
236 {
237 return constraint.matrix().transpose() * constraint.matrix();
238 }
239 };
240 } // namespace details
241
242} // namespace pinocchio
243
244#endif // ifndef __pinocchio_multibody_constraint_generic_hpp__
static void se3Action(const SE3Tpl< Scalar, Options > &m, const Eigen::MatrixBase< Mat > &iV, Eigen::MatrixBase< MatRet > const &jV)
SE3 action on a set of motions, represented by a 6xN matrix whose column represent a spatial motion.
static void se3ActionInverse(const SE3Tpl< Scalar, Options > &m, const Eigen::MatrixBase< Mat > &iV, Eigen::MatrixBase< MatRet > const &jV)
Inverse SE3 action on a set of motions, represented by a 6xN matrix whose column represent a spatial ...
static void motionAction(const MotionDense< MotionDerived > &v, const Eigen::MatrixBase< Mat > &iF, Eigen::MatrixBase< MatRet > const &jF)
Action of a motion on a set of motions, represented by a 6xN matrix whose columns represent a spatial...
static void inertiaAction(const InertiaTpl< Scalar, Options > &I, const Eigen::MatrixBase< Mat > &iF, Eigen::MatrixBase< MatRet > const &jF)
Action of an Inertia matrix on a set of motions, represented by a 6xN matrix whose columns represent ...
Main pinocchio namespace.
Definition treeview.dox:11
Return type of the Constraint::Transpose * Force operation.
Return type of the Constraint::Transpose * ForceSet operation.
Return type of the ation of a Motion onto an object of type D.
Definition motion.hpp:46
Common traits structure to fully define base classes for CRTP.
Definition fwd.hpp:72