model.hxx 14.2 KB
Newer Older
1
//
jcarpent's avatar
jcarpent committed
2
// Copyright (c) 2015-2016 CNRS
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
// 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"
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/spatial/force.hpp"
#include "pinocchio/spatial/motion.hpp"
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/multibody/joint/joint-variant.hpp"
28
#include "pinocchio/multibody/joint/joint-basic-visitors.hpp"
29
30
#include <iostream>

31
32
#include <boost/bind.hpp>

jcarpent's avatar
jcarpent committed
33
34
/// @cond DEV

35
36
37
38
39
40
41
namespace se3
{
  inline std::ostream& operator<< (std::ostream & os, const Model & model)
  {
    os << "Nb bodies = " << model.nbody << " (nq="<< model.nq<<",nv="<<model.nv<<")" << std::endl;
    for(Model::Index i=0;i<(Model::Index)(model.nbody);++i)
      {
42
	os << "  Joint "<<model.names[i] << ": parent=" << model.parents[i]  << std::endl;
43
44
45
46
      }

    return os;
  }
47
  
48

49
50

  template<typename D>
51
  Model::JointIndex Model::addJointAndBody(JointIndex parent, const JointModelBase<D> & j, const SE3 & jointPlacement,
52
53
54
                                           const Inertia & Y, const std::string & jointName,
                                           const std::string & bodyName)
  {
55
    JointIndex idx = addJoint(parent,j,jointPlacement,jointName);
56
57
58
59
    appendBodyToJoint(idx, SE3::Identity(), Y, bodyName);
    return idx;
  }

60
  template<typename D>
61
  Model::JointIndex Model::addJointAndBody(JointIndex parent, const JointModelBase<D> & j, const SE3 & jointPlacement,
62
63
64
65
66
                                           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)
67
  {
68
    JointIndex idx = addJoint(parent,j,jointPlacement,
69
70
71
72
73
74
75
76
                              effort, velocity, lowPos, upPos,
                              jointName);
    appendBodyToJoint(idx, SE3::Identity(), Y, bodyName);
    return idx;
  }


  template<typename D>
77
  Model::JointIndex Model::addJoint(JointIndex parent, const JointModelBase<D> & j, const SE3 & jointPlacement,
78
79
80
81
                                    const std::string & jointName)
  {
    assert( (njoint==(int)joints.size())&&(njoint==(int)inertias.size())
      &&(njoint==(int)parents.size())&&(njoint==(int)jointPlacements.size()) );
82
83
    assert( (j.nq()>=0)&&(j.nv()>=0) );

84
    Model::JointIndex idx = (Model::JointIndex) (njoint ++);
85

86
    joints         .push_back(JointModel(j.derived())); 
87
    boost::get<D>(joints.back()).setIndexes(idx,nq,nv);
88

89
    inertias       .push_back(Inertia::Zero());
90
    parents        .push_back(parent);
91
    jointPlacements.push_back(jointPlacement);
92
93
94
    names          .push_back( (jointName!="")?jointName:random(8) );
    nq += j.nq();
    nv += j.nv();
95

96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
    neutralConfiguration.conservativeResize(nq);
    neutralConfiguration.bottomRows<D::NQ>() = j.neutralConfiguration();

    effortLimit.conservativeResize(nv);
    effortLimit.bottomRows<D::NV>().fill(std::numeric_limits<double>::infinity());

    velocityLimit.conservativeResize(nv);
    velocityLimit.bottomRows<D::NV>().fill(std::numeric_limits<double>::infinity());

    lowerPositionLimit.conservativeResize(nq);
    lowerPositionLimit.bottomRows<D::NQ>().fill(-std::numeric_limits<double>::infinity());

    upperPositionLimit.conservativeResize(nq);
    upperPositionLimit.bottomRows<D::NQ>().fill(std::numeric_limits<double>::infinity());

111
112
113
114
    return idx;
  }

  template<typename D>
115
  Model::JointIndex Model::addJoint(JointIndex parent,const JointModelBase<D> & j,const SE3 & jointPlacement,
116
117
118
                     const Eigen::VectorXd & effort, const Eigen::VectorXd & velocity,
                     const Eigen::VectorXd & lowPos, const Eigen::VectorXd & upPos,
                     const std::string & jointName)
119
  {
120
121
    assert( (njoint==(int)joints.size())&&(njoint==(int)inertias.size())
      &&(njoint==(int)parents.size())&&(njoint==(int)jointPlacements.size()) );
122
123
    assert( (j.nq()>=0)&&(j.nv()>=0) );

124
125
126
    assert( effort.size() == j.nv() && velocity.size() == j.nv()
           && lowPos.size() == j.nq() && upPos.size() == j.nq() );

127
    Model::JointIndex idx = (Model::JointIndex) (njoint ++);
128

129
    joints         .push_back(JointModel(j.derived())); 
130
    boost::get<D>(joints.back()).setIndexes(idx,nq,nv);
131

132
    inertias       .push_back(Inertia::Zero());
133
    parents        .push_back(parent);
134
    jointPlacements.push_back(jointPlacement);
135
136
137
    names          .push_back( (jointName!="")?jointName:random(8) );
    nq += j.nq();
    nv += j.nv();
138

139
    
140
141
142
143
    effortLimit.conservativeResize(nv);effortLimit.bottomRows<D::NV>() = effort;
    velocityLimit.conservativeResize(nv);velocityLimit.bottomRows<D::NV>() = velocity;
    lowerPositionLimit.conservativeResize(nq);lowerPositionLimit.bottomRows<D::NQ>() = lowPos;
    upperPositionLimit.conservativeResize(nq);upperPositionLimit.bottomRows<D::NQ>() = upPos;
144

145
146
147
    // Ensure that limits are not inf
    Eigen::VectorXd neutralConf((lowerPositionLimit.bottomRows<D::NQ>() + upperPositionLimit.bottomRows<D::NQ>())/2 );
    
148
    neutralConfiguration.conservativeResize(nq);
149
150
    if ( std::isfinite(neutralConf.norm()) )
    {
151
      neutralConfiguration.bottomRows<D::NQ>() = neutralConf;
152
153
154
155
    }
    else
    {
      assert( false && "One of the position limit is inf or NaN");
156
      neutralConfiguration.bottomRows<D::NQ>() = j.neutralConfiguration();
157
158
    }

159
160
    addFrame((jointName!="")?jointName:random(8), idx, SE3::Identity(), JOINT);

161
162
163
    return idx;
  }

164

165
  inline void Model::appendBodyToJoint(const JointIndex parent, const SE3 & bodyPlacement, const Inertia & Y,
166
167
                                       const std::string & bodyName)
  {
168
    const Inertia & iYf = Y.se3Action(bodyPlacement);
169
170
    inertias[parent] += iYf;

171
    addFrame((bodyName!="")?bodyName:random(8), parent, bodyPlacement, BODY);
172
173
174

    nbody ++;
  }
jcarpent's avatar
jcarpent committed
175
  
176
  inline Model::JointIndex Model::getBodyId (const std::string & name) const
177
  {
178
    return getFrameId(name);
179
180
181
182
  }
  
  inline bool Model::existBodyName (const std::string & name) const
  {
183
    return existFrame(name);
184
185
  }

186
  inline const std::string& Model::getBodyName (const Model::JointIndex index) const
187
188
  {
    assert( index < (Model::Index)nbody );
189
    return getFrameName(index);
190
191
  }

192
  inline Model::JointIndex Model::getJointId (const std::string & name) const
193
  {
jcarpent's avatar
jcarpent committed
194
195
    typedef std::vector<std::string>::iterator::difference_type it_diff_t;
    it_diff_t res = std::find(names.begin(),names.end(),name) - names.begin();
196
    assert( (res<INT_MAX) && "Id superior to int range. Should never happen.");
jcarpent's avatar
jcarpent committed
197
    assert( (res>=0)&&(res<(it_diff_t) joints.size()) && "The joint name you asked does not exist" );
198
    return Model::JointIndex(res);
199
200
201
202
203
204
205
  }
  
  inline bool Model::existJointName (const std::string & name) const
  {
    return (names.end() != std::find(names.begin(),names.end(),name));
  }

206
  inline const std::string& Model::getJointName (const JointIndex index) const
207
  {
208
    assert( index < (Model::JointIndex)joints.size() );
209
210
211
    return names[index];
  }

212
  inline Model::FrameIndex Model::getFrameId ( const std::string & name ) const
213
  {
214
215
    std::vector<Frame>::const_iterator it = std::find_if( frames.begin()
                                                        , frames.end()
216
217
                                                        , boost::bind(&Frame::name, _1) == name
                                                        );
218
    return Model::FrameIndex(it - frames.begin());
219
220
221
222
  }

  inline bool Model::existFrame ( const std::string & name ) const
  {
223
    return std::find_if( frames.begin(), frames.end(), boost::bind(&Frame::name, _1) == name) != frames.end();
224
225
  }

226
  inline const std::string & Model::getFrameName ( const FrameIndex index ) const
227
  {
228
    return frames[index].name;
229
230
  }

jcarpent's avatar
jcarpent committed
231
  inline Model::JointIndex Model::getFrameParent( const std::string & name ) const
232
  {
233
    assert(existFrame(name) && "The Frame you requested does not exist");
234
235
    std::vector<Frame>::const_iterator it = std::find_if( frames.begin()
                                                        , frames.end()
236
237
                                                        , boost::bind(&Frame::name, _1) == name
                                                        );
238
    
239
    std::vector<Frame>::iterator::difference_type it_diff = it - frames.begin();
240
    return getFrameParent(Model::JointIndex(it_diff));
241
242
  }

jcarpent's avatar
jcarpent committed
243
  inline Model::JointIndex Model::getFrameParent( const FrameIndex index ) const
244
  {
245
    return frames[index].parent;
246
247
  }

248
249
250
  inline FrameType Model::getFrameType( const std::string & name ) const
  {
    assert(existFrame(name) && "The Frame you requested does not exist");
251
252
    std::vector<Frame>::const_iterator it = std::find_if( frames.begin()
                                                        , frames.end()
253
254
255
                                                        , boost::bind(&Frame::name, _1) == name
                                                        );
    
256
    std::vector<Frame>::iterator::difference_type it_diff = it - frames.begin();
257
258
259
260
261
    return getFrameType(Model::JointIndex(it_diff));
  }

  inline FrameType Model::getFrameType( const FrameIndex index ) const
  {
262
    return frames[index].type;
263
264
  }

265
  inline const SE3 & Model::getFramePlacement( const std::string & name) const
266
267
  {
    assert(existFrame(name) && "The Frame you requested does not exist");
268
269
    std::vector<Frame>::const_iterator it = std::find_if( frames.begin()
                                                        , frames.end()
270
271
                                                        , boost::bind(&Frame::name, _1) == name
                                                        );
272
    
273
    std::vector<Frame>::iterator::difference_type it_diff = it - frames.begin();
274
    return getFramePlacement(Model::Index(it_diff));
275
276
  }

277
  inline const SE3 & Model::getFramePlacement( const FrameIndex index ) const
278
  {
279
    return frames[index].placement;
280
281
  }

282
  inline bool Model::addFrame ( const Frame & frame )
283
284
  {
    if( !existFrame(frame.name) )
285
    {
286
287
      frames.push_back(frame);
      nFrames++;
288
289
290
291
292
293
      return true;
    }
    else
    {
      return false;
    }
294
295
  }

296
  inline bool Model::addFrame ( const std::string & name, JointIndex index, const SE3 & placement, const FrameType type)
297
298
  {
    if( !existFrame(name) )
299
      return addFrame(Frame(name, index, placement, type));
300
301
    else
      return false;
302
303
304
  }


305
306
307
308
  inline Data::Data (const Model & ref)
    :model(ref)
    ,joints(0)
    ,a((std::size_t)ref.nbody)
309
    ,a_gf((std::size_t)ref.nbody)
310
311
312
313
314
315
    ,v((std::size_t)ref.nbody)
    ,f((std::size_t)ref.nbody)
    ,oMi((std::size_t)ref.nbody)
    ,liMi((std::size_t)ref.nbody)
    ,tau(ref.nv)
    ,nle(ref.nv)
316
    ,oMf((std::size_t)ref.nFrames)
317
318
    ,Ycrb((std::size_t)ref.nbody)
    ,M(ref.nv,ref.nv)
jcarpent's avatar
jcarpent committed
319
320
321
    ,ddq(ref.nv)
    ,Yaba((std::size_t)ref.nbody)
    ,u(ref.nv)
322
    ,Ag(6, ref.nv)
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
    ,Fcrb((std::size_t)ref.nbody)
    ,lastChild((std::size_t)ref.nbody)
    ,nvSubtree((std::size_t)ref.nbody)
    ,U(ref.nv,ref.nv)
    ,D(ref.nv)
    ,tmp(ref.nv)
    ,parents_fromRow((std::size_t)ref.nv)
    ,nvSubtree_fromRow((std::size_t)ref.nv)
    ,J(6,ref.nv)
    ,iMf((std::size_t)ref.nbody)
    ,com((std::size_t)ref.nbody)
    ,vcom((std::size_t)ref.nbody)
    ,acom((std::size_t)ref.nbody)
    ,mass((std::size_t)ref.nbody)
    ,Jcom(3,ref.nv)
338
339
    ,JMinvJt()
    ,llt_JMinvJt()
340
    ,lambda_c()
341
342
    ,sDUiJt(ref.nv,ref.nv)
    ,torque_residual(ref.nv)
343
344
    ,dq_after(model.nv)
    ,impulse_c()
345
346
  {
    /* Create data strcture associated to the joints */
347
    for(Model::Index i=0;i<(Model::JointIndex)(model.nbody);++i) 
348
349
350
351
352
353
354
355
      joints.push_back(CreateJointData::run(model.joints[i]));

    /* Init for CRBA */
    M.fill(0);
    for(Model::Index i=0;i<(Model::Index)(ref.nbody);++i ) { Fcrb[i].resize(6,model.nv); }
    computeLastChild(ref);

    /* Init for Cholesky */
356
    U.setIdentity();
357
358
359
360
361
362
363
364
365
    computeParents_fromRow(ref);

    /* Init Jacobian */
    J.fill(0);
    
    /* Init universe states relatively to itself */
    
    a[0].setZero();
    v[0].setZero();
366
    a_gf[0] = -model.gravity;
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
    f[0].setZero();
    oMi[0].setIdentity();
    liMi[0].setIdentity();
  }

  inline void Data::computeLastChild (const Model & model)
  {
    typedef Model::Index Index;
    std::fill(lastChild.begin(),lastChild.end(),-1);
    for( int i=model.nbody-1;i>=0;--i )
    {
      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)
  {
    for( Model::Index joint=1;joint<(Model::Index)(model.nbody);joint++)
    {
      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
410
411
/// @endcond

412
#endif // ifndef __se3_model_hxx__