model.hxx 14.1 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
    neutralConfigurations.conservativeResize(nq);neutralConfigurations.bottomRows<D::NQ>() = j.neutralConfiguration();
97
98
99
100
    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());
101
102
103
104
    return idx;
  }

  template<typename D>
105
  Model::JointIndex Model::addJoint(JointIndex parent,const JointModelBase<D> & j,const SE3 & jointPlacement,
106
107
108
                     const Eigen::VectorXd & effort, const Eigen::VectorXd & velocity,
                     const Eigen::VectorXd & lowPos, const Eigen::VectorXd & upPos,
                     const std::string & jointName)
109
  {
110
111
    assert( (njoint==(int)joints.size())&&(njoint==(int)inertias.size())
      &&(njoint==(int)parents.size())&&(njoint==(int)jointPlacements.size()) );
112
113
    assert( (j.nq()>=0)&&(j.nv()>=0) );

114
115
116
    assert( effort.size() == j.nv() && velocity.size() == j.nv()
           && lowPos.size() == j.nq() && upPos.size() == j.nq() );

117
    Model::JointIndex idx = (Model::JointIndex) (njoint ++);
118

119
    joints         .push_back(JointModel(j.derived())); 
120
    boost::get<D>(joints.back()).setIndexes(idx,nq,nv);
121

122
    inertias       .push_back(Inertia::Zero());
123
    parents        .push_back(parent);
124
    jointPlacements.push_back(jointPlacement);
125
126
127
    names          .push_back( (jointName!="")?jointName:random(8) );
    nq += j.nq();
    nv += j.nv();
128

129
    
130
131
132
133
    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;
134

135
136
137
138
139
140
141
142
143
144
145
146
147
148
    // Ensure that limits are not inf
    Eigen::VectorXd neutralConf((lowerPositionLimit.bottomRows<D::NQ>() + upperPositionLimit.bottomRows<D::NQ>())/2 );
    
    neutralConfigurations.conservativeResize(nq);
    if ( std::isfinite(neutralConf.norm()) )
    {
      neutralConfigurations.bottomRows<D::NQ>() = neutralConf;
    }
    else
    {
      assert( false && "One of the position limit is inf or NaN");
      neutralConfigurations.bottomRows<D::NQ>() = j.neutralConfiguration();
    }

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

151
152
153
    return idx;
  }

154

155
  inline void Model::appendBodyToJoint(const JointIndex parent, const SE3 & bodyPlacement, const Inertia & Y,
156
157
                                       const std::string & bodyName)
  {
158
    const Inertia & iYf = Y.se3Action(bodyPlacement);
159
160
    inertias[parent] += iYf;

161
    addFrame((bodyName!="")?bodyName:random(8), parent, bodyPlacement, BODY);
162
163
164

    nbody ++;
  }
jcarpent's avatar
jcarpent committed
165
  
166
  inline Model::JointIndex Model::getBodyId (const std::string & name) const
167
  {
168
    return getFrameId(name);
169
170
171
172
  }
  
  inline bool Model::existBodyName (const std::string & name) const
  {
173
    return existFrame(name);
174
175
  }

176
  inline const std::string& Model::getBodyName (const Model::JointIndex index) const
177
178
  {
    assert( index < (Model::Index)nbody );
179
    return getFrameName(index);
180
181
  }

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

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

202
  inline Model::FrameIndex Model::getFrameId ( const std::string & name ) const
203
  {
204
205
    std::vector<Frame>::const_iterator it = std::find_if( frames.begin()
                                                        , frames.end()
206
207
                                                        , boost::bind(&Frame::name, _1) == name
                                                        );
208
    return Model::FrameIndex(it - frames.begin());
209
210
211
212
  }

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

216
  inline const std::string & Model::getFrameName ( const FrameIndex index ) const
217
  {
218
    return frames[index].name;
219
220
  }

jcarpent's avatar
jcarpent committed
221
  inline Model::JointIndex Model::getFrameParent( const std::string & name ) const
222
  {
223
    assert(existFrame(name) && "The Frame you requested does not exist");
224
225
    std::vector<Frame>::const_iterator it = std::find_if( frames.begin()
                                                        , frames.end()
226
227
                                                        , boost::bind(&Frame::name, _1) == name
                                                        );
228
    
229
    std::vector<Frame>::iterator::difference_type it_diff = it - frames.begin();
230
    return getFrameParent(Model::JointIndex(it_diff));
231
232
  }

jcarpent's avatar
jcarpent committed
233
  inline Model::JointIndex Model::getFrameParent( const FrameIndex index ) const
234
  {
235
    return frames[index].parent;
236
237
  }

238
239
240
  inline FrameType Model::getFrameType( const std::string & name ) const
  {
    assert(existFrame(name) && "The Frame you requested does not exist");
241
242
    std::vector<Frame>::const_iterator it = std::find_if( frames.begin()
                                                        , frames.end()
243
244
245
                                                        , boost::bind(&Frame::name, _1) == name
                                                        );
    
246
    std::vector<Frame>::iterator::difference_type it_diff = it - frames.begin();
247
248
249
250
251
    return getFrameType(Model::JointIndex(it_diff));
  }

  inline FrameType Model::getFrameType( const FrameIndex index ) const
  {
252
    return frames[index].type;
253
254
  }

255
  inline const SE3 & Model::getFramePlacement( const std::string & name) const
256
257
  {
    assert(existFrame(name) && "The Frame you requested does not exist");
258
259
    std::vector<Frame>::const_iterator it = std::find_if( frames.begin()
                                                        , frames.end()
260
261
                                                        , boost::bind(&Frame::name, _1) == name
                                                        );
262
    
263
    std::vector<Frame>::iterator::difference_type it_diff = it - frames.begin();
264
    return getFramePlacement(Model::Index(it_diff));
265
266
  }

267
  inline const SE3 & Model::getFramePlacement( const FrameIndex index ) const
268
  {
269
    return frames[index].placement;
270
271
  }

272
  inline bool Model::addFrame ( const Frame & frame )
273
274
  {
    if( !existFrame(frame.name) )
275
    {
276
277
      frames.push_back(frame);
      nFrames++;
278
279
280
281
282
283
      return true;
    }
    else
    {
      return false;
    }
284
285
  }

286
  inline bool Model::addFrame ( const std::string & name, JointIndex index, const SE3 & placement, const FrameType type)
287
288
  {
    if( !existFrame(name) )
289
      return addFrame(Frame(name, index, placement, type));
290
291
    else
      return false;
292
293
294
  }


295
296
297
298
  inline Data::Data (const Model & ref)
    :model(ref)
    ,joints(0)
    ,a((std::size_t)ref.nbody)
299
    ,a_gf((std::size_t)ref.nbody)
300
301
302
303
304
305
    ,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)
306
    ,oMf((std::size_t)ref.nFrames)
307
308
    ,Ycrb((std::size_t)ref.nbody)
    ,M(ref.nv,ref.nv)
jcarpent's avatar
jcarpent committed
309
310
311
    ,ddq(ref.nv)
    ,Yaba((std::size_t)ref.nbody)
    ,u(ref.nv)
312
    ,Ag(6, ref.nv)
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
    ,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)
328
329
    ,JMinvJt()
    ,llt_JMinvJt()
330
    ,lambda_c()
331
332
    ,sDUiJt(ref.nv,ref.nv)
    ,torque_residual(ref.nv)
333
334
    ,dq_after(model.nv)
    ,impulse_c()
335
336
  {
    /* Create data strcture associated to the joints */
337
    for(Model::Index i=0;i<(Model::JointIndex)(model.nbody);++i) 
338
339
340
341
342
343
344
345
      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 */
346
    U.setIdentity();
347
348
349
350
351
352
353
354
355
    computeParents_fromRow(ref);

    /* Init Jacobian */
    J.fill(0);
    
    /* Init universe states relatively to itself */
    
    a[0].setZero();
    v[0].setZero();
356
    a_gf[0] = -model.gravity;
357
358
359
360
361
362
363
364
365
366
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
    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
400
401
/// @endcond

402
#endif // ifndef __se3_model_hxx__