model.hxx 13 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
#include <boost/bind.hpp>
26
#include <boost/utility.hpp>
27

jcarpent's avatar
jcarpent committed
28
29
/// @cond DEV

30
31
namespace se3
{
32
33
34
35
36
37
38
39
40
41
42
43
  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); }
    };
  }

44
45
  inline std::ostream& operator<< (std::ostream & os, const Model & model)
  {
46
47
    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
48
49
50
51
    {
      os << "  Joint "<< model.names[i] << ": parent=" << model.parents[i]  << std::endl;
    }
    
52
53
    return os;
  }
54
  
jcarpent's avatar
jcarpent committed
55
56
57
58
59
60
61
  template<typename JointModelDerived>
  Model::JointIndex Model::addJoint(const Model::JointIndex parent,
                                    const JointModelBase<JointModelDerived> & joint_model,
                                    const SE3 & joint_placement,
                                    const Eigen::VectorXd & max_effort,
                                    const Eigen::VectorXd & max_velocity,
                                    const Eigen::VectorXd & min_config,
62
63
                                    const Eigen::VectorXd & max_config,
                                    const std::string & joint_name
jcarpent's avatar
jcarpent committed
64
                                    )
65
  {
jcarpent's avatar
jcarpent committed
66
    typedef JointModelDerived D;
67
    assert( (njoint==(int)joints.size())&&(njoint==(int)inertias.size())
jcarpent's avatar
jcarpent committed
68
69
           &&(njoint==(int)parents.size())&&(njoint==(int)jointPlacements.size()) );
    assert((joint_model.nq()>=0) && (joint_model.nv()>=0));
70

jcarpent's avatar
jcarpent committed
71
72
73
74
    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());
75

jcarpent's avatar
jcarpent committed
76
    Model::JointIndex idx = (Model::JointIndex) (njoint++);
77

jcarpent's avatar
jcarpent committed
78
79
80
    joints         .push_back(JointModel(joint_model.derived()));
    boost::get<JointModelDerived>(joints.back()).setIndexes(idx,nq,nv);
    
81
    inertias       .push_back(Inertia::Zero());
82
    parents        .push_back(parent);
jcarpent's avatar
jcarpent committed
83
84
85
86
    jointPlacements.push_back(joint_placement);
    names          .push_back((joint_name!="")?joint_name:randomStringGenerator(8));
    nq += joint_model.nq();
    nv += joint_model.nv();
87

88
    if (D::NV == Eigen::Dynamic)
89
    {
90
91
92
93
      effortLimit.conservativeResize(nv);effortLimit.bottomRows(joint_model.nv()) = max_effort;
      velocityLimit.conservativeResize(nv);velocityLimit.bottomRows(joint_model.nv()) = max_velocity;
      lowerPositionLimit.conservativeResize(nq);lowerPositionLimit.bottomRows(joint_model.nq()) = min_config;
      upperPositionLimit.conservativeResize(nq);upperPositionLimit.bottomRows(joint_model.nq()) = max_config;
94
95
96
97
98
99
100
101
    }
    else
    {
      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;
    }
102
    
103
    neutralConfiguration.conservativeResize(nq);
jcarpent's avatar
jcarpent committed
104
105
    neutralConfiguration.tail(joint_model.nq()) = joint_model.neutralConfiguration();
    
jcarpent's avatar
jcarpent committed
106
107
108
109
    // Init and add joint index to its parent subtrees.
    subtrees.push_back(IndexVector(1));
    subtrees[idx][0] = idx;
    addJointIndexToParentSubtrees(idx);
110
111
112
    return idx;
  }

113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
  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
                                    )
  {
    typedef JointModelDerived D;
    Eigen::VectorXd max_effort, max_velocity, min_config, max_config;

    if (D::NV == Eigen::Dynamic)
    {
      max_effort = Eigen::VectorXd::Constant(joint_model.nv(), std::numeric_limits<double>::max());
      max_velocity = Eigen::VectorXd::Constant(joint_model.nv(), std::numeric_limits<double>::max());
      min_config = Eigen::VectorXd::Constant(joint_model.nq(), std::numeric_limits<double>::max());
      max_config = Eigen::VectorXd::Constant(joint_model.nq(), std::numeric_limits<double>::max());
    }
    else
    {
      max_effort = Eigen::VectorXd::Constant(D::NV, std::numeric_limits<double>::max());
      max_velocity = Eigen::VectorXd::Constant(D::NV, std::numeric_limits<double>::max());
      min_config = Eigen::VectorXd::Constant(D::NQ, std::numeric_limits<double>::max());
      max_config = Eigen::VectorXd::Constant(D::NQ, std::numeric_limits<double>::max());
    }
    return addJoint(parent, joint_model, joint_placement, max_effort, max_velocity, min_config, max_config, joint_name);
  }
  
140
141
  inline int Model::addJointFrame (const JointIndex& jidx,
                                         int         fidx)
142
143
  {
    if (fidx < 0) {
144
145
146
      // FIXED_JOINT is required because the parent can be the universe and its
      // type is FIXED_JOINT
      fidx = getFrameId(names[parents[jidx]], (FrameType)(JOINT | FIXED_JOINT));
147
    }
148
    assert(fidx < frames.size() && "Frame index out of bound");
149
    // Add a the joint frame attached to itself to the frame vector - redundant information but useful.
150
    return addFrame(Frame(names[jidx],jidx,fidx,SE3::Identity(),JOINT));
151
152
  }

153

jcarpent's avatar
jcarpent committed
154
155
  inline void Model::appendBodyToJoint(const Model::JointIndex joint_index,
                                       const Inertia & Y,
156
                                       const SE3 & body_placement)
157
  {
jcarpent's avatar
jcarpent committed
158
159
160
    const Inertia & iYf = Y.se3Action(body_placement);
    inertias[joint_index] += iYf;
    nbody++;
161
  }
162

163
164
165
166
  inline int Model::addBodyFrame (const std::string & body_name,
                                  const JointIndex  & parentJoint,
                                  const SE3         & body_placement,
                                        int           previousFrame)
167
168
  {
    if (previousFrame < 0) {
169
170
171
      // FIXED_JOINT is required because the parent can be the universe and its
      // type is FIXED_JOINT
      previousFrame = getFrameId(names[parentJoint], (FrameType)(JOINT | FIXED_JOINT));
172
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
173
    assert(previousFrame < frames.size() && "Frame index out of bound");
174
    return addFrame(Frame(body_name, parentJoint, previousFrame, body_placement, BODY));
175
  }
jcarpent's avatar
jcarpent committed
176
  
177
  inline Model::JointIndex Model::getBodyId (const std::string & name) const
178
  {
179
    return getFrameId(name, BODY);
180
181
182
183
  }
  
  inline bool Model::existBodyName (const std::string & name) const
  {
184
    return existFrame(name, BODY);
185
186
  }

187
  inline const std::string& Model::getBodyName (const Model::JointIndex index) const
188
189
  {
    assert( index < (Model::Index)nbody );
190
    return frames[index].name;
191
192
  }

193
  inline Model::JointIndex Model::getJointId (const std::string & name) const
194
  {
jcarpent's avatar
jcarpent committed
195
196
    typedef std::vector<std::string>::iterator::difference_type it_diff_t;
    it_diff_t res = std::find(names.begin(),names.end(),name) - names.begin();
197
    assert( (res<INT_MAX) && "Id superior to int range. Should never happen.");
jcarpent's avatar
jcarpent committed
198
    assert( (res>=0)&&(res<(it_diff_t) joints.size()) && "The joint name you asked does not exist" );
199
    return Model::JointIndex(res);
200
201
202
203
204
205
206
  }
  
  inline bool Model::existJointName (const std::string & name) const
  {
    return (names.end() != std::find(names.begin(),names.end(),name));
  }

207
  inline const std::string& Model::getJointName (const JointIndex index) const
208
  {
209
    assert( index < (Model::JointIndex)joints.size() );
210
211
212
    return names[index];
  }

213
  inline Model::FrameIndex Model::getFrameId ( const std::string & name, const FrameType & type ) const
214
  {
215
216
    std::vector<Frame>::const_iterator it = std::find_if( frames.begin()
                                                        , frames.end()
217
                                                        , details::FilterFrame (name, type)
218
                                                        );
219
220
221
    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");
222
    return Model::FrameIndex(it - frames.begin());
223
224
  }

225
  inline bool Model::existFrame ( const std::string & name, const FrameType & type) const
226
  {
227
228
    return std::find_if( frames.begin(), frames.end(),
        details::FilterFrame (name, type)) != frames.end();
229
230
231
  }


232
  inline int Model::addFrame ( const Frame & frame )
233
  {
234
    if( !existFrame(frame.name, frame.type) )
235
    {
236
237
      frames.push_back(frame);
      nFrames++;
238
      return nFrames - 1;
239
240
241
    }
    else
    {
242
      return -1;
243
    }
244
  }
jcarpent's avatar
jcarpent committed
245
246
247
248
249
250
  
  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);
  }
251
252


253
  inline Data::Data (const Model & model)
254
    :joints(0)
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
    ,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)
285
286
    ,JMinvJt()
    ,llt_JMinvJt()
287
    ,lambda_c()
288
289
290
    ,sDUiJt(model.nv,model.nv)
    ,torque_residual(model.nv)
    ,dq_after(model.nv)
291
    ,impulse_c()
292
293
  {
    /* Create data strcture associated to the joints */
294
295
    for(Model::Index i=0;i<(Model::JointIndex)(model.njoint);++i) 
      joints.push_back(CreateJointData::run(model.joints[i]));
296
297
298

    /* Init for CRBA */
    M.fill(0);
299
300
    for(Model::Index i=0;i<(Model::Index)(model.njoint);++i ) { Fcrb[i].resize(6,model.nv); }
    computeLastChild(model);
301
302

    /* Init for Cholesky */
303
    U.setIdentity();
304
    computeParents_fromRow(model);
305
306

    /* Init Jacobian */
jcarpent's avatar
jcarpent committed
307
308
    J.setZero();
    Ag.setZero();
309
310
311
312
313
    
    /* Init universe states relatively to itself */
    
    a[0].setZero();
    v[0].setZero();
314
    a_gf[0] = -model.gravity;
315
316
317
318
319
320
321
322
323
    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);
324
    for( int i=model.njoint-1;i>=0;--i )
325
326
327
328
329
330
331
332
333
334
335
336
337
    {
      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)
  {
338
    for( Model::Index joint=1;joint<(Model::Index)(model.njoint);joint++)
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
    {
      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
358
359
/// @endcond

360
#endif // ifndef __se3_model_hxx__