GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/pinocchio/bindings/python/multibody/model.hpp Lines: 77 97 79.4 %
Date: 2023-08-09 08:43:58 Branches: 99 238 41.6 %

Line Branch Exec Source
1
//
2
// Copyright (c) 2015-2021 CNRS INRIA
3
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4
//
5
6
#ifndef __pinocchio_python_multibody_model_hpp__
7
#define __pinocchio_python_multibody_model_hpp__
8
9
#include <eigenpy/eigen-to-python.hpp>
10
11
#include "pinocchio/multibody/model.hpp"
12
#include "pinocchio/serialization/model.hpp"
13
14
#include <boost/python/overloads.hpp>
15
#include <eigenpy/memory.hpp>
16
#include <eigenpy/exception.hpp>
17
18
#include "pinocchio/algorithm/check.hpp"
19
#include "pinocchio/bindings/python/serialization/serializable.hpp"
20
#include "pinocchio/bindings/python/utils/printable.hpp"
21
#include "pinocchio/bindings/python/utils/copyable.hpp"
22
#include "pinocchio/bindings/python/utils/std-map.hpp"
23
#include "pinocchio/bindings/python/utils/pickle-map.hpp"
24
#include "pinocchio/bindings/python/utils/std-vector.hpp"
25
26
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(pinocchio::Model)
27
28
namespace pinocchio
29
{
30
  namespace python
31
  {
32
    namespace bp = boost::python;
33
34
50
    BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(getFrameId_overload,Model::getFrameId,1,2)
35
19
    BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(existFrame_overload,Model::existFrame,1,2)
36
19
    BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(addJointFrame_overload,Model::addJointFrame,1,2)
37
38
    template<typename Model>
39
    struct PickleModel : bp::pickle_suite
40
    {
41
      static bp::tuple getinitargs(const Model &)
42
      {
43
        return bp::make_tuple();
44
      }
45
46
      static bp::tuple getstate(const Model & model)
47
      {
48
        const std::string str(model.saveToString());
49
        return bp::make_tuple(bp::str(str));
50
      }
51
52
      static void setstate(Model & model, bp::tuple tup)
53
      {
54
        if(bp::len(tup) == 0 || bp::len(tup) > 1)
55
        {
56
          throw eigenpy::Exception("Pickle was not able to reconstruct the model from the loaded data.\n"
57
                                   "The pickle data structure contains too many elements.");
58
        }
59
60
        bp::object py_obj = tup[0];
61
        boost::python::extract<std::string> obj_as_string(py_obj.ptr());
62
        if(obj_as_string.check())
63
        {
64
          const std::string str = obj_as_string;
65
          model.loadFromString(str);
66
        }
67
        else
68
        {
69
          throw eigenpy::Exception("Pickle was not able to reconstruct the model from the loaded data.\n"
70
                                   "The entry is not a string.");
71
        }
72
73
      }
74
75
19
      static bool getstate_manages_dict() { return true; }
76
77
    };
78
79
    template<typename Model>
80
    struct ModelPythonVisitor
81
    : public bp::def_visitor< ModelPythonVisitor<Model> >
82
    {
83
    public:
84
      typedef typename Model::Scalar Scalar;
85
86
      typedef typename Model::Index Index;
87
      typedef typename Model::JointIndex JointIndex;
88
      typedef typename Model::FrameIndex FrameIndex;
89
      typedef typename Model::IndexVector IndexVector;
90
91
      typedef typename Model::SE3 SE3;
92
      typedef typename Model::Motion Motion;
93
      typedef typename Model::Force Force;
94
      typedef typename Model::Frame Frame;
95
      typedef typename Model::Inertia Inertia;
96
97
      typedef typename Model::Data Data;
98
99
      typedef typename Model::VectorXs VectorXs;
100
101
38
      BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(addFrame_overload,Model::addFrame,1,2)
102
103
    public:
104
105
      /* --- Exposing C++ API to python through the handler ----------------- */
106
      template<class PyClass>
107
19
      void visit(PyClass& cl) const
108
      {
109
19
        cl
110
        .def(bp::init<>(bp::arg("self"),
111
                        "Default constructor. Constructs an empty model."))
112
113
        // Class Members
114

19
        .add_property("nq", &Model::nq)
115
19
        .add_property("nv", &Model::nv)
116
19
        .add_property("njoints", &Model::njoints)
117
19
        .add_property("nbodies", &Model::nbodies)
118
19
        .add_property("nframes", &Model::nframes)
119
19
        .add_property("inertias",&Model::inertias)
120
19
        .add_property("jointPlacements",&Model::jointPlacements)
121
19
        .add_property("joints",&Model::joints)
122
19
        .add_property("idx_qs",&Model::idx_qs)
123
19
        .add_property("nqs",&Model::nqs)
124
19
        .add_property("idx_vs",&Model::idx_vs)
125
19
        .add_property("nvs",&Model::nvs)
126
19
        .add_property("parents",&Model::parents)
127
19
        .add_property("names",&Model::names)
128
19
        .def_readwrite("name",&Model::name)
129
19
        .def_readwrite("referenceConfigurations", &Model::referenceConfigurations)
130
131
19
        .def_readwrite("rotorInertia",&Model::rotorInertia,
132
                       "Vector of rotor inertia parameters.")
133
19
        .def_readwrite("rotorGearRatio",&Model::rotorGearRatio,
134
                       "Vector of rotor gear ratio parameters.")
135
19
        .def_readwrite("friction",&Model::friction,
136
                       "Vector of joint friction parameters.")
137
19
        .def_readwrite("damping",&Model::damping,
138
                       "Vector of joint damping parameters.")
139
19
        .def_readwrite("effortLimit",&Model::effortLimit,
140
                       "Joint max effort.")
141
19
        .def_readwrite("velocityLimit",&Model::velocityLimit,
142
                       "Joint max velocity.")
143
19
        .def_readwrite("lowerPositionLimit",&Model::lowerPositionLimit,
144
                       "Limit for joint lower position.")
145
19
        .def_readwrite("upperPositionLimit",&Model::upperPositionLimit,
146
                       "Limit for joint upper position.")
147
148
19
        .def_readwrite("frames",&Model::frames,
149
                       "Vector of frames contained in the model.")
150
151
19
        .def_readwrite("supports",
152
                       &Model::supports,
153
                       "Vector of supports. supports[j] corresponds to the list of joints on the path between\n"
154
                       "the current *j* to the root of the kinematic tree.")
155
156
19
        .def_readwrite("subtrees",
157
                       &Model::subtrees,
158
                       "Vector of subtrees. subtree[j] corresponds to the subtree supported by the joint j.")
159
160
19
        .def_readwrite("gravity",&Model::gravity,
161
                       "Motion vector corresponding to the gravity field expressed in the world Frame.")
162
163
        // Class Methods
164
19
        .def("addJoint",&ModelPythonVisitor::addJoint0,
165
             bp::args("self","parent_id","joint_model","joint_placement","joint_name"),
166
             "Adds a joint to the kinematic tree. The joint is defined by its placement relative to its parent joint and its name.")
167

38
        .def("addJoint",&ModelPythonVisitor::addJoint1,
168
             bp::args("self","parent_id","joint_model","joint_placement","joint_name",
169
                      "max_effort","max_velocity","min_config","max_config"),
170
             "Adds a joint to the kinematic tree with given bounds. The joint is defined by its placement relative to its parent joint and its name."
171
             "This signature also takes as input effort, velocity limits as well as the bounds on the joint configuration.")
172

38
        .def("addJoint",&ModelPythonVisitor::addJoint2,
173
             bp::args("self","parent_id","joint_model","joint_placement","joint_name",
174
                      "max_effort","max_velocity","min_config","max_config",
175
                      "friction","damping"),
176
             "Adds a joint to the kinematic tree with given bounds. The joint is defined by its placement relative to its parent joint and its name.\n"
177
             "This signature also takes as input effort, velocity limits as well as the bounds on the joint configuration.\n"
178
             "The user should also provide the friction and damping related to the joint.")
179

38
        .def("addJointFrame", &Model::addJointFrame,
180
             addJointFrame_overload(bp::args("self","joint_id", "frame_id"),
181
                                    "Add the joint provided by its joint_id as a frame to the frame tree.\n"
182
                                    "The frame_id may be optionally provided."))
183

38
        .def("appendBodyToJoint",&Model::appendBodyToJoint,
184
             bp::args("self","joint_id","body_inertia","body_placement"),
185
             "Appends a body to the joint given by its index. The body is defined by its inertia, its relative placement regarding to the joint and its name.")
186
187

38
        .def("addBodyFrame", &Model::addBodyFrame, bp::args("self","body_name", "parentJoint", "body_placement", "previous_frame"), "add a body to the frame tree")
188

38
        .def("getBodyId",&Model::getBodyId, bp::args("self","name"), "Return the index of a frame of type BODY given by its name")
189

38
        .def("existBodyName", &Model::existBodyName, bp::args("self","name"), "Check if a frame of type BODY exists, given its name")
190

38
        .def("getJointId",&Model::getJointId, bp::args("self","name"), "Return the index of a joint given by its name")
191

38
        .def("existJointName", &Model::existJointName, bp::args("self","name"), "Check if a joint given by its name exists")
192
193

38
        .def("getFrameId",&Model::getFrameId,getFrameId_overload(bp::args("self","name","type"),"Returns the index of the frame given by its name and its type. If the frame is not in the frames vector, it returns the current size of the frames vector."))
194
195

38
        .def("existFrame",&Model::existFrame,existFrame_overload(bp::args("self","name","type"),"Returns true if the frame given by its name exists inside the Model with the given type."))
196
197

38
        .def("addFrame",&Model::addFrame,
198

19
             addFrame_overload((bp::arg("self"), bp::arg("frame"), bp::arg("append_inertia") = true),
199
                               "Add a frame to the vector of frames. If append_inertia set to True, "
200
                               "the inertia value contained in frame will be added to the inertia supported by the parent joint."))
201
202



57
        .def("createData",
203
             &ModelPythonVisitor::createData,bp::arg("self"),
204
             "Create a Data object for the given model.")
205
206

38
        .def("check",(bool (Model::*)(const Data &) const) &Model::check,bp::args("self","data"),
207
             "Check consistency of data wrt model.")
208

38
        .def("hasConfigurationLimit",&Model::hasConfigurationLimit, bp::args("self"), "Returns list of boolean if joints have configuration limit.")
209
38
        .def("hasConfigurationLimitInTangent",&Model::hasConfigurationLimitInTangent, bp::args("self"), "Returns list of boolean if joints have configuration limit in tangent space  .")
210
211

57
        .def(bp::self == bp::self)
212

57
        .def(bp::self != bp::self)
213
        ;
214
19
      }
215
216
2
      static JointIndex addJoint0(Model & model,
217
                                  JointIndex parent_id,
218
                                  const JointModel & jmodel,
219
                                  const SE3 & joint_placement,
220
                                  const std::string & joint_name)
221
      {
222
2
        return model.addJoint(parent_id,jmodel,joint_placement,joint_name);
223
      }
224
225
      static JointIndex addJoint1(Model & model,
226
                                  JointIndex parent_id,
227
                                  const JointModel & jmodel,
228
                                  const SE3 & joint_placement,
229
                                  const std::string & joint_name,
230
                                  const VectorXs & max_effort,
231
                                  const VectorXs & max_velocity,
232
                                  const VectorXs & min_config,
233
                                  const VectorXs & max_config)
234
      {
235
        return model.addJoint(parent_id,jmodel,joint_placement,joint_name,
236
                              max_effort,max_velocity,min_config,max_config);
237
      }
238
239
      static JointIndex addJoint2(Model & model,
240
                                  JointIndex parent_id,
241
                                  const JointModel & jmodel,
242
                                  const SE3 & joint_placement,
243
                                  const std::string & joint_name,
244
                                  const VectorXs & max_effort,
245
                                  const VectorXs & max_velocity,
246
                                  const VectorXs & min_config,
247
                                  const VectorXs & max_config,
248
                                  const VectorXs & friction,
249
                                  const VectorXs & damping)
250
      {
251
        return model.addJoint(parent_id,jmodel,joint_placement,joint_name,
252
                              max_effort,max_velocity,min_config,max_config,
253
                              friction,damping);
254
      }
255
256
21
      static Data createData(const Model & model) { return Data(model); }
257
258
      ///
259
      /// \brief Provide equivalent to python list index function for
260
      ///        vectors.
261
      ///
262
      /// \param[in] x The input vector.
263
      /// \param[in] v The value of to look for in the vector.
264
      ///
265
      /// \return The index of the matching element of the vector. If
266
      ///         no element is found, return the size of the vector.
267
      ///
268
      template<typename T>
269
      static Index index(std::vector<T> const& x,
270
                                typename std::vector<T>::value_type const& v)
271
      {
272
        Index i = 0;
273
        for(typename std::vector<T>::const_iterator it = x.begin(); it != x.end(); ++it, ++i)
274
        {
275
          if(*it == v)
276
          {
277
            return i;
278
          }
279
        }
280
        return x.size();
281
      }
282
283
      /* --- Expose --------------------------------------------------------- */
284
19
      static void expose()
285
      {
286
        typedef typename Model::ConfigVectorMap ConfigVectorMap;
287
        typedef bp::map_indexing_suite<ConfigVectorMap,false> map_indexing_suite;
288

19
        StdVectorPythonVisitor<Index>::expose("StdVec_Index");
289
19
        serialize< std::vector<Index> >();
290

19
        StdVectorPythonVisitor<IndexVector>::expose("StdVec_IndexVector");
291
19
        serialize< std::vector<IndexVector> >();
292

19
        StdVectorPythonVisitor<std::string>::expose("StdVec_StdString");
293
19
        serialize< std::vector<std::string> >();
294

19
        StdVectorPythonVisitor<bool>::expose("StdVec_Bool");
295
19
        serialize< std::vector<bool> >();
296

19
        StdVectorPythonVisitor<Scalar>::expose("StdVec_Double");
297
19
        serialize< std::vector<Scalar> >();
298
        bp::class_<typename Model::ConfigVectorMap>("StdMap_String_VectorXd")
299
          .def(map_indexing_suite())
300
19
          .def_pickle(PickleMap<typename Model::ConfigVectorMap>())
301

19
          .def(details::overload_base_get_item_for_std_map<typename Model::ConfigVectorMap>());
302
303
        bp::class_<Model>("Model",
304
                          "Articulated Rigid Body model",
305
                          bp::no_init)
306
        .def(ModelPythonVisitor())
307
19
        .def(SerializableVisitor<Model>())
308
19
        .def(PrintableVisitor<Model>())
309
19
        .def(CopyableVisitor<Model>())
310

19
        .def_pickle(PickleModel<Model>())
311
        ;
312
19
      }
313
    };
314
315
  }} // namespace pinocchio::python
316
317
#endif // ifndef __pinocchio_python_multibody_model_hpp__