Biorbd Release_1.13.0
Loading...
Searching...
No Matches
ActuatorGauss3p.cpp
1#define BIORBD_API_EXPORTS
2#include "InternalForces/Actuators/ActuatorGauss3p.h"
3
4#include "RigidBody/GeneralizedCoordinates.h"
5#include "RigidBody/GeneralizedVelocity.h"
6#include "Utils/Error.h"
7
8#ifdef USE_SMOOTH_IF_ELSE
9#include "Utils/CasadiExpand.h"
10#endif
11
12using namespace BIORBD_NAMESPACE;
13
15 : internal_forces::actuator::Actuator(),
16 m_k(std::make_shared<utils::Scalar>(4.3)),
17 m_Tmax(std::make_shared<utils::Scalar>(0)),
18 m_T0(std::make_shared<utils::Scalar>(0)),
19 m_wmax(std::make_shared<utils::Scalar>(0)),
20 m_wc(std::make_shared<utils::Scalar>(0)),
21 m_amax(std::make_shared<utils::Scalar>(1.0)),
22 m_amin(std::make_shared<utils::Scalar>(0)),
23 m_wr(std::make_shared<utils::Scalar>(0)),
24 m_w1(std::make_shared<utils::Scalar>(0)),
25 m_r(std::make_shared<utils::Scalar>(0)),
26 m_qopt(std::make_shared<utils::Scalar>(0)) {
27 setType();
28}
29
32 : internal_forces::actuator::Actuator(other),
33 m_k(other.m_k),
34 m_Tmax(other.m_Tmax),
35 m_T0(other.m_T0),
36 m_wmax(other.m_wmax),
37 m_wc(other.m_wc),
38 m_amax(other.m_amax),
39 m_amin(other.m_amin),
40 m_wr(other.m_wr),
41 m_w1(other.m_w1),
42 m_r(other.m_r),
43 m_qopt(other.m_qopt) {}
44
46 int direction,
47 const utils::Scalar& Tmax,
48 const utils::Scalar& T0,
49 const utils::Scalar& wmax,
50 const utils::Scalar& wc,
51 const utils::Scalar& amin,
52 const utils::Scalar& wr,
53 const utils::Scalar& w1,
54 const utils::Scalar& r,
55 const utils::Scalar& qopt,
56 size_t dofIdx)
57 : internal_forces::actuator::Actuator(direction, dofIdx),
58 m_k(std::make_shared<utils::Scalar>(4.3)),
59 m_Tmax(std::make_shared<utils::Scalar>(Tmax)),
60 m_T0(std::make_shared<utils::Scalar>(T0)),
61 m_wmax(std::make_shared<utils::Scalar>(wmax)),
62 m_wc(std::make_shared<utils::Scalar>(wc)),
63 m_amax(std::make_shared<utils::Scalar>(1.0)),
64 m_amin(std::make_shared<utils::Scalar>(amin)),
65 m_wr(std::make_shared<utils::Scalar>(wr)),
66 m_w1(std::make_shared<utils::Scalar>(w1)),
67 m_r(std::make_shared<utils::Scalar>(r)),
68 m_qopt(std::make_shared<utils::Scalar>(qopt)) {
69 setType();
70}
71
73 int direction,
74 const utils::Scalar& Tmax,
75 const utils::Scalar& T0,
76 const utils::Scalar& wmax,
77 const utils::Scalar& wc,
78 const utils::Scalar& amin,
79 const utils::Scalar& wr,
80 const utils::Scalar& w1,
81 const utils::Scalar& r,
82 const utils::Scalar& qopt,
83 size_t dofIdx,
84 const utils::String& jointName)
85 : internal_forces::actuator::Actuator(direction, dofIdx, jointName),
86 m_k(std::make_shared<utils::Scalar>(4.3)),
87 m_Tmax(std::make_shared<utils::Scalar>(Tmax)),
88 m_T0(std::make_shared<utils::Scalar>(T0)),
89 m_wmax(std::make_shared<utils::Scalar>(wmax)),
90 m_wc(std::make_shared<utils::Scalar>(wc)),
91 m_amax(std::make_shared<utils::Scalar>(1.0)),
92 m_amin(std::make_shared<utils::Scalar>(amin)),
93 m_wr(std::make_shared<utils::Scalar>(wr)),
94 m_w1(std::make_shared<utils::Scalar>(w1)),
95 m_r(std::make_shared<utils::Scalar>(r)),
96 m_qopt(std::make_shared<utils::Scalar>(qopt)) {
97 setType();
98}
99
101
108
112 *m_k = *other.m_k;
113 *m_Tmax = *other.m_Tmax;
114 *m_T0 = *other.m_T0;
115 *m_wmax = *other.m_wmax;
116 *m_wc = *other.m_wc;
117 *m_amax = *other.m_amax;
118 *m_amin = *other.m_amin;
119 *m_wr = *other.m_wr;
120 *m_w1 = *other.m_w1;
121 *m_r = *other.m_r;
122 *m_qopt = *other.m_qopt;
123}
124
127 "torqueMax for ActuatorGauss3p must be called with Q and Qdot");
128}
129
132 const rigidbody::GeneralizedVelocity& Qdot) {
133 utils::Scalar pos(Q[static_cast<unsigned int>(*m_dofIdx)] * 180 / M_PI);
134 utils::Scalar speed(Qdot[static_cast<unsigned int>(*m_dofIdx)] * 180 / M_PI);
135
136 // Tetanic torque max
137 utils::Scalar Tc = *m_T0 * *m_wc / *m_wmax;
138 utils::Scalar C = Tc * (*m_wmax + *m_wc); // concentric
139 utils::Scalar we = ((*m_Tmax - *m_T0) * *m_wmax * *m_wc) /
140 (*m_k * *m_T0 * (*m_wmax + *m_wc));
141 utils::Scalar E = -(*m_Tmax - *m_T0) * we; // excentric
142
143 utils::Scalar Tw;
144#ifdef BIORBD_USE_CASADI_MATH
145 Tw = IF_ELSE_NAMESPACE::if_else(
146 IF_ELSE_NAMESPACE::ge(speed, 0),
147 C / (*m_wc + speed) - Tc,
148 E / (we - speed) + *m_Tmax);
149#else
150 if (speed >= 0) {
151 Tw = C / (*m_wc + speed) - Tc; // For the concentric
152 } else {
153 Tw = E / (we - speed) + *m_Tmax; // For the excentric
154 }
155#endif
156
157 // Differential activation
158 utils::Scalar A =
159 *m_amin +
160 (*m_amax - *m_amin) / (1 + utils::Scalar(exp(-(speed - *m_w1) / *m_wr)));
161
162 // Torque angle
163 utils::Scalar Ta =
164 exp(-(*m_qopt - pos) * (*m_qopt - pos) / (2 * *m_r * *m_r));
165
166 // Calculation of the max torque
167 return Tw * A * Ta;
168}
169
171 *m_type = internal_forces::actuator::TYPE::GAUSS3P;
172}
Class ActuatorGauss3p is a joint actuator type which maximum is 3 parameter gaussian (Gauss3p) Please...
std::shared_ptr< utils::Scalar > m_w1
Mid point plateau.
std::shared_ptr< utils::Scalar > m_amax
Maximum activation level (set to 1).
virtual utils::Scalar torqueMax()
Return the maximal torque (invalid).
ActuatorGauss3p DeepCopy() const
Deep copy of the Gauss3p actuator.
std::shared_ptr< utils::Scalar > m_Tmax
Maximum torque in the eccentric phase.
std::shared_ptr< utils::Scalar > m_k
Ratio of slope of the eccentric and concentric phases.
std::shared_ptr< utils::Scalar > m_wr
1/10 de la distance amax/amin
std::shared_ptr< utils::Scalar > m_r
Width of the gaussian curve.
std::shared_ptr< utils::Scalar > m_amin
Low plateau level.
std::shared_ptr< utils::Scalar > m_T0
Maximum torque isometric.
std::shared_ptr< utils::Scalar > m_qopt
Optimal position.
void DeepCopy(const Actuator &other)
Deep copy of actuator.
Definition Actuator.cpp:48
int direction() const
Return the direction of the actuator.
Definition Actuator.cpp:58
std::shared_ptr< TYPE > m_type
The type of the actuator.
Definition Actuator.h:90
std::shared_ptr< size_t > m_dofIdx
Index of the DoF associated with the actuator.
Definition Actuator.h:96
static void raise(const String &message)
Throw an error message.
Definition Error.cpp:6
Wrapper around the std::string class with augmented functionality.
Definition String.h:18