Biorbd Release_1.13.0
Loading...
Searching...
No Matches
MuscleGeometry.cpp
1#define BIORBD_API_EXPORTS
2
3#include "InternalForces/Muscles/MuscleGeometry.h"
4
5#include <rbdl/Model.h>
6
7#include <rbdl/Kinematics.h>
8
9#include "InternalForces/Geometry.h"
10#include "InternalForces/Muscles/Characteristics.h"
11#include "InternalForces/PathModifiers.h"
12#include "InternalForces/ViaPoint.h"
13#include "InternalForces/WrappingObject.h"
14#include "RigidBody/GeneralizedCoordinates.h"
15#include "RigidBody/GeneralizedVelocity.h"
16#include "RigidBody/Joints.h"
17#include "RigidBody/NodeSegment.h"
18#include "Utils/Error.h"
19#include "Utils/Matrix.h"
20#include "Utils/RotoTrans.h"
21
22using namespace BIORBD_NAMESPACE;
23
25 : internal_forces::Geometry(),
26 m_muscleTendonLength(std::make_shared<utils::Scalar>(0)),
27 m_muscleLength(std::make_shared<utils::Scalar>(0))
28
29{}
30
32 const utils::Vector3d& origin,
33 const utils::Vector3d& insertion)
34 : internal_forces::Geometry(origin, insertion),
35 m_muscleTendonLength(std::make_shared<utils::Scalar>(0)),
36 m_muscleLength(std::make_shared<utils::Scalar>(0)) {}
37
44
51
52// ------ PUBLIC FUNCTIONS ------ //
54 rigidbody::Joints& updatedModel,
55 const internal_forces::muscles::Characteristics& characteristics,
59
60 // Position of the points in space
61 setPointsInGlobal(updatedModel, *Q);
62
63 // Compute the Jacobian of the muscle points
64 jacobian(updatedModel, *Q);
65
66 // Complete the update
67 _updateKinematics(Qdot, &characteristics);
68}
69
71 rigidbody::Joints& updatedModel,
72 const internal_forces::muscles::Characteristics& characteristics,
73 internal_forces::PathModifiers& pathModifiers,
77
78 // Position of the points in space
79 setPointsInGlobal(updatedModel, *Q, &pathModifiers);
80
81 // Compute the Jacobian of the muscle points
82 jacobian(updatedModel, *Q);
83
84 // Complete the update
85 _updateKinematics(Qdot, &characteristics, &pathModifiers);
86}
87
89 std::vector<utils::Vector3d>& pointsInGlobal,
90 utils::Matrix& jacoPointsInGlobal,
91 const internal_forces::muscles::Characteristics& characteristics,
94
95 // Position of the points in space
97
98 // Compute the Jacobian of the muscle points
99 jacobian(jacoPointsInGlobal);
100
101 // Complete the update
102 _updateKinematics(Qdot, &characteristics);
103}
104
108 "Geometry must be computed at least before calling length()");
109 return *m_muscleLength;
110}
111
112const utils::Scalar&
116 "Geometry must be computed at least before calling length()");
117 return *m_muscleTendonLength;
118}
119
120// --------------------------------------- //
121
124 const internal_forces::muscles::Characteristics* characteristics,
125 internal_forces::PathModifiers* pathModifiers) {
126 // Compute the length and velocities
127 length(characteristics, pathModifiers);
128 *m_isGeometryComputed = true;
129
130 // Compute the jacobian of the lengths
132 if (Qdot != nullptr) {
133 velocity(*Qdot);
134 *m_isVelocityComputed = true;
135 } else {
136 *m_isVelocityComputed = false;
137 }
138}
139
141 const internal_forces::muscles::Characteristics* characteristics,
142 internal_forces::PathModifiers* pathModifiers) {
144
145 // because we can't combine, test the first (0) will let us know all the types
146 // if more than one
147 if (pathModifiers != nullptr && pathModifiers->nbWraps() != 0) {
149 pathModifiers->nbVia() == 0, "Cannot mix wrapping and via points yet");
151 pathModifiers->nbWraps() < 2,
152 "Cannot compute more than one wrapping yet");
153
154 utils::Vector3d pi_wrap(
155 0, 0, 0); // point on the wrapping related to insertion
156 utils::Vector3d po_wrap(
157 0, 0, 0); // point on the wrapping related to origin
158 utils::Scalar lengthWrap(0);
159 static_cast<internal_forces::WrappingObject&>(pathModifiers->object(0))
160 .wrapPoints(po_wrap, pi_wrap, &lengthWrap);
161
163 ((*m_pointsInGlobal)[0] - po_wrap).norm() + // length before the wrap
164 lengthWrap + // length on the wrap
165 ((*m_pointsInGlobal)[3] - pi_wrap).norm(); // length after the wrap
166
167 } else {
168 for (size_t i = 0; i < m_pointsInGlobal->size() - 1; ++i) {
170 ((*m_pointsInGlobal)[i + 1] - (*m_pointsInGlobal)[i]).norm();
171 }
172 }
174 (*m_muscleTendonLength - characteristics->tendonSlackLength()) /
175 std::cos(characteristics->pennationAngle());
176 return *m_muscleLength;
177}
const utils::Scalar & velocity() const
Return the previously computed velocity.
Definition Geometry.cpp:193
std::shared_ptr< bool > m_isGeometryComputed
Definition Geometry.h:316
void computeJacobianLength()
Compute the muscle length jacobian.
Definition Geometry.cpp:444
void setPointsInGlobal(std::vector< utils::Vector3d > &ptsInGlobal)
Set the muscle points in global.
Definition Geometry.cpp:271
std::shared_ptr< bool > m_posAndJacoWereForced
Definition Geometry.h:321
Geometry DeepCopy() const
Deep copy of a geometry.
Definition Geometry.cpp:58
const std::vector< utils::Vector3d > & pointsInGlobal() const
Return the muscles points in the global reference.
Definition Geometry.cpp:176
const utils::Matrix & jacobian() const
Return the previously computed muscle jacobian.
Definition Geometry.cpp:201
std::shared_ptr< bool > m_isVelocityComputed
Definition Geometry.h:318
std::shared_ptr< std::vector< utils::Vector3d > > m_pointsInGlobal
Position of all the points in the global reference.
Definition Geometry.h:305
Holder of all the path modifiers of a muscle.
size_t nbVia() const
Return the total number of via points in the set.
size_t nbWraps() const
Return the total number of wrapping objects in the set.
utils::Vector3d & object(size_t idx)
Return the object at a specific index in the set.
Base class for the wrapping objects.
virtual void wrapPoints(const utils::RotoTrans &rt, const utils::Vector3d &p1_bone, const utils::Vector3d &p2_bone, utils::Vector3d &p1, utils::Vector3d &p2, utils::Scalar *muscleLength=nullptr)=0
From the position of the wrapping object, return the 2 locations where the muscle leaves the wrapping...
const utils::Scalar & pennationAngle() const
Return the angle of pennation.
const utils::Scalar & tendonSlackLength() const
Return the tendon slack length.
std::shared_ptr< utils::Scalar > m_muscleTendonLength
muscle tendon length
const utils::Scalar & musculoTendonLength() const
Return the previously computed muscle tendon length.
std::shared_ptr< utils::Scalar > m_muscleLength
length
void updateKinematics(rigidbody::Joints &updatedModel, const Characteristics &characteristics, const rigidbody::GeneralizedCoordinates *Q=nullptr, const rigidbody::GeneralizedVelocity *Qdot=nullptr)
Updates the position and dynamic elements of the muscles.
void _updateKinematics(const rigidbody::GeneralizedVelocity *Qdot, const Characteristics *characteristics, internal_forces::PathModifiers *pathModifiers=nullptr)
Actual function that implements the update of the kinematics.
MuscleGeometry DeepCopy() const
Deep copy of a muscle geometry.
const utils::Scalar & length() const
Return the previously computed muscle length.
This is the core of the musculoskeletal model in biorbd.
Definition Joints.h:48
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 Eigen Vector3d and attach it to a parent.
Definition Vector3d.h:27