Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/pinocchio/bindings/python/multibody/data.hpp	Lines:	76	90	84.4 %
Date:	2024-04-26 13:14:21	Branches:	76	190	40.0 %


//
// Copyright (c) 2015-2022 CNRS INRIA
//

#ifndef __pinocchio_python_multibody_data_hpp__
#define __pinocchio_python_multibody_data_hpp__

#include <boost/python.hpp>

#include "pinocchio/multibody/data.hpp"
#include "pinocchio/serialization/data.hpp"

#include <eigenpy/memory.hpp>
#include <eigenpy/eigen-to-python.hpp>
#include <eigenpy/exception.hpp>

#include "pinocchio/bindings/python/serialization/serializable.hpp"
#include "pinocchio/bindings/python/utils/std-vector.hpp"
#include "pinocchio/bindings/python/utils/std-aligned-vector.hpp"

#include "pinocchio/bindings/python/utils/copyable.hpp"

#if EIGENPY_VERSION_AT_MOST(2,8,1)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(pinocchio::Data)
#endif

namespace pinocchio
{
  namespace python
  {
    namespace bp = boost::python;

    template<typename Data>
    struct PickleData : bp::pickle_suite
    {
      static bp::tuple getinitargs(const Data &)
      {
        return bp::make_tuple();
      }

      static bp::tuple getstate(const Data & data)
      {
        const std::string str(data.saveToString());
        return bp::make_tuple(bp::str(str));
      }

      static void setstate(Data & data, bp::tuple tup)
      {
        if(bp::len(tup) == 0 || bp::len(tup) > 1)
        {
          throw eigenpy::Exception("Pickle was not able to reconstruct the model from the loaded data.\n"
                                   "The pickle data structure contains too many elements.");
        }

        bp::object py_obj = tup[0];
        boost::python::extract<std::string> obj_as_string(py_obj.ptr());
        if(obj_as_string.check())
        {
          const std::string str = obj_as_string;
          data.loadFromString(str);
        }
        else
        {
          throw eigenpy::Exception("Pickle was not able to reconstruct the model from the loaded data.\n"
                                   "The entry is not a string.");
        }

      }

      static bool getstate_manages_dict() { return true; }
    };

    struct DataPythonVisitor
      : public boost::python::def_visitor< DataPythonVisitor >
    {
      typedef Data::Matrix6x Matrix6x;
      typedef Data::Matrix3x Matrix3x;
      typedef Data::Vector3 Vector3;

    public:

#define ADD_DATA_PROPERTY(NAME,DOC)         \
      def_readwrite(#NAME,                  \
      &Data::NAME,                          \
      DOC)

#define ADD_DATA_PROPERTY_READONLY(NAME,DOC)      \
      def_readonly(#NAME,                         \
      &Data::NAME,                                \
      DOC)

#define ADD_DATA_PROPERTY_READONLY_BYVALUE(NAME,DOC)                            \
      add_property(#NAME,                                                       \
      make_getter(&Data::NAME,bp::return_value_policy<bp::return_by_value>()),  \
      DOC)


      /* --- Exposing C++ API to python through the handler ----------------- */
      template<class PyClass>
      void visit(PyClass& cl) const
      {
        cl
        .def(bp::init<>(bp::arg("self"),"Default constructor."))
        .def(bp::init<Model>(bp::arg("model"),"Constructs a data structure from a given model."))

        .ADD_DATA_PROPERTY(a,"Joint spatial acceleration")
        .ADD_DATA_PROPERTY(oa,
                           "Joint spatial acceleration expressed at the origin of the world frame.")
        .ADD_DATA_PROPERTY(a_gf,
                           "Joint spatial acceleration containing also the contribution of the gravity acceleration")
        .ADD_DATA_PROPERTY(oa_gf,"Joint spatial acceleration containing also the contribution of the gravity acceleration, but expressed at the origin of the world frame.")

        .ADD_DATA_PROPERTY(v,"Joint spatial velocity expressed in the joint frame.")
        .ADD_DATA_PROPERTY(ov,"Joint spatial velocity expressed at the origin of the world frame.")

        .ADD_DATA_PROPERTY(f,"Joint spatial force expresssed in the joint frame.")
        .ADD_DATA_PROPERTY(of,"Joint spatial force expresssed at the origin of the world frame.")
        .ADD_DATA_PROPERTY(h,"Vector of spatial momenta expressed in the local frame of the joint.")
        .ADD_DATA_PROPERTY(oMi,"Body absolute placement (wrt world)")
        .ADD_DATA_PROPERTY(oMf,"frames absolute placement (wrt world)")
        .ADD_DATA_PROPERTY(liMi,"Body relative placement (wrt parent)")
        .ADD_DATA_PROPERTY(tau,"Joint torques (output of RNEA)")
        .ADD_DATA_PROPERTY(nle,"Non Linear Effects (output of nle algorithm)")
        .ADD_DATA_PROPERTY(ddq,"Joint accelerations (output of ABA)")
        .ADD_DATA_PROPERTY(Ycrb,"Inertia of the sub-tree composit rigid body")
        .ADD_DATA_PROPERTY(M,"The joint space inertia matrix")
        .ADD_DATA_PROPERTY(Minv,"The inverse of the joint space inertia matrix")
        .ADD_DATA_PROPERTY(C,"The Coriolis C(q,v) matrix such that the Coriolis effects are given by c(q,v) = C(q,v)v")
        .ADD_DATA_PROPERTY(g,"Vector of generalized gravity (dim model.nv).")
        .ADD_DATA_PROPERTY(Fcrb,"Spatial forces set, used in CRBA")
        .ADD_DATA_PROPERTY(lastChild,"Index of the last child (for CRBA)")
        .ADD_DATA_PROPERTY(nvSubtree,"Dimension of the subtree motion space (for CRBA)")
        .ADD_DATA_PROPERTY(U,"Joint Inertia square root (upper triangle)")
        .ADD_DATA_PROPERTY(D,"Diagonal of UDUT inertia decomposition")
        .ADD_DATA_PROPERTY(parents_fromRow,"First previous non-zero row in M (used in Cholesky)")
        .ADD_DATA_PROPERTY(nvSubtree_fromRow,"Subtree of the current row index (used in Cholesky)")
        .ADD_DATA_PROPERTY(J,"Jacobian of joint placement")
        .ADD_DATA_PROPERTY(dJ,"Time variation of the Jacobian of joint placement (data.J).")
        .ADD_DATA_PROPERTY(iMf,"Body placement wrt to algorithm end effector.")

        .ADD_DATA_PROPERTY(Ivx,"Right variation of the inertia matrix.")
        .ADD_DATA_PROPERTY(vxI,"Left variation of the inertia matrix.")
        .ADD_DATA_PROPERTY(B,"Combined variations of the inertia matrix consistent with Christoffel symbols.")

        .ADD_DATA_PROPERTY(Ag,
                           "Centroidal matrix which maps from joint velocity to the centroidal momentum.")
        .ADD_DATA_PROPERTY(dAg,
                           "Time derivative of the centroidal momentum matrix Ag.")
        .ADD_DATA_PROPERTY(hg,
                           "Centroidal momentum (expressed in the frame centered at the CoM and aligned with the world frame).")
        .ADD_DATA_PROPERTY(dhg,
                           "Centroidal momentum time derivative (expressed in the frame centered at the CoM and aligned with the world frame).")
        .ADD_DATA_PROPERTY(Ig,
                           "Centroidal Composite Rigid Body Inertia.")

        .ADD_DATA_PROPERTY(com,"CoM position of the subtree starting at joint index i.")
        .ADD_DATA_PROPERTY(vcom,"CoM velocity of the subtree starting at joint index i.")
        .ADD_DATA_PROPERTY(acom,"CoM acceleration of the subtree starting at joint index i.")
        .ADD_DATA_PROPERTY(mass,"Mass of the subtree starting at joint index i.")
        .ADD_DATA_PROPERTY(Jcom,"Jacobian of center of mass.")

        .ADD_DATA_PROPERTY(dtau_dq,"Partial derivative of the joint torque vector with respect to the joint configuration.")
        .ADD_DATA_PROPERTY(dtau_dv,"Partial derivative of the joint torque vector with respect to the joint velocity.")
        .ADD_DATA_PROPERTY(ddq_dq,"Partial derivative of the joint acceleration vector with respect to the joint configuration.")
        .ADD_DATA_PROPERTY(ddq_dv,"Partial derivative of the joint acceleration vector with respect to the joint velocity.")

        .ADD_DATA_PROPERTY(kinetic_energy,"Kinetic energy in [J] computed by computeKineticEnergy")
        .ADD_DATA_PROPERTY(potential_energy,"Potential energy in [J] computed by computePotentialEnergy")

        .ADD_DATA_PROPERTY(lambda_c,"Lagrange Multipliers linked to contact forces")
        .ADD_DATA_PROPERTY(impulse_c,"Lagrange Multipliers linked to contact impulses")

        .ADD_DATA_PROPERTY(dq_after,"Generalized velocity after the impact.")
        .ADD_DATA_PROPERTY(staticRegressor,"Static regressor.")
        .ADD_DATA_PROPERTY(jointTorqueRegressor,"Joint torque regressor.")

        .def(bp::self == bp::self)
        .def(bp::self != bp::self)
        ;
      }

      /* --- Expose --------------------------------------------------------- */
      static void expose()
      {
        bp::class_<Data>("Data",
                         "Articulated rigid body data related to a Model.\n"
                         "It contains all the data that can be modified by the Pinocchio algorithms.",
                         bp::no_init)
        .def(DataPythonVisitor())
        .def(CopyableVisitor<Data>())
        .def(SerializableVisitor<Data>())
        .def_pickle(PickleData<Data>());

        typedef PINOCCHIO_ALIGNED_STD_VECTOR(Vector3) StdVec_Vector3;
        typedef PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6x) StdVec_Matrix6x;

        StdAlignedVectorPythonVisitor<Vector3,false>::expose("StdVec_Vector3")
        .def(details::overload_base_get_item_for_std_vector<StdVec_Vector3>());
        serialize<StdAlignedVectorPythonVisitor<Vector3,false>::vector_type>();

        StdAlignedVectorPythonVisitor<Matrix6x,false>::expose("StdVec_Matrix6x")
        .def(details::overload_base_get_item_for_std_vector<StdVec_Matrix6x>());
        serialize<StdAlignedVectorPythonVisitor<Matrix6x,false>::vector_type>();

        StdVectorPythonVisitor<int>::expose("StdVec_Int");
        serialize<StdVectorPythonVisitor<int>::vector_type>();
      }

    };

  }} // namespace pinocchio::python

#endif // ifndef __pinocchio_python_multibody_data_hpp__


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2015-2022 CNRS INRIA
3			//
4
5			#ifndef __pinocchio_python_multibody_data_hpp__
6			#define __pinocchio_python_multibody_data_hpp__
7
8			#include <boost/python.hpp>
9
10			#include "pinocchio/multibody/data.hpp"
11			#include "pinocchio/serialization/data.hpp"
12
13			#include <eigenpy/memory.hpp>
14			#include <eigenpy/eigen-to-python.hpp>
15			#include <eigenpy/exception.hpp>
16
17			#include "pinocchio/bindings/python/serialization/serializable.hpp"
18			#include "pinocchio/bindings/python/utils/std-vector.hpp"
19			#include "pinocchio/bindings/python/utils/std-aligned-vector.hpp"
20
21			#include "pinocchio/bindings/python/utils/copyable.hpp"
22
23			#if EIGENPY_VERSION_AT_MOST(2,8,1)
24			EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(pinocchio::Data)
25			#endif
26
27			namespace pinocchio
28			{
29			namespace python
30			{
31			namespace bp = boost::python;
32
33			template<typename Data>
34			struct PickleData : bp::pickle_suite
35			{
36			static bp::tuple getinitargs(const Data &)
37			{
38			return bp::make_tuple();
39			}
40
41			static bp::tuple getstate(const Data & data)
42			{
43			const std::string str(data.saveToString());
44			return bp::make_tuple(bp::str(str));
45			}
46
47			static void setstate(Data & data, bp::tuple tup)
48			{
49			if(bp::len(tup) == 0 \|\| bp::len(tup) > 1)
50			{
51			throw eigenpy::Exception("Pickle was not able to reconstruct the model from the loaded data.\n"
52			"The pickle data structure contains too many elements.");
53			}
54
55			bp::object py_obj = tup[0];
56			boost::python::extract<std::string> obj_as_string(py_obj.ptr());
57			if(obj_as_string.check())
58			{
59			const std::string str = obj_as_string;
60			data.loadFromString(str);
61			}
62			else
63			{
64			throw eigenpy::Exception("Pickle was not able to reconstruct the model from the loaded data.\n"
65			"The entry is not a string.");
66			}
67
68			}
69
70		19	static bool getstate_manages_dict() { return true; }
71			};
72
73			struct DataPythonVisitor
74			: public boost::python::def_visitor< DataPythonVisitor >
75			{
76			typedef Data::Matrix6x Matrix6x;
77			typedef Data::Matrix3x Matrix3x;
78			typedef Data::Vector3 Vector3;
79
80			public:
81
82			#define ADD_DATA_PROPERTY(NAME,DOC) \
83			def_readwrite(#NAME, \
84			&Data::NAME, \
85			DOC)
86
87			#define ADD_DATA_PROPERTY_READONLY(NAME,DOC) \
88			def_readonly(#NAME, \
89			&Data::NAME, \
90			DOC)
91
92			#define ADD_DATA_PROPERTY_READONLY_BYVALUE(NAME,DOC) \
93			add_property(#NAME, \
94			make_getter(&Data::NAME,bp::return_value_policy<bp::return_by_value>()), \
95			DOC)
96
97
98			/* --- Exposing C++ API to python through the handler ----------------- */
99			template<class PyClass>
100		19	void visit(PyClass& cl) const
101			{
102	✓✗	19	cl
103			.def(bp::init<>(bp::arg("self"),"Default constructor."))
104	✓✗✓✗	19	.def(bp::init<Model>(bp::arg("model"),"Constructs a data structure from a given model."))
105
106	✓✗✓✗ ✓✗	38	.ADD_DATA_PROPERTY(a,"Joint spatial acceleration")
107	✓✗	19	.ADD_DATA_PROPERTY(oa,
108			"Joint spatial acceleration expressed at the origin of the world frame.")
109	✓✗	19	.ADD_DATA_PROPERTY(a_gf,
110			"Joint spatial acceleration containing also the contribution of the gravity acceleration")
111	✓✗	19	.ADD_DATA_PROPERTY(oa_gf,"Joint spatial acceleration containing also the contribution of the gravity acceleration, but expressed at the origin of the world frame.")
112
113	✓✗	19	.ADD_DATA_PROPERTY(v,"Joint spatial velocity expressed in the joint frame.")
114	✓✗	19	.ADD_DATA_PROPERTY(ov,"Joint spatial velocity expressed at the origin of the world frame.")
115
116	✓✗	19	.ADD_DATA_PROPERTY(f,"Joint spatial force expresssed in the joint frame.")
117	✓✗	19	.ADD_DATA_PROPERTY(of,"Joint spatial force expresssed at the origin of the world frame.")
118	✓✗	19	.ADD_DATA_PROPERTY(h,"Vector of spatial momenta expressed in the local frame of the joint.")
119	✓✗	19	.ADD_DATA_PROPERTY(oMi,"Body absolute placement (wrt world)")
120	✓✗	19	.ADD_DATA_PROPERTY(oMf,"frames absolute placement (wrt world)")
121	✓✗	19	.ADD_DATA_PROPERTY(liMi,"Body relative placement (wrt parent)")
122	✓✗	19	.ADD_DATA_PROPERTY(tau,"Joint torques (output of RNEA)")
123	✓✗	19	.ADD_DATA_PROPERTY(nle,"Non Linear Effects (output of nle algorithm)")
124	✓✗	19	.ADD_DATA_PROPERTY(ddq,"Joint accelerations (output of ABA)")
125	✓✗	19	.ADD_DATA_PROPERTY(Ycrb,"Inertia of the sub-tree composit rigid body")
126	✓✗	19	.ADD_DATA_PROPERTY(M,"The joint space inertia matrix")
127	✓✗	19	.ADD_DATA_PROPERTY(Minv,"The inverse of the joint space inertia matrix")
128	✓✗	19	.ADD_DATA_PROPERTY(C,"The Coriolis C(q,v) matrix such that the Coriolis effects are given by c(q,v) = C(q,v)v")
129	✓✗	19	.ADD_DATA_PROPERTY(g,"Vector of generalized gravity (dim model.nv).")
130	✓✗	19	.ADD_DATA_PROPERTY(Fcrb,"Spatial forces set, used in CRBA")
131	✓✗	19	.ADD_DATA_PROPERTY(lastChild,"Index of the last child (for CRBA)")
132	✓✗	19	.ADD_DATA_PROPERTY(nvSubtree,"Dimension of the subtree motion space (for CRBA)")
133	✓✗	19	.ADD_DATA_PROPERTY(U,"Joint Inertia square root (upper triangle)")
134	✓✗	19	.ADD_DATA_PROPERTY(D,"Diagonal of UDUT inertia decomposition")
135	✓✗	19	.ADD_DATA_PROPERTY(parents_fromRow,"First previous non-zero row in M (used in Cholesky)")
136	✓✗	19	.ADD_DATA_PROPERTY(nvSubtree_fromRow,"Subtree of the current row index (used in Cholesky)")
137	✓✗	19	.ADD_DATA_PROPERTY(J,"Jacobian of joint placement")
138	✓✗	19	.ADD_DATA_PROPERTY(dJ,"Time variation of the Jacobian of joint placement (data.J).")
139	✓✗	19	.ADD_DATA_PROPERTY(iMf,"Body placement wrt to algorithm end effector.")
140
141	✓✗	19	.ADD_DATA_PROPERTY(Ivx,"Right variation of the inertia matrix.")
142	✓✗	19	.ADD_DATA_PROPERTY(vxI,"Left variation of the inertia matrix.")
143	✓✗	19	.ADD_DATA_PROPERTY(B,"Combined variations of the inertia matrix consistent with Christoffel symbols.")
144
145	✓✗	19	.ADD_DATA_PROPERTY(Ag,
146			"Centroidal matrix which maps from joint velocity to the centroidal momentum.")
147	✓✗	19	.ADD_DATA_PROPERTY(dAg,
148			"Time derivative of the centroidal momentum matrix Ag.")
149	✓✗	19	.ADD_DATA_PROPERTY(hg,
150			"Centroidal momentum (expressed in the frame centered at the CoM and aligned with the world frame).")
151	✓✗	19	.ADD_DATA_PROPERTY(dhg,
152			"Centroidal momentum time derivative (expressed in the frame centered at the CoM and aligned with the world frame).")
153	✓✗	19	.ADD_DATA_PROPERTY(Ig,
154			"Centroidal Composite Rigid Body Inertia.")
155
156	✓✗	19	.ADD_DATA_PROPERTY(com,"CoM position of the subtree starting at joint index i.")
157	✓✗	19	.ADD_DATA_PROPERTY(vcom,"CoM velocity of the subtree starting at joint index i.")
158	✓✗	19	.ADD_DATA_PROPERTY(acom,"CoM acceleration of the subtree starting at joint index i.")
159	✓✗	19	.ADD_DATA_PROPERTY(mass,"Mass of the subtree starting at joint index i.")
160	✓✗	19	.ADD_DATA_PROPERTY(Jcom,"Jacobian of center of mass.")
161
162	✓✗	19	.ADD_DATA_PROPERTY(dtau_dq,"Partial derivative of the joint torque vector with respect to the joint configuration.")
163	✓✗	19	.ADD_DATA_PROPERTY(dtau_dv,"Partial derivative of the joint torque vector with respect to the joint velocity.")
164	✓✗	19	.ADD_DATA_PROPERTY(ddq_dq,"Partial derivative of the joint acceleration vector with respect to the joint configuration.")
165	✓✗	19	.ADD_DATA_PROPERTY(ddq_dv,"Partial derivative of the joint acceleration vector with respect to the joint velocity.")
166
167	✓✗	19	.ADD_DATA_PROPERTY(kinetic_energy,"Kinetic energy in [J] computed by computeKineticEnergy")
168	✓✗	19	.ADD_DATA_PROPERTY(potential_energy,"Potential energy in [J] computed by computePotentialEnergy")
169
170	✓✗	19	.ADD_DATA_PROPERTY(lambda_c,"Lagrange Multipliers linked to contact forces")
171	✓✗	19	.ADD_DATA_PROPERTY(impulse_c,"Lagrange Multipliers linked to contact impulses")
172
173	✓✗	19	.ADD_DATA_PROPERTY(dq_after,"Generalized velocity after the impact.")
174	✓✗	19	.ADD_DATA_PROPERTY(staticRegressor,"Static regressor.")
175	✓✗	19	.ADD_DATA_PROPERTY(jointTorqueRegressor,"Joint torque regressor.")
176
177	✓✗	19	.def(bp::self == bp::self)
178	✓✗	38	.def(bp::self != bp::self)
179			;
180		19	}
181
182			/* --- Expose --------------------------------------------------------- */
183		19	static void expose()
184			{
185		19	bp::class_<Data>("Data",
186			"Articulated rigid body data related to a Model.\n"
187			"It contains all the data that can be modified by the Pinocchio algorithms.",
188			bp::no_init)
189	✓✗	19	.def(DataPythonVisitor())
190	✓✗	19	.def(CopyableVisitor<Data>())
191	✓✗	19	.def(SerializableVisitor<Data>())
192	✓✗	19	.def_pickle(PickleData<Data>());
193
194			typedef PINOCCHIO_ALIGNED_STD_VECTOR(Vector3) StdVec_Vector3;
195			typedef PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6x) StdVec_Matrix6x;
196
197	✓✗✓✗ ✓✗	38	StdAlignedVectorPythonVisitor<Vector3,false>::expose("StdVec_Vector3")
198	✓✗	19	.def(details::overload_base_get_item_for_std_vector<StdVec_Vector3>());
199		19	serialize<StdAlignedVectorPythonVisitor<Vector3,false>::vector_type>();
200
201	✓✗✓✗ ✓✗	38	StdAlignedVectorPythonVisitor<Matrix6x,false>::expose("StdVec_Matrix6x")
202	✓✗	19	.def(details::overload_base_get_item_for_std_vector<StdVec_Matrix6x>());
203		19	serialize<StdAlignedVectorPythonVisitor<Matrix6x,false>::vector_type>();
204
205	✓✗✓✗ ✓✗	19	StdVectorPythonVisitor<int>::expose("StdVec_Int");
206		19	serialize<StdVectorPythonVisitor<int>::vector_type>();
207		19	}
208
209			};
210
211			}} // namespace pinocchio::python
212
213			#endif // ifndef __pinocchio_python_multibody_data_hpp__