Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/pinocchio/bindings/python/multibody/data.hpp	Lines:	76	90	84.4 %
Date:	2024-01-23 21:41:47	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"

EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(pinocchio::Data)

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