GCC Code Coverage Report

Directory: ./
File: include/pinocchio/bindings/python/spatial/explog.hpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 46 64 71.9%
Branches: 17 71 23.9%
Line Branch Exec Source
1 //
2 // Copyright (c) 2015-2021 CNRS INRIA
3 // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4 //
5
6 #ifndef __pinocchio_python_explog_hpp__
7 #define __pinocchio_python_explog_hpp__
8
9 #include "pinocchio/spatial/explog.hpp"
10
11 namespace pinocchio
12 {
13 namespace python
14 {
15
16 template<typename Vector3Like>
17 Eigen::
18 Matrix<typename Vector3Like::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
19 4 exp3_proxy(const Vector3Like & v)
20 {
21 4 return exp3(v);
22 }
23
24 template<typename Vector3Like>
25 Eigen::
26 Matrix<typename Vector3Like::Scalar, 4, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
27 1 exp3_proxy_quat(const Vector3Like & v)
28 {
29 typedef typename Vector3Like::Scalar Scalar;
30 typedef Eigen::Quaternion<Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
31 Quaternion_t;
32 typedef Eigen::Map<Quaternion_t> QuaternionMap_t;
33 typedef Eigen::Matrix<Scalar, 4, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
34 ReturnType;
35
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 ReturnType res;
36
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 QuaternionMap_t quat_out(res.derived().data());
37
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 quaternion::exp3(v, quat_out);
38 2 return res;
39 }
40
41 template<typename Vector3Like>
42 Eigen::
43 Matrix<typename Vector3Like::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
44 1 Jexp3_proxy(const Vector3Like & v)
45 {
46 typedef Eigen::Matrix<
47 typename Vector3Like::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
48 ReturnType;
49 1 ReturnType res;
50
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 1 Jexp3(v, res);
51 1 return res;
52 }
53
54 template<typename Matrix3Like>
55 11 typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like) Jlog3_proxy(const Matrix3Like & M)
56 {
57 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like) ReturnType;
58 11 ReturnType res;
59
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 11 Jlog3(M, res);
60 11 return res;
61 }
62
63 template<typename Matrix3Like, typename Vector3Like>
64 typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)
65 ✗ Hlog3_proxy(const Matrix3Like & M, const Vector3Like & v)
66 {
67 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like) ReturnType;
68 ✗ ReturnType res;
69 ✗ Hlog3(M, v, res);
70 ✗ return res;
71 }
72
73 template<typename Scalar, int Options>
74 5 SE3Tpl<Scalar, Options> exp6_proxy(const MotionTpl<Scalar, Options> & v)
75 {
76 5 return exp6(v);
77 }
78
79 template<typename Vector6Like>
80 SE3Tpl<typename Vector6Like::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6Like)::Options>
81 1 exp6_proxy(const Vector6Like & vec6)
82 {
83 1 return exp6(vec6);
84 }
85
86 template<typename Vector6Like>
87 Eigen::
88 Matrix<typename Vector6Like::Scalar, 7, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6Like)::Options>
89 ✗ exp6_proxy_quatvec(const Vector6Like & vec6)
90 {
91 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector6Like, 6);
92 ✗ return quaternion::exp6(vec6); // use quaternion-exp6 overload
93 }
94
95 template<typename Scalar, int Options>
96 100 typename SE3Tpl<Scalar, Options>::Matrix6 Jlog6_proxy(const SE3Tpl<Scalar, Options> & M)
97 {
98 typedef typename SE3Tpl<Scalar, Options>::Matrix6 ReturnType;
99 100 ReturnType res;
100
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 100 Jlog6(M, res);
101 100 return res;
102 }
103
104 template<typename Scalar, int Options>
105 1 typename MotionTpl<Scalar, Options>::Matrix6 Jexp6_proxy(const MotionTpl<Scalar, Options> & v)
106 {
107 typedef typename MotionTpl<Scalar, Options>::Matrix6 ReturnType;
108 1 ReturnType res;
109
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 1 Jexp6(v, res);
110 1 return res;
111 }
112
113 template<typename Vector6Like>
114 Eigen::
115 Matrix<typename Vector6Like::Scalar, 6, 6, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6Like)::Options>
116 1 Jexp6_proxy(const Vector6Like & vec6)
117 {
118 typedef MotionRef<const Vector6Like> Motion;
119
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Motion v(vec6);
120 typedef typename Motion::Matrix6 ReturnType;
121
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 ReturnType res;
122
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Jexp6(v, res);
123 2 return res;
124 }
125
126 template<typename Matrix3Like>
127 Eigen::
128 Matrix<typename Matrix3Like::Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options>
129 15 log3_proxy(const Matrix3Like & R)
130 {
131 15 return log3(R);
132 }
133
134 template<typename Matrix3Like, typename Matrix1Like>
135 Eigen::
136 Matrix<typename Matrix3Like::Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options>
137 ✗ log3_proxy(const Matrix3Like & R, Eigen::Ref<Matrix1Like> theta)
138 {
139 ✗ return log3(R, theta.coeffRef(0, 0));
140 }
141
142 template<typename Matrix3Like, typename Scalar>
143 Eigen::
144 Matrix<typename Matrix3Like::Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options>
145 ✗ log3_proxy_fix(const Matrix3Like & R, Scalar & theta)
146 {
147 ✗ return log3(R, theta);
148 }
149
150 template<typename QuaternionLike>
151 Eigen::Matrix<
152 typename QuaternionLike::Scalar,
153 3,
154 1,
155 PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Vector3)::Options>
156 1 log3_proxy(const QuaternionLike & quat)
157 {
158 1 return quaternion::log3(quat);
159 }
160
161 template<typename Vector4Like>
162 Eigen::
163 Matrix<typename Vector4Like::Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector4Like)::Options>
164 1 log3_proxy_quatvec(const Vector4Like & v)
165 {
166 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector4Like, 4);
167 typedef typename Vector4Like::Scalar Scalar;
168 typedef Eigen::Quaternion<Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector4Like)::Options>
169 Quaternion_t;
170 typedef Eigen::Map<const Quaternion_t> ConstQuaternionMap_t;
171
172
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 ConstQuaternionMap_t q(v.derived().data());
173
2/7
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 1 assert(quaternion::isNormalized(
174 q, typename Vector4Like::RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
175
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 return quaternion::log3(q);
176 }
177
178 template<typename Vector4Like, typename Matrix1Like>
179 Eigen::
180 Matrix<typename Vector4Like::Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector4Like)::Options>
181 ✗ log3_proxy_quatvec(const Vector4Like & v, Eigen::Ref<Matrix1Like> theta)
182 {
183 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector4Like, 4);
184 typedef typename Vector4Like::Scalar Scalar;
185 typedef Eigen::Quaternion<Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector4Like)::Options>
186 Quaternion_t;
187 typedef Eigen::Map<const Quaternion_t> ConstQuaternionMap_t;
188
189 ✗ ConstQuaternionMap_t q(v.derived().data());
190 ✗ assert(quaternion::isNormalized(
191 q, typename Vector4Like::RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
192
193 ✗ return quaternion::log3(q, theta.coeffRef(0, 0));
194 }
195
196 template<typename Vector4Like, typename _Scalar>
197 Eigen::
198 Matrix<typename Vector4Like::Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector4Like)::Options>
199 ✗ log3_proxy_quatvec_fix(const Vector4Like & v, _Scalar & theta)
200 {
201 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector4Like, 4);
202 typedef typename Vector4Like::Scalar Scalar;
203 typedef Eigen::Quaternion<Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector4Like)::Options>
204 Quaternion_t;
205 typedef Eigen::Map<const Quaternion_t> ConstQuaternionMap_t;
206
207 ✗ ConstQuaternionMap_t q(v.derived().data());
208 ✗ assert(quaternion::isNormalized(
209 q, typename Vector4Like::RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
210
211 ✗ return quaternion::log3(q, theta);
212 }
213
214 template<typename Matrix4Like>
215 MotionTpl<typename Matrix4Like::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix4Like)::Options>
216 2 log6_proxy(const Matrix4Like & homegenous_matrix)
217 {
218 2 return log6(homegenous_matrix);
219 }
220
221 template<typename Vector7Like>
222 MotionTpl<typename Vector7Like::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector7Like)::Options>
223 1 log6_proxy_quatvec(const Vector7Like & q)
224 {
225 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector7Like, 7);
226 typedef typename Vector7Like::Scalar Scalar;
227 enum
228 {
229 Options = PINOCCHIO_EIGEN_PLAIN_TYPE(Vector7Like)::Options
230 };
231 typedef Eigen::Quaternion<Scalar, Options> Quaternion;
232 typedef Eigen::Map<const Quaternion, Options> ConstQuaternionMap;
233 typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
234
235
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 const Vector3 v(q.derived().template head<3>());
236
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 1 ConstQuaternionMap quat(q.derived().template tail<4>().data());
237
238
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 return log6(quat, v);
239 }
240 } // namespace python
241 } // namespace pinocchio
242
243 #endif // ifndef __pinocchio_python_explog_hpp__
244