model.hxx 12.9 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
42
43
44
45
46
47
48
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)
      {
	os << "  Joint "<<model.names[i] << ": parent=" << model.parents[i] 
	   << ( model.hasVisual[i] ? " (has visual) " : "(doesnt have visual)" ) << std::endl;
      }

    return os;
  }

49
  
50
51

  template<typename D>
52
  Model::JointIndex Model::addBody (JointIndex parent, const JointModelBase<D> & j, const SE3 & placement,
53
54
55
56
57
58
59
                               const Inertia & Y, const std::string & jointName,
                               const std::string & bodyName, bool visual)
  {
    assert( (nbody==(int)joints.size())&&(nbody==(int)inertias.size())
      &&(nbody==(int)parents.size())&&(nbody==(int)jointPlacements.size()) );
    assert( (j.nq()>=0)&&(j.nv()>=0) );

60
    Model::JointIndex idx = (Model::JointIndex) (nbody ++);
61
62

    joints         .push_back(j.derived()); 
63
    boost::get<D>(joints.back()).setIndexes(idx,nq,nv);
64
65
66
67
68
69
70
71
72

    inertias       .push_back(Y);
    parents        .push_back(parent);
    jointPlacements.push_back(placement);
    names          .push_back( (jointName!="")?jointName:random(8) );
    hasVisual      .push_back(visual);
    bodyNames      .push_back( (bodyName!="")?bodyName:random(8));
    nq += j.nq();
    nv += j.nv();
73
74
75
76
77

    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());
78
79
80
81
    return idx;
  }

  template<typename D>
82
  Model::JointIndex Model::addBody (JointIndex parent, const JointModelBase<D> & j, const SE3 & placement,
83
84
85
86
87
88
89
90
91
                               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, bool visual)
  {
    assert( (nbody==(int)joints.size())&&(nbody==(int)inertias.size())
	    &&(nbody==(int)parents.size())&&(nbody==(int)jointPlacements.size()) );
    assert( (j.nq()>=0)&&(j.nv()>=0) );
92
93
94
95
    
    assert( effort.size() == j.nv() && velocity.size() == j.nv()
      && lowPos.size() == j.nq() && upPos.size() == j.nq() );

96

97
    Model::JointIndex idx = (Model::JointIndex) (nbody ++);
98
99

    joints         .push_back(j.derived()); 
100
    boost::get<D>(joints.back()).setIndexes(idx,nq,nv);
101
102
103
104
105
106
107
108
109
110


    inertias       .push_back(Y);
    parents        .push_back(parent);
    jointPlacements.push_back(placement);
    names          .push_back( (jointName!="")?jointName:random(8) );
    hasVisual      .push_back(visual);
    bodyNames      .push_back( (bodyName!="")?bodyName:random(8));
    nq += j.nq();
    nv += j.nv();
111
112
113
114
115

    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;
116
117
118
    return idx;
  }

119
  inline Model::JointIndex Model::addFixedBody (JointIndex lastMovingParent,
120
121
122
123
124
                                           const SE3 & placementFromLastMoving,
                                           const std::string & bodyName,
                                           bool visual)
  {

125
    Model::JointIndex idx = (Model::JointIndex) (nFixBody++);
126
127
128
129
130
131
132
    fix_lastMovingParent.push_back(lastMovingParent);
    fix_lmpMi      .push_back(placementFromLastMoving);
    fix_hasVisual  .push_back(visual);
    fix_bodyNames  .push_back( (bodyName!="")?bodyName:random(8));
    return idx;
  }

jcarpent's avatar
jcarpent committed
133
  inline void Model::mergeFixedBody (const JointIndex parent, const SE3 & placement, const Inertia & Y)
134
135
136
137
138
  {
    const Inertia & iYf = Y.se3Action(placement); //TODO
    inertias[parent] += iYf;
  }

139
  inline Model::JointIndex Model::getBodyId (const std::string & name) const
140
141
142
143
144
  {
    std::vector<std::string>::iterator::difference_type
      res = std::find(bodyNames.begin(),bodyNames.end(),name) - bodyNames.begin();
    assert( (res<INT_MAX) && "Id superior to int range. Should never happen.");
    assert( (res>=0)&&(res<nbody) && "The body name you asked does not exist" );
145
    return Model::JointIndex(res);
146
147
148
149
150
151
152
  }
  
  inline bool Model::existBodyName (const std::string & name) const
  {
    return (bodyNames.end() != std::find(bodyNames.begin(),bodyNames.end(),name));
  }

153
  inline const std::string& Model::getBodyName (const Model::JointIndex index) const
154
155
156
157
158
  {
    assert( index < (Model::Index)nbody );
    return bodyNames[index];
  }

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

173
  inline const std::string& Model::getJointName (const JointIndex index) const
174
  {
175
    assert( index < (Model::JointIndex)joints.size() );
176
177
178
    return names[index];
  }

179
  inline Model::FrameIndex Model::getFrameId ( const std::string & name ) const
180
181
182
183
184
  {
    std::vector<Frame>::const_iterator it = std::find_if( operational_frames.begin()
                                                        , operational_frames.end()
                                                        , boost::bind(&Frame::name, _1) == name
                                                        );
185
    return Model::FrameIndex(it - operational_frames.begin());
186
187
188
189
190
191
192
  }

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

193
  inline const std::string & Model::getFrameName ( const FrameIndex index ) const
194
195
196
197
  {
    return operational_frames[index].name;
  }

jcarpent's avatar
jcarpent committed
198
  inline Model::JointIndex Model::getFrameParent( const std::string & name ) const
199
  {
200
    assert(existFrame(name) && "The Frame you requested does not exist");
201
202
203
204
    std::vector<Frame>::const_iterator it = std::find_if( operational_frames.begin()
                                                        , operational_frames.end()
                                                        , boost::bind(&Frame::name, _1) == name
                                                        );
205
206
    
    std::vector<Frame>::iterator::difference_type it_diff = it - operational_frames.begin();
207
    return getFrameParent(Model::JointIndex(it_diff));
208
209
  }

jcarpent's avatar
jcarpent committed
210
  inline Model::JointIndex Model::getFrameParent( const FrameIndex index ) const
211
  {
212
    return operational_frames[index].parent;
213
214
  }

215
  inline const SE3 & Model::getFramePlacement( const std::string & name) const
216
217
218
219
220
221
  {
    assert(existFrame(name) && "The Frame you requested does not exist");
    std::vector<Frame>::const_iterator it = std::find_if( operational_frames.begin()
                                                        , operational_frames.end()
                                                        , boost::bind(&Frame::name, _1) == name
                                                        );
222
223
    
    std::vector<Frame>::iterator::difference_type it_diff = it - operational_frames.begin();
224
    return getFramePlacement(Model::Index(it_diff));
225
226
  }

227
  inline const SE3 & Model::getFramePlacement( const FrameIndex index ) const
228
  {
229
    return operational_frames[index].placement;
230
231
  }

232
  inline bool Model::addFrame ( const Frame & frame )
233
234
  {
    if( !existFrame(frame.name) )
235
236
237
238
239
240
241
242
243
    {
      operational_frames.push_back(frame);
      nOperationalFrames++;
      return true;
    }
    else
    {
      return false;
    }
244
245
  }

246
  inline bool Model::addFrame ( const std::string & name, JointIndex index, const SE3 & placement)
247
248
  {
    if( !existFrame(name) )
249
250
251
      return addFrame(Frame(name, index, placement));
    else
      return false;
252
253
254
  }


255
256
257
258
  inline Data::Data (const Model & ref)
    :model(ref)
    ,joints(0)
    ,a((std::size_t)ref.nbody)
259
    ,a_gf((std::size_t)ref.nbody)
260
261
262
263
264
265
    ,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)
266
    ,oMof((std::size_t)ref.nOperationalFrames)
267
268
    ,Ycrb((std::size_t)ref.nbody)
    ,M(ref.nv,ref.nv)
jcarpent's avatar
jcarpent committed
269
270
271
    ,ddq(ref.nv)
    ,Yaba((std::size_t)ref.nbody)
    ,u(ref.nv)
272
    ,Ag(6, ref.nv)
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
    ,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)
288
289
    ,JMinvJt()
    ,llt_JMinvJt()
290
    ,lambda_c()
291
292
    ,sDUiJt(ref.nv,ref.nv)
    ,torque_residual(ref.nv)
293
294
    ,dq_after(model.nv)
    ,impulse_c()
295
296
  {
    /* Create data strcture associated to the joints */
297
    for(Model::Index i=0;i<(Model::JointIndex)(model.nbody);++i) 
298
299
300
301
302
303
304
305
      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 */
306
    U.setIdentity();
307
308
309
310
311
312
313
314
315
    computeParents_fromRow(ref);

    /* Init Jacobian */
    J.fill(0);
    
    /* Init universe states relatively to itself */
    
    a[0].setZero();
    v[0].setZero();
316
    a_gf[0] = -model.gravity;
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
    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
360
361
/// @endcond

362
#endif // ifndef __se3_model_hxx__