Biorbd
Release_1.13.0
Toggle main menu visibility
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
12
using namespace
BIORBD_NAMESPACE;
13
14
internal_forces::actuator::ActuatorGauss3p::ActuatorGauss3p
()
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
30
internal_forces::actuator::ActuatorGauss3p::ActuatorGauss3p
(
31
const
internal_forces::actuator::ActuatorGauss3p
& other)
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
45
internal_forces::actuator::ActuatorGauss3p::ActuatorGauss3p
(
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
72
internal_forces::actuator::ActuatorGauss3p::ActuatorGauss3p
(
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
100
internal_forces::actuator::ActuatorGauss3p::~ActuatorGauss3p
() {}
101
102
internal_forces::actuator::ActuatorGauss3p
103
internal_forces::actuator::ActuatorGauss3p::DeepCopy
()
const
{
104
internal_forces::actuator::ActuatorGauss3p
copy;
105
copy.
DeepCopy
(*
this
);
106
return
copy;
107
}
108
109
void
internal_forces::actuator::ActuatorGauss3p::DeepCopy
(
110
const
internal_forces::actuator::ActuatorGauss3p
& other) {
111
internal_forces::actuator::Actuator::DeepCopy
(other);
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
125
utils::Scalar
internal_forces::actuator::ActuatorGauss3p::torqueMax
() {
126
utils::Error::raise
(
127
"torqueMax for ActuatorGauss3p must be called with Q and Qdot"
);
128
}
129
130
utils::Scalar
internal_forces::actuator::ActuatorGauss3p::torqueMax
(
131
const
rigidbody::GeneralizedCoordinates
& Q,
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
170
void
internal_forces::actuator::ActuatorGauss3p::setType
() {
171
*
m_type
= internal_forces::actuator::TYPE::GAUSS3P;
172
}
BIORBD_NAMESPACE::internal_forces::actuator::ActuatorGauss3p
Class ActuatorGauss3p is a joint actuator type which maximum is 3 parameter gaussian (Gauss3p) Please...
Definition
ActuatorGauss3p.h:22
BIORBD_NAMESPACE::internal_forces::actuator::ActuatorGauss3p::m_w1
std::shared_ptr< utils::Scalar > m_w1
Mid point plateau.
Definition
ActuatorGauss3p.h:151
BIORBD_NAMESPACE::internal_forces::actuator::ActuatorGauss3p::m_wmax
std::shared_ptr< utils::Scalar > m_wmax
Definition
ActuatorGauss3p.h:140
BIORBD_NAMESPACE::internal_forces::actuator::ActuatorGauss3p::~ActuatorGauss3p
virtual ~ActuatorGauss3p()
Destroy the class properly.
Definition
ActuatorGauss3p.cpp:100
BIORBD_NAMESPACE::internal_forces::actuator::ActuatorGauss3p::m_amax
std::shared_ptr< utils::Scalar > m_amax
Maximum activation level (set to 1).
Definition
ActuatorGauss3p.h:148
BIORBD_NAMESPACE::internal_forces::actuator::ActuatorGauss3p::torqueMax
virtual utils::Scalar torqueMax()
Return the maximal torque (invalid).
Definition
ActuatorGauss3p.cpp:125
BIORBD_NAMESPACE::internal_forces::actuator::ActuatorGauss3p::DeepCopy
ActuatorGauss3p DeepCopy() const
Deep copy of the Gauss3p actuator.
Definition
ActuatorGauss3p.cpp:103
BIORBD_NAMESPACE::internal_forces::actuator::ActuatorGauss3p::setType
virtual void setType()
Set the type of actuator.
Definition
ActuatorGauss3p.cpp:170
BIORBD_NAMESPACE::internal_forces::actuator::ActuatorGauss3p::m_Tmax
std::shared_ptr< utils::Scalar > m_Tmax
Maximum torque in the eccentric phase.
Definition
ActuatorGauss3p.h:138
BIORBD_NAMESPACE::internal_forces::actuator::ActuatorGauss3p::m_wc
std::shared_ptr< utils::Scalar > m_wc
Definition
ActuatorGauss3p.h:143
BIORBD_NAMESPACE::internal_forces::actuator::ActuatorGauss3p::m_k
std::shared_ptr< utils::Scalar > m_k
Ratio of slope of the eccentric and concentric phases.
Definition
ActuatorGauss3p.h:136
BIORBD_NAMESPACE::internal_forces::actuator::ActuatorGauss3p::m_wr
std::shared_ptr< utils::Scalar > m_wr
1/10 de la distance amax/amin
Definition
ActuatorGauss3p.h:150
BIORBD_NAMESPACE::internal_forces::actuator::ActuatorGauss3p::m_r
std::shared_ptr< utils::Scalar > m_r
Width of the gaussian curve.
Definition
ActuatorGauss3p.h:154
BIORBD_NAMESPACE::internal_forces::actuator::ActuatorGauss3p::ActuatorGauss3p
ActuatorGauss3p()
Construct Gauss3p actuator.
Definition
ActuatorGauss3p.cpp:14
BIORBD_NAMESPACE::internal_forces::actuator::ActuatorGauss3p::m_amin
std::shared_ptr< utils::Scalar > m_amin
Low plateau level.
Definition
ActuatorGauss3p.h:149
BIORBD_NAMESPACE::internal_forces::actuator::ActuatorGauss3p::m_T0
std::shared_ptr< utils::Scalar > m_T0
Maximum torque isometric.
Definition
ActuatorGauss3p.h:139
BIORBD_NAMESPACE::internal_forces::actuator::ActuatorGauss3p::m_qopt
std::shared_ptr< utils::Scalar > m_qopt
Optimal position.
Definition
ActuatorGauss3p.h:155
BIORBD_NAMESPACE::internal_forces::actuator::Actuator::DeepCopy
void DeepCopy(const Actuator &other)
Deep copy of actuator.
Definition
Actuator.cpp:48
BIORBD_NAMESPACE::internal_forces::actuator::Actuator::direction
int direction() const
Return the direction of the actuator.
Definition
Actuator.cpp:58
BIORBD_NAMESPACE::internal_forces::actuator::Actuator::m_type
std::shared_ptr< TYPE > m_type
The type of the actuator.
Definition
Actuator.h:90
BIORBD_NAMESPACE::internal_forces::actuator::Actuator::m_dofIdx
std::shared_ptr< size_t > m_dofIdx
Index of the DoF associated with the actuator.
Definition
Actuator.h:96
BIORBD_NAMESPACE::internal_forces::actuator::Actuator::Actuator
Actuator()
Construct actuator.
Definition
Actuator.cpp:9
BIORBD_NAMESPACE::rigidbody::GeneralizedCoordinates
Class GeneralizedCoordinates.
Definition
GeneralizedCoordinates.h:15
BIORBD_NAMESPACE::rigidbody::GeneralizedVelocity
Class GeneralizedVelocity.
Definition
GeneralizedVelocity.h:15
BIORBD_NAMESPACE::utils::Error::raise
static void raise(const String &message)
Throw an error message.
Definition
Error.cpp:6
BIORBD_NAMESPACE::utils::String
Wrapper around the std::string class with augmented functionality.
Definition
String.h:18
src
InternalForces
Actuators
ActuatorGauss3p.cpp
Generated by
1.18.0