pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
quaternion.hpp
1//
2// Copyright (c) 2016-2020 CNRS INRIA
3//
4
5#ifndef __pinocchio_math_quaternion_hpp__
6#define __pinocchio_math_quaternion_hpp__
7
8#ifndef PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE
9 #define PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE 1e-8
10#endif
11
12#include "pinocchio/math/fwd.hpp"
13#include "pinocchio/math/comparison-operators.hpp"
14#include "pinocchio/math/matrix.hpp"
15#include "pinocchio/math/sincos.hpp"
16#include "pinocchio/utils/static-if.hpp"
17
18#include <boost/type_traits.hpp>
19#include <Eigen/Geometry>
20
21namespace pinocchio
22{
23 namespace quaternion
24 {
25
34 template<typename D1, typename D2>
35 typename D1::Scalar angleBetweenQuaternions(
36 const Eigen::QuaternionBase<D1> & q1, const Eigen::QuaternionBase<D2> & q2)
37 {
38 typedef typename D1::Scalar Scalar;
39 const Scalar innerprod = q1.dot(q2);
40 Scalar theta = math::acos(innerprod);
41 static const Scalar PI_value = PI<Scalar>();
42
43 theta = internal::if_then_else(
44 internal::LT, innerprod, Scalar(0), static_cast<Scalar>(PI_value - theta), theta);
45 return theta;
46 }
47
57 template<typename D1, typename D2>
59 const Eigen::QuaternionBase<D1> & q1,
60 const Eigen::QuaternionBase<D2> & q2,
61 const typename D1::RealScalar & prec =
62 Eigen::NumTraits<typename D1::Scalar>::dummy_precision())
63 {
64 return (q1.coeffs().isApprox(q2.coeffs(), prec) || q1.coeffs().isApprox(-q2.coeffs(), prec));
65 }
66
89 template<typename D>
90 void firstOrderNormalize(const Eigen::QuaternionBase<D> & q)
91 {
92 typedef typename D::Scalar Scalar;
93 const Scalar N2 = q.squaredNorm();
94#ifndef NDEBUG
95 const Scalar epsilon = sqrt(sqrt(Eigen::NumTraits<Scalar>::epsilon()));
98 assert(static_leq::op(math::fabs(static_cast<Scalar>(N2 - Scalar(1))), epsilon));
99#endif
100 const Scalar alpha = ((Scalar)3 - N2) / Scalar(2);
101 PINOCCHIO_EIGEN_CONST_CAST(D, q).coeffs() *= alpha;
102#ifndef NDEBUG
103 const Scalar M =
104 Scalar(3) * math::pow(Scalar(1) - epsilon, ((Scalar)-Scalar(5)) / Scalar(2)) / Scalar(4);
105 assert(static_leq::op(
106 math::fabs(static_cast<Scalar>(q.norm() - Scalar(1))),
107 math::max(
108 M * sqrt(N2) * (N2 - Scalar(1)) * (N2 - Scalar(1)) / Scalar(2),
109 Eigen::NumTraits<Scalar>::dummy_precision())));
110#endif
111 }
112
114 template<typename Derived>
115 void uniformRandom(Eigen::QuaternionBase<Derived> & q)
116 {
117 typedef typename Derived::Scalar Scalar;
118
119 // Rotational part
120 const Scalar u1 = (Scalar)rand() / RAND_MAX;
121 const Scalar u2 = (Scalar)rand() / RAND_MAX;
122 const Scalar u3 = (Scalar)rand() / RAND_MAX;
123
124 const Scalar mult1 = sqrt(Scalar(1) - u1);
125 const Scalar mult2 = sqrt(u1);
126
127 static const Scalar PI_value = PI<Scalar>();
128 Scalar s2, c2;
129 SINCOS(Scalar(2) * PI_value * u2, &s2, &c2);
130 Scalar s3, c3;
131 SINCOS(Scalar(2) * PI_value * u3, &s3, &c3);
132
133 q.w() = mult1 * s2;
134 q.x() = mult1 * c2;
135 q.y() = mult2 * s3;
136 q.z() = mult2 * c3;
137 }
138
139 namespace internal
140 {
141
143 struct quaternionbase_assign_impl;
144
145 template<Eigen::DenseIndex i>
146 struct quaternionbase_assign_impl_if_t_negative
147 {
148 template<typename Scalar, typename Matrix3, typename QuaternionDerived>
149 static inline void
150 run(Scalar t, Eigen::QuaternionBase<QuaternionDerived> & q, const Matrix3 & mat)
151 {
152 using pinocchio::math::sqrt;
153
154 Eigen::DenseIndex j = (i + 1) % 3;
155 Eigen::DenseIndex k = (j + 1) % 3;
156
157 t = sqrt(mat.coeff(i, i) - mat.coeff(j, j) - mat.coeff(k, k) + Scalar(1.0));
158 q.coeffs().coeffRef(i) = Scalar(0.5) * t;
159 t = Scalar(0.5) / t;
160 q.w() = (mat.coeff(k, j) - mat.coeff(j, k)) * t;
161 q.coeffs().coeffRef(j) = (mat.coeff(j, i) + mat.coeff(i, j)) * t;
162 q.coeffs().coeffRef(k) = (mat.coeff(k, i) + mat.coeff(i, k)) * t;
163 }
164 };
165
166 struct quaternionbase_assign_impl_if_t_positive
167 {
168 template<typename Scalar, typename Matrix3, typename QuaternionDerived>
169 static inline void
170 run(Scalar t, Eigen::QuaternionBase<QuaternionDerived> & q, const Matrix3 & mat)
171 {
172 using pinocchio::math::sqrt;
173
174 t = sqrt(t + Scalar(1.0));
175 q.w() = Scalar(0.5) * t;
176 t = Scalar(0.5) / t;
177 q.x() = (mat.coeff(2, 1) - mat.coeff(1, 2)) * t;
178 q.y() = (mat.coeff(0, 2) - mat.coeff(2, 0)) * t;
179 q.z() = (mat.coeff(1, 0) - mat.coeff(0, 1)) * t;
180 }
181 };
182
183 template<typename Scalar>
184 struct quaternionbase_assign_impl<Scalar, true>
185 {
186 template<typename Matrix3, typename QuaternionDerived>
187 static inline void run(Eigen::QuaternionBase<QuaternionDerived> & q, const Matrix3 & mat)
188 {
189 using pinocchio::math::sqrt;
190
191 Scalar t = mat.trace();
192 if (t > Scalar(0.))
193 quaternionbase_assign_impl_if_t_positive::run(t, q, mat);
194 else
195 {
196 Eigen::DenseIndex i = 0;
197 if (mat.coeff(1, 1) > mat.coeff(0, 0))
198 i = 1;
199 if (mat.coeff(2, 2) > mat.coeff(i, i))
200 i = 2;
201
202 if (i == 0)
203 quaternionbase_assign_impl_if_t_negative<0>::run(t, q, mat);
204 else if (i == 1)
205 quaternionbase_assign_impl_if_t_negative<1>::run(t, q, mat);
206 else
207 quaternionbase_assign_impl_if_t_negative<2>::run(t, q, mat);
208 }
209 }
210 };
211
212 } // namespace internal
213
214 template<typename D, typename Matrix3>
215 void assignQuaternion(Eigen::QuaternionBase<D> & quat, const Eigen::MatrixBase<Matrix3> & R)
216 {
217 internal::quaternionbase_assign_impl<typename Matrix3::Scalar>::run(
218 quat.derived(), R.derived());
219 }
220
229 template<typename Quaternion>
230 inline bool isNormalized(
231 const Eigen::QuaternionBase<Quaternion> & quat,
232 const typename Quaternion::Coefficients::RealScalar & prec)
233 {
234 return pinocchio::isNormalized(quat.coeffs(), prec);
235 }
236
244 template<typename Quaternion>
245 inline bool isNormalized(const Eigen::QuaternionBase<Quaternion> & quat)
246 {
247 typedef typename Quaternion::Coefficients::RealScalar RealScalar;
248 const RealScalar prec = math::sqrt(Eigen::NumTraits<RealScalar>::epsilon());
249 return pinocchio::isNormalized(quat.coeffs(), prec);
250 }
251
257 template<typename Quaternion>
258 inline void normalize(const Eigen::QuaternionBase<Quaternion> & quat)
259 {
260 return pinocchio::normalize(quat.const_cast_derived().coeffs());
261 }
262
269 template<
270 typename Scalar,
271 typename QuaternionIn1,
272 typename QuaternionIn2,
273 typename QuaternionOut>
274 inline void slerp(
275 const Scalar & u,
276 const Eigen::QuaternionBase<QuaternionIn1> & quat0,
277 const Eigen::QuaternionBase<QuaternionIn2> & quat1,
278 const Eigen::QuaternionBase<QuaternionOut> & res)
279 {
280 const Scalar one = Scalar(1) - Eigen::NumTraits<Scalar>::epsilon();
281 const Scalar d = quat0.dot(quat1);
282 const Scalar absD = fabs(d);
283
284 const Scalar theta = acos(absD);
285 const Scalar sinTheta = sin(theta);
286
287 using namespace pinocchio::internal;
288
289 const Scalar scale0 = if_then_else(
290 pinocchio::internal::GE, absD, one,
291 static_cast<Scalar>(Scalar(1) - u), // then
292 static_cast<Scalar>(sin((Scalar(1) - u) * theta) / sinTheta) // else
293 );
294
295 const Scalar scale1_factor =
296 if_then_else(pinocchio::internal::LT, d, Scalar(0), Scalar(-1), Scalar(1));
297 const Scalar scale1 = if_then_else(
298 pinocchio::internal::GE, absD, one,
299 u, // then
300 static_cast<Scalar>(sin((u * theta)) / sinTheta) // else
301 )
303
304 PINOCCHIO_EIGEN_CONST_CAST(QuaternionOut, res.derived()).coeffs() =
305 scale0 * quat0.coeffs() + scale1 * quat1.coeffs();
306 }
307
308 } // namespace quaternion
309
310} // namespace pinocchio
311#endif // #ifndef __pinocchio_math_quaternion_hpp__
bool defineSameRotation(const Eigen::QuaternionBase< D1 > &q1, const Eigen::QuaternionBase< D2 > &q2, const typename D1::RealScalar &prec=Eigen::NumTraits< typename D1::Scalar >::dummy_precision())
Check if two quaternions define the same rotations.
bool isNormalized(const Eigen::QuaternionBase< Quaternion > &quat, const typename Quaternion::Coefficients::RealScalar &prec)
Check whether the input quaternion is Normalized within the given precision.
void firstOrderNormalize(const Eigen::QuaternionBase< D > &q)
D1::Scalar angleBetweenQuaternions(const Eigen::QuaternionBase< D1 > &q1, const Eigen::QuaternionBase< D2 > &q2)
Compute the minimal angle between q1 and q2.
void slerp(const Scalar &u, const Eigen::QuaternionBase< QuaternionIn1 > &quat0, const Eigen::QuaternionBase< QuaternionIn2 > &quat1, const Eigen::QuaternionBase< QuaternionOut > &res)
void normalize(const Eigen::QuaternionBase< Quaternion > &quat)
Normalize the input quaternion.
void uniformRandom(Eigen::QuaternionBase< Derived > &q)
Uniformly random quaternion sphere.
Main pinocchio namespace.
Definition treeview.dox:11
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
void SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
Definition sincos.hpp:27
bool isNormalized(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision())
Check whether a configuration vector is normalized within the given precision provided by prec.