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