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/multibody/model.hpp" |
9 |
|
|
#include "pinocchio/multibody/data.hpp" |
10 |
|
|
#include "pinocchio/algorithm/check.hpp" |
11 |
|
|
|
12 |
|
|
namespace pinocchio |
13 |
|
|
{ |
14 |
|
|
|
15 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
16 |
|
|
inline Scalar |
17 |
|
225 |
computeKineticEnergy(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
18 |
|
|
DataTpl<Scalar,Options,JointCollectionTpl> & data) |
19 |
|
|
{ |
20 |
✓✗ |
225 |
assert(model.check(data) && "data is not consistent with model."); |
21 |
|
|
|
22 |
|
|
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
23 |
|
|
typedef typename Model::JointIndex JointIndex; |
24 |
|
|
|
25 |
|
225 |
data.kinetic_energy = Scalar(0); |
26 |
|
|
|
27 |
✓✓ |
5872 |
for(JointIndex i=1; i<(JointIndex)(model.njoints); ++i) |
28 |
|
5647 |
data.kinetic_energy += model.inertias[i].vtiv(data.v[i]); |
29 |
|
|
|
30 |
|
225 |
data.kinetic_energy *= .5; |
31 |
|
|
|
32 |
|
225 |
return data.kinetic_energy; |
33 |
|
|
} |
34 |
|
|
|
35 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
36 |
|
|
inline Scalar |
37 |
|
225 |
computePotentialEnergy(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
38 |
|
|
DataTpl<Scalar,Options,JointCollectionTpl> & data) |
39 |
|
|
{ |
40 |
✓✗✓✗
|
225 |
assert(model.check(data) && "data is not consistent with model.");; |
41 |
|
|
|
42 |
|
|
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
43 |
|
|
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
44 |
|
|
typedef typename Model::JointIndex JointIndex; |
45 |
|
|
typedef typename Model::Motion Motion; |
46 |
|
|
|
47 |
|
225 |
data.potential_energy = Scalar(0); |
48 |
✓✗ |
225 |
const typename Motion::ConstLinearType & g = model.gravity.linear(); |
49 |
|
|
|
50 |
✓✗ |
225 |
typename Data::Vector3 com_global; // tmp variable |
51 |
✓✓ |
5872 |
for(JointIndex i=1; i<(JointIndex)(model.njoints); ++i) |
52 |
|
|
{ |
53 |
✓✗✓✗
✓✗✓✗
✓✗✓✗
|
5647 |
com_global.noalias() = data.oMi[i].translation() + data.oMi[i].rotation() * model.inertias[i].lever(); |
54 |
✓✗ |
5647 |
data.potential_energy -= model.inertias[i].mass() * com_global.dot(g); |
55 |
|
|
} |
56 |
|
|
|
57 |
|
225 |
return data.potential_energy; |
58 |
|
|
} |
59 |
|
|
} |
60 |
|
|
#endif // __pinocchio_algorithm_energy_hxx__ |
61 |
|
|
|