model.hxx 13.5 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(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

    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());
100
101
102
103
    return idx;
  }

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

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

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

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

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

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

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

135
136
137
    return idx;
  }

138

139
  inline void Model::appendBodyToJoint(const JointIndex parent, const SE3 & bodyPlacement, const Inertia & Y,
140
141
                                       const std::string & bodyName)
  {
142
    const Inertia & iYf = Y.se3Action(bodyPlacement);
143
144
    inertias[parent] += iYf;

145
    addFrame((bodyName!="")?bodyName:random(8), parent, bodyPlacement, BODY);
146
147
148
149
150

    nbody ++;
  }


151
 
152

153

154
  inline Model::JointIndex Model::getBodyId (const std::string & name) const
155
  {
156
    return getFrameId(name);
157
158
159
160
  }
  
  inline bool Model::existBodyName (const std::string & name) const
  {
161
    return existFrame(name);
162
163
  }

164
  inline const std::string& Model::getBodyName (const Model::JointIndex index) const
165
166
  {
    assert( index < (Model::Index)nbody );
167
    return getFrameName(index);
168
169
  }

170
  inline Model::JointIndex Model::getJointId (const std::string & name) const
171
  {
jcarpent's avatar
jcarpent committed
172
173
    typedef std::vector<std::string>::iterator::difference_type it_diff_t;
    it_diff_t res = std::find(names.begin(),names.end(),name) - names.begin();
174
    assert( (res<INT_MAX) && "Id superior to int range. Should never happen.");
jcarpent's avatar
jcarpent committed
175
    assert( (res>=0)&&(res<(it_diff_t) joints.size()) && "The joint name you asked does not exist" );
176
    return Model::JointIndex(res);
177
178
179
180
181
182
183
  }
  
  inline bool Model::existJointName (const std::string & name) const
  {
    return (names.end() != std::find(names.begin(),names.end(),name));
  }

184
  inline const std::string& Model::getJointName (const JointIndex index) const
185
  {
186
    assert( index < (Model::JointIndex)joints.size() );
187
188
189
    return names[index];
  }

190
  inline Model::FrameIndex Model::getFrameId ( const std::string & name ) const
191
  {
192
193
    std::vector<Frame>::const_iterator it = std::find_if( frames.begin()
                                                        , frames.end()
194
195
                                                        , boost::bind(&Frame::name, _1) == name
                                                        );
196
    return Model::FrameIndex(it - frames.begin());
197
198
199
200
  }

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

204
  inline const std::string & Model::getFrameName ( const FrameIndex index ) const
205
  {
206
    return frames[index].name;
207
208
  }

jcarpent's avatar
jcarpent committed
209
  inline Model::JointIndex Model::getFrameParent( const std::string & name ) const
210
  {
211
    assert(existFrame(name) && "The Frame you requested does not exist");
212
213
    std::vector<Frame>::const_iterator it = std::find_if( frames.begin()
                                                        , frames.end()
214
215
                                                        , boost::bind(&Frame::name, _1) == name
                                                        );
216
    
217
    std::vector<Frame>::iterator::difference_type it_diff = it - frames.begin();
218
    return getFrameParent(Model::JointIndex(it_diff));
219
220
  }

jcarpent's avatar
jcarpent committed
221
  inline Model::JointIndex Model::getFrameParent( const FrameIndex index ) const
222
  {
223
    return frames[index].parent;
224
225
  }

226
227
228
  inline FrameType Model::getFrameType( const std::string & name ) const
  {
    assert(existFrame(name) && "The Frame you requested does not exist");
229
230
    std::vector<Frame>::const_iterator it = std::find_if( frames.begin()
                                                        , frames.end()
231
232
233
                                                        , boost::bind(&Frame::name, _1) == name
                                                        );
    
234
    std::vector<Frame>::iterator::difference_type it_diff = it - frames.begin();
235
236
237
238
239
    return getFrameType(Model::JointIndex(it_diff));
  }

  inline FrameType Model::getFrameType( const FrameIndex index ) const
  {
240
    return frames[index].type;
241
242
  }

243
  inline const SE3 & Model::getFramePlacement( const std::string & name) const
244
245
  {
    assert(existFrame(name) && "The Frame you requested does not exist");
246
247
    std::vector<Frame>::const_iterator it = std::find_if( frames.begin()
                                                        , frames.end()
248
249
                                                        , boost::bind(&Frame::name, _1) == name
                                                        );
250
    
251
    std::vector<Frame>::iterator::difference_type it_diff = it - frames.begin();
252
    return getFramePlacement(Model::Index(it_diff));
253
254
  }

255
  inline const SE3 & Model::getFramePlacement( const FrameIndex index ) const
256
  {
257
    return frames[index].placement;
258
259
  }

260
  inline bool Model::addFrame ( const Frame & frame )
261
262
  {
    if( !existFrame(frame.name) )
263
    {
264
265
      frames.push_back(frame);
      nFrames++;
266
267
268
269
270
271
      return true;
    }
    else
    {
      return false;
    }
272
273
  }

274
  inline bool Model::addFrame ( const std::string & name, JointIndex index, const SE3 & placement, const FrameType type)
275
276
  {
    if( !existFrame(name) )
277
      return addFrame(Frame(name, index, placement, type));
278
279
    else
      return false;
280
281
282
  }


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

    /* Init Jacobian */
    J.fill(0);
    
    /* Init universe states relatively to itself */
    
    a[0].setZero();
    v[0].setZero();
344
    a_gf[0] = -model.gravity;
345
346
347
348
349
350
351
352
353
354
355
356
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
    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
388
389
/// @endcond

390
#endif // ifndef __se3_model_hxx__