compute-all-terms.hpp 7.89 KB
Newer Older
1
//
2
// Copyright (c) 2015-2017 CNRS
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
//
// 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/>.

18
19
20
21
22
#ifndef __se3_compute_all_terms_hpp__
#define __se3_compute_all_terms_hpp__

#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/multibody/model.hpp"
jcarpent's avatar
jcarpent committed
23
#include "pinocchio/multibody/data.hpp"
24
#include "pinocchio/spatial/act-on-set.hpp"
25
#include "pinocchio/algorithm/center-of-mass.hpp"
jcarpent's avatar
jcarpent committed
26
#include "pinocchio/algorithm/energy.hpp"
27
#include "pinocchio/algorithm/check.hpp"
28
29
30

namespace se3
{
jcarpent's avatar
jcarpent committed
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
  ///
  /// \brief Computes efficiently all the terms needed for dynamic simulation. It is equivalent to the call at the same time to:
  ///         - se3::forwardKinematics
  ///         - se3::crba
  ///         - se3::nonLinearEffects
  ///         - se3::computeJacobians
  ///         - se3::centerOfMass
  ///         - se3::jacobianCenterOfMass
  ///         - se3::kineticEnergy
  ///         - se3::potentialEnergy
  ///
  /// \param[in] model The model structure of the rigid body system.
  /// \param[in] data The data structure of the rigid body system.
  /// \param[in] q The joint configuration vector (dim model.nq).
  /// \param[in] v The joint velocity vector (dim model.nv).
  ///
  /// \return All the results are stored in data. Please refer to the specific algorithm for further details.
48
49
50
51
52
53
54
55
  inline void
  computeAllTerms(const Model & model,
                  Data & data,
                  const Eigen::VectorXd & q,
                  const Eigen::VectorXd & v);

} // namespace se3

jcarpent's avatar
jcarpent committed
56

57
58
59
/* --- Details -------------------------------------------------------------------- */
namespace se3
{
jcarpent's avatar
jcarpent committed
60
  
61
62
63
64
65
66
67
68
69
70
71
72
  struct CATForwardStep : public fusion::JointVisitor<CATForwardStep>
  {
    typedef boost::fusion::vector< const se3::Model &,
    se3::Data &,
    const Eigen::VectorXd &,
    const Eigen::VectorXd &
    > ArgsType;

    JOINT_VISITOR_INIT(CATForwardStep);

    template<typename JointModel>
    static void algo(const se3::JointModelBase<JointModel> & jmodel,
73
                     se3::JointDataBase<typename JointModel::JointDataDerived> & jdata,
74
75
76
77
78
79
80
81
                     const se3::Model & model,
                     se3::Data & data,
                     const Eigen::VectorXd & q,
                     const Eigen::VectorXd & v)
    {
      using namespace Eigen;
      using namespace se3;

82
83
      const Model::JointIndex & i = (Model::JointIndex) jmodel.id();
      const Model::JointIndex & parent = model.parents[i];
jcarpent's avatar
jcarpent committed
84
      
85
      jmodel.calc(jdata.derived(),q,v);
jcarpent's avatar
jcarpent committed
86
      
87
      // CRBA
88
89
      data.liMi[i] = model.jointPlacements[i]*jdata.M();
      data.Ycrb[i] = model.inertias[i];
90
91

      // Jacobian + NLE
92
      data.v[i] = jdata.v();
93
94
95

      if(parent>0)
      {
96
97
        data.oMi[i] = data.oMi[parent]*data.liMi[i];
        data.v[i] += data.liMi[i].actInv(data.v[parent]);
98
99
      }
      else
100
        data.oMi[i] = data.liMi[i];
101

102
      jmodel.jointCols(data.J) = data.oMi[i].act(jdata.S());
103

104
105
106
107
108
      data.a_gf[i] = data.a[i] = jdata.c() + (data.v[i] ^ jdata.v());
      if (parent > 0)
        data.a[i] += data.liMi[i].actInv(data.a[parent]);
      
      data.a_gf[i] += data.liMi[i].actInv(data.a_gf[parent]);
109

110
      data.f[i] = model.inertias[i]*data.a_gf[i] + model.inertias[i].vxiv(data.v[i]); // -f_ext
jcarpent's avatar
jcarpent committed
111
112
113
114
115
116
117
118
119
      
      // CoM
      const double mass = model.inertias[i].mass();
      const SE3::Vector3 & lever = model.inertias[i].lever();
      
      data.com[i]  = mass * lever;
      data.mass[i] = mass;

      data.vcom[i] = mass * (data.v[i].angular().cross(lever) + data.v[i].linear());
120
121
122
123
124
125
126
127
128
129
130
131
132
    }

  };

  struct CATBackwardStep : public fusion::JointVisitor<CATBackwardStep>
  {
    typedef boost::fusion::vector<const Model &,
    Data &>  ArgsType;

    JOINT_VISITOR_INIT(CATBackwardStep);

    template<typename JointModel>
    static void algo(const JointModelBase<JointModel> & jmodel,
133
                     JointDataBase<typename JointModel::JointDataDerived> & jdata,
134
135
136
137
138
139
140
141
142
143
                     const Model & model,
                     Data & data)
    {
      /*
       * F[1:6,i] = Y*S
       * M[i,SUBTREE] = S'*F[1:6,SUBTREE]
       * if li>0
       *   Yli += liXi Yi
       *   F[1:6,SUBTREE] = liXi F[1:6,SUBTREE]
       */
144
145
      const Model::JointIndex & i = (Model::JointIndex) jmodel.id();
      const Model::JointIndex & parent = model.parents[i];
jcarpent's avatar
jcarpent committed
146
      const SE3 & oMi = data.oMi[i];
147
148

      /* F[1:6,i] = Y*S */
149
      jmodel.jointCols(data.Fcrb[i]) = data.Ycrb[i] * jdata.S();
150
151

      /* M[i,SUBTREE] = S'*F[1:6,SUBTREE] */
152
      data.M.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i])
153
      = jdata.S().transpose()*data.Fcrb[i].middleCols(jmodel.idx_v(),data.nvSubtree[i]);
154
155


156
      jmodel.jointVelocitySelector(data.nle)  = jdata.S().transpose()*data.f[i];
157
158
159
      if(parent>0)
      {
        /*   Yli += liXi Yi */
160
        data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]);
161
162
163

        /*   F[1:6,SUBTREE] = liXi F[1:6,SUBTREE] */
        Eigen::Block<typename Data::Matrix6x> jF
164
        = data.Fcrb[parent].block(0,jmodel.idx_v(),6,data.nvSubtree[i]);
jcarpent's avatar
jcarpent committed
165
166
167
        Eigen::Block<typename Data::Matrix6x> iF
        = data.Fcrb[i].block(0,jmodel.idx_v(),6,data.nvSubtree[i]);
        forceSet::se3Action(data.liMi[i], iF, jF);
168

169
        data.f[parent] += data.liMi[i].act(data.f[i]);
170
      }
jcarpent's avatar
jcarpent committed
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
      
      // CoM
      const SE3 & liMi = data.liMi[i];
      
      data.com[parent] += (liMi.rotation()*data.com[i]
                           + data.mass[i] * liMi.translation());
      
      SE3::Vector3 com_in_world (oMi.rotation() * data.com[i] + data.mass[i] * oMi.translation());
      
      data.vcom[parent] += liMi.rotation()*data.vcom[i];
      data.mass[parent] += data.mass[i];
      
      typedef Data::Matrix6x Matrix6x;
      typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6x>::Type ColBlock;
      
      ColBlock Jcols = jmodel.jointCols(data.J);
      
      if( JointModel::NV==1 )
        data.Jcom.col(jmodel.idx_v())
        = data.mass[i] * Jcols.template topLeftCorner<3,1>()
        - com_in_world.cross(Jcols.template bottomLeftCorner<3,1>()) ;
      else
        jmodel.jointCols(data.Jcom)
        = data.mass[i] * Jcols.template topRows<3>()
jcarpent's avatar
jcarpent committed
195
        - skew(com_in_world) * Jcols.template bottomRows<3>();
jcarpent's avatar
jcarpent committed
196
197
198
      
      data.com[i] /= data.mass[i];
      data.vcom[i] /= data.mass[i];
199
200
    }
  };
jcarpent's avatar
jcarpent committed
201
  
202
203
204
205
206
207
  inline void
  computeAllTerms(const Model & model,
                  Data & data,
                  const Eigen::VectorXd & q,
                  const Eigen::VectorXd & v)
  {
208
    assert(model.check(data) && "data is not consistent with model.");
209
210
211
    data.v[0].setZero();
    data.a[0].setZero();
    data.a_gf[0] = -model.gravity;
jcarpent's avatar
jcarpent committed
212
213
214
215
    
    data.mass[0] = 0;
    data.com[0].setZero ();
    data.vcom[0].setZero ();
216

217
    for(Model::JointIndex i=1;i<(Model::JointIndex) model.njoints;++i)
218
219
    {
      CATForwardStep::run(model.joints[i],data.joints[i],
jcarpent's avatar
jcarpent committed
220
                          CATForwardStep::ArgsType(model,data,q,v));
221
222
    }

223
    for(Model::JointIndex i=(Model::JointIndex)(model.njoints-1);i>0;--i)
224
225
    {
      CATBackwardStep::run(model.joints[i],data.joints[i],
jcarpent's avatar
jcarpent committed
226
                           CATBackwardStep::ArgsType(model,data));
227
    }
228
    
jcarpent's avatar
jcarpent committed
229
230
231
232
233
234
235
236
237
238
    // CoM
    data.com[0] /= data.mass[0];
    data.vcom[0] /= data.mass[0];
    
    // JCoM
    data.Jcom /= data.mass[0];
    
    // Energy
    kineticEnergy(model, data, q, v, false);
    potentialEnergy(model, data, q, false);
239
240
241

  }
} // namespace se3
jcarpent's avatar
jcarpent committed
242

243
244
245

#endif // ifndef __se3_compute_all_terms_hpp__