Biorbd Release_1.13.0
Loading...
Searching...
No Matches
StateDynamics.cpp
1#define BIORBD_API_EXPORTS
2#include "InternalForces/Muscles/StateDynamics.h"
3
4#include "InternalForces/Muscles/Characteristics.h"
5#include "Utils/Error.h"
6#include "Utils/String.h"
7
8#ifdef USE_SMOOTH_IF_ELSE
9#include "Utils/CasadiExpand.h"
10#endif
11
12using namespace BIORBD_NAMESPACE;
13
15 const utils::Scalar& excitation,
16 const utils::Scalar& activation)
17 : internal_forces::muscles::State(excitation, activation),
18 m_previousExcitation(std::make_shared<utils::Scalar>(0)),
19 m_previousActivation(std::make_shared<utils::Scalar>(0)),
20 m_activationDot(std::make_shared<utils::Scalar>(0)) {
21 setType();
22}
23
26 : internal_forces::muscles::State(other) {
27 const auto& state =
28 dynamic_cast<const internal_forces::muscles::StateDynamics&>(other);
29 m_previousExcitation = state.m_previousExcitation;
30 m_previousActivation = state.m_previousActivation;
31 m_activationDot = state.m_activationDot;
32}
33
37
44
53
54const utils::Scalar&
57 const internal_forces::muscles::Characteristics& characteristics,
58 bool alreadyNormalized) {
60 emg.excitation(), emg.activation(), characteristics, alreadyNormalized);
61}
62
63const utils::Scalar&
65 const utils::Scalar& excitation,
66 const utils::Scalar& activation,
67 const internal_forces::muscles::Characteristics& characteristics,
68 bool alreadyNormalized) {
71 return timeDerivativeActivation(characteristics, alreadyNormalized);
72}
73
74const utils::Scalar&
76 const internal_forces::muscles::Characteristics& characteristics,
77 bool alreadyNormalized) {
78 // Implémentation de la fonction da/dt = (u-a)/GeneralizedTorque(u,a)
79 // ou GeneralizedTorque(u,a) = t_act(0.5+1.5*a) is u>a et
80 // GeneralizedTorque(u,a)=t_deact(0.5+1.5*a) sinon
81#ifdef BIORBD_USE_CASADI_MATH
82 *m_activation = IF_ELSE_NAMESPACE::if_else(
83 IF_ELSE_NAMESPACE::lt(*m_activation, characteristics.minActivation()),
84 characteristics.minActivation(),
86 *m_excitation = IF_ELSE_NAMESPACE::if_else(
87 IF_ELSE_NAMESPACE::lt(*m_excitation, characteristics.minActivation()),
88 characteristics.minActivation(),
90#else
91 if (*m_activation < characteristics.minActivation()) {
92 *m_activation = characteristics.minActivation();
93 }
94
95 if (*m_excitation < characteristics.minActivation()) {
96 *m_excitation = characteristics.minActivation();
97 }
98#endif
99
100 // see doi:10.1016/j.humov.2011.08.006
101 // see doi:10.1016/S0021-9290(03)00010-1
102
103 // http://simtk-confluence.stanford.edu:8080/display/OpenSim/First-Order+Activation+Dynamics
104
105 utils::Scalar num;
106 if (alreadyNormalized) {
107 num = *m_excitation - *m_activation; // numérateur
108 } else {
109 num = normalizeExcitation(characteristics.stateMax()) -
110 *m_activation; // numérateur
111 }
112
113 utils::Scalar denom; // dénominateur
114#ifdef BIORBD_USE_CASADI_MATH
115 denom = IF_ELSE_NAMESPACE::if_else(
116 IF_ELSE_NAMESPACE::gt(num, 0),
117 characteristics.torqueActivation() * (0.5 + 1.5 * *m_activation),
118 characteristics.torqueDeactivation() / (0.5 + 1.5 * *m_activation));
119#else
120 if (num > 0) {
121 denom = characteristics.torqueActivation() * (0.5 + 1.5 * *m_activation);
122 } else {
123 denom = characteristics.torqueDeactivation() / (0.5 + 1.5 * *m_activation);
124 }
125#endif
126 *m_activationDot = num / denom;
127 return *m_activationDot;
128}
129
130const utils::Scalar&
134
136 const utils::Scalar& val,
137 bool turnOffWarnings) {
140}
141
142const utils::Scalar&
146
148 const utils::Scalar& val,
149 bool turnOffWarnings) {
152}
153
154const utils::Scalar&
158
160 *m_stateType = internal_forces::muscles::STATE_TYPE::DYNAMIC;
161}
const utils::Scalar & torqueActivation() const
Return the time activation constant.
const utils::Scalar & minActivation() const
Return the minimal activation of the muscle.
const utils::Scalar & torqueDeactivation() const
Return the time deactivation constant.
const State & stateMax() const
Return the maximal excitation and activation of the muscle.
EMG with the capability to compute the time derivative.
virtual const utils::Scalar & timeDerivativeActivation()
Return the previously computed activation time derivative.
virtual void setActivation(const utils::Scalar &val, bool turnOffWarnings=false)
Set the muscle activation.
const utils::Scalar & previousActivation() const
Return the previous activation.
std::shared_ptr< utils::Scalar > m_activationDot
The activation velocity.
std::shared_ptr< utils::Scalar > m_previousActivation
The previous activation.
virtual void setType()
Set the type to simple_state.
StateDynamics DeepCopy() const
Deep copy of state dynamics.
virtual const utils::Scalar & timeDerivativeActivation(const utils::Scalar &excitation, const utils::Scalar &activation, const Characteristics &characteristics, bool alreadyNormalized=false)
Compute and return the activation time derivative from the excitation and activation.
std::shared_ptr< utils::Scalar > m_previousExcitation
The previous excitation.
const utils::Scalar & previousExcitation() const
Return the previous activation.
virtual void setExcitation(const utils::Scalar &val, bool turnOffWarnings=false)
Set the muscle excitation.
StateDynamics(const utils::Scalar &excitation=0, const utils::Scalar &activation=0)
Construct the state dynamics.
EMG holder to interact with the muscle.
Definition State.h:17
State DeepCopy() const
Deep copy of state.
Definition State.cpp:28
std::shared_ptr< STATE_TYPE > m_stateType
The state type.
Definition State.h:127
virtual void setActivation(const utils::Scalar &val, bool turnOffWarnings=false)
Set the muscle activation.
Definition State.cpp:89
virtual void setExcitation(const utils::Scalar &val, bool turnOffWarnings=false)
Set the muscle excitation.
Definition State.cpp:43
std::shared_ptr< utils::Scalar > m_activation
The muscle activation.
Definition State.h:131
const utils::Scalar & excitation() const
Return the muscle excitation.
Definition State.cpp:61
std::shared_ptr< utils::Scalar > m_excitationNorm
The normalized excitation.
Definition State.h:130
const utils::Scalar & normalizeExcitation(const State &emgMax, bool turnOffWarnings=false)
Compute and return the normalized excitation.
Definition State.cpp:65
std::shared_ptr< utils::Scalar > m_excitation
The muscle excitation.
Definition State.h:128
const utils::Scalar & activation() const
Return the muscle activation.
Definition State.cpp:117
State(const utils::Scalar &excitation=0, const utils::Scalar &activation=0)
Construct a state.
Definition State.cpp:7