model.hxx 12 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
// 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"
jcarpent's avatar
jcarpent committed
23
#include "pinocchio/tools/string-generator.hpp"
24

25
26
#include <boost/bind.hpp>

jcarpent's avatar
jcarpent committed
27
28
/// @cond DEV

29
30
31
32
33
34
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)
jcarpent's avatar
jcarpent committed
35
36
37
38
    {
      os << "  Joint "<< model.names[i] << ": parent=" << model.parents[i]  << std::endl;
    }
    
39
40
    return os;
  }
41
  
jcarpent's avatar
jcarpent committed
42
43
44
45
46
47
48
49
50
51
  template<typename JointModelDerived>
  Model::JointIndex Model::addJoint(const Model::JointIndex parent,
                                    const JointModelBase<JointModelDerived> & joint_model,
                                    const SE3 & joint_placement,
                                    const std::string & joint_name,
                                    const Eigen::VectorXd & max_effort,
                                    const Eigen::VectorXd & max_velocity,
                                    const Eigen::VectorXd & min_config,
                                    const Eigen::VectorXd & max_config
                                    )
52
  {
jcarpent's avatar
jcarpent committed
53
    typedef JointModelDerived D;
54
    assert( (njoint==(int)joints.size())&&(njoint==(int)inertias.size())
jcarpent's avatar
jcarpent committed
55
56
57
58
59
60
61
62
63
64
65
66
67
           &&(njoint==(int)parents.size())&&(njoint==(int)jointPlacements.size()) );
    assert((joint_model.nq()>=0) && (joint_model.nv()>=0));
    
    assert(max_effort.size() == joint_model.nv()
           && max_velocity.size() == joint_model.nv()
           && min_config.size() == joint_model.nq()
           && max_config.size() == joint_model.nq());
    
    Model::JointIndex idx = (Model::JointIndex) (njoint++);
    
    joints         .push_back(JointModel(joint_model.derived()));
    boost::get<JointModelDerived>(joints.back()).setIndexes(idx,nq,nv);
    
68
    inertias       .push_back(Inertia::Zero());
69
    parents        .push_back(parent);
jcarpent's avatar
jcarpent committed
70
71
72
73
    jointPlacements.push_back(joint_placement);
    names          .push_back((joint_name!="")?joint_name:randomStringGenerator(8));
    nq += joint_model.nq();
    nv += joint_model.nv();
74
    
jcarpent's avatar
jcarpent committed
75
76
77
78
    effortLimit.conservativeResize(nv);effortLimit.bottomRows<D::NV>() = max_effort;
    velocityLimit.conservativeResize(nv);velocityLimit.bottomRows<D::NV>() = max_velocity;
    lowerPositionLimit.conservativeResize(nq);lowerPositionLimit.bottomRows<D::NQ>() = min_config;
    upperPositionLimit.conservativeResize(nq);upperPositionLimit.bottomRows<D::NQ>() = max_config;
79
    
80
    neutralConfiguration.conservativeResize(nq);
jcarpent's avatar
jcarpent committed
81
82
83
84
85
    neutralConfiguration.tail(joint_model.nq()) = joint_model.neutralConfiguration();
    
    // Add a the joint frame attached to itself to the frame vector - redundant information but useful.
    addFrame(names[idx],idx,SE3::Identity(),JOINT);
    
jcarpent's avatar
jcarpent committed
86
87
88
89
    // Init and add joint index to its parent subtrees.
    subtrees.push_back(IndexVector(1));
    subtrees[idx][0] = idx;
    addJointIndexToParentSubtrees(idx);
90
91
92
    return idx;
  }

jcarpent's avatar
jcarpent committed
93
94
95
96
  inline void Model::appendBodyToJoint(const Model::JointIndex joint_index,
                                       const Inertia & Y,
                                       const SE3 & body_placement,
                                       const std::string & body_name)
97
  {
jcarpent's avatar
jcarpent committed
98
99
    const Inertia & iYf = Y.se3Action(body_placement);
    inertias[joint_index] += iYf;
100

jcarpent's avatar
jcarpent committed
101
102
    addFrame((body_name!="")?body_name:randomStringGenerator(8), joint_index, body_placement, BODY);
    nbody++;
103
  }
jcarpent's avatar
jcarpent committed
104
  
105
  inline Model::JointIndex Model::getBodyId (const std::string & name) const
106
  {
107
    return getFrameId(name);
108
109
110
111
  }
  
  inline bool Model::existBodyName (const std::string & name) const
  {
112
    return existFrame(name);
113
114
  }

115
  inline const std::string& Model::getBodyName (const Model::JointIndex index) const
116
117
  {
    assert( index < (Model::Index)nbody );
118
    return getFrameName(index);
119
120
  }

121
  inline Model::JointIndex Model::getJointId (const std::string & name) const
122
  {
jcarpent's avatar
jcarpent committed
123
124
    typedef std::vector<std::string>::iterator::difference_type it_diff_t;
    it_diff_t res = std::find(names.begin(),names.end(),name) - names.begin();
125
    assert( (res<INT_MAX) && "Id superior to int range. Should never happen.");
jcarpent's avatar
jcarpent committed
126
    assert( (res>=0)&&(res<(it_diff_t) joints.size()) && "The joint name you asked does not exist" );
127
    return Model::JointIndex(res);
128
129
130
131
132
133
134
  }
  
  inline bool Model::existJointName (const std::string & name) const
  {
    return (names.end() != std::find(names.begin(),names.end(),name));
  }

135
  inline const std::string& Model::getJointName (const JointIndex index) const
136
  {
137
    assert( index < (Model::JointIndex)joints.size() );
138
139
140
    return names[index];
  }

141
  inline Model::FrameIndex Model::getFrameId ( const std::string & name ) const
142
  {
143
144
    std::vector<Frame>::const_iterator it = std::find_if( frames.begin()
                                                        , frames.end()
145
146
                                                        , boost::bind(&Frame::name, _1) == name
                                                        );
147
    return Model::FrameIndex(it - frames.begin());
148
149
150
151
  }

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

155
  inline const std::string & Model::getFrameName ( const FrameIndex index ) const
156
  {
157
    return frames[index].name;
158
159
  }

jcarpent's avatar
jcarpent committed
160
  inline Model::JointIndex Model::getFrameParent( const std::string & name ) const
161
  {
162
    assert(existFrame(name) && "The Frame you requested does not exist");
163
164
    std::vector<Frame>::const_iterator it = std::find_if( frames.begin()
                                                        , frames.end()
165
166
                                                        , boost::bind(&Frame::name, _1) == name
                                                        );
167
    
168
    std::vector<Frame>::iterator::difference_type it_diff = it - frames.begin();
169
    return getFrameParent(Model::JointIndex(it_diff));
170
171
  }

jcarpent's avatar
jcarpent committed
172
  inline Model::JointIndex Model::getFrameParent( const FrameIndex index ) const
173
  {
174
    return frames[index].parent;
175
176
  }

177
178
179
  inline FrameType Model::getFrameType( const std::string & name ) const
  {
    assert(existFrame(name) && "The Frame you requested does not exist");
180
181
    std::vector<Frame>::const_iterator it = std::find_if( frames.begin()
                                                        , frames.end()
182
183
184
                                                        , boost::bind(&Frame::name, _1) == name
                                                        );
    
185
    std::vector<Frame>::iterator::difference_type it_diff = it - frames.begin();
186
187
188
189
190
    return getFrameType(Model::JointIndex(it_diff));
  }

  inline FrameType Model::getFrameType( const FrameIndex index ) const
  {
191
    return frames[index].type;
192
193
  }

194
  inline const SE3 & Model::getFramePlacement( const std::string & name) const
195
196
  {
    assert(existFrame(name) && "The Frame you requested does not exist");
197
198
    std::vector<Frame>::const_iterator it = std::find_if( frames.begin()
                                                        , frames.end()
199
200
                                                        , boost::bind(&Frame::name, _1) == name
                                                        );
201
    
202
    std::vector<Frame>::iterator::difference_type it_diff = it - frames.begin();
203
    return getFramePlacement(Model::Index(it_diff));
204
205
  }

206
  inline const SE3 & Model::getFramePlacement( const FrameIndex index ) const
207
  {
208
    return frames[index].placement;
209
210
  }

211
  inline bool Model::addFrame ( const Frame & frame )
212
213
  {
    if( !existFrame(frame.name) )
214
    {
215
216
      frames.push_back(frame);
      nFrames++;
217
218
219
220
221
222
      return true;
    }
    else
    {
      return false;
    }
223
224
  }

225
  inline bool Model::addFrame ( const std::string & name, JointIndex index, const SE3 & placement, const FrameType type)
226
227
  {
    if( !existFrame(name) )
228
      return addFrame(Frame(name, index, placement, type));
229
230
    else
      return false;
231
  }
jcarpent's avatar
jcarpent committed
232
233
234
235
236
237
  
  inline void Model::addJointIndexToParentSubtrees(const JointIndex joint_id)
  {
    for(JointIndex parent = parents[joint_id]; parent>0; parent = parents[parent])
      subtrees[parent].push_back(joint_id);
  }
238
239


240
241
242
243
  inline Data::Data (const Model & ref)
    :model(ref)
    ,joints(0)
    ,a((std::size_t)ref.nbody)
244
    ,a_gf((std::size_t)ref.nbody)
245
246
247
248
249
250
    ,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)
251
    ,oMf((std::size_t)ref.nFrames)
252
253
    ,Ycrb((std::size_t)ref.nbody)
    ,M(ref.nv,ref.nv)
jcarpent's avatar
jcarpent committed
254
255
256
    ,ddq(ref.nv)
    ,Yaba((std::size_t)ref.nbody)
    ,u(ref.nv)
jcarpent's avatar
jcarpent committed
257
    ,Ag(6,ref.nv)
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
    ,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)
273
274
    ,JMinvJt()
    ,llt_JMinvJt()
275
    ,lambda_c()
276
277
    ,sDUiJt(ref.nv,ref.nv)
    ,torque_residual(ref.nv)
278
279
    ,dq_after(model.nv)
    ,impulse_c()
280
281
  {
    /* Create data strcture associated to the joints */
282
    for(Model::Index i=0;i<(Model::JointIndex)(model.nbody);++i) 
283
284
285
286
287
288
289
290
      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 */
291
    U.setIdentity();
292
293
294
    computeParents_fromRow(ref);

    /* Init Jacobian */
jcarpent's avatar
jcarpent committed
295
296
    J.setZero();
    Ag.setZero();
297
298
299
300
301
    
    /* Init universe states relatively to itself */
    
    a[0].setZero();
    v[0].setZero();
302
    a_gf[0] = -model.gravity;
303
304
305
306
307
308
309
310
311
312
313
314
315
316
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
    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
346
347
/// @endcond

348
#endif // ifndef __se3_model_hxx__