GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2018-2020 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 |
|||
12 |
namespace pinocchio |
||
13 |
{ |
||
14 |
namespace quaternion |
||
15 |
{ |
||
16 |
|||
17 |
/// |
||
18 |
/// \brief Exp: so3 -> SO3 (quaternion) |
||
19 |
/// |
||
20 |
/// \returns the integral of the velocity vector as a queternion. |
||
21 |
/// |
||
22 |
/// \param[in] v The angular velocity vector. |
||
23 |
/// \param[out] qout The quanternion where the result is stored. |
||
24 |
/// |
||
25 |
template<typename Vector3Like, typename QuaternionLike> |
||
26 |
12969 |
void exp3(const Eigen::MatrixBase<Vector3Like> & v, |
|
27 |
Eigen::QuaternionBase<QuaternionLike> & quat_out) |
||
28 |
{ |
||
29 |
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like); |
||
30 |
✓✗✗✓ |
12969 |
assert(v.size() == 3); |
31 |
|||
32 |
typedef typename Vector3Like::Scalar Scalar; |
||
33 |
enum { Options = PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Coefficients)::Options }; |
||
34 |
typedef Eigen::Quaternion<typename QuaternionLike::Scalar,Options> QuaternionPlain; |
||
35 |
|||
36 |
✓✗ | 12969 |
const Scalar t2 = v.squaredNorm(); |
37 |
12969 |
const Scalar t = math::sqrt(t2); |
|
38 |
|||
39 |
✓✓✓✗ |
12969 |
static const Scalar ts_prec = math::sqrt(Eigen::NumTraits<Scalar>::epsilon()); // Precision for the Taylor series expansion. |
40 |
|||
41 |
✓✗✓✗ |
12969 |
Eigen::AngleAxis<Scalar> aa(t,v/t); |
42 |
✓✗ | 12969 |
QuaternionPlain quat_then(aa); |
43 |
|||
44 |
✓✗ | 12969 |
QuaternionPlain quat_else; |
45 |
✓✗✓✗ ✓✗ |
12969 |
quat_else.vec() = (Scalar(1)/Scalar(2) - t2/48) * v; |
46 |
✓✗ | 12969 |
quat_else.w() = Scalar(1) - t2/8; |
47 |
|||
48 |
using ::pinocchio::internal::if_then_else; |
||
49 |
✓✓ | 64845 |
for(Eigen::DenseIndex k = 0; k < 4; ++k) |
50 |
{ |
||
51 |
✓✗✓✓ ✓✗✓✗ ✓✗✓✗ |
103752 |
quat_out.coeffs().coeffRef(k) = if_then_else(::pinocchio::internal::GT, t2, ts_prec, |
52 |
51876 |
quat_then.coeffs().coeffRef(k), |
|
53 |
51876 |
quat_else.coeffs().coeffRef(k)); |
|
54 |
} |
||
55 |
|||
56 |
12969 |
} |
|
57 |
|||
58 |
/// |
||
59 |
/// \brief Exp: so3 -> SO3 (quaternion) |
||
60 |
/// |
||
61 |
/// \returns the integral of the velocity vector as a queternion. |
||
62 |
/// |
||
63 |
/// \param[in] v The angular velocity vector. |
||
64 |
/// |
||
65 |
template<typename Vector3Like> |
||
66 |
Eigen::Quaternion<typename Vector3Like::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options> |
||
67 |
exp3(const Eigen::MatrixBase<Vector3Like> & v) |
||
68 |
{ |
||
69 |
typedef Eigen::Quaternion<typename Vector3Like::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options> ReturnType; |
||
70 |
ReturnType res; exp3(v,res); |
||
71 |
return res; |
||
72 |
} |
||
73 |
|||
74 |
/// |
||
75 |
/// \brief Same as \ref log3 but with a unit quaternion as input. |
||
76 |
/// |
||
77 |
/// \param[in] quat the unit quaternion. |
||
78 |
/// \param[out] theta the angle value (resuling from compurations). |
||
79 |
/// |
||
80 |
/// \return The angular velocity vector associated to the rotation matrix. |
||
81 |
/// |
||
82 |
template<typename QuaternionLike> |
||
83 |
Eigen::Matrix<typename QuaternionLike::Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Vector3)::Options> |
||
84 |
12438 |
log3(const Eigen::QuaternionBase<QuaternionLike> & quat, |
|
85 |
typename QuaternionLike::Scalar & theta) |
||
86 |
{ |
||
87 |
typedef typename QuaternionLike::Scalar Scalar; |
||
88 |
enum { Options = PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Vector3)::Options }; |
||
89 |
typedef Eigen::Matrix<Scalar,3,1,Options> Vector3; |
||
90 |
|||
91 |
✓✗ | 12438 |
Vector3 res; |
92 |
✓✗✓✗ |
12438 |
const Scalar norm_squared = quat.vec().squaredNorm(); |
93 |
|||
94 |
✓✓✓✗ |
12438 |
static const Scalar eps = Eigen::NumTraits<Scalar>::epsilon(); |
95 |
✓✓✓✗ ✓✗✗✗ |
12438 |
static const Scalar ts_prec = TaylorSeriesExpansion<Scalar>::template precision<2>(); |
96 |
12438 |
const Scalar norm = math::sqrt(norm_squared + eps * eps); |
|
97 |
|||
98 |
using ::pinocchio::internal::if_then_else; |
||
99 |
using ::pinocchio::internal::GE; |
||
100 |
using ::pinocchio::internal::LT; |
||
101 |
|||
102 |
✓✗✓✗ |
12438 |
const Scalar pos_neg = if_then_else(GE, quat.w(), Scalar(0), |
103 |
Scalar(+1), |
||
104 |
Scalar(-1)); |
||
105 |
|||
106 |
✓✗ | 12438 |
Eigen::Quaternion<Scalar, Options> quat_pos; |
107 |
✓✗✓✗ |
12438 |
quat_pos.w() = pos_neg * quat.w(); |
108 |
✓✗✓✗ ✓✗✓✗ |
12438 |
quat_pos.vec() = pos_neg * quat.vec(); |
109 |
|||
110 |
✓✗✓✗ |
12438 |
const Scalar theta_2 = math::atan2(norm,quat_pos.w()); // in [0,pi] |
111 |
✓✗ | 12438 |
const Scalar y_x = norm / quat_pos.w(); // nonnegative |
112 |
✓✗✓✗ |
12438 |
const Scalar y_x_sq = norm_squared / (quat_pos.w() * quat_pos.w()); |
113 |
|||
114 |
24876 |
theta = if_then_else(LT, norm_squared, ts_prec, |
|
115 |
Scalar(2.)*(Scalar(1) - y_x_sq / Scalar(3)) * y_x, |
||
116 |
✓✗ | 12438 |
Scalar(2.)*theta_2); |
117 |
|||
118 |
12438 |
const Scalar th2_2 = theta * theta / Scalar(4); |
|
119 |
24876 |
const Scalar inv_sinc = if_then_else(LT, norm_squared, ts_prec, |
|
120 |
Scalar(2) * (Scalar(1) + th2_2 / Scalar(6) + Scalar(7)/Scalar(360) * th2_2*th2_2), |
||
121 |
✓✗ | 12438 |
theta / math::sin(theta_2)); |
122 |
|||
123 |
✓✓ | 49752 |
for(Eigen::DenseIndex k = 0; k < 3; ++k) |
124 |
{ |
||
125 |
// res[k] = if_then_else(LT, norm_squared, ts_prec, |
||
126 |
// Scalar(2) * (Scalar(1) + y_x_sq / Scalar(6) - y_x_sq*y_x_sq / Scalar(9)) * pos_neg * quat.vec()[k], |
||
127 |
// inv_sinc * pos_neg * quat.vec()[k]); |
||
128 |
✓✗✓✗ ✓✗ |
37314 |
res[k] = inv_sinc * quat_pos.vec()[k]; |
129 |
} |
||
130 |
24876 |
return res; |
|
131 |
} |
||
132 |
|||
133 |
/// |
||
134 |
/// \brief Log: SO3 -> so3. |
||
135 |
/// |
||
136 |
/// Pseudo-inverse of log from \f$ SO3 -> { v \in so3, ||v|| \le pi } \f$. |
||
137 |
/// |
||
138 |
/// \param[in] quat The unit quaternion representing a certain rotation. |
||
139 |
/// |
||
140 |
/// \return The angular velocity vector associated to the quaternion. |
||
141 |
/// |
||
142 |
template<typename QuaternionLike> |
||
143 |
Eigen::Matrix<typename QuaternionLike::Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Vector3)::Options> |
||
144 |
4996 |
log3(const Eigen::QuaternionBase<QuaternionLike> & quat) |
|
145 |
{ |
||
146 |
typename QuaternionLike::Scalar theta; |
||
147 |
✓✗ | 9992 |
return log3(quat.derived(),theta); |
148 |
} |
||
149 |
|||
150 |
/// |
||
151 |
/// \brief Derivative of \f$ q = \exp{\mathbf{v} + \delta\mathbf{v}} \f$ where \f$ \delta\mathbf{v} \f$ |
||
152 |
/// is a small perturbation of \f$ \mathbf{v} \f$ at identity. |
||
153 |
/// |
||
154 |
/// \returns The Jacobian of the quaternion components variation. |
||
155 |
/// |
||
156 |
template<typename Vector3Like, typename Matrix43Like> |
||
157 |
64 |
void Jexp3CoeffWise(const Eigen::MatrixBase<Vector3Like> & v, |
|
158 |
const Eigen::MatrixBase<Matrix43Like> & Jexp) |
||
159 |
{ |
||
160 |
// EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix43Like,4,3); |
||
161 |
✓✗✓✗ ✓✗✓✗ |
64 |
assert(Jexp.rows() == 4 && Jexp.cols() == 3 && "Jexp does have the right size."); |
162 |
64 |
Matrix43Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix43Like,Jexp); |
|
163 |
|||
164 |
typedef typename Vector3Like::Scalar Scalar; |
||
165 |
|||
166 |
✓✗ | 64 |
const Scalar n2 = v.squaredNorm(); |
167 |
64 |
const Scalar n = math::sqrt(n2); |
|
168 |
64 |
const Scalar theta = Scalar(0.5) * n; |
|
169 |
64 |
const Scalar theta2 = Scalar(0.25) * n2; |
|
170 |
|||
171 |
✓✓ | 64 |
if(n2 > math::sqrt(Eigen::NumTraits<Scalar>::epsilon())) |
172 |
{ |
||
173 |
Scalar c, s; |
||
174 |
63 |
SINCOS(theta,&s,&c); |
|
175 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
63 |
Jout.template topRows<3>().noalias() = ((Scalar(0.5)/n2) * (c - 2*s/n)) * v * v.transpose(); |
176 |
✓✗✓✗ ✓✗✓✗ |
63 |
Jout.template topRows<3>().diagonal().array() += s/n; |
177 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
63 |
Jout.template bottomRows<1>().noalias() = -s/(2*n) * v.transpose(); |
178 |
} |
||
179 |
else |
||
180 |
{ |
||
181 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1 |
Jout.template topRows<3>().noalias() = (-Scalar(1)/Scalar(12) + n2/Scalar(480)) * v * v.transpose(); |
182 |
✓✗✓✗ ✓✗✓✗ |
1 |
Jout.template topRows<3>().diagonal().array() += Scalar(0.5) * (1 - theta2/6); |
183 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
1 |
Jout.template bottomRows<1>().noalias() = (Scalar(-0.25) * (Scalar(1) - theta2/6)) * v.transpose(); |
184 |
|||
185 |
} |
||
186 |
64 |
} |
|
187 |
|||
188 |
/// |
||
189 |
/// \brief Computes the Jacobian of log3 operator for a unit quaternion. |
||
190 |
/// |
||
191 |
/// \param[in] quat A unit quaternion representing the input rotation. |
||
192 |
/// \param[out] Jlog The resulting Jacobian of the log operator. |
||
193 |
/// |
||
194 |
template<typename QuaternionLike, typename Matrix3Like> |
||
195 |
341 |
void Jlog3(const Eigen::QuaternionBase<QuaternionLike> & quat, |
|
196 |
const Eigen::MatrixBase<Matrix3Like> & Jlog) |
||
197 |
{ |
||
198 |
typedef typename QuaternionLike::Scalar Scalar; |
||
199 |
typedef Eigen::Matrix<Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Coefficients)::Options> Vector3; |
||
200 |
|||
201 |
Scalar t; |
||
202 |
✓✗ | 341 |
Vector3 w(log3(quat,t)); |
203 |
✓✗ | 341 |
pinocchio::Jlog3(t,w,PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,Jlog)); |
204 |
341 |
} |
|
205 |
} // namespace quaternion |
||
206 |
} |
||
207 |
|||
208 |
#endif // ifndef __pinocchio_spatial_explog_quaternion_hpp__ |
Generated by: GCOVR (Version 4.2) |