model.hxx 14.1 KB
Newer Older
1
//
2
// Copyright (c) 2015-2020 CNRS INRIA
3
4
5
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//

Justin Carpentier's avatar
Justin Carpentier committed
6
7
#ifndef __pinocchio_multibody_model_hxx__
#define __pinocchio_multibody_model_hxx__
8
9

#include "pinocchio/spatial/fwd.hpp"
jcarpent's avatar
jcarpent committed
10
#include "pinocchio/utils/string-generator.hpp"
11
#include "pinocchio/multibody/liegroup/liegroup-algo.hpp"
12

jcarpent's avatar
jcarpent committed
13
14
/// @cond DEV

15
namespace pinocchio
16
{
17
18
  namespace details
  {
19
20
21
    struct FilterFrame
    {
      const std::string & name;
22
      const FrameType & typeMask;
23
24
25
26
27
28
29
      
      FilterFrame(const std::string& name, const FrameType & typeMask)
      : name(name), typeMask(typeMask)
      {}
      
      template<typename Scalar, int Options>
      bool operator()(const FrameTpl<Scalar,Options> & frame) const
30
      { return (typeMask & frame.type) && (name == frame.name); }
31
      
32
33
    };
  }
34
35
36
  
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  const typename ModelTpl<Scalar,Options,JointCollectionTpl>::
37
  Vector3 ModelTpl<Scalar,Options,JointCollectionTpl>::gravity981((Scalar)0,(Scalar)0,(Scalar)-9.81);
38

39
40
41
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline std::ostream& operator<<(std::ostream & os,
                                  const ModelTpl<Scalar,Options,JointCollectionTpl> & model)
42
  {
43
    typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::Index Index;
44
    
45
    os << "Nb joints = " << model.njoints << " (nq="<< model.nq<<",nv="<<model.nv<<")" << std::endl;
46
    for(Index i=0;i<(Index)(model.njoints);++i)
jcarpent's avatar
jcarpent committed
47
    {
48
      os << "  Joint " << i << " " << model.names[i] << ": parent=" << model.parents[i]  << std::endl;
jcarpent's avatar
jcarpent committed
49
50
    }
    
51
52
    return os;
  }
53
  
54
55
56
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex
  ModelTpl<Scalar,Options,JointCollectionTpl>::addJoint(const JointIndex parent,
57
                                                        const JointModel & joint_model,
58
59
60
61
62
                                                        const SE3 & joint_placement,
                                                        const std::string & joint_name,
                                                        const VectorXs & max_effort,
                                                        const VectorXs & max_velocity,
                                                        const VectorXs & min_config,
63
64
65
                                                        const VectorXs & max_config,
                                                        const VectorXs & joint_friction,
                                                        const VectorXs & joint_damping
66
                                                        )
67
  {
68
69
    assert( (njoints==(int)joints.size())&&(njoints==(int)inertias.size())
           &&(njoints==(int)parents.size())&&(njoints==(int)jointPlacements.size()) );
jcarpent's avatar
jcarpent committed
70
    assert((joint_model.nq()>=0) && (joint_model.nv()>=0));
71
    assert(joint_model.nq() >= joint_model.nv());
72

73
74
75
76
77
78
79
    PINOCCHIO_CHECK_ARGUMENT_SIZE(max_effort.size(),joint_model.nv(),"The joint maximum effort vector is not of right size");
    PINOCCHIO_CHECK_ARGUMENT_SIZE(max_velocity.size(),joint_model.nv(),"The joint maximum velocity vector is not of right size");
    PINOCCHIO_CHECK_ARGUMENT_SIZE(min_config.size(),joint_model.nq(),"The joint lower configuration bound is not of right size");
    PINOCCHIO_CHECK_ARGUMENT_SIZE(max_config.size(),joint_model.nq(),"The joint upper configuration bound is not of right size");
    PINOCCHIO_CHECK_ARGUMENT_SIZE(joint_friction.size(),joint_model.nv(),"The joint friction vector is not of right size");
    PINOCCHIO_CHECK_ARGUMENT_SIZE(joint_damping.size(),joint_model.nv(),"The joint damping vector is not of right size");

80
    JointIndex idx = (JointIndex)(njoints++);
81
    
jcarpent's avatar
jcarpent committed
82
    joints         .push_back(JointModel(joint_model.derived()));
83
84
//    JointModelDerived & jmodel = boost::get<JointModelDerived>(joints.back());
    JointModel & jmodel = joints.back();
85
    jmodel.setIndexes(idx,nq,nv);
jcarpent's avatar
jcarpent committed
86
    
87
88
89
90
91
92
93
    const int joint_nq = jmodel.nq();
    const int joint_idx_q = jmodel.idx_q();
    const int joint_nv = jmodel.nv();
    const int joint_idx_v = jmodel.idx_v();
    
    assert(joint_idx_q >= 0);
    assert(joint_idx_v >= 0);
Justin Carpentier's avatar
Justin Carpentier committed
94

95
    inertias       .push_back(Inertia::Zero());
96
    parents        .push_back(parent);
jcarpent's avatar
jcarpent committed
97
    jointPlacements.push_back(joint_placement);
98
    names          .push_back(joint_name);
99
100
101
    
    nq += joint_nq; nqs.push_back(joint_nq); idx_qs.push_back(joint_idx_q);
    nv += joint_nv; nvs.push_back(joint_nv); idx_vs.push_back(joint_idx_v);
102

103
    if(joint_nq > 0 && joint_nv > 0)
104
105
106
107
108
109
110
111
112
113
114
115
116
    {
      effortLimit.conservativeResize(nv);
      jmodel.jointVelocitySelector(effortLimit) = max_effort;
      velocityLimit.conservativeResize(nv);
      jmodel.jointVelocitySelector(velocityLimit) = max_velocity;
      lowerPositionLimit.conservativeResize(nq);
      jmodel.jointConfigSelector(lowerPositionLimit) = min_config;
      upperPositionLimit.conservativeResize(nq);
      jmodel.jointConfigSelector(upperPositionLimit) = max_config;
      
      rotorInertia.conservativeResize(nv);
      jmodel.jointVelocitySelector(rotorInertia).setZero();
      rotorGearRatio.conservativeResize(nv);
117
      jmodel.jointVelocitySelector(rotorGearRatio).setOnes();
118
119
120
121
      friction.conservativeResize(nv);
      jmodel.jointVelocitySelector(friction) = joint_friction;
      damping.conservativeResize(nv);
      jmodel.jointVelocitySelector(damping) = joint_damping;
122
    }
jcarpent's avatar
jcarpent committed
123
    
jcarpent's avatar
jcarpent committed
124
125
126
127
    // Init and add joint index to its parent subtrees.
    subtrees.push_back(IndexVector(1));
    subtrees[idx][0] = idx;
    addJointIndexToParentSubtrees(idx);
128
129
130
131
132
    
    // Init and add joint index to the supports
    supports.push_back(supports[parent]);
    supports[idx].push_back(idx);
    
133
134
    return idx;
  }
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
    
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex
  ModelTpl<Scalar,Options,JointCollectionTpl>::addJoint(const JointIndex parent,
                                                        const JointModel & joint_model,
                                                        const SE3 & joint_placement,
                                                        const std::string & joint_name,
                                                        const VectorXs & max_effort,
                                                        const VectorXs & max_velocity,
                                                        const VectorXs & min_config,
                                                        const VectorXs & max_config)
  {
    const VectorXs friction = VectorXs::Constant(joint_model.nv(), static_cast<Scalar>(0));
    const VectorXs damping = VectorXs::Constant(joint_model.nv(), static_cast<Scalar>(0));
    
    return addJoint(parent, joint_model,
                    joint_placement, joint_name,
                    max_effort, max_velocity, min_config, max_config,
                    friction, damping);
  }
155

156
157
158
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex
  ModelTpl<Scalar,Options,JointCollectionTpl>::addJoint(const JointIndex parent,
159
                                                        const JointModel & joint_model,
160
161
                                                        const SE3 & joint_placement,
                                                        const std::string & joint_name)
162
  {
163
164
165
166
    const VectorXs max_effort = VectorXs::Constant(joint_model.nv(), std::numeric_limits<Scalar>::max());
    const VectorXs max_velocity = VectorXs::Constant(joint_model.nv(), std::numeric_limits<Scalar>::max());
    const VectorXs min_config = VectorXs::Constant(joint_model.nq(), -std::numeric_limits<Scalar>::max());
    const VectorXs max_config = VectorXs::Constant(joint_model.nq(), std::numeric_limits<Scalar>::max());
167

168
169
    return addJoint(parent, joint_model, joint_placement, joint_name,
                    max_effort, max_velocity, min_config, max_config);
170
171
  }
  
172
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
173
174
  inline typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex
  ModelTpl<Scalar,Options,JointCollectionTpl>::
175
176
  addJointFrame(const JointIndex & joint_index,
                int previous_frame_index)
177
  {
178
179
180
181
    PINOCCHIO_CHECK_INPUT_ARGUMENT(joint_index >= 0 && joint_index < joints.size(),
                                   "The joint index is larger than the number of joints in the model.");
    if(previous_frame_index < 0)
    {
182
183
      // FIXED_JOINT is required because the parent can be the universe and its
      // type is FIXED_JOINT
184
      previous_frame_index = (int)getFrameId(names[parents[joint_index]], (FrameType)(JOINT | FIXED_JOINT));
185
    }
186
187
    assert((size_t)previous_frame_index < frames.size() && "Frame index out of bound");
    
188
    // Add a the joint frame attached to itself to the frame vector - redundant information but useful.
189
    return addFrame(Frame(names[joint_index],joint_index,(FrameIndex)previous_frame_index,SE3::Identity(),JOINT));
190
191
  }

192
193
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline void ModelTpl<Scalar,Options,JointCollectionTpl>::
194
  appendBodyToJoint(const typename ModelTpl::JointIndex joint_index,
195
196
                    const Inertia & Y,
                    const SE3 & body_placement)
197
  {
jcarpent's avatar
jcarpent committed
198
199
    const Inertia & iYf = Y.se3Action(body_placement);
    inertias[joint_index] += iYf;
200
    nbodies++;
201
  }
202

203
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
204
205
  inline typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex
  ModelTpl<Scalar,Options,JointCollectionTpl>::
206
207
208
209
  addBodyFrame(const std::string & body_name,
               const JointIndex  & parentJoint,
               const SE3         & body_placement,
               int           previousFrame)
210
  {
jcarpent's avatar
jcarpent committed
211
    if(previousFrame < 0) {
212
213
      // FIXED_JOINT is required because the parent can be the universe and its
      // type is FIXED_JOINT
214
      previousFrame = (int)getFrameId(names[parentJoint], (FrameType)(JOINT | FIXED_JOINT));
215
    }
216
217
    assert((size_t)previousFrame < frames.size() && "Frame index out of bound");
    return addFrame(Frame(body_name, parentJoint, (FrameIndex)previousFrame, body_placement, BODY));
218
  }
jcarpent's avatar
jcarpent committed
219
  
220
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
221
  inline typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex
222
223
  ModelTpl<Scalar,Options,JointCollectionTpl>::
  getBodyId(const std::string & name) const
224
  {
225
    return getFrameId(name, BODY);
226
227
  }
  
228
229
230
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline bool ModelTpl<Scalar,Options,JointCollectionTpl>::
  existBodyName(const std::string & name) const
231
  {
232
    return existFrame(name, BODY);
233
234
  }

235
236
237
238
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline typename  ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex
  ModelTpl<Scalar,Options,JointCollectionTpl>::
  getJointId(const std::string & name) const
239
  {
jcarpent's avatar
jcarpent committed
240
241
    typedef std::vector<std::string>::iterator::difference_type it_diff_t;
    it_diff_t res = std::find(names.begin(),names.end(),name) - names.begin();
jcarpent's avatar
jcarpent committed
242
    assert((res<INT_MAX) && "Id superior to int range. Should never happen.");
243
    return ModelTpl::JointIndex(res);
244
245
  }
  
246
247
248
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline bool ModelTpl<Scalar,Options,JointCollectionTpl>::
  existJointName(const std::string & name) const
249
250
251
252
  {
    return (names.end() != std::find(names.begin(),names.end(),name));
  }

253
254
255
256
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex
  ModelTpl<Scalar,Options,JointCollectionTpl>::
  getFrameId(const std::string & name, const FrameType & type) const
257
  {
258
    typename PINOCCHIO_ALIGNED_STD_VECTOR(Frame)::const_iterator it
259
260
261
    = std::find_if(frames.begin()
                   ,frames.end()
                   ,details::FilterFrame(name, type));
262
263
    assert(((it == frames.end()) ||
            (std::find_if( boost::next(it), frames.end(), details::FilterFrame(name, type)) == frames.end()))
264
        && "Several frames match the filter");
265
    return FrameIndex(it - frames.begin());
266
  }
267
  
268
269
270
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline bool ModelTpl<Scalar,Options,JointCollectionTpl>::
  existFrame(const std::string & name, const FrameType & type) const
271
  {
272
273
    return std::find_if(frames.begin(), frames.end(),
                        details::FilterFrame(name, type)) != frames.end();
274
275
  }

276
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
277
278
  inline typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex
  ModelTpl<Scalar,Options,JointCollectionTpl>::
279
  addFrame(const Frame & frame, const bool append_inertia)
280
  {
Justin Carpentier's avatar
Justin Carpentier committed
281
    PINOCCHIO_CHECK_INPUT_ARGUMENT(frame.parent < (JointIndex)njoints,
282
                                   "The index of the parent joint is not valid.");
283
    
284
285
286
//    TODO: fix it
//    PINOCCHIO_CHECK_INPUT_ARGUMENT(frame.inertia.isValid(),
//                                   "The input inertia is not valid.")
287
    
288
289
    // Check if the frame.name exists with the same type
    if(existFrame(frame.name,frame.type))
290
      return getFrameId(frame.name,frame.type);
291
    
292
    frames.push_back(frame);
293
    if(append_inertia)
294
      inertias[frame.parent] += frame.placement.act(frame.inertia);
295
    nframes++;
296
    return FrameIndex(nframes - 1);
297
  }
jcarpent's avatar
jcarpent committed
298
  
299
300
301
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline void ModelTpl<Scalar,Options,JointCollectionTpl>::
  addJointIndexToParentSubtrees(const JointIndex joint_id)
jcarpent's avatar
jcarpent committed
302
303
304
  {
    for(JointIndex parent = parents[joint_id]; parent>0; parent = parents[parent])
      subtrees[parent].push_back(joint_id);
305
306
307
    
    // Also add joint_id to the universe
    subtrees[0].push_back(joint_id);
jcarpent's avatar
jcarpent committed
308
  }
309

310
} // namespace pinocchio
311

jcarpent's avatar
jcarpent committed
312
313
/// @endcond

Justin Carpentier's avatar
Justin Carpentier committed
314
#endif // ifndef __pinocchio_multibody_model_hxx__