model.hxx 13.4 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
    return idx;
  }

135

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

142
    addFrame((bodyName!="")?bodyName:random(8), parent, bodyPlacement, BODY);
143
    bodyParents.push_back(parent);
144
    bodyPlacements.push_back(bodyPlacement);
145
146
147
148
149
150
    bodyNames.push_back( (bodyName!="")?bodyName:random(8));

    nbody ++;
  }


151
 
152

153

154
  inline Model::JointIndex Model::getBodyId (const std::string & name) const
155
156
157
158
159
  {
    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" );
160
    return Model::JointIndex(res);
161
162
163
164
165
166
167
  }
  
  inline bool Model::existBodyName (const std::string & name) const
  {
    return (bodyNames.end() != std::find(bodyNames.begin(),bodyNames.end(),name));
  }

168
  inline const std::string& Model::getBodyName (const Model::JointIndex index) const
169
170
171
172
173
  {
    assert( index < (Model::Index)nbody );
    return bodyNames[index];
  }

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

188
  inline const std::string& Model::getJointName (const JointIndex index) const
189
  {
190
    assert( index < (Model::JointIndex)joints.size() );
191
192
193
    return names[index];
  }

194
  inline Model::FrameIndex Model::getFrameId ( const std::string & name ) const
195
196
197
198
199
  {
    std::vector<Frame>::const_iterator it = std::find_if( operational_frames.begin()
                                                        , operational_frames.end()
                                                        , boost::bind(&Frame::name, _1) == name
                                                        );
200
    return Model::FrameIndex(it - operational_frames.begin());
201
202
203
204
205
206
207
  }

  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();
  }

208
  inline const std::string & Model::getFrameName ( const FrameIndex index ) const
209
210
211
212
  {
    return operational_frames[index].name;
  }

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

jcarpent's avatar
jcarpent committed
225
  inline Model::JointIndex Model::getFrameParent( const FrameIndex index ) const
226
  {
227
    return operational_frames[index].parent;
228
229
  }

230
  inline const SE3 & Model::getFramePlacement( const std::string & name) const
231
232
233
234
235
236
  {
    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
                                                        );
237
238
    
    std::vector<Frame>::iterator::difference_type it_diff = it - operational_frames.begin();
239
    return getFramePlacement(Model::Index(it_diff));
240
241
  }

242
  inline const SE3 & Model::getFramePlacement( const FrameIndex index ) const
243
  {
244
    return operational_frames[index].placement;
245
246
  }

247
  inline bool Model::addFrame ( const Frame & frame )
248
249
  {
    if( !existFrame(frame.name) )
250
251
252
253
254
255
256
257
258
    {
      operational_frames.push_back(frame);
      nOperationalFrames++;
      return true;
    }
    else
    {
      return false;
    }
259
260
  }

261
  inline bool Model::addFrame ( const std::string & name, JointIndex index, const SE3 & placement, const FrameType type)
262
263
  {
    if( !existFrame(name) )
264
      return addFrame(Frame(name, index, placement, type));
265
266
    else
      return false;
267
268
269
  }


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

    /* Init Jacobian */
    J.fill(0);
    
    /* Init universe states relatively to itself */
    
    a[0].setZero();
    v[0].setZero();
331
    a_gf[0] = -model.gravity;
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
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
    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
375
376
/// @endcond

377
#endif // ifndef __se3_model_hxx__