GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/energy.hxx
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 46 52 88.5%
Branches: 41 86 47.7%
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 14444 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 7222 times.
✗ Branch 2 not taken.
 14444 const JointIndex & i = jmodel.id();
31
1/2
✓ Branch 3 taken 7222 times.
✗ Branch 4 not taken.
 14444 data.kinetic_energy += model.inertias[i].vtiv(data.v[i]);
32 14444 data.kinetic_energy +=
33
4/8
✓ Branch 1 taken 7222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7222 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7222 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7222 times.
✗ Branch 11 not taken.
 28888 (jmodel.jointVelocitySelector(model.armature).array() * jdata.joint_v().array().square())
34
3/6
✓ Branch 1 taken 7222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7222 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7222 times.
✗ Branch 8 not taken.
 28888 .sum();
35 }
36 };
37
38 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
39 8 Scalar computeKineticEnergy(
40 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
41 DataTpl<Scalar, Options, JointCollectionTpl> & data)
42 {
43
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 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 8 data.kinetic_energy = Scalar(0);
49 typedef KineticEnergyAlgoForwardStep<Scalar, Options, JointCollectionTpl> Pass;
50
2/2
✓ Branch 0 taken 216 times.
✓ Branch 1 taken 8 times.
 224 for (JointIndex i = 1; i < (JointIndex)(model.njoints); ++i)
51 {
52
1/2
✓ Branch 4 taken 216 times.
✗ Branch 5 not taken.
 216 Pass::run(model.joints[i], data.joints[i], typename Pass::ArgsType(model, data));
53 }
54 8 data.kinetic_energy *= .5;
55
56 8 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 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/2
✓ Branch 3 taken 189 times.
✗ Branch 4 not taken.
 189 data.potential_energy -= model.inertias[i].mass() * com_global.dot(g);
81 }
82
83 7 return data.potential_energy;
84 }
85
86 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
87 283 Scalar computeMechanicalEnergy(
88 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
89 DataTpl<Scalar, Options, JointCollectionTpl> & data)
90 {
91
2/4
✓ Branch 1 taken 283 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 283 times.
✗ Branch 4 not taken.
 283 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 283 data.kinetic_energy = Scalar(0);
99 283 data.potential_energy = Scalar(0);
100
101
1/2
✓ Branch 1 taken 283 times.
✗ Branch 2 not taken.
 283 typename Data::Vector3 com_global; // tmp variable
102
1/2
✓ Branch 1 taken 283 times.
✗ Branch 2 not taken.
 283 const typename Motion::ConstLinearType & g = model.gravity.linear();
103 typedef KineticEnergyAlgoForwardStep<Scalar, Options, JointCollectionTpl> Pass;
104
2/2
✓ Branch 0 taken 7006 times.
✓ Branch 1 taken 283 times.
 7289 for (JointIndex i = 1; i < (JointIndex)(model.njoints); ++i)
105 {
106
2/4
✓ Branch 1 taken 7006 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 7006 times.
✗ Branch 7 not taken.
 7006 Pass::run(model.joints[i], data.joints[i], typename Pass::ArgsType(model, data));
107
2/4
✓ Branch 1 taken 7006 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7006 times.
✗ Branch 5 not taken.
 7006 com_global.noalias() =
108
4/8
✓ Branch 4 taken 7006 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7006 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 7006 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7006 times.
✗ Branch 15 not taken.
 7006 data.oMi[i].translation() + data.oMi[i].rotation() * model.inertias[i].lever();
109
1/2
✓ Branch 3 taken 7006 times.
✗ Branch 4 not taken.
 7006 data.potential_energy -= model.inertias[i].mass() * com_global.dot(g);
110 }
111 283 data.kinetic_energy *= .5;
112
113 283 data.mechanical_energy = data.kinetic_energy + data.potential_energy;
114
115 283 return data.mechanical_energy;
116 }
117
118 namespace impl
119 {
120 template<
121 typename Scalar,
122 int Options,
123 template<typename, int>
124 class JointCollectionTpl,
125 typename ConfigVectorType,
126 typename TangentVectorType>
127 7 Scalar computeKineticEnergy(
128 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
129 DataTpl<Scalar, Options, JointCollectionTpl> & data,
130 const Eigen::MatrixBase<ConfigVectorType> & q,
131 const Eigen::MatrixBase<TangentVectorType> & v)
132 {
133 7 pinocchio::impl::forwardKinematics(model, data, q.derived(), v.derived());
134 7 return pinocchio::computeKineticEnergy(model, data);
135 }
136
137 template<
138 typename Scalar,
139 int Options,
140 template<typename, int>
141 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>
156 class JointCollectionTpl,
157 typename ConfigVectorType,
158 typename TangentVectorType>
159 34 Scalar computeMechanicalEnergy(
160 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
161 DataTpl<Scalar, Options, JointCollectionTpl> & data,
162 const Eigen::MatrixBase<ConfigVectorType> & q,
163 const Eigen::MatrixBase<TangentVectorType> & v)
164 {
165 34 pinocchio::impl::forwardKinematics(model, data, q, v);
166 34 return pinocchio::computeMechanicalEnergy(model, data);
167 }
168 } // namespace impl
169
170 template<
171 typename Scalar,
172 int Options,
173 template<typename, int>
174 class JointCollectionTpl,
175 typename ConfigVectorType,
176 typename TangentVectorType>
177 ✗ Scalar computeKineticEnergy(
178 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
179 DataTpl<Scalar, Options, JointCollectionTpl> & data,
180 const Eigen::MatrixBase<ConfigVectorType> & q,
181 const Eigen::MatrixBase<TangentVectorType> & v)
182 {
183 ✗ return impl::computeKineticEnergy(model, data, make_const_ref(q), make_const_ref(v));
184 }
185
186 template<
187 typename Scalar,
188 int Options,
189 template<typename, int>
190 class JointCollectionTpl,
191 typename ConfigVectorType>
192 ✗ Scalar computePotentialEnergy(
193 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
194 DataTpl<Scalar, Options, JointCollectionTpl> & data,
195 const Eigen::MatrixBase<ConfigVectorType> & q)
196 {
197 ✗ return impl::computePotentialEnergy(model, data, make_const_ref(q));
198 }
199
200 template<
201 typename Scalar,
202 int Options,
203 template<typename, int>
204 class JointCollectionTpl,
205 typename ConfigVectorType,
206 typename TangentVectorType>
207 ✗ Scalar computeMechanicalEnergy(
208 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
209 DataTpl<Scalar, Options, JointCollectionTpl> & data,
210 const Eigen::MatrixBase<ConfigVectorType> & q,
211 const Eigen::MatrixBase<TangentVectorType> & v)
212 {
213 ✗ return impl::computeMechanicalEnergy(model, data, make_const_ref(q), make_const_ref(v));
214 }
215
216 } // namespace pinocchio
217 #endif // __pinocchio_algorithm_energy_hxx__
218