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

#ifndef __se3_model_hxx__
#define __se3_model_hxx__

#include "pinocchio/spatial/fwd.hpp"
jcarpent's avatar
jcarpent committed
23
#include "pinocchio/tools/string-generator.hpp"
24

25
#include <boost/bind.hpp>
26
#include <boost/utility.hpp>
27

jcarpent's avatar
jcarpent committed
28
29
/// @cond DEV

30
31
namespace se3
{
32
33
34
35
36
37
38
39
40
41
42
43
  namespace details
  {
    struct FilterFrame {
      const std::string& name;
      const FrameType & typeMask;
      FilterFrame (const std::string& name, const FrameType& typeMask)
        : name (name), typeMask (typeMask) {}
      bool operator() (const Frame& frame) const
      { return (typeMask & frame.type) && (name == frame.name); }
    };
  }

44
45
  inline std::ostream& operator<< (std::ostream & os, const Model & model)
  {
46
47
    os << "Nb joints = " << model.njoints << " (nq="<< model.nq<<",nv="<<model.nv<<")" << std::endl;
    for(Model::Index i=0;i<(Model::Index)(model.njoints);++i)
jcarpent's avatar
jcarpent committed
48
49
50
51
    {
      os << "  Joint "<< model.names[i] << ": parent=" << model.parents[i]  << std::endl;
    }
    
52
53
    return os;
  }
54
  
jcarpent's avatar
jcarpent committed
55
56
57
58
  template<typename JointModelDerived>
  Model::JointIndex Model::addJoint(const Model::JointIndex parent,
                                    const JointModelBase<JointModelDerived> & joint_model,
                                    const SE3 & joint_placement,
59
                                    const std::string & joint_name,
jcarpent's avatar
jcarpent committed
60
61
62
                                    const Eigen::VectorXd & max_effort,
                                    const Eigen::VectorXd & max_velocity,
                                    const Eigen::VectorXd & min_config,
63
                                    const Eigen::VectorXd & max_config
jcarpent's avatar
jcarpent committed
64
                                    )
65
  {
66
67
    assert( (njoints==(int)joints.size())&&(njoints==(int)inertias.size())
           &&(njoints==(int)parents.size())&&(njoints==(int)jointPlacements.size()) );
jcarpent's avatar
jcarpent committed
68
    assert((joint_model.nq()>=0) && (joint_model.nv()>=0));
69

jcarpent's avatar
jcarpent committed
70
71
72
73
    assert(max_effort.size() == joint_model.nv()
           && max_velocity.size() == joint_model.nv()
           && min_config.size() == joint_model.nq()
           && max_config.size() == joint_model.nq());
74
75
76
    
    Model::JointIndex idx = (Model::JointIndex) (njoints++);
    
jcarpent's avatar
jcarpent committed
77
    joints         .push_back(JointModel(joint_model.derived()));
78
79
    JointModelDerived & jmodel = boost::get<JointModelDerived>(joints.back());
    jmodel.setIndexes(idx,nq,nv);
jcarpent's avatar
jcarpent committed
80
    
81
    inertias       .push_back(Inertia::Zero());
82
    parents        .push_back(parent);
jcarpent's avatar
jcarpent committed
83
    jointPlacements.push_back(joint_placement);
84
    names          .push_back(joint_name);
jcarpent's avatar
jcarpent committed
85
86
    nq += joint_model.nq();
    nv += joint_model.nv();
87

88
89
90
    // Optimal efficiency here would be using the static-dim bottomRows, while specifying the dimension in argument in the case where D::NV is Eigen::Dynamic.
    // However, this option is not compiling in Travis (why?).
    // As efficiency of Model::addJoint is not critical, the dynamic bottomRows is used here.
91
92
93
94
95
96
97
98
    effortLimit.conservativeResize(nv);
    jmodel.jointConfigSelector(effortLimit) = max_effort;
    velocityLimit.conservativeResize(nv);
    jmodel.jointVelocitySelector(effortLimit) = max_velocity;
    lowerPositionLimit.conservativeResize(nq);
    jmodel.jointConfigSelector(lowerPositionLimit) = min_config;
    upperPositionLimit.conservativeResize(nq);
    jmodel.jointConfigSelector(upperPositionLimit) = max_config;
99
    
100
    neutralConfiguration.conservativeResize(nq);
jcarpent's avatar
jcarpent committed
101
    neutralConfiguration.tail(joint_model.nq()) = joint_model.neutralConfiguration();
102

103
    rotorInertia.conservativeResize(nv);
104
    jmodel.jointVelocitySelector(rotorInertia).setZero();
105
    rotorGearRatio.conservativeResize(nv);
106
    jmodel.jointVelocitySelector(rotorGearRatio).setZero();
jcarpent's avatar
jcarpent committed
107
    
jcarpent's avatar
jcarpent committed
108
109
110
111
    // Init and add joint index to its parent subtrees.
    subtrees.push_back(IndexVector(1));
    subtrees[idx][0] = idx;
    addJointIndexToParentSubtrees(idx);
112
113
114
    return idx;
  }

115
116
117
118
119
120
121
122
123
  template<typename JointModelDerived>
  Model::JointIndex Model::addJoint(const Model::JointIndex parent,
                                    const JointModelBase<JointModelDerived> & joint_model,
                                    const SE3 & joint_placement,
                                    const std::string & joint_name
                                    )
  {
    Eigen::VectorXd max_effort, max_velocity, min_config, max_config;

124
125
126
127
128
    max_effort = Eigen::VectorXd::Constant(joint_model.nv(), std::numeric_limits<double>::max());
    max_velocity = Eigen::VectorXd::Constant(joint_model.nv(), std::numeric_limits<double>::max());
    min_config = Eigen::VectorXd::Constant(joint_model.nq(), std::numeric_limits<double>::max());
    max_config = Eigen::VectorXd::Constant(joint_model.nq(), std::numeric_limits<double>::max());

129
    return addJoint(parent, joint_model, joint_placement, joint_name, max_effort, max_velocity, min_config, max_config);
130
131
  }
  
132
133
  inline int Model::addJointFrame (const JointIndex& jidx,
                                         int         fidx)
134
135
  {
    if (fidx < 0) {
136
137
      // FIXED_JOINT is required because the parent can be the universe and its
      // type is FIXED_JOINT
138
      fidx = (int)getFrameId(names[parents[jidx]], (FrameType)(JOINT | FIXED_JOINT));
139
    }
140
    assert((size_t)fidx < frames.size() && "Frame index out of bound");
141
    // Add a the joint frame attached to itself to the frame vector - redundant information but useful.
142
    return addFrame(Frame(names[jidx],jidx,(FrameIndex)fidx,SE3::Identity(),JOINT));
143
144
  }

145

jcarpent's avatar
jcarpent committed
146
147
  inline void Model::appendBodyToJoint(const Model::JointIndex joint_index,
                                       const Inertia & Y,
148
                                       const SE3 & body_placement)
149
  {
jcarpent's avatar
jcarpent committed
150
151
    const Inertia & iYf = Y.se3Action(body_placement);
    inertias[joint_index] += iYf;
152
    nbodies++;
153
  }
154

155
156
157
158
  inline int Model::addBodyFrame (const std::string & body_name,
                                  const JointIndex  & parentJoint,
                                  const SE3         & body_placement,
                                        int           previousFrame)
159
160
  {
    if (previousFrame < 0) {
161
162
      // FIXED_JOINT is required because the parent can be the universe and its
      // type is FIXED_JOINT
163
      previousFrame = (int)getFrameId(names[parentJoint], (FrameType)(JOINT | FIXED_JOINT));
164
    }
165
166
    assert((size_t)previousFrame < frames.size() && "Frame index out of bound");
    return addFrame(Frame(body_name, parentJoint, (FrameIndex)previousFrame, body_placement, BODY));
167
  }
jcarpent's avatar
jcarpent committed
168
  
169
  inline Model::JointIndex Model::getBodyId (const std::string & name) const
170
  {
171
    return getFrameId(name, BODY);
172
173
174
175
  }
  
  inline bool Model::existBodyName (const std::string & name) const
  {
176
    return existFrame(name, BODY);
177
178
179
  }


180
  inline Model::JointIndex Model::getJointId (const std::string & name) const
181
  {
jcarpent's avatar
jcarpent committed
182
183
    typedef std::vector<std::string>::iterator::difference_type it_diff_t;
    it_diff_t res = std::find(names.begin(),names.end(),name) - names.begin();
184
    assert( (res<INT_MAX) && "Id superior to int range. Should never happen.");
185
    return Model::JointIndex(res);
186
187
188
189
190
191
192
  }
  
  inline bool Model::existJointName (const std::string & name) const
  {
    return (names.end() != std::find(names.begin(),names.end(),name));
  }

193
  inline const std::string& Model::getJointName (const JointIndex index) const
194
  {
195
    assert( index < (Model::JointIndex)joints.size() );
196
197
198
    return names[index];
  }

199
  inline Model::FrameIndex Model::getFrameId ( const std::string & name, const FrameType & type ) const
200
  {
201
    container::aligned_vector<Frame>::const_iterator it = std::find_if( frames.begin()
202
                                                        , frames.end()
203
                                                        , details::FilterFrame (name, type)
204
                                                        );
205
206
207
    assert (it != frames.end() && "Frame not found");
    assert ((std::find_if( boost::next(it), frames.end(), details::FilterFrame (name, type)) == frames.end())
        && "Several frames match the filter");
208
    return Model::FrameIndex(it - frames.begin());
209
210
  }

211
  inline bool Model::existFrame ( const std::string & name, const FrameType & type) const
212
  {
213
214
    return std::find_if( frames.begin(), frames.end(),
        details::FilterFrame (name, type)) != frames.end();
215
216
217
  }


218
  inline int Model::addFrame ( const Frame & frame )
219
  {
220
    if( !existFrame(frame.name, frame.type) )
221
    {
222
      frames.push_back(frame);
223
224
      nframes++;
      return nframes - 1;
225
226
227
    }
    else
    {
228
      return -1;
229
    }
230
  }
jcarpent's avatar
jcarpent committed
231
232
233
234
235
236
  
  inline void Model::addJointIndexToParentSubtrees(const JointIndex joint_id)
  {
    for(JointIndex parent = parents[joint_id]; parent>0; parent = parents[parent])
      subtrees[parent].push_back(joint_id);
  }
237
238


239
  inline Data::Data (const Model & model)
240
    :joints(0)
241
    ,a((std::size_t)model.njoints)
242
    ,oa((std::size_t)model.njoints)
243
244
    ,a_gf((std::size_t)model.njoints)
    ,v((std::size_t)model.njoints)
245
    ,ov((std::size_t)model.njoints)
246
    ,f((std::size_t)model.njoints)
247
    ,of((std::size_t)model.njoints)
248
    ,h((std::size_t)model.njoints)
249
    ,oh((std::size_t)model.njoints)
250
251
    ,oMi((std::size_t)model.njoints)
    ,liMi((std::size_t)model.njoints)
252
253
    ,tau(model.nv)
    ,nle(model.nv)
254
    ,g(model.nv)
255
256
    ,oMf((std::size_t)model.nframes)
    ,Ycrb((std::size_t)model.njoints)
257
    ,dYcrb((std::size_t)model.njoints)
258
    ,M(model.nv,model.nv)
259
    ,Minv(model.nv,model.nv)
260
    ,C(model.nv,model.nv)
261
    ,dFdq(6,model.nv)
262
    ,dFdv(6,model.nv)
263
    ,dFda(6,model.nv)
264
265
266
    ,SDinv(6,model.nv)
    ,UDinv(6,model.nv)
    ,IS(6,model.nv)
267
268
    ,vxI((std::size_t)model.njoints)
    ,Ivx((std::size_t)model.njoints)
jcarpent's avatar
jcarpent committed
269
270
    ,oYcrb((std::size_t)model.njoints)
    ,doYcrb((std::size_t)model.njoints)
271
    ,ddq(model.nv)
272
    ,Yaba((std::size_t)model.njoints)
273
274
    ,u(model.nv)
    ,Ag(6,model.nv)
275
    ,dAg(6,model.nv)
276
277
278
    ,Fcrb((std::size_t)model.njoints)
    ,lastChild((std::size_t)model.njoints)
    ,nvSubtree((std::size_t)model.njoints)
279
280
    ,U(model.nv,model.nv)
    ,D(model.nv)
281
    ,Dinv(model.nv)
282
283
284
285
    ,tmp(model.nv)
    ,parents_fromRow((std::size_t)model.nv)
    ,nvSubtree_fromRow((std::size_t)model.nv)
    ,J(6,model.nv)
286
    ,dJ(6,model.nv)
287
288
289
    ,dVdq(6,model.nv)
    ,dAdq(6,model.nv)
    ,dAdv(6,model.nv)
290
291
    ,dtau_dq(Eigen::MatrixXd::Zero(model.nv,model.nv))
    ,dtau_dv(Eigen::MatrixXd::Zero(model.nv,model.nv))
292
293
294
295
296
    ,iMf((std::size_t)model.njoints)
    ,com((std::size_t)model.njoints)
    ,vcom((std::size_t)model.njoints)
    ,acom((std::size_t)model.njoints)
    ,mass((std::size_t)model.njoints)
297
    ,Jcom(3,model.nv)
298
299
    ,JMinvJt()
    ,llt_JMinvJt()
300
    ,lambda_c()
301
302
303
    ,sDUiJt(model.nv,model.nv)
    ,torque_residual(model.nv)
    ,dq_after(model.nv)
304
    ,impulse_c()
305
    ,staticRegressor(3,4*(model.njoints-1))
306
307
  {
    /* Create data strcture associated to the joints */
308
    for(Model::Index i=0;i<(Model::JointIndex)(model.njoints);++i) 
309
      joints.push_back(CreateJointData::run(model.joints[i]));
310
311

    /* Init for CRBA */
312
    M.fill(0); Minv.setZero();
313
    for(Model::Index i=0;i<(Model::Index)(model.njoints);++i ) { Fcrb[i].resize(6,model.nv); }
314
    computeLastChild(model);
315
316

    /* Init for Cholesky */
317
    U.setIdentity();
318
    computeParents_fromRow(model);
319
320

    /* Init Jacobian */
jcarpent's avatar
jcarpent committed
321
322
    J.setZero();
    Ag.setZero();
323
324
325
326
    
    /* Init universe states relatively to itself */
    
    a[0].setZero();
327
    oa[0].setZero();
328
    v[0].setZero();
329
    ov[0].setZero();
330
    a_gf[0] = -model.gravity;
331
    f[0].setZero();
332
    h[0].setZero();
333
334
    oMi[0].setIdentity();
    liMi[0].setIdentity();
335
    oMf[0].setIdentity();
336
337
338
339
340
341
  }

  inline void Data::computeLastChild (const Model & model)
  {
    typedef Model::Index Index;
    std::fill(lastChild.begin(),lastChild.end(),-1);
342
    for( int i=model.njoints-1;i>=0;--i )
343
344
345
346
347
348
349
350
351
352
353
354
355
    {
      if(lastChild[(Index)i] == -1) lastChild[(Index)i] = i;
      const Index & parent = model.parents[(Index)i];
      lastChild[parent] = std::max(lastChild[(Index)i],lastChild[parent]);
      
      nvSubtree[(Index)i]
      = idx_v(model.joints[(Index)lastChild[(Index)i]]) + nv(model.joints[(Index)lastChild[(Index)i]])
      - idx_v(model.joints[(Index)i]);
    }
  }

  inline void Data::computeParents_fromRow (const Model & model)
  {
356
    for( Model::Index joint=1;joint<(Model::Index)(model.njoints);joint++)
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
    {
      const Model::Index & parent = model.parents[joint];
      const int nvj    = nv   (model.joints[joint]);
      const int idx_vj = idx_v(model.joints[joint]);
      
      if(parent>0) parents_fromRow[(Model::Index)idx_vj] = idx_v(model.joints[parent])+nv(model.joints[parent])-1;
      else         parents_fromRow[(Model::Index)idx_vj] = -1;
      nvSubtree_fromRow[(Model::Index)idx_vj] = nvSubtree[joint];
      
      for(int row=1;row<nvj;++row)
      {
        parents_fromRow[(Model::Index)(idx_vj+row)] = idx_vj+row-1;
        nvSubtree_fromRow[(Model::Index)(idx_vj+row)] = nvSubtree[joint]-row;
      }
    }
  }

} // namespace se3

jcarpent's avatar
jcarpent committed
376
377
/// @endcond

378
#endif // ifndef __se3_model_hxx__