Biorbd Release_1.13.0
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
17using namespace BIORBD_NAMESPACE;
18
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
30
32
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<
41 static_cast<
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<
50 static_cast<
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<
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
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
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<
91 static_cast<
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<
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<
111 static_cast<
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
122const std::shared_ptr<internal_forces::passive_torques::PassiveTorque> &
125 dof < nbPassiveTorques(),
126 "Idx asked is higher than number of passive torque");
127 return (*m_pas)[dof];
128}
129
134
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 =
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 {
168 "Wrong type (should never get here because of previous safety)");
169 }
170 }
171 return GeneralizedTorque_all;
172}
Class PassiveTorqueConstant is a joint passive torque type which maximum is contant.
Class PassiveTorqueExponential is a joint passive torque type that linearly evolves passive torque is...
Class PassiveTorqueLinear is a joint passive torque type that linearly evolves Linear passive torque ...
size_t index() const
Return the index of the DoF associated with passive torque.
TORQUE_TYPE type() const
Return the type of the passive torque.
std::shared_ptr< std::vector< bool > > m_isDofSet
If DoF all dof are set.
size_t nbPassiveTorques() const
Return the toal number of passive torques.
const std::shared_ptr< PassiveTorque > & getPassiveTorque(size_t dof)
Return a specific passive torque.
virtual ~PassiveTorques()
Destroy passive torques class properly.
std::shared_ptr< std::vector< std::shared_ptr< internal_forces::passive_torques::PassiveTorque > > > m_pas
Passive torque to add.
void addPassiveTorque(const PassiveTorque &other)
Add a passive torque to the set of passive torques.
void DeepCopy(const PassiveTorques &other)
Deep copy of the passive torque holder from other passive torque holder.
rigidbody::GeneralizedTorque passiveJointTorque(const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot)
Return the passiveJointTorques.
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