GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/pinocchio/algorithm/energy.hpp Lines: 6 6 100.0 %
Date: 2024-01-23 21:41:47 Branches: 0 0 - %

Line Branch Exec Source
1
//
2
// Copyright (c) 2016-2020 CNRS INRIA
3
//
4
5
#ifndef __pinocchio_algorithm_energy_hpp__
6
#define __pinocchio_algorithm_energy_hpp__
7
8
#include "pinocchio/multibody/model.hpp"
9
#include "pinocchio/multibody/data.hpp"
10
#include "pinocchio/algorithm/kinematics.hpp"
11
#include "pinocchio/algorithm/check.hpp"
12
13
namespace pinocchio {
14
15
  ///
16
  /// \brief Computes the kinetic energy of the system.
17
  ///        The result is accessible through data.kinetic_energy.
18
  ///
19
  /// \tparam JointCollection Collection of Joint types.
20
  ///
21
  /// \param[in] model The model structure of the rigid body system.
22
  /// \param[in] data The data structure of the rigid body system.
23
  ///
24
  /// \return The kinetic energy of the system in [J].
25
  ///
26
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
27
  inline Scalar
28
  computeKineticEnergy(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
29
                       DataTpl<Scalar,Options,JointCollectionTpl> & data);
30
31
  ///
32
  /// \brief Computes the kinetic energy of the system.
33
  ///        The result is accessible through data.kinetic_energy.
34
  ///
35
  /// \tparam JointCollection Collection of Joint types.
36
  /// \tparam ConfigVectorType Type of the joint configuration vector.
37
  /// \tparam TangentVectorType Type of the joint velocity vector.
38
  ///
39
  /// \param[in] model The model structure of the rigid body system.
40
  /// \param[in] data The data structure of the rigid body system.
41
  /// \param[in] q The joint configuration vector (dim model.nq).
42
  /// \param[in] v The joint velocity vector (dim model.nv).
43
  ///
44
  /// \return The kinetic energy of the system in [J].
45
  ///
46
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
47
  inline Scalar
48
5
  computeKineticEnergy(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
49
                       DataTpl<Scalar,Options,JointCollectionTpl> & data,
50
                       const Eigen::MatrixBase<ConfigVectorType> & q,
51
                       const Eigen::MatrixBase<TangentVectorType> & v)
52
  {
53
5
    forwardKinematics(model,data,q.derived(),v.derived());
54
5
    return computeKineticEnergy(model,data);
55
  }
56
57
  ///
58
  /// \brief Computes the kinetic energy of the system.
59
  ///        The result is accessible through data.kinetic_energy.
60
  ///
61
  /// \tparam JointCollection Collection of Joint types.
62
  /// \tparam ConfigVectorType Type of the joint configuration vector.
63
  /// \tparam TangentVectorType Type of the joint velocity vector.
64
  ///
65
  /// \param[in] model The model structure of the rigid body system.
66
  /// \param[in] data The data structure of the rigid body system.
67
  /// \param[in] q The joint configuration vector (dim model.nq).
68
  /// \param[in] v The joint velocity vector (dim model.nv).
69
  /// \param[in] update_kinematics If true, first apply the forward kinematics on the kinematic tree.
70
  ///
71
  /// \return The kinetic energy of the system in [J].
72
  ///
73
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
74
  PINOCCHIO_DEPRECATED
75
  inline Scalar
76
  kineticEnergy(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
77
                DataTpl<Scalar,Options,JointCollectionTpl> & data,
78
                const Eigen::MatrixBase<ConfigVectorType> & q,
79
                const Eigen::MatrixBase<TangentVectorType> & v,
80
                const bool update_kinematics)
81
  {
82
    if(update_kinematics)
83
      return computeKineticEnergy(model,data,q.derived(),v.derived());
84
    else
85
      return computeKineticEnergy(model,data);
86
  }
87
88
  ///
89
  /// \brief Computes the potential energy of the system, i.e. the potential energy linked to the gravity field.
90
  ///        The result is accessible through data.potential_energy.
91
  ///
92
  /// \tparam JointCollection Collection of Joint types.
93
  ///
94
  /// \note This potential energy are of the for \f$ \sum_{i} - m_{i}gh_{i} \f$ where:
95
  ///       -  \f$ m_{i} \f$ is the mass of the body \f$ i \f$,
96
  ///       -  \f$ h_{i} \f$ is the height of the body \f$ i \f$,
97
  ///       -  \f$ g \f$ is the gravity value.
98
  ///
99
  /// \tparam JointCollection Collection of Joint types.
100
  ///
101
  /// \param[in] model The model structure of the rigid body system.
102
  /// \param[in] data The data structure of the rigid body system.
103
  ///
104
  /// \return The potential energy of the system expressed in [J].
105
  ///
106
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
107
  inline Scalar
108
  computePotentialEnergy(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
109
                         DataTpl<Scalar,Options,JointCollectionTpl> & data);
110
111
  ///
112
  /// \brief Computes the potential energy of the system, i.e. the potential energy linked to the gravity field.
113
  ///        The result is accessible through data.potential_energy.
114
  ///
115
  /// \tparam JointCollection Collection of Joint types.
116
  /// \tparam ConfigVectorType Type of the joint configuration vector.
117
  ///
118
  /// \note This potential energy are of the for \f$ \sum_{i} - m_{i}gh_{i} \f$ where:
119
  ///       -  \f$ m_{i} \f$ is the mass of the body \f$ i \f$,
120
  ///       -  \f$ h_{i} \f$ is the height of the body \f$ i \f$,
121
  ///       -  \f$ g \f$ is the gravity value.
122
  ///
123
  /// \param[in] model The model structure of the rigid body system.
124
  /// \param[in] data The data structure of the rigid body system.
125
  /// \param[in] q The joint configuration vector (dim model.nq).
126
  ///
127
  /// \return The potential energy of the system expressed in [J].
128
  ///
129
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
130
  inline Scalar
131
5
  computePotentialEnergy(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
132
                         DataTpl<Scalar,Options,JointCollectionTpl> & data,
133
                         const Eigen::MatrixBase<ConfigVectorType> & q)
134
  {
135
5
    forwardKinematics(model,data,q);
136
5
    return computePotentialEnergy(model,data);
137
  }
138
139
  ///
140
  /// \brief Computes the potential energy of the system, i.e. the potential energy linked to the gravity field.
141
  ///        The result is accessible through data.potential_energy.
142
  ///
143
  /// \tparam JointCollection Collection of Joint types.
144
  /// \tparam ConfigVectorType Type of the joint configuration vector.
145
  ///
146
  /// \param[in] model The model structure of the rigid body system.
147
  /// \param[in] data The data structure of the rigid body system.
148
  /// \param[in] q The joint configuration vector (dim model.nq).
149
  /// \param[in] update_kinematics If true, first apply the forward kinematics on the kinematic tree.
150
  ///
151
  /// \return The potential energy of the system expressed in [J].
152
  ///
153
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
154
  PINOCCHIO_DEPRECATED
155
  inline Scalar
156
  potentialEnergy(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
157
                  DataTpl<Scalar,Options,JointCollectionTpl> & data,
158
                  const Eigen::MatrixBase<ConfigVectorType> & q,
159
                  const bool update_kinematics)
160
  {
161
    if(update_kinematics)
162
      return computePotentialEnergy(model,data,q);
163
    else
164
      return computePotentialEnergy(model,data);
165
  }
166
}
167
168
#include "pinocchio/algorithm/energy.hxx"
169
170
#endif // __pinocchio_algorithm_energy_hpp__