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
#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
62
63
64
  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
                                    )
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
  inline int Model::addJointFrame (const JointIndex& jidx,
                                         int         fidx)
115
116
  {
    if (fidx < 0) {
117
118
119
      // 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));
120
    }
121
    assert(fidx < frames.size() && "Frame index out of bound");
122
    // Add a the joint frame attached to itself to the frame vector - redundant information but useful.
123
    return addFrame(Frame(names[jidx],jidx,fidx,SE3::Identity(),JOINT));
124
125
  }

jcarpent's avatar
jcarpent committed
126
127
  inline void Model::appendBodyToJoint(const Model::JointIndex joint_index,
                                       const Inertia & Y,
128
                                       const SE3 & body_placement)
129
  {
jcarpent's avatar
jcarpent committed
130
131
132
    const Inertia & iYf = Y.se3Action(body_placement);
    inertias[joint_index] += iYf;
    nbody++;
133
  }
134

135
136
137
138
  inline int Model::addBodyFrame (const std::string & body_name,
                                  const JointIndex  & parentJoint,
                                  const SE3         & body_placement,
                                        int           previousFrame)
139
140
  {
    if (previousFrame < 0) {
141
142
143
      // 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));
144
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
145
    assert(previousFrame < frames.size() && "Frame index out of bound");
146
    return addFrame(Frame(body_name, parentJoint, previousFrame, body_placement, BODY));
147
  }
jcarpent's avatar
jcarpent committed
148
  
149
  inline Model::JointIndex Model::getBodyId (const std::string & name) const
150
  {
151
    return getFrameId(name, BODY);
152
153
154
155
  }
  
  inline bool Model::existBodyName (const std::string & name) const
  {
156
    return existFrame(name, BODY);
157
158
  }

159
  inline const std::string& Model::getBodyName (const Model::JointIndex index) const
160
161
  {
    assert( index < (Model::Index)nbody );
162
    return frames[index].name;
163
164
  }

165
  inline Model::JointIndex Model::getJointId (const std::string & name) const
166
  {
jcarpent's avatar
jcarpent committed
167
168
    typedef std::vector<std::string>::iterator::difference_type it_diff_t;
    it_diff_t res = std::find(names.begin(),names.end(),name) - names.begin();
169
    assert( (res<INT_MAX) && "Id superior to int range. Should never happen.");
jcarpent's avatar
jcarpent committed
170
    assert( (res>=0)&&(res<(it_diff_t) joints.size()) && "The joint name you asked does not exist" );
171
    return Model::JointIndex(res);
172
173
174
175
176
177
178
  }
  
  inline bool Model::existJointName (const std::string & name) const
  {
    return (names.end() != std::find(names.begin(),names.end(),name));
  }

179
  inline const std::string& Model::getJointName (const JointIndex index) const
180
  {
181
    assert( index < (Model::JointIndex)joints.size() );
182
183
184
    return names[index];
  }

185
  inline Model::FrameIndex Model::getFrameId ( const std::string & name, const FrameType & type ) const
186
  {
187
188
    std::vector<Frame>::const_iterator it = std::find_if( frames.begin()
                                                        , frames.end()
189
                                                        , details::FilterFrame (name, type)
190
                                                        );
191
192
193
    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");
194
    return Model::FrameIndex(it - frames.begin());
195
196
  }

197
  inline bool Model::existFrame ( const std::string & name, const FrameType & type) const
198
  {
199
200
    return std::find_if( frames.begin(), frames.end(),
        details::FilterFrame (name, type)) != frames.end();
201
202
203
  }


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


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

    /* Init for CRBA */
    M.fill(0);
271
272
    for(Model::Index i=0;i<(Model::Index)(model.njoint);++i ) { Fcrb[i].resize(6,model.nv); }
    computeLastChild(model);
273
274

    /* Init for Cholesky */
275
    U.setIdentity();
276
    computeParents_fromRow(model);
277
278

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

332
#endif // ifndef __se3_model_hxx__