model.hxx 12.5 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
89
90
91
92
93
94
    // Optimal efficiency here would be using the static-dim bottomRows, while specifying the dimension in argument in the case where D::NV is Eigen::Dynamic.
    // However, this option is not compiling in Travis (why?).
    // As efficiency of Model::addJoint is not critical, the dynamic bottomRows is used here.
    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;
95
    
96
    neutralConfiguration.conservativeResize(nq);
jcarpent's avatar
jcarpent committed
97
98
    neutralConfiguration.tail(joint_model.nq()) = joint_model.neutralConfiguration();
    
jcarpent's avatar
jcarpent committed
99
100
101
102
    // Init and add joint index to its parent subtrees.
    subtrees.push_back(IndexVector(1));
    subtrees[idx][0] = idx;
    addJointIndexToParentSubtrees(idx);
103
104
105
    return idx;
  }

106
107
108
109
110
111
112
113
114
115
  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;

116
117
118
119
120
    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());

121
122
123
    return addJoint(parent, joint_model, joint_placement, max_effort, max_velocity, min_config, max_config, joint_name);
  }
  
124
125
  inline int Model::addJointFrame (const JointIndex& jidx,
                                         int         fidx)
126
127
  {
    if (fidx < 0) {
128
129
130
      // 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));
131
    }
132
    assert(fidx < frames.size() && "Frame index out of bound");
133
    // Add a the joint frame attached to itself to the frame vector - redundant information but useful.
134
    return addFrame(Frame(names[jidx],jidx,fidx,SE3::Identity(),JOINT));
135
136
  }

137

jcarpent's avatar
jcarpent committed
138
139
  inline void Model::appendBodyToJoint(const Model::JointIndex joint_index,
                                       const Inertia & Y,
140
                                       const SE3 & body_placement)
141
  {
jcarpent's avatar
jcarpent committed
142
143
144
    const Inertia & iYf = Y.se3Action(body_placement);
    inertias[joint_index] += iYf;
    nbody++;
145
  }
146

147
148
149
150
  inline int Model::addBodyFrame (const std::string & body_name,
                                  const JointIndex  & parentJoint,
                                  const SE3         & body_placement,
                                        int           previousFrame)
151
152
  {
    if (previousFrame < 0) {
153
154
155
      // 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));
156
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
157
    assert(previousFrame < frames.size() && "Frame index out of bound");
158
    return addFrame(Frame(body_name, parentJoint, previousFrame, body_placement, BODY));
159
  }
jcarpent's avatar
jcarpent committed
160
  
161
  inline Model::JointIndex Model::getBodyId (const std::string & name) const
162
  {
163
    return getFrameId(name, BODY);
164
165
166
167
  }
  
  inline bool Model::existBodyName (const std::string & name) const
  {
168
    return existFrame(name, BODY);
169
170
  }

171
  inline const std::string& Model::getBodyName (const Model::JointIndex index) const
172
173
  {
    assert( index < (Model::Index)nbody );
174
    return frames[index].name;
175
176
  }

177
  inline Model::JointIndex Model::getJointId (const std::string & name) const
178
  {
jcarpent's avatar
jcarpent committed
179
180
    typedef std::vector<std::string>::iterator::difference_type it_diff_t;
    it_diff_t res = std::find(names.begin(),names.end(),name) - names.begin();
181
    assert( (res<INT_MAX) && "Id superior to int range. Should never happen.");
jcarpent's avatar
jcarpent committed
182
    assert( (res>=0)&&(res<(it_diff_t) joints.size()) && "The joint name you asked does not exist" );
183
    return Model::JointIndex(res);
184
185
186
187
188
189
190
  }
  
  inline bool Model::existJointName (const std::string & name) const
  {
    return (names.end() != std::find(names.begin(),names.end(),name));
  }

191
  inline const std::string& Model::getJointName (const JointIndex index) const
192
  {
193
    assert( index < (Model::JointIndex)joints.size() );
194
195
196
    return names[index];
  }

197
  inline Model::FrameIndex Model::getFrameId ( const std::string & name, const FrameType & type ) const
198
  {
199
200
    std::vector<Frame>::const_iterator it = std::find_if( frames.begin()
                                                        , frames.end()
201
                                                        , details::FilterFrame (name, type)
202
                                                        );
203
204
205
    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");
206
    return Model::FrameIndex(it - frames.begin());
207
208
  }

209
  inline bool Model::existFrame ( const std::string & name, const FrameType & type) const
210
  {
211
212
    return std::find_if( frames.begin(), frames.end(),
        details::FilterFrame (name, type)) != frames.end();
213
214
215
  }


216
  inline int Model::addFrame ( const Frame & frame )
217
  {
218
    if( !existFrame(frame.name, frame.type) )
219
    {
220
221
      frames.push_back(frame);
      nFrames++;
222
      return nFrames - 1;
223
224
225
    }
    else
    {
226
      return -1;
227
    }
228
  }
jcarpent's avatar
jcarpent committed
229
230
231
232
233
234
  
  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);
  }
235
236


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

    /* Init for CRBA */
    M.fill(0);
283
284
    for(Model::Index i=0;i<(Model::Index)(model.njoint);++i ) { Fcrb[i].resize(6,model.nv); }
    computeLastChild(model);
285
286

    /* Init for Cholesky */
287
    U.setIdentity();
288
    computeParents_fromRow(model);
289
290

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

344
#endif // ifndef __se3_model_hxx__