Biorbd
Release_1.13.0
Toggle main menu visibility
Loading...
Searching...
No Matches
PassiveTorqueExponential.cpp
1
#define BIORBD_API_EXPORTS
2
#include "InternalForces/PassiveTorques/PassiveTorqueExponential.h"
3
4
#include "RigidBody/GeneralizedCoordinates.h"
5
#include "Utils/Error.h"
6
7
using namespace
BIORBD_NAMESPACE;
8
9
internal_forces::passive_torques::PassiveTorqueExponential::
10
PassiveTorqueExponential
()
11
: internal_forces::passive_torques::
PassiveTorque
(),
12
m_k1
(std::make_shared<utils::Scalar>(0)),
13
m_k2
(std::make_shared<utils::Scalar>(0)),
14
m_b1
(std::make_shared<utils::Scalar>(0)),
15
m_b2
(std::make_shared<utils::Scalar>(0)),
16
m_qMid
(std::make_shared<utils::Scalar>(0)),
17
m_tauEq
(std::make_shared<utils::Scalar>(0)),
18
m_pBeta
(std::make_shared<utils::Scalar>(0)),
19
m_wMax
(std::make_shared<utils::Scalar>(0)),
20
m_sV
(std::make_shared<utils::Scalar>(0)),
21
m_deltaP
(std::make_shared<utils::Scalar>(0)) {
22
setType
();
23
}
24
25
internal_forces::passive_torques::PassiveTorqueExponential::
26
PassiveTorqueExponential
(
27
const
internal_forces::passive_torques::PassiveTorqueExponential
& other)
28
: internal_forces::passive_torques::
PassiveTorque
(other),
29
m_k1
(other.
m_k1
),
30
m_k2
(other.
m_k2
),
31
m_b1
(other.
m_b1
),
32
m_b2
(other.
m_b2
),
33
m_qMid
(other.
m_qMid
),
34
m_tauEq
(other.
m_tauEq
),
35
m_pBeta
(other.
m_pBeta
),
36
m_wMax
(other.
m_wMax
),
37
m_sV
(other.
m_sV
),
38
m_deltaP
(other.
m_deltaP
) {
39
setType
();
40
}
41
42
internal_forces::passive_torques::PassiveTorqueExponential::
43
PassiveTorqueExponential
(
44
const
utils::Scalar& k1,
45
const
utils::Scalar& k2,
46
const
utils::Scalar& b1,
47
const
utils::Scalar& b2,
48
const
utils::Scalar& qMid,
49
const
utils::Scalar& tauEq,
50
const
utils::Scalar& pBeta,
51
const
utils::Scalar& wMax,
52
const
utils::Scalar& sV,
53
const
utils::Scalar& deltaP,
54
size_t
dofIdx)
55
: internal_forces::passive_torques::
PassiveTorque
(dofIdx),
56
m_k1
(std::make_shared<utils::Scalar>(k1)),
57
m_k2
(std::make_shared<utils::Scalar>(k2)),
58
m_b1
(std::make_shared<utils::Scalar>(b1)),
59
m_b2
(std::make_shared<utils::Scalar>(b2)),
60
m_qMid
(std::make_shared<utils::Scalar>(qMid)),
61
m_tauEq
(std::make_shared<utils::Scalar>(tauEq)),
62
m_pBeta
(std::make_shared<utils::Scalar>(pBeta)),
63
m_wMax
(std::make_shared<utils::Scalar>(wMax)),
64
m_sV
(std::make_shared<utils::Scalar>(sV)),
65
m_deltaP
(std::make_shared<utils::Scalar>(deltaP)) {
66
setType
();
67
}
68
69
internal_forces::passive_torques::PassiveTorqueExponential::
70
PassiveTorqueExponential
(
71
const
utils::Scalar& k1,
72
const
utils::Scalar& k2,
73
const
utils::Scalar& b1,
74
const
utils::Scalar& b2,
75
const
utils::Scalar& qMid,
76
const
utils::Scalar& tauEq,
77
const
utils::Scalar& pBeta,
78
const
utils::Scalar& wMax,
79
const
utils::Scalar& sV,
80
const
utils::Scalar& deltaP,
81
size_t
dofIdx,
82
const
utils::String
& jointName)
83
: internal_forces::passive_torques::
PassiveTorque
(dofIdx, jointName),
84
m_k1
(std::make_shared<utils::Scalar>(k1)),
85
m_k2
(std::make_shared<utils::Scalar>(k2)),
86
m_b1
(std::make_shared<utils::Scalar>(b1)),
87
m_b2
(std::make_shared<utils::Scalar>(b2)),
88
m_qMid
(std::make_shared<utils::Scalar>(qMid)),
89
m_tauEq
(std::make_shared<utils::Scalar>(tauEq)),
90
m_pBeta
(std::make_shared<utils::Scalar>(pBeta)),
91
m_wMax
(std::make_shared<utils::Scalar>(wMax)),
92
m_sV
(std::make_shared<utils::Scalar>(sV)),
93
m_deltaP
(std::make_shared<utils::Scalar>(deltaP)) {
94
setType
();
95
}
96
97
internal_forces::passive_torques::PassiveTorqueExponential::
98
~PassiveTorqueExponential
() {}
99
100
internal_forces::passive_torques::PassiveTorqueExponential
101
internal_forces::passive_torques::PassiveTorqueExponential::DeepCopy
()
const
{
102
internal_forces::passive_torques::PassiveTorqueExponential
copy;
103
copy.
DeepCopy
(*
this
);
104
return
copy;
105
}
106
107
void
internal_forces::passive_torques::PassiveTorqueExponential::DeepCopy
(
108
const
internal_forces::passive_torques::PassiveTorqueExponential
& other) {
109
internal_forces::passive_torques::PassiveTorque::DeepCopy
(other);
110
*
m_k1
= *other.
m_k1
;
111
*
m_k2
= *other.
m_k2
;
112
*
m_b1
= *other.
m_b1
;
113
*
m_b2
= *other.
m_b2
;
114
*
m_qMid
= *other.
m_qMid
;
115
*
m_tauEq
= *other.
m_tauEq
;
116
*
m_pBeta
= *other.
m_pBeta
;
117
*
m_wMax
= *other.
m_wMax
;
118
*
m_sV
= *other.
m_sV
;
119
*
m_deltaP
= *other.
m_deltaP
;
120
}
121
122
utils::Scalar
123
internal_forces::passive_torques::PassiveTorqueExponential::passiveTorque
() {
124
utils::Error::raise
(
125
"passiveTorque for PassiveTorqueExponential must be called with Q"
);
126
}
127
128
utils::Scalar
129
internal_forces::passive_torques::PassiveTorqueExponential::passiveTorque
(
130
const
rigidbody::GeneralizedCoordinates
& Q,
131
const
rigidbody::GeneralizedCoordinates
& Qdot)
const
{
132
unsigned
int
dofIdx(
static_cast<
unsigned
int
>
(*
m_dofIdx
));
133
return
(*
m_b1
* exp(*
m_k1
* (Q[dofIdx] - *
m_qMid
)) +
134
*
m_b2
* exp(*
m_k2
* (Q[dofIdx] - *
m_qMid
))) *
135
(1 - *
m_pBeta
* (Qdot[dofIdx] / (*
m_sV
* *
m_wMax
))) *
136
(Q[dofIdx] - *
m_deltaP
) +
137
*
m_tauEq
;
138
}
139
140
void
internal_forces::passive_torques::PassiveTorqueExponential::setType
() {
141
*
m_type
= internal_forces::passive_torques::TORQUE_TYPE::TORQUE_EXPONENTIAL;
142
}
BIORBD_NAMESPACE::internal_forces::passive_torques::PassiveTorqueExponential
Class PassiveTorqueExponential is a joint passive torque type that linearly evolves passive torque is...
Definition
PassiveTorqueExponential.h:21
BIORBD_NAMESPACE::internal_forces::passive_torques::PassiveTorqueExponential::passiveTorque
virtual utils::Scalar passiveTorque()
Return the passive torque (not available for this type, Q and QDot must be provided).
Definition
PassiveTorqueExponential.cpp:123
BIORBD_NAMESPACE::internal_forces::passive_torques::PassiveTorqueExponential::~PassiveTorqueExponential
virtual ~PassiveTorqueExponential()
Destroy the class properly.
Definition
PassiveTorqueExponential.cpp:98
BIORBD_NAMESPACE::internal_forces::passive_torques::PassiveTorqueExponential::m_k2
std::shared_ptr< utils::Scalar > m_k2
Stiffness of the 2nd exponential.
Definition
PassiveTorqueExponential.h:133
BIORBD_NAMESPACE::internal_forces::passive_torques::PassiveTorqueExponential::m_qMid
std::shared_ptr< utils::Scalar > m_qMid
Midpoint angle.
Definition
PassiveTorqueExponential.h:136
BIORBD_NAMESPACE::internal_forces::passive_torques::PassiveTorqueExponential::setType
virtual void setType()
Set the type of passive torque.
Definition
PassiveTorqueExponential.cpp:140
BIORBD_NAMESPACE::internal_forces::passive_torques::PassiveTorqueExponential::m_sV
std::shared_ptr< utils::Scalar > m_sV
Velocity scaling factor.
Definition
PassiveTorqueExponential.h:140
BIORBD_NAMESPACE::internal_forces::passive_torques::PassiveTorqueExponential::m_b1
std::shared_ptr< utils::Scalar > m_b1
Scaling of the 1st exponential.
Definition
PassiveTorqueExponential.h:134
BIORBD_NAMESPACE::internal_forces::passive_torques::PassiveTorqueExponential::m_b2
std::shared_ptr< utils::Scalar > m_b2
Scaling of the 2nd exponential.
Definition
PassiveTorqueExponential.h:135
BIORBD_NAMESPACE::internal_forces::passive_torques::PassiveTorqueExponential::DeepCopy
PassiveTorqueExponential DeepCopy() const
Deep copy of the linear passive torque.
Definition
PassiveTorqueExponential.cpp:101
BIORBD_NAMESPACE::internal_forces::passive_torques::PassiveTorqueExponential::m_deltaP
std::shared_ptr< utils::Scalar > m_deltaP
Angle offset.
Definition
PassiveTorqueExponential.h:141
BIORBD_NAMESPACE::internal_forces::passive_torques::PassiveTorqueExponential::m_k1
std::shared_ptr< utils::Scalar > m_k1
Stiffness of the 1st exponential.
Definition
PassiveTorqueExponential.h:132
BIORBD_NAMESPACE::internal_forces::passive_torques::PassiveTorqueExponential::m_tauEq
std::shared_ptr< utils::Scalar > m_tauEq
Torque offset.
Definition
PassiveTorqueExponential.h:137
BIORBD_NAMESPACE::internal_forces::passive_torques::PassiveTorqueExponential::m_pBeta
std::shared_ptr< utils::Scalar > m_pBeta
Velocity damping factor.
Definition
PassiveTorqueExponential.h:138
BIORBD_NAMESPACE::internal_forces::passive_torques::PassiveTorqueExponential::m_wMax
std::shared_ptr< utils::Scalar > m_wMax
Maximal angular velocity.
Definition
PassiveTorqueExponential.h:139
BIORBD_NAMESPACE::internal_forces::passive_torques::PassiveTorqueExponential::PassiveTorqueExponential
PassiveTorqueExponential()
Construct a linear passive torque.
Definition
PassiveTorqueExponential.cpp:10
BIORBD_NAMESPACE::internal_forces::passive_torques::PassiveTorque::DeepCopy
void DeepCopy(const PassiveTorque &other)
Deep copy of passive torque.
Definition
PassiveTorque.cpp:44
BIORBD_NAMESPACE::internal_forces::passive_torques::PassiveTorque::m_type
std::shared_ptr< TORQUE_TYPE > m_type
The type of the passive torque.
Definition
PassiveTorque.h:88
BIORBD_NAMESPACE::internal_forces::passive_torques::PassiveTorque::m_dofIdx
std::shared_ptr< size_t > m_dofIdx
Index of the DoF associated with the passive torque.
Definition
PassiveTorque.h:91
BIORBD_NAMESPACE::internal_forces::passive_torques::PassiveTorque::PassiveTorque
PassiveTorque()
Construct passive torque.
Definition
PassiveTorque.cpp:8
BIORBD_NAMESPACE::rigidbody::GeneralizedCoordinates
Class GeneralizedCoordinates.
Definition
GeneralizedCoordinates.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
PassiveTorques
PassiveTorqueExponential.cpp
Generated by
1.18.0