data.hpp 7.98 KB
Newer Older
1
//
jcarpent's avatar
jcarpent committed
2
// Copyright (c) 2015-2018 CNRS
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// Pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.

18
19
20
#ifndef __se3_python_data_hpp__
#define __se3_python_data_hpp__

jcarpent's avatar
jcarpent committed
21
#include "pinocchio/multibody/data.hpp"
22

jcarpent's avatar
jcarpent committed
23
#include <eigenpy/memory.hpp>
24
#include <eigenpy/eigenpy.hpp>
25
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
26

jcarpent's avatar
jcarpent committed
27
28
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(se3::Data)

29
30
31
32
33
34
35
36
37
namespace se3
{
  namespace python
  {
    namespace bp = boost::python;

    struct DataPythonVisitor
      : public boost::python::def_visitor< DataPythonVisitor >
    {
38
39
      typedef Data::Matrix6x Matrix6x;
      typedef Data::Matrix3x Matrix3x;
40
      typedef Data::Vector3 Vector3;
41
42
43

    public:

44
#define ADD_DATA_PROPERTY(TYPE,NAME,DOC)				 \
jcarpent's avatar
jcarpent committed
45
46
47
48
49
50
51
52
53
54
      def_readwrite(#NAME,						 \
      &Data::NAME,		 \
      DOC)
      
#define ADD_DATA_PROPERTY_READONLY(TYPE,NAME,DOC)				 \
      def_readonly(#NAME,						 \
      &Data::NAME,		 \
      DOC)
      
#define ADD_DATA_PROPERTY_READONLY_BYVALUE(TYPE,NAME,DOC)				 \
55
      add_property(#NAME,						 \
jcarpent's avatar
jcarpent committed
56
57
58
      make_getter(&Data::NAME,bp::return_value_policy<bp::return_by_value>()), \
      DOC)
      
59
	 
60
61
62
63
      /* --- Exposing C++ API to python through the handler ----------------- */
      template<class PyClass>
      void visit(PyClass& cl) const 
      {
64
        cl
65
        .def(bp::init<Model>(bp::arg("model"),"Constructs a data structure from a given model."))
66
        
67
68
69
70
        .ADD_DATA_PROPERTY(container::aligned_vector<Motion>,a,"Joint spatial acceleration")
        .ADD_DATA_PROPERTY(container::aligned_vector<Motion>,a_gf,"Joint spatial acceleration containing also the contribution of the gravity acceleration")
        .ADD_DATA_PROPERTY(container::aligned_vector<Motion>,v,"Joint spatial velocity expressed in the joint frame.")
        .ADD_DATA_PROPERTY(container::aligned_vector<Force>,f,"Joint spatial force expresssed in the joint frame.")
71
72
73
        .ADD_DATA_PROPERTY(container::aligned_vector<SE3>,oMi,"Body absolute placement (wrt world)")
        .ADD_DATA_PROPERTY(container::aligned_vector<SE3>,oMf,"frames absolute placement (wrt world)")
        .ADD_DATA_PROPERTY(container::aligned_vector<SE3>,liMi,"Body relative placement (wrt parent)")
74
75
76
        .ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::VectorXd,tau,"Joint torques (output of RNEA)")
        .ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::VectorXd,nle,"Non Linear Effects (output of nle algorithm)")
        .ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::VectorXd,ddq,"Joint accelerations (output of ABA)")
77
        .ADD_DATA_PROPERTY(container::aligned_vector<Inertia>,Ycrb,"Inertia of the sub-tree composit rigid body")
78
        .ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::MatrixXd,M,"The joint space inertia matrix")
79
        .ADD_DATA_PROPERTY_READONLY_BYVALUE(Data::RowMatrixXs,Minv,"The inverse of the joint space inertia matrix")
80
        .ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::MatrixXd,C,"The Coriolis C(q,v) matrix such that the Coriolis effects are given by c(q,v) = C(q,v)v")
jcarpent's avatar
jcarpent committed
81
        .ADD_DATA_PROPERTY(container::aligned_vector<Matrix6x>,Fcrb,"Spatial forces set, used in CRBA")
82
83
        .ADD_DATA_PROPERTY(std::vector<int>,lastChild,"Index of the last child (for CRBA)")
        .ADD_DATA_PROPERTY(std::vector<int>,nvSubtree,"Dimension of the subtree motion space (for CRBA)")
jcarpent's avatar
jcarpent committed
84
85
        .ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::MatrixXd,U,"Joint Inertia square root (upper triangle)")
        .ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::VectorXd,D,"Diagonal of UDUT inertia decomposition")
86
        .ADD_DATA_PROPERTY(std::vector<int>,parents_fromRow,"First previous non-zero row in M (used in Cholesky)")
87
        .ADD_DATA_PROPERTY(std::vector<int>,nvSubtree_fromRow,"Subtree of the current row index (used in Cholesky)")
jcarpent's avatar
jcarpent committed
88
        .ADD_DATA_PROPERTY_READONLY_BYVALUE(Matrix6x,J,"Jacobian of joint placement")
89
        .ADD_DATA_PROPERTY_READONLY_BYVALUE(Matrix6x,dJ,"Time variation of the Jacobian of joint placement (data.J).")
90
        .ADD_DATA_PROPERTY(container::aligned_vector<SE3>,iMf,"Body placement wrt to algorithm end effector.")
91
        
jcarpent's avatar
jcarpent committed
92
        .ADD_DATA_PROPERTY_READONLY_BYVALUE(Matrix6x,Ag,
jcarpent's avatar
jcarpent committed
93
                                            "Centroidal matrix which maps from joint velocity to the centroidal momentum.")
94
        .ADD_DATA_PROPERTY_READONLY_BYVALUE(Matrix6x,dAg,
jcarpent's avatar
jcarpent committed
95
                                            "Time derivative of the centroidal momentum matrix Ag.")
jcarpent's avatar
jcarpent committed
96
97
98
99
        .ADD_DATA_PROPERTY_READONLY(Force,hg,
                                    "Centroidal momentum (expressed in the frame centered at the CoM and aligned with the inertial frame).")
        .ADD_DATA_PROPERTY_READONLY(Inertia,Ig,
                                    "Centroidal Composite Rigid Body Inertia.")
100
        
101
102
103
104
        .ADD_DATA_PROPERTY(container::aligned_vector<Vector3>,com,"CoM position of the subtree starting at joint index i.")
        .ADD_DATA_PROPERTY(container::aligned_vector<Vector3>,vcom,"CoM velocity of the subtree starting at joint index i.")
        .ADD_DATA_PROPERTY(container::aligned_vector<Vector3>,acom,"CoM acceleration of the subtree starting at joint index i..")
        .ADD_DATA_PROPERTY(std::vector<double>,mass,"Mass of the subtree starting at joint index i.")
jcarpent's avatar
jcarpent committed
105
        .ADD_DATA_PROPERTY_READONLY_BYVALUE(Matrix3x,Jcom,"Jacobian of center of mass.")
106

jcarpent's avatar
jcarpent committed
107
108
109
        .ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::MatrixXd,C,"Joint space Coriolis matrix.")
        .ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::MatrixXd,dtau_dq,"Partial derivative of the joint torque vector with respect to the joint configuration.")
        .ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::MatrixXd,dtau_dv,"Partial derivative of the joint torque vector with respect to the joint velocity.")
110
111
        .ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::MatrixXd,ddq_dq,"Partial derivative of the joint acceleration vector with respect to the joint configuration.")
        .ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::MatrixXd,ddq_dv,"Partial derivative of the joint acceleration vector with respect to the joint velocity.")
jcarpent's avatar
jcarpent committed
112
        
jcarpent's avatar
jcarpent committed
113
114
        .ADD_DATA_PROPERTY_READONLY_BYVALUE(double,kinetic_energy,"Kinetic energy in [J] computed by kineticEnergy(model,data,q,v,True/False)")
        .ADD_DATA_PROPERTY_READONLY_BYVALUE(double,potential_energy,"Potential energy in [J] computed by potentialEnergy(model,data,q,True/False)")
115
        
jcarpent's avatar
jcarpent committed
116
117
        .ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::VectorXd,lambda_c,"Lagrange Multipliers linked to contact forces")
        .ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::VectorXd,impulse_c,"Lagrange Multipliers linked to contact impulses")
118
        
jcarpent's avatar
jcarpent committed
119
        .ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::VectorXd,dq_after,"Generalized velocity after the impact.")
120
        ;
121
122
      }

123
124
      /* --- Expose --------------------------------------------------------- */
      static void expose()
125
      {
jcarpent's avatar
jcarpent committed
126
        bp::class_<Data>("Data",
127
128
                         "Articulated rigid body data.\n"
                         "It contains all the data that can be modified by the algorithms.",
jcarpent's avatar
jcarpent committed
129
                         bp::no_init)
130
131
        .def(DataPythonVisitor());
        
132
133
        bp::class_< container::aligned_vector<Vector3> >("StdVec_vec3d")
        .def(bp::vector_indexing_suite< container::aligned_vector<Vector3>, true >());
134
135
        bp::class_< std::vector<int> >("StdVec_int")
        .def(bp::vector_indexing_suite< std::vector<int> >());
136
        
137
        eigenpy::enableEigenPySpecific<Data::RowMatrixXs>();
138
139
140
141
142
143
144
145
      }

    };
    
  }} // namespace se3::python

#endif // ifndef __se3_python_data_hpp__