1#define BIORBD_API_EXPORTS
2#include "InternalForces/Actuators/ActuatorGauss6p.h"
4#include "RigidBody/GeneralizedCoordinates.h"
5#include "RigidBody/GeneralizedVelocity.h"
6#include "Utils/Error.h"
8#ifdef USE_SMOOTH_IF_ELSE
9#include "Utils/CasadiExpand.h"
12using namespace BIORBD_NAMESPACE;
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)) {
35 : internal_forces::actuator::
Actuator(other),
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,
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)) {
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,
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)) {
147 "torqueMax for ActuatorGauss6p must be called with Q and 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);
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);
171 Tw = C / (*
m_wc + speed) - Tc;
173 Tw = E / (we - speed) + *
m_Tmax;
195 *
m_type = internal_forces::actuator::TYPE::GAUSS6P;
Class ActuatorGauss6p is a joint actuator type which maximum is bimodal 6 parameter gaussian (Gauss6p...
virtual void setType()
Set the type of actuator.
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.
virtual ~ActuatorGauss6p()
Destroy the class properly.
std::shared_ptr< utils::Scalar > m_r2
width of the 2nd gaussian curve
std::shared_ptr< utils::Scalar > m_wmax
std::shared_ptr< utils::Scalar > m_r
width of the 1st gaussian curve
std::shared_ptr< utils::Scalar > m_qopt2
2nd Optimal position
std::shared_ptr< utils::Scalar > m_wc
ActuatorGauss6p()
Construct Gauss6p actuator.
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.
int direction() const
Return the direction of the actuator.
std::shared_ptr< TYPE > m_type
The type of the actuator.
std::shared_ptr< size_t > m_dofIdx
Index of the DoF associated with the actuator.
Actuator()
Construct actuator.
Class GeneralizedCoordinates.
Class GeneralizedVelocity.
static void raise(const String &message)
Throw an error message.
Wrapper around the std::string class with augmented functionality.