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