model.hpp 21.2 KB
Newer Older
1
//
jcarpent's avatar
jcarpent committed
2
// Copyright (c) 2015-2017 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/multibody/fwd.hpp"
28
#include "pinocchio/multibody/frame.hpp"
29
#include "pinocchio/multibody/joint/joint.hpp"
30
#include "pinocchio/deprecated.hh"
31
#include "pinocchio/tools/string-generator.hpp"
32
#include "pinocchio/container/aligned-vector.hpp"
33

34
#include <iostream>
35
#include <Eigen/Cholesky>
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
36
37
38

namespace se3
{
39
  struct Model
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
40
  {
41
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
42
43
44
45
    typedef se3::Index Index;
    typedef se3::JointIndex JointIndex;
    typedef se3::GeomIndex GeomIndex;
    typedef se3::FrameIndex FrameIndex;
jcarpent's avatar
jcarpent committed
46
    typedef std::vector<Index> IndexVector;
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
47

jcarpent's avatar
jcarpent committed
48
49
50
51
52
53
    /// \brief Dimension of the configuration vector representation.
    int nq;
    
    /// \brief Dimension of the velocity vector space.
    int nv;
    
jcarpent's avatar
jcarpent committed
54
    /// \brief Number of joints.
55
    int njoints;
56

jcarpent's avatar
jcarpent committed
57
    /// \brief Number of bodies.
58
    int nbodies;
jcarpent's avatar
jcarpent committed
59
60
    
    /// \brief Number of operational frames.
61
    int nframes;
62

jcarpent's avatar
jcarpent committed
63
    /// \brief Spatial inertias of the body <i> expressed in the supporting joint frame <i>.
64
    container::aligned_vector<Inertia> inertias;
jcarpent's avatar
jcarpent committed
65
66
    
    /// \brief Placement (SE3) of the input of joint <i> regarding to the parent joint output <li>.
67
    container::aligned_vector<SE3> jointPlacements;
68

69
    /// \brief Model of joint <i>, encapsulated in a JointModelAccessor.
70
    JointModelVector joints;
jcarpent's avatar
jcarpent committed
71
72
73
    
    /// \brief Joint parent of joint <i>, denoted <li> (li==parents[i]).
    std::vector<JointIndex> parents;
74

jcarpent's avatar
jcarpent committed
75
76
77
    /// \brief Name of joint <i>
    std::vector<std::string> names;
    
78
    /// \brief Vector of joint's neutral configurations
79
    Eigen::VectorXd neutralConfiguration;
80

81
82
83
84
85
86
87
88
89
90
    /// \brief Vector of maximal joint torques
    Eigen::VectorXd effortLimit;
    /// \brief Vector of maximal joint velocities
    Eigen::VectorXd velocityLimit;

    /// \brief Lower joint configuration limit
    Eigen::VectorXd lowerPositionLimit;
    /// \brief Upper joint configuration limit
    Eigen::VectorXd upperPositionLimit;

jcarpent's avatar
jcarpent committed
91
    /// \brief Vector of operational frames registered on the model.
92
    container::aligned_vector<Frame> frames;
jcarpent's avatar
jcarpent committed
93
    
jcarpent's avatar
jcarpent committed
94
    /// \brief Vector of subtrees.
jcarpent's avatar
jcarpent committed
95
    /// subtree[j] corresponds to the subtree supported by the joint j.
jcarpent's avatar
jcarpent committed
96
    /// The first element of subtree[j] is the index of the joint j itself.
jcarpent's avatar
jcarpent committed
97
    std::vector<IndexVector> subtrees;
98

jcarpent's avatar
jcarpent committed
99
    /// \brief Spatial gravity of the model.
jcarpent's avatar
jcarpent committed
100
    Motion gravity;
jcarpent's avatar
jcarpent committed
101
    
jcarpent's avatar
jcarpent committed
102
103
    /// \brief Default 3D gravity vector (=(0,0,-9.81)).
    static const Eigen::Vector3d gravity981;
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
104

105
106
107
    /// \brief Model name;
    std::string name;

jcarpent's avatar
jcarpent committed
108
    /// \brief Default constructor. Builds an empty model with no joints.
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
109
110
111
    Model()
      : nq(0)
      , nv(0)
112
113
114
      , njoints(1)
      , nbodies(1)
      , nframes(0)
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
115
      , inertias(1)
116
      , jointPlacements(1, SE3::Identity())
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
117
      , joints(1)
118
      , parents(1, 0)
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
119
      , names(1)
jcarpent's avatar
jcarpent committed
120
      , subtrees(1)
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
121
122
      , gravity( gravity981,Eigen::Vector3d::Zero() )
    {
123
      names[0]     = "universe";     // Should be "universe joint (trivial)"
124
125
126
      // FIXME Should the universe joint be a FIXED_JOINT even if it is
      // in the list of joints ? See comment in definition of
      // Model::addJointFrame and Model::addBodyFrame
127
      addFrame(Frame("universe", 0, 0, SE3::Identity(), FIXED_JOINT));
128
129
130
131
      // Inertia of universe has no sense.
      inertias[0].mass() = std::numeric_limits<double>::quiet_NaN();
      inertias[0].lever().fill (std::numeric_limits<double>::quiet_NaN());
      inertias[0].inertia().fill (std::numeric_limits<double>::quiet_NaN());
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
132
    }
Nicolas Mansard's avatar
Nicolas Mansard committed
133
    ~Model() {} // std::cout << "Destroy model" << std::endl; }
jcarpent's avatar
jcarpent committed
134
135
    
    ///
136
    /// \brief Add a joint to the kinematic tree with given bounds.
jcarpent's avatar
jcarpent committed
137
    ///
138
139
    /// \remark This method does not add a Frame of same name to the vector of frames.
    ///         Use Model::addJointFrame.
jcarpent's avatar
jcarpent committed
140
    /// \remark The inertia supported by the joint is set to Zero.
jcarpent's avatar
jcarpent committed
141
    ///
jcarpent's avatar
jcarpent committed
142
    /// \tparam JointModelDerived The type of the joint model.
jcarpent's avatar
jcarpent committed
143
144
    ///
    /// \param[in] parent Index of the parent joint.
jcarpent's avatar
jcarpent committed
145
146
147
    /// \param[in] joint_model The joint model.
    /// \param[in] joint_placement Placement of the joint inside its parent joint.
    /// \param[in] joint_name Name of the joint. If empty, the name is random.
148
149
150
151
    /// \param[in] max_effort Maximal joint torque.
    /// \param[in] max_velocity Maximal joint velocity.
    /// \param[in] min_config Lower joint configuration.
    /// \param[in] max_config Upper joint configuration.
152
    ///
jcarpent's avatar
jcarpent committed
153
    /// \return The index of the new joint.
154
    ///
155
    /// \sa Model::appendBodyToJoint, Model::addJointFrame
156
    ///
jcarpent's avatar
jcarpent committed
157
158
    template<typename JointModelDerived>
    JointIndex addJoint(const JointIndex parent, const JointModelBase<JointModelDerived> & joint_model, const SE3 & joint_placement,
159
                        const std::string & joint_name,
160
161
162
                        const Eigen::VectorXd & max_effort,
                        const Eigen::VectorXd & max_velocity,
                        const Eigen::VectorXd & min_config,
163
                        const Eigen::VectorXd & max_config
164
165
166
167
                        );

    ///
    /// \brief Add a joint to the kinematic tree with infinite bounds.
jcarpent's avatar
jcarpent committed
168
    ///
jcarpent's avatar
jcarpent committed
169
    /// \remark This method also adds a Frame of same name to the vector of frames.
jcarpent's avatar
jcarpent committed
170
    /// \remark The inertia supported by the joint is set to Zero.
jcarpent's avatar
jcarpent committed
171
    ///
jcarpent's avatar
jcarpent committed
172
    /// \tparam JointModelDerived The type of the joint model.
jcarpent's avatar
jcarpent committed
173
174
    ///
    /// \param[in] parent Index of the parent joint.
jcarpent's avatar
jcarpent committed
175
176
177
    /// \param[in] joint_model The joint model.
    /// \param[in] joint_placement Placement of the joint inside its parent joint.
    /// \param[in] joint_name Name of the joint. If empty, the name is random.
178
    ///
jcarpent's avatar
jcarpent committed
179
    /// \return The index of the new joint.
180
    ///
jcarpent's avatar
jcarpent committed
181
    /// \sa Model::appendBodyToJoint
182
    ///
jcarpent's avatar
jcarpent committed
183
184
    template<typename JointModelDerived>
    JointIndex addJoint(const JointIndex parent, const JointModelBase<JointModelDerived> & joint_model, const SE3 & joint_placement,
185
                        const std::string & joint_name
jcarpent's avatar
jcarpent committed
186
                        );
187
188

    ///
189
    /// \brief Add a joint to the frame tree.
190
    ///
191
192
193
194
    /// \param[in] jointIndex Index of the joint.
    /// \param[in] frameIndex Index of the parent frame. If negative,
    ///            the parent frame is the frame of the parent joint.
    ///
195
    /// \return The index of the new frame or -1 in case of error.
196
    ///
197
198
    int addJointFrame (const JointIndex& jointIndex,
                             int         frameIndex = -1);
199

200
    ///
jcarpent's avatar
jcarpent committed
201
    /// \brief Append a body to a given joint of the kinematic tree.
202
    ///
jcarpent's avatar
jcarpent committed
203
    /// \param[in] joint_index Index of the supporting joint.
204
    /// \param[in] Y Spatial inertia of the body.
jcarpent's avatar
jcarpent committed
205
    /// \param[in] body_placement The relative placement of the body regarding to the parent joint. Set default to the Identity placement.
206
    ///
jcarpent's avatar
jcarpent committed
207
    /// \sa Model::addJoint
208
    ///
jcarpent's avatar
jcarpent committed
209
    void appendBodyToJoint(const JointIndex joint_index, const Inertia & Y,
210
211
212
213
214
215
216
217
218
219
220
                           const SE3 & body_placement = SE3::Identity());

    ///
    /// \brief Add a body to the frame tree.
    ///
    /// \param[in] body_name Name of the body.
    /// \param[in] parentJoint Index of the parent joint.
    /// \param[in] body_placement The relative placement of the body regarding to the parent joint. Set default to the Identity placement.
    /// \param[in] previousFrame Index of the parent frame. If negative,
    ///            the parent frame is the frame of the parent joint.
    ///
221
    /// \return The index of the new frame or -1 in case of error.
222
    ///
223
224
225
226
    int addBodyFrame (const std::string & body_name,
                      const JointIndex  & parentJoint,
                      const SE3         & body_placement = SE3::Identity(),
                            int           previousFrame  = -1);
227

jcarpent's avatar
jcarpent committed
228
229
    ///
    /// \brief Return the index of a body given by its name.
230
231
232
233
234
    /// 
    /// \warning If no body is found, return the number of elements at time T.
    /// This can lead to errors if the model is expanded after this method is called
    /// (for example to get the id of a parent body)
    /// 
jcarpent's avatar
jcarpent committed
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
    /// \param[in] name Name of the body.
    ///
    /// \return Index of the body.
    ///
    JointIndex getBodyId(const std::string & name) const;
    
    ///
    /// \brief Check if a body given by its name exists.
    ///
    /// \param[in] name Name of the body.
    ///
    /// \return True if the body exists in the kinematic tree.
    ///
    bool existBodyName(const std::string & name) const;
    
    
    ///
    /// \brief Return the index of a joint given by its name.
    ///
254
255
256
    /// \warning If no joint is found, return the number of elements at time T.
    /// This can lead to errors if the model is expanded after this method is called
    /// (for example to get the id of a parent joint)
jcarpent's avatar
jcarpent committed
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
    /// \param[in] index Index of the joint.
    ///
    /// \return Index of the joint.
    ///
    JointIndex getJointId(const std::string & name) const;
    
    ///
    /// \brief Check if a joint given by its name exists.
    ///
    /// \param[in] name Name of the joint.
    ///
    /// \return True if the joint exists in the kinematic tree.
    ///
    bool existJointName(const std::string & name) const;
    
    ///
    /// \brief Get the name of a joint given by its index.
    ///
    /// \param[in] index Index of the joint.
    ///
    /// \return Name of the joint.
    ///
279
    PINOCCHIO_DEPRECATED const std::string & getJointName(const JointIndex index) const;
280

jcarpent's avatar
jcarpent committed
281
    ///
jcarpent's avatar
jcarpent committed
282
283
    /// \brief Returns the index of a frame given by its name.
    ///        \sa Model::existFrame to check if the frame exists or not.
jcarpent's avatar
jcarpent committed
284
    ///
jcarpent's avatar
jcarpent committed
285
    /// \warning If no frame is found, returns the size of the vector of Model::frames.
286
    /// This can lead to errors if the model is expanded after this method is called
jcarpent's avatar
jcarpent committed
287
    /// (for example to get the id of a parent frame).
288
    /// 
jcarpent's avatar
jcarpent committed
289
    /// \param[in] name Name of the frame.
290
    /// \param[in] type Type of the frame.
jcarpent's avatar
jcarpent committed
291
292
293
    ///
    /// \return Index of the frame.
    ///
294
    FrameIndex getFrameId (const std::string & name, const FrameType& type = (FrameType) (JOINT | FIXED_JOINT | BODY | OP_FRAME | SENSOR )) const;
jcarpent's avatar
jcarpent committed
295
296
    
    ///
jcarpent's avatar
jcarpent committed
297
    /// \brief Checks if a frame given by its name exists.
jcarpent's avatar
jcarpent committed
298
299
    ///
    /// \param[in] name Name of the frame.
300
    /// \param[in] type Type of the frame.
jcarpent's avatar
jcarpent committed
301
    ///
jcarpent's avatar
jcarpent committed
302
    /// \return Returns true if the frame exists.
jcarpent's avatar
jcarpent committed
303
    ///
304
    bool existFrame (const std::string & name, const FrameType& type = (FrameType) (JOINT | FIXED_JOINT | BODY | OP_FRAME | SENSOR )) const;
jcarpent's avatar
jcarpent committed
305
    
306
    
jcarpent's avatar
jcarpent committed
307
    ///
jcarpent's avatar
jcarpent committed
308
    /// \brief Adds a frame to the kinematic tree.
jcarpent's avatar
jcarpent committed
309
310
311
    ///
    /// \param[in] frame The frame to add to the kinematic tree.
    ///
312
313
    /// \return Returns the index of the frame if it has been successfully added,
    ///         -1 otherwise.
jcarpent's avatar
jcarpent committed
314
    ///
315
    int addFrame(const Frame & frame);
316

317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
    /// Check the validity of the attributes of Model with respect to the specification of some
    /// algorithms.
    ///
    /// The method is a template so that the checkers can be defined in each algorithms.
    /// \param[in] checker a class, typically defined in the algorithm module, that 
    /// validates the attributes of model.
    /// \return true if the Model is valid, false otherwise.
    template<typename D>
    inline bool check(const AlgorithmCheckerBase<D> & checker = AlgorithmCheckerBase<D>()) const
    { return checker.checkModel(*this); }

    /// Multiple checks for a fusion::vector of AlgorithmCheckerBase.
    ///
    /// Run the check test for several conditons.
    /// \param[in] v fusion::vector of algo checkers. The param is typically initialize with 
    /// boost::fusion::make_list( AlgoChecker1(), AlgoChecker2(), ...)
    /// make_list is defined in #include <boost/fusion/include/make_list.hpp>
    /// \warning no more than 10 checkers can be added (or Model API should be extended).
    /// \note This method is implemented in src/algo/check.hxx.
    template<class T1,class T2,class T3,class T4,class T5,
             class T6,class T7,class T8,class T9,class T10>
jcarpent's avatar
jcarpent committed
338
    inline bool check( const boost::fusion::list<T1,T2,T3,T4,T5,T6,T7,T8,T9,T10> & checkerList ) const;
339
340

    /// Run check(fusion::list) with DEFAULT_CHECKERS as argument.
jcarpent's avatar
jcarpent committed
341
    inline bool check() const;
342
343
344
345
346
347
348
    
    /// Run checkData on data and current model.
    ///
    /// \param[in] data to be checked wrt *this.
    ///
    /// \return true if the data is valid, false otherwise.
    inline bool check(const Data & data) const;
349

jcarpent's avatar
jcarpent committed
350
351
352
353
354
355
356
  protected:
    
    /// \brief Add the joint_id to its parent subtrees.
    ///
    /// \param[in] joint_id The id of the joint to add to the subtrees
    ///
    void addJointIndexToParentSubtrees(const JointIndex joint_id);
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
357
358
  };

359
  struct Data
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
360
  {
361
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
jcarpent's avatar
jcarpent committed
362
    /// \brief The 6d jacobian type (temporary)
363
    typedef Eigen::Matrix<double,6,Eigen::Dynamic> Matrix6x;
jcarpent's avatar
jcarpent committed
364
    /// \brief The 3d jacobian type (temporary)
365
    typedef Eigen::Matrix<double,3,Eigen::Dynamic> Matrix3x;
366
    typedef SE3::Vector3 Vector3;
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
367
    
368
369
    /// \brief Vector of se3::JointData associated to the se3::JointModel stored in model, 
    /// encapsulated in JointDataAccessor.
370
    JointDataVector joints;
371
372
    
    /// \brief Vector of joint accelerations.
373
    container::aligned_vector<Motion> a;
374
375
    
    /// \brief Vector of joint accelerations due to the gravity field.
376
    container::aligned_vector<Motion> a_gf;
377
378
    
    /// \brief Vector of joint velocities.
379
    container::aligned_vector<Motion> v;
380
381
382
    
    /// \brief Vector of body forces. For each body, the force represents the sum of
    ///        all external forces acting on the body.
383
    container::aligned_vector<Force> f;
384
385
    
    /// \brief Vector of absolute joint placements (wrt the world).
386
    container::aligned_vector<SE3> oMi;
jcarpent's avatar
jcarpent committed
387

388
    /// \brief Vector of relative joint placements (wrt the body parent).
389
    container::aligned_vector<SE3> liMi;
390
391
392
393
394
395
396
397
    
    /// \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
398

399
    /// \brief Vector of absolute operationnel frame placements (wrt the world).
400
    container::aligned_vector<SE3> oMf;
401

402
    /// \brief Vector of sub-tree composite rigid body inertias, i.e. the apparent inertia of the subtree supported by the joint.
403
    container::aligned_vector<Inertia> Ycrb;
404
405
406
    
    /// \brief The joint space inertia matrix (a square matrix of dim model.nv).
    Eigen::MatrixXd M;
jcarpent's avatar
jcarpent committed
407
408
409
410
411
412
    
    /// \brief The joint accelerations computed from ABA
    Eigen::VectorXd ddq;
    
    // ABA internal data
    /// \brief Inertia matrix of the subtree expressed as dense matrix [ABA]
413
    container::aligned_vector<Inertia::Matrix6> Yaba;
jcarpent's avatar
jcarpent committed
414
415
    
    /// \brief Intermediate quantity corresponding to apparent torque [ABA]
416
417
418
    Eigen::VectorXd u;                  // Joint Inertia
    
    // CCRBA return quantities
jcarpent's avatar
jcarpent committed
419
420
    /// \brief Centroidal Momentum Matrix
    /// \note \f$ hg = Ag \dot{q}\f$ maps the joint velocity set to the centroidal momentum.
421
422
    Matrix6x Ag;
    
jcarpent's avatar
jcarpent committed
423
424
425
    /// \brief Centroidal momentum quantity.
    /// \note The centroidal momentum is expressed in the frame centered at the CoM and aligned with the inertial frame.
    ///
426
427
    Force hg;
    
jcarpent's avatar
jcarpent committed
428
429
    /// \brief Centroidal Composite Rigid Body Inertia.
    /// \note \f$ hg = Ig v_{\text{mean}}\f$ map a mean velocity to the current centroil momentum quantity.
430
    Inertia Ig;
431

jcarpent's avatar
jcarpent committed
432
    /// \brief Spatial forces set, used in CRBA and CCRBA
433
    container::aligned_vector<Matrix6x> Fcrb;
434

jcarpent's avatar
jcarpent committed
435
436
437
438
    /// \brief Index of the last child (for CRBA)
    std::vector<int> lastChild;
    /// \brief Dimension of the subtree motion space (for CRBA)
    std::vector<int> nvSubtree;
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
439

jcarpent's avatar
jcarpent committed
440
441
442
443
444
445
446
447
448
449
450
451
452
453
    /// \brief Joint space intertia matrix square root (upper trianglular part) computed with a Cholesky Decomposition.
    Eigen::MatrixXd U;
    
    /// \brief Diagonal of the joint space intertia matrix obtained by a Cholesky Decomposition.
    Eigen::VectorXd D;
    
    /// \brief Temporary of size NV used in Cholesky Decomposition.
    Eigen::VectorXd tmp;
    
    /// \brief First previous non-zero row in M (used in Cholesky Decomposition).
    std::vector<int> parents_fromRow;
    
    /// \brief Subtree of the current row index (used in Cholesky Decomposition).
    std::vector<int> nvSubtree_fromRow;
454
    
455
456
457
458
    /// \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;
    
459
460
461
    /// \brief Derivative of the Jacobian with respect to the time.
    Matrix6x dJ;
    
462
    /// \brief Vector of joint placements wrt to algorithm end effector.
463
    container::aligned_vector<SE3> iMf;
Nicolas Mansard's avatar
Nicolas Mansard committed
464

465
    /// \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.
466
    container::aligned_vector<Eigen::Vector3d> com;
467
468
    
    /// \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.
469
    container::aligned_vector<Eigen::Vector3d> vcom;
470
471
    
    /// \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.
472
    container::aligned_vector<Eigen::Vector3d> acom;
473
474
475
476
477
    
    /// \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.
jcarpent's avatar
jcarpent committed
478
    /// \note This Jacobian maps the joint velocity vector to the velocity of the center of mass, expressed in the inertial frame. In other words, \f$ v_{\text{CoM}} = J_{\text{CoM}} \dot{q}\f$.
479
    Matrix3x Jcom;
480

481
    
jcarpent's avatar
jcarpent committed
482
483
484
485
486
487
    /// \brief Kinetic energy of the model.
    double kinetic_energy;
    
    /// \brief Potential energy of the model.
    double potential_energy;
    
488
489
490
491
492
493
494
495
    // Temporary variables used in forward dynamics
    
    /// \brief Inverse of the operational-space inertia matrix
    Eigen::MatrixXd JMinvJt;
    
    /// \brief Cholesky decompostion of \JMinvJt.
    Eigen::LLT<Eigen::MatrixXd> llt_JMinvJt;
    
496
    /// \brief Lagrange Multipliers corresponding to the contact forces in se3::forwardDynamics.
497
    Eigen::VectorXd lambda_c;
498
499
500
501
502
503
504
    
    /// \brief Temporary corresponding to \f$ \sqrt{D} U^{-1} J^{\top} \f$.
    Eigen::MatrixXd sDUiJt;
    
    /// \brief Temporary corresponding to the residual torque \f$ \tau - b(q,\dot{q}) \f$.
    Eigen::VectorXd torque_residual;
    
505
506
507
508
509
510
    /// \brief Generalized velocity after impact.
    Eigen::VectorXd dq_after;
    
    /// \brief Lagrange Multipliers corresponding to the contact impulses in se3::impulseDynamics.
    Eigen::VectorXd impulse_c;
    
jcarpent's avatar
jcarpent committed
511
    ///
512
513
514
    /// \brief Default constructor of se3::Data from a se3::Model.
    ///
    /// \param[in] model The model structure of the rigid body system.
jcarpent's avatar
jcarpent committed
515
    ///
516
    explicit Data (const Model & model);
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
517

518
  private:
519
    void computeLastChild(const Model& model);
520
    void computeParents_fromRow(const Model& model);
521

Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
522
523
524
525
526
527
528
  };

} // namespace se3

/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
529
#include "pinocchio/multibody/model.hxx"
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
530
531

#endif // ifndef __se3_model_hpp__