inertia.hpp 10.4 KB
Newer Older
1
//
2
// Copyright (c) 2015-2020 CNRS INRIA
3
// Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4
5
//

6
7
#ifndef __pinocchio_python_spatial_inertia_hpp__
#define __pinocchio_python_spatial_inertia_hpp__
8
9

#include <eigenpy/exception.hpp>
10
#include <eigenpy/memory.hpp>
11
#include <eigenpy/eigen-to-python.hpp>
12
#include <boost/python/tuple.hpp>
13

14
#include "pinocchio/spatial/inertia.hpp"
15
#include "pinocchio/bindings/python/utils/copyable.hpp"
jcarpent's avatar
jcarpent committed
16
17
#include "pinocchio/bindings/python/utils/printable.hpp"

18
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(pinocchio::Inertia)
19

20
namespace pinocchio
21
22
23
24
{
  namespace python
  {
    namespace bp = boost::python;
25
      
26
    template<typename T> struct call;
27
      
28
29
30
31
32
33
34
    template<typename Scalar, int Options>
    struct call< InertiaTpl<Scalar,Options> >
    {
      typedef InertiaTpl<Scalar,Options> Inertia;
      
      static bool isApprox(const Inertia & self, const Inertia & other,
                           const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
35
      {
36
37
38
39
40
41
42
43
44
        return self.isApprox(other,prec);
      }
      
      static bool isZero(const Inertia & self,
                         const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
      {
        return self.isZero(prec);
      }
    };
45
    
46
47
    BOOST_PYTHON_FUNCTION_OVERLOADS(isApproxInertia_overload,call<Inertia>::isApprox,2,3)
    BOOST_PYTHON_FUNCTION_OVERLOADS(isZero_overload,call<Inertia>::isZero,1,2)
48
49
50
51
52

    template<typename Inertia>
    struct InertiaPythonVisitor
      : public boost::python::def_visitor< InertiaPythonVisitor<Inertia> >
    {
jcarpent's avatar
jcarpent committed
53
54
55
      
      typedef typename Inertia::Scalar Scalar;
      typedef typename Inertia::Vector3 Vector3;
56
57
      typedef typename Inertia::Matrix3 Matrix3;
      typedef typename Inertia::Vector6 Vector6;
jcarpent's avatar
jcarpent committed
58
      typedef typename Inertia::Matrix6 Matrix6;
59
60
61
62
63
64

    public:

      template<class PyClass>
      void visit(PyClass& cl) const 
      {
65
66
67
68
69
70
        cl
        .def("__init__",
             bp::make_constructor(&InertiaPythonVisitor::makeFromMCI,
                                  bp::default_call_policies(),
                                  (bp::arg("mass"),bp::arg("lever"),bp::arg("inertia"))),
             "Initialize from mass, lever and 3d inertia.")
Justin Carpentier's avatar
Justin Carpentier committed
71
        .def(bp::init<Inertia>((bp::arg("self"),bp::arg("other")),"Copy constructor."))
72
        
jcarpent's avatar
jcarpent committed
73
74
75
76
77
        .add_property("mass",
                      &InertiaPythonVisitor::getMass,
                      &InertiaPythonVisitor::setMass,
                      "Mass of the Spatial Inertia.")
        .add_property("lever",
78
79
                      bp::make_function((typename Inertia::Vector3 & (Inertia::*)())&Inertia::lever,
                                        bp::return_internal_reference<>()),
jcarpent's avatar
jcarpent committed
80
81
82
83
84
85
                      &InertiaPythonVisitor::setLever,
                      "Center of mass location of the Spatial Inertia. It corresponds to the location of the center of mass regarding to the frame where the Spatial Inertia is expressed.")
        .add_property("inertia",
                      &InertiaPythonVisitor::getInertia,
                      &InertiaPythonVisitor::setInertia,
                      "Rotational part of the Spatial Inertia, i.e. a symmetric matrix representing the rotational inertia around the center of mass.")
86
        
Justin Carpentier's avatar
Justin Carpentier committed
87
        .def("matrix",&Inertia::matrix,bp::arg("self"))
jcarpent's avatar
jcarpent committed
88
        .def("se3Action",&Inertia::se3Action,
Justin Carpentier's avatar
Justin Carpentier committed
89
             bp::args("self","M"),"Returns the result of the action of M on *this.")
jcarpent's avatar
jcarpent committed
90
        .def("se3ActionInverse",&Inertia::se3ActionInverse,
Justin Carpentier's avatar
Justin Carpentier committed
91
             bp::args("self","M"),"Returns the result of the action of the inverse of M on *this.")
92
        
Justin Carpentier's avatar
Justin Carpentier committed
93
94
95
96
97
98
        .def("setIdentity",&Inertia::setIdentity,bp::arg("self"),
             "Set *this to be the Identity inertia.")
        .def("setZero",&Inertia::setZero,bp::arg("self"),
             "Set all the components of *this to zero.")
        .def("setRandom",&Inertia::setRandom,bp::arg("self"),
             "Set all the components of *this to random values.")
99
        
jcarpent's avatar
jcarpent committed
100
101
        .def(bp::self + bp::self)
        .def(bp::self * bp::other<Motion>() )
102
        .add_property("np",&Inertia::matrix)
Justin Carpentier's avatar
Justin Carpentier committed
103
104
105
106
107
108
109
110
111
112
113
        .def("vxiv",&Inertia::vxiv,bp::args("self","v"),"Returns the result of v x Iv.")
        .def("vtiv",&Inertia::vtiv,bp::args("self","v"),"Returns the result of v.T * Iv.")
        .def("vxi",(Matrix6 (Inertia::*)(const Motion &) const)&Inertia::vxi,
             bp::args("self","v"),
             "Returns the result of v x* I, a 6x6 matrix.")
        .def("ivx",(Matrix6 (Inertia::*)(const Motion &) const)&Inertia::ivx,
             bp::args("self","v"),
             "Returns the result of I vx, a 6x6 matrix.")
        .def("variation",(Matrix6 (Inertia::*)(const Motion &) const)&Inertia::variation,
             bp::args("self","v"),
             "Returns the time derivative of the inertia.")
114
        
115
116
117
        .def(bp::self == bp::self)
        .def(bp::self != bp::self)
        
118
        .def("isApprox",
119
             call<Inertia>::isApprox,
Justin Carpentier's avatar
Justin Carpentier committed
120
             isApproxInertia_overload(bp::args("self","other","prec"),
121
                                      "Returns true if *this is approximately equal to other, within the precision given by prec."))
122
123
124
                                                                                                                         
        .def("isZero",
             call<Inertia>::isZero,
Justin Carpentier's avatar
Justin Carpentier committed
125
             isZero_overload(bp::args("self","prec"),
126
                             "Returns true if *this is approximately equal to the zero Inertia, within the precision given by prec."))
127
        
jcarpent's avatar
jcarpent committed
128
        .def("Identity",&Inertia::Identity,"Returns the identity Inertia.")
129
        .staticmethod("Identity")
jcarpent's avatar
jcarpent committed
130
        .def("Zero",&Inertia::Zero,"Returns the null Inertia.")
131
        .staticmethod("Zero")
jcarpent's avatar
jcarpent committed
132
        .def("Random",&Inertia::Random,"Returns a random Inertia.")
133
        .staticmethod("Random")
jcarpent's avatar
jcarpent committed
134
        
Justin Carpentier's avatar
Justin Carpentier committed
135
        .def("toDynamicParameters",&InertiaPythonVisitor::toDynamicParameters_proxy,bp::arg("self"),
136
137
138
139
140
             "Returns the representation of the matrix as a vector of dynamic parameters."
              "\nThe parameters are given as v = [m, mc_x, mc_y, mc_z, I_{xx}, I_{xy}, I_{yy}, I_{xz}, I_{yz}, I_{zz}]^T "
              "where I = I_C + mS^T(c)S(c) and I_C has its origin at the barycenter"
        )
        .def("FromDynamicParameters",&Inertia::template FromDynamicParameters<Eigen::VectorXd>,
Justin Carpentier's avatar
Justin Carpentier committed
141
              bp::args("dynamic_parameters"),
142
              "Builds and inertia matrix from a vector of dynamic parameters."
Justin Carpentier's avatar
Justin Carpentier committed
143
144
              "\nThe parameters are given as dynamic_parameters = [m, mc_x, mc_y, mc_z, I_{xx}, I_{xy}, I_{yy}, I_{xz}, I_{yz}, I_{zz}]^T "
              "where I = I_C + mS^T(c)S(c) and I_C has its origin at the barycenter."
145
146
147
        )
        .staticmethod("FromDynamicParameters")

148
149
        .def("FromSphere", &Inertia::FromSphere,
             bp::args("mass","radius"),
Justin Carpentier's avatar
Justin Carpentier committed
150
             "Returns the Inertia of a sphere defined by a given mass and radius.")
151
        .staticmethod("FromSphere")
152
        .def("FromEllipsoid", &Inertia::FromEllipsoid,
jcarpent's avatar
jcarpent committed
153
             bp::args("mass","length_x","length_y","length_z"),
Justin Carpentier's avatar
Justin Carpentier committed
154
             "Returns the Inertia of an ellipsoid shape defined by a mass and given dimensions the semi-axis of values length_{x,y,z}.")
155
        .staticmethod("FromEllipsoid")
156
        .def("FromCylinder", &Inertia::FromCylinder,
jcarpent's avatar
jcarpent committed
157
             bp::args("mass","radius","length"),
Justin Carpentier's avatar
Justin Carpentier committed
158
             "Returns the Inertia of a cylinder defined by its mass, radius and length along the Z axis.")
159
        .staticmethod("FromCylinder")
160
        .def("FromBox", &Inertia::FromBox,
jcarpent's avatar
jcarpent committed
161
             bp::args("mass","length_x","length_y","length_z"),
Justin Carpentier's avatar
Justin Carpentier committed
162
             "Returns the Inertia of a box shape with a mass and of dimension the semi axis of length_{x,y,z}.")
163
        .staticmethod("FromBox")
164
        
165
166
        .def("__array__",&Inertia::matrix)
        
167
        .def_pickle(Pickle())
168
        ;
jcarpent's avatar
jcarpent committed
169
      }
170
      
171
172
      static Scalar getMass( const Inertia & self ) { return self.mass(); }
      static void setMass( Inertia & self, Scalar mass ) { self.mass() = mass; }
173
      
174
      static void setLever( Inertia & self, const Vector3 & lever ) { self.lever() = lever; }
175
      
176
      static Matrix3 getInertia( const Inertia & self ) { return self.inertia().matrix(); }
177
178
179
180
181
182
183
184
185
186
187
188
//      static void setInertia(Inertia & self, const Vector6 & minimal_inertia) { self.inertia().data() = minimal_inertia; }
      static void setInertia(Inertia & self, const Matrix3 & symmetric_inertia)
      {
        assert(symmetric_inertia.isApprox(symmetric_inertia.transpose()));
        self.inertia().data() <<
        symmetric_inertia(0,0),
        symmetric_inertia(1,0),
        symmetric_inertia(1,1),
        symmetric_inertia(0,2),
        symmetric_inertia(1,2),
        symmetric_inertia(2,2);
      }
189

190
191
192
193
      static Eigen::VectorXd toDynamicParameters_proxy(const Inertia & self)
      {
        return self.toDynamicParameters();
      }
194

195
      static Inertia* makeFromMCI(const double & mass,
jcarpent's avatar
jcarpent committed
196
197
                                  const Vector3 & lever,
                                  const Matrix3 & inertia) 
198
      {
199
200
201
202
203
204
        if(! inertia.isApprox(inertia.transpose()) )
          throw eigenpy::Exception("The 3d inertia should be symmetric.");
        if( (Eigen::Vector3d::UnitX().transpose()*inertia*Eigen::Vector3d::UnitX()<0)
           || (Eigen::Vector3d::UnitY().transpose()*inertia*Eigen::Vector3d::UnitY()<0)
           || (Eigen::Vector3d::UnitZ().transpose()*inertia*Eigen::Vector3d::UnitZ()<0) )
          throw eigenpy::Exception("The 3d inertia should be positive.");
205
        return new Inertia(mass,lever,inertia);
jcarpent's avatar
jcarpent committed
206
           }
207
      
208
209
      static void expose()
      {
210
        bp::class_<Inertia>("Inertia",
jcarpent's avatar
jcarpent committed
211
212
                            "This class represenses a sparse version of a Spatial Inertia and its is defined by its mass, its center of mass location and the rotational inertia expressed around this center of mass.\n\n"
                            "Supported operations ...",
Justin Carpentier's avatar
Justin Carpentier committed
213
                            bp::init<>(bp::arg("self"),"Default constructor."))
214
        .def(InertiaPythonVisitor<Inertia>())
215
        .def(CopyableVisitor<Inertia>())
jcarpent's avatar
jcarpent committed
216
        .def(PrintableVisitor<Inertia>())
217
218
219
        ;
        
      }
220
221
222
223
224
225
226
227
228
229
230
      
    private:
      
      struct Pickle : bp::pickle_suite
      {
        static
        boost::python::tuple
        getinitargs(const Inertia & I)
        { return bp::make_tuple(I.mass(),(Vector3)I.lever(),I.inertia().matrix()); }
      };
      
231

232
    }; // struct InertiaPythonVisitor
233
    
jcarpent's avatar
jcarpent committed
234
  } // namespace python
235
} // namespace pinocchio
236

237
#endif // ifndef __pinocchio_python_spatial_inertia_hpp__