GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/pinocchio/algorithm/crba.hxx Lines: 70 70 100.0 %
Date: 2024-01-23 21:41:47 Branches: 95 207 45.9 %

Line Branch Exec Source
1
//
2
// Copyright (c) 2015-2019 CNRS
3
//
4
5
#ifndef __pinocchio_crba_hxx__
6
#define __pinocchio_crba_hxx__
7
8
#include "pinocchio/multibody/visitor.hpp"
9
#include "pinocchio/spatial/act-on-set.hpp"
10
#include "pinocchio/algorithm/kinematics.hpp"
11
#include "pinocchio/algorithm/check.hpp"
12
13
/// @cond DEV
14
15
namespace pinocchio
16
{
17
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
18
  struct CrbaForwardStep
19
  : public fusion::JointUnaryVisitorBase< CrbaForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType> >
20
  {
21
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
22
    typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
23
24
    typedef boost::fusion::vector<const Model &,
25
                                  Data &,
26
                                  const ConfigVectorType &
27
                                  > ArgsType;
28
29
    template<typename JointModel>
30
1892
    static void algo(const JointModelBase<JointModel> & jmodel,
31
                     JointDataBase<typename JointModel::JointDataDerived> & jdata,
32
                     const Model & model,
33
                     Data & data,
34
                     const Eigen::MatrixBase<ConfigVectorType> & q)
35
    {
36
      typedef typename Model::JointIndex JointIndex;
37
38
1892
      const JointIndex & i = jmodel.id();
39
1892
      jmodel.calc(jdata.derived(),q.derived());
40
41
1892
      data.liMi[i] = model.jointPlacements[i]*jdata.M();
42
1892
      data.Ycrb[i] = model.inertias[i];
43
    }
44
45
  };
46
47
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
48
  struct CrbaBackwardStep
49
  : public fusion::JointUnaryVisitorBase< CrbaBackwardStep<Scalar,Options,JointCollectionTpl> >
50
  {
51
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
52
    typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
53
54
    typedef boost::fusion::vector<const Model &,
55
				                          Data &>  ArgsType;
56
57
    template<typename JointModel>
58
1892
    static void algo(const JointModelBase<JointModel> & jmodel,
59
                     JointDataBase<typename JointModel::JointDataDerived> & jdata,
60
                     const Model & model,
61
                     Data & data)
62
    {
63
      /*
64
       * F[1:6,i] = Y*S
65
       * M[i,SUBTREE] = S'*F[1:6,SUBTREE]
66
       * if li>0
67
       *   Yli += liXi Yi
68
       *   F[1:6,SUBTREE] = liXi F[1:6,SUBTREE]
69
       */
70
71
      typedef typename Model::JointIndex JointIndex;
72
      typedef typename Data::Matrix6x::ColsBlockXpr Block;
73
1892
      const JointIndex & i = jmodel.id();
74
75
      /* F[1:6,i] = Y*S */
76
      //data.Fcrb[i].block<6,JointModel::NV>(0,jmodel.idx_v()) = data.Ycrb[i] * jdata.S();
77

1892
      jmodel.jointCols(data.Fcrb[i]) = data.Ycrb[i] * jdata.S();
78
79
      /* M[i,SUBTREE] = S'*F[1:6,SUBTREE] */
80


1892
      data.M.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]).noalias()
81



3784
      = jdata.S().transpose()*data.Fcrb[i].middleCols(jmodel.idx_v(),data.nvSubtree[i]);
82
83
1892
      const JointIndex & parent = model.parents[i];
84
1892
      if(parent>0)
85
      {
86
        /*   Yli += liXi Yi */
87

1784
        data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]);
88
89
        /*   F[1:6,SUBTREE] = liXi F[1:6,SUBTREE] */
90

1784
        Block jF
91
1784
        = data.Fcrb[parent].middleCols(jmodel.idx_v(),data.nvSubtree[i]);
92

1784
        Block iF
93
1784
        = data.Fcrb[i].middleCols(jmodel.idx_v(),data.nvSubtree[i]);
94
1784
        forceSet::se3Action(data.liMi[i], iF, jF);
95
      }
96
    }
97
  };
98
99
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
100
  struct CrbaForwardStepMinimal
101
  : public fusion::JointUnaryVisitorBase< CrbaForwardStepMinimal<Scalar,Options,JointCollectionTpl,ConfigVectorType> >
102
  {
103
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
104
    typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
105
106
    typedef boost::fusion::vector<const Model &,
107
                                  Data &,
108
                                  const ConfigVectorType &
109
                                  > ArgsType;
110
111
    template<typename JointModel>
112
216
    static void algo(const JointModelBase<JointModel> & jmodel,
113
                     JointDataBase<typename JointModel::JointDataDerived> & jdata,
114
                     const Model & model,
115
                     Data & data,
116
                     const Eigen::MatrixBase<ConfigVectorType> & q)
117
    {
118
      typedef typename Model::JointIndex JointIndex;
119
120
216
      const JointIndex & i = jmodel.id();
121
216
      jmodel.calc(jdata.derived(),q.derived());
122
123
216
      data.liMi[i] = model.jointPlacements[i]*jdata.M();
124
125
216
      const JointIndex & parent = model.parents[i];
126
216
      if (parent>0) data.oMi[i] = data.oMi[parent]*data.liMi[i];
127
8
      else data.oMi[i] = data.liMi[i];
128
129

216
      jmodel.jointCols(data.J) = data.oMi[i].act(jdata.S());
130
131
216
      data.Ycrb[i] = model.inertias[i];
132
    }
133
134
  };
135
136
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
137
  struct CrbaBackwardStepMinimal
138
  : public fusion::JointUnaryVisitorBase< CrbaBackwardStepMinimal<Scalar,Options,JointCollectionTpl> >
139
  {
140
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
141
    typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
142
143
    typedef boost::fusion::vector<const Model &,
144
                                  Data &>  ArgsType;
145
146
    template<typename JointModel>
147
216
    static void algo(const JointModelBase<JointModel> & jmodel,
148
                     JointDataBase<typename JointModel::JointDataDerived> & jdata,
149
                     const Model & model,
150
                     Data & data)
151
    {
152
      typedef typename Model::JointIndex JointIndex;
153
      typedef typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type ColsBlock;
154
216
      const JointIndex & i = jmodel.id();
155
156
      /* F[1:6,i] = Y*S */
157

216
      jdata.U() = data.Ycrb[i] * jdata.S();
158

216
      ColsBlock jF = data.Ag.template middleCols<JointModel::NV>(jmodel.idx_v());
159
      //        = data.Ag.middleCols(jmodel.idx_v(), jmodel.nv());
160
161

216
      forceSet::se3Action(data.oMi[i],jdata.U(),jF);
162
163
      /* M[i,SUBTREE] = S'*F[1:6,SUBTREE] */
164


216
      data.M.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]).noalias()
165



432
      = jmodel.jointCols(data.J).transpose()*data.Ag.middleCols(jmodel.idx_v(),data.nvSubtree[i]);
166
167
216
      const JointIndex & parent = model.parents[i];
168
      /*   Yli += liXi Yi */
169

216
      data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]);
170
    }
171
  };
172
173
174
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
175
  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::MatrixXs &
176
54
  crba(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
177
       DataTpl<Scalar,Options,JointCollectionTpl> & data,
178
       const Eigen::MatrixBase<ConfigVectorType> & q)
179
  {
180
54
    assert(model.check(data) && "data is not consistent with model.");
181






54
    PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size");
182
183
    typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
184
185
    typedef CrbaForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType> Pass1;
186
1000
    for(JointIndex i=1; i<(JointIndex)(model.njoints); ++i)
187
    {
188
946
      Pass1::run(model.joints[i],data.joints[i],
189
                 typename Pass1::ArgsType(model,data,q.derived()));
190
    }
191
192
    typedef CrbaBackwardStep<Scalar,Options,JointCollectionTpl> Pass2;
193
1000
    for(JointIndex i=(JointIndex)(model.njoints-1); i>0; --i)
194
    {
195
946
      Pass2::run(model.joints[i],data.joints[i],
196
                 typename Pass2::ArgsType(model,data));
197
    }
198
199
54
    return data.M;
200
  }
201
202
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
203
  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::MatrixXs &
204
4
  crbaMinimal(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
205
              DataTpl<Scalar,Options,JointCollectionTpl> & data,
206
              const Eigen::MatrixBase<ConfigVectorType> & q)
207
  {
208

4
    assert(model.check(data) && "data is not consistent with model.");
209







4
    PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size");
210
211
    typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
212
213
    typedef CrbaForwardStepMinimal<Scalar,Options,JointCollectionTpl,ConfigVectorType> Pass1;
214
4
    data.Ycrb[0].setZero();
215
112
    for(JointIndex i=1; i<(JointIndex)(model.njoints); ++i)
216
    {
217

108
      Pass1::run(model.joints[i],data.joints[i],
218
                 typename Pass1::ArgsType(model,data,q.derived()));
219
    }
220
221
    typedef CrbaBackwardStepMinimal<Scalar,Options,JointCollectionTpl> Pass2;
222
112
    for(JointIndex i=(JointIndex)(model.njoints-1); i>0; --i)
223
    {
224

108
      Pass2::run(model.joints[i],data.joints[i],
225
                 typename Pass2::ArgsType(model,data));
226
    }
227
228
    // Retrieve the Centroidal Momemtum map
229
    typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
230
    typedef typename Data::Force Force;
231
    typedef Eigen::Block<typename Data::Matrix6x,3,-1> Block3x;
232
233
4
    data.mass[0] = data.Ycrb[0].mass();
234
4
    data.com[0] = data.Ycrb[0].lever();
235
236
4
    const Block3x Ag_lin = data.Ag.template middleRows<3>(Force::LINEAR);
237
4
    Block3x Ag_ang = data.Ag.template middleRows<3>(Force::ANGULAR);
238
132
    for(long i = 0; i<model.nv; ++i)
239


128
      Ag_ang.col(i) += Ag_lin.col(i).cross(data.com[0]);
240
241
4
    return data.M;
242
  }
243
244
  // --- CHECKER ---------------------------------------------------------------
245
  // --- CHECKER ---------------------------------------------------------------
246
  // --- CHECKER ---------------------------------------------------------------
247
248
  namespace internal
249
  {
250
    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
251
6495
    inline bool isDescendant(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
252
                             const typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex j,
253
                             const typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex root)
254
    {
255
      typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
256
      typedef typename Model::JointIndex JointIndex;
257
258
6495
      if(j>=(JointIndex)model.njoints)  return false;
259
6471
      if(j==0)                          return root==0;
260

5697
      return (j==root) || isDescendant(model,model.parents[j],root);
261
    }
262
  }
263
264
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
265
3
  inline bool CRBAChecker::checkModel_impl(const ModelTpl<Scalar,Options,JointCollectionTpl> & model) const
266
  {
267
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
268
    typedef typename Model::JointIndex JointIndex;
269
270
    // For CRBA, the tree must be "compact", i.e. all descendants of a node i are stored
271
    // immediately after i in the "parents" map, i.e. forall joint i, the interval i+1..n-1
272
    // can be separated in two intervals [i+1..k] and [k+1..n-1], where any [i+1..k] is a descendant
273
    // of i and none of [k+1..n-1] is a descendant of i.
274
81
    for(JointIndex i=1; i < (JointIndex)(model.njoints-1); ++i) // no need to check joints 0 and N-1
275
      {
276
78
        JointIndex k=i+1;
277
411
        while(internal::isDescendant(model,k,i)) ++k;
278
798
        for( ; int(k)<model.njoints; ++k )
279
720
          if( internal::isDescendant(model,k,i) ) return false;
280
      }
281
3
    return true;
282
  }
283
284
285
} // namespace pinocchio
286
287
/// @endcond
288
289
#endif // ifndef __pinocchio_crba_hxx__