1#define BIORBD_API_EXPORTS
2#include "RigidBody/Segment.h"
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"
18using namespace BIORBD_NAMESPACE;
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)),
34 m_QRanges(std::make_shared<std::vector<utils::Range>>()),
35 m_QdotRanges(std::make_shared<std::vector<utils::Range>>()),
39 m_dof(std::make_shared<std::vector<RigidBodyDynamics::Joint>>()),
40 m_idxDof(std::make_shared<std::vector<size_t>>()),
43 m_nameDof(std::make_shared<std::vector<utils::String>>()),
57 const std::vector<utils::Range> &
QRanges,
65 m_cor(std::make_shared<utils::SpatialTransform>(cor)),
68 m_nbDof(std::make_shared<size_t>(0)),
69 m_nbQdot(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>>()),
81 m_dof(std::make_shared<std::vector<RigidBodyDynamics::Joint>>()),
82 m_idxDof(std::make_shared<std::vector<size_t>>()),
85 m_nameDof(std::make_shared<std::vector<utils::String>>()),
100 const std::vector<utils::Range> &
QRanges,
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)),
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>>()),
127 m_dof(std::make_shared<std::vector<RigidBodyDynamics::Joint>>()),
128 m_idxDof(std::make_shared<std::vector<size_t>>()),
131 m_nameDof(std::make_shared<std::vector<utils::String>>()),
168 for (
size_t i = 0; i < other.
m_dof->size(); ++i) {
169 (*m_dof)[i] = (*other.
m_dof)[i];
175 for (
size_t i = 0; i < other.
m_nameDof->size(); ++i) {
229 "QRanges must be empty or of the same size as the number of dof");
238 const std::vector<utils::Range> &
QdotRanges) {
241 "QdotRanges must be empty or of the same size as the number of dof");
253 "QddotRanges must be empty or of the same size as the number of dof");
265 "jointDampings must be empty or of the same size as the number of dof");
278 m_cor->rotation().transpose(),
m_cor->translation());
294 RigidBodyDynamics::Math::SpatialRigidBodyInertia rbi = RigidBodyDynamics::
295 Math::SpatialRigidBodyInertia::createFromMassComInertiaC(
317 const std::vector<utils::Range> &
QRanges,
325 size_t nRot(
seqR.length());
341 const std::vector<T> &vec,
342 bool isQLevel)
const {
345 return vec.size() == 0 || vec.size() == nTrans + nRot;
386 std::vector<size_t> &sequenceInteger,
388 for (
size_t i = 0; i < sequenceText.length(); i++) {
389 char tp = sequenceText.
tolower()[i];
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;
438 (*m_dofPosition)[i] = (*m_sequenceTrans)[i];
456 for (
size_t i = 0; i < *
m_nbDof - 1; i++) {
479 (*m_dof)[i] = RigidBodyDynamics::Joint(
480 RigidBodyDynamics::JointTypePrismatic, axis[(*
m_dofPosition)[i]]);
486 RigidBodyDynamics::Joint(RigidBodyDynamics::JointTypeSpherical);
489 (*m_dof)[i] = RigidBodyDynamics::Joint(
490 RigidBodyDynamics::JointTypeRevolute, axis[(*
m_dofPosition)[i]]);
495 (*m_dof)[0] = RigidBodyDynamics::Joint(RigidBodyDynamics::JointTypeFixed);
515 unsigned int parent_id(model.GetBodyId(
parent().c_str()));
517 if (parent_id == std::numeric_limits<unsigned int>::max()) {
521 (*m_idxDof)[0] = model.AddBody(
524 (*m_idxDof)[0] = model.AddBody(
527 (*m_idxDof)[0] = model.AddBody(
529 for (
size_t i = 1; i < *
m_nbDof; i++)
531 (*m_idxDof)[i] = model.AddBody(
532 static_cast<unsigned int>((*
m_idxDof)[i - 1]),
538 static_cast<unsigned int>((*
m_idxDof)[i - 1]),
550 for (
size_t i = 0; i <
nbDof(); ++i) {
560 "Type should be \"Rot\" or \"Trans\" and axis should be \"X\", \"Y\" "
563 "\"RotY\" for Rotation around y or \"TransX\" for Translation on x");
573 if (segment->
nbDof() > 0)
return *segment;
575 segment->
parent().compare(
"root"),
576 "Could not find any dof for " +
name() +
" up to the root.");
588 for (
int i = 0; i < static_cast<int>(model.
nbSegment()); ++i) {
591 if (!segment.
name().compare(toCompare))
return dofCount;
593 dofCount += segment.
nbDof();
596 throw std::runtime_error(
"Segment " +
name() +
" not found in the model");
This is the core of the musculoskeletal model in biorbd.
size_t nbSegment() const
Return the actual number of segment.
Segment & segment(size_t idx)
Get a segment of index idx.
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.
std::shared_ptr< std::vector< utils::String > > m_nameDof
To store the DoF names.
std::shared_ptr< std::vector< RigidBodyDynamics::Joint > > m_dof
bool isRotationAQuaternion() const
Return if the rotation DoF of this segment is a quaternion.
SegmentCharacteristics & characteristics()
Return the segment characteristics.
const std::vector< utils::Range > & QRanges() const
Return the ranges for all the dof, translations and rotations respectively.
utils::RotoTrans localJCS() const
Return the joint coordinate system (JCS) in the parent reference frame.
std::shared_ptr< bool > m_isQuaternion
If DoF in rotation is a Quaternion.
const utils::String & seqT() const
Return the translation sequence in text.
size_t getDofIdx(const utils::String &dofName) const
Return the index of a specified DoF.
std::shared_ptr< utils::SpatialTransform > m_cor
Attitude of the segment in parent reference frame.
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).
std::shared_ptr< utils::String > m_seqT
Translation sequence.
std::shared_ptr< std::vector< SegmentCharacteristics > > m_dofCharacteristics
Segment DeepCopy() const
Create a deep copy of Segment.
std::shared_ptr< std::vector< utils::Range > > m_QdotRanges
std::shared_ptr< int > m_idxInModel
Index in RBDL model.
virtual ~Segment()
Destroy the class properly.
std::shared_ptr< std::vector< size_t > > m_sequenceTrans
Translation sequence.
void determineIfRotIsQuaternion(const utils::String &seqR)
Determines if DoF in rotation is a Quaternion.
size_t getLastDofIndexInGeneralizedCoordinates(const rigidbody::Joints &model) const
Returns the last index of the segment in the Generalized Coordinates vector.
void fillSequence()
Fill the transation and rotation sequences.
virtual void setJointAxis()
Determine the rotation axis in relation to the requested sequence.
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...
const utils::String & nameDof(const size_t i) const
Return the name of the specified DoF.
size_t nbGeneralizedTorque() const
Return the number of generalized torque.
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.
size_t nbDofTrans() const
Return the number of translation DoF of the segment.
size_t nbQddot() const
Return the number of generalized accelerations.
void setDofCharacteristicsOnLastBody()
Set the DoF segment characteristics on the last body.
std::shared_ptr< size_t > m_nbDofTrueOutside
std::shared_ptr< std::vector< utils::Range > > m_QddotRanges
const std::vector< utils::Range > & QdotRanges() const
Return the ranges for all the dof velocity, translations and rotations respectively.
const utils::String & seqR() const
Return the angle sequence in text.
const std::vector< utils::Scalar > & jointDampings() const
Return the joint damping to apply to the dynamics.
void setQRanges(const std::vector< utils::Range > &QRanges)
Set the ranges for all the dof, translations and rotations respectively.
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.
std::shared_ptr< std::vector< utils::Range > > m_QRanges
size_t nbQ() const
Return the number of generalized coordinates.
std::shared_ptr< size_t > m_nbQddot
Number of generalized accelerations.
size_t getFirstDofIndexInGeneralizedCoordinates(const rigidbody::Joints &model) const
Returns the first index of the segment in the Generalized Coordinates vector.
size_t nbDofRot() const
Return the number of rotation DoF of the segment.
void setLocalJCS(rigidbody::Joints &model, utils::RotoTrans &rototrans)
set the joint coordinate system (JCS) in the parent reference frame
std::shared_ptr< size_t > m_nbDofQuat
void setType()
Set the type of the segment.
void setQddotRanges(const std::vector< utils::Range > &QddotRanges)
Set the ranges for all the dof acceleration, translations and rotations respectively.
std::shared_ptr< std::vector< utils::Scalar > > m_jointDampings
The joint dampings to apply to the dynamics.
virtual void setJoints(rigidbody::Joints &model)
Function that adds the segment to the RBDL body set.
size_t id() const
Return the Segment index.
std::shared_ptr< SegmentCharacteristics > m_characteristics
const std::vector< utils::Range > & QddotRanges() const
Return the ranges for all the dof acceleration, translations and rotations respectively.
size_t nbQdot() const
Return the number of generalized velocities.
std::shared_ptr< std::vector< size_t > > m_sequenceRot
Euler rotation sequence.
size_t nbDof() const
Return the number of DoF of the segment.
void updateCharacteristics(rigidbody::Joints &model, const SegmentCharacteristics &characteristics)
updateCharacteristics Change the inertia characteristics of the segment
void setNumberOfDof(size_t nbTrans, size_t nbRot)
Set the total number of DoF.
std::shared_ptr< size_t > m_nbDofTrue
void setJointDampings(const std::vector< utils::Scalar > &jointDampings)
Set the joint damping to apply to the dynamics.
std::shared_ptr< size_t > m_nbDofRot
Number of degrees of freedom in rotation.
void setSequence(const utils::String &seqT, const utils::String &seqR)
Set angle and translation sequences, adjust angle sequence and redeclare if is necessary.
std::shared_ptr< std::vector< size_t > > m_dofPosition
Position in the x, y, and z sequence.
std::shared_ptr< size_t > m_nbDofTrans
Number of degrees of freedom in translation.
std::shared_ptr< size_t > m_nbQdot
Number of generalized velocities.
Segment()
Construct a Segment.
std::shared_ptr< size_t > m_nbDof
Number of degrees of freedom.
std::shared_ptr< std::vector< size_t > > m_idxDof
Index of the parent segment.
void setQdotRanges(const std::vector< utils::Range > &QdotRanges)
Set the ranges for all the dof velocity, translations and rotations respectively.
std::shared_ptr< utils::String > m_seqR
Euler rotation sequence.
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.
const String & name() const
Return the name of the node.
std::shared_ptr< NODE_TYPE > m_typeOfNode
The type of the node.
void DeepCopy(const Node &other)
Deep copy of the node in another node.
const String & parent() const
Return the parent name of the node.
Homogenous matrix to describe translations and rotations simultaneously.
Rotation rot() const
Return the rotation matrix.
Vector3d trans() const
Return the translation vector.
Wrapper around the std::string class with augmented functionality.
static String tolower(const String &str)
Convert a string to a lower case string.
static String toupper(const String &str)
Convert a string to a upper case string.
Wrapper around Eigen Vector3d and attach it to a parent.