bindings/python/spatial/explog.hpp
Go to the documentation of this file.
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 
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  exp3_proxy(const Vector3Like & v)
20  {
21  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  exp3_proxy_quat(const Vector3Like & v)
28  {
29  typedef typename Vector3Like::Scalar Scalar;
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  ReturnType res;
36  QuaternionMap_t quat_out(res.derived().data());
37  quaternion::exp3(v, quat_out);
38  return res;
39  }
40 
41  template<typename Vector3Like>
42  Eigen::
43  Matrix<typename Vector3Like::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
44  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  ReturnType res;
50  Jexp3(v, res);
51  return res;
52  }
53 
54  template<typename Matrix3Like>
55  typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like) Jlog3_proxy(const Matrix3Like & M)
56  {
57  typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like) ReturnType;
58  ReturnType res;
59  Jlog3(M, res);
60  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>
75  {
76  return exp6(v);
77  }
78 
79  template<typename Vector6Like>
81  exp6_proxy(const Vector6Like & vec6)
82  {
83  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>
97  {
98  typedef typename SE3Tpl<Scalar, Options>::Matrix6 ReturnType;
99  ReturnType res;
100  Jlog6(M, res);
101  return res;
102  }
103 
104  template<typename Scalar, int Options>
106  {
107  typedef typename MotionTpl<Scalar, Options>::Matrix6 ReturnType;
108  ReturnType res;
109  Jexp6(v, res);
110  return res;
111  }
112 
113  template<typename Vector6Like>
114  Eigen::
115  Matrix<typename Vector6Like::Scalar, 6, 6, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6Like)::Options>
116  Jexp6_proxy(const Vector6Like & vec6)
117  {
119  Motion v(vec6);
120  typedef typename Motion::Matrix6 ReturnType;
121  ReturnType res;
122  Jexp6(v, res);
123  return res;
124  }
125 
126  template<typename Matrix3Like>
127  Eigen::
128  Matrix<typename Matrix3Like::Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options>
129  log3_proxy(const Matrix3Like & R)
130  {
131  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,
156  log3_proxy(const QuaternionLike & quat)
157  {
158  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  log3_proxy_quatvec(const Vector4Like & v)
165  {
166  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector4Like, 4);
167  typedef typename Vector4Like::Scalar Scalar;
169  Quaternion_t;
170  typedef Eigen::Map<const Quaternion_t> ConstQuaternionMap_t;
171 
172  ConstQuaternionMap_t q(v.derived().data());
174  q, typename Vector4Like::RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
175  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;
186  Quaternion_t;
187  typedef Eigen::Map<const Quaternion_t> ConstQuaternionMap_t;
188 
189  ConstQuaternionMap_t q(v.derived().data());
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;
204  Quaternion_t;
205  typedef Eigen::Map<const Quaternion_t> ConstQuaternionMap_t;
206 
207  ConstQuaternionMap_t q(v.derived().data());
209  q, typename Vector4Like::RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
210 
211  return quaternion::log3(q, theta);
212  }
213 
214  template<typename Matrix4Like>
216  log6_proxy(const Matrix4Like & homegenous_matrix)
217  {
218  return log6(homegenous_matrix);
219  }
220 
221  template<typename Vector7Like>
223  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  {
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  const Vector3 v(q.derived().template head<3>());
236  ConstQuaternionMap quat(q.derived().template tail<4>().data());
237 
238  return log6(quat, v);
239  }
240  } // namespace python
241 } // namespace pinocchio
242 
243 #endif // ifndef __pinocchio_python_explog_hpp__
pinocchio::python::log3_proxy_quatvec_fix
Eigen::Matrix< typename Vector4Like::Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector4Like)::Options > log3_proxy_quatvec_fix(const Vector4Like &v, _Scalar &theta)
Definition: bindings/python/spatial/explog.hpp:199
Eigen
pinocchio::python::Jexp3_proxy
Eigen::Matrix< typename Vector3Like::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options > Jexp3_proxy(const Vector3Like &v)
Definition: bindings/python/spatial/explog.hpp:44
quat
quat
pinocchio::Jexp6
void Jexp6(const MotionDense< MotionDerived > &nu, const Eigen::MatrixBase< Matrix6Like > &Jexp)
Derivative of exp6 Computed as the inverse of Jlog6.
Definition: spatial/explog.hpp:496
pinocchio::python::exp3_proxy_quat
Eigen::Matrix< typename Vector3Like::Scalar, 4, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options > exp3_proxy_quat(const Vector3Like &v)
Definition: bindings/python/spatial/explog.hpp:27
pinocchio::SE3Tpl< Scalar, Options >
pinocchio::quaternion::isNormalized
bool isNormalized(const Eigen::QuaternionBase< Quaternion > &quat, const typename Quaternion::Coefficients::RealScalar &prec=Eigen::NumTraits< typename Quaternion::Coefficients::RealScalar >::dummy_precision())
Check whether the input quaternion is Normalized within the given precision.
Definition: math/quaternion.hpp:229
pinocchio::python::log3_proxy
Eigen::Matrix< typename Matrix3Like::Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options > log3_proxy(const Matrix3Like &R)
Definition: bindings/python/spatial/explog.hpp:129
pinocchio::python::Scalar
context::Scalar Scalar
Definition: admm-solver.cpp:29
pinocchio::exp3
Eigen::Matrix< typename Vector3Like::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options > exp3(const Eigen::MatrixBase< Vector3Like > &v)
Exp: so3 -> SO3.
Definition: spatial/explog.hpp:36
explog.hpp
R
R
pinocchio::Jlog6
void Jlog6(const SE3Tpl< Scalar, Options > &M, const Eigen::MatrixBase< Matrix6Like > &Jlog)
Derivative of log6.
Definition: spatial/explog.hpp:668
pinocchio::python::PINOCCHIO_EIGEN_PLAIN_TYPE
PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixType) inv(const Eigen
Definition: expose-linalg.cpp:18
pinocchio::python::log6_proxy
MotionTpl< typename Matrix4Like::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix4Like)::Options > log6_proxy(const Matrix4Like &homegenous_matrix)
Definition: bindings/python/spatial/explog.hpp:216
pinocchio::Jlog3
void Jlog3(const Scalar &theta, const Eigen::MatrixBase< Vector3Like > &log, const Eigen::MatrixBase< Matrix3Like > &Jlog)
Derivative of log3.
Definition: spatial/explog.hpp:240
pinocchio::python::context::Quaternion
Eigen::Quaternion< Scalar, Options > Quaternion
Definition: bindings/python/context/generic.hpp:49
pinocchio::python::Options
@ Options
Definition: expose-contact-inverse-dynamics.cpp:22
pinocchio::python::log6_proxy_quatvec
MotionTpl< typename Vector7Like::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector7Like)::Options > log6_proxy_quatvec(const Vector7Like &q)
Definition: bindings/python/spatial/explog.hpp:223
pinocchio::python::log3_proxy_fix
Eigen::Matrix< typename Matrix3Like::Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options > log3_proxy_fix(const Matrix3Like &R, Scalar &theta)
Definition: bindings/python/spatial/explog.hpp:145
pinocchio::quaternion::exp3
void exp3(const Eigen::MatrixBase< Vector3Like > &v, Eigen::QuaternionBase< QuaternionLike > &quat_out)
Exp: so3 -> SO3 (quaternion)
Definition: explog-quaternion.hpp:27
python
pinocchio::python::v
const Vector3Like & v
Definition: bindings/python/spatial/explog.hpp:66
pinocchio::exp6
SE3Tpl< typename MotionDerived::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionDerived::Vector3)::Options > exp6(const MotionDense< MotionDerived > &nu)
Exp: se3 -> SE3.
Definition: spatial/explog.hpp:347
pinocchio::quaternion::log3
Eigen::Matrix< typename QuaternionLike::Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(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::python::exp6_proxy_quatvec
Eigen::Matrix< typename Vector6Like::Scalar, 7, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6Like)::Options > exp6_proxy_quatvec(const Vector6Like &vec6)
Definition: bindings/python/spatial/explog.hpp:89
M
M
pinocchio::context::Vector3
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
Definition: context/generic.hpp:53
pinocchio::python::Jlog6_proxy
SE3Tpl< Scalar, Options >::Matrix6 Jlog6_proxy(const SE3Tpl< Scalar, Options > &M)
Definition: bindings/python/spatial/explog.hpp:96
pinocchio::Motion
MotionTpl<::CppAD::AD< double >, 0 > Motion
Definition: context/cppad.hpp:37
pinocchio::q
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
Definition: joint-configuration.hpp:1083
PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE
#define PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE
Definition: math/quaternion.hpp:9
pinocchio::log6
MotionTpl< Scalar, Options > log6(const SE3Tpl< Scalar, Options > &M)
Log: SE3 -> se3.
Definition: spatial/explog.hpp:435
pinocchio::python::Hlog3
Hlog3(M, v, res)
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::python::Jexp6_proxy
MotionTpl< Scalar, Options >::Matrix6 Jexp6_proxy(const MotionTpl< Scalar, Options > &v)
Definition: bindings/python/spatial/explog.hpp:105
pinocchio::Jexp3
void Jexp3(const Eigen::MatrixBase< Vector3Like > &r, const Eigen::MatrixBase< Matrix3Like > &Jexp)
Derivative of .
Definition: spatial/explog.hpp:118
pinocchio::log3
Eigen::Matrix< typename Matrix3Like::Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options > log3(const Eigen::MatrixBase< Matrix3Like > &R, typename Matrix3Like::Scalar &theta)
Same as log3.
Definition: spatial/explog.hpp:83
pinocchio::python::exp3_proxy
Eigen::Matrix< typename Vector3Like::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options > exp3_proxy(const Vector3Like &v)
Definition: bindings/python/spatial/explog.hpp:19
pinocchio::python::exp6_proxy
SE3Tpl< Scalar, Options > exp6_proxy(const MotionTpl< Scalar, Options > &v)
Definition: bindings/python/spatial/explog.hpp:74
pinocchio::MotionTpl< Scalar, Options >
pinocchio::MotionRef
Definition: context/casadi.hpp:38
pinocchio::python::log3_proxy_quatvec
Eigen::Matrix< typename Vector4Like::Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector4Like)::Options > log3_proxy_quatvec(const Vector4Like &v)
Definition: bindings/python/spatial/explog.hpp:164
pinocchio::python::res
ReturnType res
Definition: bindings/python/spatial/explog.hpp:68
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27


pinocchio
Author(s):
autogenerated on Sun Dec 22 2024 03:41:09