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
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
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
  inline Data::Data (const Model & ref)
241
    :joints(0)
242
243
244
245
246
247
    ,a((std::size_t)ref.njoint)
    ,a_gf((std::size_t)ref.njoint)
    ,v((std::size_t)ref.njoint)
    ,f((std::size_t)ref.njoint)
    ,oMi((std::size_t)ref.njoint)
    ,liMi((std::size_t)ref.njoint)
248
249
    ,tau(ref.nv)
    ,nle(ref.nv)
250
    ,oMf((std::size_t)ref.nFrames)
251
    ,Ycrb((std::size_t)ref.njoint)
252
    ,M(ref.nv,ref.nv)
jcarpent's avatar
jcarpent committed
253
    ,ddq(ref.nv)
254
    ,Yaba((std::size_t)ref.njoint)
jcarpent's avatar
jcarpent committed
255
    ,u(ref.nv)
jcarpent's avatar
jcarpent committed
256
    ,Ag(6,ref.nv)
257
258
259
    ,Fcrb((std::size_t)ref.njoint)
    ,lastChild((std::size_t)ref.njoint)
    ,nvSubtree((std::size_t)ref.njoint)
260
261
262
263
264
265
    ,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)
266
267
268
269
270
    ,iMf((std::size_t)ref.njoint)
    ,com((std::size_t)ref.njoint)
    ,vcom((std::size_t)ref.njoint)
    ,acom((std::size_t)ref.njoint)
    ,mass((std::size_t)ref.njoint)
271
    ,Jcom(3,ref.nv)
272
273
    ,JMinvJt()
    ,llt_JMinvJt()
274
    ,lambda_c()
275
276
    ,sDUiJt(ref.nv,ref.nv)
    ,torque_residual(ref.nv)
277
    ,dq_after(ref.nv)
278
    ,impulse_c()
279
280
  {
    /* Create data strcture associated to the joints */
281
282
    for(Model::Index i=0;i<(Model::JointIndex)(ref.njoint);++i) 
      joints.push_back(CreateJointData::run(ref.joints[i]));
283
284
285

    /* Init for CRBA */
    M.fill(0);
286
    for(Model::Index i=0;i<(Model::Index)(ref.njoint);++i ) { Fcrb[i].resize(6,ref.nv); }
287
288
289
    computeLastChild(ref);

    /* Init for Cholesky */
290
    U.setIdentity();
291
292
293
    computeParents_fromRow(ref);

    /* Init Jacobian */
jcarpent's avatar
jcarpent committed
294
295
    J.setZero();
    Ag.setZero();
296
297
298
299
300
    
    /* Init universe states relatively to itself */
    
    a[0].setZero();
    v[0].setZero();
301
    a_gf[0] = -ref.gravity;
302
303
304
305
306
307
308
309
310
    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);
311
    for( int i=model.njoint-1;i>=0;--i )
312
313
314
315
316
317
318
319
320
321
322
323
324
    {
      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)
  {
325
    for( Model::Index joint=1;joint<(Model::Index)(model.njoint);joint++)
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
    {
      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
345
346
/// @endcond

347
#endif // ifndef __se3_model_hxx__