Directory: | ./ |
---|---|
File: | include/pinocchio/algorithm/compute-all-terms.hxx |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 85 | 85 | 100.0% |
Branches: | 154 | 335 | 46.0% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2015-2021 CNRS INRIA | ||
3 | // | ||
4 | |||
5 | #ifndef __pinocchio_compute_all_terms_hxx__ | ||
6 | #define __pinocchio_compute_all_terms_hxx__ | ||
7 | |||
8 | #include "pinocchio/multibody/visitor.hpp" | ||
9 | #include "pinocchio/spatial/act-on-set.hpp" | ||
10 | #include "pinocchio/algorithm/center-of-mass.hpp" | ||
11 | #include "pinocchio/algorithm/energy.hpp" | ||
12 | #include "pinocchio/algorithm/check.hpp" | ||
13 | |||
14 | namespace pinocchio | ||
15 | { | ||
16 | namespace impl | ||
17 | { | ||
18 | template< | ||
19 | typename Scalar, | ||
20 | int Options, | ||
21 | template<typename, int> | ||
22 | class JointCollectionTpl, | ||
23 | typename ConfigVectorType, | ||
24 | typename TangentVectorType> | ||
25 | struct CATForwardStep | ||
26 | : public fusion::JointUnaryVisitorBase< | ||
27 | CATForwardStep<Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType>> | ||
28 | { | ||
29 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
30 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
31 | |||
32 | typedef boost::fusion:: | ||
33 | vector<const Model &, Data &, const ConfigVectorType &, const TangentVectorType &> | ||
34 | ArgsType; | ||
35 | |||
36 | template<typename JointModel> | ||
37 | 12162 | static void algo( | |
38 | const JointModelBase<JointModel> & jmodel, | ||
39 | JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
40 | const Model & model, | ||
41 | Data & data, | ||
42 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
43 | const Eigen::MatrixBase<TangentVectorType> & v) | ||
44 | { | ||
45 | typedef typename Model::JointIndex JointIndex; | ||
46 | typedef | ||
47 | typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type | ||
48 | ColsBlock; | ||
49 | |||
50 |
1/2✓ Branch 1 taken 6081 times.
✗ Branch 2 not taken.
|
12162 | const JointIndex i = jmodel.id(); |
51 | 12162 | const JointIndex parent = model.parents[i]; | |
52 | |||
53 |
1/2✓ Branch 4 taken 6081 times.
✗ Branch 5 not taken.
|
12162 | jmodel.calc(jdata.derived(), q.derived(), v.derived()); |
54 | |||
55 | // CRBA | ||
56 |
6/10✓ Branch 1 taken 6081 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5848 times.
✓ Branch 5 taken 233 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5848 times.
✓ Branch 9 taken 233 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 5848 times.
✗ Branch 13 not taken.
|
12162 | data.liMi[i] = model.jointPlacements[i] * jdata.M(); |
57 | |||
58 | // Jacobian + NLE | ||
59 |
2/4✓ Branch 1 taken 6081 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6081 times.
✗ Branch 6 not taken.
|
12162 | data.v[i] = jdata.v(); |
60 | |||
61 |
2/2✓ Branch 0 taken 5839 times.
✓ Branch 1 taken 242 times.
|
12162 | if (parent > 0) |
62 | { | ||
63 |
2/4✓ Branch 3 taken 5839 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 5839 times.
✗ Branch 8 not taken.
|
11678 | data.oMi[i] = data.oMi[parent] * data.liMi[i]; |
64 |
2/4✓ Branch 3 taken 5839 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 5839 times.
✗ Branch 8 not taken.
|
11678 | data.v[i] += data.liMi[i].actInv(data.v[parent]); |
65 | } | ||
66 | else | ||
67 |
1/2✓ Branch 3 taken 242 times.
✗ Branch 4 not taken.
|
484 | data.oMi[i] = data.liMi[i]; |
68 | |||
69 |
2/4✓ Branch 3 taken 6081 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 6081 times.
✗ Branch 8 not taken.
|
12162 | data.ov[i] = data.oMi[i].act(data.v[i]); |
70 | |||
71 |
2/4✓ Branch 3 taken 6081 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 6081 times.
✗ Branch 8 not taken.
|
12162 | data.oYcrb[i] = data.oMi[i].act(model.inertias[i]); |
72 |
1/2✓ Branch 3 taken 6081 times.
✗ Branch 4 not taken.
|
12162 | data.doYcrb[i] = data.oYcrb[i].variation(data.ov[i]); |
73 | |||
74 |
1/2✓ Branch 1 taken 6081 times.
✗ Branch 2 not taken.
|
12162 | ColsBlock J_cols = jmodel.jointCols(data.J); |
75 |
3/6✓ Branch 2 taken 6081 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6081 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6081 times.
✗ Branch 9 not taken.
|
12162 | J_cols = data.oMi[i].act(jdata.S()); |
76 | |||
77 |
1/2✓ Branch 1 taken 6081 times.
✗ Branch 2 not taken.
|
12162 | ColsBlock dJ_cols = jmodel.jointCols(data.dJ); |
78 |
1/2✓ Branch 2 taken 6081 times.
✗ Branch 3 not taken.
|
12162 | motionSet::motionAction(data.ov[i], J_cols, dJ_cols); |
79 | |||
80 |
8/16✓ Branch 1 taken 6081 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6081 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6081 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 5 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 6076 times.
✗ Branch 14 not taken.
✓ Branch 15 taken 5 times.
✗ Branch 16 not taken.
✓ Branch 17 taken 6076 times.
✗ Branch 18 not taken.
✓ Branch 19 taken 5 times.
✗ Branch 20 not taken.
|
12162 | data.a_gf[i] = data.a[i] = jdata.c() + (data.v[i] ^ jdata.v()); |
81 |
2/2✓ Branch 0 taken 5839 times.
✓ Branch 1 taken 242 times.
|
12162 | if (parent > 0) |
82 |
2/4✓ Branch 3 taken 5839 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 5839 times.
✗ Branch 8 not taken.
|
11678 | data.a[i] += data.liMi[i].actInv(data.a[parent]); |
83 | |||
84 |
2/4✓ Branch 3 taken 6081 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 6081 times.
✗ Branch 8 not taken.
|
12162 | data.a_gf[i] += data.liMi[i].actInv(data.a_gf[parent]); |
85 | |||
86 |
2/4✓ Branch 3 taken 6081 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 6081 times.
✗ Branch 8 not taken.
|
12162 | data.h[i] = model.inertias[i] * data.v[i]; |
87 |
4/8✓ Branch 3 taken 6081 times.
✗ Branch 4 not taken.
✓ Branch 8 taken 6081 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 6081 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 6081 times.
✗ Branch 16 not taken.
|
12162 | data.f[i] = model.inertias[i] * data.a_gf[i] + data.v[i].cross(data.h[i]); // -f_ext |
88 | } | ||
89 | }; | ||
90 | |||
91 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
92 | struct CATBackwardStep | ||
93 | : public fusion::JointUnaryVisitorBase<CATBackwardStep<Scalar, Options, JointCollectionTpl>> | ||
94 | { | ||
95 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
96 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
97 | |||
98 | typedef boost::fusion::vector<const Model &, Data &> ArgsType; | ||
99 | |||
100 | template<typename JointModel> | ||
101 | 12148 | static void algo( | |
102 | const JointModelBase<JointModel> & jmodel, | ||
103 | JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
104 | const Model & model, | ||
105 | Data & data) | ||
106 | { | ||
107 | typedef typename Model::JointIndex JointIndex; | ||
108 | typedef | ||
109 | typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type | ||
110 | ColsBlock; | ||
111 | |||
112 |
1/2✓ Branch 1 taken 6074 times.
✗ Branch 2 not taken.
|
12148 | const JointIndex i = jmodel.id(); |
113 | 12148 | const JointIndex parent = model.parents[i]; | |
114 | |||
115 |
1/2✓ Branch 1 taken 6074 times.
✗ Branch 2 not taken.
|
12148 | ColsBlock J_cols = jmodel.jointCols(data.J); |
116 |
1/2✓ Branch 1 taken 6074 times.
✗ Branch 2 not taken.
|
12148 | ColsBlock dJ_cols = jmodel.jointCols(data.dJ); |
117 |
1/2✓ Branch 1 taken 6074 times.
✗ Branch 2 not taken.
|
12148 | ColsBlock Ag_cols = jmodel.jointCols(data.Ag); |
118 |
1/2✓ Branch 1 taken 6074 times.
✗ Branch 2 not taken.
|
12148 | ColsBlock dAg_cols = jmodel.jointCols(data.dAg); |
119 | |||
120 | // Calc Ag = Y * S | ||
121 |
1/2✓ Branch 2 taken 6074 times.
✗ Branch 3 not taken.
|
12148 | motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols); |
122 | |||
123 | // Calc dAg = Ivx + vxI | ||
124 |
3/6✓ Branch 2 taken 6074 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6074 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6074 times.
✗ Branch 9 not taken.
|
12148 | dAg_cols.noalias() = data.doYcrb[i] * J_cols; |
125 |
1/2✓ Branch 2 taken 6074 times.
✗ Branch 3 not taken.
|
12148 | motionSet::inertiaAction<ADDTO>(data.oYcrb[i], dJ_cols, dAg_cols); |
126 | |||
127 | /* M[i,SUBTREE] = S'*F[1:6,SUBTREE] */ | ||
128 |
6/12✓ Branch 2 taken 6074 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6074 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6074 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 6074 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 6074 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 6074 times.
✗ Branch 18 not taken.
|
12148 | data.M.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]).noalias() = |
129 |
4/8✓ Branch 2 taken 6074 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6074 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6074 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 6074 times.
✗ Branch 12 not taken.
|
12148 | J_cols.transpose() * data.Ag.middleCols(jmodel.idx_v(), data.nvSubtree[i]); |
130 | |||
131 |
8/13✓ Branch 2 taken 6074 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5849 times.
✓ Branch 6 taken 225 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 5849 times.
✓ Branch 9 taken 225 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 5849 times.
✓ Branch 12 taken 225 times.
✗ Branch 13 not taken.
✓ Branch 14 taken 5849 times.
✗ Branch 15 not taken.
|
12148 | jmodel.jointVelocitySelector(data.nle) = jdata.S().transpose() * data.f[i]; |
132 | |||
133 |
1/2✓ Branch 3 taken 6074 times.
✗ Branch 4 not taken.
|
12148 | data.oYcrb[parent] += data.oYcrb[i]; |
134 |
1/2✓ Branch 3 taken 6074 times.
✗ Branch 4 not taken.
|
12148 | data.doYcrb[parent] += data.doYcrb[i]; |
135 |
2/4✓ Branch 3 taken 6074 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 6074 times.
✗ Branch 8 not taken.
|
12148 | data.h[parent] += data.liMi[i].act(data.h[i]); |
136 |
2/4✓ Branch 3 taken 6074 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 6074 times.
✗ Branch 8 not taken.
|
12148 | data.f[parent] += data.liMi[i].act(data.f[i]); |
137 | |||
138 | // CoM | ||
139 | 12148 | data.mass[i] = data.oYcrb[i].mass(); | |
140 |
1/2✓ Branch 4 taken 6074 times.
✗ Branch 5 not taken.
|
12148 | data.com[i] = data.oMi[i].actInv(data.oYcrb[i].lever()); |
141 |
3/6✓ Branch 3 taken 6074 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6074 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 6074 times.
✗ Branch 11 not taken.
|
12148 | data.vcom[i] = data.h[i].linear() / data.mass[i]; |
142 | } | ||
143 | }; | ||
144 | |||
145 | template< | ||
146 | typename Scalar, | ||
147 | int Options, | ||
148 | template<typename, int> | ||
149 | class JointCollectionTpl, | ||
150 | typename ConfigVectorType, | ||
151 | typename TangentVectorType> | ||
152 | 242 | void computeAllTerms( | |
153 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
154 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
155 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
156 | const Eigen::MatrixBase<TangentVectorType> & v) | ||
157 | { | ||
158 |
2/4✓ Branch 1 taken 242 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 242 times.
✗ Branch 4 not taken.
|
242 | assert(model.check(data) && "data is not consistent with model."); |
159 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 242 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
|
242 | PINOCCHIO_CHECK_ARGUMENT_SIZE( |
160 | q.size(), model.nq, "The configuration vector is not of right size"); | ||
161 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 242 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
|
242 | PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The velocity vector is not of right size"); |
162 | |||
163 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
164 | |||
165 |
1/2✓ Branch 2 taken 242 times.
✗ Branch 3 not taken.
|
242 | data.v[0].setZero(); |
166 |
1/2✓ Branch 2 taken 242 times.
✗ Branch 3 not taken.
|
242 | data.a[0].setZero(); |
167 |
1/2✓ Branch 2 taken 242 times.
✗ Branch 3 not taken.
|
242 | data.h[0].setZero(); |
168 |
2/4✓ Branch 1 taken 242 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 242 times.
✗ Branch 6 not taken.
|
242 | data.a_gf[0] = -model.gravity; |
169 |
1/2✓ Branch 2 taken 242 times.
✗ Branch 3 not taken.
|
242 | data.oYcrb[0].setZero(); |
170 | |||
171 | typedef CATForwardStep< | ||
172 | Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType> | ||
173 | Pass1; | ||
174 |
2/2✓ Branch 0 taken 6081 times.
✓ Branch 1 taken 242 times.
|
6323 | for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) |
175 | { | ||
176 |
1/2✓ Branch 1 taken 6081 times.
✗ Branch 2 not taken.
|
6081 | Pass1::run( |
177 | 6081 | model.joints[i], data.joints[i], | |
178 |
1/2✓ Branch 3 taken 6081 times.
✗ Branch 4 not taken.
|
12162 | typename Pass1::ArgsType(model, data, q.derived(), v.derived())); |
179 | } | ||
180 | |||
181 | typedef CATBackwardStep<Scalar, Options, JointCollectionTpl> Pass2; | ||
182 |
2/2✓ Branch 0 taken 6081 times.
✓ Branch 1 taken 242 times.
|
6323 | for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) |
183 | { | ||
184 |
2/4✓ Branch 1 taken 6081 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 6081 times.
✗ Branch 7 not taken.
|
6081 | Pass2::run(model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data)); |
185 | } | ||
186 | |||
187 | // CoM | ||
188 | 242 | data.mass[0] = data.oYcrb[0].mass(); | |
189 |
1/2✓ Branch 4 taken 242 times.
✗ Branch 5 not taken.
|
242 | data.com[0] = data.oYcrb[0].lever(); |
190 |
3/6✓ Branch 3 taken 242 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 242 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 242 times.
✗ Branch 11 not taken.
|
242 | data.vcom[0] = data.h[0].linear() / data.mass[0]; |
191 | |||
192 | // Centroidal | ||
193 | typedef Eigen::Block<typename Data::Matrix6x, 3, -1> Block3x; | ||
194 |
1/2✓ Branch 1 taken 242 times.
✗ Branch 2 not taken.
|
242 | const Block3x Ag_lin = data.Ag.template middleRows<3>(Force::LINEAR); |
195 |
1/2✓ Branch 1 taken 242 times.
✗ Branch 2 not taken.
|
242 | Block3x Ag_ang = data.Ag.template middleRows<3>(Force::ANGULAR); |
196 |
2/2✓ Branch 0 taken 7216 times.
✓ Branch 1 taken 242 times.
|
7458 | for (long i = 0; i < model.nv; ++i) |
197 |
4/8✓ Branch 1 taken 7216 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7216 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7216 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7216 times.
✗ Branch 12 not taken.
|
7216 | Ag_ang.col(i) += Ag_lin.col(i).cross(data.com[0]); |
198 | |||
199 |
1/2✓ Branch 1 taken 242 times.
✗ Branch 2 not taken.
|
242 | const Block3x dAg_lin = data.dAg.template middleRows<3>(Force::LINEAR); |
200 |
1/2✓ Branch 1 taken 242 times.
✗ Branch 2 not taken.
|
242 | Block3x dAg_ang = data.dAg.template middleRows<3>(Force::ANGULAR); |
201 |
2/2✓ Branch 0 taken 7216 times.
✓ Branch 1 taken 242 times.
|
7458 | for (Eigen::DenseIndex i = 0; i < model.nv; ++i) |
202 |
7/14✓ Branch 1 taken 7216 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7216 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7216 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 7216 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 7216 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 7216 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 7216 times.
✗ Branch 22 not taken.
|
7216 | dAg_ang.col(i) += dAg_lin.col(i).cross(data.com[0]) + Ag_lin.col(i).cross(data.vcom[0]); |
203 | |||
204 |
1/2✓ Branch 2 taken 242 times.
✗ Branch 3 not taken.
|
242 | data.hg = data.h[0]; |
205 |
4/8✓ Branch 1 taken 242 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 242 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 242 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 242 times.
✗ Branch 12 not taken.
|
242 | data.hg.angular() += data.hg.linear().cross(data.com[0]); |
206 | |||
207 |
1/2✓ Branch 2 taken 242 times.
✗ Branch 3 not taken.
|
242 | data.dhg = data.f[0]; |
208 |
4/8✓ Branch 1 taken 242 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 242 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 242 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 242 times.
✗ Branch 12 not taken.
|
242 | data.dhg.angular() += data.dhg.linear().cross(data.com[0]); |
209 | |||
210 | // Add the armature contribution | ||
211 |
2/4✓ Branch 1 taken 242 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 242 times.
✗ Branch 5 not taken.
|
242 | data.M.diagonal() += model.armature; |
212 | |||
213 | // JCoM | ||
214 |
3/6✓ Branch 2 taken 242 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 242 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 242 times.
✗ Branch 9 not taken.
|
242 | data.Jcom = data.Ag.template middleRows<3>(Force::LINEAR) / data.mass[0]; |
215 | |||
216 | 242 | data.Ig.mass() = data.oYcrb[0].mass(); | |
217 |
1/2✓ Branch 2 taken 242 times.
✗ Branch 3 not taken.
|
242 | data.Ig.lever().setZero(); |
218 |
1/2✓ Branch 4 taken 242 times.
✗ Branch 5 not taken.
|
242 | data.Ig.inertia() = data.oYcrb[0].inertia(); |
219 | |||
220 | // Gravity | ||
221 |
4/8✓ Branch 1 taken 242 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 242 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 242 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 242 times.
✗ Branch 11 not taken.
|
242 | data.g.noalias() = |
222 |
3/6✓ Branch 1 taken 242 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 242 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 242 times.
✗ Branch 8 not taken.
|
242 | -data.Ag.template middleRows<3>(Force::LINEAR).transpose() * model.gravity.linear(); |
223 | |||
224 | // Energy | ||
225 |
1/2✓ Branch 1 taken 242 times.
✗ Branch 2 not taken.
|
242 | ::pinocchio::computeMechanicalEnergy(model, data); |
226 | 242 | } | |
227 | } // namespace impl | ||
228 | |||
229 | template< | ||
230 | typename Scalar, | ||
231 | int Options, | ||
232 | template<typename, int> | ||
233 | class JointCollectionTpl, | ||
234 | typename ConfigVectorType, | ||
235 | typename TangentVectorType> | ||
236 | 20 | void computeAllTerms( | |
237 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
238 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
239 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
240 | const Eigen::MatrixBase<TangentVectorType> & v) | ||
241 | { | ||
242 |
2/4✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 20 times.
✗ Branch 6 not taken.
|
20 | pinocchio::impl::computeAllTerms(model, data, make_const_ref(q), make_const_ref(v)); |
243 | 20 | } | |
244 | |||
245 | } // namespace pinocchio | ||
246 | |||
247 | /// \endinternal | ||
248 | |||
249 | #endif // ifndef __pinocchio_compute_all_terms_hxx__ | ||
250 |