Biorbd
Release_1.13.0
Toggle main menu visibility
Loading...
Searching...
No Matches
PassiveTorques.cpp
1
#define BIORBD_API_EXPORTS
2
3
#include "InternalForces/PassiveTorques/PassiveTorques.h"
4
5
#include <vector>
6
7
#include "InternalForces/PassiveTorques/PassiveTorque.h"
8
#include "InternalForces/PassiveTorques/PassiveTorqueConstant.h"
9
#include "InternalForces/PassiveTorques/PassiveTorqueExponential.h"
10
#include "InternalForces/PassiveTorques/PassiveTorqueLinear.h"
11
#include "RigidBody/GeneralizedCoordinates.h"
12
#include "RigidBody/GeneralizedTorque.h"
13
#include "RigidBody/GeneralizedVelocity.h"
14
#include "RigidBody/Joints.h"
15
#include "Utils/Error.h"
16
17
using namespace
BIORBD_NAMESPACE;
18
19
internal_forces::passive_torques::PassiveTorques::PassiveTorques
()
20
:
m_pas
(
21
std::make_shared<std::vector<std::shared_ptr<
22
internal_forces::passive_torques::
PassiveTorque
>>>()),
23
m_isDofSet
(std::make_shared<std::vector<bool>>(true)) {
24
(*m_isDofSet)[0] =
false
;
25
}
26
27
internal_forces::passive_torques::PassiveTorques::PassiveTorques
(
28
const
internal_forces::passive_torques::PassiveTorques
&other)
29
:
m_pas
(other.
m_pas
),
m_isDofSet
(other.
m_isDofSet
) {}
30
31
internal_forces::passive_torques::PassiveTorques::~PassiveTorques
() {}
32
33
void
internal_forces::passive_torques::PassiveTorques::DeepCopy
(
34
const
internal_forces::passive_torques::PassiveTorques
&other) {
35
m_pas
->resize(other.
m_pas
->size());
36
for
(
size_t
i = 0; i < other.
m_pas
->size(); ++i) {
37
if
((*other.
m_pas
)[i]->type() ==
38
internal_forces::passive_torques::TORQUE_TYPE::TORQUE_CONSTANT) {
39
(*m_pas)[i] = std::make_shared<
40
internal_forces::passive_torques::PassiveTorqueConstant
>(
41
static_cast<
42
const
internal_forces::passive_torques::PassiveTorqueConstant
&
>
(
43
*(*other.
m_pas
)[i]));
44
return
;
45
}
else
if
(
46
(*other.
m_pas
)[i]->type() ==
47
internal_forces::passive_torques::TORQUE_TYPE::TORQUE_LINEAR) {
48
(*m_pas)[i] = std::make_shared<
49
internal_forces::passive_torques::PassiveTorqueLinear
>(
50
static_cast<
51
const
internal_forces::passive_torques::PassiveTorqueLinear
&
>
(
52
*(*other.
m_pas
)[i]));
53
return
;
54
}
else
if
(
55
(*other.
m_pas
)[i]->type() ==
56
internal_forces::passive_torques::TORQUE_TYPE::TORQUE_EXPONENTIAL) {
57
(*m_pas)[i] = std::make_shared<
58
internal_forces::passive_torques::PassiveTorqueExponential
>(
59
static_cast<
const
internal_forces::passive_torques::
60
PassiveTorqueExponential
&
>
(*(*other.
m_pas
)[i]));
61
return
;
62
}
63
}
64
*
m_isDofSet
= *other.
m_isDofSet
;
65
}
66
67
void
internal_forces::passive_torques::PassiveTorques::addPassiveTorque
(
68
const
internal_forces::passive_torques::PassiveTorque
&other) {
69
// Assuming that this is also a Joints type (via BiorbdModel)
70
const
rigidbody::Joints
&model =
dynamic_cast<
rigidbody::Joints
&
>
(*this);
71
72
// Verify that the target dof is associated to a dof that already exists in
73
// the model
74
utils::Error::check
(
75
other.
index
() < model.
nbDof
(),
"Sent index is out of dof range"
);
76
77
// For speed purposes and coherence with the Q, set the passive torque to the
78
// same index as its associated dof
79
size_t
idx(other.
index
());
80
81
// If there are less actuators declared than dof, the vector must be enlarged
82
if
(idx >=
m_pas
->size()) {
83
m_pas
->resize(idx + 1);
84
m_isDofSet
->resize(idx + 1,
false
);
85
}
86
// Add a passive torque to the pool of passive torques according to its type
87
if
(other.
type
() ==
88
internal_forces::passive_torques::TORQUE_TYPE::TORQUE_CONSTANT) {
89
(*m_pas)[idx] = std::make_shared<
90
internal_forces::passive_torques::PassiveTorqueConstant
>(
91
static_cast<
92
const
internal_forces::passive_torques::PassiveTorqueConstant
&
>
(
93
other));
94
(*m_isDofSet)[idx] =
true
;
95
return
;
96
}
else
if
(
97
other.
type
() ==
98
internal_forces::passive_torques::TORQUE_TYPE::TORQUE_LINEAR) {
99
(*m_pas)[idx] =
100
std::make_shared<internal_forces::passive_torques::PassiveTorqueLinear>(
101
static_cast<
102
const
internal_forces::passive_torques::PassiveTorqueLinear
&
>
(
103
other));
104
(*m_isDofSet)[idx] =
true
;
105
return
;
106
}
else
if
(
107
other.
type
() ==
108
internal_forces::passive_torques::TORQUE_TYPE::TORQUE_EXPONENTIAL) {
109
(*m_pas)[idx] = std::make_shared<
110
internal_forces::passive_torques::PassiveTorqueExponential
>(
111
static_cast<
112
const
internal_forces::passive_torques::PassiveTorqueExponential
&
>
(
113
other));
114
(*m_isDofSet)[idx] =
true
;
115
return
;
116
}
else
{
117
utils::Error::raise
(
"Passive Torque type not found"
);
118
}
119
return
;
120
}
121
122
const
std::shared_ptr<internal_forces::passive_torques::PassiveTorque> &
123
internal_forces::passive_torques::PassiveTorques::getPassiveTorque
(
size_t
dof) {
124
utils::Error::check
(
125
dof <
nbPassiveTorques
(),
126
"Idx asked is higher than number of passive torque"
);
127
return
(*
m_pas
)[dof];
128
}
129
130
size_t
internal_forces::passive_torques::PassiveTorques::nbPassiveTorques
()
131
const
{
132
return
m_pas
->size();
133
}
134
135
rigidbody::GeneralizedTorque
136
internal_forces::passive_torques::PassiveTorques::passiveJointTorque
(
137
const
rigidbody::GeneralizedCoordinates
&Q,
138
const
rigidbody::GeneralizedVelocity
&Qdot) {
139
// Assuming that this is also a Joints type (via BiorbdModel)
140
const
rigidbody::Joints
&model =
dynamic_cast<
rigidbody::Joints
&
>
(*this);
141
rigidbody::GeneralizedTorque
GeneralizedTorque_all =
142
rigidbody::GeneralizedTorque
(model);
143
144
// If there are less actuators declared than dof, the vector must be enlarged
145
if
(model.
nbDof
() >=
m_pas
->size()) {
146
m_pas
->resize(model.
nbDof
() + 1);
147
m_isDofSet
->resize(model.
nbDof
() + 1,
false
);
148
}
149
150
for
(
unsigned
int
i = 0; i < model.
nbDof
(); ++i) {
151
if
(!(*
m_isDofSet
)[i]) {
152
GeneralizedTorque_all[i] = 0;
153
}
else
if
(std::dynamic_pointer_cast<PassiveTorqueConstant>((*
m_pas
)[i])) {
154
GeneralizedTorque_all[i] =
155
std::static_pointer_cast<PassiveTorqueConstant>((*
m_pas
)[i])
156
->passiveTorque();
157
}
else
if
(std::dynamic_pointer_cast<PassiveTorqueLinear>((*
m_pas
)[i])) {
158
GeneralizedTorque_all[i] =
159
std::static_pointer_cast<PassiveTorqueLinear>((*
m_pas
)[i])
160
->passiveTorque(Q);
161
}
else
if
(std::dynamic_pointer_cast<PassiveTorqueExponential>(
162
(*
m_pas
)[i])) {
163
GeneralizedTorque_all[i] =
164
std::static_pointer_cast<PassiveTorqueExponential>((*
m_pas
)[i])
165
->passiveTorque(Q, Qdot);
166
}
else
{
167
utils::Error::raise
(
168
"Wrong type (should never get here because of previous safety)"
);
169
}
170
}
171
return
GeneralizedTorque_all;
172
}
BIORBD_NAMESPACE::internal_forces::passive_torques::PassiveTorqueConstant
Class PassiveTorqueConstant is a joint passive torque type which maximum is contant.
Definition
PassiveTorqueConstant.h:16
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::PassiveTorqueLinear
Class PassiveTorqueLinear is a joint passive torque type that linearly evolves Linear passive torque ...
Definition
PassiveTorqueLinear.h:19
BIORBD_NAMESPACE::internal_forces::passive_torques::PassiveTorque
Class PassiveTorque.
Definition
PassiveTorque.h:21
BIORBD_NAMESPACE::internal_forces::passive_torques::PassiveTorque::index
size_t index() const
Return the index of the DoF associated with passive torque.
Definition
PassiveTorque.cpp:51
BIORBD_NAMESPACE::internal_forces::passive_torques::PassiveTorque::type
TORQUE_TYPE type() const
Return the type of the passive torque.
Definition
PassiveTorque.cpp:56
BIORBD_NAMESPACE::internal_forces::passive_torques::PassiveTorques
Class holder for a set of passive torques.
Definition
PassiveTorques.h:28
BIORBD_NAMESPACE::internal_forces::passive_torques::PassiveTorques::PassiveTorques
PassiveTorques()
Construct passive torques.
Definition
PassiveTorques.cpp:19
BIORBD_NAMESPACE::internal_forces::passive_torques::PassiveTorques::m_isDofSet
std::shared_ptr< std::vector< bool > > m_isDofSet
If DoF all dof are set.
Definition
PassiveTorques.h:87
BIORBD_NAMESPACE::internal_forces::passive_torques::PassiveTorques::nbPassiveTorques
size_t nbPassiveTorques() const
Return the toal number of passive torques.
Definition
PassiveTorques.cpp:130
BIORBD_NAMESPACE::internal_forces::passive_torques::PassiveTorques::getPassiveTorque
const std::shared_ptr< PassiveTorque > & getPassiveTorque(size_t dof)
Return a specific passive torque.
Definition
PassiveTorques.cpp:123
BIORBD_NAMESPACE::internal_forces::passive_torques::PassiveTorques::~PassiveTorques
virtual ~PassiveTorques()
Destroy passive torques class properly.
Definition
PassiveTorques.cpp:31
BIORBD_NAMESPACE::internal_forces::passive_torques::PassiveTorques::m_pas
std::shared_ptr< std::vector< std::shared_ptr< internal_forces::passive_torques::PassiveTorque > > > m_pas
Passive torque to add.
Definition
PassiveTorques.h:86
BIORBD_NAMESPACE::internal_forces::passive_torques::PassiveTorques::addPassiveTorque
void addPassiveTorque(const PassiveTorque &other)
Add a passive torque to the set of passive torques.
Definition
PassiveTorques.cpp:67
BIORBD_NAMESPACE::internal_forces::passive_torques::PassiveTorques::DeepCopy
void DeepCopy(const PassiveTorques &other)
Deep copy of the passive torque holder from other passive torque holder.
Definition
PassiveTorques.cpp:33
BIORBD_NAMESPACE::internal_forces::passive_torques::PassiveTorques::passiveJointTorque
rigidbody::GeneralizedTorque passiveJointTorque(const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot)
Return the passiveJointTorques.
Definition
PassiveTorques.cpp:136
BIORBD_NAMESPACE::rigidbody::GeneralizedCoordinates
Class GeneralizedCoordinates.
Definition
GeneralizedCoordinates.h:15
BIORBD_NAMESPACE::rigidbody::GeneralizedTorque
Class GeneralizedTorque.
Definition
GeneralizedTorque.h:15
BIORBD_NAMESPACE::rigidbody::GeneralizedVelocity
Class GeneralizedVelocity.
Definition
GeneralizedVelocity.h:15
BIORBD_NAMESPACE::rigidbody::Joints
This is the core of the musculoskeletal model in biorbd.
Definition
Joints.h:48
BIORBD_NAMESPACE::rigidbody::Joints::nbDof
size_t nbDof() const
Return the number of degrees of freedom (DoF).
Definition
Joints.cpp:87
BIORBD_NAMESPACE::utils::Error::raise
static void raise(const String &message)
Throw an error message.
Definition
Error.cpp:6
BIORBD_NAMESPACE::utils::Error::check
static void check(bool cond, const String &message)
Assert that raises the error message if false.
Definition
Error.cpp:10
src
InternalForces
PassiveTorques
PassiveTorques.cpp
Generated by
1.18.0