model.hxx 11.2 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
70
71
72
73
74
75
76
77
78
79
80
           &&(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);
    
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
    
jcarpent's avatar
jcarpent committed
88
89
90
91
    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;
92
    
93
    neutralConfiguration.conservativeResize(nq);
jcarpent's avatar
jcarpent committed
94
95
    neutralConfiguration.tail(joint_model.nq()) = joint_model.neutralConfiguration();
    
jcarpent's avatar
jcarpent committed
96
97
98
99
    // Init and add joint index to its parent subtrees.
    subtrees.push_back(IndexVector(1));
    subtrees[idx][0] = idx;
    addJointIndexToParentSubtrees(idx);
100
101
102
    return idx;
  }

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

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

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

149
  inline const std::string& Model::getBodyName (const Model::JointIndex index) const
150
151
  {
    assert( index < (Model::Index)nbody );
152
    return frames[index].name;
153
154
  }

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

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

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

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


194
  inline int Model::addFrame ( const Frame & frame )
195
  {
196
    if( !existFrame(frame.name, frame.type) )
197
    {
198
199
      frames.push_back(frame);
      nFrames++;
200
      return nFrames - 1;
201
202
203
    }
    else
    {
204
      return -1;
205
    }
206
  }
jcarpent's avatar
jcarpent committed
207
208
209
210
211
212
  
  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);
  }
213
214


215
  inline Data::Data (const Model & model)
216
    :joints(0)
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
    ,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)
247
248
    ,JMinvJt()
    ,llt_JMinvJt()
249
    ,lambda_c()
250
251
252
    ,sDUiJt(model.nv,model.nv)
    ,torque_residual(model.nv)
    ,dq_after(model.nv)
253
    ,impulse_c()
254
255
  {
    /* Create data strcture associated to the joints */
256
257
    for(Model::Index i=0;i<(Model::JointIndex)(model.njoint);++i) 
      joints.push_back(CreateJointData::run(model.joints[i]));
258
259
260

    /* Init for CRBA */
    M.fill(0);
261
262
    for(Model::Index i=0;i<(Model::Index)(model.njoint);++i ) { Fcrb[i].resize(6,model.nv); }
    computeLastChild(model);
263
264

    /* Init for Cholesky */
265
    U.setIdentity();
266
    computeParents_fromRow(model);
267
268

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

322
#endif // ifndef __se3_model_hxx__