pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
se3-base.hpp
1//
2// Copyright (c) 2015-2021 CNRS INRIA
3// Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4//
5
6#ifndef __pinocchio_spatial_se3_base_hpp__
7#define __pinocchio_spatial_se3_base_hpp__
8
9namespace pinocchio
10{
29 template<class Derived>
30 struct SE3Base : NumericalBase<Derived>
31 {
32 PINOCCHIO_SE3_TYPEDEF_TPL(Derived);
33
34 Derived & derived()
35 {
36 return *static_cast<Derived *>(this);
37 }
38 const Derived & derived() const
39 {
40 return *static_cast<const Derived *>(this);
41 }
42
43 Derived & const_cast_derived() const
44 {
45 return *const_cast<Derived *>(&derived());
46 }
47
48 ConstAngularRef rotation() const
49 {
50 return derived().rotation_impl();
51 }
52 ConstLinearRef translation() const
53 {
54 return derived().translation_impl();
55 }
56 AngularRef rotation()
57 {
58 return derived().rotation_impl();
59 }
60 LinearRef translation()
61 {
62 return derived().translation_impl();
63 }
64 void rotation(const AngularType & R)
65 {
66 derived().rotation_impl(R);
67 }
68 void translation(const LinearType & t)
69 {
70 derived().translation_impl(t);
71 }
72
73 HomogeneousMatrixType toHomogeneousMatrix() const
74 {
75 return derived().toHomogeneousMatrix_impl();
76 }
77 operator HomogeneousMatrixType() const
78 {
79 return toHomogeneousMatrix();
80 }
81
92 ActionMatrixType toActionMatrix() const
93 {
94 return derived().toActionMatrix_impl();
95 }
96 operator ActionMatrixType() const
97 {
98 return toActionMatrix();
99 }
100
101 template<typename Matrix6Like>
102 void toActionMatrix(const Eigen::MatrixBase<Matrix6Like> & action_matrix) const
103 {
104 derived().toActionMatrix_impl(action_matrix);
105 }
106
111 ActionMatrixType toActionMatrixInverse() const
112 {
113 return derived().toActionMatrixInverse_impl();
114 }
115
116 template<typename Matrix6Like>
117 void toActionMatrixInverse(const Eigen::MatrixBase<Matrix6Like> & action_matrix_inverse) const
118 {
119 derived().toActionMatrixInverse_impl(action_matrix_inverse.const_cast_derived());
120 }
121
122 ActionMatrixType toDualActionMatrix() const
123 {
124 return derived().toDualActionMatrix_impl();
125 }
126
127 void disp(std::ostream & os) const
128 {
129 static_cast<const Derived *>(this)->disp_impl(os);
130 }
131
132 template<typename Matrix6Like>
133 void toDualActionMatrix(const Eigen::MatrixBase<Matrix6Like> & dual_action_matrix) const
134 {
135 derived().toDualActionMatrix_impl(dual_action_matrix);
136 }
137
138 typename SE3GroupAction<Derived>::ReturnType operator*(const Derived & m2) const
139 {
140 return derived().__mult__(m2);
141 }
142
144 template<typename D>
145 typename SE3GroupAction<D>::ReturnType act(const D & d) const
146 {
147 return derived().act_impl(d);
148 }
149
151 template<typename D>
152 typename SE3GroupAction<D>::ReturnType actInv(const D & d) const
153 {
154 return derived().actInv_impl(d);
155 }
156
157 bool operator==(const Derived & other) const
158 {
159 return derived().isEqual(other);
160 }
161
162 bool operator!=(const Derived & other) const
163 {
164 return !(*this == other);
165 }
166
167 bool isApprox(
168 const Derived & other,
169 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
170 {
171 return derived().isApprox_impl(other, prec);
172 }
173
174 friend std::ostream & operator<<(std::ostream & os, const SE3Base<Derived> & X)
175 {
176 X.disp(os);
177 return os;
178 }
179
185 const typename traits<Derived>::Scalar & prec =
186 Eigen::NumTraits<typename traits<Derived>::Scalar>::dummy_precision()) const
187 {
188 return derived().isIdentity(prec);
189 }
190
195 bool isNormalized(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
196 {
197 return derived().isNormalized(prec);
198 }
199
204 {
205 derived().normalize();
206 }
207
212 PlainType normalized() const
213 {
214 derived().normalized();
215 }
216
217 }; // struct SE3Base
218
219} // namespace pinocchio
220
221#endif // ifndef __pinocchio_spatial_se3_base_hpp__
Main pinocchio namespace.
Definition treeview.dox:11
Base class for rigid transformation.
Definition se3-base.hpp:31
SE3GroupAction< D >::ReturnType act(const D &d) const
ay = aXb.act(by)
Definition se3-base.hpp:145
bool isIdentity(const typename traits< Derived >::Scalar &prec=Eigen::NumTraits< typename traits< Derived >::Scalar >::dummy_precision()) const
Definition se3-base.hpp:184
PlainType normalized() const
Definition se3-base.hpp:212
ActionMatrixType toActionMatrixInverse() const
The action matrix of .
Definition se3-base.hpp:111
bool isNormalized(const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision()) const
Definition se3-base.hpp:195
void normalize()
Normalize *this in such a way the rotation part of *this lies on SO(3).
Definition se3-base.hpp:203
ActionMatrixType toActionMatrix() const
The action matrix of .
Definition se3-base.hpp:92
SE3GroupAction< D >::ReturnType actInv(const D &d) const
by = aXb.actInv(ay)
Definition se3-base.hpp:152
Common traits structure to fully define base classes for CRTP.
Definition fwd.hpp:72