1 |
|
|
// |
2 |
|
|
// Copyright (c) 2015-2018 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::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::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options> |
25 |
|
|
Jexp3_proxy(const Vector3Like & v) |
26 |
|
|
{ |
27 |
|
|
typedef Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options> ReturnType; |
28 |
|
|
ReturnType res; Jexp3(v,res); |
29 |
|
|
return res; |
30 |
|
|
} |
31 |
|
|
|
32 |
|
|
template<typename Matrix3Like> |
33 |
|
|
typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like) |
34 |
|
|
Jlog3_proxy(const Matrix3Like & M) |
35 |
|
|
{ |
36 |
|
|
typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like) ReturnType; |
37 |
|
|
ReturnType res; Jlog3(M,res); |
38 |
|
|
return res; |
39 |
|
|
} |
40 |
|
|
|
41 |
|
|
template<typename Matrix3Like, typename Vector3Like> |
42 |
|
|
typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like) |
43 |
|
|
Hlog3_proxy(const Matrix3Like & M, const Vector3Like & v) |
44 |
|
|
{ |
45 |
|
|
typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like) ReturnType; |
46 |
|
|
ReturnType res; Hlog3(M,v,res); |
47 |
|
|
return res; |
48 |
|
|
} |
49 |
|
|
|
50 |
|
|
template<typename Scalar, int Options> |
51 |
|
|
SE3Tpl<Scalar,Options> exp6_proxy(const MotionTpl<Scalar,Options> & v) |
52 |
|
|
{ |
53 |
|
|
return exp6(v); |
54 |
|
|
} |
55 |
|
|
|
56 |
|
|
template<typename Vector6Like> |
57 |
|
|
SE3Tpl<typename Vector6Like::Scalar,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6Like)::Options> |
58 |
|
|
exp6_proxy(const Vector6Like & vec6) |
59 |
|
|
{ |
60 |
|
|
return exp6(vec6); |
61 |
|
|
} |
62 |
|
|
|
63 |
|
|
template<typename Scalar, int Options> |
64 |
|
89 |
typename SE3Tpl<Scalar,Options>::Matrix6 Jlog6_proxy(const SE3Tpl<Scalar,Options> & M) |
65 |
|
|
{ |
66 |
|
|
typedef typename SE3Tpl<Scalar,Options>::Matrix6 ReturnType; |
67 |
|
89 |
ReturnType res; Jlog6(M,res); |
68 |
|
89 |
return res; |
69 |
|
|
} |
70 |
|
|
|
71 |
|
|
template<typename Scalar, int Options> |
72 |
|
|
typename MotionTpl<Scalar,Options>::Matrix6 |
73 |
|
|
Jexp6_proxy(const MotionTpl<Scalar,Options> & v) |
74 |
|
|
{ |
75 |
|
|
typedef typename MotionTpl<Scalar,Options>::Matrix6 ReturnType; |
76 |
|
|
ReturnType res; Jexp6(v,res); |
77 |
|
|
return res; |
78 |
|
|
} |
79 |
|
|
|
80 |
|
|
template<typename Vector6Like> |
81 |
|
|
Eigen::Matrix<typename Vector6Like::Scalar,6,6,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6Like)::Options> |
82 |
|
|
Jexp6_proxy(const Vector6Like & vec6) |
83 |
|
|
{ |
84 |
|
|
typedef MotionRef<const Vector6Like> Motion; |
85 |
|
|
Motion v(vec6); |
86 |
|
|
typedef typename Motion::Matrix6 ReturnType; |
87 |
|
|
ReturnType res; Jexp6(v,res); |
88 |
|
|
return res; |
89 |
|
|
} |
90 |
|
|
|
91 |
|
|
template<typename Matrix3Like> |
92 |
|
|
Eigen::Matrix<typename Matrix3Like::Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options> |
93 |
|
|
log3_proxy(const Matrix3Like & R) |
94 |
|
|
{ |
95 |
|
|
return log3(R); |
96 |
|
|
} |
97 |
|
|
|
98 |
|
|
template<typename Matrix4Like> |
99 |
|
|
MotionTpl<typename Matrix4Like::Scalar,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix4Like)::Options> |
100 |
|
|
log6_proxy(const Matrix4Like & homegenous_matrix) |
101 |
|
|
{ |
102 |
|
|
return log6(homegenous_matrix); |
103 |
|
|
} |
104 |
|
|
|
105 |
|
|
} // namespace python |
106 |
|
|
} //namespace pinocchio |
107 |
|
|
|
108 |
|
|
#endif // ifndef __pinocchio_python_explog_hpp__ |