Biorbd Release_1.13.0
Loading...
Searching...
No Matches
Segment.cpp
1#define BIORBD_API_EXPORTS
2#include "RigidBody/Segment.h"
3
4#include <limits.h>
5
6#include "RigidBody/Joints.h"
7#include "RigidBody/Mesh.h"
8#include "RigidBody/SegmentCharacteristics.h"
9#include "Utils/Error.h"
10#include "Utils/Matrix3d.h"
11#include "Utils/Range.h"
12#include "Utils/Rotation.h"
13#include "Utils/RotoTrans.h"
14#include "Utils/SpatialTransform.h"
15#include "Utils/String.h"
16#include "Utils/Vector3d.h"
17
18using namespace BIORBD_NAMESPACE;
19
21 : utils::Node(),
22 m_idxInModel(std::make_shared<int>(-1)),
23 m_cor(std::make_shared<utils::SpatialTransform>()),
24 m_seqT(std::make_shared<utils::String>()),
25 m_seqR(std::make_shared<utils::String>()),
26 m_nbDof(std::make_shared<size_t>(0)),
27 m_nbQdot(std::make_shared<size_t>(0)),
28 m_nbQddot(std::make_shared<size_t>(0)),
29 m_nbDofTrue(std::make_shared<size_t>(0)),
30 m_nbDofTrueOutside(std::make_shared<size_t>(0)),
31 m_nbDofTrans(std::make_shared<size_t>(0)),
32 m_nbDofRot(std::make_shared<size_t>(0)),
33 m_nbDofQuat(std::make_shared<size_t>(0)),
34 m_QRanges(std::make_shared<std::vector<utils::Range>>()),
35 m_QdotRanges(std::make_shared<std::vector<utils::Range>>()),
36 m_QddotRanges(std::make_shared<std::vector<utils::Range>>()),
37 m_jointDampings(std::make_shared<std::vector<utils::Scalar>>()),
38 m_isQuaternion(std::make_shared<bool>(false)),
39 m_dof(std::make_shared<std::vector<RigidBodyDynamics::Joint>>()),
40 m_idxDof(std::make_shared<std::vector<size_t>>()),
41 m_sequenceTrans(std::make_shared<std::vector<size_t>>()),
42 m_sequenceRot(std::make_shared<std::vector<size_t>>()),
43 m_nameDof(std::make_shared<std::vector<utils::String>>()),
44 m_dofPosition(std::make_shared<std::vector<size_t>>()),
45 m_characteristics(std::make_shared<rigidbody::SegmentCharacteristics>()),
47 std::make_shared<std::vector<rigidbody::SegmentCharacteristics>>()) {
48 setType();
49}
50
52 rigidbody::Joints &model,
53 const utils::String &name,
54 const utils::String &parentName,
55 const utils::String &seqT,
56 const utils::String &seqR,
57 const std::vector<utils::Range> &QRanges,
58 const std::vector<utils::Range> &QdotRanges,
59 const std::vector<utils::Range> &QddotRanges,
60 const std::vector<utils::Scalar> &jointDampings,
62 const utils::SpatialTransform &cor)
63 : utils::Node(name, parentName),
64 m_idxInModel(std::make_shared<int>(-1)),
65 m_cor(std::make_shared<utils::SpatialTransform>(cor)),
66 m_seqT(std::make_shared<utils::String>(seqT)),
67 m_seqR(std::make_shared<utils::String>(seqR)),
68 m_nbDof(std::make_shared<size_t>(0)),
69 m_nbQdot(std::make_shared<size_t>(0)),
70 m_nbQddot(std::make_shared<size_t>(0)),
71 m_nbDofTrue(std::make_shared<size_t>(0)),
72 m_nbDofTrueOutside(std::make_shared<size_t>(0)),
73 m_nbDofTrans(std::make_shared<size_t>(0)),
74 m_nbDofRot(std::make_shared<size_t>(0)),
75 m_nbDofQuat(std::make_shared<size_t>(0)),
76 m_QRanges(std::make_shared<std::vector<utils::Range>>()),
77 m_QdotRanges(std::make_shared<std::vector<utils::Range>>()),
78 m_QddotRanges(std::make_shared<std::vector<utils::Range>>()),
79 m_jointDampings(std::make_shared<std::vector<utils::Scalar>>()),
80 m_isQuaternion(std::make_shared<bool>(false)),
81 m_dof(std::make_shared<std::vector<RigidBodyDynamics::Joint>>()),
82 m_idxDof(std::make_shared<std::vector<size_t>>()),
83 m_sequenceTrans(std::make_shared<std::vector<size_t>>()),
84 m_sequenceRot(std::make_shared<std::vector<size_t>>()),
85 m_nameDof(std::make_shared<std::vector<utils::String>>()),
86 m_dofPosition(std::make_shared<std::vector<size_t>>()),
88 std::make_shared<rigidbody::SegmentCharacteristics>(characteristics)),
90 std::make_shared<std::vector<rigidbody::SegmentCharacteristics>>()) {
91 setType();
92 // Call proper functions
94}
96 rigidbody::Joints &model,
97 const utils::String &name, // Name of segment
98 const utils::String &parentName, // Name of segment
99 const utils::String &seqR, // Cardan sequence to classify the rotation DoF
100 const std::vector<utils::Range> &QRanges,
101 const std::vector<utils::Range> &QdotRanges,
102 const std::vector<utils::Range> &QddotRanges,
103 const std::vector<utils::Scalar> &jointDampings,
105 &characteristics, // Mass, Center of mass of segment, Inertia of
106 // segment, etc.
107 const utils::SpatialTransform &cor // Transformation from parent to child
108 )
109 : utils::Node(name, parentName),
110 m_idxInModel(std::make_shared<int>(-1)),
111 m_cor(std::make_shared<utils::SpatialTransform>(cor)),
112 m_seqT(std::make_shared<utils::String>()),
113 m_seqR(std::make_shared<utils::String>(seqR)),
114 m_nbDof(std::make_shared<size_t>(0)),
115 m_nbQdot(std::make_shared<size_t>(0)),
116 m_nbQddot(std::make_shared<size_t>(0)),
117 m_nbDofTrue(std::make_shared<size_t>(0)),
118 m_nbDofTrueOutside(std::make_shared<size_t>(0)),
119 m_nbDofTrans(std::make_shared<size_t>(0)),
120 m_nbDofRot(std::make_shared<size_t>(0)),
121 m_nbDofQuat(std::make_shared<size_t>(0)),
122 m_QRanges(std::make_shared<std::vector<utils::Range>>()),
123 m_QdotRanges(std::make_shared<std::vector<utils::Range>>()),
124 m_QddotRanges(std::make_shared<std::vector<utils::Range>>()),
125 m_jointDampings(std::make_shared<std::vector<utils::Scalar>>()),
126 m_isQuaternion(std::make_shared<bool>(false)),
127 m_dof(std::make_shared<std::vector<RigidBodyDynamics::Joint>>()),
128 m_idxDof(std::make_shared<std::vector<size_t>>()),
129 m_sequenceTrans(std::make_shared<std::vector<size_t>>()),
130 m_sequenceRot(std::make_shared<std::vector<size_t>>()),
131 m_nameDof(std::make_shared<std::vector<utils::String>>()),
132 m_dofPosition(std::make_shared<std::vector<size_t>>()),
134 std::make_shared<rigidbody::SegmentCharacteristics>(characteristics)),
136 std::make_shared<std::vector<rigidbody::SegmentCharacteristics>>()) {
137 setType();
138 // Call proper functions
140}
141
144 copy.DeepCopy(*this);
145 return copy;
146}
147
150 *m_idxInModel = *other.m_idxInModel;
151 *m_cor = *other.m_cor;
152 *m_seqT = *other.m_seqT;
153 *m_seqR = *other.m_seqR;
154 *m_nbDof = *other.m_nbDof;
155 *m_nbQdot = *other.m_nbQdot;
156 *m_nbQddot = *other.m_nbQddot;
157 *m_nbDofTrue = *other.m_nbDofTrue;
159 *m_nbDofTrans = *other.m_nbDofTrans;
160 *m_nbDofRot = *other.m_nbDofRot;
161 *m_nbDofQuat = *other.m_nbDofQuat;
162 *m_QRanges = *other.m_QRanges;
163 *m_QdotRanges = *other.m_QdotRanges;
167 m_dof->resize(other.m_dof->size());
168 for (size_t i = 0; i < other.m_dof->size(); ++i) {
169 (*m_dof)[i] = (*other.m_dof)[i];
170 }
171 *m_idxDof = *other.m_idxDof;
174 m_nameDof->resize(other.m_nameDof->size());
175 for (size_t i = 0; i < other.m_nameDof->size(); ++i) {
176 (*m_nameDof)[i] = (*other.m_nameDof)[i];
177 }
179 *m_characteristics = other.m_characteristics->DeepCopy();
180 m_dofCharacteristics->resize(other.m_dofCharacteristics->size());
181 for (size_t i = 0; i < other.m_dofCharacteristics->size(); ++i) {
182 (*m_dofCharacteristics)[i] = (*other.m_dofCharacteristics)[i].DeepCopy();
183 }
184}
185
187
191
193 *m_typeOfNode = utils::NODE_TYPE::SEGMENT;
194}
195
197 if (*m_nbDof != 0) {
198 return (*m_idxDof)[*m_nbDof - 1];
199 } else {
200 return (*m_idxDof)[*m_nbDof];
201 }
202}
203
207size_t rigidbody::Segment::nbDofRot() const { return *m_nbDofRot; }
208size_t rigidbody::Segment::nbQ() const { return *m_nbDofTrue; }
209size_t rigidbody::Segment::nbQdot() const { return *m_nbQdot; }
210size_t rigidbody::Segment::nbQddot() const { return *m_nbQddot; }
211
212const utils::String &rigidbody::Segment::nameDof(const size_t i) const {
213 // Return the number of DoF of the segment
214 utils::Error::check(i < *m_nbDofTrue, "Dof ouside N dof for this segment");
215 return (*m_nameDof)[i];
216}
217
219
221
222const std::vector<utils::Range> &rigidbody::Segment::QRanges() const {
223 return *m_QRanges;
224}
225
226void rigidbody::Segment::setQRanges(const std::vector<utils::Range> &QRanges) {
229 "QRanges must be empty or of the same size as the number of dof");
231}
232
233const std::vector<utils::Range> &rigidbody::Segment::QdotRanges() const {
234 return *m_QdotRanges;
235}
236
238 const std::vector<utils::Range> &QdotRanges) {
241 "QdotRanges must be empty or of the same size as the number of dof");
243}
244
245const std::vector<utils::Range> &rigidbody::Segment::QddotRanges() const {
246 return *m_QddotRanges;
247}
248
250 const std::vector<utils::Range> &QddotRanges) {
253 "QddotRanges must be empty or of the same size as the number of dof");
255}
256
257const std::vector<utils::Scalar> &rigidbody::Segment::jointDampings() const {
258 return *m_jointDampings;
259}
260
262 const std::vector<utils::Scalar> &jointDampings) {
265 "jointDampings must be empty or of the same size as the number of dof");
266
267 if (jointDampings.size() == 0) {
268 size_t nTrans = *m_nbDofTrans;
269 size_t nRot = *m_isQuaternion ? 3 : *m_nbDofRot;
270 *m_jointDampings = std::vector<utils::Scalar>(nTrans + nRot, 0);
271 } else {
273 }
274}
275
278 m_cor->rotation().transpose(), m_cor->translation());
279}
280
282 rigidbody::Joints &model,
283 utils::RotoTrans &rototrans) {
284 *m_cor =
285 utils::SpatialTransform(rototrans.rot().transpose(), rototrans.trans());
286 // we also modify RBDL spatial transform from parent to child
287 model.X_T[*m_idxDof->begin()] = *m_cor;
288}
289
291 rigidbody::Joints &model,
294 RigidBodyDynamics::Math::SpatialRigidBodyInertia rbi = RigidBodyDynamics::
295 Math::SpatialRigidBodyInertia::createFromMassComInertiaC(
296 m_characteristics->mMass,
297 m_characteristics->mCenterOfMass,
298 m_characteristics->mInertia);
299
300 model.Ic[*m_idxInModel] = rbi;
301 model.I[*m_idxInModel] = rbi;
302}
303
307
312
314 rigidbody::Joints &model,
315 const utils::String &seqT,
316 const utils::String &seqR,
317 const std::vector<utils::Range> &QRanges,
318 const std::vector<utils::Range> &QdotRanges,
319 const std::vector<utils::Range> &QddotRanges,
320 const std::vector<utils::Scalar> &jointDampings) {
323
324 // Control for the size of dofRange then copy it
325 size_t nRot(seqR.length());
326 if (*m_isQuaternion) {
327 nRot = 0;
328 }
329
333
335
336 setJoints(model);
337}
338
339template <typename T>
341 const std::vector<T> &vec,
342 bool isQLevel) const {
343 size_t nTrans = *m_nbDofTrans;
344 size_t nRot = *m_isQuaternion ? (isQLevel ? 4 : 3) : *m_nbDofRot;
345 return vec.size() == 0 || vec.size() == nTrans + nRot;
346}
347
349 if (!seqR.tolower().compare("q")) {
350 *m_isQuaternion = true;
351 }
352}
353
354// Member functions
356 const utils::String &seqT,
357 const utils::String &seqR) {
358 m_sequenceTrans->clear();
360 m_sequenceRot->clear();
361 if (*m_isQuaternion) {
362 m_sequenceRot->resize(1);
363 } else {
364 m_sequenceRot->resize(*m_nbDofRot);
365 }
366
369
370 // Store the names of the DoFs
371 m_nameDof->clear();
372 m_nameDof->resize(*m_nbDofTrue);
373 for (size_t i = 0; i < *m_nbDofTrans; ++i) {
374 (*m_nameDof)[i] = "Trans" + seqT(i).toupper();
375 }
376 for (size_t i = 0; i < *m_nbDofRot; ++i) {
377 (*m_nameDof)[*m_nbDofTrans + i] = "Rot" + seqR(i).toupper();
378 }
379 utils::String xyz("XYZW");
380 for (size_t i = 0; i < *m_nbDofQuat; ++i) {
381 (*m_nameDof)[*m_nbDofTrans + *m_nbDofRot + i] =
382 utils::String("Quat") + xyz.substr(i, 1);
383 }
384}
386 std::vector<size_t> &sequenceInteger,
387 const utils::String &sequenceText) {
388 for (size_t i = 0; i < sequenceText.length(); i++) {
389 char tp = sequenceText.tolower()[i];
390 if (tp == 'x') {
391 sequenceInteger[i] = 0;
392 } else if (tp == 'y') {
393 sequenceInteger[i] = 1;
394 } else if (tp == 'z') {
395 sequenceInteger[i] = 2;
396 } else if (tp == 'q') {
397 sequenceInteger[i] = 3;
398 } else {
399 utils::Error::raise("Wrong sequence!");
400 }
401 }
402}
403void rigidbody::Segment::setNumberOfDof(size_t nTrans, size_t nRot) {
404 *m_nbDofTrans = nTrans;
405 if (*m_isQuaternion) {
406 *m_nbDofRot = 0;
407 *m_nbDofQuat = 4;
408 *m_nbDof = nTrans + 1;
409 *m_nbQdot = nTrans + 3;
410 *m_nbQddot = nTrans + 3;
411 *m_nbDofTrue = nTrans + *m_nbDofQuat;
412 *m_nbDofTrueOutside = nTrans + 3;
413 } else {
414 *m_nbDofRot = nRot;
415 *m_nbDofQuat = 0;
416 *m_nbDof = nTrans + *m_nbDofRot;
417 *m_nbQdot = *m_nbDof;
418 *m_nbQddot = *m_nbDof;
421 }
422}
423
425 const utils::String &seqT,
426 const utils::String
427 &seqR) // Find the x, y, and z positions in this sequence
428{
429 setNumberOfDof(seqT.length(), seqR.length());
431 fillSequence();
432}
434 m_dofPosition->clear();
435 m_dofPosition->resize(*m_nbDof);
436
437 for (size_t i = 0; i < *m_nbDofTrans; i++) {
438 (*m_dofPosition)[i] = (*m_sequenceTrans)[i]; // Place the translation first
439 // in the requested order
440 }
441 if (*m_isQuaternion) {
442 (*m_dofPosition)[*m_nbDofTrans] = (*m_sequenceRot)[0];
443 } else
444 for (size_t i = 0; i < *m_nbDofRot; i++) {
445 (*m_dofPosition)[i + *m_nbDofTrans] =
446 (*m_sequenceRot)[i]; // Place the rotation following the
447 // translations in the requested order
448 }
449}
450
452 m_dofCharacteristics->clear();
453
454 if (*m_nbDof != 0) {
456 for (size_t i = 0; i < *m_nbDof - 1; i++) {
457 (*m_dofCharacteristics)[i] = rigidbody::SegmentCharacteristics();
458 }
459
460 (*m_dofCharacteristics)[*m_nbDof - 1] = *m_characteristics;
461 } else {
462 m_dofCharacteristics->resize(1);
463 (*m_dofCharacteristics)[0] = *m_characteristics;
464 }
465}
466
468 // Definition of the rotation axis
469 utils::Vector3d axis[3];
470 axis[0] = utils::Vector3d(1, 0, 0); // axe x
471 axis[1] = utils::Vector3d(0, 1, 0); // axe y
472 axis[2] = utils::Vector3d(0, 0, 1); // axe z
473
474 // Declaration of DoFs in translation
475 m_dof->clear();
476 if (*m_nbDof != 0) {
477 m_dof->resize(*m_nbDof);
478 for (size_t i = 0; i < *m_nbDofTrans; i++) {
479 (*m_dof)[i] = RigidBodyDynamics::Joint(
480 RigidBodyDynamics::JointTypePrismatic, axis[(*m_dofPosition)[i]]);
481 }
482
483 // Declaration of the DoFs in rotation
484 if (*m_isQuaternion) {
485 (*m_dof)[*m_nbDofTrans] =
486 RigidBodyDynamics::Joint(RigidBodyDynamics::JointTypeSpherical);
487 } else {
488 for (size_t i = *m_nbDofTrans; i < *m_nbDofRot + *m_nbDofTrans; i++) {
489 (*m_dof)[i] = RigidBodyDynamics::Joint(
490 RigidBodyDynamics::JointTypeRevolute, axis[(*m_dofPosition)[i]]);
491 }
492 }
493 } else {
494 m_dof->resize(1);
495 (*m_dof)[0] = RigidBodyDynamics::Joint(RigidBodyDynamics::JointTypeFixed);
496 }
497}
498
500 setDofCharacteristicsOnLastBody(); // Apply the segment caracteristics only
501 // to the last segment
502 setJointAxis(); // Choose the axis order in relation to the selected
503 // sequence
504
506 // Create the articulations (intra segment)
507 m_idxDof->clear();
508
509 if (*m_nbDof == 0) {
510 m_idxDof->resize(1);
511 } else {
512 m_idxDof->resize(*m_nbDof);
513 }
514
515 unsigned int parent_id(model.GetBodyId(parent().c_str()));
516
517 if (parent_id == std::numeric_limits<unsigned int>::max()) {
518 parent_id = 0;
519 }
520 if (*m_nbDof == 0)
521 (*m_idxDof)[0] = model.AddBody(
522 parent_id, *m_cor, (*m_dof)[0], (*m_dofCharacteristics)[0], name());
523 else if (*m_nbDof == 1)
524 (*m_idxDof)[0] = model.AddBody(
525 parent_id, *m_cor, (*m_dof)[0], (*m_dofCharacteristics)[0], name());
526 else {
527 (*m_idxDof)[0] = model.AddBody(
528 parent_id, *m_cor, (*m_dof)[0], (*m_dofCharacteristics)[0]);
529 for (size_t i = 1; i < *m_nbDof; i++)
530 if (i != *m_nbDof - 1)
531 (*m_idxDof)[i] = model.AddBody(
532 static_cast<unsigned int>((*m_idxDof)[i - 1]),
533 zero,
534 (*m_dof)[i],
536 else
537 (*m_idxDof)[i] = model.AddBody(
538 static_cast<unsigned int>((*m_idxDof)[i - 1]),
539 zero,
540 (*m_dof)[i],
542 name());
543 }
544 *m_idxInModel = static_cast<int>(model.I.size() - 1);
545}
546
547size_t rigidbody::Segment::getDofIdx(const utils::String &dofName) const {
548 size_t idx(INT_MAX);
549 bool found = false;
550 for (size_t i = 0; i < nbDof(); ++i) {
551 if (!dofName.tolower().compare((*m_nameDof)[i].tolower())) {
552 idx = i;
553 found = true;
554 break;
555 }
556 }
557
559 found,
560 "Type should be \"Rot\" or \"Trans\" and axis should be \"X\", \"Y\" "
561 "or "
562 "\"Z\", e.g. "
563 "\"RotY\" for Rotation around y or \"TransX\" for Translation on x");
564
565 return idx;
566}
567
569 const rigidbody::Joints &model) const {
570 const rigidbody::Segment *segment = this;
571
572 do {
573 if (segment->nbDof() > 0) return *segment;
575 segment->parent().compare("root"),
576 "Could not find any dof for " + name() + " up to the root.");
577 segment = &model.segment(segment->parent());
578 } while (true);
579}
580
582 const rigidbody::Joints &model) const {
583 const utils::String toCompare(findFirstSegmentWithDof(model).name());
584
585 // Start at root and descend the Generalized Coordinate vector until we get
586 // to the current segment
587 size_t dofCount(0); // Start
588 for (int i = 0; i < static_cast<int>(model.nbSegment()); ++i) {
589 const rigidbody::Segment &segment(model.segment(i));
590
591 if (!segment.name().compare(toCompare)) return dofCount;
592
593 dofCount += segment.nbDof();
594 }
595
596 throw std::runtime_error("Segment " + name() + " not found in the model");
597}
598
This is the core of the musculoskeletal model in biorbd.
Definition Joints.h:48
size_t nbSegment() const
Return the actual number of segment.
Definition Joints.cpp:244
Segment & segment(size_t idx)
Get a segment of index idx.
Definition Joints.cpp:205
Characteristics of a segment, namely the mass, the center of mass, the inertia, the length and its me...
SegmentCharacteristics DeepCopy() const
Deep copy of the segment characteristics.
Description of a segment.
Definition Segment.h:29
std::shared_ptr< std::vector< utils::String > > m_nameDof
To store the DoF names.
Definition Segment.h:478
std::shared_ptr< std::vector< RigidBodyDynamics::Joint > > m_dof
Definition Segment.h:434
bool isRotationAQuaternion() const
Return if the rotation DoF of this segment is a quaternion.
Definition Segment.cpp:188
SegmentCharacteristics & characteristics()
Return the segment characteristics.
Definition Segment.cpp:304
const std::vector< utils::Range > & QRanges() const
Return the ranges for all the dof, translations and rotations respectively.
Definition Segment.cpp:222
utils::RotoTrans localJCS() const
Return the joint coordinate system (JCS) in the parent reference frame.
Definition Segment.cpp:276
std::shared_ptr< bool > m_isQuaternion
If DoF in rotation is a Quaternion.
Definition Segment.h:423
const utils::String & seqT() const
Return the translation sequence in text.
Definition Segment.cpp:218
size_t getDofIdx(const utils::String &dofName) const
Return the index of a specified DoF.
Definition Segment.cpp:547
std::shared_ptr< utils::SpatialTransform > m_cor
Attitude of the segment in parent reference frame.
Definition Segment.h:346
void str2numSequence(std::vector< size_t > &sequenceInteger, const utils::String &sequenceText)
Convert a text sequence to its number counterpart (x = 0, y = 1, z = 2, q = 3).
Definition Segment.cpp:385
std::shared_ptr< utils::String > m_seqT
Translation sequence.
Definition Segment.h:389
std::shared_ptr< std::vector< SegmentCharacteristics > > m_dofCharacteristics
Definition Segment.h:507
Segment DeepCopy() const
Create a deep copy of Segment.
Definition Segment.cpp:142
std::shared_ptr< std::vector< utils::Range > > m_QdotRanges
Definition Segment.h:412
std::shared_ptr< int > m_idxInModel
Index in RBDL model.
Definition Segment.h:338
virtual ~Segment()
Destroy the class properly.
Definition Segment.cpp:186
std::shared_ptr< std::vector< size_t > > m_sequenceTrans
Translation sequence.
Definition Segment.h:474
void determineIfRotIsQuaternion(const utils::String &seqR)
Determines if DoF in rotation is a Quaternion.
Definition Segment.cpp:348
size_t getLastDofIndexInGeneralizedCoordinates(const rigidbody::Joints &model) const
Returns the last index of the segment in the Generalized Coordinates vector.
Definition Segment.cpp:599
void fillSequence()
Fill the transation and rotation sequences.
Definition Segment.cpp:433
virtual void setJointAxis()
Determine the rotation axis in relation to the requested sequence.
Definition Segment.cpp:467
const rigidbody::Segment & findFirstSegmentWithDof(const rigidbody::Joints &model) const
Traverse the model hierarchy until it gets to a segment with a dof. If the current segment has at lea...
Definition Segment.cpp:568
const utils::String & nameDof(const size_t i) const
Return the name of the specified DoF.
Definition Segment.cpp:212
size_t nbGeneralizedTorque() const
Return the number of generalized torque.
Definition Segment.cpp:204
void setDofs(rigidbody::Joints &model, const utils::String &seqT, const utils::String &seqR, const std::vector< utils::Range > &QRanges, const std::vector< utils::Range > &QdotRanges, const std::vector< utils::Range > &QddotRanges, const std::vector< utils::Scalar > &jointDamping)
Set the DoF.
Definition Segment.cpp:313
size_t nbDofTrans() const
Return the number of translation DoF of the segment.
Definition Segment.cpp:206
size_t nbQddot() const
Return the number of generalized accelerations.
Definition Segment.cpp:210
void setDofCharacteristicsOnLastBody()
Set the DoF segment characteristics on the last body.
Definition Segment.cpp:451
std::shared_ptr< size_t > m_nbDofTrueOutside
Definition Segment.h:398
std::shared_ptr< std::vector< utils::Range > > m_QddotRanges
Definition Segment.h:416
const std::vector< utils::Range > & QdotRanges() const
Return the ranges for all the dof velocity, translations and rotations respectively.
Definition Segment.cpp:233
const utils::String & seqR() const
Return the angle sequence in text.
Definition Segment.cpp:220
const std::vector< utils::Scalar > & jointDampings() const
Return the joint damping to apply to the dynamics.
Definition Segment.cpp:257
void setQRanges(const std::vector< utils::Range > &QRanges)
Set the ranges for all the dof, translations and rotations respectively.
Definition Segment.cpp:226
bool isVectorHasDofDimension(const std::vector< T > &vec, bool isQLevel) const
Check if the vector has the same number of elements as the number of DoF.
Definition Segment.cpp:340
std::shared_ptr< std::vector< utils::Range > > m_QRanges
Definition Segment.h:408
size_t nbQ() const
Return the number of generalized coordinates.
Definition Segment.cpp:208
std::shared_ptr< size_t > m_nbQddot
Number of generalized accelerations.
Definition Segment.h:393
size_t getFirstDofIndexInGeneralizedCoordinates(const rigidbody::Joints &model) const
Returns the first index of the segment in the Generalized Coordinates vector.
Definition Segment.cpp:581
size_t nbDofRot() const
Return the number of rotation DoF of the segment.
Definition Segment.cpp:207
void setLocalJCS(rigidbody::Joints &model, utils::RotoTrans &rototrans)
set the joint coordinate system (JCS) in the parent reference frame
Definition Segment.cpp:281
std::shared_ptr< size_t > m_nbDofQuat
Definition Segment.h:404
void setType()
Set the type of the segment.
Definition Segment.cpp:192
void setQddotRanges(const std::vector< utils::Range > &QddotRanges)
Set the ranges for all the dof acceleration, translations and rotations respectively.
Definition Segment.cpp:249
std::shared_ptr< std::vector< utils::Scalar > > m_jointDampings
The joint dampings to apply to the dynamics.
Definition Segment.h:421
virtual void setJoints(rigidbody::Joints &model)
Function that adds the segment to the RBDL body set.
Definition Segment.cpp:499
size_t id() const
Return the Segment index.
Definition Segment.cpp:196
std::shared_ptr< SegmentCharacteristics > m_characteristics
Definition Segment.h:503
const std::vector< utils::Range > & QddotRanges() const
Return the ranges for all the dof acceleration, translations and rotations respectively.
Definition Segment.cpp:245
size_t nbQdot() const
Return the number of generalized velocities.
Definition Segment.cpp:209
std::shared_ptr< std::vector< size_t > > m_sequenceRot
Euler rotation sequence.
Definition Segment.h:476
size_t nbDof() const
Return the number of DoF of the segment.
Definition Segment.cpp:205
void updateCharacteristics(rigidbody::Joints &model, const SegmentCharacteristics &characteristics)
updateCharacteristics Change the inertia characteristics of the segment
Definition Segment.cpp:290
void setNumberOfDof(size_t nbTrans, size_t nbRot)
Set the total number of DoF.
Definition Segment.cpp:403
std::shared_ptr< size_t > m_nbDofTrue
Definition Segment.h:395
void setJointDampings(const std::vector< utils::Scalar > &jointDampings)
Set the joint damping to apply to the dynamics.
Definition Segment.cpp:261
std::shared_ptr< size_t > m_nbDofRot
Number of degrees of freedom in rotation.
Definition Segment.h:403
void setSequence(const utils::String &seqT, const utils::String &seqR)
Set angle and translation sequences, adjust angle sequence and redeclare if is necessary.
Definition Segment.cpp:424
std::shared_ptr< std::vector< size_t > > m_dofPosition
Position in the x, y, and z sequence.
Definition Segment.h:492
std::shared_ptr< size_t > m_nbDofTrans
Number of degrees of freedom in translation.
Definition Segment.h:401
std::shared_ptr< size_t > m_nbQdot
Number of generalized velocities.
Definition Segment.h:392
Segment()
Construct a Segment.
Definition Segment.cpp:20
std::shared_ptr< size_t > m_nbDof
Number of degrees of freedom.
Definition Segment.h:391
std::shared_ptr< std::vector< size_t > > m_idxDof
Index of the parent segment.
Definition Segment.h:437
void setQdotRanges(const std::vector< utils::Range > &QdotRanges)
Set the ranges for all the dof velocity, translations and rotations respectively.
Definition Segment.cpp:237
std::shared_ptr< utils::String > m_seqR
Euler rotation sequence.
Definition Segment.h:390
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
Node()
Construct Node.
Definition Node.cpp:8
const String & name() const
Return the name of the node.
Definition Node.cpp:43
std::shared_ptr< NODE_TYPE > m_typeOfNode
The type of the node.
Definition Node.h:92
void DeepCopy(const Node &other)
Deep copy of the node in another node.
Definition Node.cpp:35
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
Rotation rot() const
Return the rotation matrix.
Vector3d trans() const
Return the translation vector.
Wrapper of the SpatialTransform (rotation and translation).
Wrapper around the std::string class with augmented functionality.
Definition String.h:18
static String tolower(const String &str)
Convert a string to a lower case string.
Definition String.cpp:76
static String toupper(const String &str)
Convert a string to a upper case string.
Definition String.cpp:84
Wrapper around Eigen Vector3d and attach it to a parent.
Definition Vector3d.h:27