GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/energy.hxx
Date: 2025-04-30 16:14:33
Exec Total Coverage
Lines: 55 55 100.0%
Branches: 59 152 38.8%
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
2/4
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
 9 assert(model.check(MimicChecker()) && "Function does not support mimic joints");
45
46 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
47 typedef typename Model::JointIndex JointIndex;
48
49
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 9 data.kinetic_energy = Scalar(0);
50 typedef KineticEnergyAlgoForwardStep<Scalar, Options, JointCollectionTpl> Pass;
51
2/2
✓ Branch 0 taken 217 times.
✓ Branch 1 taken 9 times.
 226 for (JointIndex i = 1; i < (JointIndex)(model.njoints); ++i)
52 {
53
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));
54 }
55
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 9 data.kinetic_energy *= .5;
56
57 9 return data.kinetic_energy;
58 }
59
60 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
61 7 Scalar computePotentialEnergy(
62 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
63 DataTpl<Scalar, Options, JointCollectionTpl> & data)
64 {
65
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.");
66
2/4
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
 7 assert(model.check(MimicChecker()) && "Function does not support mimic joints");
67
68 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
69 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
70 typedef typename Model::JointIndex JointIndex;
71 typedef typename Model::Motion Motion;
72
73
0/4
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 7 data.potential_energy = Scalar(0);
74
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 const typename Motion::ConstLinearType & g = model.gravity.linear();
75
76
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 typename Data::Vector3 com_global; // tmp variable
77
2/2
✓ Branch 0 taken 189 times.
✓ Branch 1 taken 7 times.
 196 for (JointIndex i = 1; i < (JointIndex)(model.njoints); ++i)
78 {
79
2/4
✓ Branch 1 taken 189 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 189 times.
✗ Branch 5 not taken.
 189 com_global.noalias() =
80
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();
81
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);
82 }
83
84
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 7 return data.potential_energy;
85 }
86
87 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
88 294 Scalar computeMechanicalEnergy(
89 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
90 DataTpl<Scalar, Options, JointCollectionTpl> & data)
91 {
92
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.");
93
2/4
✓ Branch 1 taken 294 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 294 times.
✗ Branch 4 not taken.
 294 assert(model.check(MimicChecker()) && "Function does not support mimic joints");
94
95 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
96 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
97 typedef typename Model::Motion Motion;
98 typedef typename Model::JointIndex JointIndex;
99
100
0/4
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 294 data.kinetic_energy = Scalar(0);
101
0/4
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 294 data.potential_energy = Scalar(0);
102
103
1/2
✓ Branch 1 taken 294 times.
✗ Branch 2 not taken.
 294 typename Data::Vector3 com_global; // tmp variable
104
1/2
✓ Branch 1 taken 294 times.
✗ Branch 2 not taken.
 294 const typename Motion::ConstLinearType & g = model.gravity.linear();
105 typedef KineticEnergyAlgoForwardStep<Scalar, Options, JointCollectionTpl> Pass;
106
2/2
✓ Branch 0 taken 7305 times.
✓ Branch 1 taken 294 times.
 7599 for (JointIndex i = 1; i < (JointIndex)(model.njoints); ++i)
107 {
108
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));
109
2/4
✓ Branch 1 taken 7305 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7305 times.
✗ Branch 5 not taken.
 7305 com_global.noalias() =
110
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();
111
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);
112 }
113
0/4
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 294 data.kinetic_energy *= .5;
114
115
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;
116
117
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 294 return data.mechanical_energy;
118 }
119
120 namespace impl
121 {
122 template<
123 typename Scalar,
124 int Options,
125 template<typename, int> class JointCollectionTpl,
126 typename ConfigVectorType,
127 typename TangentVectorType>
128 8 Scalar computeKineticEnergy(
129 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
130 DataTpl<Scalar, Options, JointCollectionTpl> & data,
131 const Eigen::MatrixBase<ConfigVectorType> & q,
132 const Eigen::MatrixBase<TangentVectorType> & v)
133 {
134 8 pinocchio::impl::forwardKinematics(model, data, q.derived(), v.derived());
135 8 return pinocchio::computeKineticEnergy(model, data);
136 }
137
138 template<
139 typename Scalar,
140 int Options,
141 template<typename, int> class JointCollectionTpl,
142 typename ConfigVectorType>
143 6 Scalar computePotentialEnergy(
144 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
145 DataTpl<Scalar, Options, JointCollectionTpl> & data,
146 const Eigen::MatrixBase<ConfigVectorType> & q)
147 {
148 6 pinocchio::impl::forwardKinematics(model, data, q);
149 6 return pinocchio::computePotentialEnergy(model, data);
150 }
151
152 template<
153 typename Scalar,
154 int Options,
155 template<typename, int> class JointCollectionTpl,
156 typename ConfigVectorType,
157 typename TangentVectorType>
158 34 Scalar computeMechanicalEnergy(
159 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
160 DataTpl<Scalar, Options, JointCollectionTpl> & data,
161 const Eigen::MatrixBase<ConfigVectorType> & q,
162 const Eigen::MatrixBase<TangentVectorType> & v)
163 {
164 34 pinocchio::impl::forwardKinematics(model, data, q, v);
165 34 return pinocchio::computeMechanicalEnergy(model, data);
166 }
167 } // namespace impl
168
169 template<
170 typename Scalar,
171 int Options,
172 template<typename, int> class JointCollectionTpl,
173 typename ConfigVectorType,
174 typename TangentVectorType>
175 8 Scalar computeKineticEnergy(
176 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
177 DataTpl<Scalar, Options, JointCollectionTpl> & data,
178 const Eigen::MatrixBase<ConfigVectorType> & q,
179 const Eigen::MatrixBase<TangentVectorType> & v)
180 {
181
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));
182 }
183
184 template<
185 typename Scalar,
186 int Options,
187 template<typename, int> class JointCollectionTpl,
188 typename ConfigVectorType>
189 6 Scalar computePotentialEnergy(
190 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
191 DataTpl<Scalar, Options, JointCollectionTpl> & data,
192 const Eigen::MatrixBase<ConfigVectorType> & q)
193 {
194
1/2
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
 6 return impl::computePotentialEnergy(model, data, make_const_ref(q));
195 }
196
197 template<
198 typename Scalar,
199 int Options,
200 template<typename, int> class JointCollectionTpl,
201 typename ConfigVectorType,
202 typename TangentVectorType>
203 34 Scalar computeMechanicalEnergy(
204 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
205 DataTpl<Scalar, Options, JointCollectionTpl> & data,
206 const Eigen::MatrixBase<ConfigVectorType> & q,
207 const Eigen::MatrixBase<TangentVectorType> & v)
208 {
209
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));
210 }
211
212 } // namespace pinocchio
213 #endif // __pinocchio_algorithm_energy_hxx__
214