Biorbd Release_1.13.0
Loading...
Searching...
No Matches
Muscles.cpp
1#define BIORBD_API_EXPORTS
2#include "InternalForces/Muscles/Muscles.h"
3
4#include "InternalForces/Muscles/Muscle.h"
5#include "InternalForces/Muscles/MuscleGroup.h"
6#include "InternalForces/Muscles/StateDynamics.h"
7#include "RigidBody/GeneralizedCoordinates.h"
8#include "RigidBody/GeneralizedTorque.h"
9#include "RigidBody/GeneralizedVelocity.h"
10#include "RigidBody/Joints.h"
11#include "Utils/Error.h"
12#include "Utils/Matrix.h"
13
14using namespace BIORBD_NAMESPACE;
15
17 : m_mus(
18 std::make_shared<
19 std::vector<internal_forces::muscles::MuscleGroup>>()) {}
20
24
26
33
36 m_mus->resize(other.m_mus->size());
37 for (size_t i = 0; i < other.m_mus->size(); ++i) {
38 (*m_mus)[i] = (*other.m_mus)[i];
39 }
40}
41
43 const utils::String& name,
44 const utils::String& originName,
45 const utils::String& insertionName) {
46 if (m_mus->size() > 0) {
48 getMuscleGroupId(name) == -1, "Muscle group already defined");
49 }
50
51 m_mus->push_back(
52 internal_forces::muscles::MuscleGroup(name, originName, insertionName));
53}
54
56 const utils::String& name) const {
57 for (size_t i = 0; i < m_mus->size(); ++i)
58 if (!name.compare((*m_mus)[i].name())) {
59 return static_cast<int>(i);
60 }
61 return -1;
62}
63
64std::vector<std::shared_ptr<internal_forces::muscles::Muscle>>
66 std::vector<std::shared_ptr<internal_forces::muscles::Muscle>> m;
67 for (auto group : muscleGroups()) {
68 for (auto muscle : group.muscles()) {
69 m.push_back(muscle);
70 }
71 }
72 return m;
73}
74
76 size_t idx) const {
77 for (auto g : muscleGroups()) {
78 if (idx >= g.nbMuscles()) {
79 idx -= g.nbMuscles();
80 } else {
81 return g.muscle(idx);
82 }
83 }
84 utils::Error::raise("idx is higher than the number of muscles");
85}
86
88 const {
89 std::vector<utils::String> names;
90 for (auto group : muscleGroups()) {
91 for (auto muscle : group.muscles()) {
92 names.push_back(muscle->name());
93 }
94 }
95 return names;
96}
97
98std::vector<internal_forces::muscles::MuscleGroup>&
102
103const std::vector<internal_forces::muscles::MuscleGroup>&
107
111 idx < nbMuscleGroups(),
112 "Idx asked is higher than number of muscle groups");
113 return (*m_mus)[idx];
114}
115
119 idx < nbMuscleGroups(),
120 "Idx asked is higher than number of muscle groups");
121 return (*m_mus)[idx];
122}
125 const utils::String& name) const {
126 int idx = getMuscleGroupId(name);
127 utils::Error::check(idx != -1, "Group name could not be found");
128 return muscleGroup(static_cast<size_t>(idx));
129}
130
131// From muscle activation (return muscle force)
134 // Get the Jacobian matrix and get the forces of each muscle
136
137 // Compute the reaction of the forces on the bodies
138 return rigidbody::GeneralizedTorque(-jaco.transpose() * F);
139}
140
141// From Muscular Force
144 const utils::Vector& F,
145 rigidbody::Joints& updatedModel,
148 bool updateMuscleParameters) {
149 // Update the muscular position
150 if (updateMuscleParameters) updateMuscles(updatedModel, Q, Qdot);
151 return muscularJointTorque(F);
152}
153
154// From Muscular Force
157 const utils::Vector& F,
160 int updateKin) {
161 // Update the muscular position
162 if (updateKin >= 1) updateMuscles(Q, Qdot, updateKin >= 2);
163 return muscularJointTorque(F);
164}
165
166// From muscle activation (return muscle force)
169 const std::vector<std::shared_ptr<State>>& emg) {
171}
172
173// From muscle activation (do not return muscle force)
176 rigidbody::Joints& updatedModel,
177 const std::vector<std::shared_ptr<State>>& emg,
180 bool updateMuscleParameters) {
181 return muscularJointTorque(
182 muscleForces(updatedModel, emg, Q, Qdot, updateMuscleParameters));
183}
184
185// From muscle activation (do not return muscle force)
188 const std::vector<std::shared_ptr<State>>& emg,
191 int updateKin) {
192 return muscularJointTorque(muscleForces(emg, Q, Qdot, updateKin));
193}
194
196 const std::vector<std::shared_ptr<internal_forces::muscles::State>>& emg,
197 bool areadyNormalized) {
199
200 size_t cmp(0);
201 for (size_t i = 0; i < nbMuscleGroups(); ++i)
202 for (size_t j = 0; j < muscleGroup(i).nbMuscles(); ++j) {
203 // Recueillir dérivées d'activtion
204 activationDot(static_cast<unsigned int>(cmp)) =
205 muscleGroup(i).muscle(j).activationDot(*emg[cmp], areadyNormalized);
206 ++cmp;
207 }
208
209 return activationDot;
210}
211
213 const std::vector<std::shared_ptr<State>>& emg) {
214 // Output variable
216
217 size_t cmpMus(0);
218 for (size_t i = 0; i < m_mus->size(); ++i) { // muscle group
219 for (size_t j = 0; j < (*m_mus)[i].nbMuscles(); ++j) {
220 forces(static_cast<unsigned int>(cmpMus), 0) =
221 ((*m_mus)[i].muscle(j).force(*emg[cmpMus]));
222 ++cmpMus;
223 }
224 }
225
226 // The forces
227 return forces;
228}
229
231 rigidbody::Joints& updatedModel,
232 const std::vector<std::shared_ptr<State>>& emg,
235 bool updateMuscleParameters) {
236 // Update the muscular position
237 if (updateMuscleParameters) updateMuscles(updatedModel, Q, Qdot);
238 return muscleForces(emg);
239}
240
242 const std::vector<std::shared_ptr<State>>& emg,
245 int updateKin) {
246 // Update the muscular position
247 updateMuscles(Q, Qdot, updateKin);
248 return muscleForces(emg);
249}
250
252 return m_mus->size();
253}
254
256 // Assuming that this is also a Joints type (via BiorbdModel)
257 return computeMusclesLengthJacobian(dynamic_cast<rigidbody::Joints&>(*this));
258}
259
261 const rigidbody::Joints& model) {
262 utils::Matrix tp(nbMuscleTotal(), model.nbDof());
263 size_t cmpMus(0);
264 for (size_t i = 0; i < nbMuscleGroups(); ++i)
265 for (size_t j = 0; j < ((*m_mus)[i]).nbMuscles(); ++j) {
266 tp.block(
267 static_cast<unsigned int>(cmpMus++),
268 0,
269 1,
270 static_cast<unsigned int>(model.nbDof())) =
271 ((*m_mus)[i]).muscle(j).position().jacobianLength();
272 }
273
274 return tp;
275}
276
278 rigidbody::Joints& updatedModel,
280 bool updateMuscleParameters) {
281 // Update the muscular position
282 if (updateMuscleParameters) updateMuscles(updatedModel, Q);
283 return computeMusclesLengthJacobian(updatedModel);
284}
285
288 int updateKin) {
289#ifdef BIORBD_USE_CASADI_MATH
290 if (updateKin < 2) {
293 "When using Casadi, this method must set updateKin to true. ") +
294 "Alternatively, you can call musclesLengthJacobian with the "
295 "pre-updated model.");
296 }
298#else
300#endif
301 updatedModel =
302 dynamic_cast<rigidbody::Joints&>(*this).UpdateKinematicsCustom(
303 updateKin >= 2 ? &Q : nullptr);
304
305 // Update the muscular position
306 if (updateKin >= 1) updateMuscles(updatedModel, Q);
307 return musclesLengthJacobian();
308}
309
313
315 size_t total(0);
316 for (size_t grp = 0; grp < m_mus->size(); ++grp) { // muscular group
317 total += (*m_mus)[grp].nbMuscles();
318 }
319 return total;
320}
321
323 rigidbody::Joints& updatedModel,
325 // Update all the muscles
326 for (auto group : *m_mus) { // muscle group
327 for (size_t j = 0; j < group.nbMuscles(); ++j) {
328 group.muscle(j).updateOrientations(updatedModel, Q);
329 }
330 }
331}
332
335 bool updateKin) {
336#ifdef BIORBD_USE_CASADI_MATH
337 if (!updateKin) {
340 "When using Casadi, this method must set updateKin to true. ") +
341 "Alternatively, you can call updateMuscles with the pre-updated "
342 "model.");
343 }
345#else
347#endif
348 updatedModel =
349 dynamic_cast<rigidbody::Joints&>(*this).UpdateKinematicsCustom(
350 updateKin ? &Q : nullptr);
351 updateMuscles(updatedModel, Q);
352}
353
355 rigidbody::Joints& updatedModel,
357 const rigidbody::GeneralizedVelocity& Qdot) {
358 // Update all the muscles
359 for (auto group : *m_mus) { // muscle group
360 for (size_t j = 0; j < group.nbMuscles(); ++j) {
361 group.muscle(j).updateOrientations(updatedModel, Q, Qdot);
362 }
363 }
364}
365
369 bool updateKin) {
370#ifdef BIORBD_USE_CASADI_MATH
371 if (!updateKin) {
374 "When using Casadi, this method must set updateKin to true. ") +
375 "Alternatively, you can call updateMuscles with the pre-updated "
376 "model.");
377 }
379#else
381#endif
382 updatedModel =
383 dynamic_cast<rigidbody::Joints&>(*this).UpdateKinematicsCustom(
384 updateKin ? &Q : nullptr);
385
386 updateMuscles(updatedModel, Q, Qdot);
387}
388
390 std::vector<std::vector<utils::Vector3d>>& musclePointsInGlobal,
391 std::vector<utils::Matrix>& jacoPointsInGlobal,
392 const rigidbody::GeneralizedVelocity& Qdot) {
393 size_t cmpMuscle = 0;
394 for (auto group : *m_mus) // muscle group
395 for (size_t j = 0; j < group.nbMuscles(); ++j) {
396 group.muscle(j).updateOrientations(
397 musclePointsInGlobal[cmpMuscle], jacoPointsInGlobal[cmpMuscle], Qdot);
398 ++cmpMuscle;
399 }
400}
401
402std::vector<std::shared_ptr<internal_forces::muscles::State>>
404 std::vector<std::shared_ptr<internal_forces::muscles::State>> out;
405 for (size_t i = 0; i < nbMuscles(); ++i) {
406 out.push_back(muscle(i).m_state);
407 }
408 return out;
409}
410
412 std::vector<std::vector<utils::Vector3d>>& musclePointsInGlobal,
413 std::vector<utils::Matrix>& jacoPointsInGlobal) {
414 // Updater all the muscles
415 size_t cmpMuscle = 0;
416 for (auto group : *m_mus) // muscle group
417 for (size_t j = 0; j < group.nbMuscles(); ++j) {
418 group.muscle(j).updateOrientations(
419 musclePointsInGlobal[cmpMuscle], jacoPointsInGlobal[cmpMuscle]);
420 ++cmpMuscle;
421 }
422}
A muscle group is muscle that share parents for both origin and insertion.
Definition MuscleGroup.h:28
std::vector< std::shared_ptr< Muscle > > muscles() const
Returns all the muscles. It sorts the muscles by group.
Definition Muscles.cpp:65
size_t nbMuscles() const
Return the total number of muscle.
Definition Muscles.cpp:314
void addMuscleGroup(const utils::String &name, const utils::String &originName, const utils::String &insertionName)
Add a muscle group to the set.
Definition Muscles.cpp:42
std::vector< utils::String > muscleNames() const
muscleNames Return the names for all the muscle ordered by their respective group name
Definition Muscles.cpp:87
size_t nbMuscleTotal() const
Return the total number of muscle.
Definition Muscles.cpp:310
size_t nbMuscleGroups() const
Return the total number of muscle groups.
Definition Muscles.cpp:251
std::shared_ptr< std::vector< MuscleGroup > > m_mus
Holder for muscle groups.
Definition Muscles.h:448
Muscle & muscle(size_t idx) const
Returns a specific muscle sorted by muscles().
Definition Muscles.cpp:75
std::vector< std::shared_ptr< State > > stateSet()
Get the vector of state that must be used to update states.
Definition Muscles.cpp:403
rigidbody::GeneralizedTorque muscularJointTorque(const utils::Vector &F)
Compute the muscular joint torque.
Definition Muscles.cpp:133
std::vector< MuscleGroup > & muscleGroups()
Return the muscle groups.
Definition Muscles.cpp:99
Muscles DeepCopy() const
Deep copy of muscles.
Definition Muscles.cpp:27
virtual ~Muscles()
Destroy class properly.
Definition Muscles.cpp:25
void updateMuscles(rigidbody::Joints &updatedModel, const rigidbody::GeneralizedCoordinates &Q)
Update all the muscles (positions, jacobian, etc.).
Definition Muscles.cpp:322
utils::Vector activationDot(const std::vector< std::shared_ptr< State > > &states, bool areadyNormalized=true)
Interface that returns in a vector all the activations dot.
Definition Muscles.cpp:195
utils::Matrix musclesLengthJacobian()
Return the previously computed muscle length jacobian.
Definition Muscles.cpp:255
MuscleGroup & muscleGroup(size_t idx)
Return the muscle group of specific index.
Definition Muscles.cpp:109
int getMuscleGroupId(const utils::String &name) const
Return the group ID.
Definition Muscles.cpp:55
utils::Vector muscleForces(const std::vector< std::shared_ptr< State > > &emg)
Compute and return the muscle forces.
Definition Muscles.cpp:212
utils::Matrix computeMusclesLengthJacobian(const rigidbody::Joints &model)
Assemble the muscle length jacobian using the provided model.
Definition Muscles.cpp:260
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