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
23
// 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"

24
25
#include <boost/bind.hpp>

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

28
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)
      {
35
	os << "  Joint "<<model.names[i] << ": parent=" << model.parents[i]  << std::endl;
36
37
38
39
      }

    return os;
  }
40
  
jcarpent's avatar
jcarpent committed
41
42
43
44
45
46
47
48
49
50
  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
                                    )
51
  {
jcarpent's avatar
jcarpent committed
52
    typedef JointModelDerived D;
53
    assert( (njoint==(int)joints.size())&&(njoint==(int)inertias.size())
jcarpent's avatar
jcarpent committed
54
55
56
57
58
59
60
61
62
63
64
65
66
           &&(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);
    
67
    inertias       .push_back(Inertia::Zero());
68
    parents        .push_back(parent);
jcarpent's avatar
jcarpent committed
69
70
71
72
    jointPlacements.push_back(joint_placement);
    names          .push_back((joint_name!="")?joint_name:randomStringGenerator(8));
    nq += joint_model.nq();
    nv += joint_model.nv();
73
    
jcarpent's avatar
jcarpent committed
74
75
76
77
    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;
78
    
79
    neutralConfiguration.conservativeResize(nq);
jcarpent's avatar
jcarpent committed
80
81
82
83
84
    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);
    
85
86
87
    return idx;
  }

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

jcarpent's avatar
jcarpent committed
96
97
    addFrame((body_name!="")?body_name:randomStringGenerator(8), joint_index, body_placement, BODY);
    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
219
  }

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


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

    /* Init Jacobian */
    J.fill(0);
    
    /* Init universe states relatively to itself */
    
    a[0].setZero();
    v[0].setZero();
290
    a_gf[0] = -model.gravity;
291
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
    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
334
335
/// @endcond

336
#endif // ifndef __se3_model_hxx__