Biorbd Release_1.13.0
Loading...
Searching...
No Matches
Markers.cpp
1#define BIORBD_API_EXPORTS
2#include "RigidBody/Markers.h"
3
4#include <rbdl/Model.h>
5
6#include <rbdl/Kinematics.h>
7
8#include "RigidBody/GeneralizedAcceleration.h"
9#include "RigidBody/GeneralizedCoordinates.h"
10#include "RigidBody/GeneralizedVelocity.h"
11#include "RigidBody/Joints.h"
12#include "RigidBody/NodeSegment.h"
13#include "RigidBody/Segment.h"
14#include "Utils/Error.h"
15#include "Utils/Matrix.h"
16#include "Utils/SpatialVector.h"
17#include "Utils/String.h"
18
19using namespace BIORBD_NAMESPACE;
20
22 : m_marks(std::make_shared<std::vector<rigidbody::NodeSegment>>()) {
23 // ctor
24}
25
28
32
35 copy.DeepCopy(*this);
36 return copy;
37}
38
40 m_marks->resize(other.m_marks->size());
41 for (size_t i = 0; i < other.m_marks->size(); ++i) {
42 (*m_marks)[i] = (*other.m_marks)[i].DeepCopy();
43 }
44}
45
46// Add a new marker to the markers pool
48 const rigidbody::NodeSegment &pos,
49 const utils::String &name,
50 const utils::String &parentName,
51 bool technical,
52 bool anatomical,
53 const utils::String &axesToRemove,
54 int id) {
56 pos, name, parentName, technical, anatomical, axesToRemove, id);
57 m_marks->push_back(tp);
58}
59
61 size_t index,
62 const rigidbody::NodeSegment &pos) {
63#ifndef SKIP_ASSERT
65 index < m_marks->size(),
66 utils::String("The index ") + index +
67 " is larger than the number of markers (" + m_marks->size() + ")");
68#endif // SKIP_ASSERT
69 (*m_marks)[index].setValues(pos);
70}
71
73 return (*m_marks)[idx];
74}
75
79
83 bool updateKin,
84 bool removeAxis) {
85#ifdef BIORBD_USE_CASADI_MATH
86 if (!updateKin) {
89 "When using Casadi, the marker(Q, ...) method must set "
90 "updateKin to true. ") +
91 "Alternatively, you can call the marker(updatedKinematics, Q, "
92 "...).");
93 }
94#endif
95
96 // Assuming that this is also a joint type (via BiorbdModel)
97 rigidbody::Joints &model = dynamic_cast<rigidbody::Joints &>(*this);
98#ifdef BIORBD_USE_CASADI_MATH
100#else
102#endif
103 updatedModel = model.UpdateKinematicsCustom(updateKin ? &Q : nullptr);
104 return marker(updatedModel, Q, n, removeAxis);
105}
106
108 rigidbody::Joints &updatedModel,
110 const rigidbody::NodeSegment &n,
111 bool removeAxis) {
113 Q, n.parent(), removeAxis ? n.removeAxes() : n, false));
114}
115
118 size_t idx,
119 bool updateKin,
120 bool removeAxis) {
121 return marker(Q, marker(idx), updateKin, removeAxis);
122}
123
125 rigidbody::Joints &updatedModel,
127 size_t idx,
128 bool removeAxis) {
129 return marker(updatedModel, Q, marker(idx), removeAxis);
130}
131
132// Get all the markers in the local reference
133std::vector<rigidbody::NodeSegment> rigidbody::Markers::markers(
134 bool removeAxis) const {
135 std::vector<rigidbody::NodeSegment> pos;
136 for (size_t i = 0; i < nbMarkers(); ++i) {
137 pos.push_back(removeAxis ? markerAxesRemoved(i) : marker(i));
138 }
139
140 return pos;
141}
142
143std::vector<rigidbody::NodeSegment> rigidbody::Markers::markers(
144 const utils::String &segmentName,
145 bool removeAxis) const {
146 std::vector<rigidbody::NodeSegment> out;
147 for (size_t i = 0; i < nbMarkers(); ++i) {
148 // Go through all the markers and select the right ones
149 if (!marker(i).parent().compare(segmentName)) {
150 out.push_back(removeAxis ? markerAxesRemoved(i) : marker(i));
151 }
152 }
153
154 return out;
155}
156
157std::vector<rigidbody::NodeSegment> rigidbody::Markers::markers(
158 rigidbody::Joints &updatedModel,
160 bool removeAxis) {
161 std::vector<rigidbody::NodeSegment> pos;
162 for (size_t i = 0; i < nbMarkers(); ++i) {
163 pos.push_back(marker(updatedModel, Q, i, removeAxis));
164 }
165
166 return pos;
167}
168
169std::vector<rigidbody::NodeSegment> rigidbody::Markers::markers(
171 bool updateKin,
172 bool removeAxis) {
173#ifdef BIORBD_USE_CASADI_MATH
175#else
177#endif
178 updatedModel =
179 dynamic_cast<rigidbody::Joints &>(*this).UpdateKinematicsCustom(
180 updateKin ? &Q : nullptr);
181 return markers(updatedModel, Q, removeAxis);
182}
183
184// Get a marker's velocity
186 rigidbody::Joints &updatedModel,
189 size_t idx,
190 bool removeAxis) {
191 // Calculate the velocity of the point
192 const rigidbody::NodeSegment &node(marker(idx));
193 return rigidbody::NodeSegment(updatedModel.CalcPointVelocity(
194 Q, Qdot, node.parent(), removeAxis ? node.removeAxes() : node, false));
195}
196
197// Get a marker's velocity
201 size_t idx,
202 bool updateKin,
203 bool removeAxis) {
204#ifdef BIORBD_USE_CASADI_MATH
206#else
208#endif
209 updatedModel =
210 dynamic_cast<rigidbody::Joints &>(*this).UpdateKinematicsCustom(
211 updateKin ? &Q : nullptr, updateKin ? &Qdot : nullptr);
212 return markerVelocity(updatedModel, Q, Qdot, idx, removeAxis);
213}
214
215// Get a marker's velocity
217 rigidbody::Joints &updatedModel,
220 size_t idx,
221 bool removeAxis) {
222 // Calculate the velocity of the point
224 return utils::Vector3d(updatedModel
225 .CalcPointVelocity6D(
226 Q,
227 Qdot,
228 node.parent(),
229 removeAxis ? node.removeAxes() : node,
230 false)
231 .block(0, 0, 3, 1));
232}
233
234// Get a marker's velocity
238 size_t idx,
239 bool updateKin,
240 bool removeAxis) {
241#ifdef BIORBD_USE_CASADI_MATH
243#else
245#endif
246 updatedModel =
247 dynamic_cast<rigidbody::Joints &>(*this).UpdateKinematicsCustom(
248 updateKin ? &Q : nullptr, updateKin ? &Qdot : nullptr);
249 return markerAngularVelocity(updatedModel, Q, Qdot, idx, removeAxis);
250}
251
252// Get the makers' velocities
253std::vector<rigidbody::NodeSegment> rigidbody::Markers::markersVelocity(
254 rigidbody::Joints &updatedModel,
257 bool removeAxis) {
258 std::vector<rigidbody::NodeSegment> pos;
259 for (size_t i = 0; i < nbMarkers(); ++i) {
260 pos.push_back(markerVelocity(updatedModel, Q, Qdot, i, removeAxis));
261 }
262
263 return pos;
264}
265
266// Get the makers' velocities
267std::vector<rigidbody::NodeSegment> rigidbody::Markers::markersVelocity(
270 bool updateKin,
271 bool removeAxis) {
272#ifdef BIORBD_USE_CASADI_MATH
274#else
276#endif
277 updatedModel =
278 dynamic_cast<rigidbody::Joints &>(*this).UpdateKinematicsCustom(
279 updateKin ? &Q : nullptr, updateKin ? &Qdot : nullptr);
280 return markersVelocity(updatedModel, Q, Qdot, removeAxis);
281}
282
283std::vector<rigidbody::NodeSegment> rigidbody::Markers::markersAngularVelocity(
284 rigidbody::Joints &updatedModel,
287 bool removeAxis) {
288 std::vector<rigidbody::NodeSegment> pos;
289 for (size_t i = 0; i < nbMarkers(); ++i) {
290 pos.push_back(markerAngularVelocity(updatedModel, Q, Qdot, i, removeAxis));
291 }
292 return pos;
293}
294
295std::vector<rigidbody::NodeSegment> rigidbody::Markers::markersAngularVelocity(
298 bool updateKin,
299 bool removeAxis) {
300#ifdef BIORBD_USE_CASADI_MATH
302#else
304#endif
305 updatedModel =
306 dynamic_cast<rigidbody::Joints &>(*this).UpdateKinematicsCustom(
307 updateKin ? &Q : nullptr, updateKin ? &Qdot : nullptr);
308 return markersAngularVelocity(updatedModel, Q, Qdot, removeAxis);
309}
310
312 rigidbody::Joints &updatedModel,
316 size_t idx,
317 bool removeAxis) {
318 // Calculate the acceleration of the point
319 const rigidbody::NodeSegment &node(marker(idx));
321 Q,
322 Qdot,
323 Qddot,
324 node.parent(),
325 removeAxis ? node.removeAxes() : node,
326 false));
327}
328
333 size_t idx,
334 bool updateKin,
335 bool removeAxis) {
336#ifdef BIORBD_USE_CASADI_MATH
338#else
340#endif
341 updatedModel =
342 dynamic_cast<rigidbody::Joints &>(*this).UpdateKinematicsCustom(
343 updateKin ? &Q : nullptr,
344 updateKin ? &Qdot : nullptr,
345 updateKin ? &Qddot : nullptr);
346 return markerAcceleration(updatedModel, Q, Qdot, Qddot, idx, removeAxis);
347}
348
349std::vector<rigidbody::NodeSegment> rigidbody::Markers::markerAcceleration(
350 rigidbody::Joints &updatedModel,
354 bool removeAxis) {
355 std::vector<rigidbody::NodeSegment> pos;
356 for (size_t i = 0; i < nbMarkers(); ++i) {
357 pos.push_back(
358 markerAcceleration(updatedModel, Q, Qdot, Qddot, i, removeAxis));
359 }
360
361 return pos;
362}
363
364std::vector<rigidbody::NodeSegment> rigidbody::Markers::markerAcceleration(
368 bool updateKin,
369 bool removeAxis) {
370#ifdef BIORBD_USE_CASADI_MATH
372#else
374#endif
375 updatedModel =
376 dynamic_cast<rigidbody::Joints &>(*this).UpdateKinematicsCustom(
377 updateKin ? &Q : nullptr,
378 updateKin ? &Qdot : nullptr,
379 updateKin ? &Qddot : nullptr);
380 return markerAcceleration(updatedModel, Q, Qdot, Qddot, removeAxis);
381}
382
383// Get the technical markers
384std::vector<rigidbody::NodeSegment> rigidbody::Markers::technicalMarkers(
385 rigidbody::Joints &updatedModel,
387 bool removeAxis) {
388 std::vector<rigidbody::NodeSegment> pos;
389 for (size_t i = 0; i < nbMarkers(); ++i) {
390 if (marker(i).isTechnical()) {
391 pos.push_back(marker(updatedModel, Q, i, removeAxis));
392 }
393 }
394 return pos;
395}
396
397// Get the technical markers
398std::vector<rigidbody::NodeSegment> rigidbody::Markers::technicalMarkers(
400 bool updateKin,
401 bool removeAxis) {
402#ifdef BIORBD_USE_CASADI_MATH
404#else
406#endif
407 updatedModel =
408 dynamic_cast<rigidbody::Joints &>(*this).UpdateKinematicsCustom(
409 updateKin ? &Q : nullptr);
410 return technicalMarkers(updatedModel, Q, removeAxis);
411}
412
413// Get the technical markers in a local reference
414std::vector<rigidbody::NodeSegment> rigidbody::Markers::technicalMarkers()
415 const {
416 std::vector<rigidbody::NodeSegment> out;
417 for (size_t i = 0; i < nbMarkers(); ++i) {
418 if (marker(i).isTechnical()) out.push_back(marker(i));
419 }
420
421 return out;
422}
423
424// Get the technical markers in a local reference
425std::vector<rigidbody::NodeSegment>
427 std::vector<rigidbody::NodeSegment> out;
428 for (size_t i = 0; i < nbMarkers(); ++i) {
429 if (marker(i).isTechnical()) out.push_back(markerAxesRemoved(i));
430 }
431
432 return out;
433}
434
435std::vector<rigidbody::NodeSegment> rigidbody::Markers::anatomicalMarkers(
436 rigidbody::Joints &updatedModel,
438 bool removeAxis) {
439 std::vector<rigidbody::NodeSegment> out;
440 for (size_t i = 0; i < nbMarkers(); ++i) {
441 if (marker(i).isAnatomical()) {
442 out.push_back(marker(updatedModel, Q, i, removeAxis));
443 }
444 }
445 return out;
446}
447
448std::vector<rigidbody::NodeSegment> rigidbody::Markers::anatomicalMarkers(
450 bool updateKin,
451 bool removeAxis) {
452#ifdef BIORBD_USE_CASADI_MATH
454#else
456#endif
457 updatedModel =
458 dynamic_cast<rigidbody::Joints &>(*this).UpdateKinematicsCustom(
459 updateKin ? &Q : nullptr);
460 return anatomicalMarkers(updatedModel, Q, removeAxis);
461}
462
463// Get the anatomical markers in a local reference
464std::vector<rigidbody::NodeSegment> rigidbody::Markers::anatomicalMarkers()
465 const {
466 std::vector<rigidbody::NodeSegment> out;
467 for (size_t i = 0; i < nbMarkers(); ++i) {
468 if (marker(i).isAnatomical()) out.push_back(marker(i));
469 }
470 return out;
471}
472
473// Get the anatomical markers in a local reference
474std::vector<rigidbody::NodeSegment>
476 std::vector<rigidbody::NodeSegment> out;
477 for (size_t i = 0; i < nbMarkers(); ++i) {
478 if (marker(i).isAnatomical()) out.push_back(markerAxesRemoved(i));
479 }
480 return out;
481}
482
483std::vector<rigidbody::NodeSegment> rigidbody::Markers::segmentMarkers(
484 rigidbody::Joints &updatedModel,
486 size_t idx,
487 bool removeAxis) {
488 // Name of the segment to find
489 const utils::String &name(updatedModel.segment(idx).name());
490
491 std::vector<rigidbody::NodeSegment> out;
492 for (size_t i = 0; i < nbMarkers();
493 ++i) { // Go through all the markers and select the right ones
494 if (!(*m_marks)[i].parent().compare(name)) {
495 out.push_back(marker(updatedModel, Q, i, removeAxis));
496 }
497 }
498
499 return out;
500}
501
502std::vector<rigidbody::NodeSegment> rigidbody::Markers::segmentMarkers(
504 size_t idx,
505 bool updateKin,
506 bool removeAxis) {
507#ifdef BIORBD_USE_CASADI_MATH
509#else
511#endif
512 updatedModel =
513 dynamic_cast<rigidbody::Joints &>(*this).UpdateKinematicsCustom(
514 updateKin ? &Q : nullptr);
515 return segmentMarkers(updatedModel, Q, idx, removeAxis);
516}
517
518size_t rigidbody::Markers::nbMarkers() const { return m_marks->size(); }
519
520size_t rigidbody::Markers::nbMarkers(size_t idxSegment) const {
521 // Assuming that this is also a joint type (via BiorbdModel)
522 const rigidbody::Joints &model =
523 dynamic_cast<const rigidbody::Joints &>(*this);
524
525 // Name of the segment to find
526 const utils::String &name(model.segment(idxSegment).name());
527
528 size_t n = 0;
529 for (size_t i = 0; i < nbMarkers();
530 ++i) // Go through all the markers and select the right ones
531 if (!(*m_marks)[i].parent().compare(name)) {
532 ++n;
533 }
534
535 return n;
536}
537
538std::vector<utils::Matrix> rigidbody::Markers::markersJacobian(
539 rigidbody::Joints &updatedModel,
541 bool removeAxis) {
542 return markersJacobian(updatedModel, Q, false, removeAxis);
543}
544
545std::vector<utils::Matrix> rigidbody::Markers::markersJacobian(
547 bool updateKin,
548 bool removeAxis) {
549#ifdef BIORBD_USE_CASADI_MATH
551#else
553#endif
554 updatedModel =
555 dynamic_cast<rigidbody::Joints &>(*this).UpdateKinematicsCustom(
556 updateKin ? &Q : nullptr);
557
558 return markersJacobian(updatedModel, Q, false, removeAxis);
559}
560
562 rigidbody::Joints &updatedModel,
564 bool removeAxis) {
565 return markersJacobian(updatedModel, Q, true, removeAxis);
566}
567
570 bool updateKin,
571 bool removeAxis) {
572#ifdef BIORBD_USE_CASADI_MATH
574#else
576#endif
577 updatedModel =
578 dynamic_cast<rigidbody::Joints &>(*this).UpdateKinematicsCustom(
579 updateKin ? &Q : nullptr);
580 return markersJacobian(updatedModel, Q, true, removeAxis);
581}
582
584 rigidbody::Joints &updatedModel,
586 const utils::String &parentName,
587 const rigidbody::NodeSegment &p,
588 bool removeAxis) {
589 // Calculate the Jacobien of this Tag
590 return updatedModel.CalcPointJacobian(
591 Q, parentName, removeAxis ? p.removeAxes() : p, false);
592}
593
596 const utils::String &parentName,
597 const rigidbody::NodeSegment &p,
598 bool updateKin,
599 bool removeAxis) {
600#ifdef BIORBD_USE_CASADI_MATH
602#else
604#endif
605 updatedModel =
606 dynamic_cast<rigidbody::Joints &>(*this).UpdateKinematicsCustom(
607 updateKin ? &Q : nullptr);
608 return markersJacobian(updatedModel, Q, parentName, p, removeAxis);
609}
610
611#ifndef BIORBD_USE_CASADI_MATH
613 const std::vector<rigidbody::NodeSegment> &markers,
616 bool removeAxes) {
617 // also assert for dimensions of Q. Warning: this call would not work with
618 // casadi
619 rigidbody::Joints &updatedModel =
620 dynamic_cast<rigidbody::Joints &>(*this).UpdateKinematicsCustom(&Q);
621
622 // Find the technical markers only (bodyPoint)
623 std::vector<rigidbody::NodeSegment> bodyPoint(technicalMarkers(removeAxes));
624 std::vector<RigidBodyDynamics::Math::Vector3d> bodyPointEigen;
625 for (size_t i = 0; i < bodyPoint.size(); ++i) {
626 bodyPointEigen.push_back(bodyPoint[i]);
627 }
628
629 std::vector<RigidBodyDynamics::Math::Vector3d> markersInRbdl;
630 for (size_t i = 0; i < markers.size(); ++i) {
631 markersInRbdl.push_back(markers[i]);
632 }
633
634 // Associate the body number to each technical marker (bodyPoint)
635 std::vector<unsigned int> bodyId;
636 for (size_t i = 0; i < bodyPoint.size(); ++i) {
637 bodyId.push_back(
638 static_cast<size_t>((*(bodyPoint.begin() + i)).parentId()));
639 }
640
641 // Call the base function
642 return RigidBodyDynamics::InverseKinematics(
643 updatedModel, Qinit, bodyId, bodyPointEigen, markersInRbdl, Q);
644}
645#endif
646
647// Get the Jacobian of the technical markers
648std::vector<utils::Matrix> rigidbody::Markers::markersJacobian(
649 rigidbody::Joints &updatedModel,
651 bool lookForTechnical,
652 bool removeAxis) {
653 std::vector<utils::Matrix> G;
654
655 for (size_t idx = 0; idx < nbMarkers(); ++idx) {
656 // Actual marker
657 const rigidbody::NodeSegment &node(marker(idx));
658 if (lookForTechnical && !node.isTechnical()) {
659 continue;
660 }
661
662 // Calculate the Jacobian of this Tag
663 G.push_back(updatedModel.CalcPointJacobian(
664 Q, node.parent(), removeAxis ? node.removeAxes() : node, false));
665 }
666
667 return G;
668}
669
671 size_t nTechMarkers = 0;
672 if (nTechMarkers == 0) // If the function has never been called before
673 for (auto mark : *m_marks)
674 if (mark.isTechnical()) {
675 ++nTechMarkers;
676 }
677
678 return nTechMarkers;
679}
680
681size_t rigidbody::Markers::nbTechnicalMarkers(size_t idxSegment) {
682 // Assuming that this is also a joint type (via BiorbdModel)
683 rigidbody::Joints &model = dynamic_cast<rigidbody::Joints &>(*this);
684
685 size_t nTechMarkers = 0;
686
687 // Name of the segment to find
688 const utils::String &name(model.segment(idxSegment).name());
689
690 if (nTechMarkers == 0) // If the function has never been called before
691 for (auto mark : *m_marks)
692 if (mark.isTechnical() && !mark.parent().compare(name)) {
693 ++nTechMarkers;
694 }
695
696 return nTechMarkers;
697}
698
700 size_t nAnatMarkers = 0;
701 if (nAnatMarkers == 0) // If the function has never been called before
702 for (auto mark : *m_marks)
703 if (mark.isAnatomical()) {
704 ++nAnatMarkers;
705 }
706
707 return nAnatMarkers;
708}
709
710std::vector<utils::String> rigidbody::Markers::markerNames() const {
711 // Extract the name of all the markers of a model
712 std::vector<utils::String> names;
713 for (size_t i = 0; i < nbMarkers(); ++i) {
714 names.push_back(marker(i).utils::Node::name());
715 }
716
717 return names;
718}
719
720std::vector<utils::String> rigidbody::Markers::technicalMarkerNames() const {
721 // Extract the name of all the technical markers of a model
722 std::vector<utils::String> names;
723 for (size_t i = 0; i < nbMarkers(); ++i)
724 if (marker(i).isTechnical()) {
725 names.push_back(marker(i).utils::Node::name());
726 }
727
728 return names;
729}
730
731std::vector<utils::String> rigidbody::Markers::anatomicalMarkerNames() const {
732 // Extract the names of all the anatomical markers of a model
733 std::vector<utils::String> names;
734 for (size_t i = 0; i < nbMarkers(); ++i)
735 if (marker(i).isAnatomical()) {
736 names.push_back(marker(i).utils::Node::name());
737 }
738
739 return names;
740}
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 CalcPointAcceleration(const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, const rigidbody::GeneralizedAcceleration &Qddot, utils::String segmentName, const utils::Vector3d &pointInLocal, bool updateKin=true)
Interface to call CalcPointAcceleration. It is pretty much useless in Eigen, but fixes an issue in Ca...
Definition Joints.cpp:471
Segment & segment(size_t idx)
Get a segment of index idx.
Definition Joints.cpp:205
utils::Vector3d CalcPointVelocity(const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, utils::String segmentName, const utils::Vector3d &pointInLocal, bool updateKin=true)
Interface to call CalcPointVelocity. It is pretty much useless in Eigen, but fixes an issue in Casadi...
Definition Joints.cpp:415
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
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
Holder for the marker set.
Definition Markers.h:26
const NodeSegment & marker(size_t idx) const
Return a marker of index idx in the marker set.
Definition Markers.cpp:72
std::vector< NodeSegment > anatomicalMarkers(rigidbody::Joints &updatedModel, const GeneralizedCoordinates &Q, bool removeAxis=true)
Return all the anatomical markers at a given Q in the global reference frame.
Definition Markers.cpp:435
NodeSegment markerVelocity(rigidbody::Joints &updatedModel, const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, size_t idx, bool removeAxis=true)
Return the linear velocity of a marker.
Definition Markers.cpp:185
virtual ~Markers()
Destroy class properly.
Definition Markers.cpp:29
std::vector< NodeSegment > technicalMarkersAxesRemoved() const
Return the technical markers in their respective parent reference frame.
Definition Markers.cpp:426
std::vector< NodeSegment > segmentMarkers(const GeneralizedCoordinates &Q, size_t idx, bool updateKin=true, bool removeAxis=true)
Return all the markers of the segment idx at a given Q in the global reference frame.
Definition Markers.cpp:502
void setMarker(size_t index, const NodeSegment &pos)
Change the values of a specific marker without changing other parameters.
Definition Markers.cpp:60
std::vector< NodeSegment > markersVelocity(rigidbody::Joints &updatedModel, const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, bool removeAxis=true)
Return the linear velocity of all the markers.
Definition Markers.cpp:253
NodeSegment markerAngularVelocity(rigidbody::Joints &updatedModel, const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, size_t idx, bool removeAxis=true)
Return the angular velocity of a marker.
Definition Markers.cpp:216
void addMarker(const NodeSegment &pos, const utils::String &name, const utils::String &parentName, bool technical, bool anatomical, const utils::String &axesToRemove, int id=-1)
Add a marker to the set.
Definition Markers.cpp:47
std::vector< utils::String > anatomicalMarkerNames() const
Return the names of all the anatomical markers.
Definition Markers.cpp:731
std::vector< NodeSegment > anatomicalMarkersAxesRemoved() const
Return the anatomical markers in their respective parent reference frame.
Definition Markers.cpp:475
std::vector< NodeSegment > technicalMarkers() const
Return the technical markers in their respective parent reference frame.
Definition Markers.cpp:414
std::vector< NodeSegment > anatomicalMarkers() const
Return the anatomical markers in their respective parent reference frame.
Definition Markers.cpp:464
Markers()
Construct a marker set.
Definition Markers.cpp:21
bool inverseKinematics(const std::vector< NodeSegment > &markers, const GeneralizedCoordinates &Qinit, GeneralizedCoordinates &Q, bool removeAxis=true)
Performs an inverse kinematics.
Definition Markers.cpp:612
std::vector< utils::String > technicalMarkerNames() const
Return the names of all the technical markers.
Definition Markers.cpp:720
std::vector< NodeSegment > technicalMarkers(rigidbody::Joints &updatedModel, const GeneralizedCoordinates &Q, bool removeAxis=true)
Return all the technical markers at a given Q in the global reference frame.
Definition Markers.cpp:384
std::vector< NodeSegment > markersAngularVelocity(rigidbody::Joints &updatedModel, const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, bool removeAxis=true)
Return the angular velocity of all the markers.
Definition Markers.cpp:283
std::vector< NodeSegment > markers(bool removeAxis=true) const
Return a copy of all the markers in their respective parent reference frame.
Definition Markers.cpp:133
std::vector< utils::Matrix > markersJacobian(rigidbody::Joints &updatedModel, const GeneralizedCoordinates &Q, bool removeAxis=true)
Return the jacobian of the markers.
Definition Markers.cpp:538
std::vector< NodeSegment > segmentMarkers(rigidbody::Joints &updatedModel, const GeneralizedCoordinates &Q, size_t idx, bool removeAxis=true)
Return all the markers of the segment idx at a given Q in the global reference frame.
Definition Markers.cpp:483
Markers DeepCopy() const
Deep copy of the markers.
Definition Markers.cpp:33
std::vector< utils::String > markerNames() const
Return the names of all the markers.
Definition Markers.cpp:710
size_t nbTechnicalMarkers()
Return the number of technical markers.
Definition Markers.cpp:670
size_t nbAnatomicalMarkers()
Return the number of anatomical markers.
Definition Markers.cpp:699
size_t nbMarkers() const
Return the number of markers.
Definition Markers.cpp:518
std::shared_ptr< std::vector< NodeSegment > > m_marks
The markers.
Definition Markers.h:664
NodeSegment markerAcceleration(rigidbody::Joints &updatedModel, const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, const rigidbody::GeneralizedAcceleration &Qddot, size_t idx, bool removeAxis=true)
Return the acceleration of a marker.
Definition Markers.cpp:311
std::vector< utils::Matrix > technicalMarkersJacobian(rigidbody::Joints &updatedModel, const GeneralizedCoordinates &Q, bool removeAxis=true)
Return the jacobian of the technical markers.
Definition Markers.cpp:561
NodeSegment markerAxesRemoved(size_t idx) const
Return a marker of index idx in the marker set with the axes removed.
Definition Markers.cpp:76
A point attached to a segment, generally speaking a skin marker.
Definition NodeSegment.h:20
bool isTechnical() const
Return if node is technical.
NodeSegment removeAxes() const
To remove axis.
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
const String & name() const
Return the name of the node.
Definition Node.cpp:43
const String & parent() const
Return the parent name of the node.
Definition Node.cpp:45
Wrapper around the std::string class with augmented functionality.
Definition String.h:18
Wrapper around Eigen Vector3d and attach it to a parent.
Definition Vector3d.h:27