Biorbd Release_1.13.0
Loading...
Searching...
No Matches
ActuatorSigmoidGauss3p.cpp
1#define BIORBD_API_EXPORTS
2#include "InternalForces/Actuators/ActuatorSigmoidGauss3p.h"
3
4#include "RigidBody/GeneralizedCoordinates.h"
5#include "RigidBody/GeneralizedVelocity.h"
6#include "Utils/Error.h"
7
8using namespace BIORBD_NAMESPACE;
9
11 : internal_forces::actuator::Actuator(),
12 m_theta(std::make_shared<utils::Scalar>(0)),
13 m_lambda(std::make_shared<utils::Scalar>(0)),
14 m_offset(std::make_shared<utils::Scalar>(0)),
15 m_r(std::make_shared<utils::Scalar>(0)),
16 m_qopt(std::make_shared<utils::Scalar>(0)) {
17 setType();
18}
19
28
30 int direction,
31 const utils::Scalar& theta,
32 const utils::Scalar& lambda,
33 const utils::Scalar& offset,
34 const utils::Scalar& r,
35 const utils::Scalar& qopt,
36 size_t dofIdx)
37 : internal_forces::actuator::Actuator(direction, dofIdx),
38 m_theta(std::make_shared<utils::Scalar>(theta)),
39 m_lambda(std::make_shared<utils::Scalar>(lambda)),
40 m_offset(std::make_shared<utils::Scalar>(offset)),
41 m_r(std::make_shared<utils::Scalar>(r)),
42 m_qopt(std::make_shared<utils::Scalar>(qopt)) {
43 setType();
44}
45
47 int direction,
48 const utils::Scalar& theta,
49 const utils::Scalar& lambda,
50 const utils::Scalar& offset,
51 const utils::Scalar& r,
52 const utils::Scalar& qopt,
53 size_t dofIdx,
54 const utils::String& jointName)
55 : internal_forces::actuator::Actuator(direction, dofIdx, jointName),
56 m_theta(std::make_shared<utils::Scalar>(theta)),
57 m_lambda(std::make_shared<utils::Scalar>(lambda)),
58 m_offset(std::make_shared<utils::Scalar>(offset)),
59 m_r(std::make_shared<utils::Scalar>(r)),
60 m_qopt(std::make_shared<utils::Scalar>(qopt)) {
61 setType();
62}
63
65
72
82
85 "torqueMax for ActuatorSigmoidGauss3p must be called with Q and Qdot");
86}
87
91 utils::Scalar pos(Q[static_cast<unsigned int>(*m_dofIdx)] * 180 / M_PI);
92 utils::Scalar speed(Qdot[static_cast<unsigned int>(*m_dofIdx)] * 180 / M_PI);
93
94 // Getting Tmax of Gauss3p from Sigmoid
95 utils::Scalar Tmax(*m_theta / (1 + exp(*m_lambda * speed)) + *m_offset);
96
97 // Calculation of the max torque
98 return Tmax * exp(-(*m_qopt - pos) * (*m_qopt - pos) / (2 * *m_r * *m_r));
99}
100
102 *m_type = internal_forces::actuator::TYPE::SIGMOIDGAUSS3P;
103}
Class ActuatorSigmoidGauss3p is a joint actuator type which maximum is 3 parameter gaussian (Gauss3p)...
std::shared_ptr< utils::Scalar > m_lambda
Tilt factor of the Sigmoid.
std::shared_ptr< utils::Scalar > m_r
Width of the gaussian curve.
virtual utils::Scalar torqueMax()
Return the maximal torque (invalid).
std::shared_ptr< utils::Scalar > m_theta
Amplitude of the Sigmoid.
std::shared_ptr< utils::Scalar > m_offset
Height of the Sigmoid.
ActuatorSigmoidGauss3p DeepCopy() const
Deep copy of the SigmoidGauss3p actuator.
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