Biorbd Release_1.13.0
Loading...
Searching...
No Matches
Ligaments.cpp
1#define BIORBD_API_EXPORTS
2
3#include "InternalForces/Ligaments/Ligaments.h"
4
5#include "InternalForces/Ligaments/Ligament.h"
6#include "InternalForces/Ligaments/LigamentConstant.h"
7#include "InternalForces/Ligaments/LigamentSpringLinear.h"
8#include "InternalForces/Ligaments/LigamentSpringSecondOrder.h"
9#include "RigidBody/GeneralizedCoordinates.h"
10#include "RigidBody/GeneralizedTorque.h"
11#include "RigidBody/GeneralizedVelocity.h"
12#include "RigidBody/Joints.h"
13#include "Utils/Error.h"
14#include "Utils/Matrix.h"
15
16using namespace BIORBD_NAMESPACE;
17
20 std::make_shared<std::vector<
21 std::shared_ptr<internal_forces::ligaments::Ligament>>>()) {}
22
26
28
35
38 m_ligaments->resize(other.m_ligaments->size());
39 for (size_t i = 0; i < other.m_ligaments->size(); ++i) {
40 if ((*other.m_ligaments)[i]->type() ==
41 internal_forces::ligaments::LIGAMENT_TYPE::LIGAMENT_CONSTANT) {
42 (*m_ligaments)[i] =
43 std::make_shared<internal_forces::ligaments::LigamentConstant>(
44 (*other.m_ligaments)[i]);
45 return;
46 } else if (
47 (*other.m_ligaments)[i]->type() ==
48 internal_forces::ligaments::LIGAMENT_TYPE::LIGAMENT_SPRING_LINEAR) {
49 (*m_ligaments)[i] =
50 std::make_shared<internal_forces::ligaments::LigamentSpringLinear>(
51 (*other.m_ligaments)[i]);
52 return;
53 } else if (
54 (*other.m_ligaments)[i]->type() ==
55 internal_forces::ligaments::LIGAMENT_TYPE::
56 LIGAMENT_SPRING_SECOND_ORDER) {
57 (*m_ligaments)[i] = std::make_shared<
59 (*other.m_ligaments)[i]);
60 return;
61 }
62 }
63 *m_ligaments = *other.m_ligaments;
64}
65
69 idx < nbLigaments(), "Idx asked is higher than number of ligaments");
70 return *(*m_ligaments)[idx];
71}
72
76 idx < nbLigaments(), "Idx asked is higher than number of ligaments");
77 return *(*m_ligaments)[idx];
78}
79
81 const internal_forces::ligaments::Ligament& ligamentTp) {
82 // Add a passive torque to the pool of passive torques according to its type
83 if (ligamentTp.type() ==
84 internal_forces::ligaments::LIGAMENT_TYPE::LIGAMENT_CONSTANT) {
85 m_ligaments->push_back(
86 std::make_shared<internal_forces::ligaments::LigamentConstant>(
87 ligamentTp));
88 } else if (
89 ligamentTp.type() ==
90 internal_forces::ligaments::LIGAMENT_TYPE::LIGAMENT_SPRING_LINEAR) {
91 m_ligaments->push_back(
92 std::make_shared<internal_forces::ligaments::LigamentSpringLinear>(
93 ligamentTp));
94 } else if (
95 ligamentTp.type() ==
96 internal_forces::ligaments::LIGAMENT_TYPE::LIGAMENT_SPRING_SECOND_ORDER) {
97 m_ligaments->push_back(
98 std::make_shared<internal_forces::ligaments::LigamentSpringSecondOrder>(
99 ligamentTp));
100 } else {
101 utils::Error::raise("Ligament type not found");
102 }
103 return;
104}
105
106std::vector<utils::String>
108 std::vector<utils::String> names;
109 for (size_t i = 0; i < m_ligaments->size(); ++i) {
110 names.push_back((*m_ligaments)[i]->name());
111 }
112 return names;
113}
114
115// From ligament Force
118 const utils::Vector& F) {
119 // Get the Jacobian matrix and get the forces of each muscle
121
122 // Compute the reaction of the forces on the bodies
123 return rigidbody::GeneralizedTorque(-jaco.transpose() * F);
124}
125
126// From ligament Force and kinematics
129 const utils::Vector& F,
130 rigidbody::Joints& updatedModel,
133 bool updateLigamentKinematics) {
134 // Update the ligament position
135 if (updateLigamentKinematics) updateLigaments(updatedModel, Q, Qdot);
136 return ligamentsJointTorque(F);
137}
138
139// From ligament Force and kinematics
142 const utils::Vector& F,
145 int updateKin) {
146#ifdef BIORBD_USE_CASADI_MATH
147 if (updateKin < 2) {
150 "When using Casadi, this method must set updateKin to true. ") +
151 "Alternatively, you can call ligamentsJointTorque with the pre-updated "
152 "model.");
153 }
155#else
157#endif
158 updatedModel =
159 dynamic_cast<rigidbody::Joints&>(*this).UpdateKinematicsCustom(
160 updateKin >= 2 ? &Q : nullptr, updateKin >= 2 ? &Qdot : nullptr);
161
162 return ligamentsJointTorque(F, updatedModel, Q, Qdot, updateKin >= 1);
163}
164
165// From kinematics
168 rigidbody::Joints& updatedModel,
171 bool updateLigamentKinematics) {
173 ligamentForces(updatedModel, Q, Qdot, updateLigamentKinematics));
174}
175
176// From kinematics
181 int updateKin) {
182#ifdef BIORBD_USE_CASADI_MATH
183 if (updateKin < 2) {
186 "When using Casadi, this method must set updateKin to true. ") +
187 "Alternatively, you can call ligamentsJointTorque with the pre-updated "
188 "model.");
189 }
191#else
193#endif
194 updatedModel =
195 dynamic_cast<rigidbody::Joints&>(*this).UpdateKinematicsCustom(
196 updateKin >= 2 ? &Q : nullptr, updateKin >= 2 ? &Qdot : nullptr);
197
198 return ligamentsJointTorque(ligamentForces(updatedModel, Q, Qdot, updateKin));
199}
200
202 rigidbody::Joints& updatedModel,
204 bool updateLigamentKinematics) {
205 // Output variable
206 utils::Vector forces(nbLigaments());
207 for (size_t j = 0; j < nbLigaments(); ++j) {
208 forces(static_cast<unsigned int>(j)) =
209 ((*m_ligaments)[j]->force(updatedModel, Q, updateLigamentKinematics));
210 }
211
212 // The forces
213 return forces;
214}
215
218 int updateKin) {
219#ifdef BIORBD_USE_CASADI_MATH
220 if (updateKin < 2) {
223 "When using Casadi, this method must set updateKin to true. ") +
224 "Alternatively, you can call ligamentForces with the pre-updated "
225 "model.");
226 }
228#else
230#endif
231 updatedModel =
232 dynamic_cast<rigidbody::Joints&>(*this).UpdateKinematicsCustom(
233 updateKin >= 2 ? &Q : nullptr);
234
235 // The forces
236 return ligamentForces(updatedModel, Q, updateKin >= 1);
237}
238
240 rigidbody::Joints& updatedModel,
243 bool updateLigamentKinematics) {
244 // Output variable
245 utils::Vector forces(nbLigaments());
246 for (size_t j = 0; j < nbLigaments(); ++j) {
247 forces(static_cast<unsigned int>(j)) = ((*m_ligaments)[j]->force(
248 updatedModel, Q, Qdot, updateLigamentKinematics));
249 }
250
251 // The forces
252 return forces;
253}
254
258 int updateKin) {
259#ifdef BIORBD_USE_CASADI_MATH
260 if (updateKin < 2) {
263 "When using Casadi, this method must set updateKin to true. ") +
264 "Alternatively, you can call ligamentForces with the pre-updated "
265 "model.");
266 }
268#else
270#endif
271 updatedModel =
272 dynamic_cast<rigidbody::Joints&>(*this).UpdateKinematicsCustom(
273 updateKin >= 2 ? &Q : nullptr, updateKin >= 2 ? &Qdot : nullptr);
274
275 return ligamentForces(updatedModel, Q, Qdot, updateKin >= 1);
276}
277
279 const {
280 // Assuming that this is also a Joints type (via BiorbdModel)
282 dynamic_cast<const rigidbody::Joints&>(*this));
283}
284
287 const rigidbody::Joints& model) const {
288 utils::Matrix tp(nbLigaments(), model.nbDof());
289 for (size_t j = 0; j < nbLigaments(); ++j) {
290 tp.block(
291 static_cast<unsigned int>(j),
292 0,
293 1,
294 static_cast<unsigned int>(model.nbDof())) =
295 (*m_ligaments)[j]->position().jacobianLength();
296 }
297 return tp;
298}
299
301 rigidbody::Joints& updatedModel,
303 bool updateLigamentKinematics) {
304 // Update the ligament position
305 if (updateLigamentKinematics) updateLigaments(updatedModel, Q);
306 return computeLigamentsLengthJacobian(updatedModel);
307}
308
311 int updateKin) {
312#ifdef BIORBD_USE_CASADI_MATH
313 if (updateKin < 2) {
316 "When using Casadi, this method must set updateKin to true. ") +
317 "Alternatively, you can call ligamentsLengthJacobian with the "
318 "pre-updated model.");
319 }
321#else
323#endif
324 updatedModel =
325 dynamic_cast<rigidbody::Joints&>(*this).UpdateKinematicsCustom(
326 updateKin >= 2 ? &Q : nullptr);
327
328 return ligamentsLengthJacobian(updatedModel, Q, updateKin >= 1);
329}
330
332 return m_ligaments->size();
333}
334
335std::vector<std::shared_ptr<internal_forces::ligaments::Ligament>>&
339
340const std::vector<std::shared_ptr<internal_forces::ligaments::Ligament>>&
344
346 const utils::String& nameToFind) {
347 for (size_t i = 0; i < m_ligaments->size(); ++i) {
348 if (!nameToFind.compare((*m_ligaments)[i]->name())) {
349 return static_cast<int>(i);
350 }
351 }
352 return -1;
353}
354
356 rigidbody::Joints& updatedModel,
358 const rigidbody::GeneralizedVelocity& Qdot) {
359 for (size_t j = 0; j < nbLigaments(); ++j) {
360 ligament(j).updateOrientations(updatedModel, Q, Qdot);
361 }
362}
363
367 bool updateKin) {
368#ifdef BIORBD_USE_CASADI_MATH
370#else
372#endif
373 updatedModel =
374 dynamic_cast<rigidbody::Joints&>(*this).UpdateKinematicsCustom(
375 updateKin ? &Q : nullptr, updateKin ? &Qdot : nullptr);
376 updateLigaments(updatedModel, Q, Qdot);
377}
378
380 rigidbody::Joints& updatedModel,
382 for (size_t j = 0; j < nbLigaments(); ++j) {
383 ligament(j).updateOrientations(updatedModel, Q);
384 }
385}
386
389 bool updateKin) {
390#ifdef BIORBD_USE_CASADI_MATH
392#else
394#endif
395 updatedModel =
396 dynamic_cast<rigidbody::Joints&>(*this).UpdateKinematicsCustom(
397 updateKin ? &Q : nullptr);
398 updateLigaments(updatedModel, Q);
399}
400
402 rigidbody::Joints& updatedModel,
403 std::vector<std::vector<utils::Vector3d>>& ligamentPointsInGlobal,
404 std::vector<utils::Matrix>& jacoPointsInGlobal,
405 const rigidbody::GeneralizedVelocity& Qdot) {
406 for (size_t j = 0; j < nbLigaments(); ++j) {
407 ligament(j).updateOrientations(
408 ligamentPointsInGlobal[j], jacoPointsInGlobal[j], Qdot);
409 }
410}
411
413 std::vector<std::vector<utils::Vector3d>>& ligamentPointsInGlobal,
414 std::vector<utils::Matrix>& jacoPointsInGlobal) {
415 for (size_t j = 0; j < nbLigaments(); ++j) {
416 ligament(j).updateOrientations(
417 ligamentPointsInGlobal[j], jacoPointsInGlobal[j]);
418 }
419}
Class LigamentSpringSecondOrder is a ligament with same behavior than second order spring defined by:...
LIGAMENT_TYPE type() const
Return the type of the ligament.
Definition Ligament.cpp:104
void updateLigaments(rigidbody::Joints &updatedModel, const rigidbody::GeneralizedCoordinates &Q)
Update all the ligaments (positions, jacobian, etc.).
int ligamentID(const utils::String &name)
Return the ligament index.
std::shared_ptr< std::vector< std::shared_ptr< Ligament > > > m_ligaments
Holder for ligament groups.
Definition Ligaments.h:394
utils::Vector ligamentForces(rigidbody::Joints &updatedModel, const rigidbody::GeneralizedCoordinates &Q, bool updateLigamentKinematics=true)
Compute and return the ligament forces.
rigidbody::GeneralizedTorque ligamentsJointTorque(const utils::Vector &F)
Compute the ligament joint torque.
std::vector< std::shared_ptr< Ligament > > & ligaments()
Returns all the ligaments. It sorts the ligaments by group.
utils::Matrix ligamentsLengthJacobian() const
Return the previously computed ligament length jacobian.
void addLigament(const Ligament &ligament)
Add a ligament to the set of ligaments.
Definition Ligaments.cpp:80
Ligaments DeepCopy() const
Deep copy of ligaments.
Definition Ligaments.cpp:30
Ligament & ligament(size_t idx)
Returns a specific ligament sorted by ligaments().
Definition Ligaments.cpp:67
std::vector< utils::String > ligamentNames() const
ligamentNames Return the names for all the ligament ordered by their respective name
utils::Matrix computeLigamentsLengthJacobian(const rigidbody::Joints &model) const
Assemble the ligament length jacobian using the provided model.
size_t nbLigaments() const
Return the total number of ligament.
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
Joints & UpdateKinematicsCustom(const GeneralizedCoordinates *Q=nullptr, const GeneralizedVelocity *Qdot=nullptr, const rigidbody::GeneralizedAcceleration *Qddot=nullptr)
Update the kinematic variables such as body velocities and accelerations in the model to reflect the ...
Definition Joints.cpp:1888
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
A wrapper for the Eigen::MatrixXd.
Definition Matrix.h:18
Wrapper around the std::string class with augmented functionality.
Definition String.h:18
Wrapper of the Eigen VectorXd.
Definition Vector.h:21