1 |
|
// |
2 |
|
// Copyright (c) 2015-2016 CNRS |
3 |
|
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France. |
4 |
|
// |
5 |
|
// This file is part of Pinocchio |
6 |
|
// Pinocchio is free software: you can redistribute it |
7 |
|
// and/or modify it under the terms of the GNU Lesser General Public |
8 |
|
// License as published by the Free Software Foundation, either version |
9 |
|
// 3 of the License, or (at your option) any later version. |
10 |
|
// |
11 |
|
// Pinocchio is distributed in the hope that it will be |
12 |
|
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty |
13 |
|
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU |
14 |
|
// General Lesser Public License for more details. You should have |
15 |
|
// received a copy of the GNU Lesser General Public License along with |
16 |
|
// Pinocchio If not, see |
17 |
|
// <http://www.gnu.org/licenses/>. |
18 |
|
|
19 |
|
#ifndef __se3_python_explog_hpp__ |
20 |
|
# define __se3_python_explog_hpp__ |
21 |
|
|
22 |
|
# include <eigenpy/eigenpy.hpp> |
23 |
|
|
24 |
|
# include "pinocchio/spatial/explog.hpp" |
25 |
|
|
26 |
|
namespace se3 |
27 |
|
{ |
28 |
|
namespace python |
29 |
|
{ |
30 |
|
namespace bp = boost::python; |
31 |
|
|
32 |
|
struct ExplogPythonVisitor |
33 |
|
{ |
34 |
|
typedef Eigen::Matrix3d Matrix3d; |
35 |
|
typedef Eigen::Vector3d Vector3d; |
36 |
|
typedef Eigen::Matrix4d Matrix4d; |
37 |
|
typedef Eigen::Matrix<double,6,1> Vector6d; |
38 |
|
|
39 |
|
static Matrix3d exp3_proxy(const Vector3d & v) |
40 |
|
{ |
41 |
|
return exp3(v); |
42 |
|
} |
43 |
|
|
44 |
|
static Vector3d log3_proxy(const Matrix3d & R) |
45 |
|
{ |
46 |
|
return log3(R); |
47 |
|
} |
48 |
|
|
49 |
|
static SE3 exp6FromMotion_proxy(const Motion & nu) |
50 |
|
{ |
51 |
|
return exp6(nu); |
52 |
|
} |
53 |
|
|
54 |
|
static SE3 exp6FromVector_proxy(const Vector6d & v) |
55 |
|
{ |
56 |
|
return exp6(v); |
57 |
|
} |
58 |
|
|
59 |
|
static Motion log6FromSE3_proxy(const SE3 & m) |
60 |
|
{ |
61 |
|
return log6(m); |
62 |
|
} |
63 |
|
|
64 |
|
static Motion log6FromMatrix_proxy(const Matrix4d & M) |
65 |
|
{ |
66 |
|
return log6(M); |
67 |
|
} |
68 |
|
|
69 |
1 |
static void expose() |
70 |
|
{ |
71 |
|
bp::def("exp3",exp3_proxy, |
72 |
2 |
bp::args("Angular velocity (double size 3)"), |
73 |
|
"Exp: so3 -> SO3. Return the integral of the input" |
74 |
1 |
" angular velocity during time 1."); |
75 |
|
bp::def("log3",log3_proxy, |
76 |
2 |
bp::args("Rotation matrix (double size 3x3)"), |
77 |
|
"Log: SO3 -> so3. Pseudo-inverse of log from SO3" |
78 |
1 |
" -> { v in so3, ||v|| < 2pi }.Exp: so3 -> SO3."); |
79 |
|
bp::def("exp6FromMotion",exp6FromMotion_proxy, |
80 |
2 |
bp::args("Spatial velocity (se3.Motion)"), |
81 |
|
"Exp: se3 -> SE3. Return the integral of the input" |
82 |
1 |
" spatial velocity during time 1."); |
83 |
|
bp::def("exp6FromVector",exp6FromVector_proxy, |
84 |
2 |
bp::args("Spatial velocity (double size 6)"), |
85 |
|
"Exp: se3 -> SE3. Return the integral of the input" |
86 |
1 |
" spatial velocity during time 1."); |
87 |
|
bp::def("log6FromSE3",log6FromSE3_proxy, |
88 |
2 |
bp::args("Spatial transform (se3.SE3)"), |
89 |
|
"Log: SE3 -> se3. Pseudo-inverse of exp from SE3" |
90 |
1 |
" -> { v,w in se3, ||w|| < 2pi }."); |
91 |
|
bp::def("log6FromMatrix",log6FromMatrix_proxy, |
92 |
2 |
bp::args("Spatial transform (double size 4x4)"), |
93 |
|
"Log: SE3 -> se3. Pseudo-inverse of exp from SE3" |
94 |
1 |
" -> { v,w in se3, ||w|| < 2pi }."); |
95 |
1 |
} |
96 |
|
}; |
97 |
|
} // namespace python |
98 |
|
} //namespace se3 |
99 |
|
|
100 |
|
#endif // ifndef __se3_python_explog_hpp__ |