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)
49 std::make_shared<internal_forces::actuator::ActuatorConstant>(
51 *(*other.
m_all)[i].first));
53 (*other.
m_all)[i].first->type() ==
54 internal_forces::actuator::TYPE::LINEAR)
56 std::make_shared<internal_forces::actuator::ActuatorLinear>(
58 *(*other.
m_all)[i].first));
60 (*other.
m_all)[i].first->type() ==
61 internal_forces::actuator::TYPE::GAUSS3P)
63 std::make_shared<internal_forces::actuator::ActuatorGauss3p>(
65 *(*other.
m_all)[i].first));
67 (*other.
m_all)[i].first->type() ==
68 internal_forces::actuator::TYPE::GAUSS6P)
70 std::make_shared<internal_forces::actuator::ActuatorGauss6p>(
72 *(*other.
m_all)[i].first));
74 (*other.
m_all)[i].first->type() ==
75 internal_forces::actuator::TYPE::SIGMOIDGAUSS3P)
77 std::make_shared<internal_forces::actuator::ActuatorSigmoidGauss3p>(
80 *(*other.
m_all)[i].first));
85 internal_forces::actuator::TYPE_toStr(
86 (*other.
m_all)[i].first->type())) +
88 if ((*other.
m_all)[i].second->type() ==
89 internal_forces::actuator::TYPE::CONSTANT)
91 std::make_shared<internal_forces::actuator::ActuatorConstant>(
93 *(*other.
m_all)[i].second));
95 (*other.
m_all)[i].second->type() ==
96 internal_forces::actuator::TYPE::LINEAR)
98 std::make_shared<internal_forces::actuator::ActuatorLinear>(
100 *(*other.
m_all)[i].second));
102 (*other.
m_all)[i].second->type() ==
103 internal_forces::actuator::TYPE::GAUSS3P)
105 std::make_shared<internal_forces::actuator::ActuatorGauss3p>(
107 *(*other.
m_all)[i].second));
109 (*other.
m_all)[i].second->type() ==
110 internal_forces::actuator::TYPE::GAUSS6P)
112 std::make_shared<internal_forces::actuator::ActuatorGauss6p>(
114 *(*other.
m_all)[i].second));
116 (*other.
m_all)[i].second->type() ==
117 internal_forces::actuator::TYPE::SIGMOIDGAUSS3P)
119 std::make_shared<internal_forces::actuator::ActuatorSigmoidGauss3p>(
122 *(*other.
m_all)[i].second));
127 internal_forces::actuator::TYPE_toStr(
128 (*other.
m_all)[i].second->type())) +
132 for (
size_t i = 0; i < other.
m_isDofSet->size(); ++i) {
141 !*
m_isClose,
"You can't add actuator after closing the model");
149 act.
index() < model.
nbDof(),
"Sent index is out of dof range");
153 size_t idx(act.
index());
157 if (idx >=
m_all->size()) {
158 m_all->resize(idx + 1);
163 if (act.
type() == internal_forces::actuator::TYPE::CONSTANT) {
165 (*m_all)[idx].first =
166 std::make_shared<internal_forces::actuator::ActuatorConstant>(
169 (*m_isDofSet)[idx * 2] =
true;
171 (*m_all)[idx].second =
172 std::make_shared<internal_forces::actuator::ActuatorConstant>(
175 (*m_isDofSet)[idx * 2 + 1] =
true;
178 }
else if (act.
type() == internal_forces::actuator::TYPE::LINEAR) {
180 (*m_all)[idx].first =
181 std::make_shared<internal_forces::actuator::ActuatorLinear>(
184 (*m_isDofSet)[idx * 2] =
true;
186 (*m_all)[idx].second =
187 std::make_shared<internal_forces::actuator::ActuatorLinear>(
190 (*m_isDofSet)[idx * 2 + 1] =
true;
193 }
else if (act.
type() == internal_forces::actuator::TYPE::GAUSS3P) {
195 (*m_all)[idx].first =
196 std::make_shared<internal_forces::actuator::ActuatorGauss3p>(
199 (*m_isDofSet)[idx * 2] =
true;
201 (*m_all)[idx].second =
202 std::make_shared<internal_forces::actuator::ActuatorGauss3p>(
205 (*m_isDofSet)[idx * 2 + 1] =
true;
208 }
else if (act.
type() == internal_forces::actuator::TYPE::GAUSS6P) {
210 (*m_all)[idx].first =
211 std::make_shared<internal_forces::actuator::ActuatorGauss6p>(
214 (*m_isDofSet)[idx * 2] =
true;
216 (*m_all)[idx].second =
217 std::make_shared<internal_forces::actuator::ActuatorGauss6p>(
220 (*m_isDofSet)[idx * 2 + 1] =
true;
223 }
else if (act.
type() == internal_forces::actuator::TYPE::SIGMOIDGAUSS3P) {
225 (*m_all)[idx].first =
226 std::make_shared<internal_forces::actuator::ActuatorSigmoidGauss3p>(
230 (*m_isDofSet)[idx * 2] =
true;
232 (*m_all)[idx].second =
233 std::make_shared<internal_forces::actuator::ActuatorSigmoidGauss3p>(
237 (*m_isDofSet)[idx * 2 + 1] =
true;
307 *
m_isClose,
"Close the actuator model before calling torqueMax");
312 std::pair<rigidbody::GeneralizedTorque, rigidbody::GeneralizedTorque>
313 maxGeneralizedTorque_all = std::make_pair(
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>>
320 for (
unsigned p = 0; p < 2; ++p) {
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)
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)
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);
354 "Wrong type (should never get here because of previous safety)");
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)
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)
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);
389 "Wrong type (should never get here because of previous safety)");
394 return maxGeneralizedTorque_all;
402 *
m_isClose,
"Close the actuator model before calling torqueMax");
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));
414 if (activation(i) < 0) {
415 QdotResigned(i) = -Qdot(i);
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),
429 if (activation[i] >= 0) {
430 maxGeneralizedTorque_all[i] =
433 maxGeneralizedTorque_all[i] =
439 return maxGeneralizedTorque_all;
443 const std::shared_ptr<internal_forces::actuator::Actuator>
actuator,
446 if (std::dynamic_pointer_cast<ActuatorGauss3p>(
actuator)) {
447 return std::static_pointer_cast<ActuatorGauss3p>(
actuator)->torqueMax(
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(
456 }
else if (std::dynamic_pointer_cast<ActuatorSigmoidGauss3p>(
actuator)) {
457 return std::static_pointer_cast<ActuatorSigmoidGauss3p>(
actuator)
458 ->torqueMax(Q, Qdot);
461 "Wrong type (should never get here because of previous safety)");
This is the core of the musculoskeletal model in biorbd.
size_t nbDof() const
Return the number of degrees of freedom (DoF).