1#define BIORBD_API_EXPORTS
2#include "InternalForces/Muscles/Muscles.h"
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"
14using namespace BIORBD_NAMESPACE;
37 for (
size_t i = 0; i < other.
m_mus->size(); ++i) {
38 (*m_mus)[i] = (*other.
m_mus)[i];
46 if (
m_mus->size() > 0) {
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);
64std::vector<std::shared_ptr<internal_forces::muscles::Muscle>>
66 std::vector<std::shared_ptr<internal_forces::muscles::Muscle>> m;
68 for (
auto muscle : group.muscles()) {
78 if (idx >= g.nbMuscles()) {
89 std::vector<utils::String> names;
91 for (
auto muscle : group.muscles()) {
92 names.push_back(
muscle->name());
98std::vector<internal_forces::muscles::MuscleGroup>&
103const std::vector<internal_forces::muscles::MuscleGroup>&
112 "Idx asked is higher than number of muscle groups");
113 return (*
m_mus)[idx];
120 "Idx asked is higher than number of muscle groups");
121 return (*
m_mus)[idx];
148 bool updateMuscleParameters) {
150 if (updateMuscleParameters)
updateMuscles(updatedModel, Q, Qdot);
169 const std::vector<std::shared_ptr<State>>& emg) {
177 const std::vector<std::shared_ptr<State>>& emg,
180 bool updateMuscleParameters) {
182 muscleForces(updatedModel, emg, Q, Qdot, updateMuscleParameters));
188 const std::vector<std::shared_ptr<State>>& emg,
196 const std::vector<std::shared_ptr<internal_forces::muscles::State>>& emg,
197 bool areadyNormalized) {
202 for (
size_t j = 0; j <
muscleGroup(i).nbMuscles(); ++j) {
205 muscleGroup(i).muscle(j).activationDot(*emg[cmp], areadyNormalized);
213 const std::vector<std::shared_ptr<State>>& emg) {
218 for (
size_t i = 0; i <
m_mus->size(); ++i) {
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]));
232 const std::vector<std::shared_ptr<State>>& emg,
235 bool updateMuscleParameters) {
237 if (updateMuscleParameters)
updateMuscles(updatedModel, Q, Qdot);
242 const std::vector<std::shared_ptr<State>>& emg,
252 return m_mus->size();
265 for (
size_t j = 0; j < ((*m_mus)[i]).nbMuscles(); ++j) {
267 static_cast<unsigned int>(cmpMus++),
270 static_cast<unsigned int>(model.
nbDof())) =
271 ((*m_mus)[i]).
muscle(j).position().jacobianLength();
280 bool updateMuscleParameters) {
289#ifdef BIORBD_USE_CASADI_MATH
293 "When using Casadi, this method must set updateKin to true. ") +
294 "Alternatively, you can call musclesLengthJacobian with the "
295 "pre-updated model.");
303 updateKin >= 2 ? &Q :
nullptr);
316 for (
size_t grp = 0; grp <
m_mus->size(); ++grp) {
317 total += (*m_mus)[grp].nbMuscles();
326 for (
auto group : *
m_mus) {
327 for (
size_t j = 0; j < group.nbMuscles(); ++j) {
328 group.muscle(j).updateOrientations(updatedModel, Q);
336#ifdef BIORBD_USE_CASADI_MATH
340 "When using Casadi, this method must set updateKin to true. ") +
341 "Alternatively, you can call updateMuscles with the pre-updated "
350 updateKin ? &Q :
nullptr);
359 for (
auto group : *
m_mus) {
360 for (
size_t j = 0; j < group.nbMuscles(); ++j) {
361 group.muscle(j).updateOrientations(updatedModel, Q, Qdot);
370#ifdef BIORBD_USE_CASADI_MATH
374 "When using Casadi, this method must set updateKin to true. ") +
375 "Alternatively, you can call updateMuscles with the pre-updated "
384 updateKin ? &Q :
nullptr);
390 std::vector<std::vector<utils::Vector3d>>& musclePointsInGlobal,
391 std::vector<utils::Matrix>& jacoPointsInGlobal,
393 size_t cmpMuscle = 0;
394 for (
auto group : *
m_mus)
395 for (
size_t j = 0; j < group.nbMuscles(); ++j) {
396 group.muscle(j).updateOrientations(
397 musclePointsInGlobal[cmpMuscle], jacoPointsInGlobal[cmpMuscle], Qdot);
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);
412 std::vector<std::vector<utils::Vector3d>>& musclePointsInGlobal,
413 std::vector<utils::Matrix>& jacoPointsInGlobal) {
415 size_t cmpMuscle = 0;
416 for (
auto group : *
m_mus)
417 for (
size_t j = 0; j < group.nbMuscles(); ++j) {
418 group.muscle(j).updateOrientations(
419 musclePointsInGlobal[cmpMuscle], jacoPointsInGlobal[cmpMuscle]);
A muscle group is muscle that share parents for both origin and insertion.
Base class of all muscle.
std::vector< std::shared_ptr< Muscle > > muscles() const
Returns all the muscles. It sorts the muscles by group.
size_t nbMuscles() const
Return the total number of muscle.
void addMuscleGroup(const utils::String &name, const utils::String &originName, const utils::String &insertionName)
Add a muscle group to the set.
std::vector< utils::String > muscleNames() const
muscleNames Return the names for all the muscle ordered by their respective group name
size_t nbMuscleTotal() const
Return the total number of muscle.
size_t nbMuscleGroups() const
Return the total number of muscle groups.
std::shared_ptr< std::vector< MuscleGroup > > m_mus
Holder for muscle groups.
Muscle & muscle(size_t idx) const
Returns a specific muscle sorted by muscles().
std::vector< std::shared_ptr< State > > stateSet()
Get the vector of state that must be used to update states.
rigidbody::GeneralizedTorque muscularJointTorque(const utils::Vector &F)
Compute the muscular joint torque.
std::vector< MuscleGroup > & muscleGroups()
Return the muscle groups.
Muscles DeepCopy() const
Deep copy of muscles.
virtual ~Muscles()
Destroy class properly.
void updateMuscles(rigidbody::Joints &updatedModel, const rigidbody::GeneralizedCoordinates &Q)
Update all the muscles (positions, jacobian, etc.).
utils::Vector activationDot(const std::vector< std::shared_ptr< State > > &states, bool areadyNormalized=true)
Interface that returns in a vector all the activations dot.
utils::Matrix musclesLengthJacobian()
Return the previously computed muscle length jacobian.
MuscleGroup & muscleGroup(size_t idx)
Return the muscle group of specific index.
int getMuscleGroupId(const utils::String &name) const
Return the group ID.
utils::Vector muscleForces(const std::vector< std::shared_ptr< State > > &emg)
Compute and return the muscle forces.
utils::Matrix computeMusclesLengthJacobian(const rigidbody::Joints &model)
Assemble the muscle length jacobian using the provided model.
Muscles()
Construct muscles.
Class GeneralizedCoordinates.
Class GeneralizedVelocity.
This is the core of the musculoskeletal model in biorbd.
size_t nbDof() const
Return the number of degrees of freedom (DoF).
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 ...
static void raise(const String &message)
Throw an error message.
static void check(bool cond, const String &message)
Assert that raises the error message if false.
A wrapper for the Eigen::MatrixXd.
Wrapper around the std::string class with augmented functionality.
Wrapper of the Eigen VectorXd.