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

jcarpent's avatar
jcarpent committed
90
91
92
93
  inline void Model::appendBodyToJoint(const Model::JointIndex joint_index,
                                       const Inertia & Y,
                                       const SE3 & body_placement,
                                       const std::string & body_name)
94
  {
jcarpent's avatar
jcarpent committed
95
96
97
    const Inertia & iYf = Y.se3Action(body_placement);
    inertias[joint_index] += iYf;
    nbody++;
98
  }
jcarpent's avatar
jcarpent committed
99
  
100
  inline Model::JointIndex Model::getBodyId (const std::string & name) const
101
  {
102
    return getFrameId(name);
103
104
105
106
  }
  
  inline bool Model::existBodyName (const std::string & name) const
  {
107
    return existFrame(name);
108
109
  }

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

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

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

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

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

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

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

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

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

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

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

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

206
  inline bool Model::addFrame ( const Frame & frame )
207
208
  {
    if( !existFrame(frame.name) )
209
    {
210
211
      frames.push_back(frame);
      nFrames++;
212
213
214
215
216
217
      return true;
    }
    else
    {
      return false;
    }
218
  }
jcarpent's avatar
jcarpent committed
219
220
221
222
223
224
  
  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);
  }
225
226


227
  inline Data::Data (const Model & model)
228
    :joints(0)
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
    ,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)
259
260
    ,JMinvJt()
    ,llt_JMinvJt()
261
    ,lambda_c()
262
263
264
    ,sDUiJt(model.nv,model.nv)
    ,torque_residual(model.nv)
    ,dq_after(model.nv)
265
    ,impulse_c()
266
267
  {
    /* Create data strcture associated to the joints */
268
269
    for(Model::Index i=0;i<(Model::JointIndex)(model.njoint);++i) 
      joints.push_back(CreateJointData::run(model.joints[i]));
270
271
272

    /* Init for CRBA */
    M.fill(0);
273
274
    for(Model::Index i=0;i<(Model::Index)(model.njoint);++i ) { Fcrb[i].resize(6,model.nv); }
    computeLastChild(model);
275
276

    /* Init for Cholesky */
277
    U.setIdentity();
278
    computeParents_fromRow(model);
279
280

    /* Init Jacobian */
jcarpent's avatar
jcarpent committed
281
282
    J.setZero();
    Ag.setZero();
283
284
285
286
287
    
    /* Init universe states relatively to itself */
    
    a[0].setZero();
    v[0].setZero();
288
    a_gf[0] = -model.gravity;
289
290
291
292
293
294
295
296
297
    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);
298
    for( int i=model.njoint-1;i>=0;--i )
299
300
301
302
303
304
305
306
307
308
309
310
311
    {
      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)
  {
312
    for( Model::Index joint=1;joint<(Model::Index)(model.njoint);joint++)
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
    {
      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
332
333
/// @endcond

334
#endif // ifndef __se3_model_hxx__