model.hxx 13.8 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
    neutralConfigurations.conservativeResize(nq);neutralConfigurations.bottomRows<D::NQ>() = j.neutralConfiguration();
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

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

137
138
139
    return idx;
  }

140

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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


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

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

388
#endif // ifndef __se3_model_hxx__