GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: bindings/python/spatial/expose-explog.cpp Lines: 26 26 100.0 %
Date: 2024-01-23 21:41:47 Branches: 12 24 50.0 %

Line Branch Exec Source
1
//
2
// Copyright (c) 2015-2019 CNRS INRIA
3
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4
//
5
6
#include "pinocchio/bindings/python/fwd.hpp"
7
#include "pinocchio/bindings/python/spatial/explog.hpp"
8
9
namespace pinocchio
10
{
11
  namespace python
12
  {
13
    namespace bp = boost::python;
14
15
19
    void exposeExplog()
16
    {
17
18
19
      bp::def("exp3",&exp3_proxy<Eigen::Vector3d>,
19
38
              bp::arg("v"),
20
              "Exp: so3 -> SO3. Return the integral of the input"
21
              " angular velocity during time 1.");
22
23
19
      bp::def("Jexp3",&Jexp3_proxy<Eigen::Vector3d>,
24
38
              bp::arg("v"),
25
              "Jacobian of exp(R) which maps from the tangent of SO(3) at exp(v) to"
26
              " the tangent of SO(3) at Identity.");
27
28
19
      bp::def("log3",&log3_proxy<Eigen::Matrix3d>,
29
38
              bp::arg("R"),
30
              "Log: SO3 -> so3. Pseudo-inverse of log from SO3"
31
              " -> { v in so3, ||v|| < 2pi }.Exp: so3 -> SO3.");
32
33
19
      bp::def("Jlog3",&Jlog3_proxy<Eigen::Matrix3d>,
34
38
              bp::arg("R"),
35
              "Jacobian of log(R) which maps from the tangent of SO(3) at R to"
36
              " the tangent of SO(3) at Identity.");
37
38
19
      bp::def("Hlog3",&Hlog3_proxy<Eigen::Matrix3d, Eigen::Vector3d>,
39
38
              bp::args("R","v"),
40
              "Vector v to be multiplied to the hessian",
41
              "v^T * H where H is the Hessian of log(R)");
42
43
19
      bp::def("exp6",&exp6_proxy<double,0>,
44
38
              bp::arg("v"),
45
              "Exp: se3 -> SE3. Return the integral of the input"
46
              " spatial velocity during time 1.");
47
48
19
      bp::def("exp6",&exp6_proxy<Motion::Vector6>,
49
38
              bp::arg("v"),
50
              "Exp: se3 -> SE3. Return the integral of the input"
51
              " spatial velocity during time 1.");
52
53
19
      bp::def("Jexp6",&Jexp6_proxy<double,0>,
54
38
              bp::arg("v"),
55
              "Jacobian of exp(v) which maps from the tangent of SE(3) at exp(v) to"
56
              " the tangent of SE(3) at Identity.");
57
58
19
      bp::def("Jexp6",&Jexp6_proxy<Motion::Vector6>,
59
38
              bp::arg("v"),
60
              "Jacobian of exp(v) which maps from the tangent of SE(3) at exp(v) to"
61
              " the tangent of SE(3) at Identity.");
62
63
19
      bp::def("log6",(Motion (*)(const SE3 &))&log6<double,0>,
64
38
              bp::arg("M"),
65
              "Log: SE3 -> se3. Pseudo-inverse of exp from SE3"
66
              " -> { v,w in se3, ||w|| < 2pi }.");
67
68
19
      bp::def("log6",&log6_proxy<Eigen::Matrix4d>,
69
38
              bp::arg("H"),
70
              "Log: SE3 -> se3. Pseudo-inverse of exp from SE3"
71
              " -> { v,w in se3, ||w|| < 2pi }.");
72
73
19
      bp::def("Jlog6",&Jlog6_proxy<double,0>,
74
38
              bp::arg("M"),
75
              "Jacobian of log(M) which maps from the tangent of SE(3) at M to"
76
              " the tangent of SE(3) at Identity.");
77
78
19
    }
79
80
  } // namespace python
81
} // namespace pinocchio