Biorbd Release_1.13.0
Loading...
Searching...
No Matches
ModelReader.cpp
1#define BIORBD_API_EXPORTS
2#include "ModelReader.h"
3
4#include <fstream>
5#include <limits.h>
6#include <sstream>
7
8#include "BiorbdModel.h"
9#include "RigidBody/GeneralizedCoordinates.h"
10#include "RigidBody/IMU.h"
11#include "RigidBody/Mesh.h"
12#include "RigidBody/MeshFace.h"
13#include "RigidBody/NodeSegment.h"
14#include "RigidBody/SegmentCharacteristics.h"
15#include "RigidBody/SoftContactSphere.h"
16#include "Utils/Equation.h"
17#include "Utils/Error.h"
18#include "Utils/IfStream.h"
19#include "Utils/Range.h"
20#include "Utils/Rotation.h"
21#include "Utils/SpatialVector.h"
22#include "Utils/String.h"
23#include "Utils/Vector.h"
24#include "Utils/Vector3d.h"
25
26#ifdef MODULE_ACTUATORS
27#include "InternalForces/Actuators/ActuatorConstant.h"
28#include "InternalForces/Actuators/ActuatorGauss3p.h"
29#include "InternalForces/Actuators/ActuatorGauss6p.h"
30#include "InternalForces/Actuators/ActuatorLinear.h"
31#include "InternalForces/Actuators/ActuatorSigmoidGauss3p.h"
32#endif // MODULE_ACTUATORS
33
34#ifdef MODULE_MUSCLES
35#include "InternalForces/Muscles/Characteristics.h"
36#include "InternalForces/Muscles/FatigueParameters.h"
37#include "InternalForces/Muscles/Muscle.h"
38#include "InternalForces/Muscles/MuscleGeometry.h"
39#include "InternalForces/Muscles/MuscleGroup.h"
40#include "InternalForces/Muscles/State.h"
41#include "InternalForces/Muscles/StateDynamicsBuchanan.h"
42#endif // MODULE_MUSCLES
43
44#ifdef MODULE_PASSIVE_TORQUES
45#include "InternalForces/PassiveTorques/PassiveTorqueConstant.h"
46#include "InternalForces/PassiveTorques/PassiveTorqueExponential.h"
47#include "InternalForces/PassiveTorques/PassiveTorqueLinear.h"
48#endif
49
50#ifdef MODULE_LIGAMENTS
51#include "InternalForces/Ligaments/Ligament.h"
52#include "InternalForces/Ligaments/LigamentCharacteristics.h"
53#include "InternalForces/Ligaments/LigamentConstant.h"
54#include "InternalForces/Ligaments/LigamentSpringLinear.h"
55#include "InternalForces/Ligaments/LigamentSpringSecondOrder.h"
56#include "InternalForces/Ligaments/Ligaments.h"
57#endif
58
59#if defined(MODULE_ACTUATORS) || defined(MODULE_MUSCLES) || \
60 defined(MODULE_LIGAMENTS)
61#include "InternalForces/Geometry.h"
62#include "InternalForces/PathModifiers.h"
63#include "InternalForces/ViaPoint.h"
64#include "InternalForces/WrappingHalfCylinder.h"
65#endif
66#ifdef MODULE_VTP_FILES_READER
67#include "tinyxml2.h"
68#endif
69
70using namespace BIORBD_NAMESPACE;
71
72// ------ Public methods ------ //
74 // Add the elements that have been entered
75 Model model;
76 Reader::readModelFile(path, &model);
77 return model;
78}
79
80void Reader::readModelFile(const utils::Path &path, Model *model) {
81 // Open file
82 if (!path.isFileReadable())
83 utils::Error::raise("File " + path.absolutePath() + " could not be open");
84
85#ifdef _WIN32
86 utils::IfStream file(
87 utils::Path::toWindowsFormat(path.absolutePath()).c_str(), std::ios::in);
88#else
89 utils::IfStream file(path.absolutePath().c_str(), std::ios::in);
90#endif
91
92 // Read file
93 utils::String main_tag;
94 utils::String property_tag;
95 utils::String subproperty_tag;
96 utils::String meshFolderPrefix;
97
98 // Variable used to replace doubles
99 std::map<utils::Equation, double> variable;
100
101 // Determine the file version
102 file.readSpecificTag("version", main_tag);
103 size_t version(static_cast<size_t>(atoi(main_tag.c_str())));
105 (version == 1 || version == 2 || version == 3 || version == 4),
106 "Version " + main_tag + " is not implemented yet");
107
108#ifdef MODULE_ACTUATORS
109 bool hasActuators = false;
110#endif // MODULE_ACTUATORS
111
112 utils::String name;
113 try {
114 while (file.read(
115 main_tag)) { // Attempt read into main_tag, return false if it fails
116 // Reinitialize some tags
117 name = "";
118 property_tag = "";
119 subproperty_tag = "";
120
121 // If it is a segment
122 if (!main_tag.tolower().compare("segment")) {
123 file.read(name);
124 utils::String parent_str("root");
125 utils::String trans = "";
126 utils::String rot = "";
127 bool RTinMatrix(true);
128 if (version >= 3) { // By default for version 3 (not in matrix)
129 RTinMatrix = false;
130 }
131 bool isRTset(false);
132 bool isInertiaSet(false);
133 double mass = 0.00000001;
134 utils::Matrix3d inertia(utils::Matrix3d::Zero());
136 utils::Vector3d com(0, 0, 0);
137 rigidbody::Mesh mesh;
138 bool isMeshSet(false);
139 int segmentByFile(-1); // -1 non setté, 0 pas par file, 1 par file
140 std::vector<utils::Range> QRanges;
141 std::vector<utils::Range> QdotRanges;
142 std::vector<utils::Range> QddotRanges;
143 std::vector<utils::Scalar> jointDampings;
144 bool isRangeQSet(false); // Ranges must be done only after translation
145 // AND rotations tags
146 bool isRangeQdotSet(false); // Ranges must be done only after
147 // translation AND rotations tags
148 bool isRangeQddotSet(false); // Ranges must be done only after
149 // translation AND rotations tags
150 while (file.read(property_tag) &&
151 property_tag.tolower().compare("endsegment")) {
152 if (!property_tag.tolower().compare("parent")) {
153 // Dynamically find the parent number
154 file.read(parent_str);
155 if (parent_str.tolower().compare("root")) {
157 model->GetBodyId(parent_str.c_str()),
158 "Wrong name in a segment");
159 }
160 } else if (!property_tag.tolower().compare("translations")) {
162 !isRangeQSet,
163 "Translations must appear before the rangesq tag");
165 !isRangeQdotSet,
166 "Translations must appear before the rangesqdot tag");
168 !isRangeQddotSet,
169 "Translations must appear before the rangesqddot tag");
170 file.read(trans);
171 } else if (!property_tag.tolower().compare("rotations")) {
173 !isRangeQSet, "Rotations must appear before the rangesq tag");
175 !isRangeQdotSet,
176 "Rotations must appear before the rangesqdot tag");
178 !isRangeQddotSet,
179 "Rotations must appear before the rangesqddot tag");
180 file.read(rot);
181 } else if (
182 !property_tag.tolower().compare("ranges") ||
183 !property_tag.tolower().compare("rangesq")) {
184 double min, max;
185 size_t rotLength = !rot.compare("q") ? 4 : rot.length();
186 for (size_t i = 0; i < trans.length() + rotLength; ++i) {
187 file.read(min);
188 file.read(max);
189 QRanges.push_back(utils::Range(min, max));
190 }
191 isRangeQSet = true;
192 } else if (!property_tag.tolower().compare("rangesqdot")) {
193 double min, max;
194 size_t rotLength = !rot.compare("q") ? 3 : rot.length();
195 for (size_t i = 0; i < trans.length() + rotLength; ++i) {
196 file.read(min);
197 file.read(max);
198 QdotRanges.push_back(utils::Range(min, max));
199 }
200 isRangeQdotSet = true;
201 } else if (!property_tag.tolower().compare("rangesqddot")) {
202 double min, max;
203 size_t rotLength = !rot.compare("q") ? 3 : rot.length();
204 for (size_t i = 0; i < trans.length() + rotLength; ++i) {
205 file.read(min);
206 file.read(max);
207 QddotRanges.push_back(utils::Range(min, max));
208 }
209 isRangeQddotSet = true;
210 } else if (!property_tag.tolower().compare("jointdampings")) {
211 size_t rotLength = !rot.compare("q") ? 3 : rot.length();
212 for (size_t i = 0; i < trans.length() + rotLength; ++i) {
213 double damping;
214 file.read(damping);
215 jointDampings.push_back(damping);
216 }
217 } else if (!property_tag.tolower().compare("mass")) {
218 file.read(mass, variable);
219 } else if (
220 !property_tag.tolower().compare("inertia") ||
221 !property_tag.tolower().compare("inertiamatrix")) {
223 !isInertiaSet,
224 "Inertia matrix cannot be set twice. Please note that "
225 "'Inertia', 'InertiaMatrix' and 'RadiiOfGyration' all set the "
226 "inertia matrix.");
227 readMatrix33(file, variable, inertia);
228 isInertiaSet = true;
229 } else if (!property_tag.tolower().compare("inertia_xxyyzz")) {
231 !isInertiaSet,
232 "Inertia matrix cannot be set twice. Please note that "
233 "'Inertia', 'InertiaMatrix' and 'RadiiOfGyration' all set the "
234 "inertia matrix.");
235 utils::Vector3d inertia_xxyyzz;
236 readVector3d(file, variable, inertia_xxyyzz);
237 inertia = utils::Matrix3d(
238 inertia_xxyyzz[0],
239 0,
240 0,
241 0,
242 inertia_xxyyzz[1],
243 0,
244 0,
245 0,
246 inertia_xxyyzz[2]);
247 isInertiaSet = true;
248 } else if (!property_tag.tolower().compare("rtinmatrix")) {
250 isRTset == false, "RT should not appear before RTinMatrix");
251 file.read(RTinMatrix);
252 } else if (!property_tag.tolower().compare("rt")) {
253 readRtMatrix(file, variable, RTinMatrix, RT);
254 isRTset = true;
255 } else if (
256 !property_tag.tolower().compare("com") ||
257 !property_tag.tolower().compare("centerofmass")) {
258 readVector3d(file, variable, com);
259 } else if (
260 !property_tag.tolower().compare("forceplate") ||
261 !property_tag.tolower().compare("externalforceindex")) {
262 std::cout
263 << "Please note that the 'externalforceindex' or 'forceplate' "
264 "tags were removed when the ExternalForceSet was "
265 "introduce.\n"
266 << "Please consider removing these tag from your model file."
267 << std::endl;
268 } else if (!property_tag.tolower().compare("mesh")) {
269 if (segmentByFile == -1) {
270 segmentByFile = 0;
271 } else if (segmentByFile == 1) {
272 utils::Error::raise("You must not mix file and mesh in segment");
273 }
274 utils::Vector3d tp(0, 0, 0);
275 readVector3d(file, variable, tp);
276 mesh.addPoint(tp);
277 isMeshSet = true;
278 } else if (!property_tag.tolower().compare("patch")) {
279 if (segmentByFile == -1) {
280 segmentByFile = 0;
281 } else if (segmentByFile == 1) {
282 utils::Error::raise("You must not mix file and mesh in segment");
283 }
285 for (size_t i = 0; i < 3; ++i) {
286 file.read(tp(i));
287 }
288 mesh.addFace(tp);
289 } else if (!property_tag.tolower().compare("meshfile")) {
290 if (segmentByFile == -1) {
291 segmentByFile = 1;
292 } else if (segmentByFile == 0) {
293 utils::Error::raise("You must not mix file and mesh in segment");
294 }
295 utils::String filePathInString;
296 file.read(filePathInString);
297 utils::Path filePath(meshFolderPrefix + filePathInString);
298 utils::String extension(filePath.extension().tolower());
299 if (!extension.compare("biomesh")) {
301 path.folder() + filePath.relativePath());
302 } else if (!extension.compare("ply")) {
303 mesh = readMeshFilePly(path.folder() + filePath.relativePath());
304 } else if (!extension.compare("obj")) {
305 mesh = readMeshFileObj(path.folder() + filePath.relativePath());
306 }
307#ifdef MODULE_VTP_FILES_READER
308 else if (!extension.compare("vtp")) {
309 mesh = readMeshFileVtp(path.folder() + filePath.relativePath());
310 }
311#endif
312 else if (!extension.compare("stl")) {
313 mesh = readMeshFileStl(path.folder() + filePath.relativePath());
314 } else {
316 filePath.extension() + " is an unrecognized mesh file");
317 }
318 isMeshSet = true;
319 } else if (!property_tag.tolower().compare("meshrt")) {
321 isMeshSet,
322 "mesh(es) or meshfile should be declared before meshrt");
324 readRtMatrix(file, variable, false, meshRT);
325 mesh.rotate(meshRT);
326 } else if (!property_tag.tolower().compare("meshscale")) {
328 isMeshSet,
329 "mesh(es) or meshfile should be declared before meshscale");
330 utils::Vector3d scaler;
331 readVector3d(file, variable, scaler);
332 mesh.scale(scaler);
333 } else if (!property_tag.tolower().compare("meshcolor")) {
334 utils::Vector3d meshColor;
335 readVector3d(file, variable, meshColor);
336 mesh.setColor(meshColor);
337 }
338 }
339 if (!isRangeQSet) {
340 size_t rotLength(0);
341 if (rot.compare("q")) {
342 // If not a quaternion
343 rotLength = rot.length();
344 } else {
345 rotLength = 4;
346 }
347 for (size_t i = 0; i < trans.length() + rotLength; ++i) {
348 if (!rot.compare("q") && i >= trans.length()) {
349 QRanges.push_back(utils::Range(-1, 1));
350 } else {
351 QRanges.push_back(utils::Range());
352 }
353 }
354 }
355 if (!isRangeQdotSet) {
356 size_t rotLength(0);
357 if (rot.compare("q")) {
358 // If not a quaternion
359 rotLength = rot.length();
360 } else {
361 rotLength = 3;
362 }
363 for (size_t i = 0; i < trans.length() + rotLength; ++i) {
364 QdotRanges.push_back(utils::Range(-M_PI * 10, M_PI * 10));
365 }
366 }
367 if (!isRangeQddotSet) {
368 size_t rotLength(0);
369 if (rot.compare("q")) {
370 // If not a quaternion
371 rotLength = rot.length();
372 } else {
373 rotLength = 3;
374 }
375 for (size_t i = 0; i < trans.length() + rotLength; ++i) {
376 QddotRanges.push_back(utils::Range(-M_PI * 100, M_PI * 100));
377 }
378 }
379 rigidbody::SegmentCharacteristics characteristics(
380 mass, com, inertia, mesh);
381 model->AddSegment(
382 name,
383 parent_str,
384 trans,
385 rot,
386 QRanges,
387 QdotRanges,
388 QddotRanges,
389 jointDampings,
390 characteristics,
391 RT);
392 } else if (!main_tag.tolower().compare("mesh_folder_prefix")) {
393 file.read(meshFolderPrefix);
394 } else if (!main_tag.tolower().compare("gravity")) {
395 utils::Vector3d gravity(0, 0, 0);
396 readVector3d(file, variable, gravity);
397 model->gravity = gravity;
398 } else if (!main_tag.tolower().compare("variables")) {
399 utils::String var;
400 while (file.read(var) && var.tolower().compare("endvariables")) {
401 if (!var(0).compare("$")) {
402 double value;
403 file.read(value);
405 variable.find(var) == variable.end(),
406 "Variable already defined");
407 variable[var] = value;
408 }
409 }
410 } else if (!main_tag.tolower().compare("marker")) {
411 utils::String name;
412 file.read(name);
413 size_t parent_int = 0;
414 utils::String parent_str("root");
415 utils::Vector3d pos(0, 0, 0);
416 bool technical = true;
417 bool anatomical = false;
418 utils::String axesToRemove = "";
419 while (file.read(property_tag) &&
420 property_tag.tolower().compare("endmarker"))
421 if (!property_tag.tolower().compare("parent")) {
422 // Dynamically find the parent number
423 file.read(parent_str);
424 parent_int =
425 static_cast<size_t>(model->GetBodyId(parent_str.c_str()));
426 // if parent_int still equals zero, no name has concurred
428 model->IsBodyId(static_cast<unsigned int>(parent_int)),
429 "Wrong name in a segment");
430 } else if (!property_tag.tolower().compare("position")) {
431 readVector3d(file, variable, pos);
432 } else if (!property_tag.tolower().compare("technical")) {
433 file.read(technical);
434 } else if (!property_tag.tolower().compare("anatomical")) {
435 file.read(anatomical);
436 } else if (!property_tag.tolower().compare("axestoremove")) {
437 file.read(axesToRemove);
438 }
439
440 model->addMarker(
441 pos,
442 name,
443 parent_str,
444 technical,
445 anatomical,
446 axesToRemove,
447 static_cast<int>(parent_int));
448 } else if (!main_tag.tolower().compare("mimu") && version >= 4) {
449 utils::Error::raise("MIMU is no more the right tag, change it to IMU!");
450 } else if (
451 !main_tag.tolower().compare("imu") ||
452 !main_tag.tolower().compare("mimu") ||
453 !main_tag.tolower().compare("customrt")) {
454 utils::String rtType(main_tag.tolower());
455 utils::String name;
456 file.read(name);
457 utils::String parent_str("root");
459 bool RTinMatrix(true);
460 if (version >= 3) { // By default for version 3 (not in matrix)
461 RTinMatrix = false;
462 }
463 bool isRTset(false);
464 bool technical = true;
465 bool anatomical = false;
466 bool firstTag = true;
467 bool fromMarkers(false);
468 utils::String originMarkerName("");
469 utils::String firstAxis("");
470 std::vector<utils::String> firstAxisMarkerNames(2);
471 utils::String secondAxis("");
472 std::vector<utils::String> secondAxisMarkerNames(2);
473 utils::String axisToRecalculate("");
474 while (file.read(property_tag) &&
475 !(!property_tag.tolower().compare("endimu") ||
476 !property_tag.tolower().compare("endmimu") ||
477 !property_tag.tolower().compare("endcustomrt"))) {
478 if (!property_tag.tolower().compare("parent")) {
479 // Dynamically find the parent number
480 file.read(parent_str);
481 // If parent_int still equals zero, no name has concurred
483 model->IsBodyId(model->GetBodyId(parent_str.c_str())),
484 "Wrong name in a segment");
485 } else if (!property_tag.tolower().compare("frommarkers")) {
486 if (!firstTag) {
488 "The tag 'fromMarkers' should appear first in the IMU " +
489 name);
490 } else {
491 fromMarkers = true;
492 }
493 } else if (!property_tag.tolower().compare("technical")) {
494 file.read(technical);
495 } else if (!property_tag.tolower().compare("anatomical")) {
496 file.read(anatomical);
497 }
498
499 if (fromMarkers) {
500 if (!property_tag.tolower().compare("originmarkername")) {
501 file.read(originMarkerName);
502 } else if (!property_tag.tolower().compare("firstaxis")) {
503 file.read(firstAxis);
504 } else if (!property_tag.tolower().compare(
505 "firstaxismarkernames")) {
506 for (size_t i = 0; i < 2; ++i) {
507 file.read(firstAxisMarkerNames[i]);
508 }
509 } else if (!property_tag.tolower().compare("secondaxis")) {
510 file.read(secondAxis);
511 } else if (!property_tag.tolower().compare(
512 "secondaxismarkernames")) {
513 for (size_t i = 0; i < 2; ++i) {
514 file.read(secondAxisMarkerNames[i]);
515 }
516 } else if (!property_tag.tolower().compare("recalculate")) {
517 file.read(axisToRecalculate);
518 }
519 } else {
520 if (!property_tag.tolower().compare("rtinmatrix")) {
522 isRTset == false, "RT should not appear before RTinMatrix");
523 file.read(RTinMatrix);
524 } else if (!property_tag.tolower().compare("rt")) {
525 readRtMatrix(file, variable, RTinMatrix, RT);
526 isRTset = true;
527 }
528 }
529 }
530 if (fromMarkers) {
531 std::vector<rigidbody::NodeSegment> allMarkerOnSegment(
532 model->markers(parent_str));
533 rigidbody::NodeSegment origin, axis1Beg, axis1End, axis2Beg, axis2End;
534 bool isOrigin(false), isAxis1Beg(false), isAxis1End(false),
535 isAxis2Beg(false), isAxis2End(false);
536 for (auto mark : allMarkerOnSegment) {
537 if (!mark.utils::Node::name().compare(originMarkerName)) {
538 origin = mark;
539 isOrigin = true;
540 }
541 if (!mark.utils::Node::name().compare(firstAxisMarkerNames[0])) {
542 axis1Beg = mark;
543 isAxis1Beg = true;
544 }
545 if (!mark.utils::Node::name().compare(firstAxisMarkerNames[1])) {
546 axis1End = mark;
547 isAxis1End = true;
548 }
549 if (!mark.utils::Node::name().compare(secondAxisMarkerNames[0])) {
550 axis2Beg = mark;
551 isAxis2Beg = true;
552 }
553 if (!mark.utils::Node::name().compare(secondAxisMarkerNames[1])) {
554 axis2End = mark;
555 isAxis2End = true;
556 }
557 }
558 if (!(isAxis1Beg && isAxis1End && isAxis2Beg && isAxis2End &&
559 isOrigin)) {
561 "All the axes name and origin for the " + rtType + "(" + name +
562 ") must be set and correspond to marker names "
563 "previously defined on the same parent");
564 }
565 if (!(!axisToRecalculate.tolower().compare("firstaxis") ||
566 !axisToRecalculate.tolower().compare("secondaxis"))) {
568 "The 'recalculate' option for " + rtType + "(" + name +
569 ") must be 'firstaxis' or 'secondaxis'");
570 }
571 axisToRecalculate = !axisToRecalculate.tolower().compare("firstaxis")
572 ? firstAxis
573 : secondAxis;
574
576 origin,
577 {axis1Beg, axis1End},
578 {axis2Beg, axis2End},
579 {firstAxis, secondAxis},
580 axisToRecalculate);
581 }
582 RT.setName(name);
583 RT.setParent(parent_str);
584 if (!main_tag.tolower().compare("customrt")) {
585 model->addRT(RT);
586 } else {
587 model->addIMU(RT, technical, anatomical);
588 }
589 } else if (!main_tag.tolower().compare("contact")) {
590 utils::String name;
591 file.read(name);
592 utils::String parentName("root");
593 size_t parent_int = 0;
594 utils::Vector3d pos(0, 0, 0);
595 utils::Vector3d norm(0, 0, 0);
596 utils::String axis("");
597 while (file.read(property_tag) &&
598 property_tag.tolower().compare("endcontact")) {
599 if (!property_tag.tolower().compare("parent")) {
600 // Dynamically find the parent number
601 file.read(parentName);
602 parent_int =
603 static_cast<size_t>(model->GetBodyId(parentName.c_str()));
604 // If parent_int equals zero, no name has concurred
606 model->IsBodyId(static_cast<unsigned int>(parent_int)),
607 "Wrong name in a segment");
608 } else if (!property_tag.tolower().compare("position")) {
609 readVector3d(file, variable, pos);
610 } else if (!property_tag.tolower().compare("normal")) {
611 readVector3d(file, variable, norm);
612 } else if (!property_tag.tolower().compare("axis")) {
613 file.read(axis);
614 }
615 }
616 if (version == 1) {
617 model->AddConstraint(parent_int, pos, norm, name, parentName);
618 } else if (version >= 2) {
619 utils::Error::check(axis.compare(""), "Axis must be provided");
620 model->AddConstraint(parent_int, pos, axis, name, parentName);
621 }
622 } else if (!main_tag.tolower().compare("loopconstraint")) {
623 utils::String name;
624 size_t id_predecessor = 0;
625 size_t id_successor = 0;
626 utils::String predecessor_str("root");
627 utils::String successor_str("root");
628 utils::RotoTrans X_predecessor;
629 utils::RotoTrans X_successor;
631 bool enableStabilization(false);
632 double stabilizationParam(-1);
633 while (file.read(property_tag) &&
634 property_tag.tolower().compare("endloopconstraint")) {
635 if (!property_tag.tolower().compare("predecessor")) {
636 // Dynamically find the parent number
637 file.read(predecessor_str);
638 id_predecessor =
639 static_cast<size_t>(model->GetBodyId(predecessor_str.c_str()));
640 // If parent_int equals zero, no name has concurred
642 model->IsBodyId(static_cast<unsigned int>(id_predecessor)),
643 "Wrong name in a segment");
644 }
645 if (!property_tag.tolower().compare("successor")) {
646 // Dynamically find the parent number
647 file.read(successor_str);
648 id_successor =
649 static_cast<size_t>(model->GetBodyId(successor_str.c_str()));
650 // If parent_int equals zero, no name has concurred
652 model->IsBodyId(static_cast<unsigned int>(id_successor)),
653 "Wrong name in a segment");
654 } else if (!property_tag.tolower().compare("rtpredecessor")) {
655 utils::String seq("xyz");
656 utils::Vector3d rot(0, 0, 0);
657 utils::Vector3d trans(0, 0, 0);
658 // Transcribe the rotations
659 readVector3d(file, variable, rot);
660 // Transcribe the angle sequence for the rotations
661 file.read(seq);
662 // Transcribe the translations
663 readVector3d(file, variable, trans);
664 X_predecessor = utils::RotoTrans(rot, trans, seq);
665 } else if (!property_tag.tolower().compare("rtsuccessor")) {
666 utils::String seq("xyz");
667 utils::Vector3d rot(0, 0, 0);
668 utils::Vector3d trans(0, 0, 0);
669 // Transcribe the rotations
670 readVector3d(file, variable, rot);
671 // Transcribe the angle sequence for the roations
672 file.read(seq);
673 // Transcribe the translations
674 readVector3d(file, variable, trans);
675 X_successor = utils::RotoTrans(rot, trans, seq);
676 } else if (!property_tag.tolower().compare("axis"))
677 for (size_t i = 0; i < static_cast<size_t>(axis.size()); ++i) {
678 file.read(axis(static_cast<unsigned int>(i)), variable);
679 }
680 else if (!property_tag.tolower().compare("stabilizationparameter")) {
681 file.read(stabilizationParam, variable);
682 }
683 }
684 if (stabilizationParam > 0) {
685 enableStabilization = true;
686 }
687 name = "Loop_" + predecessor_str + "_" + successor_str;
688 model->AddLoopConstraint(
689 id_predecessor,
690 id_successor,
691 X_predecessor,
692 X_successor,
693 axis,
694 name,
695 enableStabilization,
696 stabilizationParam);
697 } else if (!main_tag.tolower().compare("softcontact")) {
698 utils::String name;
699 file.read(name);
700 size_t parent_int = 0;
701 utils::String parent_str("root");
702 utils::String contactType("");
703 utils::Vector3d pos(0, 0, 0);
704 double radius(-1);
705 double stiffness(-1);
706 double damping(-1);
707 double muStatic(-1);
708 double muDynamic(-1);
709 double muViscous(-1);
710
711 while (file.read(property_tag) &&
712 property_tag.tolower().compare("endsoftcontact")) {
713 if (!property_tag.tolower().compare("parent")) {
714 // Dynamically find the parent number
715 file.read(parent_str);
716 parent_int =
717 static_cast<size_t>(model->GetBodyId(parent_str.c_str()));
718 // If parent_int equals zero, no name has concurred
720 model->IsBodyId(static_cast<unsigned int>(parent_int)),
721 "Wrong name in a segment");
722 } else if (!property_tag.tolower().compare("type")) {
723 file.read(contactType);
724 } else if (!property_tag.tolower().compare("position")) {
725 readVector3d(file, variable, pos);
726 } else if (!property_tag.tolower().compare("radius")) {
727 file.read(radius, variable);
728 } else if (!property_tag.tolower().compare("stiffness")) {
729 file.read(stiffness, variable);
730 } else if (!property_tag.tolower().compare("damping")) {
731 file.read(damping, variable);
732 } else if (!property_tag.tolower().compare("muStatic")) {
733 file.read(muStatic, variable);
734 } else if (!property_tag.tolower().compare("muDynamic")) {
735 file.read(muDynamic, variable);
736 } else if (!property_tag.tolower().compare("muViscous")) {
737 file.read(muViscous, variable);
738 }
739 }
740
741 if (!contactType.tolower().compare("sphere")) {
742 model->addSoftContact(
744 pos,
745 radius,
746 stiffness,
747 damping,
748 muStatic,
749 muDynamic,
750 muViscous,
751 name,
752 parent_str,
753 static_cast<int>(parent_int)));
754 } else {
755 utils::Error::raise("The 'type' should be 'sphere'.");
756 }
757 } else if (!main_tag.tolower().compare("actuator")) {
758#ifdef MODULE_ACTUATORS
759 hasActuators = true;
760 // The name of the actuator must correspond to the segment number to
761 // which it is attached
762 utils::String name;
763 file.read(name);
764 size_t parent_int = static_cast<size_t>(model->GetBodyId(name.c_str()));
765 // If parent_int equals zero, no name has concurred
767 model->IsBodyId(static_cast<unsigned int>(parent_int)),
768 "Wrong name in a segment");
769
770 // Declaration of all the parameters for all types
771 utils::String type;
772 bool isTypeSet = false;
773 size_t dofIdx(INT_MAX);
774 bool isDofSet = false;
775 utils::String str_direction;
776 bool isDirectionSet = false;
777 int int_direction = 0;
778 double Tmax(-1);
779 bool isTmaxSet = false;
780 double T0(-1);
781 bool isT0Set = false;
782 double slope(-1);
783 bool isSlopeSet = false;
784 double wmax(-1);
785 bool iswmaxSet = false;
786 double wc(-1);
787 bool iswcSet = false;
788 double amin(-1);
789 bool isaminSet = false;
790 double wr(-1);
791 bool iswrSet = false;
792 double w1(-1);
793 bool isw1Set = false;
794 double r(-1);
795 bool isrSet = false;
796 double qopt(-1);
797 bool isqoptSet = false;
798 double facteur6p(-1);
799 bool isFacteur6pSet = false;
800 double r2(-1);
801 bool isr2Set = false;
802 double qopt2(-1);
803 bool isqopt2Set = false;
804 double theta(-1);
805 bool isThetaSet = false;
806 double lambda(-1);
807 bool isLambdaSet = false;
808 double offset(-1);
809 bool isOffsetSet = false;
810
811 while (file.read(property_tag) &&
812 property_tag.tolower().compare("endactuator")) {
813 if (!property_tag.tolower().compare("type")) {
814 file.read(type);
815 isTypeSet = true;
816 } else if (!property_tag.tolower().compare("dof")) {
817 utils::String dofName;
818 file.read(dofName);
819 dofIdx = model->getDofIndex(name, dofName);
820 isDofSet = true;
821 } else if (!property_tag.tolower().compare("direction")) {
822 file.read(str_direction);
824 !str_direction.tolower().compare("positive") ||
825 !str_direction.tolower().compare("negative"),
826 "Direction should be \"positive\" or \"negative\"");
827 if (!str_direction.tolower().compare("positive")) {
828 int_direction = 1;
829 } else {
830 int_direction = -1;
831 }
832 isDirectionSet = true;
833 } else if (!property_tag.tolower().compare("tmax")) {
834 file.read(Tmax, variable);
835 isTmaxSet = true;
836 } else if (!property_tag.tolower().compare("t0")) {
837 file.read(T0, variable);
838 isT0Set = true;
839 } else if (
840 !property_tag.tolower().compare("pente") ||
841 !property_tag.tolower().compare("slope")) {
842 file.read(slope, variable);
843 isSlopeSet = true;
844 } else if (!property_tag.tolower().compare("wmax")) {
845 file.read(wmax, variable);
846 iswmaxSet = true;
847 } else if (!property_tag.tolower().compare("wc")) {
848 file.read(wc, variable);
849 iswcSet = true;
850 } else if (!property_tag.tolower().compare("amin")) {
851 file.read(amin, variable);
852 isaminSet = true;
853 } else if (!property_tag.tolower().compare("wr")) {
854 file.read(wr, variable);
855 iswrSet = true;
856 } else if (!property_tag.tolower().compare("w1")) {
857 file.read(w1, variable);
858 isw1Set = true;
859 } else if (!property_tag.tolower().compare("r")) {
860 file.read(r, variable);
861 isrSet = true;
862 } else if (!property_tag.tolower().compare("qopt")) {
863 file.read(qopt, variable);
864 isqoptSet = true;
865 } else if (!property_tag.tolower().compare("facteur")) {
866 file.read(facteur6p, variable);
867 isFacteur6pSet = true;
868 } else if (!property_tag.tolower().compare("r2")) {
869 file.read(r2, variable);
870 isr2Set = true;
871 } else if (!property_tag.tolower().compare("qopt2")) {
872 file.read(qopt2, variable);
873 isqopt2Set = true;
874 } else if (!property_tag.tolower().compare("theta")) {
875 file.read(theta, variable);
876 isThetaSet = true;
877 } else if (!property_tag.tolower().compare("lambda")) {
878 file.read(lambda, variable);
879 isLambdaSet = true;
880 } else if (!property_tag.tolower().compare("offset")) {
881 file.read(offset, variable);
882 isOffsetSet = true;
883 }
884 }
885 // Verify that everything is there
886 utils::Error::check(isTypeSet != 0, "Actuator type must be defined");
888
889 if (!type.tolower().compare("gauss3p")) {
891 isDofSet && isDirectionSet && isTmaxSet && isT0Set && iswmaxSet &&
892 iswcSet && isaminSet && iswrSet && isw1Set && isrSet &&
893 isqoptSet,
894 "Make sure all parameters are defined");
896 int_direction,
897 Tmax,
898 T0,
899 wmax,
900 wc,
901 amin,
902 wr,
903 w1,
904 r,
905 qopt,
906 dofIdx,
907 name);
908 } else if (!type.tolower().compare("constant")) {
910 isDofSet && isDirectionSet && isTmaxSet,
911 "Make sure all parameters are defined");
913 int_direction, Tmax, dofIdx, name);
914 } else if (!type.tolower().compare("linear")) {
916 isDofSet && isDirectionSet && isSlopeSet && isT0Set,
917 "Make sure all parameters are defined");
919 int_direction, T0, slope, dofIdx, name);
920 } else if (!type.tolower().compare("gauss6p")) {
922 isDofSet && isDirectionSet && isTmaxSet && isT0Set && iswmaxSet &&
923 iswcSet && isaminSet && iswrSet && isw1Set && isrSet &&
924 isqoptSet && isFacteur6pSet && isr2Set && isqopt2Set,
925 "Make sure all parameters are defined");
927 int_direction,
928 Tmax,
929 T0,
930 wmax,
931 wc,
932 amin,
933 wr,
934 w1,
935 r,
936 qopt,
937 facteur6p,
938 r2,
939 qopt2,
940 dofIdx,
941 name);
942 } else if (!type.tolower().compare("sigmoidgauss3p")) {
944 isDofSet && isThetaSet && isLambdaSet && isOffsetSet && isrSet &&
945 isqoptSet,
946 "Make sure all parameters are defined");
948 int_direction, theta, lambda, offset, r, qopt, dofIdx, name);
949 } else {
951 "Actuator do not correspond to an implemented one");
952#ifdef _WIN32
954 int_direction,
955 Tmax,
956 dofIdx,
957 name); // Échec de compilation sinon
958#endif
959 }
960
961 model->addActuator(*actuator);
962 delete actuator;
963#else // MODULE_ACTUATORS
965 "Biorbd was build without the module Internal forces but the model "
966 "defines ones");
967#endif // MODULE_ACTUATORS
968 } else if (!main_tag.tolower().compare("musclegroup")) {
969#ifdef MODULE_MUSCLES
970 utils::String name;
971 file.read(name); // Name of the muscular group
972 // Declaration of the variables
973 utils::String origin_parent_str("root");
974 utils::String insert_parent_str("root");
975 // Read the file
976 while (file.read(property_tag) &&
977 property_tag.tolower().compare("endmusclegroup")) {
978 if (!property_tag.tolower().compare("originparent")) {
979 // Dynamically find the parent number
980 file.read(origin_parent_str);
981 size_t idx = static_cast<size_t>(
982 model->GetBodyId(origin_parent_str.c_str()));
983 // If parent_int still equals zero, no name has concurred
985 model->IsBodyId(static_cast<unsigned int>(idx)),
986 "Wrong origin parent name for a muscle");
987 } else if (!property_tag.tolower().compare("insertionparent")) {
988 // Dynamically find the parent number
989 file.read(insert_parent_str);
990 size_t idx = static_cast<size_t>(
991 model->GetBodyId(insert_parent_str.c_str()));
992 // If parent_int still equals zero, no name has concurred
994 model->IsBodyId(static_cast<unsigned int>(idx)),
995 "Wrong insertion parent name for a muscle");
996 }
997 }
998 model->addMuscleGroup(name, origin_parent_str, insert_parent_str);
999#else // MODULE_MUSCLES
1001 "Biorbd was build without the module Muscles but the model defines "
1002 "a muscle group");
1003#endif // MODULE_MUSCLES
1004 } else if (!main_tag.tolower().compare("muscle")) {
1005#ifdef MODULE_MUSCLES
1006 utils::String name;
1007 file.read(name); // Name of the muscle
1008 // Declaration of the variables
1009 internal_forces::muscles::MUSCLE_TYPE type(
1010 internal_forces::muscles::MUSCLE_TYPE::NO_MUSCLE_TYPE);
1011 internal_forces::muscles::STATE_TYPE stateType(
1012 internal_forces::muscles::STATE_TYPE::NO_STATE_TYPE);
1013 internal_forces::muscles::STATE_FATIGUE_TYPE dynamicFatigueType(
1014 internal_forces::muscles::STATE_FATIGUE_TYPE::
1015 NO_FATIGUE_STATE_TYPE);
1016 utils::String muscleGroup("");
1017 int idxGroup(-1);
1018 utils::Vector3d origin_pos(0, 0, 0);
1019 utils::Vector3d insert_pos(0, 0, 0);
1020 double optimalLength(0);
1021 double maxForce(0);
1022 double tendonSlackLength(0);
1023 double pennAngle(0);
1024 bool useDamping(false);
1025 double maxExcitation(1);
1026 double maxActivation(1);
1027 double PCSA(0);
1028 double shapeFactor(0);
1029 double maxShorteningSpeed(10);
1031
1032 // Read file
1033 while (file.read(property_tag) &&
1034 property_tag.tolower().compare("endmuscle")) {
1035 if (!property_tag.tolower().compare("musclegroup")) {
1036 // Dynamically find the parent number
1037 file.read(muscleGroup);
1038 idxGroup = model->getMuscleGroupId(muscleGroup);
1039 // If parent_int is still equal to zero, no name has concurred
1040 utils::Error::check(idxGroup != -1, "Could not find muscle group");
1041 } else if (!property_tag.tolower().compare("type")) {
1042 utils::String tp_type;
1043 file.read(tp_type);
1044 if (!tp_type.tolower().compare("idealizedactuator")) {
1045 type = internal_forces::muscles::MUSCLE_TYPE::IDEALIZED_ACTUATOR;
1046 } else if (!tp_type.tolower().compare("hill")) {
1047 type = internal_forces::muscles::MUSCLE_TYPE::HILL;
1048 } else if (
1049 !tp_type.tolower().compare("hilldegroote") ||
1050 !tp_type.tolower().compare("degroote")) {
1051 type = internal_forces::muscles::MUSCLE_TYPE::HILL_DE_GROOTE;
1052 } else if (
1053 !tp_type.tolower().compare("hillthelen") ||
1054 !tp_type.tolower().compare("thelen")) {
1055 type = internal_forces::muscles::MUSCLE_TYPE::HILL_THELEN;
1056 } else if (
1057 !tp_type.tolower().compare("hillthelenactive") ||
1058 !tp_type.tolower().compare("thelenactive")) {
1059 type = internal_forces::muscles::MUSCLE_TYPE::HILL_THELEN_ACTIVE;
1060 } else if (
1061 !tp_type.tolower().compare("hilldegrooteactive") ||
1062 !tp_type.tolower().compare("degrooteactive")) {
1063 type =
1064 internal_forces::muscles::MUSCLE_TYPE::HILL_DE_GROOTE_ACTIVE;
1065 } else if (
1066 !tp_type.tolower().compare("hillthelenfatigable") ||
1067 !tp_type.tolower().compare("thelenfatigable")) {
1068 type =
1069 internal_forces::muscles::MUSCLE_TYPE::HILL_THELEN_FATIGABLE;
1070 } else if (
1071 !tp_type.tolower().compare("hilldegrootefatigable") ||
1072 !tp_type.tolower().compare("degrootefatigable")) {
1073 type = internal_forces::muscles::MUSCLE_TYPE::
1074 HILL_DE_GROOTE_FATIGABLE;
1075 } else {
1076 utils::Error::raise(tp_type + " is not a valid muscle type");
1077 }
1078 } else if (!property_tag.tolower().compare("statetype")) {
1079 utils::String tp_state;
1080 file.read(tp_state);
1081 if (!tp_state.tolower().compare("buchanan")) {
1082 stateType = internal_forces::muscles::STATE_TYPE::BUCHANAN;
1083 } else if (!tp_state.tolower().compare("degroote")) {
1084 stateType = internal_forces::muscles::STATE_TYPE::DE_GROOTE;
1085 } else {
1087 property_tag + " is not a valid muscle state type");
1088 }
1089 } else if (!property_tag.tolower().compare("usedamping")) {
1090 file.read(useDamping);
1091 } else if (!property_tag.tolower().compare("originposition")) {
1092 readVector3d(file, variable, origin_pos);
1093 } else if (!property_tag.tolower().compare("insertionposition")) {
1094 readVector3d(file, variable, insert_pos);
1095 } else if (!property_tag.tolower().compare("optimallength")) {
1096 file.read(optimalLength, variable);
1097 } else if (!property_tag.tolower().compare("tendonslacklength")) {
1098 file.read(tendonSlackLength, variable);
1099 } else if (!property_tag.tolower().compare("pennationangle")) {
1100 file.read(pennAngle, variable);
1101 } else if (!property_tag.tolower().compare("maximalforce")) {
1102 file.read(maxForce, variable);
1103 } else if (!property_tag.tolower().compare("maximalexcitation")) {
1104 file.read(maxExcitation, variable);
1105 } else if (!property_tag.tolower().compare("pcsa")) {
1106 file.read(PCSA, variable);
1107 } else if (!property_tag.tolower().compare("maxshorteningspeed")) {
1108 file.read(maxShorteningSpeed, variable);
1109 } else if (!property_tag.tolower().compare("fatigueparameters")) {
1110 while (file.read(subproperty_tag) &&
1111 subproperty_tag.tolower().compare("endfatigueparameters")) {
1112 if (!subproperty_tag.tolower().compare("type")) {
1113 utils::String tp_fatigue_type;
1114 file.read(tp_fatigue_type);
1115 if (!tp_fatigue_type.tolower().compare("simple")) {
1116 dynamicFatigueType = internal_forces::muscles::
1117 STATE_FATIGUE_TYPE::SIMPLE_STATE_FATIGUE;
1118 } else if (!tp_fatigue_type.tolower().compare("xia")) {
1119 dynamicFatigueType =
1120 internal_forces::muscles::STATE_FATIGUE_TYPE::DYNAMIC_XIA;
1121 } else {
1123 tp_fatigue_type +
1124 " is not a value fatigue parameter type");
1125 }
1126 } else {
1127 double param(0);
1128 file.read(param);
1129 if (!subproperty_tag.tolower().compare("fatiguerate")) {
1130 fatigueParameters.setFatigueRate(param);
1131 } else if (!subproperty_tag.tolower().compare("recoveryrate")) {
1132 fatigueParameters.setRecoveryRate(param);
1133 } else if (!subproperty_tag.tolower().compare(
1134 "developfactor")) {
1135 fatigueParameters.setDevelopFactor(param);
1136 } else if (!subproperty_tag.tolower().compare(
1137 "recoveryfactor")) {
1138 fatigueParameters.setRecoveryFactor(param);
1139 }
1140 }
1141 }
1142 } else if (!property_tag.tolower().compare("shapefactor")) {
1143 file.read(shapeFactor);
1144 }
1145 }
1146 utils::Error::check(idxGroup != -1, "No muscle group was provided!");
1149 origin_pos,
1150 name + "_origin",
1151 model->muscleGroup(static_cast<size_t>(idxGroup)).origin()),
1153 insert_pos,
1154 name + "_insertion",
1155 model->muscleGroup(static_cast<size_t>(idxGroup)).insertion()));
1156 internal_forces::muscles::State stateMax(maxExcitation, maxActivation);
1158 optimalLength,
1159 maxForce,
1160 PCSA,
1161 tendonSlackLength,
1162 pennAngle,
1163 stateMax,
1164 fatigueParameters,
1165 useDamping,
1166 maxShorteningSpeed);
1167 model->muscleGroup(static_cast<size_t>(idxGroup))
1168 .addMuscle(
1169 name,
1170 type,
1171 geo,
1172 characteristics,
1174 stateType,
1175 dynamicFatigueType);
1176 if (stateType == internal_forces::muscles::STATE_TYPE::BUCHANAN &&
1177 shapeFactor != 0) {
1178 auto &muscleGroup = model->muscleGroup(idxGroup);
1179 int nMuscInGroup(static_cast<int>(muscleGroup.nbMuscles() - 1));
1180 auto &state = muscleGroup.muscle(nMuscInGroup).state();
1182 .shapeFactor(shapeFactor);
1183 }
1184#else // MODULE_MUSCLES
1186 "Biorbd was build without the module Muscles but the model defines "
1187 "a muscle");
1188#endif // MODULE_MUSCLES
1189 } else if (!main_tag.tolower().compare("ligament")) {
1190#ifdef MODULE_LIGAMENTS
1191 utils::String name;
1192 file.read(name);
1193 utils::String type;
1194 utils::String origin;
1195 utils::String insertion;
1196 bool isTypeSet = false;
1197 utils::Vector3d origin_pos(0, 0, 0);
1198 utils::Vector3d insert_pos(0, 0, 0);
1199 double ligamentSlackLength(std::numeric_limits<double>::quiet_NaN());
1200 double stiffness(std::numeric_limits<double>::quiet_NaN());
1201 double epsilon(0);
1202 double force(std::numeric_limits<double>::quiet_NaN());
1203 double dampingFactor(0);
1204 double maxShorteningSpeed(1);
1205
1206 while (file.read(property_tag) &&
1207 property_tag.tolower().compare("endligament")) {
1208 if (!property_tag.tolower().compare("type")) {
1209 file.read(type);
1210 isTypeSet = true;
1211 } else if (!property_tag.tolower().compare("originposition")) {
1212 readVector3d(file, variable, origin_pos);
1213 } else if (!property_tag.tolower().compare("insertionposition")) {
1214 readVector3d(file, variable, insert_pos);
1215 } else if (!property_tag.tolower().compare("ligamentslacklength")) {
1216 file.read(ligamentSlackLength, variable);
1217 } else if (!property_tag.tolower().compare("stiffness")) {
1218 file.read(stiffness, variable);
1219 } else if (!property_tag.tolower().compare("epsilon")) {
1220 file.read(epsilon, variable);
1221 } else if (!property_tag.tolower().compare("force")) {
1222 file.read(force, variable);
1223 } else if (!property_tag.tolower().compare("origin")) {
1224 file.read(origin);
1225 } else if (!property_tag.tolower().compare("insertion")) {
1226 file.read(insertion);
1227 } else if (!property_tag.tolower().compare("dampingfactor")) {
1228 file.read(dampingFactor, variable);
1229 } else if (!property_tag.tolower().compare("maxshorteningspeed")) {
1230 file.read(maxShorteningSpeed, variable);
1231 }
1232 }
1233 // Verify that everything is there
1234 utils::Error::check(isTypeSet != 0, "Ligament type must be defined");
1235 std::shared_ptr<internal_forces::ligaments::Ligament> ligament;
1237 insertion != "" && origin != "",
1238 "Insertion and origin of the ligament need to be defined.");
1240 utils::Vector3d(origin_pos, name + "_origin", origin),
1241 utils::Vector3d(insert_pos, name + "_insersion", insertion));
1243 ligamentSlackLength, dampingFactor, maxShorteningSpeed);
1244 if (!type.tolower().compare("constant")) {
1246 std::isnan(force) == 0 && std::isnan(ligamentSlackLength) == 0,
1247 "Make sure all parameters are defined");
1248 ligament =
1249 std::make_shared<internal_forces::ligaments::LigamentConstant>(
1250 force, name, geo, characteristics);
1251 } else if (!type.tolower().compare("linearspring")) {
1253 std::isnan(stiffness) == 0 &&
1254 std::isnan(ligamentSlackLength) == 0,
1255 "Make sure all parameters are defined");
1256 ligament = std::make_shared<
1258 stiffness, name, geo, characteristics);
1259 } else if (!type.tolower().compare("secondorderspring")) {
1261 std::isnan(stiffness) == 0 &&
1262 std::isnan(ligamentSlackLength) == 0,
1263 "Make sure all parameters are defined");
1264 ligament = std::make_shared<
1266 stiffness, epsilon, name, geo, characteristics);
1267 } else {
1269 "Ligament type do not correspond to an implemented one");
1270 }
1271 model->addLigament(*ligament);
1272#else // MODULE_LIGAMENT
1274 "Biorbd was build without the module Ligament but the model "
1275 "defines a ligament");
1276#endif // MODULE_LIGAMENT
1277 } else if (!main_tag.tolower().compare("passivetorque")) {
1278#ifdef MODULE_PASSIVE_TORQUES
1279 utils::String name;
1280 file.read(name);
1281 size_t parent_int = static_cast<size_t>(model->GetBodyId(name.c_str()));
1282 // If parent_int equals zero, no name has concurred
1284 model->IsBodyId(static_cast<unsigned int>(parent_int)),
1285 "Wrong name in a segment");
1286 // Declaration of all the parameters for all types
1287 utils::String type;
1288 bool isTypeSet = false;
1289 size_t dofIdx(INT_MAX);
1290 bool isDofSet = false;
1291 double Tconstant(std::numeric_limits<double>::quiet_NaN());
1292 double T0(std::numeric_limits<double>::quiet_NaN());
1293 double slope(std::numeric_limits<double>::quiet_NaN());
1294 double k1(std::numeric_limits<double>::quiet_NaN());
1295 double k2(std::numeric_limits<double>::quiet_NaN());
1296 double b1(std::numeric_limits<double>::quiet_NaN());
1297 double b2(std::numeric_limits<double>::quiet_NaN());
1298 double wMax(std::numeric_limits<double>::quiet_NaN());
1299 double qMid(0);
1300 double deltaP(0);
1301 double sV(1);
1302 double tauEq(0);
1303 double pBeta(1);
1304 double isExponentialSet(0);
1305
1306 while (file.read(property_tag) &&
1307 property_tag.tolower().compare("endpassivetorque")) {
1308 if (!property_tag.tolower().compare("type")) {
1309 file.read(type);
1310 isTypeSet = true;
1311 } else if (!property_tag.tolower().compare("dof")) {
1312 utils::String dofName;
1313 file.read(dofName);
1314 dofIdx = model->getDofIndex(name, dofName);
1315 isDofSet = true;
1316 } else if (!property_tag.tolower().compare("torque")) {
1317 file.read(Tconstant, variable);
1318 } else if (!property_tag.tolower().compare("t0")) {
1319 file.read(T0, variable);
1320 } else if (
1321 !property_tag.tolower().compare("pente") ||
1322 !property_tag.tolower().compare("slope")) {
1323 file.read(slope, variable);
1324 } else if (!property_tag.tolower().compare("k1")) {
1325 file.read(k1, variable);
1326 isExponentialSet += 1;
1327 } else if (!property_tag.tolower().compare("k2")) {
1328 file.read(k2, variable);
1329 isExponentialSet += 1;
1330 } else if (!property_tag.tolower().compare("b1")) {
1331 file.read(b1, variable);
1332 isExponentialSet += 1;
1333 } else if (!property_tag.tolower().compare("b2")) {
1334 file.read(b2, variable);
1335 isExponentialSet += 1;
1336 } else if (!property_tag.tolower().compare("wmax")) {
1337 file.read(wMax, variable);
1338 isExponentialSet += 1;
1339 } else if (!property_tag.tolower().compare("qmid")) {
1340 file.read(qMid, variable);
1341 } else if (!property_tag.tolower().compare("deltap")) {
1342 file.read(deltaP, variable);
1343 } else if (!property_tag.tolower().compare("sv")) {
1344 file.read(sV, variable);
1345 } else if (!property_tag.tolower().compare("taueq")) {
1346 file.read(tauEq, variable);
1347 } else if (!property_tag.tolower().compare("pbeta")) {
1348 file.read(pBeta, variable);
1349 }
1350 }
1351 // Verify that everything is there
1353 isTypeSet != 0, "PassiveTorque type must be defined");
1355 if (!type.tolower().compare("constant")) {
1357 isDofSet && !std::isnan(Tconstant),
1358 "Make sure all parameters are defined");
1359 passiveTorque =
1361 Tconstant, dofIdx, name);
1362 } else if (!type.tolower().compare("linear")) {
1364 isDofSet && !std::isnan(slope) && !std::isnan(T0),
1365 "Make sure all parameters are defined");
1366 passiveTorque =
1368 T0, slope, dofIdx, name);
1369 } else if (!type.tolower().compare("exponential")) {
1371 isDofSet && isExponentialSet == 5,
1372 "Make sure all parameters are defined");
1373 passiveTorque =
1375 k1,
1376 k2,
1377 b1,
1378 b2,
1379 qMid,
1380 tauEq,
1381 pBeta,
1382 wMax,
1383 sV,
1384 deltaP,
1385 dofIdx,
1386 name);
1387 } else {
1389 "PassiveTorque do not correspond to an implemented one");
1390 }
1391 model->addPassiveTorque(*passiveTorque);
1392 delete passiveTorque;
1393#else // MODULE_PASSIVE_TORQUE
1395 "Biorbd was build without the module Passive torque but the model "
1396 "defines one passive torque");
1397#endif // MODULE_PASSIVE_TORQUE
1398 } else if (!main_tag.tolower().compare("viapoint")) {
1399 utils::String name;
1400 file.read(
1401 name); // Name of via point... Eventually add the muscle group
1402 // Declaration of the variables
1403 utils::String parent("");
1404 utils::String muscle("");
1405 utils::String musclegroup("");
1406 bool isMuscle = false;
1407 bool isLigament = false;
1408 int idxMuscleGroup(-1);
1409 int idxLigament(-1);
1410 int idxMuscle(-1);
1411 utils::String ligament("");
1412 internal_forces::ViaPoint position(0, 0, 0);
1413
1414 // Read file
1415 while (file.read(property_tag) &&
1416 property_tag.tolower().compare("endviapoint")) {
1417 if (!property_tag.tolower().compare("parent")) {
1418 // Dynamically find the parent number
1419 file.read(parent);
1420 size_t idx = static_cast<size_t>(model->GetBodyId(parent.c_str()));
1421 // If parent_int still equals zero, no name has concurred
1423 model->IsBodyId(static_cast<unsigned int>(idx)),
1424 "Wrong origin parent name for a muscle");
1425 } else if (!property_tag.tolower().compare("muscle")) {
1426 file.read(muscle);
1427 isMuscle = true;
1429 !isLigament,
1430 "Via point cannot be define either for ligament and muscle.");
1431 } else if (!property_tag.tolower().compare("musclegroup")) {
1432 file.read(musclegroup);
1433 } else if (!property_tag.tolower().compare("ligament")) {
1434 file.read(ligament);
1435 isLigament = true;
1437 !isMuscle,
1438 "Via point cannot be define either for ligament and muscle.");
1439 } else if (!property_tag.tolower().compare("position")) {
1440 readVector3d(file, variable, position);
1441 }
1442 }
1443 position.setName(name);
1444 position.setParent(parent);
1445 if (isMuscle) {
1446 idxMuscleGroup = model->getMuscleGroupId(musclegroup);
1448 idxMuscleGroup != -1, "No muscle group was provided!");
1449 idxMuscle = model->muscleGroup(idxMuscleGroup).muscleID(muscle);
1450 utils::Error::check(idxMuscle != -1, "No muscle was provided!");
1451 model->muscleGroup(idxMuscleGroup)
1452 .muscle(idxMuscle)
1453 .addPathObject(position);
1454 } else if (isLigament) {
1455 idxLigament = model->ligamentID(ligament);
1456 model->ligament(idxLigament).addPathObject(position);
1457 }
1458 } else if (!main_tag.tolower().compare("wrapping")) {
1459 utils::String name;
1460 file.read(name); // Name of the wrapping
1461
1462 // Declaration of the variables
1463 utils::String muscle("");
1464 utils::String musclegroup("");
1465 utils::String wrapType("");
1466 int idxMuscleGroup(-1);
1467 int idxMuscle(-1);
1468 utils::String ligament("");
1469 bool isMuscle = false;
1470 bool isLigament = false;
1471 int idxLigament(-1);
1472 utils::String parent("");
1474 bool isRTset(false);
1475 bool RTinMatrix(false);
1476 double radius(0);
1477 double length(0);
1478
1479 // Read file
1480 while (file.read(property_tag) &&
1481 property_tag.tolower().compare("endwrapping")) {
1482 if (!property_tag.tolower().compare("parent")) {
1483 // Dynamically find the parent number
1484 file.read(parent);
1485 size_t idx = static_cast<size_t>(model->GetBodyId(parent.c_str()));
1486 // If parent_int still equals zero, no name has concurred
1488 model->IsBodyId(static_cast<unsigned int>(idx)),
1489 "Wrong origin parent name for a muscle");
1490 } else if (!property_tag.tolower().compare("rtinmatrix")) {
1492 isRTset == false, "RT should not appear before RTinMatrix");
1493 file.read(RTinMatrix);
1494 } else if (!property_tag.tolower().compare("rt")) {
1495 readRtMatrix(file, variable, RTinMatrix, RT);
1496 isRTset = true;
1497 } else if (!property_tag.tolower().compare("type")) {
1498 file.read(wrapType);
1499 } else if (!property_tag.tolower().compare("muscle")) {
1500 file.read(muscle);
1501 isMuscle = true;
1503 !isLigament,
1504 "Wrapping object cannot be define either for "
1505 "ligament and muscle.");
1506 } else if (!property_tag.tolower().compare("musclegroup")) {
1507 file.read(musclegroup);
1508 } else if (!property_tag.tolower().compare("ligament")) {
1509 file.read(ligament);
1510 isLigament = true;
1512 !isMuscle,
1513 "Wrapping object cannot be define either for "
1514 "ligament and muscle.");
1515 } else if (!property_tag.tolower().compare("radius")) {
1516 file.read(radius, variable);
1517 } else if (!property_tag.tolower().compare("length")) {
1518 file.read(length, variable);
1519 }
1520 }
1521 utils::Error::check(parent != "", "Parent was not defined");
1522 if (!wrapType.tolower().compare("halfcylinder")) {
1524 radius > 0.0, "Radius must be defined and positive");
1525 utils::Error::check(length >= 0.0, "Length was must be positive");
1526 } else {
1528 "Wrapping type must be defined (choices: 'halfcylinder')");
1529 }
1531 RT, radius, length, name, parent);
1532 if (isMuscle) {
1533 idxMuscleGroup = model->getMuscleGroupId(musclegroup);
1535 idxMuscleGroup != -1, "No muscle group was provided!");
1536 idxMuscle = model->muscleGroup(idxMuscleGroup).muscleID(muscle);
1537 utils::Error::check(idxMuscle != -1, "No muscle was provided!");
1538 model->muscleGroup(idxMuscleGroup)
1539 .muscle(idxMuscle)
1540 .addPathObject(cylinder);
1541 } else if (isLigament) {
1542 idxLigament = model->ligamentID(ligament);
1543 model->ligament(idxLigament).addPathObject(cylinder);
1544 }
1545 }
1546 }
1547 } catch (std::runtime_error message) {
1548 utils::String error_message(
1549 "Reading of file \"" + path.filename() + "." + path.extension() +
1550 "\" failed with the following error:");
1551 error_message += "\n" + utils::String(message.what()) + "\n";
1552 if (name.compare("")) {
1553 error_message += "Element: " + main_tag + ", named: " + name + "\n";
1554 }
1555 if (property_tag.compare("") && property_tag.find_first_of("end") != 0) {
1556 error_message += "Property: " + property_tag + "\n";
1557 }
1558 if (subproperty_tag.compare("") &&
1559 subproperty_tag.find_first_of("end") != 0) {
1560 error_message += "Subproperty: " + subproperty_tag + "\n";
1561 }
1562
1563 utils::Error::raise(error_message);
1564 }
1565#ifdef MODULE_ACTUATORS
1566 if (hasActuators) {
1567 model->closeActuator();
1568 }
1569#endif // MODULE_ACTUATORS
1570 // Close file
1571 // std::cout << "Model file successfully loaded" << std::endl;
1572 file.close();
1573}
1574std::vector<std::vector<utils::Vector3d>> Reader::readMarkerDataFile(
1575 const utils::Path &path) {
1576 // Open file
1577 // std::cout << "Loading marker file: " << path << std::endl;
1578#ifdef _WIN32
1579 utils::IfStream file(
1580 utils::Path::toWindowsFormat(path.absolutePath()).c_str(), std::ios::in);
1581#else
1582 utils::IfStream file(path.absolutePath().c_str(), std::ios::in);
1583#endif
1584
1585 // Read file
1586 utils::String tp;
1587
1588 // Determine the version of the file
1589 file.readSpecificTag("version", tp);
1590 size_t version(static_cast<size_t>(atoi(tp.c_str())));
1591 utils::Error::check(version == 1, "Version not implemented yet");
1592
1593 // Determine the number of markers
1594 file.readSpecificTag("nbmark", tp);
1595 size_t nbMark(static_cast<size_t>(atoi(tp.c_str())));
1596
1597 // Determine the number of nodes
1598 file.readSpecificTag("nbintervals", tp);
1599 size_t nbIntervals(static_cast<size_t>(atoi(tp.c_str())));
1600
1601 std::vector<std::vector<utils::Vector3d>> markers;
1602 std::vector<utils::Vector3d> position;
1603 // Scroll down until the definition of a marker
1604 for (size_t j = 0; j < nbMark; j++) {
1605 while (tp.compare("Marker")) {
1606 bool check = file.read(tp);
1608 check, "Marker file error, wrong size of marker or intervals?");
1609 }
1610
1611 size_t noMarker;
1612 file.read(noMarker);
1613 for (size_t i = 0; i <= nbIntervals;
1614 i++) { // <= because there is nbIntervals+1 values
1615 utils::Vector3d mark(0, 0, 0);
1616 readVector3d(file, std::map<utils::Equation, double>(), mark);
1617 position.push_back(mark);
1618 }
1619 markers.push_back(position);
1620 // reinitialize for the next iteration
1621 tp = "";
1622 position.clear();
1623 }
1624
1625 // Close file
1626 // std::cout << "Marker file successfully loaded" << std::endl;
1627 file.close();
1628 return markers;
1629}
1630
1631std::vector<rigidbody::GeneralizedCoordinates> Reader::readQDataFile(
1632 const utils::Path &path) {
1633 // Open file
1634 // std::cout << "Loading kin file: " << path << std::endl;
1635#ifdef _WIN32
1636 utils::IfStream file(
1637 utils::Path::toWindowsFormat(path.absolutePath()).c_str(), std::ios::in);
1638#else
1639 utils::IfStream file(path.absolutePath().c_str(), std::ios::in);
1640#endif
1641
1642 // Read file
1643 utils::String tp;
1644
1645 // Determine the file version
1646 file.readSpecificTag("version", tp);
1647 size_t version(static_cast<size_t>(atoi(tp.c_str())));
1648 utils::Error::check(version == 1, "Version not implemented yet");
1649
1650 // Determine the number of markers
1651 file.readSpecificTag("nddl", tp);
1652 size_t NDDL(static_cast<size_t>(atoi(tp.c_str())));
1653
1654 // Determine the number of nodes
1655 file.readSpecificTag("nbintervals", tp);
1656 size_t nbIntervals(static_cast<size_t>(atoi(tp.c_str())));
1657
1658 std::vector<rigidbody::GeneralizedCoordinates> kinematics;
1659 // Scroll down until the definition of a marker
1660 for (size_t j = 0; j < nbIntervals + 1; j++) {
1661 while (tp.compare("T")) {
1662 bool check = file.read(tp);
1664 check, "Kin file error, wrong size of NDDL or intervals?");
1665 }
1666
1667 double time;
1668 file.read(time);
1669 rigidbody::GeneralizedCoordinates position(NDDL);
1670 for (size_t i = 0; i < NDDL; i++) {
1671 file.read(position(static_cast<unsigned int>(i)));
1672 }
1673
1674 kinematics.push_back(position);
1675 // reinitialise for the next iteration
1676 tp = "";
1677 }
1678
1679 // Close file
1680 // std::cout << "Kin file successfully loaded" << std::endl;
1681 file.close();
1682 return kinematics;
1683}
1684
1685std::vector<utils::Vector> Reader::readActivationDataFile(
1686 const utils::Path &path) {
1687 // Open file
1688 // std::cout << "Loading kin file: " << path << std::endl;
1689#ifdef _WIN32
1690 utils::IfStream file(
1691 utils::Path::toWindowsFormat(path.absolutePath()).c_str(), std::ios::in);
1692#else
1693 utils::IfStream file(path.absolutePath().c_str(), std::ios::in);
1694#endif
1695
1696 // Read file
1697 utils::String tp;
1698
1699 // Determine the file version
1700 file.readSpecificTag("version", tp);
1701 size_t version(static_cast<size_t>(atoi(tp.c_str())));
1702 utils::Error::check(version == 1, "Version not implemented yet");
1703
1704 // Determine the number of markers
1705 file.readSpecificTag("nbmuscles", tp);
1706 size_t nMus(static_cast<size_t>(atoi(tp.c_str())));
1707
1708 // Determine the number of nodes
1709 file.readSpecificTag("nbintervals", tp);
1710 size_t nbIntervals(static_cast<size_t>(atoi(tp.c_str())));
1711
1712 std::vector<utils::Vector> activations;
1713 // Scroll down until the definition of a marker
1714 for (size_t j = 0; j < nbIntervals + 1; j++) {
1715 while (tp.compare("T")) {
1716 bool check = file.read(tp);
1718 check,
1719 "Kin file error, wrong size of number of muscles or intervals?");
1720 }
1721
1722 double time;
1723 file.read(time);
1724 utils::Vector activation_tp(nMus);
1725 for (size_t i = 0; i < nMus; i++) {
1726 file.read(activation_tp(static_cast<unsigned int>(i)));
1727 }
1728
1729 activations.push_back(activation_tp);
1730 // reinitialize for the next iteration
1731 tp = "";
1732 }
1733
1734 // Close file
1735 // std::cout << "Activation file successfully loaded" << std::endl;
1736 file.close();
1737 return activations;
1738}
1739
1740std::vector<utils::Vector> Reader::readTorqueDataFile(const utils::Path &path) {
1741 // Open file
1742 // std::cout << "Loading kin file: " << path << std::endl;
1743#ifdef _WIN32
1744 utils::IfStream file(
1745 utils::Path::toWindowsFormat(path.absolutePath()).c_str(), std::ios::in);
1746#else
1747 utils::IfStream file(path.absolutePath().c_str(), std::ios::in);
1748#endif
1749
1750 // Read file
1751 utils::String tp;
1752
1753 // Determine the file version
1754 file.readSpecificTag("version", tp);
1755 size_t version(static_cast<size_t>(atoi(tp.c_str())));
1756 utils::Error::check(version == 1, "Version not implemented yet");
1757
1758 // Determine the number of GeneralizedTorque
1759 file.readSpecificTag("nGeneralizedTorque", tp); //
1760 size_t nGeneralizedTorque(static_cast<size_t>(atoi(tp.c_str())));
1761
1762 // Determine the number of nodes
1763 file.readSpecificTag("nbintervals", tp);
1764 size_t nbIntervals(static_cast<size_t>(atoi(tp.c_str())));
1765
1766 std::vector<utils::Vector> torque;
1767 // Scroll down until the definition of a torque
1768 for (size_t j = 0; j < nbIntervals + 1; j++) {
1769 while (tp.compare("T")) {
1770 bool check = file.read(tp);
1772 check,
1773 "Kin file error, wrong size of NGeneralizedTorque or intervals?"); //
1774 }
1775
1776 // Read the first line which represents the timestamp
1777 double time;
1778 file.read(time);
1779
1780 // Read the vector of GeneralizedTorque associated to the timestamp
1781 utils::Vector torque_tp(nGeneralizedTorque);
1782 for (size_t i = 0; i < nGeneralizedTorque; i++) {
1783 file.read(torque_tp(static_cast<unsigned int>(i)));
1784 }
1785
1786 torque.push_back(torque_tp);
1787 // reinitialise for the next iteration
1788 tp = "";
1789 }
1790
1791 // Close file
1792 file.close();
1793 return torque;
1794}
1795
1797 const utils::Path &path) {
1798 // Open file
1799 // std::cout << "Loading ground reaction force file: " << path << std::endl;
1800#ifdef _WIN32
1801 utils::IfStream file(
1802 utils::Path::toWindowsFormat(path.absolutePath()).c_str(), std::ios::in);
1803#else
1804 utils::IfStream file(path.absolutePath().c_str(), std::ios::in);
1805#endif
1806
1807 // Read file
1808 utils::String tp;
1809
1810 // Determine the file version
1811 file.readSpecificTag("version", tp);
1812 size_t version(static_cast<size_t>(atoi(tp.c_str())));
1813 utils::Error::check(version == 1, "Version not implemented yet");
1814
1815 // Determine the number of grf (ground reaction force)
1816 file.readSpecificTag("ngrf", tp); //
1817 size_t NGRF(static_cast<size_t>(atoi(tp.c_str())));
1818
1819 // Determine the number of nodes
1820 file.readSpecificTag("nbintervals", tp);
1821 size_t nbIntervals(static_cast<size_t>(atoi(tp.c_str())));
1822
1823 std::vector<utils::Vector> grf;
1824 // Scroll down until the definition of a torque
1825 for (size_t j = 0; j < nbIntervals + 1; j++) {
1826 while (tp.compare("T")) {
1827 bool check = file.read(tp);
1829 check, "Grf file error, wrong size of NR or intervals?");
1830 }
1831
1832 // Read the first line that represents the timestamp
1833 double time;
1834 file.read(time);
1835
1836 // Read the vector of GeneralizedTorque associated to the timestamp
1837 utils::Vector grf_tp(NGRF);
1838 for (size_t i = 0; i < NGRF; i++) {
1839 file.read(grf_tp(static_cast<unsigned int>(i)));
1840 }
1841
1842 grf.push_back(grf_tp);
1843 // reinitialize for the next iteration
1844 tp = "";
1845 }
1846
1847 // Close file
1848 file.close();
1849 return grf;
1850}
1851
1853 const utils::Path &path, // Path to the file
1854 std::vector<std::vector<size_t>>
1855 &frame, // Frame vector (time is frame/frequency)
1856 std::vector<size_t> &frequency, // Acquisition frequency
1857 std::vector<std::vector<utils::Vector3d>> &force, // Linear forces (x,y,z)
1858 std::vector<std::vector<utils::Vector3d>> &moment, // Moments (x,y,z)
1859 std::vector<std::vector<utils::Vector3d>>
1860 &cop) // Center of pressure (x,y,z) * number of pf
1861{
1862 // Open file
1863 // std::cout << "Loading force file: " << path << std::endl;
1864#ifdef _WIN32
1865 utils::IfStream file(
1866 utils::Path::toWindowsFormat(path.absolutePath()).c_str(), std::ios::in);
1867#else
1868 utils::IfStream file(path.absolutePath().c_str(), std::ios::in);
1869#endif
1870
1871 frame.clear();
1872 force.clear();
1873 moment.clear();
1874 cop.clear();
1875
1876 // Read file
1877 utils::String tp;
1878
1879 while (!file.eof()) {
1880 // Put everything in temporary per platform (Mettre tout en temporaire par
1881 // plate-forme)
1882 std::vector<size_t> frame1pf;
1883 std::vector<utils::Vector3d> force1fp;
1884 std::vector<utils::Vector3d> moment1fp;
1885 std::vector<utils::Vector3d> cop1fp;
1886
1887 // Get the acquisition frequency
1888 file.readSpecificTag("devices", tp);
1889 size_t frequency1pf(static_cast<size_t>(atoi(tp.c_str())));
1890
1891 // Skip the header
1892 for (size_t i = 0; i < 4; ++i) {
1893 file.getline(tp);
1894 }
1895
1896 // Transcribe the values
1897 while (!file.eof()) { // return false if it fails
1898 file.getline(tp); // take the entire line
1899 // If line is empty, it's because we are done with the forces on this
1900 // platform
1901 if (!tp.compare("")) {
1902 break;
1903 }
1904
1905 // Otherwise, we save the line
1906 // now we'll use a stringstream to separate the fields out of the line
1907 // (comma separated)
1908 std::stringstream ss(tp);
1909 utils::String field;
1910 std::vector<double> data;
1911 data.clear();
1912 while (getline(ss, field, ',')) {
1913 // for each field we wish to convert it to a double
1914 // (since we require that the CSV contains nothing but floating-point
1915 // values)
1916 std::stringstream fs(field);
1917 double f = 0.0; // (default value is 0.0)
1918 fs >> f;
1919
1920 // add the newly-converted field to the end of the record
1921 data.push_back(f);
1922 }
1923 // Make sure that the line has the right number of elements(2 times, 3
1924 // cop, 3 forces, 3 moments)
1926 data.size() == 11,
1927 "Wrong number of element in a line in the force file");
1928
1929 // Fill in the fields
1930 frame1pf.push_back(static_cast<size_t>(data[0])); // Frame stamp
1931 // size_t subFrame = static_cast<size_t>(data[1]); // Subframes
1932 // (not interesting)
1933 utils::Vector3d cop_tp(0, 0, 0); // Center of pressure
1934 utils::Vector3d for_tp(0, 0, 0); // Force
1935 utils::Vector3d M_tp(0, 0, 0); // Moment
1936 for (unsigned int i = 0; i < 3; ++i) {
1937 cop_tp[i] = data[i + 2] / 1000; // from mm to m
1938 for_tp[i] = data[i + 5];
1939 M_tp[i] = data[i + 8] / 1000; // from Nmm to Nm
1940 }
1941 cop1fp.push_back(cop_tp);
1942 force1fp.push_back(for_tp);
1943 moment1fp.push_back(M_tp);
1944 }
1945 // Push back of the values
1946 frame.push_back(frame1pf);
1947 frequency.push_back(frequency1pf);
1948 force.push_back(force1fp);
1949 moment.push_back(moment1fp);
1950 cop.push_back(cop1fp);
1951 }
1952}
1953
1954std::vector<std::vector<utils::SpatialVector>> Reader::readViconForceFile(
1955 const utils::String &path) {
1956 // Read file
1957 std::vector<std::vector<size_t>> frame;
1958 std::vector<size_t> frequency; // Acquisition frequency
1959 std::vector<std::vector<utils::Vector3d>> force; // Linear forces (x,y,z)
1960 std::vector<std::vector<utils::Vector3d>> moment; // Moments (x,y,z)
1961 std::vector<std::vector<utils::Vector3d>> cop; // Center of pressure (x,y,z)
1962 readViconForceFile(path, frame, frequency, force, moment, cop);
1963
1964 // Redispatch the values in a vector of SpatialTransform
1965 std::vector<std::vector<utils::SpatialVector>> st; // nb of platform per time
1966 for (size_t j = 0; j < force.size(); ++j) { // nb platform
1967 std::vector<utils::SpatialVector> tp;
1968 for (size_t i = 0; i < force[j].size(); ++i) { // timestamp
1969 const utils::Vector3d &f = force[j][i]; // Linear forces (x,y,z)
1970 const utils::Vector3d &m = moment[j][i]; // Moments (x,y,z)
1971 tp.push_back(utils::SpatialVector(m[0], m[1], m[2], f[0], f[1], f[2]));
1972 }
1973 st.push_back(tp);
1974 }
1975 return st;
1976}
1977
1978std::vector<std::vector<utils::Vector3d>> Reader::readViconMarkerFile(
1979 const utils::Path &path,
1980 std::vector<utils::String> &markOrder,
1981 int nFramesToGet) {
1982 // Read file
1983#ifdef _WIN32
1984 utils::IfStream file(
1985 utils::Path::toWindowsFormat(path.absolutePath()).c_str(), std::ios::in);
1986#else
1987 utils::IfStream file(path.absolutePath().c_str(), std::ios::in);
1988#endif
1989 utils::String t;
1990
1991 // Get the acquisition frequency
1992 // frequency = atoi(findImportantParameter(file, "trajectories").c_str());
1993
1994 // Get the order of the markers in the file
1995 for (size_t i = 0; i < 3; ++i) { // skip the header
1996 file.read(t);
1997 }
1998 size_t idx_tp = 0;
1999 std::vector<size_t> idx_init;
2000 std::vector<size_t> idx_end;
2001 // Find the separators (: et ,)
2002 while (idx_tp < t.length()) {
2003 idx_tp = t.find(":", idx_tp + 1);
2004 idx_init.push_back(idx_tp);
2005 idx_end.push_back(t.find(",", idx_tp + 1));
2006 }
2007 // Keep the names between the separators
2008 std::vector<utils::String> MarkersInFile;
2009 for (size_t i = 0; i < idx_init.size() - 1; ++i) {
2010 utils::String tp;
2011 for (size_t j = *(idx_init.begin() + i) + 1; j < *(idx_end.begin() + i);
2012 ++j) {
2013 tp.push_back(t.at(j));
2014 }
2015 MarkersInFile.push_back(tp);
2016 }
2017
2018 // Compare with the given order
2019 int *ordre;
2020 ordre = new int[3 * MarkersInFile.size()];
2021 for (int i = 0; i < static_cast<int>(3 * MarkersInFile.size()); ++i) {
2022 ordre[i] = -1;
2023 }
2024 for (int i = 0; i < static_cast<int>(markOrder.size()); ++i) {
2025 size_t cmp = 0;
2026 utils::String m1 = (*(markOrder.begin() + i));
2027 while (1) {
2028 utils::String m2 = (*(MarkersInFile.begin() + cmp));
2029 if (!m1.compare(m2)) {
2030 ordre[3 * cmp] = 3 * i;
2031 ordre[3 * cmp + 1] = 3 * i + 1;
2032 ordre[3 * cmp + 2] = 3 * i + 2;
2033 break;
2034 } else {
2035 ++cmp;
2036 }
2037 // utils::Error::check(cmp<MarkersInFile.size(), "The requested
2038 // marker was not found in the file!");
2039 // Le marqueur demandé n'a pas été trouvé dans le fichier!
2040 if (cmp >= MarkersInFile.size()) {
2041 break;
2042 }
2043 }
2044 }
2045
2046 // Go to the data
2047 for (size_t i = 0; i < 4; ++i) { // skip the header
2048 file.read(t);
2049 }
2050
2051 // Find the total number of frames
2052 size_t jumps(1);
2053 size_t nbFrames(0);
2054 if (nFramesToGet != -1) { // If it's all of them, the jumps are 1
2055 while (!file.eof()) {
2056 file.read(t); // Get a line
2057 nbFrames++;
2058 }
2059 file.close();
2060#ifdef _WIN32
2061 utils::IfStream file(
2063 std::ios::in);
2064#else
2065 utils::IfStream file(path.absolutePath().c_str(), std::ios::in);
2066#endif
2067 // Skip the header
2068 for (size_t i = 0; i < 7; ++i) {
2069 file.read(t);
2070 }
2072 nFramesToGet != 0 && nFramesToGet != 1 &&
2073 static_cast<size_t>(nFramesToGet) <= nbFrames,
2074 "nNode should not be 0, 1 or greater "
2075 "than number of frame");
2076 jumps = nbFrames / static_cast<size_t>(nFramesToGet) + 1;
2077 }
2078
2079 std::vector<std::vector<utils::Vector3d>> data;
2080 // now we'll use a stringstream to separate the fields out of the line (comma
2081 // separated)
2082 size_t cmpFrames(1);
2083 while (!file.eof()) {
2084 utils::Vector data_tp = utils::Vector(3 * markOrder.size());
2085 data_tp.setZero();
2086
2087 std::stringstream ss(t);
2088 utils::String field;
2089 size_t cmp = 0;
2090 while (getline(ss, field, ',')) {
2091 // for each field we wish to convert it to a double
2092 // (since we require that the CSV contains nothing but floating-point
2093 // values)
2094 std::stringstream fs(field);
2095 double f = 0.0; // (default value is 0.0)
2096 fs >> f;
2097 if (cmp > 1 && cmp < 3 * MarkersInFile.size() +
2098 2) { // Retirer les timespans et ne pas dépasser
2099 int idx = ordre[cmp - 2];
2100 if (idx >= 0) {
2101 data_tp(static_cast<size_t>(idx)) = f;
2102 }
2103 }
2104 ++cmp;
2105 }
2106 // Once the markers are in order, separate them
2107 std::vector<utils::Vector3d> data_tp2;
2108 for (unsigned int i = 0; i < data_tp.size() / 3; ++i) {
2109 utils::Vector3d node(data_tp.block(3 * i, 0, 3, 1) / 1000);
2110 data_tp2.push_back(node);
2111 }
2112 // Stock the marker vector a that time t
2113 data.push_back(data_tp2); // Put back to meters
2114 for (size_t i = 0; i < jumps; ++i) {
2115 if (cmpFrames != nbFrames) {
2116 file.read(t); // Get the line
2117 cmpFrames++;
2118 } else {
2119 file.read(t);
2120 break;
2121 }
2122 }
2123 }
2124
2125 // Close file
2126 file.close();
2127
2128 return data;
2129}
2130
2132 // Read a segment file
2133
2134 // Open file
2135 // std::cout << "Loading marker file: " << path << std::endl;
2136#ifdef _WIN32
2137 utils::IfStream file(
2138 utils::Path::toWindowsFormat(path.absolutePath()).c_str(), std::ios::in);
2139#else
2140 utils::IfStream file(path.absolutePath().c_str(), std::ios::in);
2141#endif
2142
2143 // Read file
2144 utils::String tp;
2145
2146 // Determine the file version
2147 file.readSpecificTag("version", tp);
2148 size_t version(static_cast<size_t>(atoi(tp.c_str())));
2150 version == 1 || version == 2, "Version not implemented yet");
2151
2152 // Know the number of points
2153 file.readSpecificTag("npoints", tp);
2154 size_t nPoints(static_cast<size_t>(atoi(tp.c_str())));
2155 file.readSpecificTag("nfaces", tp);
2156 size_t nFaces(static_cast<size_t>(atoi(tp.c_str())));
2157
2158 rigidbody::Mesh mesh;
2159 mesh.setPath(path);
2160 // Get all the points
2161 std::map<utils::Equation, double> variable =
2162 std::map<utils::Equation, double>();
2163 utils::Vector3d dump; // Ignore columns 4 5 6
2164 for (size_t iPoints = 0; iPoints < nPoints; ++iPoints) {
2165 utils::Vector3d nodeTp(0, 0, 0);
2166 readVector3d(file, variable, nodeTp);
2167 mesh.addPoint(nodeTp);
2168 if (version == 2) {
2169 readVector3d(file, variable, dump);
2170 }
2171 }
2172
2173 for (size_t iPoints = 0; iPoints < nFaces; ++iPoints) {
2174 rigidbody::MeshFace patchTp;
2175 size_t nVertices;
2176 file.read(nVertices);
2177 if (nVertices != 3) {
2178 utils::Error::raise("Patches must be 3 vertices!");
2179 }
2180 for (size_t i = 0; i < nVertices; ++i) {
2181 file.read(patchTp(i));
2182 }
2183 mesh.addFace(patchTp);
2184 }
2185 return mesh;
2186}
2187
2189 // Read a polygon file format mesh file
2190#ifdef _WIN32
2191 utils::String filepath(
2193#else
2194 utils::String filepath(path.absolutePath().c_str());
2195#endif
2196
2197 std::ifstream file(filepath.c_str(), std::ios::in);
2198 utils::Error::check(file.is_open(), "Failed to load file " + filepath);
2199
2200 struct PlyProperty {
2201 PlyProperty(const utils::String &name, bool isList)
2202 : name(name), isList(isList) {}
2203 utils::String name;
2204 bool isList;
2205 };
2206
2207 struct PlyElement {
2208 PlyElement(const utils::String &name, size_t count)
2209 : name(name), count(count) {}
2210 utils::String name;
2211 size_t count;
2212 std::vector<PlyProperty> properties;
2213 };
2214
2215 utils::String line;
2216 std::getline(file, line);
2217 std::stringstream firstLine(line);
2218 utils::String plyTag;
2219 firstLine >> plyTag;
2221 !plyTag.tolower().compare("ply"),
2222 filepath + " is not a valid PLY file");
2223
2224 std::vector<PlyElement> elements;
2225 bool isAscii(false);
2226 int currentElement(-1);
2227 while (std::getline(file, line)) {
2228 std::stringstream ss(line);
2229 utils::String tag;
2230 ss >> tag;
2231 tag = tag.tolower();
2232 if (!tag.compare("comment") || tag.empty()) {
2233 continue;
2234 } else if (!tag.compare("format")) {
2235 utils::String format;
2236 ss >> format;
2237 isAscii = !format.tolower().compare("ascii");
2238 } else if (!tag.compare("element")) {
2239 utils::String name;
2240 size_t count;
2241 ss >> name >> count;
2242 elements.push_back(PlyElement(name.tolower(), count));
2243 currentElement = static_cast<int>(elements.size()) - 1;
2244 } else if (!tag.compare("property")) {
2246 currentElement >= 0, "PLY property found before any element");
2247 utils::String propertyType;
2248 ss >> propertyType;
2249 if (!propertyType.tolower().compare("list")) {
2250 utils::String countType, valueType, name;
2251 ss >> countType >> valueType >> name;
2252 elements[static_cast<size_t>(currentElement)].properties.push_back(
2253 PlyProperty(name.tolower(), true));
2254 } else {
2255 utils::String name;
2256 ss >> name;
2257 elements[static_cast<size_t>(currentElement)].properties.push_back(
2258 PlyProperty(name.tolower(), false));
2259 }
2260 } else if (!tag.compare("end_header")) {
2261 break;
2262 }
2263 }
2264
2266 isAscii,
2267 utils::String("Only ASCII PLY files are supported for meshfile: ") +
2268 filepath);
2269
2270 rigidbody::Mesh mesh;
2271 mesh.setPath(path);
2272
2273 for (size_t iElement = 0; iElement < elements.size(); ++iElement) {
2274 const PlyElement &element(elements[iElement]);
2275
2276 for (size_t iRecord = 0; iRecord < element.count; ++iRecord) {
2277 if (!element.name.compare("vertex")) {
2278 utils::Vector3d vertex(0, 0, 0);
2279 for (size_t iProperty = 0; iProperty < element.properties.size();
2280 ++iProperty) {
2281 const PlyProperty &property(element.properties[iProperty]);
2282 if (property.isList) {
2283 size_t nValues;
2284 double dump;
2285 file >> nValues;
2286 for (size_t i = 0; i < nValues; ++i) {
2287 file >> dump;
2288 }
2289 } else {
2290 double value;
2291 file >> value;
2292 if (!property.name.compare("x")) {
2293 vertex[0] = value;
2294 } else if (!property.name.compare("y")) {
2295 vertex[1] = value;
2296 } else if (!property.name.compare("z")) {
2297 vertex[2] = value;
2298 }
2299 }
2300 }
2301 mesh.addPoint(vertex);
2302 } else if (!element.name.compare("face")) {
2303 std::vector<int> vertexIndices;
2304 for (size_t iProperty = 0; iProperty < element.properties.size();
2305 ++iProperty) {
2306 const PlyProperty &property(element.properties[iProperty]);
2307 if (property.isList) {
2308 size_t nValues;
2309 file >> nValues;
2310 for (size_t i = 0; i < nValues; ++i) {
2311 int value;
2312 file >> value;
2313 if (
2314 !property.name.compare("vertex_indices") ||
2315 !property.name.compare("vertex_index")) {
2316 vertexIndices.push_back(value);
2317 }
2318 }
2319 } else {
2320 double dump;
2321 file >> dump;
2322 }
2323 }
2324
2326 vertexIndices.size() >= 3,
2327 "PLY faces must have at least 3 vertices");
2328 for (size_t i = 1; i + 1 < vertexIndices.size(); ++i) {
2329 mesh.addFace(
2330 {vertexIndices[0], vertexIndices[i], vertexIndices[i + 1]});
2331 }
2332 } else {
2333 for (size_t iProperty = 0; iProperty < element.properties.size();
2334 ++iProperty) {
2335 const PlyProperty &property(element.properties[iProperty]);
2336 if (property.isList) {
2337 size_t nValues;
2338 double dump;
2339 file >> nValues;
2340 for (size_t i = 0; i < nValues; ++i) {
2341 file >> dump;
2342 }
2343 } else {
2344 double dump;
2345 file >> dump;
2346 }
2347 }
2348 }
2349 }
2350 }
2351
2352 return mesh;
2353}
2354
2356 // Read a bone file
2357
2358 // Open file
2359 // std::cout << "Loading marker file: " << path << std::endl;
2360#ifdef _WIN32
2361 utils::IfStream file(
2362 utils::Path::toWindowsFormat(path.absolutePath()).c_str(), std::ios::in);
2363#else
2364 utils::IfStream file(path.absolutePath().c_str(), std::ios::in);
2365#endif
2366
2367 // Read file
2368 utils::String tp;
2369
2370 rigidbody::Mesh mesh;
2371 mesh.setPath(path);
2372
2373 // Get all the points
2374 utils::Vector3d vertex;
2375 rigidbody::MeshFace patch;
2376 utils::String text;
2377 std::map<utils::Equation, double> variable =
2378 std::map<utils::Equation, double>();
2379 while (true) {
2380 // If we get to the end of file, exit loop
2381 if (file.eof()) {
2382 break;
2383 }
2384
2385 // Read the first element of the line
2386 file.read(text);
2387
2388 if (!text.compare("v")) {
2389 // If first element is a v, then a vertex is found
2390 readVector3d(file, variable, vertex);
2391 mesh.addPoint(vertex);
2392 } else if (!text.compare("f")) {
2393 // If first element is a f, then a face is found
2394 // Face is ignore for now
2395 for (size_t i = 0; i < 3; ++i) {
2396 file.read(text);
2397 size_t idxSlash = text.find("/");
2398 utils::String tata3(text.substr(0, idxSlash));
2399 patch(i) = (std::stoi(text.substr(0, idxSlash)) - 1);
2400 }
2401 file.getline(text); // Ignore last element if it is a 4 vertex based
2402 mesh.addFace(patch.DeepCopy());
2403 } else {
2404 // Ignore the line
2405 file.getline(text);
2406 }
2407 }
2408
2409 return mesh;
2410}
2411
2412#ifdef MODULE_VTP_FILES_READER
2413
2414rigidbody::Mesh Reader::readMeshFileVtp(const utils::Path &path) {
2415 // Read an OpenSim formatted mesh file
2416
2417 // Read the file
2418#ifdef _WIN32
2419 utils::String filepath(
2421#else
2422 utils::String filepath(path.absolutePath().c_str());
2423#endif
2424
2425 tinyxml2::XMLDocument doc;
2426 tinyxml2::XMLError err = doc.LoadFile(filepath.c_str());
2428 err == tinyxml2::XML_SUCCESS, "Failed to load file " + filepath);
2429
2430 rigidbody::Mesh mesh;
2431 mesh.setPath(path);
2432
2433 // Navigate up to VTKFile/PolyData/Piece
2434 tinyxml2::XMLElement *polydata = doc.FirstChildElement("VTKFile")
2435 ->FirstChildElement("PolyData")
2436 ->FirstChildElement("Piece");
2437
2438 int numberOfPoints = 0, NumberOfPolys = 0;
2439 polydata->QueryIntAttribute("NumberOfPoints", &numberOfPoints);
2440 polydata->QueryIntAttribute("NumberOfPolys", &NumberOfPolys);
2441
2442 // Get the points
2443 utils::String field;
2444 {
2445 tinyxml2::XMLElement *pointsElement =
2446 polydata->FirstChildElement("Points")->FirstChildElement("DataArray");
2447 const char *pointsText = pointsElement ? pointsElement->GetText() : "";
2448 std::stringstream ss(pointsText);
2449 for (int i = 0; i < numberOfPoints; ++i) {
2450 double x, y, z;
2451 ss >> x >> y >> z;
2452 mesh.addPoint(utils::Vector3d(x, y, z));
2453 }
2454 }
2455
2456 // Get the patches
2457 {
2458 tinyxml2::XMLElement *polysElement =
2459 polydata->FirstChildElement("Polys")->FirstChildElement("DataArray");
2460 const char *polysText = polysElement ? polysElement->GetText() : "";
2461 std::stringstream ss(polysText);
2462 for (int i = 0; i < NumberOfPolys; ++i) {
2463 int vertex1, vertex2, vertex3;
2464 ss >> vertex1 >> vertex2 >> vertex3;
2465 mesh.addFace({vertex1, vertex2, vertex3});
2466 }
2467 }
2468
2469 return mesh;
2470}
2471#endif // MODULE_VTP_FILES_READER
2472
2474 utils::IfStream &file,
2475 const std::map<utils::Equation, double> &variable,
2476 utils::Vector3d &vector) {
2477 for (unsigned int i = 0; i < 3; ++i) {
2478 file.read(vector(i), variable);
2479 }
2480}
2481
2483 utils::IfStream &file,
2484 const std::map<utils::Equation, double> &variable,
2485 utils::Matrix3d &matrix) {
2486 for (unsigned int i = 0; i < 3; ++i) {
2487 for (unsigned int j = 0; j < 3; ++j) {
2488 file.read(matrix(i, j), variable);
2489 }
2490 }
2491}
2492
2494 utils::IfStream &file,
2495 const std::map<utils::Equation, double> &variable,
2496 bool RTinMatrix,
2497 utils::RotoTrans &RT) {
2498 if (RTinMatrix) { // Matrix 4x4
2499 // Counter for classification (Compteur pour classification)
2500 for (unsigned int i = 0; i < 4; ++i) {
2501 for (unsigned int j = 0; j < 4; ++j) {
2502 file.read(RT(i, j), variable);
2503 }
2504 }
2505 } else {
2506 utils::String seq("xyz");
2507 utils::Vector3d rot(0, 0, 0);
2508 utils::Vector3d trans(0, 0, 0);
2509 // Transcribe the rotations
2510 for (unsigned int i = 0; i < 3; ++i) {
2511 file.read(rot(i), variable);
2512 }
2513 // Transcribe the angular sequence for the rotation
2514 file.read(seq);
2515 // Transcribe the translations
2516 for (unsigned int i = 0; i < 3; ++i) {
2517 file.read(trans(i), variable);
2518 }
2519 RT = utils::RotoTrans(rot, trans, seq);
2520 }
2521 RT.checkUnitary();
2522}
2523
2525 // Read a bone file
2526
2527 // Open file
2528 // std::cout << "Loading marker file: " << path << std::endl;
2529#ifdef _WIN32
2530 utils::IfStream file(
2532 std::ios::in | std::ios::binary);
2533#else
2534 utils::IfStream file(
2535 path.absolutePath().c_str(), std::ios::in | std::ios::binary);
2536#endif
2537
2538 // The next step test if the file is ASCII or binary
2539 bool isBinary = false;
2540 try {
2541 file.reachSpecificTag("facet", 20);
2542 } catch (std::runtime_error) {
2543 isBinary = true;
2544 file.resetCursor();
2545 }
2546
2547 rigidbody::Mesh mesh;
2548 mesh.setPath(path);
2549 if (isBinary) {
2550 // Know the number of points
2551 char headerChar[80] = "";
2552 char nbTrianglesChar[4];
2553 char dummy[2];
2554 file.readFromBinary(headerChar, 80); // Skip header
2555 file.readFromBinary(nbTrianglesChar, 4);
2556 size_t nbTriangles =
2557 static_cast<size_t>(*((unsigned int *)nbTrianglesChar));
2558
2559 utils::Vector3d normal;
2560 utils::Vector3d vertex;
2561 for (int i = 0; i < nbTriangles; ++i) {
2562 file.readFromBinary(normal);
2563 for (size_t j = 0; j < 3; ++j) {
2564 file.readFromBinary(vertex);
2565 mesh.addPoint(vertex);
2566 }
2567 file.readFromBinary(dummy, 2);
2568 mesh.addFace({3 * i + 0, 3 * i + 1, 3 * i + 2});
2569 }
2570 } else {
2571 // Due to the test, the pointer is already at the first "facet"
2572
2573 // Read file
2574 utils::String tp;
2575 int i = 0;
2576 utils::Vector3d vertex;
2577 while (true) {
2578 file.reachSpecificTag("vertex");
2579 for (size_t k = 0; k < 3; ++k) {
2580 for (size_t j = 0; j < 3; ++j) {
2581 file.read(vertex(static_cast<unsigned int>(j)));
2582 }
2583 mesh.addPoint(vertex);
2584 file.read(tp);
2585 }
2586 mesh.addFace({3 * i + 0, 3 * i + 1, 3 * i + 2});
2587
2588 for (size_t j = 0; j < 3; ++j) {
2589 // Read 3 dummies
2590 file.read(tp);
2591 }
2592
2593 ++i;
2594 if (!tp.compare("endsolid")) {
2595 break;
2596 }
2597 }
2598 }
2599
2600 return mesh;
2601}
2602
2603std::vector<std::vector<utils::Vector3d>> Reader::readViconMarkerFile(
2604 const utils::Path &path,
2605 int nFramesToGet) {
2606 // Read file
2607#ifdef _WIN32
2608 utils::IfStream file(
2609 utils::Path::toWindowsFormat(path.absolutePath()).c_str(), std::ios::in);
2610#else
2611 utils::IfStream file(path.absolutePath().c_str(), std::ios::in);
2612#endif
2613 utils::String t;
2614
2615 // Get the acquisition frequency
2616 // frequency = atoi(findImportantParameter(file, "trajectories").c_str());
2617
2618 // Get the order of the markers in the file
2619 for (size_t i = 0; i < 3; ++i) { // skip the header
2620 file.read(t);
2621 }
2622 size_t idx_tp = 0;
2623 std::vector<size_t> idx_init;
2624 std::vector<size_t> idx_end;
2625 // Find the separators (: et ,)
2626 while (idx_tp < t.length()) {
2627 idx_tp = t.find(":", idx_tp + 1);
2628 idx_init.push_back(idx_tp);
2629 idx_end.push_back(t.find(",", idx_tp + 1));
2630 }
2631 // Keep the names between the separators
2632 std::vector<utils::String> MarkersInFile;
2633 for (size_t i = 0; i < idx_init.size() - 1; ++i) {
2634 utils::String tp;
2635 for (size_t j = *(idx_init.begin() + i) + 1; j < *(idx_end.begin() + i);
2636 ++j) {
2637 tp.push_back(t.at(j));
2638 }
2639 MarkersInFile.push_back(tp);
2640 }
2641
2642 // Close file
2643 file.close();
2644
2645 return readViconMarkerFile(path, MarkersInFile, nFramesToGet);
2646}
The actual musculoskeletal model that holds everything in biorbd.
static rigidbody::Mesh readMeshFileObj(const utils::Path &path)
Read a OBJ file containing the meshing of a segment.
static std::vector< std::vector< utils::Vector3d > > readMarkerDataFile(const utils::Path &path)
Read a bioMark file, containing markers data.
static void readVector3d(utils::IfStream &file, const std::map< utils::Equation, double > &variable, utils::Vector3d &vector)
Read a Vector 3d.
static void readRtMatrix(utils::IfStream &file, const std::map< utils::Equation, double > &variable, bool RTinMatrix, utils::RotoTrans &RT)
Read a RT matrix either in 4x4 format or Rot1233 seq Trans123 format.
static std::vector< utils::Vector > readTorqueDataFile(const utils::Path &path)
Read a bioTorque file containing generalized torques data.
static rigidbody::Mesh readMeshFileBiorbdSegments(const utils::Path &path)
Read a bioMesh file containing the meshing of a segment.
static void readMatrix33(utils::IfStream &file, const std::map< utils::Equation, double > &variable, utils::Matrix3d &matrix)
Read a Matrix 3x3.
static std::vector< std::vector< utils::Vector3d > > readViconMarkerFile(const utils::Path &path, int nFramesToGet=-1)
Read a Vicon ASCII marker file (CSV formated).
static std::vector< rigidbody::GeneralizedCoordinates > readQDataFile(const utils::Path &path)
Read a bioKin file, containing kinematics data.
static std::vector< utils::Vector > readGroundReactionForceDataFile(const utils::Path &path)
Read a bioGRF file containing ground reaction force (GRF) data.
static Model readModelFile(const utils::Path &path)
Create a biorbd model from a bioMod file.
static rigidbody::Mesh readMeshFileStl(const utils::Path &path)
Read a STL file containing the meshing of a segment.
static rigidbody::Mesh readMeshFilePly(const utils::Path &path)
Read a PLY file containing the meshing of a segment.
static void readViconForceFile(const utils::Path &path, std::vector< std::vector< size_t > > &frame, std::vector< size_t > &frequency, std::vector< std::vector< utils::Vector3d > > &force, std::vector< std::vector< utils::Vector3d > > &moment, std::vector< std::vector< utils::Vector3d > > &cop)
Read a Vicon ASCII force file.
static std::vector< utils::Vector > readActivationDataFile(const utils::Path &path)
Read a bioMus fine, containing muscle activations data.
Class Geometry of the muscle.
Definition Geometry.h:31
Holder of all the path modifiers of a muscle.
Half cylinder of infinite length (the length only affect the graphical representation) object that ma...
Class ActuatorConstant is a joint actuator type which maximum is contant.
Class ActuatorGauss3p is a joint actuator type which maximum is 3 parameter gaussian (Gauss3p) Please...
Class ActuatorGauss6p is a joint actuator type which maximum is bimodal 6 parameter gaussian (Gauss6p...
Class ActuatorLinear is a joint actuator type that linearly evolves.
Class ActuatorSigmoidGauss3p is a joint actuator type which maximum is 3 parameter gaussian (Gauss3p)...
Class LigamentSpringLinear is a ligament with same behavior than linear spring defined by: k(l-l0) wi...
Class LigamentSpringSecondOrder is a ligament with same behavior than second order spring defined by:...
Class FatigueParameters that holds the muscle fatigue parameters.
void setFatigueRate(const utils::Scalar &fatigueRate)
Set the fatigue rate.
void setRecoveryFactor(const utils::Scalar &recoveryFactor)
Set the recovery factor.
void setDevelopFactor(const utils::Scalar &developFactor)
Set the develop factor.
void setRecoveryRate(const utils::Scalar &recoveryRate)
Set the recovery rate.
Time derivative of activation as described by Buchanan (https://www.sciencedirect....
EMG holder to interact with the muscle.
Definition State.h:17
Class PassiveTorqueConstant is a joint passive torque type which maximum is contant.
Class PassiveTorqueExponential is a joint passive torque type that linearly evolves passive torque is...
Class PassiveTorqueLinear is a joint passive torque type that linearly evolves Linear passive torque ...
size_t AddLoopConstraint(size_t body_id_predecessor, size_t body_id_successor, const utils::RotoTrans &X_predecessor, const utils::RotoTrans &X_successor, const utils::SpatialVector &axis, const utils::String &name, bool enableStabilization=false, double stabilizationParam=0.1)
Add a loop constraint to the constraint set.
Definition Contacts.cpp:139
size_t AddConstraint(size_t bodyId, const utils::Vector3d &bodyPoint, const utils::Vector3d &worldNormal, const utils::String &name, const utils::String &parentName)
Add a constraint to the constraint set.
Definition Contacts.cpp:46
void addIMU(bool technical=true, bool anatomical=true)
Add a new inertial measurement unit to the set.
Definition IMUs.cpp:40
size_t AddSegment(const utils::String &segmentName, const utils::String &parentName, const utils::String &translationSequence, const utils::String &rotationSequence, const std::vector< utils::Range > &QRanges, const std::vector< utils::Range > &QdotRanges, const std::vector< utils::Range > &QddotRanges, const std::vector< utils::Scalar > &jointDampings, const SegmentCharacteristics &characteristics, const utils::RotoTrans &referenceFrame)
Add a segment to the model.
Definition Joints.cpp:112
size_t getDofIndex(const utils::String &SegmentName, const utils::String &dofName)
Return the index of a DoF in a segment.
Definition Joints.cpp:1858
void addMarker(const NodeSegment &pos, const utils::String &name, const utils::String &parentName, bool technical, bool anatomical, const utils::String &axesToRemove, int id=-1)
Add a marker to the set.
Definition Markers.cpp:47
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
MeshFace DeepCopy() const
Deep copy of a MeshFace.
Definition MeshFace.cpp:11
A class that holds the geometry of a segment.
Definition Mesh.h:22
void setColor(const utils::Vector3d &color)
Set the patch color.
Definition Mesh.cpp:58
void addFace(const MeshFace &face)
Add a face patch to the mesh.
Definition Mesh.cpp:94
void addPoint(const utils::Vector3d &node)
Add a point to the mesh.
Definition Mesh.cpp:65
void scale(const utils::Vector3d &scaler)
Scale the vertex wrt to 0, 0, 0. Warnign, this function is applied when called, meaning that if it is...
Definition Mesh.cpp:82
void rotate(const utils::RotoTrans &rt)
rotateVertex Apply the RT to the vertex
Definition Mesh.cpp:73
void setPath(const utils::Path &path)
Set the path of the underlying mesh file.
Definition Mesh.cpp:107
A point attached to a segment, generally speaking a skin marker.
Definition NodeSegment.h:20
Characteristics of a segment, namely the mass, the center of mass, the inertia, the length and its me...
A point attached to a contact node, generally speaking a skin marker.
void addSoftContact(const SoftContactNode &contact)
Add a new contact to the contact set.
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
Wrapper for the an std::ifstream with increased capacities.
Definition IfStream.h:20
bool reachSpecificTag(const String &tag, size_t maxTag=-1)
Advance in the file to a specific tag.
Definition IfStream.cpp:58
bool close()
Close the file.
Definition IfStream.cpp:220
bool eof()
Return if the file is at the end.
Definition IfStream.cpp:225
bool readFromBinary(char *output, int n_elements)
Read a certain amounts of bits in binary.
Definition IfStream.cpp:152
bool readSpecificTag(const String &tag, String &text)
Advance in the file to a specific tag.
Definition IfStream.cpp:51
void getline(String &text)
Read a whole line.
Definition IfStream.cpp:210
bool read(String &text)
Read a word in the file skipping the word if it is c-like commented.
Definition IfStream.cpp:101
void resetCursor()
Reset the file cursor to the 0 position.
Definition IfStream.cpp:214
A wrapper for the Eigen::MatrixXd.
Definition Matrix3d.h:21
void setParent(const String &name)
Set the parent name of the node.
Definition Node.cpp:47
void setName(const String &name)
Set the name of the node.
Definition Node.cpp:41
Collection of methods to manipulate path.
Definition Path.h:16
String absolutePath() const
Return the absolute path relative to root.
Definition Path.cpp:265
bool isFileReadable() const
Test if file exist on the computer and is accessible.
Definition Path.cpp:92
const String & filename() const
Return the filename.
Definition Path.cpp:321
String relativePath() const
Return relative path to current working directory.
Definition Path.cpp:169
const String & folder() const
Return the folder of the file.
Definition Path.cpp:317
const String & extension() const
Return the extension of the file.
Definition Path.cpp:325
static String toWindowsFormat(const String &path)
Return the path to Windows format.
Definition Path.cpp:298
A RotoTrans which is attached to a segment.
Homogenous matrix to describe translations and rotations simultaneously.
Definition RotoTrans.h:33
void checkUnitary()
Check if the RotoTrans has a unitary matrix of rotation and the last row is (0, 0,...
static RotoTrans fromMarkers(const rigidbody::NodeSegment &origin, const std::pair< rigidbody::NodeSegment, rigidbody::NodeSegment > &axis1markers, const std::pair< rigidbody::NodeSegment, rigidbody::NodeSegment > &axis2markers, const std::pair< String, String > &axesNames, const String &axisToRecalculate)
fromMarkers Creates a system of axes from two axes and an origin defined by markers
Definition RotoTrans.cpp:75
Wrapper of the Eigen::Matrix<double, 6, 1> or Casadi::MX(6, 1).
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
Wrapper around Eigen Vector3d and attach it to a parent.
Definition Vector3d.h:27
Wrapper of the Eigen VectorXd.
Definition Vector.h:21