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
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
107
108
      // 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));
109
    }
110
    assert(fidx < frames.size() && "Frame index out of bound");
111
    // Add a the joint frame attached to itself to the frame vector - redundant information but useful.
112
    return addFrame(Frame(names[jidx],jidx,fidx,SE3::Identity(),JOINT));
113
114
  }

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

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

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

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

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

174
  inline Model::FrameIndex Model::getFrameId ( const std::string & name, const FrameType & type ) const
175
  {
176
177
    std::vector<Frame>::const_iterator it = std::find_if( frames.begin()
                                                        , frames.end()
178
                                                        , details::FilterFrame (name, type)
179
                                                        );
180
181
182
    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");
183
    return Model::FrameIndex(it - frames.begin());
184
185
  }

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

192
  inline const std::string & Model::getFrameName ( const FrameIndex index ) const
193
  {
194
    return frames[index].name;
195
196
  }

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

202
203
  inline FrameType Model::getFrameType( const FrameIndex index ) const
  {
204
    return frames[index].type;
205
206
  }

207
  inline const SE3 & Model::getFramePlacement( const FrameIndex index ) const
208
  {
209
    return frames[index].placement;
210
211
  }

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


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

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

    /* Init for Cholesky */
283
    U.setIdentity();
284
    computeParents_fromRow(model);
285
286

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

340
#endif // ifndef __se3_model_hxx__