Biorbd Release_1.13.0
Loading...
Searching...
No Matches
HillType.cpp
1#define BIORBD_API_EXPORTS
2
3#include "InternalForces/Muscles/HillType.h"
4
5#include "InternalForces/Muscles/Characteristics.h"
6#include "InternalForces/Muscles/MuscleGeometry.h"
7#include "InternalForces/Muscles/State.h"
8#include "RigidBody/GeneralizedCoordinates.h"
9#include "RigidBody/GeneralizedVelocity.h"
10#include "RigidBody/Joints.h"
11#include "Utils/Error.h"
12
13#ifdef USE_SMOOTH_IF_ELSE
14#include "Utils/CasadiExpand.h"
15#endif
16
17using namespace BIORBD_NAMESPACE;
19 : internal_forces::muscles::Muscle(),
20 m_damping(std::make_shared<utils::Scalar>()),
21 m_FlCE(std::make_shared<utils::Scalar>()),
22 m_FlPE(std::make_shared<utils::Scalar>()),
23 m_FvCE(std::make_shared<utils::Scalar>()),
24 m_cste_FlCE_1(std::make_shared<utils::Scalar>(0.15)),
25 m_cste_FlCE_2(std::make_shared<utils::Scalar>(0.45)),
26 m_cste_FvCE_1(std::make_shared<utils::Scalar>(1)),
28 std::make_shared<utils::Scalar>(
29 -.33 / 2 * *m_cste_FvCE_1 / (1 + *m_cste_FvCE_1))),
30 m_cste_FlPE_1(std::make_shared<utils::Scalar>(10.0)),
31 m_cste_FlPE_2(std::make_shared<utils::Scalar>(5.0)),
32 m_cste_eccentricForceMultiplier(std::make_shared<utils::Scalar>(1.8)),
33 m_cste_damping(std::make_shared<utils::Scalar>(0.1)) {
34 setType();
35}
36
38 const utils::String &name,
41 : internal_forces::muscles::Muscle(name, geometry, characteristics),
42 m_damping(std::make_shared<utils::Scalar>()),
43 m_FlCE(std::make_shared<utils::Scalar>()),
44 m_FlPE(std::make_shared<utils::Scalar>()),
45 m_FvCE(std::make_shared<utils::Scalar>()),
46 m_cste_FlCE_1(std::make_shared<utils::Scalar>(0.15)),
47 m_cste_FlCE_2(std::make_shared<utils::Scalar>(0.45)),
48 m_cste_FvCE_1(std::make_shared<utils::Scalar>(1)),
50 std::make_shared<utils::Scalar>(
51 -.33 / 2 * *m_cste_FvCE_1 / (1 + *m_cste_FvCE_1))),
52 m_cste_FlPE_1(std::make_shared<utils::Scalar>(10.0)),
53 m_cste_FlPE_2(std::make_shared<utils::Scalar>(5.0)),
54 m_cste_eccentricForceMultiplier(std::make_shared<utils::Scalar>(1.8)),
55 m_cste_damping(std::make_shared<utils::Scalar>(0.1)) {
56 setType();
57}
58
60 const utils::String &name,
64 : internal_forces::muscles::Muscle(name, geometry, characteristics, emg),
65 m_damping(std::make_shared<utils::Scalar>()),
66 m_FlCE(std::make_shared<utils::Scalar>()),
67 m_FlPE(std::make_shared<utils::Scalar>()),
68 m_FvCE(std::make_shared<utils::Scalar>()),
69 m_cste_FlCE_1(std::make_shared<utils::Scalar>(0.15)),
70 m_cste_FlCE_2(std::make_shared<utils::Scalar>(0.45)),
71 m_cste_FvCE_1(std::make_shared<utils::Scalar>(1)),
73 std::make_shared<utils::Scalar>(
74 -.33 / 2 * *m_cste_FvCE_1 / (1 + *m_cste_FvCE_1))),
75 m_cste_FlPE_1(std::make_shared<utils::Scalar>(10.0)),
76 m_cste_FlPE_2(std::make_shared<utils::Scalar>(5.0)),
77 m_cste_eccentricForceMultiplier(std::make_shared<utils::Scalar>(1.8)),
78 m_cste_damping(std::make_shared<utils::Scalar>(0.1)) {
79 setType();
80}
81
83 const utils::String &name,
86 const internal_forces::PathModifiers &pathModifiers)
87 : internal_forces::muscles::Muscle(
88 name,
89 geometry,
91 pathModifiers),
92 m_damping(std::make_shared<utils::Scalar>()),
93 m_FlCE(std::make_shared<utils::Scalar>()),
94 m_FlPE(std::make_shared<utils::Scalar>()),
95 m_FvCE(std::make_shared<utils::Scalar>()),
96 m_cste_FlCE_1(std::make_shared<utils::Scalar>(0.15)),
97 m_cste_FlCE_2(std::make_shared<utils::Scalar>(0.45)),
98 m_cste_FvCE_1(std::make_shared<utils::Scalar>(1)),
100 std::make_shared<utils::Scalar>(
101 -.33 / 2 * *m_cste_FvCE_1 / (1 + *m_cste_FvCE_1))),
102 m_cste_FlPE_1(std::make_shared<utils::Scalar>(10.0)),
103 m_cste_FlPE_2(std::make_shared<utils::Scalar>(5.0)),
104 m_cste_eccentricForceMultiplier(std::make_shared<utils::Scalar>(1.8)),
105 m_cste_damping(std::make_shared<utils::Scalar>(0.1)) {
106 setType();
107}
109 const utils::String &name,
112 const internal_forces::PathModifiers &pathModifiers,
114 : internal_forces::muscles::Muscle(
115 name,
116 geometry,
118 pathModifiers,
119 state),
120 m_damping(std::make_shared<utils::Scalar>()),
121 m_FlCE(std::make_shared<utils::Scalar>()),
122 m_FlPE(std::make_shared<utils::Scalar>()),
123 m_FvCE(std::make_shared<utils::Scalar>()),
124 m_cste_FlCE_1(std::make_shared<utils::Scalar>(0.15)),
125 m_cste_FlCE_2(std::make_shared<utils::Scalar>(0.45)),
126 m_cste_FvCE_1(std::make_shared<utils::Scalar>(1)),
128 std::make_shared<utils::Scalar>(
129 -.33 / 2 * *m_cste_FvCE_1 / (1 + *m_cste_FvCE_1))),
130 m_cste_FlPE_1(std::make_shared<utils::Scalar>(10.0)),
131 m_cste_FlPE_2(std::make_shared<utils::Scalar>(5.0)),
132 m_cste_eccentricForceMultiplier(std::make_shared<utils::Scalar>(1.8)),
133 m_cste_damping(std::make_shared<utils::Scalar>(0.1)) {
134 setType();
135}
136
155
157 const std::shared_ptr<Muscle> other)
158 : internal_forces::muscles::Muscle(other) {
159 const std::shared_ptr<internal_forces::muscles::HillType> m_tp(
160 std::dynamic_pointer_cast<internal_forces::muscles::HillType>(other));
161 utils::Error::check(m_tp != nullptr, "Muscle must be of a Hill Type");
162 m_damping = m_tp->m_damping;
163 m_FlCE = m_tp->m_FlCE;
164 m_FlPE = m_tp->m_FlPE;
165 m_FvCE = m_tp->m_FvCE;
166 m_cste_FlCE_1 = m_tp->m_cste_FlCE_1;
167 m_cste_FlCE_2 = m_tp->m_cste_FlCE_2;
168 m_cste_FvCE_1 = m_tp->m_cste_FvCE_1;
169 m_cste_FvCE_2 = m_tp->m_cste_FvCE_2;
170 m_cste_FlPE_1 = m_tp->m_cste_FlPE_1;
171 m_cste_FlPE_2 = m_tp->m_cste_FlPE_2;
172 m_cste_eccentricForceMultiplier = m_tp->m_cste_eccentricForceMultiplier;
173 m_cste_damping = m_tp->m_cste_damping;
174}
175
179 copy.DeepCopy(*this);
180 return copy;
181}
182
199
202 // Compute the forces of each element
203 computeFvCE();
204 computeFlCE(emg);
205 computeFlPE();
207
208 // Combine the forces
209 computeForce(emg);
210 return *m_force;
211}
212
214 rigidbody::Joints &updatedModel,
218 bool updateMuscleParameters) {
219 // Update the configuration
220 if (updateMuscleParameters) updateOrientations(updatedModel, Q, Qdot);
221
222 // Computation
223 return force(emg);
224}
225
230 bool) {
231 utils::Error::raise("Hill type needs velocity");
232#ifdef _WIN32
233 return *m_force; // Will never reach here
234#endif
235}
236
239 computeFlCE(EMG);
240 return *m_FlCE;
241}
242
244 computeFlPE();
245 return *m_FlPE;
246}
247
249 computeFvCE();
250 return *m_FvCE;
251}
252
255 return *m_damping;
256}
257
259 *m_type = internal_forces::muscles::MUSCLE_TYPE::HILL;
260}
261
263#ifdef BIORBD_USE_CASADI_MATH
264 *m_damping = IF_ELSE_NAMESPACE::if_else_zero(
265 IF_ELSE_NAMESPACE::gt(position().velocity(), 0),
266 ((position().velocity() /
267 (characteristics().optimalLength() *
268 this->characteristics().maxShorteningSpeed())) *
270
271#else
272 if (position().velocity() > 0) {
273 *m_damping = (position().velocity() /
274 (characteristics().optimalLength() *
275 this->characteristics().maxShorteningSpeed())) *
277 } else {
278 *m_damping = 0;
279 }
280#endif
281}
282
285 *m_FlCE =
286 exp(-pow(
287 (position().length() / m_characteristics->optimalLength() /
288 (*m_cste_FlCE_1 * (1 - emg.activation()) + 1) -
289 1),
290 2) /
292}
293
295 // The relation is different if velocity< 0 or > 0
296 utils::Scalar v = m_position->velocity();
297#ifdef BIORBD_USE_CASADI_MATH
298 *m_FvCE = IF_ELSE_NAMESPACE::if_else(
299 IF_ELSE_NAMESPACE::le(v, 0),
300 (1.0 - std::fabs(v) / this->characteristics().maxShorteningSpeed()) /
301 (1.0 + std::fabs(v) / this->characteristics().maxShorteningSpeed() /
303 (1.0 - 1.33 * v / this->characteristics().maxShorteningSpeed() /
304 *m_cste_FvCE_2) /
305 (1 -
306 v / this->characteristics().maxShorteningSpeed() / *m_cste_FvCE_2));
307#else
308 if (v <= 0)
309 *m_FvCE = (1 - fabs(v) / this->characteristics().maxShorteningSpeed()) /
310 (1 + fabs(v) / this->characteristics().maxShorteningSpeed() /
312 else
313 *m_FvCE =
314 (1 - 1.33 * v / this->characteristics().maxShorteningSpeed() /
315 *m_cste_FvCE_2) /
316 (1 - v / this->characteristics().maxShorteningSpeed() / *m_cste_FvCE_2);
317#endif
318}
319
321#ifdef BIORBD_USE_CASADI_MATH
322 *m_FlPE = IF_ELSE_NAMESPACE::if_else_zero(
323 IF_ELSE_NAMESPACE::gt(position().length(), 0),
324 exp(*m_cste_FlPE_1 *
325 (position().length() / characteristics().optimalLength() - 1) -
326 *m_cste_FlPE_2));
327#else
328 if (position().length() > 0) {
329 *m_FlPE =
330 exp(*m_cste_FlPE_1 *
331 (position().length() / characteristics().optimalLength() - 1) -
333 } else {
334 *m_FlPE = 0;
335 }
336#endif
337}
338
341 utils::Scalar cosAngle = cos(characteristics().pennationAngle());
342 utils::Scalar damping_param;
343 if (characteristics().useDamping()) {
344 damping_param = *m_damping;
345 } else {
346 damping_param = 0;
347 }
348 return characteristics().forceIsoMax() *
349 (emg.activation() * *m_FlCE * *m_FvCE + *m_FlPE + damping_param) *
350 cosAngle;
351}
352
const utils::String & name() const
Return the name of the muscle.
Definition Compound.cpp:48
std::shared_ptr< utils::Scalar > m_force
The last computed force.
Definition Compound.h:108
virtual const utils::Scalar & force()
Return the last computed muscle force norm.
Definition Compound.cpp:63
Holder of all the path modifiers of a muscle.
Base class for all HillType muscles.
Definition HillType.h:26
std::shared_ptr< utils::Scalar > m_cste_FlPE_2
constant 2 used in the FlPE
Definition HillType.h:234
const utils::Scalar & FvCE()
Return the Force-Velocity of the contractile element.
Definition HillType.cpp:248
std::shared_ptr< utils::Scalar > m_damping
Muscle damping (spring force).
Definition HillType.h:214
std::shared_ptr< utils::Scalar > m_cste_FlPE_1
constant 1 used in the FlPE
Definition HillType.h:232
HillType DeepCopy() const
Deep copy of a Hill-type muscle.
Definition HillType.cpp:177
const utils::Scalar & FlCE(const State &emg)
Return the Force-Length of the contractile element.
Definition HillType.cpp:237
std::shared_ptr< utils::Scalar > m_cste_FlCE_2
constant 2 used in the FlCE
Definition HillType.h:226
virtual void computeFlCE(const State &emg)
Compute the Force-length of the contractile element.
Definition HillType.cpp:283
virtual void computeFvCE()
Compute the Force-Velocity of the contractile element.
Definition HillType.cpp:294
std::shared_ptr< utils::Scalar > m_FlCE
Force-Length of the contractile element.
Definition HillType.h:216
const utils::Scalar & damping()
Return the muscle damping (spring force).
Definition HillType.cpp:253
virtual void computeDamping()
Compute the muscle damping.
Definition HillType.cpp:262
std::shared_ptr< utils::Scalar > m_FvCE
Force-Velocity of the contractile element.
Definition HillType.h:220
virtual void computeFlPE()
Compute the Force-Length of the passive element.
Definition HillType.cpp:320
std::shared_ptr< utils::Scalar > m_cste_FvCE_1
constant 1 used in the FvCE
Definition HillType.h:228
std::shared_ptr< utils::Scalar > m_cste_FvCE_2
constant 2 used in the FvCE
Definition HillType.h:230
std::shared_ptr< utils::Scalar > m_cste_FlCE_1
constant 1 used in the FlCE
Definition HillType.h:224
virtual utils::Scalar getForceFromActivation(const State &emg)
Function allowing modification of the way the multiplication is done in computeForce(EMG).
Definition HillType.cpp:339
std::shared_ptr< utils::Scalar > m_cste_damping
parameters used in damping
Definition HillType.h:238
std::shared_ptr< utils::Scalar > m_FlPE
Force-Length of the passive element.
Definition HillType.h:218
const utils::Scalar & FlPE()
Return the Force-Length of the passive element.
Definition HillType.cpp:243
std::shared_ptr< utils::Scalar > m_cste_eccentricForceMultiplier
Constant used for ForceVelocity.
Definition HillType.h:236
virtual void normalizeEmg(State &emg)
Normalize the EMG data.
Definition HillType.cpp:353
virtual const utils::Scalar & force(const State &emg)
Return the muscle force vector at origin and insertion.
Definition HillType.cpp:200
std::shared_ptr< Characteristics > m_characteristics
The muscle characteristics.
Definition Muscle.h:347
Characteristics & characteristics() const
Return the muscle characteristics.
Definition Muscle.cpp:254
virtual void computeForce(const State &emg)
Computer the forces from a specific emg.
Definition Muscle.cpp:222
std::shared_ptr< internal_forces::muscles::MuscleGeometry > m_position
The geometry (position of the nodes) of the muscle.
Definition Muscle.h:345
const utils::Scalar & length(rigidbody::Joints &updatedModel, const rigidbody::GeneralizedCoordinates &Q, bool updateMuscleParameters=true)
Get the length of the muscle.
Definition Muscle.cpp:178
std::shared_ptr< MUSCLE_TYPE > m_type
The type of the muscle.
Definition Muscle.h:343
const internal_forces::muscles::MuscleGeometry & position() const
Return the position of all the points attached to the muscle (0 being the origin).
Definition Muscle.cpp:174
const State & state() const
Return the dynamic state.
Definition Muscle.cpp:277
const utils::Scalar & velocity(rigidbody::Joints &updatedModel, const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, bool updateMuscleParameters=true)
Return the velocity of the muscle.
Definition Muscle.cpp:198
void DeepCopy(const Muscle &other)
Deep copy of a muscle in new muscle.
Definition Muscle.cpp:120
void updateOrientations(rigidbody::Joints &updatedModel, const rigidbody::GeneralizedCoordinates &Q)
Update the position of the origin and insertion positions of the muscle.
Definition Muscle.cpp:138
EMG holder to interact with the muscle.
Definition State.h:17
const utils::Scalar & normalizeExcitation(const State &emgMax, bool turnOffWarnings=false)
Compute and return the normalized excitation.
Definition State.cpp:65
const utils::Scalar & activation() const
Return the muscle activation.
Definition State.cpp:117
This is the core of the musculoskeletal model in biorbd.
Definition Joints.h:48
static void raise(const String &message)
Throw an error message.
Definition Error.cpp:6
static void check(bool cond, const String &message)
Assert that raises the error message if false.
Definition Error.cpp:10
Wrapper around the std::string class with augmented functionality.
Definition String.h:18