Biorbd Release_1.13.0
Loading...
Searching...
No Matches
ModelWriter.cpp
1#define BIORBD_API_EXPORTS
2#include "ModelWriter.h"
3
4#include <fstream>
5#include <iostream>
6
7#include "BiorbdModel.h"
8#include "RigidBody/IMU.h"
9#include "RigidBody/Mesh.h"
10#include "RigidBody/NodeSegment.h"
11#include "RigidBody/Segment.h"
12#include "RigidBody/SegmentCharacteristics.h"
13#include "Utils/Matrix3d.h"
14#include "Utils/Path.h"
15#include "Utils/Range.h"
16#include "Utils/String.h"
17#include "Utils/Vector.h"
18
19using namespace BIORBD_NAMESPACE;
20
21#ifndef BIORBD_USE_CASADI_MATH
22void Writer::writeModel(Model& model, const utils::Path& pathToWrite) {
23 utils::String sep("\t"); // separator in the file
24 utils::String comment("//"); // commentaire
25
26 // Manage the case where the destination folder does not exist
27 if (!pathToWrite.isFolderExist()) {
28 pathToWrite.createFolder();
29 }
30
31 // Open file
32 std::ofstream biorbdModelFile;
33 biorbdModelFile.open(pathToWrite.relativePath().c_str());
34
35 // Write file
36 biorbdModelFile << "version 3" << std::endl;
37 biorbdModelFile << std::endl;
38
39 // General information
40 biorbdModelFile << std::endl;
41 biorbdModelFile << comment << " General informations" << std::endl;
42 biorbdModelFile << std::endl;
43
44 // Information on the segments
45 std::vector<utils::RotoTrans> localJCS = model.localJCS();
46 for (size_t i = 0; i < model.nbSegment(); ++i) {
47 biorbdModelFile << comment << " Informations about "
48 << model.segment(i).name() << " segment" << std::endl;
49 biorbdModelFile << sep << comment << " Segment" << std::endl;
50 biorbdModelFile << sep << "segment" << sep << model.segment(i).name()
51 << std::endl;
52 biorbdModelFile << sep << sep << "parent" << sep
53 << model.segment(i).parent() << std::endl;
54 biorbdModelFile << sep << sep << "RTinMatrix" << sep << true << std::endl;
55 biorbdModelFile << sep << sep << "RT" << std::endl;
56 for (size_t j = 0; j < 4; ++j) {
57 biorbdModelFile << sep << sep << sep << localJCS[i].block(j, 0, 1, 4)
58 << std::endl;
59 }
60 if (model.segment(i).nbDofTrans() > 0) {
61 biorbdModelFile << sep << sep << "translations" << sep
62 << model.segment(i).seqT() << std::endl;
63 }
64 if (model.segment(i).nbDofRot() > 0) {
65 biorbdModelFile << sep << sep << "rotations" << sep
66 << model.segment(i).seqR() << std::endl;
67 }
68 const std::vector<utils::Range>& qRanges = model.segment(i).QRanges();
69 if (qRanges.size() > 0) {
70 biorbdModelFile << sep << sep << "rangesQ" << std::endl;
71 for (size_t j = 0; j < qRanges.size(); ++j) {
72 biorbdModelFile << sep << sep << sep << qRanges[j].min() << sep
73 << qRanges[j].max() << std::endl;
74 }
75 }
76 const std::vector<utils::Range>& qdotRanges =
77 model.segment(i).QdotRanges();
78 if (qdotRanges.size() > 0) {
79 biorbdModelFile << sep << sep << "rangesQdot" << std::endl;
80 for (size_t j = 0; j < qdotRanges.size(); ++j) {
81 biorbdModelFile << sep << sep << sep << qdotRanges[j].min() << sep
82 << qdotRanges[j].max() << std::endl;
83 }
84 }
85 const std::vector<utils::Range>& qddotRanges =
86 model.segment(i).QddotRanges();
87 if (qddotRanges.size() > 0) {
88 biorbdModelFile << sep << sep << "rangesQddot" << std::endl;
89 for (size_t j = 0; j < qddotRanges.size(); ++j) {
90 biorbdModelFile << sep << sep << sep << qddotRanges[j].min() << sep
91 << qddotRanges[j].max() << std::endl;
92 }
93 }
94 biorbdModelFile << sep << sep << "jointDampings" << sep;
95 for (auto damping : model.segment(i).jointDampings()) {
96 biorbdModelFile << sep << damping;
97 }
98 biorbdModelFile << std::endl;
99 biorbdModelFile << sep << sep << "mass" << sep
100 << model.segment(i).characteristics().mass() << std::endl;
101 biorbdModelFile << sep << sep << "inertia" << std::endl
102 << sep << sep << sep
103 << model.segment(i).characteristics().inertia()
104 << std::endl;
105 biorbdModelFile
106 << sep << sep << "com" << sep
107 << model.segment(i).characteristics().mCenterOfMass.transpose()
108 << std::endl;
109 if (model.segment(i).characteristics().mesh().path().filename().compare(
110 "")) {
111 biorbdModelFile
112 << sep << sep << "meshfile" << sep
113 << model.segment(i).characteristics().mesh().path().originalPath()
114 << std::endl;
115 biorbdModelFile
116 << sep << sep << "meshcolor" << sep
117 << model.segment(i).characteristics().mesh().color().transpose()
118 << std::endl;
119 biorbdModelFile
120 << sep << sep << "meshscale" << sep
121 << model.segment(i).characteristics().mesh().getScale().transpose()
122 << std::endl;
123 biorbdModelFile
124 << sep << sep << "meshrt" << sep
127 utils::String("xyz"))
128 .transpose()
129 << " xyz "
130 << model.segment(i)
132 .mesh()
133 .getRotation()
134 .trans()
135 .transpose()
136 << std::endl;
137 }
138 biorbdModelFile << sep << "endsegment" << sep << std::endl;
139 biorbdModelFile << std::endl;
140
141 // Write the prospective markers
142 std::vector<rigidbody::NodeSegment> markers(
143 model.markers(model.segment(i).name()));
144 if (markers.size() > 0) {
145 biorbdModelFile << sep << comment << " Markers" << std::endl;
146 for (size_t j = 0; j < markers.size(); ++j) {
147 biorbdModelFile << sep << "marker" << sep
148 << markers[j].utils::Node::name() << std::endl;
149 biorbdModelFile << sep << sep << "parent" << sep << markers[j].parent()
150 << std::endl;
151 biorbdModelFile << sep << sep << "position" << sep
152 << markers[j].transpose() << std::endl;
153 biorbdModelFile << sep << sep << "technical" << sep
154 << markers[j].isTechnical() << std::endl;
155 biorbdModelFile << sep << sep << "anatomical" << sep
156 << markers[j].isAnatomical() << std::endl;
157 if (markers[j].nbAxesToRemove() != 0) {
158 biorbdModelFile << sep << sep << "axestoremove" << sep
159 << markers[j].axesToRemoveAsString() << std::endl;
160 }
161 biorbdModelFile << sep << "endmarker" << sep << std::endl;
162 }
163 }
164 biorbdModelFile << std::endl;
165
166 // Write the inertial units
167 std::vector<rigidbody::IMU> imus(model.IMU(model.segment(i).name()));
168 if (imus.size() > 0) {
169 biorbdModelFile << sep << comment << " Inertial Magnetic Unit"
170 << std::endl;
171 for (size_t j = 0; j < imus.size(); ++j) {
172 biorbdModelFile << sep << "imu" << sep << imus[j].utils::Node::name()
173 << std::endl;
174 biorbdModelFile << sep << sep << "parent" << sep << imus[j].parent()
175 << std::endl;
176 biorbdModelFile << sep << sep << "RTinMatrix" << sep << true
177 << std::endl;
178 biorbdModelFile << sep << sep << "RT" << std::endl;
179 for (size_t k = 0; k < 4; ++k) {
180 biorbdModelFile << sep << sep << sep << imus[j].block(k, 0, 1, 4)
181 << std::endl;
182 }
183 biorbdModelFile << sep << sep << "technical" << sep
184 << imus[j].isTechnical() << std::endl;
185 biorbdModelFile << sep << sep << "anatomical" << sep
186 << imus[j].isAnatomical() << std::endl;
187 biorbdModelFile << sep << "endimu" << sep << std::endl;
188 }
189 }
190
191 // Write the custom RT
192 std::vector<utils::RotoTransNode> rts(model.RTs(model.segment(i).name()));
193 if (rts.size() > 0) {
194 biorbdModelFile << sep << comment << " Custom RT" << std::endl;
195 for (size_t j = 0; j < rts.size(); ++j) {
196 biorbdModelFile << sep << "customRT" << sep
197 << rts[j].utils::Node::name() << std::endl;
198 biorbdModelFile << sep << sep << "parent" << sep << rts[j].parent()
199 << std::endl;
200 biorbdModelFile << sep << sep << "RTinMatrix" << sep << true
201 << std::endl;
202 biorbdModelFile << sep << sep << "RT" << std::endl;
203 for (size_t k = 0; k < 4; ++k) {
204 biorbdModelFile << sep << sep << sep << rts[j].block(k, 0, 1, 4)
205 << std::endl;
206 }
207 biorbdModelFile << sep << "endcustomrt" << sep << std::endl;
208 }
209 }
210 biorbdModelFile << std::endl;
211 biorbdModelFile << std::endl;
212 }
213 biorbdModelFile << std::endl;
214 biorbdModelFile << std::endl;
215 biorbdModelFile << std::endl;
216 biorbdModelFile << std::endl;
217 biorbdModelFile << std::endl;
218 biorbdModelFile << std::endl;
219
220 biorbdModelFile << std::endl;
221 // Close file
222 biorbdModelFile.close();
223}
224#endif
The actual musculoskeletal model that holds everything in biorbd.
static void writeModel(Model &model, const utils::Path &pathToWrite)
Writes the model on the computer.
const std::vector< rigidbody::IMU > & IMU() const
Return all the IMU in the local reference of the segment.
Definition IMUs.cpp:55
size_t nbSegment() const
Return the actual number of segment.
Definition Joints.cpp:244
std::vector< utils::RotoTrans > localJCS() const
Return all the joint coordinate system (JCS) in its parent reference frame.
Definition Joints.cpp:375
Segment & segment(size_t idx)
Get a segment of index idx.
Definition Joints.cpp:205
std::vector< NodeSegment > markers(bool removeAxis=true) const
Return a copy of all the markers in their respective parent reference frame.
Definition Markers.cpp:133
utils::RotoTrans & getRotation() const
Get the rotation applied to the vertex.
Definition Mesh.cpp:80
utils::Vector3d & color() const
Get the patch color.
Definition Mesh.cpp:62
utils::Vector3d & getScale() const
Get the scaling applied to the vertex wrt to 0, 0, 0.
Definition Mesh.cpp:91
utils::Path & path()
Return the path of the mesh file.
Definition Mesh.cpp:109
const std::vector< utils::RotoTransNode > & RTs() const
Return all the RTs in the local reference of the segment.
const utils::Matrix3d inertia() const
Returns the segment inertia matrix.
utils::Scalar mass() const
Returns the segment mass.
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
const utils::String & seqT() const
Return the translation sequence in text.
Definition Segment.cpp:218
size_t nbDofTrans() const
Return the number of translation DoF of the segment.
Definition Segment.cpp:206
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
size_t nbDofRot() const
Return the number of rotation DoF of the segment.
Definition Segment.cpp:207
const std::vector< utils::Range > & QddotRanges() const
Return the ranges for all the dof acceleration, translations and rotations respectively.
Definition Segment.cpp:245
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
Collection of methods to manipulate path.
Definition Path.h:16
void createFolder() const
To create folder on the computer.
Definition Path.cpp:361
const String & originalPath() const
Return original path as it was at constructor time.
Definition Path.cpp:308
const String & filename() const
Return the filename.
Definition Path.cpp:321
bool isFolderExist() const
Test if folder exists on the computer.
Definition Path.cpp:105
String relativePath() const
Return relative path to current working directory.
Definition Path.cpp:169
Vector3d trans() const
Return the translation vector.
static Vector toEulerAngles(const RotoTrans &rt, const String &seq)
Return extracted angles from the rotation matrix into Euler angles using the provided sequence.
Wrapper around the std::string class with augmented functionality.
Definition String.h:18