Biorbd Release_1.13.0
Loading...
Searching...
No Matches
RotoTransNodes.cpp
1#define BIORBD_API_EXPORTS
2#include "RigidBody/RotoTransNodes.h"
3
4#include <rbdl/Model.h>
5
6#include <rbdl/Kinematics.h>
7
8#include "RigidBody/GeneralizedCoordinates.h"
9#include "RigidBody/Joints.h"
10#include "RigidBody/Segment.h"
11#include "Utils/Matrix.h"
12#include "Utils/Rotation.h"
13#include "Utils/RotoTransNode.h"
14#include "Utils/String.h"
15
16using namespace BIORBD_NAMESPACE;
17
19 : m_RTs(std::make_shared<std::vector<utils::RotoTransNode>>()) {
20 // ctor
21}
22
27
29
35
37 const rigidbody::RotoTransNodes &other) {
38 m_RTs->resize(other.m_RTs->size());
39 for (size_t i = 0; i < other.m_RTs->size(); ++i) {
40 (*m_RTs)[i] = (*other.m_RTs)[i].DeepCopy();
41 }
42}
43
47
48// Add a new marker to the existing pool of markers
50 m_RTs->push_back(utils::RotoTransNode(RotoTrans));
51}
52
53size_t rigidbody::RotoTransNodes::nbRTs() const { return m_RTs->size(); }
54
55size_t rigidbody::RotoTransNodes::size() const { return m_RTs->size(); }
56
57// Get the markers in the global reference
58const std::vector<utils::RotoTransNode> &rigidbody::RotoTransNodes::RTs()
59 const {
60 return *m_RTs;
61}
62
63std::vector<utils::RotoTransNode> rigidbody::RotoTransNodes::RTs(
64 const utils::String &segmentName) {
65 std::vector<utils::RotoTransNode> pos;
66 for (size_t i = 0; i < nbRTs();
67 ++i) // Scan through all the markers and select the good ones
68 if (!RT(i).parent().compare(segmentName)) {
69 pos.push_back(RT(i));
70 }
71 return pos;
72}
73
75 return (*m_RTs)[idx];
76}
77
78// Get the RotoTransNodes at the position given by Q
79std::vector<utils::RotoTransNode> rigidbody::RotoTransNodes::RTs(
81 bool updateKin) {
82 std::vector<utils::RotoTransNode> pos;
83 for (size_t i = 0; i < nbRTs(); ++i) {
84 pos.push_back(RT(Q, i, updateKin));
85 updateKin = false;
86 }
87
88 return pos;
89}
90
91// Get an IMU at the position given by Q
94 size_t idx,
95 bool updateKin) {
96 // Assuming that this is also a Joints type (via BiorbdModel)
97 rigidbody::Joints &model = dynamic_cast<rigidbody::Joints &>(*this);
98
99 utils::RotoTransNode node = RT(idx);
100 return model.globalJCS(Q, node.parent(), updateKin) * node;
101}
102
103std::vector<utils::RotoTransNode> rigidbody::RotoTransNodes::segmentRTs(
105 size_t idx,
106 bool updateKin) {
107 // Assuming that this is also a Joints type (via BiorbdModel)
108 rigidbody::Joints &model = dynamic_cast<rigidbody::Joints &>(*this);
109
110 // Segment name to find
111 utils::String name(model.segment(idx).name());
112
113 std::vector<utils::RotoTransNode> pos;
114 for (size_t i = 0; i < nbRTs();
115 ++i) // scan all the markers and select the right ones
116 if (!((*m_RTs)[i]).parent().compare(name)) {
117 pos.push_back(RT(Q, i, updateKin));
118 updateKin = false;
119 }
120
121 return pos;
122}
123
124// Get the Jacobian of the markers
125std::vector<utils::Matrix> rigidbody::RotoTransNodes::RTsJacobian(
127 bool updateKin) {
128 // Assuming that this is also a Joints type (via BiorbdModel)
129 rigidbody::Joints &model = dynamic_cast<rigidbody::Joints &>(*this);
130
131 std::vector<utils::Matrix> G;
132
133 for (size_t idx = 0; idx < nbRTs(); ++idx) {
134 // Actual marker
135 utils::RotoTransNode node = RT(idx);
136
137 size_t id = model.GetBodyId(node.parent().c_str());
138 utils::Matrix G_tp(utils::Matrix::Zero(9, model.dof_count));
139
140 // Calculate the Jacobian of this Tag
141 model.CalcMatRotJacobian(Q, id, node.rot(), G_tp, updateKin);
142 updateKin = false; // False for speed
143
144 G.push_back(G_tp);
145 }
146
147 return G;
148}
149
150std::vector<utils::String> rigidbody::RotoTransNodes::RTsNames() {
151 // Extract the name of all the markers of a model
152 std::vector<utils::String> names;
153 for (size_t i = 0; i < nbRTs(); ++i) {
154 names.push_back(RT(i).utils::Node::name());
155 }
156 return names;
157}
158
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
std::vector< utils::Matrix > RTsJacobian(const GeneralizedCoordinates &Q, bool updateKin=true)
Return the jacobian of the RTs.
const utils::RotoTransNode & RT(size_t idx)
Return the RTs of a specified index.
size_t size() const
Return the number of RTs in the set.
size_t nbRTs() const
Return the number of RTs in the set.
RotoTransNodes DeepCopy() const
Deep copy of the RTs data.
std::vector< utils::String > RTsNames()
Return the names of the RTs.
std::shared_ptr< std::vector< utils::RotoTransNode > > m_RTs
All the RTs.
std::vector< utils::RotoTransNode > segmentRTs(const GeneralizedCoordinates &Q, size_t idx, bool updateKin=true)
Return all the RTs on a specified segment.
const std::vector< utils::RotoTransNode > & RTs() const
Return all the RTs in the local reference of the segment.
virtual ~RotoTransNodes()
Destroy the class properly.
virtual void clear()
Return to an internal state similar to initial declaration.
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