1 |
|
|
// |
2 |
|
|
// Copyright (c) 2015-2021 CNRS INRIA |
3 |
|
|
// Copyright (c) 2020 Wandercraft |
4 |
|
|
// |
5 |
|
|
|
6 |
|
|
#include "pinocchio/bindings/python/fwd.hpp" |
7 |
|
|
#include "pinocchio/spatial/se3.hpp" |
8 |
|
|
#include "pinocchio/spatial/skew.hpp" |
9 |
|
|
|
10 |
|
|
namespace pinocchio |
11 |
|
|
{ |
12 |
|
|
namespace python |
13 |
|
|
{ |
14 |
|
|
namespace bp = boost::python; |
15 |
|
|
|
16 |
|
|
// We need to resort to another call, because it seems that Boost.Python is not aligning the Eigen::MatrixBase. TODO: fix it! |
17 |
|
|
template<typename Vector3> |
18 |
|
|
Eigen::Matrix<typename Vector3::Scalar,3,3,Vector3::Options> skew(const Vector3 & v) |
19 |
|
|
{ |
20 |
|
|
return pinocchio::skew(v); |
21 |
|
|
} |
22 |
|
|
|
23 |
|
|
// We need to resort to another call, because it seems that Boost.Python is not aligning the Eigen::MatrixBase. TODO: fix it! |
24 |
|
|
template<typename Vector3> |
25 |
|
|
Eigen::Matrix<typename Vector3::Scalar,3,3,Vector3::Options> skewSquare(const Vector3 & u, const Vector3 & v) |
26 |
|
|
{ |
27 |
|
|
return pinocchio::skewSquare(u,v); |
28 |
|
|
} |
29 |
|
|
|
30 |
|
|
// We need to resort to another call, because it seems that Boost.Python is not aligning the Eigen::MatrixBase. TODO: fix it! |
31 |
|
|
template<typename Matrix3> |
32 |
|
|
Eigen::Matrix<typename Matrix3::Scalar,3,1,Matrix3::Options> unSkew(const Matrix3 & mat) |
33 |
|
|
{ |
34 |
|
|
return pinocchio::unSkew(mat); |
35 |
|
|
} |
36 |
|
|
|
37 |
|
19 |
void exposeSkew() |
38 |
|
|
{ |
39 |
|
|
typedef SE3::Matrix3 Matrix3; |
40 |
|
|
typedef SE3::Vector3 Vector3; |
41 |
|
|
|
42 |
✓✗ |
19 |
bp::def("skew",&skew<Vector3>, |
43 |
|
38 |
bp::arg("u"), |
44 |
|
|
"Computes the skew representation of a given 3d vector, " |
45 |
|
|
"i.e. the antisymmetric matrix representation of the cross product operator, aka U = [u]x.\n" |
46 |
|
|
"Parameters:\n" |
47 |
|
|
"\tu: the input vector of dimension 3"); |
48 |
|
|
|
49 |
✓✗ |
19 |
bp::def("skewSquare",&skewSquare<Vector3>, |
50 |
|
38 |
bp::args("u","v"), |
51 |
|
|
"Computes the skew square representation of two given 3d vectors, " |
52 |
|
|
"i.e. the antisymmetric matrix representation of the chained cross product operator, u x (v x w), where w is another 3d vector.\n" |
53 |
|
|
"Parameters:\n" |
54 |
|
|
"\tu: the first input vector of dimension 3\n" |
55 |
|
|
"\tv: the second input vector of dimension 3"); |
56 |
|
|
|
57 |
✓✗ |
19 |
bp::def("unSkew",&unSkew<Matrix3>, |
58 |
|
38 |
bp::arg("U"), |
59 |
|
|
"Inverse of skew operator. From a given skew symmetric matrix U (i.e U = -U.T)" |
60 |
|
|
"of dimension 3x3, it extracts the supporting vector, i.e. the entries of U.\n" |
61 |
|
|
"Mathematically speacking, it computes v such that U.dot(x) = cross(u, x).\n" |
62 |
|
|
"Parameters:\n" |
63 |
|
|
"\tU: the input skew symmetric matrix of dimension 3x3."); |
64 |
|
19 |
} |
65 |
|
|
|
66 |
|
|
} // namespace python |
67 |
|
|
} // namespace pinocchio |