model.hpp 11.6 KB
Newer Older
1
//
2
// Copyright (c) 2015-2016 CNRS
3
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
//
// 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/>.

Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
19
20
21
22
23
24
#ifndef __se3_model_hpp__
#define __se3_model_hpp__

#include "pinocchio/spatial/fwd.hpp"
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/spatial/force.hpp"
25
26
#include "pinocchio/spatial/motion.hpp"
#include "pinocchio/spatial/inertia.hpp"
27
#include "pinocchio/spatial/frame.hpp"
28
#include "pinocchio/multibody/fwd.hpp"
29
#include "pinocchio/multibody/joint/joint-variant.hpp"
30
#include "pinocchio/tools/string-generator.hpp"
31
#include <iostream>
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
32

33
34
35
36
37
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::SE3)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::Inertia)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::Force)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::Motion)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,6,Eigen::Dynamic>)
38
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::SE3::Vector3)
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
39
40
41

namespace se3
{
42
43
  class Model;
  class Data;
44
  
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
45
46
47
  class Model
  {
  public:
48
49
50
51
    typedef se3::Index Index;
    typedef se3::JointIndex JointIndex;
    typedef se3::GeomIndex GeomIndex;
    typedef se3::FrameIndex FrameIndex;
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
52

53
54
55
56
    int nq;                               // Dimension of the configuration representation
    int nv;                               // Dimension of the velocity vector space
    int nbody;                            // Number of bodies (= number of joints + 1)
    int nFixBody;                         // Number of fixed-bodies (= number of fixed-joints)
57
    int nOperationalFrames;               // Number of operational frames
58
59
60
61

    std::vector<Inertia> inertias;        // Spatial inertias of the body <i> in the supporting joint frame <i>
    std::vector<SE3> jointPlacements;     // Placement (SE3) of the input of joint <i> in parent joint output <li>
    JointModelVector joints;              // Model of joint <i>
62
    std::vector<JointIndex> parents;           // Joint parent of joint <i>, denoted <li> (li==parents[i])
63
64
65
66
67
    std::vector<std::string> names;       // Name of joint <i>
    std::vector<std::string> bodyNames;   // Name of the body attached to the output of joint <i>
    std::vector<bool> hasVisual;          // True iff body <i> has a visual mesh.

    std::vector<SE3> fix_lmpMi;           // Fixed-body relative placement (wrt last moving parent)
68
    std::vector<Model::JointIndex> fix_lastMovingParent; // Fixed-body index of the last moving parent
69
    std::vector<bool> fix_hasVisual;      // True iff fixed-body <i> has a visual mesh.
70
71
    std::vector<std::string> fix_bodyNames;// Name of fixed-joint <i>

72
73
    std::vector<Frame> operational_frames;

74
    Motion gravity;                       // Spatial gravity
75
    static const Eigen::Vector3d gravity981; // Default 3D gravity (=(0,0,-9.81))
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
76
77
78
79
80

    Model()
      : nq(0)
      , nv(0)
      , nbody(1)
81
      , nFixBody(0)
82
      , nOperationalFrames(0)
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
83
84
85
86
87
      , inertias(1)
      , jointPlacements(1)
      , joints(1)
      , parents(1)
      , names(1)
88
89
      , bodyNames(1)
      , hasVisual(1)
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
90
91
      , gravity( gravity981,Eigen::Vector3d::Zero() )
    {
92
93
      names[0]     = "universe";     // Should be "universe joint (trivial)"
      bodyNames[0] = "universe";
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
94
    }
Nicolas Mansard's avatar
Nicolas Mansard committed
95
    ~Model() {} // std::cout << "Destroy model" << std::endl; }
Nicolas Mansard's avatar
Nicolas Mansard committed
96
    template<typename D>
97
    JointIndex addBody(  JointIndex parent,const JointModelBase<D> & j,const SE3 & placement,
98
99
100
101
		                const Inertia & Y,
                    const std::string & jointName = "", const std::string & bodyName = "",
                    bool visual = false);
    template<typename D>
102
    JointIndex addBody(  JointIndex parent,const JointModelBase<D> & j,const SE3 & placement,
103
104
105
106
107
                    const Inertia & Y,
                    const Eigen::VectorXd & effort, const Eigen::VectorXd & velocity,
                    const Eigen::VectorXd & lowPos, const Eigen::VectorXd & upPos,
                    const std::string & jointName = "", const std::string & bodyName = "",
                    bool visual = false);
108
    JointIndex addFixedBody( JointIndex fix_lastMovingParent,
109
110
111
                        const SE3 & placementFromLastMoving,
                        const std::string &jointName = "",
                        bool visual=false);
112
113
    void mergeFixedBody(JointIndex parent, const SE3 & placement, const Inertia & Y);
    JointIndex getBodyId( const std::string & name ) const;
114
    bool existBodyName( const std::string & name ) const;
115
116
    const std::string& getBodyName( const JointIndex index ) const;
    JointIndex getJointId( const std::string & name ) const;
117
    bool existJointName( const std::string & name ) const;
118
    const std::string& getJointName( const JointIndex index ) const;
119

120
    FrameIndex getFrameId ( const std::string & name ) const;
121
    bool existFrame ( const std::string & name ) const;
122
123
124
    const std::string & getFrameName ( const FrameIndex index ) const;
    const JointIndex& getFrameParent( const std::string & name ) const;
    const JointIndex& getFrameParent( const FrameIndex index ) const;
125
    const SE3 & getFramePlacement( const std::string & name ) const;
126
    const SE3 & getFramePlacement( const FrameIndex index ) const;
127

128
    bool addFrame ( const Frame & frame );
129
    bool addFrame ( const std::string & name, JointIndex index_parent, const SE3 & placement );
130

Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
131
132
133
134
135
  };

  class Data
  {
  public:
136
137
    typedef Eigen::Matrix<double,6,Eigen::Dynamic> Matrix6x;
    typedef Eigen::Matrix<double,3,Eigen::Dynamic> Matrix3x;
138
    typedef SE3::Vector3 Vector3;
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
139
    
140
  public:
141
142
143
144
    /// \brief A const reference to the reference model.
    const Model & model;
    
    /// \brief Vector of se3::JointData associated to the se3::JointModel stored in model.
Nicolas Mansard's avatar
Nicolas Mansard committed
145
    JointDataVector joints;
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
    
    /// \brief Vector of joint accelerations.
    std::vector<Motion> a;
    
    /// \brief Vector of joint accelerations due to the gravity field.
    std::vector<Motion> a_gf;
    
    /// \brief Vector of joint velocities.
    std::vector<Motion> v;
    
    /// \brief Vector of body forces. For each body, the force represents the sum of
    ///        all external forces acting on the body.
    std::vector<Force> f;
    
    /// \brief Vector of absolute joint placements (wrt the world).
    std::vector<SE3> oMi;
    /// \brief Vector of relative joint placements (wrt the body parent).
    std::vector<SE3> liMi;
    
    /// \brief Vector of joint torques (dim model.nv).
    Eigen::VectorXd tau;
    
    /// \brief Vector of Non Linear Effects (dim model.nv). It corresponds to the Coriolis, centrifugal and gravitational effects.
    /// \note  In the equation \f$ M\ddot{q} + b = \tau \f$,
    ///        the non linear effects are associated to the \f$b\f$ term.
    Eigen::VectorXd nle;
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
172

173
174
    /// \brief Vector of absolute operationnel frame placements (wrt the world).
    std::vector<SE3> oMof;
175

176
177
178
179
180
    /// \brief Vector of sub-tree composite rigid body inertias, i.e. the apparent inertia of the subtree supported by the joint.
    std::vector<Inertia> Ycrb;
    
    /// \brief The joint space inertia matrix (a square matrix of dim model.nv).
    Eigen::MatrixXd M;
181
182

    std::vector<Matrix6x> Fcrb;           // Spatial forces set, used in CRBA
183

184
    std::vector<int> lastChild;  // Index of the last child (for CRBA)
185
    std::vector<int> nvSubtree;           // Dimension of the subtree motion space (for CRBA)
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
186

187
188
    Eigen::MatrixXd U;                    // Joint Inertia square root (upper triangle)
    Eigen::VectorXd D;                    // Diagonal of UDUT inertia decomposition
189
    Eigen::VectorXd tmp;                  // Temporary of size NV used in Cholesky
190
191
    std::vector<int> parents_fromRow;     // First previous non-zero row in M (used in Cholesky)
    std::vector<int> nvSubtree_fromRow;   // 
192
    
193
194
195
196
197
198
    /// \brief Jacobian of joint placements.
    /// \note The columns of J corresponds to the basis of the spatial velocities of each joint and expressed at the origin of the inertial frame. In other words, if \f$ v_{J_{i}} = S_{i} \dot{q}_{i}\f$ is the relative velocity of the joint i regarding to its parent, then \f$J = \begin{bmatrix} ^{0}X_{1} S_{1} & \cdots & ^{0}X_{i} S_{i} & \cdots & ^{0}X_{\text{nj}} S_{\text{nj}} \end{bmatrix} \f$. This Jacobian has no special meaning. To get the jacobian of a precise joint, you need to call se3::getJacobian
    Matrix6x J;
    
    /// \brief Vector of joint placements wrt to algorithm end effector.
    std::vector<SE3> iMf;
Nicolas Mansard's avatar
Nicolas Mansard committed
199

200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
    /// \brief Vector of subtree center of mass positions expressed in the root joint of the subtree. In other words, com[j] is the CoM position of the subtree supported by joint \f$ j \f$ and expressed in the joint frame \f$ j \f$. The element com[0] corresponds to the center of mass position of the whole model and expressed in the global frame.
    std::vector<Eigen::Vector3d> com;
    
    /// \brief Vector of subtree center of mass linear velocities expressed in the root joint of the subtree. In other words, vcom[j] is the CoM linear velocity of the subtree supported by joint \f$ j \f$ and expressed in the joint frame \f$ j \f$. The element vcom[0] corresponds to the velocity of the CoM of the whole model expressed in the global frame.
    std::vector<Eigen::Vector3d> vcom;
    
    /// \brief Vector of subtree center of mass linear accelerations expressed in the root joint of the subtree. In other words, acom[j] is the CoM linear acceleration of the subtree supported by joint \f$ j \f$ and expressed in the joint frame \f$ j \f$. The element acom[0] corresponds to the acceleration of the CoM of the whole model expressed in the global frame.
    std::vector<Eigen::Vector3d> acom;
    
    /// \brief Vector of subtree mass. In other words, mass[j] is the mass of the subtree supported by joint \f$ j \f$. The element mass[0] corrresponds to the total mass of the model.
    std::vector<double> mass;
    
    /// \brief Jacobien of center of mass.
    /// \note This Jacobian maps the joint velocity vector to the velocity of the center of mass, expressed in the inertia frame. In other words, \f$ v_{\text{CoM}} = J_{\text{CoM}} \dot{q}\f$.
    Matrix3x Jcom;
215
216
217
218
219
220

    Eigen::VectorXd effortLimit;          // Joint max effort
    Eigen::VectorXd velocityLimit;        // Joint max velocity

    Eigen::VectorXd lowerPositionLimit;   // limit for joint lower position
    Eigen::VectorXd upperPositionLimit;   // limit for joint upper position
221
222
    
    double kinetic_energy; // kinetic energy of the model
223
    double potential_energy; // potential energy of the model
224

225
226
227
228
    /// \brief Default constructor of se3::Data from a se3::Model.
    ///
    /// \param[in] model The model structure of the rigid body system.
    Data (const Model & model);
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
229

230
  private:
231
    void computeLastChild(const Model& model);
232
    void computeParents_fromRow(const Model& model);
233

Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
234
235
236
237
238
239
240
  };

} // namespace se3

/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
241
#include "pinocchio/multibody/model.hxx"
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
242
243

#endif // ifndef __se3_model_hpp__