expose-skew.cpp 1.99 KB
Newer Older
1
//
2
3
// Copyright (c) 2015-2020 CNRS INRIA
// Copyright (c) 2020 Wandercraft
4
5
6
//

#include <boost/python.hpp>
7
8
9

#include "pinocchio/bindings/python/fwd.hpp"
#include "pinocchio/spatial/se3.hpp"
10
11
12
13
14
15
16
#include "pinocchio/spatial/skew.hpp"

namespace pinocchio
{
  namespace python
  {
    namespace bp = boost::python;
17
18
19
20
  
    // We need to resort to another call, because it seems that Boost.Python is not aligning the Eigen::MatrixBase. TODO: fix it!
    template<typename Matrix3>
    Eigen::Matrix<typename Matrix3::Scalar,3,1,Matrix3::Options> unSkew(const Matrix3 & mat)
21
    {
22
      return pinocchio::unSkew(mat);
23
    }
24
  
25
26
    void exposeSkew()
    {
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
      typedef SE3::Matrix3 Matrix3;
      typedef SE3::Vector3 Vector3;
      
      bp::def("skew",&skew<Vector3>,
              bp::arg("u"),
              "Computes the skew representation of a given 3d vector, "
              "i.e. the antisymmetric matrix representation of the cross product operator, aka U = [u]x.\n"
              "Parameters:\n"
              "\tu: the input vector of dimension 3");
      
      bp::def("skewSquare",&skewSquare<Vector3,Vector3>,
              bp::args("u","v"),
              "Computes the skew square representation of two given 3d vectors, "
              "i.e. the antisymmetric matrix representation of the chained cross product operator, u x (v x w), where w is another 3d vector.\n"
              "Parameters:\n"
              "\tu: the first input vector of dimension 3\n"
              "\tv: the second input vector of dimension 3");

      bp::def("unSkew",&unSkew<Matrix3>,
              bp::arg("U"),
              "Inverse of skew operator. From a given skew symmetric matrix U (i.e U = -U.T)"
              "of dimension 3x3, it extracts the supporting vector, i.e. the entries of U.\n"
              "Mathematically speacking, it computes v such that U.dot(x) = cross(u, x).\n"
              "Parameters:\n"
              "\tU: the input skew symmetric matrix of dimension 3x3.");
52
53
54
55
    }

  } // namespace python
} // namespace pinocchio