model.hxx 12.6 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
namespace se3
{
  inline std::ostream& operator<< (std::ostream & os, const Model & model)
  {
33
34
    os << "Nb joints = " << model.njoint << " (nq="<< model.nq<<",nv="<<model.nv<<")" << std::endl;
    for(Model::Index i=0;i<(Model::Index)(model.njoint);++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
    neutralConfiguration.tail(joint_model.nq()) = joint_model.neutralConfiguration();
    
jcarpent's avatar
jcarpent committed
83
84
85
86
    // Init and add joint index to its parent subtrees.
    subtrees.push_back(IndexVector(1));
    subtrees[idx][0] = idx;
    addJointIndexToParentSubtrees(idx);
87
88
89
    return idx;
  }

90
91
92
93
94
95
96
97
98
99
100
101
102
  inline FrameIndex Model::addJointFrame (const JointIndex& jidx,
                                                int         fidx)
  {
    if (fidx < 0) {
      fidx = getFrameId(names[parents[jidx]]);
    }
    if (fidx >= frames.size())
      throw std::invalid_argument ("Frame not found");
    // Add a the joint frame attached to itself to the frame vector - redundant information but useful.
    addFrame(Frame(names[jidx],jidx,fidx,SE3::Identity(),JOINT));
    return frames.size() - 1;
  }

jcarpent's avatar
jcarpent committed
103
104
  inline void Model::appendBodyToJoint(const Model::JointIndex joint_index,
                                       const Inertia & Y,
105
                                       const SE3 & body_placement)
106
  {
jcarpent's avatar
jcarpent committed
107
108
109
    const Inertia & iYf = Y.se3Action(body_placement);
    inertias[joint_index] += iYf;
    nbody++;
110
  }
111
112
113
114
115
116
117
118
119

  inline FrameIndex Model::addBodyFrame (const std::string& body_name,
                                         const JointIndex& parentJoint,
                                         const SE3 & body_placement,
                                               int         previousFrame)
  {
    if (previousFrame < 0) {
      previousFrame = getFrameId(names[parentJoint]);
    }
120
    assert(previousFrame >= frames.size() && "Frame index out of bound");
121
122
123
    addFrame(Frame(body_name, parentJoint, previousFrame, body_placement, BODY));
    return frames.size() - 1;
  }
jcarpent's avatar
jcarpent committed
124
  
125
  inline Model::JointIndex Model::getBodyId (const std::string & name) const
126
  {
127
    return getFrameId(name);
128
129
130
131
  }
  
  inline bool Model::existBodyName (const std::string & name) const
  {
132
    return existFrame(name);
133
134
  }

135
  inline const std::string& Model::getBodyName (const Model::JointIndex index) const
136
137
  {
    assert( index < (Model::Index)nbody );
138
    return getFrameName(index);
139
140
  }

141
  inline Model::JointIndex Model::getJointId (const std::string & name) const
142
  {
jcarpent's avatar
jcarpent committed
143
144
    typedef std::vector<std::string>::iterator::difference_type it_diff_t;
    it_diff_t res = std::find(names.begin(),names.end(),name) - names.begin();
145
    assert( (res<INT_MAX) && "Id superior to int range. Should never happen.");
jcarpent's avatar
jcarpent committed
146
    assert( (res>=0)&&(res<(it_diff_t) joints.size()) && "The joint name you asked does not exist" );
147
    return Model::JointIndex(res);
148
149
150
151
152
153
154
  }
  
  inline bool Model::existJointName (const std::string & name) const
  {
    return (names.end() != std::find(names.begin(),names.end(),name));
  }

155
  inline const std::string& Model::getJointName (const JointIndex index) const
156
  {
157
    assert( index < (Model::JointIndex)joints.size() );
158
159
160
    return names[index];
  }

161
  inline Model::FrameIndex Model::getFrameId ( const std::string & name ) const
162
  {
163
164
    std::vector<Frame>::const_iterator it = std::find_if( frames.begin()
                                                        , frames.end()
165
166
                                                        , boost::bind(&Frame::name, _1) == name
                                                        );
167
    return Model::FrameIndex(it - frames.begin());
168
169
170
171
  }

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

175
  inline const std::string & Model::getFrameName ( const FrameIndex index ) const
176
  {
177
    return frames[index].name;
178
179
  }

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

jcarpent's avatar
jcarpent committed
192
  inline Model::JointIndex Model::getFrameParent( const FrameIndex index ) const
193
  {
194
    return frames[index].parent;
195
196
  }

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

  inline FrameType Model::getFrameType( const FrameIndex index ) const
  {
211
    return frames[index].type;
212
213
  }

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

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

231
  inline bool Model::addFrame ( const Frame & frame )
232
233
  {
    if( !existFrame(frame.name) )
234
    {
235
236
      frames.push_back(frame);
      nFrames++;
237
238
239
240
241
242
      return true;
    }
    else
    {
      return false;
    }
243
  }
jcarpent's avatar
jcarpent committed
244
245
246
247
248
249
  
  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);
  }
250
251


252
  inline Data::Data (const Model & model)
253
    :joints(0)
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
    ,a((std::size_t)model.njoint)
    ,a_gf((std::size_t)model.njoint)
    ,v((std::size_t)model.njoint)
    ,f((std::size_t)model.njoint)
    ,oMi((std::size_t)model.njoint)
    ,liMi((std::size_t)model.njoint)
    ,tau(model.nv)
    ,nle(model.nv)
    ,oMf((std::size_t)model.nFrames)
    ,Ycrb((std::size_t)model.njoint)
    ,M(model.nv,model.nv)
    ,ddq(model.nv)
    ,Yaba((std::size_t)model.njoint)
    ,u(model.nv)
    ,Ag(6,model.nv)
    ,Fcrb((std::size_t)model.njoint)
    ,lastChild((std::size_t)model.njoint)
    ,nvSubtree((std::size_t)model.njoint)
    ,U(model.nv,model.nv)
    ,D(model.nv)
    ,tmp(model.nv)
    ,parents_fromRow((std::size_t)model.nv)
    ,nvSubtree_fromRow((std::size_t)model.nv)
    ,J(6,model.nv)
    ,iMf((std::size_t)model.njoint)
    ,com((std::size_t)model.njoint)
    ,vcom((std::size_t)model.njoint)
    ,acom((std::size_t)model.njoint)
    ,mass((std::size_t)model.njoint)
    ,Jcom(3,model.nv)
284
285
    ,JMinvJt()
    ,llt_JMinvJt()
286
    ,lambda_c()
287
288
289
    ,sDUiJt(model.nv,model.nv)
    ,torque_residual(model.nv)
    ,dq_after(model.nv)
290
    ,impulse_c()
291
292
  {
    /* Create data strcture associated to the joints */
293
294
    for(Model::Index i=0;i<(Model::JointIndex)(model.njoint);++i) 
      joints.push_back(CreateJointData::run(model.joints[i]));
295
296
297

    /* Init for CRBA */
    M.fill(0);
298
299
    for(Model::Index i=0;i<(Model::Index)(model.njoint);++i ) { Fcrb[i].resize(6,model.nv); }
    computeLastChild(model);
300
301

    /* Init for Cholesky */
302
    U.setIdentity();
303
    computeParents_fromRow(model);
304
305

    /* Init Jacobian */
jcarpent's avatar
jcarpent committed
306
307
    J.setZero();
    Ag.setZero();
308
309
310
311
312
    
    /* Init universe states relatively to itself */
    
    a[0].setZero();
    v[0].setZero();
313
    a_gf[0] = -model.gravity;
314
315
316
317
318
319
320
321
322
    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);
323
    for( int i=model.njoint-1;i>=0;--i )
324
325
326
327
328
329
330
331
332
333
334
335
336
    {
      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)
  {
337
    for( Model::Index joint=1;joint<(Model::Index)(model.njoint);joint++)
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
    {
      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
357
358
/// @endcond

359
#endif // ifndef __se3_model_hxx__