Biorbd Release_1.13.0
Loading...
Searching...
No Matches
Geometry.cpp
1#define BIORBD_API_EXPORTS
2
3#include "InternalForces/Geometry.h"
4
5#include <rbdl/Model.h>
6
7#include <rbdl/Kinematics.h>
8
9#include "InternalForces/PathModifiers.h"
10#include "InternalForces/ViaPoint.h"
11#include "InternalForces/WrappingObject.h"
12#include "RigidBody/GeneralizedCoordinates.h"
13#include "RigidBody/GeneralizedVelocity.h"
14#include "RigidBody/Joints.h"
15#include "RigidBody/NodeSegment.h"
16#include "Utils/Error.h"
17#include "Utils/Matrix.h"
18#include "Utils/RotoTrans.h"
19
20using namespace BIORBD_NAMESPACE;
21
23 : m_origin(std::make_shared<utils::Vector3d>()),
24 m_insertion(std::make_shared<utils::Vector3d>()),
26 std::make_shared<utils::Vector3d>(utils::Vector3d::Zero())),
28 std::make_shared<utils::Vector3d>(utils::Vector3d::Zero())),
29 m_pointsInGlobal(std::make_shared<std::vector<utils::Vector3d>>()),
30 m_pointsInLocal(std::make_shared<std::vector<utils::Vector3d>>()),
31 m_jacobian(std::make_shared<utils::Matrix>()),
32 m_jacobianLength(std::make_shared<utils::Matrix>()),
33 m_length(std::make_shared<utils::Scalar>(0)),
34 m_velocity(std::make_shared<utils::Scalar>(0)),
35 m_isGeometryComputed(std::make_shared<bool>(false)),
36 m_isVelocityComputed(std::make_shared<bool>(false)),
37 m_posAndJacoWereForced(std::make_shared<bool>(false)) {}
38
40 const utils::Vector3d &origin,
41 const utils::Vector3d &insertion)
42 : m_origin(std::make_shared<utils::Vector3d>(origin)),
43 m_insertion(std::make_shared<utils::Vector3d>(insertion)),
45 std::make_shared<utils::Vector3d>(utils::Vector3d::Zero())),
47 std::make_shared<utils::Vector3d>(utils::Vector3d::Zero())),
48 m_pointsInGlobal(std::make_shared<std::vector<utils::Vector3d>>()),
49 m_pointsInLocal(std::make_shared<std::vector<utils::Vector3d>>()),
50 m_jacobian(std::make_shared<utils::Matrix>()),
51 m_jacobianLength(std::make_shared<utils::Matrix>()),
52 m_length(std::make_shared<utils::Scalar>(0)),
53 m_velocity(std::make_shared<utils::Scalar>(0)),
54 m_isGeometryComputed(std::make_shared<bool>(false)),
55 m_isVelocityComputed(std::make_shared<bool>(false)),
56 m_posAndJacoWereForced(std::make_shared<bool>(false)) {}
57
63
65 const internal_forces::Geometry &other) {
66 *m_origin = other.m_origin->DeepCopy();
67 *m_insertion = other.m_insertion->DeepCopy();
68 *m_originInGlobal = other.m_originInGlobal->DeepCopy();
69 *m_insertionInGlobal = other.m_insertionInGlobal->DeepCopy();
70 m_pointsInGlobal->resize(other.m_pointsInGlobal->size());
71 for (size_t i = 0; i < other.m_pointsInGlobal->size(); ++i) {
72 (*m_pointsInGlobal)[i] = (*other.m_pointsInGlobal)[i].DeepCopy();
73 }
74 m_pointsInLocal->resize(other.m_pointsInLocal->size());
75 for (size_t i = 0; i < other.m_pointsInLocal->size(); ++i) {
76 (*m_pointsInLocal)[i] = (*other.m_pointsInLocal)[i].DeepCopy();
77 }
78 *m_jacobian = *other.m_jacobian;
80 *m_length = *other.m_length;
81 *m_velocity = *other.m_velocity;
85}
86
87// ------ PUBLIC FUNCTIONS ------ //
89 rigidbody::Joints &updatedModel,
93
94 // Position of the points in space
95 setPointsInGlobal(updatedModel, *Q);
96
97 // Compute the Jacobian of the muscle points
98 jacobian(updatedModel, *Q);
99
100 // Complete the update
101 _updateKinematics(Qdot, nullptr);
102}
103
105 rigidbody::Joints &updatedModel,
106 internal_forces::PathModifiers &pathModifiers,
108 const rigidbody::GeneralizedVelocity *Qdot) {
109 *m_posAndJacoWereForced = false;
110 // Position of the points in space
111 setPointsInGlobal(updatedModel, *Q, &pathModifiers);
112
113 // Compute the Jacobian of the muscle points
114 jacobian(updatedModel, *Q);
115
116 // Complete the update
117 _updateKinematics(Qdot, &pathModifiers);
118}
119
121 std::vector<utils::Vector3d> &pointsInGlobal,
122 utils::Matrix &jacoPointsInGlobal,
123 const rigidbody::GeneralizedVelocity *Qdot) {
125
126 // Position of the points in space
128
129 // Compute the Jacobian of the muscle points
130 jacobian(jacoPointsInGlobal);
131
132 // Complete the update
133 _updateKinematics(Qdot, nullptr);
134}
135
136// Get and set the positions of the origins and insertions
138 if (dynamic_cast<const rigidbody::NodeSegment *>(&position)) {
139 *m_origin = position;
140 } else {
141 // Preserve the Node information
142 m_origin->RigidBodyDynamics::Math::Vector3d::operator=(position);
143 }
144}
148
150 const utils::Vector3d &position) {
151 if (dynamic_cast<const rigidbody::NodeSegment *>(&position)) {
152 *m_insertion = position;
153 } else {
154 // Preserve the Node information
155 m_insertion->RigidBodyDynamics::Math::Vector3d::operator=(position);
156 }
157}
161
162// Position of the muscles in space
166 "Geometry must be computed at least once before calling originInLocal()");
167 return *m_originInGlobal;
168}
172 "Geometry must be computed at least once before calling "
173 "insertionInGlobal()");
174 return *m_insertionInGlobal;
175}
176const std::vector<utils::Vector3d> &internal_forces::Geometry::pointsInGlobal()
177 const {
180 "Geometry must be computed at least once before calling "
181 "musclesPointsInGlobal()");
182 return *m_pointsInGlobal;
183}
184
185// Return the length and muscular velocity
186const utils::Scalar &internal_forces::Geometry::length() const {
189 "Geometry must be computed at least before calling length()");
190 return *m_length;
191}
192
193const utils::Scalar &internal_forces::Geometry::velocity() const {
196 "Geometry must be computed before calling velocity()");
197 return *m_velocity;
198}
199
200// Return the Jacobian
204 "Geometry must be computed before calling jacobian()");
205 return *m_jacobian;
206} // Return the last Jacobian
210 "Geometry must be computed before calling jacobianOrigin()");
211 return m_jacobian->block(0, 0, 3, m_jacobian->cols());
212}
216 "Geometry must be computed before calling jacobianInsertion()");
217 return m_jacobian->block(m_jacobian->rows() - 3, 0, 3, m_jacobian->cols());
218}
222 "Geometry must be computed before calling jacobian(i)");
223 return m_jacobian->block(
224 3 * static_cast<unsigned int>(idxViaPoint), 0, 3, m_jacobian->cols());
225}
226
230 "Geometry must be computed before calling jacobianLength()");
231 return *m_jacobianLength;
232}
233
234// --------------------------------------- //
235
238 internal_forces::PathModifiers *pathModifiers) {
239 // Compute the length and velocities
240 length(pathModifiers);
241 *m_isGeometryComputed = true;
242
243 // Compute the jacobian of the lengths
245 if (Qdot != nullptr) {
246 velocity(*Qdot);
247 *m_isVelocityComputed = true;
248 } else {
249 *m_isVelocityComputed = false;
250 }
251}
252
254 rigidbody::Joints &model,
256 // Return the position of the marker in function of the given position
257 m_originInGlobal->block(0, 0, 3, 1) =
258 model.CalcBodyToBaseCoordinates(Q, m_origin->parent(), *m_origin, false);
259 return *m_originInGlobal;
260}
261
263 rigidbody::Joints &model,
265 // Return the position of the marker in function of the given position
266 m_insertionInGlobal->block(0, 0, 3, 1) = model.CalcBodyToBaseCoordinates(
267 Q, m_insertion->parent(), *m_insertion, false);
268 return *m_insertionInGlobal;
269}
270
272 std::vector<utils::Vector3d> &ptsInGlobal) {
274 ptsInGlobal.size() >= 2,
275 "ptsInGlobal must at least have an origin and an insertion");
277 ->clear(); // In this mode, we don't need the local, because the Jacobian
278 // of the points has to be given as well
279 *m_pointsInGlobal = ptsInGlobal;
280}
281
283 rigidbody::Joints &model,
285 internal_forces::PathModifiers *pathModifiers) {
286 // Output varible (reset to zero)
287 m_pointsInLocal->clear();
288 m_pointsInGlobal->clear();
289
290 // Do not apply on wrapping objects
291 if (pathModifiers->nbWraps() != 0) {
292 // CHECK TO MODIFY BEFOR GOING FORWARD WITH PROJECTS
294 pathModifiers->nbVia() == 0, "Cannot mix wrapping and via points yet");
296 pathModifiers->nbWraps() < 2,
297 "Cannot compute more than one wrapping yet");
298
299 // Get the matrix of Rt of the wrap
301 static_cast<internal_forces::WrappingObject &>(
302 pathModifiers->object(0));
303 const utils::RotoTrans &RT = w.RT(model, Q, false);
304
305 // Alias
306 const utils::Vector3d &po_mus = originInGlobal(model, Q); // Origin on bone
307 const utils::Vector3d &pi_mus =
308 insertionInGlobal(model, Q); // Insertion on bone
309
310 utils::Vector3d pi_wrap(
311 0,
312 0,
313 0); // point on the wrapping related to insertion
314 utils::Vector3d po_wrap(
315 0,
316 0,
317 0); // point on the wrapping related to origin
318
319 utils::Scalar a; // Force the computation of the length
320 w.wrapPoints(RT, po_mus, pi_mus, po_wrap, pi_wrap, &a);
321
322 // Store the points in local
323 m_pointsInLocal->push_back(originInLocal());
324 m_pointsInLocal->push_back(
326 model.CalcBodyToBaseCoordinates(Q, w.parent(), po_wrap, false),
327 "wrap_o",
328 w.parent()));
329 m_pointsInLocal->push_back(
331 model.CalcBodyToBaseCoordinates(Q, w.parent(), pi_wrap, false),
332 "wrap_i",
333 w.parent()));
334 m_pointsInLocal->push_back(insertionInLocal());
335
336 // Store the points in global
337 m_pointsInGlobal->push_back(po_mus);
338 m_pointsInGlobal->push_back(po_wrap);
339 m_pointsInGlobal->push_back(pi_wrap);
340 m_pointsInGlobal->push_back(pi_mus);
341
342 }
343
344 else if (
345 pathModifiers->nbObjects() != 0 &&
346 pathModifiers->object(0).typeOfNode() == utils::NODE_TYPE::VIA_POINT) {
347 m_pointsInLocal->push_back(originInLocal());
348 m_pointsInGlobal->push_back(originInGlobal(model, Q));
349 for (size_t i = 0; i < pathModifiers->nbObjects(); ++i) {
350 const internal_forces::ViaPoint &node(
351 static_cast<internal_forces::ViaPoint &>(pathModifiers->object(i)));
352 m_pointsInLocal->push_back(node);
353 m_pointsInGlobal->push_back(
354 model.CalcBodyToBaseCoordinates(Q, node.parent(), node, false));
355 }
356 m_pointsInLocal->push_back(insertionInLocal());
357 m_pointsInGlobal->push_back(insertionInGlobal(model, Q));
358
359 } else if (pathModifiers->nbObjects() == 0) {
360 m_pointsInLocal->push_back(originInLocal());
361 m_pointsInLocal->push_back(insertionInLocal());
362 m_pointsInGlobal->push_back(originInGlobal(model, Q));
363 m_pointsInGlobal->push_back(insertionInGlobal(model, Q));
364 } else {
365 utils::Error::raise("Length for this type of object was not implemented");
366 }
367
368 // Set the dimension of jacobian
370}
371
373 internal_forces::PathModifiers *pathModifiers) {
374 *m_length = 0;
375
376 // because we can't combine, test the first (0) will let us know all the types
377 // if more than one
378 if (pathModifiers != nullptr && pathModifiers->nbWraps() != 0) {
380 pathModifiers->nbVia() == 0, "Cannot mix wrapping and via points yet");
382 pathModifiers->nbWraps() < 2,
383 "Cannot compute more than one wrapping yet");
384
385 utils::Vector3d pi_wrap(
386 0,
387 0,
388 0); // point on the wrapping related to insertion
389 utils::Vector3d po_wrap(
390 0,
391 0,
392 0); // point on the wrapping related to origin
393 utils::Scalar lengthWrap(0);
394 static_cast<internal_forces::WrappingObject &>(pathModifiers->object(0))
395 .wrapPoints(po_wrap, pi_wrap, &lengthWrap);
396 *m_length =
397 ((*m_pointsInGlobal)[0] - po_wrap).norm() + // length before the wrap
398 lengthWrap + // length on the wrap
399 ((*m_pointsInGlobal)[3] - pi_wrap).norm(); // length after the wrap
400
401 } else {
402 for (size_t i = 0; i < m_pointsInGlobal->size() - 1; ++i) {
403 *m_length += ((*m_pointsInGlobal)[i + 1] - (*m_pointsInGlobal)[i]).norm();
404 }
405 }
406 return *m_length;
407}
408
410 const rigidbody::GeneralizedVelocity &Qdot) {
411 // Compute the velocity of the muscular elongation
412 *m_velocity = (jacobianLength() * Qdot)[0];
413 return *m_velocity;
414}
415
417 *m_jacobian = utils::Matrix::Zero(
418 static_cast<unsigned int>(m_pointsInLocal->size() * 3), model.dof_count);
419}
420
423 jaco.rows() / 3 == static_cast<int>(m_pointsInGlobal->size()),
424 "Jacobian is the wrong size");
425 *m_jacobian = jaco;
426}
427
429 rigidbody::Joints &model,
431 // jacobian is a protected method, so all method that calls it is expected to
432 // have updated the kinematics if required
433 bool updateKin = false;
434 for (size_t i = 0; i < m_pointsInLocal->size(); ++i) {
435 m_jacobian->block(3 * static_cast<unsigned int>(i), 0, 3, model.dof_count) =
436 model.CalcPointJacobian(
437 Q,
438 (*m_pointsInLocal)[i].parent(),
439 (*m_pointsInLocal)[i],
440 updateKin);
441 }
442}
443
445 *m_jacobianLength = utils::Matrix::Zero(1, m_jacobian->cols());
446
447 // jacobian approximates as if there were no wrapping object
448 const std::vector<utils::Vector3d> &p = *m_pointsInGlobal;
449 for (size_t i = 0; i < p.size() - 1; ++i) {
451 ((p[i + 1] - p[i]).transpose() * (jacobian(i + 1) - jacobian(i))) /
452 (p[i + 1] - p[i]).norm();
453 }
454}
Class Geometry of the muscle.
Definition Geometry.h:31
std::shared_ptr< utils::Matrix > m_jacobianLength
The muscle length jacobian.
Definition Geometry.h:310
std::shared_ptr< utils::Vector3d > m_insertion
Insertion node.
Definition Geometry.h:297
utils::Matrix jacobianOrigin() const
Return the gradient of the jacobian for the origin node.
Definition Geometry.cpp:207
const utils::Vector3d & insertionInGlobal() const
Return the insertion position in the global reference.
Definition Geometry.cpp:169
utils::Matrix jacobianInsertion() const
Return the gradient of the jacobian for the insertion node.
Definition Geometry.cpp:213
const utils::Scalar & velocity() const
Return the previously computed velocity.
Definition Geometry.cpp:193
std::shared_ptr< bool > m_isGeometryComputed
Definition Geometry.h:316
std::shared_ptr< utils::Matrix > m_jacobian
The jacobian matrix.
Definition Geometry.h:308
std::shared_ptr< utils::Vector3d > m_originInGlobal
Position of the origin in the global reference.
Definition Geometry.h:300
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
const utils::Vector3d & insertionInLocal() const
Return the insertion position.
Definition Geometry.cpp:158
Geometry DeepCopy() const
Deep copy of a geometry.
Definition Geometry.cpp:58
std::shared_ptr< utils::Scalar > m_length
length
Definition Geometry.h:312
const std::vector< utils::Vector3d > & pointsInGlobal() const
Return the muscles points in the global reference.
Definition Geometry.cpp:176
void setInsertionInLocal(const utils::Vector3d &position)
Set the insertion position.
Definition Geometry.cpp:149
std::shared_ptr< utils::Scalar > m_velocity
Velocity of the muscular elongation.
Definition Geometry.h:314
std::shared_ptr< utils::Vector3d > m_insertionInGlobal
Definition Geometry.h:302
void setJacobianDimension(rigidbody::Joints &model)
Set the jacobian dimensions.
Definition Geometry.cpp:416
const utils::Matrix & jacobian() const
Return the previously computed muscle jacobian.
Definition Geometry.cpp:201
void _updateKinematics(const rigidbody::GeneralizedVelocity *Qdot, internal_forces::PathModifiers *pathModifiers=nullptr)
Actual function that implements the update of the kinematics.
Definition Geometry.cpp:236
std::shared_ptr< std::vector< utils::Vector3d > > m_pointsInLocal
Position of all the points in local.
Definition Geometry.h:307
const utils::Vector3d & originInGlobal() const
Return the origin position in the global reference.
Definition Geometry.cpp:163
const utils::Matrix & jacobianLength() const
Return the jacobian length of the muscle.
Definition Geometry.cpp:227
void updateKinematics(rigidbody::Joints &updatedModel, const rigidbody::GeneralizedCoordinates *Q=nullptr, const rigidbody::GeneralizedVelocity *Qdot=nullptr)
Updates the position and dynamic elements of the muscles.
Definition Geometry.cpp:88
void setOrigin(const utils::Vector3d &position)
Set the origin position in the local reference frame of the muscle.
Definition Geometry.cpp:137
std::shared_ptr< bool > m_isVelocityComputed
Definition Geometry.h:318
std::shared_ptr< utils::Vector3d > m_origin
Origin node.
Definition Geometry.h:296
const utils::Scalar & length() const
Return the previously computed muscle length.
Definition Geometry.cpp:186
const utils::Vector3d & originInLocal() const
Return the origin position.
Definition Geometry.cpp:145
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.
size_t nbObjects() const
Return the total number of path modifier 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 const utils::RotoTrans & RT(rigidbody::Joints &model, const rigidbody::GeneralizedCoordinates &Q, bool updateKin=true)=0
Return the RotoTrans matrix of the wrapping object.
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...
This is the core of the musculoskeletal model in biorbd.
Definition Joints.h:48
utils::Matrix CalcPointJacobian(const rigidbody::GeneralizedCoordinates &Q, utils::String segmentName, const utils::Vector3d &pointInLocal, bool updateKin=true)
Interface to call CalcPointJacobian. Instead of sending the jacobian as an parameter,...
Definition Joints.cpp:508
utils::Vector3d CalcBodyToBaseCoordinates(const rigidbody::GeneralizedCoordinates &Q, utils::String segmentName, const utils::Vector3d &pointInLocal, bool updateKin=true)
Interface to call CalcBodyToBaseCoordinates. It is pretty much useless in Eigen, but fixes an issue i...
Definition Joints.cpp:390
A point attached to a segment, generally speaking a skin marker.
Definition NodeSegment.h:20
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
NODE_TYPE typeOfNode() const
Return the type of node.
Definition Node.cpp:49
const String & parent() const
Return the parent name of the node.
Definition Node.cpp:45
Homogenous matrix to describe translations and rotations simultaneously.
Definition RotoTrans.h:33
Wrapper around Eigen Vector3d and attach it to a parent.
Definition Vector3d.h:27