Biorbd Release_1.13.0
Loading...
Searching...
No Matches
Actuators.cpp
1#define BIORBD_API_EXPORTS
2#include "InternalForces/Actuators/Actuators.h"
3
4#include <vector>
5
6#include "InternalForces/Actuators/Actuator.h"
7#include "InternalForces/Actuators/ActuatorConstant.h"
8#include "InternalForces/Actuators/ActuatorGauss3p.h"
9#include "InternalForces/Actuators/ActuatorGauss6p.h"
10#include "InternalForces/Actuators/ActuatorLinear.h"
11#include "InternalForces/Actuators/ActuatorSigmoidGauss3p.h"
12#include "RigidBody/GeneralizedCoordinates.h"
13#include "RigidBody/GeneralizedTorque.h"
14#include "RigidBody/GeneralizedVelocity.h"
15#include "RigidBody/Joints.h"
16#include "Utils/Error.h"
17
18#ifdef USE_SMOOTH_IF_ELSE
19#include "Utils/CasadiExpand.h"
20#endif
21
22using namespace BIORBD_NAMESPACE;
23
25 : m_all(
26 std::make_shared<std::vector<std::pair<
27 std::shared_ptr<internal_forces::actuator::Actuator>,
28 std::shared_ptr<internal_forces::actuator::Actuator>>>>()),
29 m_isDofSet(std::make_shared<std::vector<bool>>(1)),
30 m_isClose(std::make_shared<bool>(false)) {
31 (*m_isDofSet)[0] = false;
32}
33
39
41
44 m_all->resize(other.m_all->size());
45 for (size_t i = 0; i < other.m_all->size(); ++i) {
46 if ((*other.m_all)[i].first->type() ==
47 internal_forces::actuator::TYPE::CONSTANT)
48 (*m_all)[i].first =
49 std::make_shared<internal_forces::actuator::ActuatorConstant>(
51 *(*other.m_all)[i].first));
52 else if (
53 (*other.m_all)[i].first->type() ==
54 internal_forces::actuator::TYPE::LINEAR)
55 (*m_all)[i].first =
56 std::make_shared<internal_forces::actuator::ActuatorLinear>(
58 *(*other.m_all)[i].first));
59 else if (
60 (*other.m_all)[i].first->type() ==
61 internal_forces::actuator::TYPE::GAUSS3P)
62 (*m_all)[i].first =
63 std::make_shared<internal_forces::actuator::ActuatorGauss3p>(
65 *(*other.m_all)[i].first));
66 else if (
67 (*other.m_all)[i].first->type() ==
68 internal_forces::actuator::TYPE::GAUSS6P)
69 (*m_all)[i].first =
70 std::make_shared<internal_forces::actuator::ActuatorGauss6p>(
72 *(*other.m_all)[i].first));
73 else if (
74 (*other.m_all)[i].first->type() ==
75 internal_forces::actuator::TYPE::SIGMOIDGAUSS3P)
76 (*m_all)[i].first =
77 std::make_shared<internal_forces::actuator::ActuatorSigmoidGauss3p>(
78 static_cast<
80 *(*other.m_all)[i].first));
81 else
83 "Actuator " +
85 internal_forces::actuator::TYPE_toStr(
86 (*other.m_all)[i].first->type())) +
87 " in DeepCopy");
88 if ((*other.m_all)[i].second->type() ==
89 internal_forces::actuator::TYPE::CONSTANT)
90 (*m_all)[i].second =
91 std::make_shared<internal_forces::actuator::ActuatorConstant>(
93 *(*other.m_all)[i].second));
94 else if (
95 (*other.m_all)[i].second->type() ==
96 internal_forces::actuator::TYPE::LINEAR)
97 (*m_all)[i].second =
98 std::make_shared<internal_forces::actuator::ActuatorLinear>(
100 *(*other.m_all)[i].second));
101 else if (
102 (*other.m_all)[i].second->type() ==
103 internal_forces::actuator::TYPE::GAUSS3P)
104 (*m_all)[i].second =
105 std::make_shared<internal_forces::actuator::ActuatorGauss3p>(
107 *(*other.m_all)[i].second));
108 else if (
109 (*other.m_all)[i].second->type() ==
110 internal_forces::actuator::TYPE::GAUSS6P)
111 (*m_all)[i].second =
112 std::make_shared<internal_forces::actuator::ActuatorGauss6p>(
114 *(*other.m_all)[i].second));
115 else if (
116 (*other.m_all)[i].second->type() ==
117 internal_forces::actuator::TYPE::SIGMOIDGAUSS3P)
118 (*m_all)[i].second =
119 std::make_shared<internal_forces::actuator::ActuatorSigmoidGauss3p>(
120 static_cast<
122 *(*other.m_all)[i].second));
123 else
125 "Actuator " +
127 internal_forces::actuator::TYPE_toStr(
128 (*other.m_all)[i].second->type())) +
129 " in DeepCopy");
130 }
131 m_isDofSet->resize(other.m_isDofSet->size());
132 for (size_t i = 0; i < other.m_isDofSet->size(); ++i) {
133 (*m_isDofSet)[i] = (*other.m_isDofSet)[i];
134 }
135 *m_isClose = *other.m_isClose;
136}
137
141 !*m_isClose, "You can't add actuator after closing the model");
142
143 // Assuming that this is also a Joints type (via BiorbdModel)
144 const rigidbody::Joints &model = dynamic_cast<rigidbody::Joints &>(*this);
145
146 // Verify that the target dof is associated to a dof that
147 // already exists in the model
149 act.index() < model.nbDof(), "Sent index is out of dof range");
150
151 // For speed purposes and coherence with the Q,
152 // set the actuator to the same index as its associated dof
153 size_t idx(act.index());
154
155 // If there are less actuators declared than dof,
156 // the vector must be enlarged
157 if (idx >= m_all->size()) {
158 m_all->resize(idx + 1);
159 m_isDofSet->resize((idx + 1) * 2, false);
160 }
161
162 // Add an actuator to the pool of actuators according to its type
163 if (act.type() == internal_forces::actuator::TYPE::CONSTANT) {
164 if (act.direction() == 1) {
165 (*m_all)[idx].first =
166 std::make_shared<internal_forces::actuator::ActuatorConstant>(
168 act));
169 (*m_isDofSet)[idx * 2] = true;
170 } else {
171 (*m_all)[idx].second =
172 std::make_shared<internal_forces::actuator::ActuatorConstant>(
174 act));
175 (*m_isDofSet)[idx * 2 + 1] = true;
176 }
177 return;
178 } else if (act.type() == internal_forces::actuator::TYPE::LINEAR) {
179 if (act.direction() == 1) {
180 (*m_all)[idx].first =
181 std::make_shared<internal_forces::actuator::ActuatorLinear>(
183 act));
184 (*m_isDofSet)[idx * 2] = true;
185 } else {
186 (*m_all)[idx].second =
187 std::make_shared<internal_forces::actuator::ActuatorLinear>(
189 act));
190 (*m_isDofSet)[idx * 2 + 1] = true;
191 }
192 return;
193 } else if (act.type() == internal_forces::actuator::TYPE::GAUSS3P) {
194 if (act.direction() == 1) {
195 (*m_all)[idx].first =
196 std::make_shared<internal_forces::actuator::ActuatorGauss3p>(
198 act));
199 (*m_isDofSet)[idx * 2] = true;
200 } else {
201 (*m_all)[idx].second =
202 std::make_shared<internal_forces::actuator::ActuatorGauss3p>(
204 act));
205 (*m_isDofSet)[idx * 2 + 1] = true;
206 }
207 return;
208 } else if (act.type() == internal_forces::actuator::TYPE::GAUSS6P) {
209 if (act.direction() == 1) {
210 (*m_all)[idx].first =
211 std::make_shared<internal_forces::actuator::ActuatorGauss6p>(
213 act));
214 (*m_isDofSet)[idx * 2] = true;
215 } else {
216 (*m_all)[idx].second =
217 std::make_shared<internal_forces::actuator::ActuatorGauss6p>(
219 act));
220 (*m_isDofSet)[idx * 2 + 1] = true;
221 }
222 return;
223 } else if (act.type() == internal_forces::actuator::TYPE::SIGMOIDGAUSS3P) {
224 if (act.direction() == 1) {
225 (*m_all)[idx].first =
226 std::make_shared<internal_forces::actuator::ActuatorSigmoidGauss3p>(
227 static_cast<
229 act));
230 (*m_isDofSet)[idx * 2] = true;
231 } else {
232 (*m_all)[idx].second =
233 std::make_shared<internal_forces::actuator::ActuatorSigmoidGauss3p>(
234 static_cast<
236 act));
237 (*m_isDofSet)[idx * 2 + 1] = true;
238 }
239 return;
240 } else {
241 utils::Error::raise("Actuator type not found");
242 }
243}
244
246 // Assuming that this is also a Joints type (via BiorbdModel)
247 const rigidbody::Joints &model = dynamic_cast<rigidbody::Joints &>(*this);
248
250 model.nbDof() == m_all->size(), "All dof must have their actuators set");
251
252 for (size_t i = 0; i < m_all->size() * 2; ++i)
254 (*m_isDofSet)[i],
255 "All DoF must have their actuators set "
256 "before closing the model");
257
258 *m_isClose = true;
259}
260
261const std::pair<
262 std::shared_ptr<internal_forces::actuator::Actuator>,
263 std::shared_ptr<internal_forces::actuator::Actuator>> &
266 dof < nbActuators(), "Idx asked is higher than number of actuator");
267 return (*m_all)[dof];
268}
271 const std::pair<
272 std::shared_ptr<internal_forces::actuator::Actuator>,
273 std::shared_ptr<internal_forces::actuator::Actuator>> &tp(actuator(dof));
274
275 if (concentric) {
276 return *tp.first;
277 } else {
278 return *tp.second;
279 }
280}
281
283 return m_all->size();
284}
285
287 const utils::Vector &activation,
289 const rigidbody::GeneralizedVelocity &Qdot) {
290 // Calculate the maximal torques
291 rigidbody::GeneralizedTorque GeneralizedTorque(
292 torqueMax(activation, Q, Qdot));
293
294 // Put the signs
295 for (unsigned int i = 0; i < GeneralizedTorque.size(); ++i) {
296 GeneralizedTorque(i) = GeneralizedTorque(i) * activation(i);
297 }
298
299 return GeneralizedTorque;
300}
301
302std::pair<rigidbody::GeneralizedTorque, rigidbody::GeneralizedTorque>
305 const rigidbody::GeneralizedVelocity &Qdot) {
307 *m_isClose, "Close the actuator model before calling torqueMax");
308
309 // Assuming that this is also a Joints type (via BiorbdModel)
310 const rigidbody::Joints &model = dynamic_cast<rigidbody::Joints &>(*this);
311
312 std::pair<rigidbody::GeneralizedTorque, rigidbody::GeneralizedTorque>
313 maxGeneralizedTorque_all = std::make_pair(
316
317 for (unsigned int i = 0; i < static_cast<unsigned int>(model.nbDof()); ++i) {
318 std::pair<std::shared_ptr<Actuator>, std::shared_ptr<Actuator>>
319 GeneralizedTorque_tp(actuator(i));
320 for (unsigned p = 0; p < 2; ++p) {
321 if (p == 0) // First
322 if (std::dynamic_pointer_cast<ActuatorGauss3p>(
323 GeneralizedTorque_tp.first)) {
324 maxGeneralizedTorque_all.first[i] =
325 std::static_pointer_cast<ActuatorGauss3p>(
326 GeneralizedTorque_tp.first)
327 ->torqueMax(Q, Qdot);
328 } else if (std::dynamic_pointer_cast<ActuatorConstant>(
329 GeneralizedTorque_tp.first)) {
330 maxGeneralizedTorque_all.first[i] =
331 std::static_pointer_cast<ActuatorConstant>(
332 GeneralizedTorque_tp.first)
333 ->torqueMax();
334 } else if (std::dynamic_pointer_cast<ActuatorLinear>(
335 GeneralizedTorque_tp.first)) {
336 maxGeneralizedTorque_all.first[i] =
337 std::static_pointer_cast<ActuatorLinear>(
338 GeneralizedTorque_tp.first)
339 ->torqueMax(Q);
340 } else if (std::dynamic_pointer_cast<ActuatorGauss6p>(
341 GeneralizedTorque_tp.first)) {
342 maxGeneralizedTorque_all.first[i] =
343 std::static_pointer_cast<ActuatorGauss6p>(
344 GeneralizedTorque_tp.first)
345 ->torqueMax(Q, Qdot);
346 } else if (std::dynamic_pointer_cast<ActuatorSigmoidGauss3p>(
347 GeneralizedTorque_tp.first)) {
348 maxGeneralizedTorque_all.first[i] =
349 std::static_pointer_cast<ActuatorSigmoidGauss3p>(
350 GeneralizedTorque_tp.first)
351 ->torqueMax(Q, Qdot);
352 } else {
354 "Wrong type (should never get here because of previous safety)");
355 }
356 else // Second
357 if (std::dynamic_pointer_cast<ActuatorGauss3p>(
358 GeneralizedTorque_tp.second)) {
359 maxGeneralizedTorque_all.second[i] =
360 std::static_pointer_cast<ActuatorGauss3p>(
361 GeneralizedTorque_tp.second)
362 ->torqueMax(Q, Qdot);
363 } else if (std::dynamic_pointer_cast<ActuatorConstant>(
364 GeneralizedTorque_tp.second)) {
365 maxGeneralizedTorque_all.second[i] =
366 std::static_pointer_cast<ActuatorConstant>(
367 GeneralizedTorque_tp.second)
368 ->torqueMax();
369 } else if (std::dynamic_pointer_cast<ActuatorLinear>(
370 GeneralizedTorque_tp.second)) {
371 maxGeneralizedTorque_all.second[i] =
372 std::static_pointer_cast<ActuatorLinear>(
373 GeneralizedTorque_tp.second)
374 ->torqueMax(Q);
375 } else if (std::dynamic_pointer_cast<ActuatorGauss6p>(
376 GeneralizedTorque_tp.second)) {
377 maxGeneralizedTorque_all.second[i] =
378 std::static_pointer_cast<ActuatorGauss6p>(
379 GeneralizedTorque_tp.second)
380 ->torqueMax(Q, Qdot);
381 } else if (std::dynamic_pointer_cast<ActuatorSigmoidGauss3p>(
382 GeneralizedTorque_tp.second)) {
383 maxGeneralizedTorque_all.second[i] =
384 std::static_pointer_cast<ActuatorSigmoidGauss3p>(
385 GeneralizedTorque_tp.second)
386 ->torqueMax(Q, Qdot);
387 } else {
389 "Wrong type (should never get here because of previous safety)");
390 }
391 }
392 }
393
394 return maxGeneralizedTorque_all;
395}
396
398 const utils::Vector &activation,
400 const rigidbody::GeneralizedVelocity &Qdot) {
402 *m_isClose, "Close the actuator model before calling torqueMax");
403
404 // Assuming that this is also a Joints type (via BiorbdModel)
405 const rigidbody::Joints &model = dynamic_cast<rigidbody::Joints &>(*this);
406
407 // Set qdot to be positive if concentric and negative if excentric
408 rigidbody::GeneralizedVelocity QdotResigned(Qdot);
409 for (unsigned int i = 0; i < static_cast<unsigned int>(Qdot.size()); ++i) {
410#ifdef BIORBD_USE_CASADI_MATH
411 QdotResigned(i) = IF_ELSE_NAMESPACE::if_else(
412 IF_ELSE_NAMESPACE::lt(activation(i), 0), -Qdot(i), Qdot(i));
413#else
414 if (activation(i) < 0) {
415 QdotResigned(i) = -Qdot(i);
416 }
417#endif
418 }
419
420 rigidbody::GeneralizedTorque maxGeneralizedTorque_all(model);
421
422 for (unsigned int i = 0; i < static_cast<unsigned int>(model.nbDof()); ++i) {
423#ifdef BIORBD_USE_CASADI_MATH
424 maxGeneralizedTorque_all[i] = IF_ELSE_NAMESPACE::if_else(
425 IF_ELSE_NAMESPACE::ge(activation(i, 0), 0),
426 getTorqueMaxDirection(actuator(i).first, Q, QdotResigned),
427 getTorqueMaxDirection(actuator(i).second, Q, QdotResigned));
428#else
429 if (activation[i] >= 0) { // First
430 maxGeneralizedTorque_all[i] =
431 getTorqueMaxDirection(actuator(i).first, Q, QdotResigned);
432 } else {
433 maxGeneralizedTorque_all[i] =
434 getTorqueMaxDirection(actuator(i).second, Q, QdotResigned);
435 }
436#endif
437 }
438
439 return maxGeneralizedTorque_all;
440}
441
443 const std::shared_ptr<internal_forces::actuator::Actuator> actuator,
445 const rigidbody::GeneralizedVelocity &Qdot) const {
446 if (std::dynamic_pointer_cast<ActuatorGauss3p>(actuator)) {
447 return std::static_pointer_cast<ActuatorGauss3p>(actuator)->torqueMax(
448 Q, Qdot);
449 } else if (std::dynamic_pointer_cast<ActuatorConstant>(actuator)) {
450 return std::static_pointer_cast<ActuatorConstant>(actuator)->torqueMax();
451 } else if (std::dynamic_pointer_cast<ActuatorLinear>(actuator)) {
452 return std::static_pointer_cast<ActuatorLinear>(actuator)->torqueMax(Q);
453 } else if (std::dynamic_pointer_cast<ActuatorGauss6p>(actuator)) {
454 return std::static_pointer_cast<ActuatorGauss6p>(actuator)->torqueMax(
455 Q, Qdot);
456 } else if (std::dynamic_pointer_cast<ActuatorSigmoidGauss3p>(actuator)) {
457 return std::static_pointer_cast<ActuatorSigmoidGauss3p>(actuator)
458 ->torqueMax(Q, Qdot);
459 } else {
461 "Wrong type (should never get here because of previous safety)");
462 }
463}
Class ActuatorConstant is a joint actuator type which maximum is contant.
Class ActuatorGauss3p is a joint actuator type which maximum is 3 parameter gaussian (Gauss3p) Please...
Class ActuatorGauss6p is a joint actuator type which maximum is bimodal 6 parameter gaussian (Gauss6p...
Class ActuatorLinear is a joint actuator type that linearly evolves.
Class ActuatorSigmoidGauss3p is a joint actuator type which maximum is 3 parameter gaussian (Gauss3p)...
TYPE type() const
Return the type of the actuator.
Definition Actuator.cpp:62
int direction() const
Return the direction of the actuator.
Definition Actuator.cpp:58
size_t index() const
Return the index of the DoF associated with actuator.
Definition Actuator.cpp:56
Class holder for a set of actuators.
Definition Actuators.h:28
size_t nbActuators() const
Return the toal number of actuators.
virtual ~Actuators()
Destroy actuators class properly.
Definition Actuators.cpp:40
std::pair< rigidbody::GeneralizedTorque, rigidbody::GeneralizedTorque > torqueMax(const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot)
Return two vectors of max torque (it is impossible to know if eccentric or concentric is required,...
rigidbody::GeneralizedTorque torque(const utils::Vector &activation, const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot)
Return the generalized torque.
const std::pair< std::shared_ptr< Actuator >, std::shared_ptr< Actuator > > & actuator(size_t dof)
Return a specific concentric/eccentric actuator.
utils::Scalar getTorqueMaxDirection(const std::shared_ptr< Actuator > actuator, const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot) const
getTorqueMaxDirection Get the max torque of a specific actuator (interface necessary because of CasAD...
std::shared_ptr< bool > m_isClose
If the set is ready.
Definition Actuators.h:133
void DeepCopy(const Actuators &other)
Deep copy of the actuator holder from other actuator holder.
Definition Actuators.cpp:42
void closeActuator()
Indicate to biorbd to are done adding actuators, sanity checks are performed.
std::shared_ptr< std::vector< bool > > m_isDofSet
If DoF all dof are set.
Definition Actuators.h:132
void addActuator(const Actuator &a)
Add an actuator to the set of actuators.
std::shared_ptr< std::vector< std::pair< std::shared_ptr< Actuator >, std::shared_ptr< Actuator > > > > m_all
All the actuators reunited /pair (+ or -).
Definition Actuators.h:131
This is the core of the musculoskeletal model in biorbd.
Definition Joints.h:48
size_t nbDof() const
Return the number of degrees of freedom (DoF).
Definition Joints.cpp:87
static void raise(const String &message)
Throw an error message.
Definition Error.cpp:6
static void check(bool cond, const String &message)
Assert that raises the error message if false.
Definition Error.cpp:10
Wrapper around the std::string class with augmented functionality.
Definition String.h:18
Wrapper of the Eigen VectorXd.
Definition Vector.h:21