pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
explog-quaternion.hpp
1//
2// Copyright (c) 2018-2021 CNRS INRIA
3//
4
5#ifndef __pinocchio_spatial_explog_quaternion_hpp__
6#define __pinocchio_spatial_explog_quaternion_hpp__
7
8#include "pinocchio/math/quaternion.hpp"
9#include "pinocchio/spatial/explog.hpp"
10#include "pinocchio/utils/static-if.hpp"
11
12namespace pinocchio
13{
14 namespace quaternion
15 {
16
25 template<typename Vector3Like, typename QuaternionLike>
26 void
27 exp3(const Eigen::MatrixBase<Vector3Like> & v, Eigen::QuaternionBase<QuaternionLike> & quat_out)
28 {
29 EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
30 assert(v.size() == 3);
31
32 typedef typename Vector3Like::Scalar Scalar;
33 enum
34 {
35 Options = PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Coefficients)::Options
36 };
37 typedef Eigen::Quaternion<typename QuaternionLike::Scalar, Options> QuaternionPlain;
38 const Scalar eps = Eigen::NumTraits<Scalar>::epsilon();
39
40 const Scalar t2 = v.squaredNorm();
41 const Scalar t = math::sqrt(t2 + eps * eps);
42
43 static const Scalar ts_prec =
44 TaylorSeriesExpansion<Scalar>::template precision<3>(); // Precision for the Taylor series
45 // expansion.
46
47 Eigen::AngleAxis<Scalar> aa(t, v / t);
48 QuaternionPlain quat_then(aa);
49
50 // order 4 Taylor expansion in theta / (order 2 in t2)
51 QuaternionPlain quat_else;
52 const Scalar t2_2 = t2 / 4; // theta/2 squared
53 quat_else.vec() =
54 Scalar(0.5) * (Scalar(1) - t2_2 / Scalar(6) + t2_2 * t2_2 / Scalar(120)) * v;
55 quat_else.w() = Scalar(1) - t2_2 / 2 + t2_2 * t2_2 / 24;
56
57 using ::pinocchio::internal::if_then_else;
58 for (Eigen::DenseIndex k = 0; k < 4; ++k)
59 {
60 quat_out.coeffs().coeffRef(k) = if_then_else(
61 ::pinocchio::internal::GT, t2, ts_prec, quat_then.coeffs().coeffRef(k),
62 quat_else.coeffs().coeffRef(k));
63 }
64 }
65
72 template<typename Vector3Like>
73 Eigen::
74 Quaternion<typename Vector3Like::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
75 exp3(const Eigen::MatrixBase<Vector3Like> & v)
76 {
77 typedef Eigen::Quaternion<
78 typename Vector3Like::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
79 ReturnType;
80 ReturnType res;
81 exp3(v, res);
82 return res;
83 }
84
91 template<typename MotionDerived, typename Config_t>
92 void exp6(const MotionDense<MotionDerived> & motion, Eigen::MatrixBase<Config_t> & qout)
93 {
94 enum
95 {
96 Options = PINOCCHIO_EIGEN_PLAIN_TYPE(Config_t)::Options
97 };
98 typedef typename Config_t::Scalar Scalar;
99 typedef typename MotionDerived::Vector3 Vector3;
100 typedef Eigen::Quaternion<Scalar, Options> Quaternion_t;
101 const Scalar eps = Eigen::NumTraits<Scalar>::epsilon();
102
103 const typename MotionDerived::ConstAngularType & w = motion.angular();
104 const typename MotionDerived::ConstLinearType & v = motion.linear();
105
106 const Scalar t2 = w.squaredNorm() + eps * eps;
107 const Scalar t = math::sqrt(t2);
108
109 Scalar ct, st;
110 SINCOS(t, &st, &ct);
111
112 const Scalar inv_t2 = Scalar(1) / t2;
113 const Scalar ts_prec =
114 TaylorSeriesExpansion<Scalar>::template precision<3>(); // Taylor expansion precision
115
116 using ::pinocchio::internal::if_then_else;
117 using ::pinocchio::internal::LT;
118
119 const Scalar alpha_wxv = if_then_else(
120 LT, t, ts_prec,
121 Scalar(0.5) - t2 / Scalar(24), // then: use Taylor expansion
122 (Scalar(1) - ct) * inv_t2 // else
123 );
124
125 const Scalar alpha_w2 = if_then_else(
126 LT, t, ts_prec, Scalar(1) / Scalar(6) - t2 / Scalar(120), (t - st) * inv_t2 / t);
127
128 // linear part
129 Eigen::Map<Vector3> trans_(qout.derived().template head<3>().data());
130 trans_.noalias() = v + alpha_wxv * w.cross(v) + alpha_w2 * w.cross(w.cross(v));
131
132 // quaternion part
133 typedef Eigen::Map<Quaternion_t> QuaternionMap_t;
134 QuaternionMap_t quat_(qout.derived().template tail<4>().data());
135 exp3(w, quat_);
136 }
137
143 template<typename MotionDerived>
144 Eigen::Matrix<
145 typename MotionDerived::Scalar,
146 7,
147 1,
148 PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionDerived::Vector3)::Options>
149 exp6(const MotionDense<MotionDerived> & motion)
150 {
151 typedef typename MotionDerived::Scalar Scalar;
152 enum
153 {
154 Options = PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionDerived::Vector3)::Options
155 };
156 typedef Eigen::Matrix<Scalar, 7, 1, Options> ReturnType;
157
158 ReturnType qout;
159 exp6(motion, qout);
160 return qout;
161 }
162
169 template<typename Vector6Like, typename Config_t>
170 void exp6(const Eigen::MatrixBase<Vector6Like> & vec6, Eigen::MatrixBase<Config_t> & qout)
171 {
172 MotionRef<const Vector6Like> nu(vec6.derived());
173 ::pinocchio::quaternion::exp6(nu, qout);
174 }
175
181 template<typename Vector6Like>
182 Eigen::
183 Matrix<typename Vector6Like::Scalar, 7, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6Like)::Options>
184 exp6(const Eigen::MatrixBase<Vector6Like> & vec6)
185 {
186 typedef typename Vector6Like::Scalar Scalar;
187 enum
188 {
189 Options = PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6Like)::Options
190 };
191 typedef Eigen::Matrix<Scalar, 7, 1, Options> ReturnType;
192
193 ReturnType qout;
194 ::pinocchio::quaternion::exp6(vec6, qout);
195 return qout;
196 }
197
205 template<typename QuaternionLike>
206 Eigen::Matrix<
207 typename QuaternionLike::Scalar,
208 3,
209 1,
210 PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Vector3)::Options>
211 log3(
212 const Eigen::QuaternionBase<QuaternionLike> & quat, typename QuaternionLike::Scalar & theta)
213 {
214 typedef typename QuaternionLike::Scalar Scalar;
215 enum
216 {
217 Options = PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Vector3)::Options
218 };
219 typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
220
221 Vector3 res;
222 const Scalar norm_squared = quat.vec().squaredNorm();
223
224 static const Scalar eps = Eigen::NumTraits<Scalar>::epsilon();
225 static const Scalar ts_prec = TaylorSeriesExpansion<Scalar>::template precision<2>();
226 const Scalar norm = math::sqrt(norm_squared + eps * eps);
227
228 using ::pinocchio::internal::GE;
229 using ::pinocchio::internal::if_then_else;
230 using ::pinocchio::internal::LT;
231
232 const Scalar pos_neg = if_then_else(GE, quat.w(), Scalar(0), Scalar(+1), Scalar(-1));
233
234 Eigen::Quaternion<Scalar, Options> quat_pos;
235 quat_pos.w() = pos_neg * quat.w();
236 quat_pos.vec() = pos_neg * quat.vec();
237
238 const Scalar theta_2 = math::atan2(norm, quat_pos.w()); // in [0,pi]
239 const Scalar y_x = norm / quat_pos.w(); // nonnegative
240 const Scalar y_x_sq = norm_squared / (quat_pos.w() * quat_pos.w());
241
242 theta = if_then_else(
243 LT, norm_squared, ts_prec, Scalar(2.) * (Scalar(1) - y_x_sq / Scalar(3)) * y_x,
244 Scalar(2.) * theta_2);
245
246 const Scalar th2_2 = theta * theta / Scalar(4);
247 const Scalar inv_sinc = if_then_else(
248 LT, norm_squared, ts_prec,
249 Scalar(2) * (Scalar(1) + th2_2 / Scalar(6) + Scalar(7) / Scalar(360) * th2_2 * th2_2),
250 theta / math::sin(theta_2));
251
252 for (Eigen::DenseIndex k = 0; k < 3; ++k)
253 {
254 // res[k] = if_then_else(LT, norm_squared, ts_prec,
255 // Scalar(2) * (Scalar(1) + y_x_sq / Scalar(6) - y_x_sq*y_x_sq /
256 // Scalar(9)) * pos_neg * quat.vec()[k], inv_sinc * pos_neg *
257 // quat.vec()[k]);
258 res[k] = inv_sinc * quat_pos.vec()[k];
259 }
260 return res;
261 }
262
272 template<typename QuaternionLike>
273 Eigen::Matrix<
274 typename QuaternionLike::Scalar,
275 3,
276 1,
277 PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Vector3)::Options>
278 log3(const Eigen::QuaternionBase<QuaternionLike> & quat)
279 {
280 typename QuaternionLike::Scalar theta;
281 return log3(quat.derived(), theta);
282 }
283
291 template<typename Vector3Like, typename Matrix43Like>
292 void Jexp3CoeffWise(
293 const Eigen::MatrixBase<Vector3Like> & v, const Eigen::MatrixBase<Matrix43Like> & Jexp)
294 {
295 // EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix43Like,4,3);
296 assert(Jexp.rows() == 4 && Jexp.cols() == 3 && "Jexp does have the right size.");
297 Matrix43Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix43Like, Jexp);
298
299 typedef typename Vector3Like::Scalar Scalar;
300
301 const Scalar n2 = v.squaredNorm();
302 const Scalar n = math::sqrt(n2);
303 const Scalar theta = Scalar(0.5) * n;
304 const Scalar theta2 = Scalar(0.25) * n2;
305
306 if (n2 > math::sqrt(Eigen::NumTraits<Scalar>::epsilon()))
307 {
308 Scalar c, s;
309 SINCOS(theta, &s, &c);
310 Jout.template topRows<3>().noalias() =
311 ((Scalar(0.5) / n2) * (c - 2 * s / n)) * v * v.transpose();
312 Jout.template topRows<3>().diagonal().array() += s / n;
313 Jout.template bottomRows<1>().noalias() = -s / (2 * n) * v.transpose();
314 }
315 else
316 {
317 Jout.template topRows<3>().noalias() =
318 (-Scalar(1) / Scalar(12) + n2 / Scalar(480)) * v * v.transpose();
319 Jout.template topRows<3>().diagonal().array() += Scalar(0.5) * (1 - theta2 / 6);
320 Jout.template bottomRows<1>().noalias() =
321 (Scalar(-0.25) * (Scalar(1) - theta2 / 6)) * v.transpose();
322 }
323 }
324
331 template<typename QuaternionLike, typename Matrix3Like>
332 void Jlog3(
333 const Eigen::QuaternionBase<QuaternionLike> & quat,
334 const Eigen::MatrixBase<Matrix3Like> & Jlog)
335 {
336 typedef typename QuaternionLike::Scalar Scalar;
337 typedef Eigen::Matrix<
338 Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Coefficients)::Options>
339 Vector3;
340
341 Scalar t;
342 Vector3 w(log3(quat, t));
343 pinocchio::Jlog3(t, w, PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like, Jlog));
344 }
345 } // namespace quaternion
346} // namespace pinocchio
347
348#endif // ifndef __pinocchio_spatial_explog_quaternion_hpp__
pinocchio::quaternion::Jexp3CoeffWise
void Jexp3CoeffWise(const Eigen::MatrixBase< Vector3Like > &v, const Eigen::MatrixBase< Matrix43Like > &Jexp)
Derivative of where is a small perturbation of at identity.
Definition explog-quaternion.hpp:292
pinocchio::quaternion::exp6
void exp6(const MotionDense< MotionDerived > &motion, Eigen::MatrixBase< Config_t > &qout)
The se3 -> SE3 exponential map, using quaternions to represent the output rotation.
Definition explog-quaternion.hpp:92
pinocchio::quaternion::log3
Eigen::Matrix< typename QuaternionLike::Scalar, 3, 1, typename QuaternionLike::Vector3 ::Options > log3(const Eigen::QuaternionBase< QuaternionLike > &quat, typename QuaternionLike::Scalar &theta)
Same as log3 but with a unit quaternion as input.
Definition explog-quaternion.hpp:211
pinocchio::quaternion::exp3
void exp3(const Eigen::MatrixBase< Vector3Like > &v, Eigen::QuaternionBase< QuaternionLike > &quat_out)
Exp: so3 -> SO3 (quaternion)
Definition explog-quaternion.hpp:27
pinocchio::quaternion::Jlog3
void Jlog3(const Eigen::QuaternionBase< QuaternionLike > &quat, const Eigen::MatrixBase< Matrix3Like > &Jlog)
Computes the Jacobian of log3 operator for a unit quaternion.
Definition explog-quaternion.hpp:332
pinocchio
Main pinocchio namespace.
Definition treeview.dox:11
pinocchio::Jlog3
void Jlog3(const Scalar &theta, const Eigen::MatrixBase< Vector3Like > &log, const Eigen::MatrixBase< Matrix3Like > &Jlog)
Derivative of log3.
Definition explog.hpp:240
pinocchio::SINCOS
void SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
Definition sincos.hpp:27
pinocchio::motion
MotionTpl< Scalar, Options > motion(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataTpl through JointMotionVisitor to get the joint internal motion as a dense motion.