model.hpp 13.2 KB
Newer Older
1
//
jcarpent's avatar
jcarpent committed
2
// Copyright (c) 2015-2018 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

Guilhem Saurel's avatar
Guilhem Saurel 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
    
Guilhem Saurel's avatar
Guilhem Saurel committed
66
    /// \brief Placement (SE3) of the input of joint *i* regarding to the parent joint output *li*.
67
    container::aligned_vector<SE3> jointPlacements;
68

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

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

81
82
    /// \brief Vector of rotor inertia parameters
    Eigen::VectorXd rotorInertia;
83
    
84
    /// \brief Vector of rotor gear ratio parameters
85
86
    Eigen::VectorXd rotorGearRatio;
    
87
88
89
90
91
92
93
94
95
96
    /// \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
97
    /// \brief Vector of operational frames registered on the model.
98
    container::aligned_vector<Frame> frames;
jcarpent's avatar
jcarpent committed
99
    
jcarpent's avatar
jcarpent committed
100
    /// \brief Vector of subtrees.
Guilhem Saurel's avatar
Guilhem Saurel committed
101
102
    /// subtree[j] corresponds to the subtree supported by the joint *j*.
    /// The first element of subtree[j] is the index of the joint *j* itself.
jcarpent's avatar
jcarpent committed
103
    std::vector<IndexVector> subtrees;
104

jcarpent's avatar
jcarpent committed
105
    /// \brief Spatial gravity of the model.
jcarpent's avatar
jcarpent committed
106
    Motion gravity;
jcarpent's avatar
jcarpent committed
107
    
jcarpent's avatar
jcarpent committed
108
109
    /// \brief Default 3D gravity vector (=(0,0,-9.81)).
    static const Eigen::Vector3d gravity981;
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
110

111
112
113
    /// \brief Model name;
    std::string name;

jcarpent's avatar
jcarpent committed
114
    /// \brief Default constructor. Builds an empty model with no joints.
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
115
116
117
    Model()
      : nq(0)
      , nv(0)
118
119
120
      , njoints(1)
      , nbodies(1)
      , nframes(0)
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
121
      , inertias(1)
122
      , jointPlacements(1, SE3::Identity())
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
123
      , joints(1)
124
      , parents(1, 0)
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
125
      , names(1)
jcarpent's avatar
jcarpent committed
126
      , subtrees(1)
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
127
128
      , gravity( gravity981,Eigen::Vector3d::Zero() )
    {
129
      names[0]     = "universe";     // Should be "universe joint (trivial)"
130
131
132
      // 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
133
      addFrame(Frame("universe", 0, 0, SE3::Identity(), FIXED_JOINT));
134
135
136
137
      // 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
138
    }
Nicolas Mansard's avatar
Nicolas Mansard committed
139
    ~Model() {} // std::cout << "Destroy model" << std::endl; }
jcarpent's avatar
jcarpent committed
140
141
    
    ///
142
    /// \brief Add a joint to the kinematic tree with given bounds.
jcarpent's avatar
jcarpent committed
143
    ///
144
145
    /// \remark This method does not add a Frame of same name to the vector of frames.
    ///         Use Model::addJointFrame.
jcarpent's avatar
jcarpent committed
146
    /// \remark The inertia supported by the joint is set to Zero.
jcarpent's avatar
jcarpent committed
147
    ///
jcarpent's avatar
jcarpent committed
148
    /// \tparam JointModelDerived The type of the joint model.
jcarpent's avatar
jcarpent committed
149
150
    ///
    /// \param[in] parent Index of the parent joint.
jcarpent's avatar
jcarpent committed
151
152
153
    /// \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.
154
155
156
157
    /// \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.
158
    ///
jcarpent's avatar
jcarpent committed
159
    /// \return The index of the new joint.
160
    ///
161
    /// \sa Model::appendBodyToJoint, Model::addJointFrame
162
    ///
jcarpent's avatar
jcarpent committed
163
164
    template<typename JointModelDerived>
    JointIndex addJoint(const JointIndex parent, const JointModelBase<JointModelDerived> & joint_model, const SE3 & joint_placement,
165
                        const std::string & joint_name,
166
167
168
                        const Eigen::VectorXd & max_effort,
                        const Eigen::VectorXd & max_velocity,
                        const Eigen::VectorXd & min_config,
169
                        const Eigen::VectorXd & max_config
170
171
172
173
                        );

    ///
    /// \brief Add a joint to the kinematic tree with infinite bounds.
jcarpent's avatar
jcarpent committed
174
    ///
jcarpent's avatar
jcarpent committed
175
    /// \remark This method also adds a Frame of same name to the vector of frames.
jcarpent's avatar
jcarpent committed
176
    /// \remark The inertia supported by the joint is set to Zero.
jcarpent's avatar
jcarpent committed
177
    ///
jcarpent's avatar
jcarpent committed
178
    /// \tparam JointModelDerived The type of the joint model.
jcarpent's avatar
jcarpent committed
179
180
    ///
    /// \param[in] parent Index of the parent joint.
jcarpent's avatar
jcarpent committed
181
182
183
    /// \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.
184
    ///
jcarpent's avatar
jcarpent committed
185
    /// \return The index of the new joint.
186
    ///
jcarpent's avatar
jcarpent committed
187
    /// \sa Model::appendBodyToJoint
188
    ///
jcarpent's avatar
jcarpent committed
189
190
    template<typename JointModelDerived>
    JointIndex addJoint(const JointIndex parent, const JointModelBase<JointModelDerived> & joint_model, const SE3 & joint_placement,
191
                        const std::string & joint_name
jcarpent's avatar
jcarpent committed
192
                        );
193
194

    ///
195
    /// \brief Add a joint to the frame tree.
196
    ///
197
198
199
200
    /// \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.
    ///
201
    /// \return The index of the new frame or -1 in case of error.
202
    ///
203
204
    int addJointFrame (const JointIndex& jointIndex,
                             int         frameIndex = -1);
205

206
    ///
jcarpent's avatar
jcarpent committed
207
    /// \brief Append a body to a given joint of the kinematic tree.
208
    ///
jcarpent's avatar
jcarpent committed
209
    /// \param[in] joint_index Index of the supporting joint.
210
    /// \param[in] Y Spatial inertia of the body.
jcarpent's avatar
jcarpent committed
211
    /// \param[in] body_placement The relative placement of the body regarding to the parent joint. Set default to the Identity placement.
212
    ///
jcarpent's avatar
jcarpent committed
213
    /// \sa Model::addJoint
214
    ///
jcarpent's avatar
jcarpent committed
215
    void appendBodyToJoint(const JointIndex joint_index, const Inertia & Y,
216
217
218
219
220
221
222
223
224
225
226
                           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.
    ///
227
    /// \return The index of the new frame or -1 in case of error.
228
    ///
229
230
231
232
    int addBodyFrame (const std::string & body_name,
                      const JointIndex  & parentJoint,
                      const SE3         & body_placement = SE3::Identity(),
                            int           previousFrame  = -1);
233

jcarpent's avatar
jcarpent committed
234
235
    ///
    /// \brief Return the index of a body given by its name.
236
237
238
239
240
    /// 
    /// \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
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
    /// \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.
    ///
260
261
262
    /// \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)
Guilhem Saurel's avatar
Guilhem Saurel committed
263
    /// \param[in] name Name of the joint.
jcarpent's avatar
jcarpent committed
264
    ///
Guilhem Saurel's avatar
Guilhem Saurel committed
265
    /// \return name Name of the joint.
jcarpent's avatar
jcarpent committed
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
    ///
    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.
    ///
285
    PINOCCHIO_DEPRECATED const std::string & getJointName(const JointIndex index) const;
286

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

323
324
325
326
327
328
329
330
331
332
333
334
    /// 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); }

    /// Run check(fusion::list) with DEFAULT_CHECKERS as argument.
jcarpent's avatar
jcarpent committed
335
    inline bool check() const;
336
337
338
339
340
341
342
    
    /// 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;
343

jcarpent's avatar
jcarpent committed
344
345
346
347
348
349
350
  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
351
352
353
354
355
356
357
  };

} // namespace se3

/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
358
#include "pinocchio/multibody/model.hxx"
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
359
360

#endif // ifndef __se3_model_hpp__