GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/energy.hxx
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 52 52 100.0%
Branches: 53 140 37.9%
Line Branch Exec Source
1 //
2 // Copyright (c) 2016-2020 CNRS INRIA
3 //
4
5 #ifndef __pinocchio_algorithm_energy_hxx__
6 #define __pinocchio_algorithm_energy_hxx__
7
8 #include "pinocchio/algorithm/check.hpp"
9
10 namespace pinocchio
11 {
12
13 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
14 struct KineticEnergyAlgoForwardStep
15 : public fusion::JointUnaryVisitorBase<
16 KineticEnergyAlgoForwardStep<Scalar, Options, JointCollectionTpl>>
17 {
18 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
19 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
20
21 typedef boost::fusion::vector<const Model &, Data &> ArgsType;
22
23 template<typename JointModel>
24 15044 static void algo(
25 const JointModelBase<JointModel> & jmodel,
26 JointDataBase<typename JointModel::JointDataDerived> & jdata,
27 const Model & model,
28 Data & data)
29 {
30
1/2
✓ Branch 1 taken 7522 times.
✗ Branch 2 not taken.
 15044 const JointIndex & i = jmodel.id();
31
2/4
✓ Branch 3 taken 7522 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 15044 data.kinetic_energy += model.inertias[i].vtiv(data.v[i]);
32
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 15046 data.kinetic_energy +=
33
4/8
✓ Branch 1 taken 7522 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7522 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7522 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7522 times.
✗ Branch 11 not taken.
 30088 (jmodel.jointVelocitySelector(model.armature).array() * jdata.joint_v().array().square())
34
3/6
✓ Branch 1 taken 7521 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7521 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7521 times.
✗ Branch 8 not taken.
 30084 .sum();
35 }
36 };
37
38 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
39 9 Scalar computeKineticEnergy(
40 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
41 DataTpl<Scalar, Options, JointCollectionTpl> & data)
42 {
43
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 assert(model.check(data) && "data is not consistent with model.");
44
45 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
46 typedef typename Model::JointIndex JointIndex;
47
48
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 9 data.kinetic_energy = Scalar(0);
49 typedef KineticEnergyAlgoForwardStep<Scalar, Options, JointCollectionTpl> Pass;
50
2/2
✓ Branch 0 taken 217 times.
✓ Branch 1 taken 9 times.
 226 for (JointIndex i = 1; i < (JointIndex)(model.njoints); ++i)
51 {
52
1/2
✓ Branch 4 taken 217 times.
✗ Branch 5 not taken.
 217 Pass::run(model.joints[i], data.joints[i], typename Pass::ArgsType(model, data));
53 }
54
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 9 data.kinetic_energy *= .5;
55
56 9 return data.kinetic_energy;
57 }
58
59 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
60 7 Scalar computePotentialEnergy(
61 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
62 DataTpl<Scalar, Options, JointCollectionTpl> & data)
63 {
64
2/4
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
 7 assert(model.check(data) && "data is not consistent with model.");
65 ;
66
67 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
68 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
69 typedef typename Model::JointIndex JointIndex;
70 typedef typename Model::Motion Motion;
71
72
0/4
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 7 data.potential_energy = Scalar(0);
73
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 const typename Motion::ConstLinearType & g = model.gravity.linear();
74
75
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 typename Data::Vector3 com_global; // tmp variable
76
2/2
✓ Branch 0 taken 189 times.
✓ Branch 1 taken 7 times.
 196 for (JointIndex i = 1; i < (JointIndex)(model.njoints); ++i)
77 {
78
2/4
✓ Branch 1 taken 189 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 189 times.
✗ Branch 5 not taken.
 189 com_global.noalias() =
79
4/8
✓ Branch 4 taken 189 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 189 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 189 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 189 times.
✗ Branch 15 not taken.
 189 data.oMi[i].translation() + data.oMi[i].rotation() * model.inertias[i].lever();
80
1/10
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 189 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
 189 data.potential_energy -= model.inertias[i].mass() * com_global.dot(g);
81 }
82
83
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 7 return data.potential_energy;
84 }
85
86 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
87 294 Scalar computeMechanicalEnergy(
88 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
89 DataTpl<Scalar, Options, JointCollectionTpl> & data)
90 {
91
2/4
✓ Branch 1 taken 294 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 294 times.
✗ Branch 4 not taken.
 294 assert(model.check(data) && "data is not consistent with model.");
92
93 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
94 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
95 typedef typename Model::Motion Motion;
96 typedef typename Model::JointIndex JointIndex;
97
98
0/4
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 294 data.kinetic_energy = Scalar(0);
99
0/4
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 294 data.potential_energy = Scalar(0);
100
101
1/2
✓ Branch 1 taken 294 times.
✗ Branch 2 not taken.
 294 typename Data::Vector3 com_global; // tmp variable
102
1/2
✓ Branch 1 taken 294 times.
✗ Branch 2 not taken.
 294 const typename Motion::ConstLinearType & g = model.gravity.linear();
103 typedef KineticEnergyAlgoForwardStep<Scalar, Options, JointCollectionTpl> Pass;
104
2/2
✓ Branch 0 taken 7305 times.
✓ Branch 1 taken 294 times.
 7599 for (JointIndex i = 1; i < (JointIndex)(model.njoints); ++i)
105 {
106
2/4
✓ Branch 1 taken 7305 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 7305 times.
✗ Branch 7 not taken.
 7305 Pass::run(model.joints[i], data.joints[i], typename Pass::ArgsType(model, data));
107
2/4
✓ Branch 1 taken 7305 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7305 times.
✗ Branch 5 not taken.
 7305 com_global.noalias() =
108
4/8
✓ Branch 4 taken 7305 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7305 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 7305 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7305 times.
✗ Branch 15 not taken.
 7305 data.oMi[i].translation() + data.oMi[i].rotation() * model.inertias[i].lever();
109
1/10
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 7305 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
 7305 data.potential_energy -= model.inertias[i].mass() * com_global.dot(g);
110 }
111
0/4
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 294 data.kinetic_energy *= .5;
112
113
0/4
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 294 data.mechanical_energy = data.kinetic_energy + data.potential_energy;
114
115
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 294 return data.mechanical_energy;
116 }
117
118 namespace impl
119 {
120 template<
121 typename Scalar,
122 int Options,
123 template<typename, int> class JointCollectionTpl,
124 typename ConfigVectorType,
125 typename TangentVectorType>
126 8 Scalar computeKineticEnergy(
127 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
128 DataTpl<Scalar, Options, JointCollectionTpl> & data,
129 const Eigen::MatrixBase<ConfigVectorType> & q,
130 const Eigen::MatrixBase<TangentVectorType> & v)
131 {
132 8 pinocchio::impl::forwardKinematics(model, data, q.derived(), v.derived());
133 8 return pinocchio::computeKineticEnergy(model, data);
134 }
135
136 template<
137 typename Scalar,
138 int Options,
139 template<typename, int> class JointCollectionTpl,
140 typename ConfigVectorType>
141 6 Scalar computePotentialEnergy(
142 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
143 DataTpl<Scalar, Options, JointCollectionTpl> & data,
144 const Eigen::MatrixBase<ConfigVectorType> & q)
145 {
146 6 pinocchio::impl::forwardKinematics(model, data, q);
147 6 return pinocchio::computePotentialEnergy(model, data);
148 }
149
150 template<
151 typename Scalar,
152 int Options,
153 template<typename, int> class JointCollectionTpl,
154 typename ConfigVectorType,
155 typename TangentVectorType>
156 34 Scalar computeMechanicalEnergy(
157 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
158 DataTpl<Scalar, Options, JointCollectionTpl> & data,
159 const Eigen::MatrixBase<ConfigVectorType> & q,
160 const Eigen::MatrixBase<TangentVectorType> & v)
161 {
162 34 pinocchio::impl::forwardKinematics(model, data, q, v);
163 34 return pinocchio::computeMechanicalEnergy(model, data);
164 }
165 } // namespace impl
166
167 template<
168 typename Scalar,
169 int Options,
170 template<typename, int> class JointCollectionTpl,
171 typename ConfigVectorType,
172 typename TangentVectorType>
173 8 Scalar computeKineticEnergy(
174 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
175 DataTpl<Scalar, Options, JointCollectionTpl> & data,
176 const Eigen::MatrixBase<ConfigVectorType> & q,
177 const Eigen::MatrixBase<TangentVectorType> & v)
178 {
179
2/4
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
 8 return impl::computeKineticEnergy(model, data, make_const_ref(q), make_const_ref(v));
180 }
181
182 template<
183 typename Scalar,
184 int Options,
185 template<typename, int> class JointCollectionTpl,
186 typename ConfigVectorType>
187 6 Scalar computePotentialEnergy(
188 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
189 DataTpl<Scalar, Options, JointCollectionTpl> & data,
190 const Eigen::MatrixBase<ConfigVectorType> & q)
191 {
192
1/2
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
 6 return impl::computePotentialEnergy(model, data, make_const_ref(q));
193 }
194
195 template<
196 typename Scalar,
197 int Options,
198 template<typename, int> class JointCollectionTpl,
199 typename ConfigVectorType,
200 typename TangentVectorType>
201 34 Scalar computeMechanicalEnergy(
202 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
203 DataTpl<Scalar, Options, JointCollectionTpl> & data,
204 const Eigen::MatrixBase<ConfigVectorType> & q,
205 const Eigen::MatrixBase<TangentVectorType> & v)
206 {
207
2/4
✓ Branch 2 taken 34 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 34 times.
✗ Branch 6 not taken.
 34 return impl::computeMechanicalEnergy(model, data, make_const_ref(q), make_const_ref(v));
208 }
209
210 } // namespace pinocchio
211 #endif // __pinocchio_algorithm_energy_hxx__
212