1#define BIORBD_API_EXPORTS
2#include "RigidBody/Markers.h"
6#include <rbdl/Kinematics.h>
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"
19using namespace BIORBD_NAMESPACE;
41 for (
size_t i = 0; i < other.
m_marks->size(); ++i) {
56 pos, name, parentName, technical, anatomical, axesToRemove,
id);
65 index < m_marks->size(),
67 " is larger than the number of markers (" +
m_marks->size() +
")");
69 (*m_marks)[index].setValues(pos);
85#ifdef BIORBD_USE_CASADI_MATH
89 "When using Casadi, the marker(Q, ...) method must set "
90 "updateKin to true. ") +
91 "Alternatively, you can call the marker(updatedKinematics, Q, "
98#ifdef BIORBD_USE_CASADI_MATH
104 return marker(updatedModel, Q, n, removeAxis);
129 return marker(updatedModel, Q,
marker(idx), removeAxis);
134 bool removeAxis)
const {
135 std::vector<rigidbody::NodeSegment> pos;
136 for (
size_t i = 0; i <
nbMarkers(); ++i) {
145 bool removeAxis)
const {
146 std::vector<rigidbody::NodeSegment> out;
147 for (
size_t i = 0; i <
nbMarkers(); ++i) {
149 if (!
marker(i).parent().compare(segmentName)) {
161 std::vector<rigidbody::NodeSegment> pos;
162 for (
size_t i = 0; i <
nbMarkers(); ++i) {
163 pos.push_back(
marker(updatedModel, Q, i, removeAxis));
173#ifdef BIORBD_USE_CASADI_MATH
180 updateKin ? &Q :
nullptr);
181 return markers(updatedModel, Q, removeAxis);
204#ifdef BIORBD_USE_CASADI_MATH
211 updateKin ? &Q :
nullptr, updateKin ? &Qdot :
nullptr);
225 .CalcPointVelocity6D(
241#ifdef BIORBD_USE_CASADI_MATH
248 updateKin ? &Q :
nullptr, updateKin ? &Qdot :
nullptr);
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));
272#ifdef BIORBD_USE_CASADI_MATH
279 updateKin ? &Q :
nullptr, updateKin ? &Qdot :
nullptr);
288 std::vector<rigidbody::NodeSegment> pos;
289 for (
size_t i = 0; i <
nbMarkers(); ++i) {
300#ifdef BIORBD_USE_CASADI_MATH
307 updateKin ? &Q :
nullptr, updateKin ? &Qdot :
nullptr);
336#ifdef BIORBD_USE_CASADI_MATH
343 updateKin ? &Q :
nullptr,
344 updateKin ? &Qdot :
nullptr,
345 updateKin ? &Qddot :
nullptr);
355 std::vector<rigidbody::NodeSegment> pos;
356 for (
size_t i = 0; i <
nbMarkers(); ++i) {
370#ifdef BIORBD_USE_CASADI_MATH
377 updateKin ? &Q :
nullptr,
378 updateKin ? &Qdot :
nullptr,
379 updateKin ? &Qddot :
nullptr);
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));
402#ifdef BIORBD_USE_CASADI_MATH
409 updateKin ? &Q :
nullptr);
416 std::vector<rigidbody::NodeSegment> out;
417 for (
size_t i = 0; i <
nbMarkers(); ++i) {
425std::vector<rigidbody::NodeSegment>
427 std::vector<rigidbody::NodeSegment> out;
428 for (
size_t i = 0; i <
nbMarkers(); ++i) {
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));
452#ifdef BIORBD_USE_CASADI_MATH
459 updateKin ? &Q :
nullptr);
466 std::vector<rigidbody::NodeSegment> out;
467 for (
size_t i = 0; i <
nbMarkers(); ++i) {
474std::vector<rigidbody::NodeSegment>
476 std::vector<rigidbody::NodeSegment> out;
477 for (
size_t i = 0; i <
nbMarkers(); ++i) {
491 std::vector<rigidbody::NodeSegment> out;
494 if (!(*
m_marks)[i].parent().compare(name)) {
495 out.push_back(
marker(updatedModel, Q, i, removeAxis));
507#ifdef BIORBD_USE_CASADI_MATH
514 updateKin ? &Q :
nullptr);
531 if (!(*
m_marks)[i].parent().compare(name)) {
549#ifdef BIORBD_USE_CASADI_MATH
556 updateKin ? &Q :
nullptr);
572#ifdef BIORBD_USE_CASADI_MATH
579 updateKin ? &Q :
nullptr);
591 Q, parentName, removeAxis ? p.
removeAxes() : p,
false);
600#ifdef BIORBD_USE_CASADI_MATH
607 updateKin ? &Q :
nullptr);
611#ifndef BIORBD_USE_CASADI_MATH
613 const std::vector<rigidbody::NodeSegment> &
markers,
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]);
629 std::vector<RigidBodyDynamics::Math::Vector3d> markersInRbdl;
630 for (
size_t i = 0; i <
markers.size(); ++i) {
631 markersInRbdl.push_back(
markers[i]);
635 std::vector<unsigned int> bodyId;
636 for (
size_t i = 0; i < bodyPoint.size(); ++i) {
638 static_cast<size_t>((*(bodyPoint.begin() + i)).parentId()));
642 return RigidBodyDynamics::InverseKinematics(
643 updatedModel, Qinit, bodyId, bodyPointEigen, markersInRbdl, Q);
651 bool lookForTechnical,
653 std::vector<utils::Matrix> G;
655 for (
size_t idx = 0; idx <
nbMarkers(); ++idx) {
671 size_t nTechMarkers = 0;
672 if (nTechMarkers == 0)
674 if (mark.isTechnical()) {
685 size_t nTechMarkers = 0;
690 if (nTechMarkers == 0)
692 if (mark.isTechnical() && !mark.parent().compare(name)) {
700 size_t nAnatMarkers = 0;
701 if (nAnatMarkers == 0)
703 if (mark.isAnatomical()) {
712 std::vector<utils::String> names;
713 for (
size_t i = 0; i <
nbMarkers(); ++i) {
722 std::vector<utils::String> names;
724 if (
marker(i).isTechnical()) {
733 std::vector<utils::String> names;
735 if (
marker(i).isAnatomical()) {
Class GeneralizedAcceleration.
Class GeneralizedCoordinates.
Class GeneralizedVelocity.
This is the core of the musculoskeletal model in biorbd.
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,...
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...
Segment & segment(size_t idx)
Get a segment of index idx.
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...
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 ...
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...
Holder for the marker set.
const NodeSegment & marker(size_t idx) const
Return a marker of index idx in the marker set.
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.
NodeSegment markerVelocity(rigidbody::Joints &updatedModel, const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, size_t idx, bool removeAxis=true)
Return the linear velocity of a marker.
virtual ~Markers()
Destroy class properly.
std::vector< NodeSegment > technicalMarkersAxesRemoved() const
Return the technical markers in their respective parent reference frame.
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.
void setMarker(size_t index, const NodeSegment &pos)
Change the values of a specific marker without changing other parameters.
std::vector< NodeSegment > markersVelocity(rigidbody::Joints &updatedModel, const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, bool removeAxis=true)
Return the linear velocity of all the markers.
NodeSegment markerAngularVelocity(rigidbody::Joints &updatedModel, const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, size_t idx, bool removeAxis=true)
Return the angular velocity of a marker.
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.
std::vector< utils::String > anatomicalMarkerNames() const
Return the names of all the anatomical markers.
std::vector< NodeSegment > anatomicalMarkersAxesRemoved() const
Return the anatomical markers in their respective parent reference frame.
std::vector< NodeSegment > technicalMarkers() const
Return the technical markers in their respective parent reference frame.
std::vector< NodeSegment > anatomicalMarkers() const
Return the anatomical markers in their respective parent reference frame.
Markers()
Construct a marker set.
bool inverseKinematics(const std::vector< NodeSegment > &markers, const GeneralizedCoordinates &Qinit, GeneralizedCoordinates &Q, bool removeAxis=true)
Performs an inverse kinematics.
std::vector< utils::String > technicalMarkerNames() const
Return the names of all the technical markers.
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.
std::vector< NodeSegment > markersAngularVelocity(rigidbody::Joints &updatedModel, const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, bool removeAxis=true)
Return the angular velocity of all the markers.
std::vector< NodeSegment > markers(bool removeAxis=true) const
Return a copy of all the markers in their respective parent reference frame.
std::vector< utils::Matrix > markersJacobian(rigidbody::Joints &updatedModel, const GeneralizedCoordinates &Q, bool removeAxis=true)
Return the jacobian of the markers.
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.
Markers DeepCopy() const
Deep copy of the markers.
std::vector< utils::String > markerNames() const
Return the names of all the markers.
size_t nbTechnicalMarkers()
Return the number of technical markers.
size_t nbAnatomicalMarkers()
Return the number of anatomical markers.
size_t nbMarkers() const
Return the number of markers.
std::shared_ptr< std::vector< NodeSegment > > m_marks
The markers.
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.
std::vector< utils::Matrix > technicalMarkersJacobian(rigidbody::Joints &updatedModel, const GeneralizedCoordinates &Q, bool removeAxis=true)
Return the jacobian of the technical markers.
NodeSegment markerAxesRemoved(size_t idx) const
Return a marker of index idx in the marker set with the axes removed.
A point attached to a segment, generally speaking a skin marker.
bool isTechnical() const
Return if node is technical.
NodeSegment removeAxes() const
To remove axis.
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.
const String & name() const
Return the name of the node.
const String & parent() const
Return the parent name of the node.
Wrapper around the std::string class with augmented functionality.
Wrapper around Eigen Vector3d and attach it to a parent.