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(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

    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(JointModel(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

    nbody ++;
  }
jcarpent's avatar
jcarpent committed
149
  
150
  inline Model::JointIndex Model::getBodyId (const std::string & name) const
151
  {
152
    return getFrameId(name);
153
154
155
156
  }
  
  inline bool Model::existBodyName (const std::string & name) const
  {
157
    return existFrame(name);
158
159
  }

160
  inline const std::string& Model::getBodyName (const Model::JointIndex index) const
161
162
  {
    assert( index < (Model::Index)nbody );
163
    return getFrameName(index);
164
165
  }

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

180
  inline const std::string& Model::getJointName (const JointIndex index) const
181
  {
182
    assert( index < (Model::JointIndex)joints.size() );
183
184
185
    return names[index];
  }

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

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

200
  inline const std::string & Model::getFrameName ( const FrameIndex index ) const
201
  {
202
    return frames[index].name;
203
204
  }

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

jcarpent's avatar
jcarpent committed
217
  inline Model::JointIndex Model::getFrameParent( const FrameIndex index ) const
218
  {
219
    return frames[index].parent;
220
221
  }

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

  inline FrameType Model::getFrameType( const FrameIndex index ) const
  {
236
    return frames[index].type;
237
238
  }

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

251
  inline const SE3 & Model::getFramePlacement( const FrameIndex index ) const
252
  {
253
    return frames[index].placement;
254
255
  }

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

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


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

    /* Init Jacobian */
    J.fill(0);
    
    /* Init universe states relatively to itself */
    
    a[0].setZero();
    v[0].setZero();
340
    a_gf[0] = -model.gravity;
341
342
343
344
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
    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
384
385
/// @endcond

386
#endif // ifndef __se3_model_hxx__