model.hxx 11.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
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);
    
86
87
88
    return idx;
  }

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

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

111
  inline const std::string& Model::getBodyName (const Model::JointIndex index) const
112
113
  {
    assert( index < (Model::Index)nbody );
114
    return getFrameName(index);
115
116
  }

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

131
  inline const std::string& Model::getJointName (const JointIndex index) const
132
  {
133
    assert( index < (Model::JointIndex)joints.size() );
134
135
136
    return names[index];
  }

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

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

151
  inline const std::string & Model::getFrameName ( const FrameIndex index ) const
152
  {
153
    return frames[index].name;
154
155
  }

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

jcarpent's avatar
jcarpent committed
168
  inline Model::JointIndex Model::getFrameParent( const FrameIndex index ) const
169
  {
170
    return frames[index].parent;
171
172
  }

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

  inline FrameType Model::getFrameType( const FrameIndex index ) const
  {
187
    return frames[index].type;
188
189
  }

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

202
  inline const SE3 & Model::getFramePlacement( const FrameIndex index ) const
203
  {
204
    return frames[index].placement;
205
206
  }

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

221
  inline bool Model::addFrame ( const std::string & name, JointIndex index, const SE3 & placement, const FrameType type)
222
223
  {
    if( !existFrame(name) )
224
      return addFrame(Frame(name, index, placement, type));
225
226
    else
      return false;
227
228
229
  }


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

    /* Init Jacobian */
    J.fill(0);
    
    /* Init universe states relatively to itself */
    
    a[0].setZero();
    v[0].setZero();
291
    a_gf[0] = -model.gravity;
292
293
294
295
296
297
298
299
300
301
302
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
    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
335
336
/// @endcond

337
#endif // ifndef __se3_model_hxx__