1#define BIORBD_API_EXPORTS
2#include "RigidBody/IMUs.h"
6#include <rbdl/Kinematics.h>
8#include "RigidBody/GeneralizedCoordinates.h"
9#include "RigidBody/IMU.h"
10#include "RigidBody/Joints.h"
11#include "RigidBody/Segment.h"
12#include "Utils/Matrix.h"
13#include "Utils/Rotation.h"
14#include "Utils/String.h"
16using namespace BIORBD_NAMESPACE;
19 :
m_IMUs(std::make_shared<std::vector<rigidbody::
IMU>>()) {
35 for (
size_t i = 0; i < other.
m_IMUs->size(); ++i) {
61 std::vector<rigidbody::IMU> pos;
62 for (
size_t i = 0; i <
nbIMUs();
64 if (!
IMU(i).parent().compare(segmentName)) {
65 pos.push_back(
IMU(i));
78 std::vector<rigidbody::IMU> pos;
79 for (
size_t i = 0; i <
nbIMUs(); ++i) {
80 pos.push_back(
IMU(Q, i, updateKin));
103 std::vector<rigidbody::IMU> pos;
104 for (
size_t i = 0; i <
nbIMUs(); ++i)
105 if (
IMU(i).isTechnical()) {
106 pos.push_back(
IMU(Q, i, updateKin));
113 std::vector<rigidbody::IMU> pos;
114 for (
size_t i = 0; i <
nbIMUs(); ++i)
115 if (
IMU(i).isTechnical()) {
116 pos.push_back(
IMU(i));
125 std::vector<rigidbody::IMU> pos;
126 for (
size_t i = 0; i <
nbIMUs(); ++i)
127 if (
IMU(i).isAnatomical()) {
128 pos.push_back(
IMU(Q, i, updateKin));
135 std::vector<rigidbody::IMU> pos;
136 for (
size_t i = 0; i <
nbIMUs(); ++i)
137 if (
IMU(i).isAnatomical()) {
138 pos.push_back(
IMU(i));
153 std::vector<rigidbody::IMU> pos;
154 for (
size_t i = 0; i <
nbIMUs();
156 if (!((*
m_IMUs)[i]).parent().compare(name)) {
157 pos.push_back(
IMU(Q, i, updateKin));
182 bool lookForTechnical) {
186 std::vector<utils::Matrix> G;
188 for (
size_t idx = 0; idx <
nbIMUs(); ++idx) {
195 size_t id =
static_cast<size_t>(model.GetBodyId(node.
parent().c_str()));
200#ifndef BIORBD_USE_CASADI_MATH
214 if (imu.isTechnical()) {
225 if (imu.isAnatomical()) {
234 std::vector<utils::String> names;
235 for (
size_t i = 0; i <
nbIMUs(); ++i) {
243 std::vector<utils::String> names;
244 for (
size_t i = 0; i <
nbIMUs(); ++i)
245 if (
IMU(i).isTechnical()) {
254 std::vector<utils::String> names;
255 for (
size_t i = 0; i <
nbIMUs(); ++i)
256 if (
IMU(i).isAnatomical()) {
Class GeneralizedCoordinates.
IMU(bool isTechnical=true, bool isAnatomical=true)
Construct inertial measurement unit.
bool isTechnical() const
Return if the IMU is technical.
std::vector< utils::String > technicalIMUsNames()
Return the names of the technical inertial measurement units (IMU).
std::shared_ptr< std::vector< rigidbody::IMU > > m_IMUs
All the inertial Measurement Units.
std::vector< rigidbody::IMU > anatomicalIMU()
Return all the anatomical inertial measurement units (IMU) in their respective segment local referenc...
virtual ~IMUs()
Destroy the class properly.
const std::vector< rigidbody::IMU > & IMU() const
Return all the IMU in the local reference of the segment.
size_t nbTechIMUs()
Return the number of technical inertial measurement units (IMU).
std::vector< utils::String > IMUsNames()
Return the names of the inertial measurement units (IMU).
IMUs DeepCopy() const
Deep copy of the inertial measurement units data.
std::vector< utils::Matrix > IMUJacobian(const GeneralizedCoordinates &Q, bool updateKin=true)
Return the jacobian of the inertial measurement units (IMU).
std::vector< utils::String > anatomicalIMUsNames()
Return the names of the anatomical inertial measurement units (IMU).
void addIMU(bool technical=true, bool anatomical=true)
Add a new inertial measurement unit to the set.
IMUs()
Construct inertial measurement units set.
size_t nbIMUs() const
Return the number of inertial measurement units (IMU) in the set.
std::vector< rigidbody::IMU > segmentIMU(const GeneralizedCoordinates &Q, size_t idx, bool updateKin=true)
Return all the inertial measurement units (IMU) on a specified segment.
std::vector< utils::Matrix > TechnicalIMUJacobian(const GeneralizedCoordinates &Q, bool updateKin=true)
Return the jacobian of the technical inertial measurement units (IMU).
size_t nbAnatIMUs()
Return the number of anatomical inertial measurement units (IMU).
std::vector< rigidbody::IMU > technicalIMU()
Return all the technical inertial measurement units (IMU) in their respective segment local reference...
This is the core of the musculoskeletal model in biorbd.
void CalcMatRotJacobian(const GeneralizedCoordinates &Q, size_t segmentIdx, const utils::Matrix3d &rotation, utils::Matrix &G, bool updateKin)
Calculate the jacobian matrix of a rotation matrix.
Segment & segment(size_t idx)
Get a segment of index idx.
utils::RotoTrans globalJCS(const GeneralizedCoordinates &Q, const utils::String &name, bool updateKin=true)
Return the joint coordinate system (JCS) for the segment in global reference frame at a given Q.
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.
A RotoTrans which is attached to a segment.
Rotation rot() const
Return the rotation matrix.
Wrapper around the std::string class with augmented functionality.