Biorbd Release_1.13.0
Loading...
Searching...
No Matches
ActuatorGauss6p.cpp
1#define BIORBD_API_EXPORTS
2#include "InternalForces/Actuators/ActuatorGauss6p.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 m_facteur(std::make_shared<utils::Scalar>(0)),
28 m_r2(std::make_shared<utils::Scalar>(0)),
29 m_qopt2(std::make_shared<utils::Scalar>(0)) {
30 setType();
31}
32
35 : internal_forces::actuator::Actuator(other),
36 m_k(other.m_k),
37 m_Tmax(other.m_Tmax),
38 m_T0(other.m_T0),
39 m_wmax(other.m_wmax),
40 m_wc(other.m_wc),
41 m_amax(other.m_amax),
42 m_amin(other.m_amin),
43 m_wr(other.m_wr),
44 m_w1(other.m_w1),
45 m_r(other.m_r),
46 m_qopt(other.m_qopt),
47 m_facteur(other.m_facteur),
48 m_r2(other.m_r2),
49 m_qopt2(other.m_qopt2) {}
51 int direction,
52 const utils::Scalar& Tmax,
53 const utils::Scalar& T0,
54 const utils::Scalar& wmax,
55 const utils::Scalar& wc,
56 const utils::Scalar& amin,
57 const utils::Scalar& wr,
58 const utils::Scalar& w1,
59 const utils::Scalar& r,
60 const utils::Scalar& qopt,
61 const utils::Scalar& facteur,
62 const utils::Scalar& r2,
63 const utils::Scalar& qopt2,
64 size_t dofIdx)
65 : Actuator(direction, dofIdx),
66 m_k(std::make_shared<utils::Scalar>(4.3)),
67 m_Tmax(std::make_shared<utils::Scalar>(Tmax)),
68 m_T0(std::make_shared<utils::Scalar>(T0)),
69 m_wmax(std::make_shared<utils::Scalar>(wmax)),
70 m_wc(std::make_shared<utils::Scalar>(wc)),
71 m_amax(std::make_shared<utils::Scalar>(1.0)),
72 m_amin(std::make_shared<utils::Scalar>(amin)),
73 m_wr(std::make_shared<utils::Scalar>(wr)),
74 m_w1(std::make_shared<utils::Scalar>(w1)),
75 m_r(std::make_shared<utils::Scalar>(r)),
76 m_qopt(std::make_shared<utils::Scalar>(qopt)),
77 m_facteur(std::make_shared<utils::Scalar>(facteur)),
78 m_r2(std::make_shared<utils::Scalar>(r2)),
79 m_qopt2(std::make_shared<utils::Scalar>(qopt2)) {
80 setType();
81}
82
84 int direction,
85 const utils::Scalar& Tmax,
86 const utils::Scalar& T0,
87 const utils::Scalar& wmax,
88 const utils::Scalar& wc,
89 const utils::Scalar& amin,
90 const utils::Scalar& wr,
91 const utils::Scalar& w1,
92 const utils::Scalar& r,
93 const utils::Scalar& qopt,
94 const utils::Scalar& facteur,
95 const utils::Scalar& r2,
96 const utils::Scalar& qopt2,
97 size_t dofIdx,
98 const utils::String& jointName)
99 : Actuator(direction, dofIdx, jointName),
100 m_k(std::make_shared<utils::Scalar>(4.3)),
101 m_Tmax(std::make_shared<utils::Scalar>(Tmax)),
102 m_T0(std::make_shared<utils::Scalar>(T0)),
103 m_wmax(std::make_shared<utils::Scalar>(wmax)),
104 m_wc(std::make_shared<utils::Scalar>(wc)),
105 m_amax(std::make_shared<utils::Scalar>(1.0)),
106 m_amin(std::make_shared<utils::Scalar>(amin)),
107 m_wr(std::make_shared<utils::Scalar>(wr)),
108 m_w1(std::make_shared<utils::Scalar>(w1)),
109 m_r(std::make_shared<utils::Scalar>(r)),
110 m_qopt(std::make_shared<utils::Scalar>(qopt)),
111 m_facteur(std::make_shared<utils::Scalar>(facteur)),
112 m_r2(std::make_shared<utils::Scalar>(r2)),
113 m_qopt2(std::make_shared<utils::Scalar>(qopt2)) {
114 setType();
115}
116
118
125
129 *m_k = *other.m_k;
130 *m_Tmax = *other.m_Tmax;
131 *m_T0 = *other.m_T0;
132 *m_wmax = *other.m_wmax;
133 *m_wc = *other.m_wc;
134 *m_amax = *other.m_amax;
135 *m_amin = *other.m_amin;
136 *m_wr = *other.m_wr;
137 *m_w1 = *other.m_w1;
138 *m_r = *other.m_r;
139 *m_qopt = *other.m_qopt;
140 *m_facteur = *other.m_facteur;
141 *m_r2 = *other.m_r2;
142 *m_qopt2 = *other.m_qopt2;
143}
144
147 "torqueMax for ActuatorGauss6p must be called with Q and Qdot");
148}
149
152 const rigidbody::GeneralizedVelocity& Qdot) {
153 utils::Scalar pos(Q[static_cast<unsigned int>(*m_dofIdx)] * 180 / M_PI);
154 utils::Scalar speed(Qdot[static_cast<unsigned int>(*m_dofIdx)] * 180 / M_PI);
155
156 // Tetanic torque max
157 utils::Scalar Tc = *m_T0 * *m_wc / *m_wmax;
158 utils::Scalar C = Tc * (*m_wmax + *m_wc); // concentric
159 utils::Scalar we = ((*m_Tmax - *m_T0) * *m_wmax * *m_wc) /
160 (*m_k * *m_T0 * (*m_wmax + *m_wc));
161 utils::Scalar E = -(*m_Tmax - *m_T0) * we; // eccentric
162
163 utils::Scalar Tw;
164#ifdef BIORBD_USE_CASADI_MATH
165 Tw = IF_ELSE_NAMESPACE::if_else(
166 IF_ELSE_NAMESPACE::ge(speed, 0),
167 C / (*m_wc + speed) - Tc,
168 E / (we - speed) + *m_Tmax);
169#else
170 if (speed >= 0) {
171 Tw = C / (*m_wc + speed) - Tc; // For the concentric
172 } else {
173 Tw = E / (we - speed) + *m_Tmax; // For the eccentric
174 }
175#endif
176
177 // Differential activation
178 utils::Scalar A =
179 *m_amin +
180 (*m_amax - *m_amin) / (1 + utils::Scalar(exp(-(speed - *m_w1) / *m_wr)));
181
182 // Torque angle
183 utils::Scalar Ta =
184 utils::Scalar(
185 exp(-(*m_qopt - pos) * (*m_qopt - pos) / (2 * *m_r * *m_r))) +
186 *m_facteur *
187 utils::Scalar(
188 exp(-(*m_qopt2 - pos) * (*m_qopt2 - pos) / (2 * *m_r2 * *m_r2)));
189
190 // Calculation of the max torque
191 return Tw * A * Ta;
192}
193
195 *m_type = internal_forces::actuator::TYPE::GAUSS6P;
196}
Class ActuatorGauss6p is a joint actuator type which maximum is bimodal 6 parameter gaussian (Gauss6p...
std::shared_ptr< utils::Scalar > m_facteur
Factor of the 6p.
std::shared_ptr< utils::Scalar > m_amin
Low plateau level.
ActuatorGauss6p DeepCopy() const
Deep copy of the Gauss6p actuator.
std::shared_ptr< utils::Scalar > m_T0
Maximum torque isometric.
std::shared_ptr< utils::Scalar > m_Tmax
Maximum torque in the eccentric phase.
std::shared_ptr< utils::Scalar > m_w1
Mid point plateau.
std::shared_ptr< utils::Scalar > m_k
Ratio of slope of the eccentric and concentric phases.
std::shared_ptr< utils::Scalar > m_r2
width of the 2nd gaussian curve
std::shared_ptr< utils::Scalar > m_r
width of the 1st gaussian curve
std::shared_ptr< utils::Scalar > m_qopt2
2nd Optimal position
virtual utils::Scalar torqueMax()
Return the maximal torque (invalid).
std::shared_ptr< utils::Scalar > m_amax
Maximum activation level (set to 1).
std::shared_ptr< utils::Scalar > m_qopt
1st Optimal position
std::shared_ptr< utils::Scalar > m_wr
1/10 of the distance amax/amin
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