GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/pinocchio/bindings/python/spatial/motion.hpp Lines: 44 60 73.3 %
Date: 2024-01-23 21:41:47 Branches: 67 150 44.7 %

Line Branch Exec Source
1
//
2
// Copyright (c) 2015-2023 CNRS INRIA
3
// Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4
//
5
6
#ifndef __pinocchio_python_spatial_motion_hpp__
7
#define __pinocchio_python_spatial_motion_hpp__
8
9
#include <eigenpy/eigenpy.hpp>
10
#include <eigenpy/memory.hpp>
11
#include <boost/python/tuple.hpp>
12
13
#include "pinocchio/spatial/se3.hpp"
14
#include "pinocchio/spatial/motion.hpp"
15
#include "pinocchio/spatial/force.hpp"
16
#include "pinocchio/bindings/python/fwd.hpp"
17
#include "pinocchio/bindings/python/utils/copyable.hpp"
18
#include "pinocchio/bindings/python/utils/printable.hpp"
19
20
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(pinocchio::Motion)
21
22
namespace pinocchio
23
{
24
  namespace python
25
  {
26
    namespace bp = boost::python;
27
28
    template<typename T> struct call;
29
30
    template<typename Scalar, int Options>
31
    struct call< MotionTpl<Scalar,Options> >
32
    {
33
      typedef MotionTpl<Scalar,Options> Motion;
34
35
      static bool isApprox(const Motion & self, const Motion & other,
36
                           const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
37
      {
38
        return self.isApprox(other,prec);
39
      }
40
41
      static bool isZero(const Motion & self,
42
                         const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
43
      {
44
        return self.isZero(prec);
45
      }
46
    };
47
48
38
    BOOST_PYTHON_FUNCTION_OVERLOADS(isApproxMotion_overload,call<Motion>::isApprox,2,3)
49
    BOOST_PYTHON_FUNCTION_OVERLOADS(isZero_overload,call<Motion>::isZero,1,2)
50
51
    template<typename Motion>
52
    struct MotionPythonVisitor
53
      : public boost::python::def_visitor< MotionPythonVisitor<Motion> >
54
    {
55
      enum { Options = traits<Motion>::Options };
56
57
      typedef typename Motion::Scalar Scalar;
58
      typedef ForceTpl<Scalar,traits<Motion>::Options> Force;
59
      typedef typename Motion::Vector6 Vector6;
60
      typedef typename Motion::Vector3 Vector3;
61
62
      typedef typename Eigen::Map<Vector3> MapVector3;
63
      typedef typename Eigen::Ref<Vector3> RefVector3;
64
65
    public:
66
67
      template<class PyClass>
68
19
      void visit(PyClass& cl) const
69
      {
70
        cl
71
        .def(bp::init<>(bp::arg("self"),"Default constructor"))
72

19
        .def(bp::init<Vector3,Vector3>
73
             (bp::args("self","linear","angular"),
74
              "Initialize from linear and angular components of a Motion vector (don't mix the order)."))
75

38
        .def(bp::init<Vector6>((bp::arg("self"),bp::arg("array")),"Init from a vector 6 [linear velocity, angular velocity]"))
76


38
        .def(bp::init<Motion>((bp::arg("self"),bp::arg("other")),"Copy constructor."))
77
78


38
        .add_property("linear",
79
                      bp::make_function(&MotionPythonVisitor::getLinear,
80
                                        bp::with_custodian_and_ward_postcall<0,1>()),
81
                      &MotionPythonVisitor::setLinear,
82
                      "Linear part of a *this, corresponding to the linear velocity in case of a Spatial velocity.")
83

38
        .add_property("angular",
84
                      bp::make_function(&MotionPythonVisitor::getAngular,
85
                                        bp::with_custodian_and_ward_postcall<0,1>()),
86
                      &MotionPythonVisitor::setAngular,
87
                      "Angular part of a *this, corresponding to the angular velocity in case of a Spatial velocity.")
88

38
        .add_property("vector",
89
                      bp::make_function((typename Motion::ToVectorReturnType (Motion::*)())&Motion::toVector,
90
                                         bp::return_internal_reference<>()),
91
                      &MotionPythonVisitor::setVector,
92
                      "Returns the components of *this as a 6d vector.")
93

38
        .add_property("np",
94
                      bp::make_function((typename Motion::ToVectorReturnType (Motion::*)())&Motion::toVector,
95
                                         bp::return_internal_reference<>()))
96
97

38
        .def("se3Action",&Motion::template se3Action<Scalar,Options>,
98
             bp::args("self","M"),"Returns the result of the action of M on *this.")
99
38
        .def("se3ActionInverse",&Motion::template se3ActionInverse<Scalar,Options>,
100
             bp::args("self","M"),"Returns the result of the action of the inverse of M on *this.")
101
102

38
        .add_property("action",&Motion::toActionMatrix,"Returns the action matrix of *this (acting on Motion).")
103
19
        .add_property("dualAction",&Motion::toDualActionMatrix,"Returns the dual action matrix of *this (acting on Force).")
104
19
        .add_property("homogeneous", &Motion::toHomogeneousMatrix, "Equivalent homogeneous representation of the Motion vector")
105
106
19
        .def("setZero",&MotionPythonVisitor::setZero,bp::arg("self"),
107
             "Set the linear and angular components of *this to zero.")
108

38
        .def("setRandom",&MotionPythonVisitor::setRandom,bp::arg("self"),
109
             "Set the linear and angular components of *this to random values.")
110
111

38
        .def("cross",(Motion (Motion::*)(const Motion &) const) &Motion::cross,
112
             bp::args("self","m"),"Action of *this onto another Motion m. Returns ยจ*this x m.")
113
38
        .def("cross",(Force (Motion::*)(const Force &) const) &Motion::cross,
114
             bp::args("self","f"),"Dual action of *this onto a Force f. Returns *this x* f.")
115
116

57
        .def(bp::self + bp::self)
117

38
        .def(bp::self += bp::self)
118
19
        .def(bp::self - bp::self)
119
19
        .def(bp::self -= bp::self)
120
19
        .def(-bp::self)
121
19
        .def(bp::self ^ bp::self)
122

19
        .def(bp::self ^ Force())
123
124
19
        .def(bp::self == bp::self)
125
19
        .def(bp::self != bp::self)
126
127
19
        .def(bp::self * Scalar())
128
19
        .def(Scalar() * bp::self)
129
19
        .def(bp::self / Scalar())
130
131
19
        .def("isApprox",
132
             call<Motion>::isApprox,
133
             isApproxMotion_overload(bp::args("self","other","prec"),
134
                                     "Returns true if *this is approximately equal to other, within the precision given by prec."))
135
136

38
        .def("isZero",
137
             call<Motion>::isZero,
138
             isZero_overload(bp::args("self","prec"),
139
                             "Returns true if *this is approximately equal to the zero Motion, within the precision given by prec."))
140
141

38
        .def("Random",&Motion::Random,"Returns a random Motion.")
142
19
        .staticmethod("Random")
143
19
        .def("Zero",&Motion::Zero,"Returns a zero Motion.")
144
19
        .staticmethod("Zero")
145
146
19
        .def("__array__",bp::make_function((typename Motion::ToVectorReturnType (Motion::*)())&Motion::toVector,
147
                                            bp::return_internal_reference<>()))
148
149

19
        .def_pickle(Pickle())
150
        ;
151
19
      }
152
153
19
      static void expose()
154
      {
155
#if PY_MAJOR_VERSION == 3 && PY_MINOR_VERSION == 6 && EIGENPY_VERSION_AT_LEAST(2,9,0)
156
    typedef PINOCCHIO_SHARED_PTR_HOLDER_TYPE(Motion) HolderType;
157
#else
158
    typedef ::boost::python::detail::not_specified HolderType;
159
#endif
160
        bp::class_<Motion,HolderType>("Motion",
161
                           "Motion vectors, in se3 == M^6.\n\n"
162
                           "Supported operations ...",
163
                           bp::no_init)
164
        .def(MotionPythonVisitor<Motion>())
165
19
        .def(CopyableVisitor<Motion>())
166

19
        .def(PrintableVisitor<Motion>())
167
        ;
168
19
      }
169
170
    private:
171
172
      struct Pickle : bp::pickle_suite
173
      {
174
        static
175
        boost::python::tuple
176
        getinitargs(const Motion & m)
177
        { return bp::make_tuple((Vector3)m.linear(),(Vector3)m.angular()); }
178
179
19
        static bool getstate_manages_dict() { return true; }
180
      };
181
182
      static RefVector3 getLinear(Motion & self) { return self.linear(); }
183
      static void setLinear (Motion & self, const Vector3 & v) { self.linear(v); }
184
      static RefVector3 getAngular(Motion & self) { return self.angular(); }
185
      static void setAngular(Motion & self, const Vector3 & w) { self.angular(w); }
186
187
      static void setVector(Motion & self, const Vector6 & v) { self = v; }
188
189
      static void setZero(Motion & self) { self.setZero(); }
190
      static void setRandom(Motion & self) { self.setRandom(); }
191
192
    };
193
194
  }} // namespace pinocchio::python
195
196
#endif // ifndef __pinocchio_python_spatial_motion_hpp__