model.hxx 11.4 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
namespace se3
{
31
32
33
34
35
36
37
38
39
40
41
42
  namespace details
  {
    struct FilterFrame {
      const std::string& name;
      const FrameType & typeMask;
      FilterFrame (const std::string& name, const FrameType& typeMask)
        : name (name), typeMask (typeMask) {}
      bool operator() (const Frame& frame) const
      { return (typeMask & frame.type) && (name == frame.name); }
    };
  }

43
44
  inline std::ostream& operator<< (std::ostream & os, const Model & model)
  {
45
46
    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
47
48
49
50
    {
      os << "  Joint "<< model.names[i] << ": parent=" << model.parents[i]  << std::endl;
    }
    
51
52
    return os;
  }
53
  
jcarpent's avatar
jcarpent committed
54
55
56
57
58
59
60
61
62
63
  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
                                    )
64
  {
jcarpent's avatar
jcarpent committed
65
    typedef JointModelDerived D;
66
    assert( (njoint==(int)joints.size())&&(njoint==(int)inertias.size())
jcarpent's avatar
jcarpent committed
67
68
69
70
71
72
73
74
75
76
77
78
79
           &&(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);
    
80
    inertias       .push_back(Inertia::Zero());
81
    parents        .push_back(parent);
jcarpent's avatar
jcarpent committed
82
83
84
85
    jointPlacements.push_back(joint_placement);
    names          .push_back((joint_name!="")?joint_name:randomStringGenerator(8));
    nq += joint_model.nq();
    nv += joint_model.nv();
86
    
jcarpent's avatar
jcarpent committed
87
88
89
90
    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;
91
    
92
    neutralConfiguration.conservativeResize(nq);
jcarpent's avatar
jcarpent committed
93
94
    neutralConfiguration.tail(joint_model.nq()) = joint_model.neutralConfiguration();
    
jcarpent's avatar
jcarpent committed
95
96
97
98
    // Init and add joint index to its parent subtrees.
    subtrees.push_back(IndexVector(1));
    subtrees[idx][0] = idx;
    addJointIndexToParentSubtrees(idx);
99
100
101
    return idx;
  }

102
103
  inline int Model::addJointFrame (const JointIndex& jidx,
                                         int         fidx)
104
105
  {
    if (fidx < 0) {
106
      fidx = getFrameId(names[parents[jidx]], JOINT);
107
108
109
110
    }
    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.
111
    return addFrame(Frame(names[jidx],jidx,fidx,SE3::Identity(),JOINT));
112
113
  }

jcarpent's avatar
jcarpent committed
114
115
  inline void Model::appendBodyToJoint(const Model::JointIndex joint_index,
                                       const Inertia & Y,
116
                                       const SE3 & body_placement)
117
  {
jcarpent's avatar
jcarpent committed
118
119
120
    const Inertia & iYf = Y.se3Action(body_placement);
    inertias[joint_index] += iYf;
    nbody++;
121
  }
122

123
124
125
126
  inline int Model::addBodyFrame (const std::string & body_name,
                                  const JointIndex  & parentJoint,
                                  const SE3         & body_placement,
                                        int           previousFrame)
127
128
  {
    if (previousFrame < 0) {
129
      previousFrame = getFrameId(names[parentJoint], JOINT);
130
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
131
    assert(previousFrame < frames.size() && "Frame index out of bound");
132
    return addFrame(Frame(body_name, parentJoint, previousFrame, body_placement, BODY));
133
  }
jcarpent's avatar
jcarpent committed
134
  
135
  inline Model::JointIndex Model::getBodyId (const std::string & name) const
136
  {
137
    return getFrameId(name, BODY);
138
139
140
141
  }
  
  inline bool Model::existBodyName (const std::string & name) const
  {
142
    return existFrame(name, BODY);
143
144
  }

145
  inline const std::string& Model::getBodyName (const Model::JointIndex index) const
146
147
  {
    assert( index < (Model::Index)nbody );
148
    return getFrameName(index);
149
150
  }

151
  inline Model::JointIndex Model::getJointId (const std::string & name) const
152
  {
jcarpent's avatar
jcarpent committed
153
154
    typedef std::vector<std::string>::iterator::difference_type it_diff_t;
    it_diff_t res = std::find(names.begin(),names.end(),name) - names.begin();
155
    assert( (res<INT_MAX) && "Id superior to int range. Should never happen.");
jcarpent's avatar
jcarpent committed
156
    assert( (res>=0)&&(res<(it_diff_t) joints.size()) && "The joint name you asked does not exist" );
157
    return Model::JointIndex(res);
158
159
160
161
162
163
164
  }
  
  inline bool Model::existJointName (const std::string & name) const
  {
    return (names.end() != std::find(names.begin(),names.end(),name));
  }

165
  inline const std::string& Model::getJointName (const JointIndex index) const
166
  {
167
    assert( index < (Model::JointIndex)joints.size() );
168
169
170
    return names[index];
  }

171
  inline Model::FrameIndex Model::getFrameId ( const std::string & name, const FrameType & type ) const
172
  {
173
174
    std::vector<Frame>::const_iterator it = std::find_if( frames.begin()
                                                        , frames.end()
175
                                                        , details::FilterFrame (name, type)
176
                                                        );
177
178
179
    assert (it != frames.end() && "Frame not found");
    assert ((std::find_if( boost::next(it), frames.end(), details::FilterFrame (name, type)) == frames.end())
        && "Several frames match the filter");
180
    return Model::FrameIndex(it - frames.begin());
181
182
  }

183
  inline bool Model::existFrame ( const std::string & name, const FrameType & type) const
184
  {
185
186
    return std::find_if( frames.begin(), frames.end(),
        details::FilterFrame (name, type)) != frames.end();
187
188
  }

189
  inline const std::string & Model::getFrameName ( const FrameIndex index ) const
190
  {
191
    return frames[index].name;
192
193
  }

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

199
200
  inline FrameType Model::getFrameType( const FrameIndex index ) const
  {
201
    return frames[index].type;
202
203
  }

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

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


230
  inline Data::Data (const Model & model)
231
    :joints(0)
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
259
260
261
    ,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)
262
263
    ,JMinvJt()
    ,llt_JMinvJt()
264
    ,lambda_c()
265
266
267
    ,sDUiJt(model.nv,model.nv)
    ,torque_residual(model.nv)
    ,dq_after(model.nv)
268
    ,impulse_c()
269
270
  {
    /* Create data strcture associated to the joints */
271
272
    for(Model::Index i=0;i<(Model::JointIndex)(model.njoint);++i) 
      joints.push_back(CreateJointData::run(model.joints[i]));
273
274
275

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

    /* Init for Cholesky */
280
    U.setIdentity();
281
    computeParents_fromRow(model);
282
283

    /* Init Jacobian */
jcarpent's avatar
jcarpent committed
284
285
    J.setZero();
    Ag.setZero();
286
287
288
289
290
    
    /* 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
    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);
301
    for( int i=model.njoint-1;i>=0;--i )
302
303
304
305
306
307
308
309
310
311
312
313
314
    {
      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)
  {
315
    for( Model::Index joint=1;joint<(Model::Index)(model.njoint);joint++)
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
    {
      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__