model.hxx 10.7 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
31
32
namespace se3
{
  inline std::ostream& operator<< (std::ostream & os, const Model & model)
  {
33
34
    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
35
36
37
38
    {
      os << "  Joint "<< model.names[i] << ": parent=" << model.parents[i]  << std::endl;
    }
    
39
40
    return os;
  }
41
  
jcarpent's avatar
jcarpent committed
42
43
44
45
46
47
48
49
50
51
  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
                                    )
52
  {
jcarpent's avatar
jcarpent committed
53
    typedef JointModelDerived D;
54
    assert( (njoint==(int)joints.size())&&(njoint==(int)inertias.size())
jcarpent's avatar
jcarpent committed
55
56
57
58
59
60
61
62
63
64
65
66
67
           &&(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);
    
68
    inertias       .push_back(Inertia::Zero());
69
    parents        .push_back(parent);
jcarpent's avatar
jcarpent committed
70
71
72
73
    jointPlacements.push_back(joint_placement);
    names          .push_back((joint_name!="")?joint_name:randomStringGenerator(8));
    nq += joint_model.nq();
    nv += joint_model.nv();
74
    
jcarpent's avatar
jcarpent committed
75
76
77
78
    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;
79
    
80
    neutralConfiguration.conservativeResize(nq);
jcarpent's avatar
jcarpent committed
81
82
    neutralConfiguration.tail(joint_model.nq()) = joint_model.neutralConfiguration();
    
jcarpent's avatar
jcarpent committed
83
84
85
86
    // Init and add joint index to its parent subtrees.
    subtrees.push_back(IndexVector(1));
    subtrees[idx][0] = idx;
    addJointIndexToParentSubtrees(idx);
87
88
89
    return idx;
  }

90
91
  inline int Model::addJointFrame (const JointIndex& jidx,
                                         int         fidx)
92
93
94
95
96
97
98
  {
    if (fidx < 0) {
      fidx = getFrameId(names[parents[jidx]]);
    }
    if (fidx >= frames.size())
      throw std::invalid_argument ("Frame not found");
    // Add a the joint frame attached to itself to the frame vector - redundant information but useful.
99
    return addFrame(Frame(names[jidx],jidx,fidx,SE3::Identity(),JOINT));
100
101
  }

jcarpent's avatar
jcarpent committed
102
103
  inline void Model::appendBodyToJoint(const Model::JointIndex joint_index,
                                       const Inertia & Y,
104
                                       const SE3 & body_placement)
105
  {
jcarpent's avatar
jcarpent committed
106
107
108
    const Inertia & iYf = Y.se3Action(body_placement);
    inertias[joint_index] += iYf;
    nbody++;
109
  }
110

111
112
113
114
  inline int Model::addBodyFrame (const std::string & body_name,
                                  const JointIndex  & parentJoint,
                                  const SE3         & body_placement,
                                        int           previousFrame)
115
116
117
118
  {
    if (previousFrame < 0) {
      previousFrame = getFrameId(names[parentJoint]);
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
119
    assert(previousFrame < frames.size() && "Frame index out of bound");
120
    return addFrame(Frame(body_name, parentJoint, previousFrame, body_placement, BODY));
121
  }
jcarpent's avatar
jcarpent committed
122
  
123
  inline Model::JointIndex Model::getBodyId (const std::string & name) const
124
  {
125
    return getFrameId(name);
126
127
128
129
  }
  
  inline bool Model::existBodyName (const std::string & name) const
  {
130
    return existFrame(name);
131
132
  }

133
  inline const std::string& Model::getBodyName (const Model::JointIndex index) const
134
135
  {
    assert( index < (Model::Index)nbody );
136
    return getFrameName(index);
137
138
  }

139
  inline Model::JointIndex Model::getJointId (const std::string & name) const
140
  {
jcarpent's avatar
jcarpent committed
141
142
    typedef std::vector<std::string>::iterator::difference_type it_diff_t;
    it_diff_t res = std::find(names.begin(),names.end(),name) - names.begin();
143
    assert( (res<INT_MAX) && "Id superior to int range. Should never happen.");
jcarpent's avatar
jcarpent committed
144
    assert( (res>=0)&&(res<(it_diff_t) joints.size()) && "The joint name you asked does not exist" );
145
    return Model::JointIndex(res);
146
147
148
149
150
151
152
  }
  
  inline bool Model::existJointName (const std::string & name) const
  {
    return (names.end() != std::find(names.begin(),names.end(),name));
  }

153
  inline const std::string& Model::getJointName (const JointIndex index) const
154
  {
155
    assert( index < (Model::JointIndex)joints.size() );
156
157
158
    return names[index];
  }

159
  inline Model::FrameIndex Model::getFrameId ( const std::string & name ) const
160
  {
161
162
    std::vector<Frame>::const_iterator it = std::find_if( frames.begin()
                                                        , frames.end()
163
164
                                                        , boost::bind(&Frame::name, _1) == name
                                                        );
165
    return Model::FrameIndex(it - frames.begin());
166
167
168
169
  }

  inline bool Model::existFrame ( const std::string & name ) const
  {
170
    return std::find_if( frames.begin(), frames.end(), boost::bind(&Frame::name, _1) == name) != frames.end();
171
172
  }

173
  inline const std::string & Model::getFrameName ( const FrameIndex index ) const
174
  {
175
    return frames[index].name;
176
177
  }

jcarpent's avatar
jcarpent committed
178
  inline Model::JointIndex Model::getFrameParent( const FrameIndex index ) const
179
  {
180
    return frames[index].parent;
181
182
  }

183
184
  inline FrameType Model::getFrameType( const FrameIndex index ) const
  {
185
    return frames[index].type;
186
187
  }

188
  inline const SE3 & Model::getFramePlacement( const FrameIndex index ) const
189
  {
190
    return frames[index].placement;
191
192
  }

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


214
  inline Data::Data (const Model & model)
215
    :joints(0)
216
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
    ,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)
246
247
    ,JMinvJt()
    ,llt_JMinvJt()
248
    ,lambda_c()
249
250
251
    ,sDUiJt(model.nv,model.nv)
    ,torque_residual(model.nv)
    ,dq_after(model.nv)
252
    ,impulse_c()
253
254
  {
    /* Create data strcture associated to the joints */
255
256
    for(Model::Index i=0;i<(Model::JointIndex)(model.njoint);++i) 
      joints.push_back(CreateJointData::run(model.joints[i]));
257
258
259

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

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

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

321
#endif // ifndef __se3_model_hxx__