Biorbd Release_1.13.0
Loading...
Searching...
No Matches
IMUs.cpp
1#define BIORBD_API_EXPORTS
2#include "RigidBody/IMUs.h"
3
4#include <rbdl/Model.h>
5
6#include <rbdl/Kinematics.h>
7
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"
15
16using namespace BIORBD_NAMESPACE;
17
19 : m_IMUs(std::make_shared<std::vector<rigidbody::IMU>>()) {
20 // ctor
21}
22
24
26
28 rigidbody::IMUs copy;
29 copy.DeepCopy(*this);
30 return copy;
31}
32
34 m_IMUs->resize(other.m_IMUs->size());
35 for (size_t i = 0; i < other.m_IMUs->size(); ++i) {
36 (*m_IMUs)[i] = (*other.m_IMUs)[i].DeepCopy();
37 }
38}
39
40void rigidbody::IMUs::addIMU(bool technical, bool anatomical) {
41 m_IMUs->push_back(rigidbody::IMU(technical, anatomical));
42}
43
44// Add a new marker to the existing pool of markers
46 const utils::RotoTransNode &RotoTrans,
47 bool technical,
48 bool anatomical) {
49 m_IMUs->push_back(rigidbody::IMU(RotoTrans, technical, anatomical));
50}
51
52size_t rigidbody::IMUs::nbIMUs() const { return m_IMUs->size(); }
53
54// Get the markers in the global reference
55const std::vector<rigidbody::IMU> &rigidbody::IMUs::IMU() const {
56 return *m_IMUs;
57}
58
59std::vector<rigidbody::IMU> rigidbody::IMUs::IMU(
60 const utils::String &segmentName) {
61 std::vector<rigidbody::IMU> pos;
62 for (size_t i = 0; i < nbIMUs();
63 ++i) // Scan through all the markers and select the good ones
64 if (!IMU(i).parent().compare(segmentName)) {
65 pos.push_back(IMU(i));
66 }
67 return pos;
68}
69
71 return (*m_IMUs)[idx];
72}
73
74// Get the IMUs at the position given by Q
75std::vector<rigidbody::IMU> rigidbody::IMUs::IMU(
77 bool updateKin) {
78 std::vector<rigidbody::IMU> pos;
79 for (size_t i = 0; i < nbIMUs(); ++i) {
80 pos.push_back(IMU(Q, i, updateKin));
81 updateKin = false;
82 }
83
84 return pos;
85}
86
87// Get an IMU at the position given by Q
90 size_t idx,
91 bool updateKin) {
92 // Assuming that this is also a Joints type (via BiorbdModel)
93 rigidbody::Joints &model = dynamic_cast<rigidbody::Joints &>(*this);
94
95 rigidbody::IMU node = IMU(idx);
96 return model.globalJCS(Q, node.parent(), updateKin) * node;
97}
98
99// Get the technical IMUs
100std::vector<rigidbody::IMU> rigidbody::IMUs::technicalIMU(
102 bool 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));
107 updateKin = false;
108 }
109 return pos;
110}
111// Get the technical IMUs in the local reference
112std::vector<rigidbody::IMU> rigidbody::IMUs::technicalIMU() {
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)); // Forward kinematics
117 }
118 return pos;
119}
120
121// Get all the anatomical IMUs
122std::vector<rigidbody::IMU> rigidbody::IMUs::anatomicalIMU(
124 bool updateKin) {
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));
129 updateKin = false;
130 }
131 return pos;
132}
133// Get the anatomical IMUs in the local reference
134std::vector<rigidbody::IMU> rigidbody::IMUs::anatomicalIMU() {
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)); // Forward kinematics
139 }
140 return pos;
141}
142
143std::vector<rigidbody::IMU> rigidbody::IMUs::segmentIMU(
145 size_t idx,
146 bool updateKin) {
147 // Assuming that this is also a Joints type (via BiorbdModel)
148 rigidbody::Joints &model = dynamic_cast<rigidbody::Joints &>(*this);
149
150 // Segment name to find
151 utils::String name(model.segment(idx).name());
152
153 std::vector<rigidbody::IMU> pos;
154 for (size_t i = 0; i < nbIMUs();
155 ++i) // scan all the markers and select the right ones
156 if (!((*m_IMUs)[i]).parent().compare(name)) {
157 pos.push_back(IMU(Q, i, updateKin));
158 updateKin = false;
159 }
160
161 return pos;
162}
163
164// Get the Jacobian of the markers
165std::vector<utils::Matrix> rigidbody::IMUs::IMUJacobian(
167 bool updateKin) {
168 return IMUJacobian(Q, updateKin, false);
169}
170
171// Get the Jacobian of the technical markers
172std::vector<utils::Matrix> rigidbody::IMUs::TechnicalIMUJacobian(
174 bool updateKin) {
175 return IMUJacobian(Q, updateKin, true);
176}
177
178// Protected function
179std::vector<utils::Matrix> rigidbody::IMUs::IMUJacobian(
181 bool updateKin,
182 bool lookForTechnical) {
183 // Assuming that this is also a Joints type (via BiorbdModel)
184 rigidbody::Joints &model = dynamic_cast<rigidbody::Joints &>(*this);
185
186 std::vector<utils::Matrix> G;
187
188 for (size_t idx = 0; idx < nbIMUs(); ++idx) {
189 // Actual marker
190 rigidbody::IMU node = IMU(idx);
191 if (lookForTechnical && !node.isTechnical()) {
192 continue;
193 }
194
195 size_t id = static_cast<size_t>(model.GetBodyId(node.parent().c_str()));
196 utils::Matrix G_tp(utils::Matrix::Zero(9, model.dof_count));
197
198 // Calculate the Jacobian of this Tag
199 model.CalcMatRotJacobian(Q, id, node.rot(), G_tp, updateKin);
200#ifndef BIORBD_USE_CASADI_MATH
201 updateKin = false;
202#endif
203
204 G.push_back(G_tp);
205 }
206
207 return G;
208}
209
211 size_t nbTech = 0;
212 if (nbTech == 0) // If the function has never been called before
213 for (rigidbody::IMU imu : *m_IMUs)
214 if (imu.isTechnical()) {
215 ++nbTech;
216 }
217
218 return nbTech;
219}
220
222 size_t nbAnat = 0;
223 if (nbAnat == 0) // If the function has never been called before
224 for (rigidbody::IMU imu : *m_IMUs)
225 if (imu.isAnatomical()) {
226 ++nbAnat;
227 }
228
229 return nbAnat;
230}
231
232std::vector<utils::String> rigidbody::IMUs::IMUsNames() {
233 // Extract the name of all the markers of a model
234 std::vector<utils::String> names;
235 for (size_t i = 0; i < nbIMUs(); ++i) {
236 names.push_back(IMU(i).utils::Node::name());
237 }
238 return names;
239}
240
241std::vector<utils::String> rigidbody::IMUs::technicalIMUsNames() {
242 // Extract the names of all the technical markers of a model
243 std::vector<utils::String> names;
244 for (size_t i = 0; i < nbIMUs(); ++i)
245 if (IMU(i).isTechnical()) {
246 names.push_back(IMU(i).utils::Node::name());
247 }
248
249 return names;
250}
251
252std::vector<utils::String> rigidbody::IMUs::anatomicalIMUsNames() {
253 // Extract the names of all the anatomical markers of a model
254 std::vector<utils::String> names;
255 for (size_t i = 0; i < nbIMUs(); ++i)
256 if (IMU(i).isAnatomical()) {
257 names.push_back(IMU(i).utils::Node::name());
258 }
259
260 return names;
261}
IMU(bool isTechnical=true, bool isAnatomical=true)
Construct inertial measurement unit.
Definition IMU.cpp:8
bool isTechnical() const
Return if the IMU is technical.
Definition IMU.cpp:54
Hold a set of IMUs.
Definition IMUs.h:22
std::vector< utils::String > technicalIMUsNames()
Return the names of the technical inertial measurement units (IMU).
Definition IMUs.cpp:241
std::shared_ptr< std::vector< rigidbody::IMU > > m_IMUs
All the inertial Measurement Units.
Definition IMUs.h:235
std::vector< rigidbody::IMU > anatomicalIMU()
Return all the anatomical inertial measurement units (IMU) in their respective segment local referenc...
Definition IMUs.cpp:134
virtual ~IMUs()
Destroy the class properly.
Definition IMUs.cpp:25
const std::vector< rigidbody::IMU > & IMU() const
Return all the IMU in the local reference of the segment.
Definition IMUs.cpp:55
size_t nbTechIMUs()
Return the number of technical inertial measurement units (IMU).
Definition IMUs.cpp:210
std::vector< utils::String > IMUsNames()
Return the names of the inertial measurement units (IMU).
Definition IMUs.cpp:232
IMUs DeepCopy() const
Deep copy of the inertial measurement units data.
Definition IMUs.cpp:27
std::vector< utils::Matrix > IMUJacobian(const GeneralizedCoordinates &Q, bool updateKin=true)
Return the jacobian of the inertial measurement units (IMU).
Definition IMUs.cpp:165
std::vector< utils::String > anatomicalIMUsNames()
Return the names of the anatomical inertial measurement units (IMU).
Definition IMUs.cpp:252
void addIMU(bool technical=true, bool anatomical=true)
Add a new inertial measurement unit to the set.
Definition IMUs.cpp:40
IMUs()
Construct inertial measurement units set.
Definition IMUs.cpp:18
size_t nbIMUs() const
Return the number of inertial measurement units (IMU) in the set.
Definition IMUs.cpp:52
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.
Definition IMUs.cpp:143
std::vector< utils::Matrix > TechnicalIMUJacobian(const GeneralizedCoordinates &Q, bool updateKin=true)
Return the jacobian of the technical inertial measurement units (IMU).
Definition IMUs.cpp:172
size_t nbAnatIMUs()
Return the number of anatomical inertial measurement units (IMU).
Definition IMUs.cpp:221
std::vector< rigidbody::IMU > technicalIMU()
Return all the technical inertial measurement units (IMU) in their respective segment local reference...
Definition IMUs.cpp:112
This is the core of the musculoskeletal model in biorbd.
Definition Joints.h:48
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.
Definition Joints.cpp:1905
Segment & segment(size_t idx)
Get a segment of index idx.
Definition Joints.cpp:205
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.
Definition Joints.cpp:360
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
A RotoTrans which is attached to a segment.
Rotation rot() const
Return the rotation matrix.
Wrapper around the std::string class with augmented functionality.
Definition String.h:18