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 |