Biorbd
Release_1.13.0
Toggle main menu visibility
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
8
using namespace
BIORBD_NAMESPACE;
9
10
internal_forces::actuator::ActuatorSigmoidGauss3p::ActuatorSigmoidGauss3p
()
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
20
internal_forces::actuator::ActuatorSigmoidGauss3p::ActuatorSigmoidGauss3p
(
21
const
internal_forces::actuator::ActuatorSigmoidGauss3p
& other)
22
: internal_forces::actuator::
Actuator
(other),
23
m_theta
(other.
m_theta
),
24
m_lambda
(other.
m_lambda
),
25
m_offset
(other.
m_offset
),
26
m_r
(other.
m_r
),
27
m_qopt
(other.
m_qopt
) {}
28
29
internal_forces::actuator::ActuatorSigmoidGauss3p::ActuatorSigmoidGauss3p
(
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
46
internal_forces::actuator::ActuatorSigmoidGauss3p::ActuatorSigmoidGauss3p
(
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
64
internal_forces::actuator::ActuatorSigmoidGauss3p::~ActuatorSigmoidGauss3p
() {}
65
66
internal_forces::actuator::ActuatorSigmoidGauss3p
67
internal_forces::actuator::ActuatorSigmoidGauss3p::DeepCopy
()
const
{
68
internal_forces::actuator::ActuatorSigmoidGauss3p
copy;
69
copy.
DeepCopy
(*
this
);
70
return
copy;
71
}
72
73
void
internal_forces::actuator::ActuatorSigmoidGauss3p::DeepCopy
(
74
const
internal_forces::actuator::ActuatorSigmoidGauss3p
& other) {
75
internal_forces::actuator::Actuator::DeepCopy
(other);
76
*
m_theta
= *other.
m_theta
;
77
*
m_lambda
= *other.
m_lambda
;
78
*
m_offset
= *other.
m_offset
;
79
*
m_r
= *other.
m_r
;
80
*
m_qopt
= *other.
m_qopt
;
81
}
82
83
utils::Scalar
internal_forces::actuator::ActuatorSigmoidGauss3p::torqueMax
() {
84
utils::Error::raise
(
85
"torqueMax for ActuatorSigmoidGauss3p must be called with Q and Qdot"
);
86
}
87
88
utils::Scalar
internal_forces::actuator::ActuatorSigmoidGauss3p::torqueMax
(
89
const
rigidbody::GeneralizedCoordinates
& Q,
90
const
rigidbody::GeneralizedVelocity
& Qdot) {
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
101
void
internal_forces::actuator::ActuatorSigmoidGauss3p::setType
() {
102
*
m_type
= internal_forces::actuator::TYPE::SIGMOIDGAUSS3P;
103
}
BIORBD_NAMESPACE::internal_forces::actuator::ActuatorSigmoidGauss3p
Class ActuatorSigmoidGauss3p is a joint actuator type which maximum is 3 parameter gaussian (Gauss3p)...
Definition
ActuatorSigmoidGauss3p.h:22
BIORBD_NAMESPACE::internal_forces::actuator::ActuatorSigmoidGauss3p::m_lambda
std::shared_ptr< utils::Scalar > m_lambda
Tilt factor of the Sigmoid.
Definition
ActuatorSigmoidGauss3p.h:124
BIORBD_NAMESPACE::internal_forces::actuator::ActuatorSigmoidGauss3p::m_qopt
std::shared_ptr< utils::Scalar > m_qopt
Optimal position.
Definition
ActuatorSigmoidGauss3p.h:120
BIORBD_NAMESPACE::internal_forces::actuator::ActuatorSigmoidGauss3p::~ActuatorSigmoidGauss3p
virtual ~ActuatorSigmoidGauss3p()
Destroy the class properly.
Definition
ActuatorSigmoidGauss3p.cpp:64
BIORBD_NAMESPACE::internal_forces::actuator::ActuatorSigmoidGauss3p::m_r
std::shared_ptr< utils::Scalar > m_r
Width of the gaussian curve.
Definition
ActuatorSigmoidGauss3p.h:119
BIORBD_NAMESPACE::internal_forces::actuator::ActuatorSigmoidGauss3p::setType
virtual void setType()
Set the type of actuator.
Definition
ActuatorSigmoidGauss3p.cpp:101
BIORBD_NAMESPACE::internal_forces::actuator::ActuatorSigmoidGauss3p::torqueMax
virtual utils::Scalar torqueMax()
Return the maximal torque (invalid).
Definition
ActuatorSigmoidGauss3p.cpp:83
BIORBD_NAMESPACE::internal_forces::actuator::ActuatorSigmoidGauss3p::ActuatorSigmoidGauss3p
ActuatorSigmoidGauss3p()
Construct SigmoidGauss3p actuator.
Definition
ActuatorSigmoidGauss3p.cpp:10
BIORBD_NAMESPACE::internal_forces::actuator::ActuatorSigmoidGauss3p::m_theta
std::shared_ptr< utils::Scalar > m_theta
Amplitude of the Sigmoid.
Definition
ActuatorSigmoidGauss3p.h:123
BIORBD_NAMESPACE::internal_forces::actuator::ActuatorSigmoidGauss3p::m_offset
std::shared_ptr< utils::Scalar > m_offset
Height of the Sigmoid.
Definition
ActuatorSigmoidGauss3p.h:125
BIORBD_NAMESPACE::internal_forces::actuator::ActuatorSigmoidGauss3p::DeepCopy
ActuatorSigmoidGauss3p DeepCopy() const
Deep copy of the SigmoidGauss3p actuator.
Definition
ActuatorSigmoidGauss3p.cpp:67
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
ActuatorSigmoidGauss3p.cpp
Generated by
1.18.0