Biorbd
ModelReader.cpp
1 #define BIORBD_API_EXPORTS
2 #include "ModelReader.h"
3 
4 #include <limits.h>
5 
6 #include "BiorbdModel.h"
7 #include "Utils/Error.h"
8 #include "Utils/IfStream.h"
9 #include "Utils/String.h"
10 #include "Utils/Equation.h"
11 #include "Utils/Vector.h"
12 #include "Utils/Vector3d.h"
13 #include "Utils/Rotation.h"
14 #include "Utils/Range.h"
15 #include "Utils/SpatialVector.h"
16 #include "RigidBody/GeneralizedCoordinates.h"
17 #include "RigidBody/Mesh.h"
18 #include "RigidBody/SegmentCharacteristics.h"
19 #include "RigidBody/IMU.h"
20 #include "RigidBody/MeshFace.h"
21 #include "RigidBody/NodeSegment.h"
22 
23 #ifdef MODULE_ACTUATORS
24 #include "Actuators/ActuatorConstant.h"
25 #include "Actuators/ActuatorLinear.h"
26 #include "Actuators/ActuatorGauss3p.h"
27 #include "Actuators/ActuatorGauss6p.h"
28 #endif // MODULE_ACTUATORS
29 
30 #ifdef MODULE_MUSCLES
31 #include "Muscles/Muscle.h"
32 #include "Muscles/Geometry.h"
33 #include "Muscles/MuscleGroup.h"
34 #include "Muscles/ViaPoint.h"
35 #include "Muscles/WrappingCylinder.h"
36 #include "Muscles/FatigueParameters.h"
37 #include "Muscles/State.h"
38 #include "Muscles/Characteristics.h"
39 #include "Muscles/ViaPoint.h"
40 #include "Muscles/PathModifiers.h"
41 #endif // MODULE_MUSCLES
42 
43 // ------ Public methods ------ //
44 biorbd::Model biorbd::Reader::readModelFile(const biorbd::utils::Path &path)
45 {
46  // Add the elements that have been entered
47  biorbd::Model model;
48  readModelFile(path, &model);
49  return model;
50 }
51 
52 void biorbd::Reader::readModelFile(
53  const biorbd::utils::Path &path,
54  biorbd::Model *model)
55 { // Open file
56  if (!path.isFileReadable())
57  biorbd::utils::Error::raise("File " + path.absolutePath()
58  + " could not be open");
59 
60 #ifdef _WIN32
61  biorbd::utils::IfStream file(
62  biorbd::utils::Path::toWindowsFormat(
63  path.absolutePath()).c_str(), std::ios::in);
64 #else
65  biorbd::utils::IfStream file(
66  path.absolutePath().c_str(), std::ios::in);
67 #endif
68 
69  // Read file
70  biorbd::utils::String main_tag;
71  biorbd::utils::String property_tag;
72  biorbd::utils::String subproperty_tag;
73 
74  // Variable used to replace doubles
75  std::map<biorbd::utils::Equation, double> variable;
76 
77  // Determine the file version
78  file.readSpecificTag("version", main_tag);
79  unsigned int version(static_cast<unsigned int>(atoi(main_tag.c_str())));
80  biorbd::utils::Error::check((version == 1 || version == 2 || version == 3 || version == 4),
81  "Version " + main_tag + " is not implemented yet");
82 
83 #ifdef MODULE_ACTUATORS
84  bool hasActuators = false;
85 #endif // MODULE_ACTUATORS
86 
87  biorbd::utils::String name;
88  try {
89  while(file.read(main_tag)){ // Attempt read into main_tag, return false if it fails
90  // Reinitialize some tags
91  name = "";
92  property_tag = "";
93  subproperty_tag = "";
94 
95  // If it is a segment
96  if (!main_tag.tolower().compare("segment")){
97  file.read(name);
98  biorbd::utils::String parent_str("root");
99  biorbd::utils::String trans = "";
100  biorbd::utils::String rot = "";
101  bool RTinMatrix(true);
102  if (version >= 3) // By default for version 3 (not in matrix)
103  RTinMatrix = false;
104  bool isRTset(false);
105  double mass = 0.00000001;
106  RigidBodyDynamics::Math::Matrix3d inertia(RigidBodyDynamics::Math::Matrix3d::Identity());
107  biorbd::utils::Rotation RT_R(RigidBodyDynamics::Math::Matrix3d::Identity());
108  biorbd::utils::Vector3d RT_T(0,0,0);
109  biorbd::utils::Vector3d com(0,0,0);
110  biorbd::rigidbody::Mesh mesh;
111  int segmentByFile(-1); // -1 non setté, 0 pas par file, 1 par file
112  int PF = -1;
113  std::vector<biorbd::utils::Range> QRanges;
114  std::vector<biorbd::utils::Range> QDotRanges;
115  std::vector<biorbd::utils::Range> QDDotRanges;
116  bool isRangeQSet(false); // Ranges must be done only after translation AND rotations tags
117  bool isRangeQDotSet(false); // Ranges must be done only after translation AND rotations tags
118  bool isRangeQDDotSet(false); // Ranges must be done only after translation AND rotations tags
119  while(file.read(property_tag) && property_tag.tolower().compare("endsegment")){
120  if (!property_tag.tolower().compare("parent")){
121  // Dynamically find the parent number
122  file.read(parent_str);
123  if (parent_str.tolower().compare("root"))
124  biorbd::utils::Error::check(model->GetBodyId(parent_str.c_str()), "Wrong name in a segment");
125  }
126  else if (!property_tag.tolower().compare("translations")){
127  biorbd::utils::Error::check(
128  !isRangeQSet, "Translations must appear before the rangesq tag");
129  biorbd::utils::Error::check(
130  !isRangeQDotSet, "Translations must appear before the rangesqdot tag");
131  biorbd::utils::Error::check(
132  !isRangeQDDotSet, "Translations must appear before the rangesqddot tag");
133  file.read(trans);
134  }
135  else if (!property_tag.tolower().compare("rotations")){
136  biorbd::utils::Error::check(
137  !isRangeQSet, "Rotations must appear before the rangesq tag");
138  biorbd::utils::Error::check(
139  !isRangeQDotSet, "Rotations must appear before the rangesqdot tag");
140  biorbd::utils::Error::check(
141  !isRangeQDDotSet, "Rotations must appear before the rangesqddot tag");
142  file.read(rot);
143  }
144  else if (!property_tag.tolower().compare("ranges") ||
145  !property_tag.tolower().compare("rangesq")){
146  double min, max;
147  size_t rotLength(0);
148  if (rot.compare("q")){
149  // If not a quaternion
150  rotLength = rot.length();
151  }
152  for (size_t i=0; i<trans.length() + rotLength; ++i){
153  file.read(min);
154  file.read(max);
155  QRanges.push_back(
156  biorbd::utils::Range (min, max));
157  }
158  isRangeQSet = true;
159  }
160  else if (!property_tag.tolower().compare("rangesqdot")){
161  double min, max;
162  size_t rotLength(0);
163  if (rot.compare("q")){
164  // If not a quaternion
165  rotLength = rot.length();
166  }
167  for (size_t i=0; i<trans.length() + rotLength; ++i){
168  file.read(min);
169  file.read(max);
170  QDotRanges.push_back(
171  biorbd::utils::Range (min, max));
172  }
173  isRangeQDotSet = true;
174  }
175  else if (!property_tag.tolower().compare("rangesqddot")){
176  double min, max;
177  size_t rotLength(0);
178  if (rot.compare("q")){
179  // If not a quaternion
180  rotLength = rot.length();
181  }
182  for (size_t i=0; i<trans.length() + rotLength; ++i){
183  file.read(min);
184  file.read(max);
185  QDDotRanges.push_back(
186  biorbd::utils::Range (min, max));
187  }
188  isRangeQDDotSet = true;
189  }
190  else if (!property_tag.tolower().compare("mass"))
191  file.read(mass, variable);
192  else if (!property_tag.tolower().compare("inertia")){
193  for (unsigned int i=0; i<3;++i){
194  for (unsigned int j=0; j<3; ++j){
195  file.read(inertia(i,j), variable);
196  }
197  }
198  }
199  else if (!property_tag.tolower().compare("rtinmatrix")){
200  biorbd::utils::Error::check(isRTset==false, "RT should not appear before RTinMatrix");
201  file.read(RTinMatrix);
202  }
203  else if (!property_tag.tolower().compare("rt")){
204  if (RTinMatrix){ // Matrix 4x4
205  // Counter for classification (Compteur pour classification)
206  for (unsigned int i=0; i<3;++i){ // ignore the last line
207  for (unsigned int j=0; j<4; ++j){
208  if (j!=3){
209  file.read(RT_R(j, i), variable);
210  } else {
211  file.read(RT_T(i), variable);
212  }
213  }
214  }
215  }
216  else{
217  biorbd::utils::String seq("xyz");
218  biorbd::utils::Vector3d rot(0, 0, 0);
219  biorbd::utils::Vector3d trans(0, 0, 0);
220  // Transcribe the rotations
221  for (unsigned int i=0; i<3; ++i)
222  file.read(rot(i));
223  // Transcribe the angular sequence for the rotation
224  file.read(seq);
225  //Transcribe the translations
226  for (unsigned int i=0; i<3; ++i)
227  file.read(trans(i));
228  biorbd::utils::RotoTrans RT(rot, trans, seq);
229  RT_R = RT.rot().transpose();
230  RT_T = RT.trans();
231 
232  }
233 
234  isRTset = true;
235  }
236  else if (!property_tag.tolower().compare("com"))
237  for (unsigned int i=0; i<3; ++i)
238  file.read(com(i), variable);
239  else if (!property_tag.tolower().compare("forceplate") || !property_tag.tolower().compare("externalforceindex"))
240  file.read(PF);
241  else if (!property_tag.tolower().compare("mesh")){
242  if (segmentByFile==-1)
243  segmentByFile = 0;
244  else if (segmentByFile == 1)
245  biorbd::utils::Error::raise("You must not mix file and mesh in segment");
246  biorbd::utils::Vector3d tp(0, 0, 0);
247  for (unsigned int i=0; i<3; ++i)
248  file.read(tp(i), variable);
249  mesh.addPoint(tp);
250  }
251  else if (!property_tag.tolower().compare("patch")){
252  if (segmentByFile==-1)
253  segmentByFile = 0;
254  else if (segmentByFile == 1)
255  biorbd::utils::Error::raise("You must not mix file and mesh in segment");
256  biorbd::rigidbody::MeshFace tp;
257  for (unsigned int i=0; i<3; ++i)
258  file.read(tp(i));
259  mesh.addFace(tp);
260  }
261  else if (!property_tag.tolower().compare("meshfile")){
262  if (segmentByFile==-1)
263  segmentByFile = 1;
264  else if (segmentByFile == 0)
265  biorbd::utils::Error::raise("You must not mix file and mesh in segment");
266  biorbd::utils::String filePathInString;
267  file.read(filePathInString);
268  biorbd::utils::Path filePath(filePathInString);
269  if (!filePath.extension().compare("bioMesh"))
270  mesh = readMeshFileBiorbdSegments(path.folder() + filePath.relativePath());
271  else if (!filePath.extension().compare("ply"))
272  mesh = readMeshFilePly(path.folder() + filePath.relativePath());
273  else if (!filePath.extension().compare("obj"))
274  mesh = readMeshFileObj(path.folder() + filePath.relativePath());
275 #ifdef MODULE_VTP_FILES_READER
276  else if (!filePath.extension().compare("vtp"))
277  mesh = readMeshFileVtp(path.folder() + filePath.relativePath());
278 #endif
279  else
280  biorbd::utils::Error::raise(filePath.extension() + " is an unrecognized mesh file");
281  }
282  }
283  if (!isRangeQSet){
284  size_t rotLength(0);
285  if (rot.compare("q")){
286  // If not a quaternion
287  rotLength = rot.length();
288  }
289  for (size_t i=0; i<trans.length() + rotLength; ++i){
290  QRanges.push_back(
291  biorbd::utils::Range ());
292  }
293  }
294  if (!isRangeQDotSet){
295  size_t rotLength(0);
296  if (rot.compare("q")){
297  // If not a quaternion
298  rotLength = rot.length();
299  }
300  for (size_t i=0; i<trans.length() + rotLength; ++i){
301  QDotRanges.push_back(
302  biorbd::utils::Range (-M_PI*10, M_PI*10));
303  }
304  }
305  if (!isRangeQDDotSet){
306  size_t rotLength(0);
307  if (rot.compare("q")){
308  // If not a quaternion
309  rotLength = rot.length();
310  }
311  for (size_t i=0; i<trans.length() + rotLength; ++i){
312  QDDotRanges.push_back(
313  biorbd::utils::Range (-M_PI*100, M_PI*100));
314  }
315  }
316  RigidBodyDynamics::Math::SpatialTransform RT(RT_R, RT_T);
317  biorbd::rigidbody::SegmentCharacteristics characteristics(mass,com,inertia,mesh);
318  model->AddSegment(name, parent_str, trans, rot, QRanges, QDotRanges, QDDotRanges, characteristics, RT, PF);
319  }
320  else if (!main_tag.tolower().compare("gravity")){
321  biorbd::utils::Vector3d gravity(0,0,0);
322  for (unsigned int i=0; i<3; ++i)
323  file.read(gravity(i), variable);
324  model->gravity = gravity;
325  }
326  else if (!main_tag.tolower().compare("variables")){
327  biorbd::utils::String var;
328  while(file.read(var) && var.tolower().compare("endvariables")){
329  if (!var(0).compare("$")){
330  double value;
331  file.read(value);
332  biorbd::utils::Error::check(variable.find(var) == variable.end(), "Variable already defined");
333  variable[var] = value;
334  }
335  }
336  }
337  else if (!main_tag.tolower().compare("marker")){
338  biorbd::utils::String name;
339  file.read(name);
340  unsigned int parent_int = 0;
341  biorbd::utils::String parent_str("root");
342  biorbd::utils::Vector3d pos(0,0,0);
343  bool technical = true;
344  bool anatomical = false;
345  biorbd::utils::String axesToRemove;
346  while(file.read(property_tag) && property_tag.tolower().compare("endmarker"))
347  if (!property_tag.tolower().compare("parent")){
348  // Dynamically find the parent number
349  file.read(parent_str);
350  parent_int = model->GetBodyId(parent_str.c_str());
351  // if parent_int still equals zero, no name has concurred
352  biorbd::utils::Error::check(model->IsBodyId(parent_int), "Wrong name in a segment");
353  }
354  else if (!property_tag.tolower().compare("position"))
355  for (unsigned int i=0; i<3; ++i)
356  file.read(pos(i), variable);
357  else if (!property_tag.tolower().compare("technical"))
358  file.read(technical);
359  else if (!property_tag.tolower().compare("anatomical"))
360  file.read(anatomical);
361  else if (!property_tag.tolower().compare("axestoremove"))
362  file.read(axesToRemove);
363 
364  model->addMarker(pos, name, parent_str, technical, anatomical, axesToRemove, static_cast<int>(parent_int));
365  }
366  else if (!main_tag.tolower().compare("mimu") && version >= 4){
367  biorbd::utils::Error::raise("MIMU is no more the right tag, change it to IMU!");
368  }
369  else if (!main_tag.tolower().compare("imu") || !main_tag.tolower().compare("mimu") || !main_tag.tolower().compare("customrt")){
370  biorbd::utils::String name;
371  file.read(name);
372  biorbd::utils::String parent_str("root");
373  biorbd::utils::RotoTransNode RT;
374  bool RTinMatrix(true);
375  if (version >= 3) // By default for version 3 (not in matrix)
376  RTinMatrix = false;
377  bool isRTset(false);
378  bool technical = true;
379  bool anatomical = false;
380  bool firstTag = true;
381  bool fromMarkers(false);
382  biorbd::utils::String originMarkerName("");
383  biorbd::utils::String firstAxis("");
384  std::vector<biorbd::utils::String> firstAxisMarkerNames(2);
385  biorbd::utils::String secondAxis("");
386  std::vector<biorbd::utils::String> secondAxisMarkerNames(2);
387  biorbd::utils::String axisToRecalculate("");
388  while(file.read(property_tag) && !(!property_tag.tolower().compare("endimu")
389  || !property_tag.tolower().compare("endmimu")
390  || !property_tag.tolower().compare("endcustomrt"))){
391  if (!property_tag.tolower().compare("parent")){
392  // Dynamically find the parent number
393  file.read(parent_str);
394  // If parent_int still equals zero, no name has concurred
395  biorbd::utils::Error::check(model->IsBodyId(model->GetBodyId(parent_str.c_str())), "Wrong name in a segment");
396  }
397  else if (!property_tag.tolower().compare("frommarkers")){
398  if (!firstTag){
399  biorbd::utils::Error::raise("The tag 'fromMarkers' should appear first in the IMU " + name);
400  }
401  else {
402  fromMarkers = true;
403  }
404  }
405  else if (!property_tag.tolower().compare("technical"))
406  file.read(technical);
407  else if (!property_tag.tolower().compare("anatomical"))
408  file.read(anatomical);
409 
410  if (fromMarkers){
411  if (!property_tag.tolower().compare("originmarkername")){
412  file.read(originMarkerName);
413  }
414  else if (!property_tag.tolower().compare("firstaxis")){
415  file.read(firstAxis);
416  }
417  else if (!property_tag.tolower().compare("firstaxismarkernames")){
418  for (unsigned int i = 0; i<2; ++i){
419  file.read(firstAxisMarkerNames[i]);
420  }
421  }
422  else if (!property_tag.tolower().compare("secondaxis")){
423  file.read(secondAxis);
424  }
425  else if (!property_tag.tolower().compare("secondaxismarkernames")){
426  for (unsigned int i = 0; i<2; ++i){
427  file.read(secondAxisMarkerNames[i]);
428  }
429  }
430  else if (!property_tag.tolower().compare("recalculate")){
431  file.read(axisToRecalculate);
432  }
433  }
434  else {
435  if (!property_tag.tolower().compare("rtinmatrix")){
436  biorbd::utils::Error::check(isRTset==false, "RT should not appear before RTinMatrix");
437  file.read(RTinMatrix);
438  }
439  else if (!property_tag.tolower().compare("rt")){
440  if (RTinMatrix){ // Matrix 4x4
441  for (unsigned int i=0; i<4;++i)
442  for (unsigned int j=0; j<4;++j)
443  file.read(RT(i,j), variable);
444  }
445  else {
446  biorbd::utils::String seq("xyz");
447  biorbd::utils::Vector3d rot(0, 0, 0);
448  biorbd::utils::Vector3d trans(0, 0, 0);
449  // Transcribe the rotations
450  for (unsigned int i=0; i<3; ++i)
451  file.read(rot(i));
452  // Transcribe the angle sequence for the rotations
453  file.read(seq);
454  // Transcribe the translations
455  for (unsigned int i=0; i<3; ++i)
456  file.read(trans(i));
457  RT = biorbd::utils::RotoTrans(rot, trans, seq);
458  }
459  isRTset = true;
460  }
461  }
462  }
463  if (fromMarkers){
464  std::vector<biorbd::rigidbody::NodeSegment> allMarkerOnSegment(model->marker(parent_str));
465  biorbd::rigidbody::NodeSegment origin, axis1Beg, axis1End, axis2Beg, axis2End;
466  bool isOrigin(false), isAxis1Beg(false), isAxis1End(false), isAxis2Beg(false), isAxis2End(false);
467  for (auto mark : allMarkerOnSegment){
468  if (!mark.biorbd::utils::Node::name().compare(originMarkerName)){
469  origin = mark;
470  isOrigin = true;
471  }
472  if (!mark.biorbd::utils::Node::name().compare(firstAxisMarkerNames[0])){
473  axis1Beg = mark;
474  isAxis1Beg = true;
475  }
476  if (!mark.biorbd::utils::Node::name().compare(firstAxisMarkerNames[1])){
477  axis1End = mark;
478  isAxis1End = true;
479  }
480  if (!mark.biorbd::utils::Node::name().compare(secondAxisMarkerNames[0])){
481  axis2Beg = mark;
482  isAxis2Beg = true;
483  }
484  if (!mark.biorbd::utils::Node::name().compare(secondAxisMarkerNames[1])){
485  axis2End = mark;
486  isAxis2End = true;
487  }
488  }
489  if (! (isAxis1Beg && isAxis1End && isAxis2Beg && isAxis2End)){
490  biorbd::utils::Error::raise("All the axes name and origin for the IMU " + name + " must be set and correspond to marker names previously defined on the same parent");
491  }
492  if (!(!axisToRecalculate.tolower().compare("firstaxis") || !axisToRecalculate.tolower().compare("secondaxis"))){
493  biorbd::utils::Error::raise("The 'recalculate' option for IMU " + name + " must be 'firstaxis' or 'secondaxis'");
494  }
495  axisToRecalculate = !axisToRecalculate.tolower().compare("firstaxis") ? firstAxis : secondAxis;
496 
497  RT = biorbd::utils::RotoTrans::fromMarkers(
498  origin,
499  {axis1Beg, axis1End},
500  {axis2Beg, axis2End},
501  {firstAxis, secondAxis}, axisToRecalculate);
502  }
503  RT.setName(name);
504  RT.setParent(parent_str);
505  if (!main_tag.tolower().compare("customrt")){
506  model->addRT(RT);
507  }
508  else {
509  model->addIMU(RT, technical, anatomical);
510  }
511  }
512  else if (!main_tag.tolower().compare("contact")){
513  biorbd::utils::String name;
514  file.read(name);
515  unsigned int parent_int = 0;
516  biorbd::utils::String parent_str("root");
517  biorbd::utils::Vector3d pos(0,0,0);
518  biorbd::utils::Vector3d norm(0,0,0);
519  biorbd::utils::String axis("");
520  double acc = 0;
521  while(file.read(property_tag) && property_tag.tolower().compare("endcontact")){
522  if (!property_tag.tolower().compare("parent")){
523  // Dynamically find the parent number
524  file.read(parent_str);
525  parent_int = model->GetBodyId(parent_str.c_str());
526  // If parent_int equals zero, no name has concurred
527  biorbd::utils::Error::check(model->IsBodyId(parent_int), "Wrong name in a segment");
528  }
529  else if (!property_tag.tolower().compare("position"))
530  for (unsigned int i=0; i<3; ++i)
531  file.read(pos(i), variable);
532  else if (!property_tag.tolower().compare("normal"))
533  for (unsigned int i=0; i<3; ++i)
534  file.read(norm(i), variable);
535  else if (!property_tag.tolower().compare("axis"))
536  file.read(axis);
537  else if (!property_tag.tolower().compare("acceleration"))
538  file.read(acc, variable);
539  }
540  if (version == 1){
541 #ifndef BIORBD_USE_CASADI_MATH
542  biorbd::utils::Error::check(norm.norm() == 1.0, "Normal of the contact must be provided" );
543 #endif
544  model->AddConstraint(parent_int, pos, norm, name, acc);
545  }
546  else if (version >= 2){
547  biorbd::utils::Error::check(axis.compare(""), "Axis must be provided");
548  model->AddConstraint(parent_int, pos, axis, name, acc);
549  }
550  }
551  else if (!main_tag.tolower().compare("loopconstraint")){
552  biorbd::utils::String name;
553  unsigned int id_predecessor = 0;
554  unsigned int id_successor = 0;
555  biorbd::utils::String predecessor_str("root");
556  biorbd::utils::String successor_str("root");
557  biorbd::utils::RotoTrans X_predecessor;
558  biorbd::utils::RotoTrans X_successor;
559  biorbd::utils::SpatialVector axis;
560  bool enableStabilization(false);
561  double stabilizationParam(-1);
562  while(file.read(property_tag) && property_tag.tolower().compare("endloopconstraint")){
563  if (!property_tag.tolower().compare("predecessor")){
564  // Dynamically find the parent number
565  file.read(predecessor_str);
566  id_predecessor = model->GetBodyId(predecessor_str.c_str());
567  // If parent_int equals zero, no name has concurred
568  biorbd::utils::Error::check(model->IsBodyId(id_predecessor), "Wrong name in a segment");
569  }
570  if (!property_tag.tolower().compare("successor")){
571  // Dynamically find the parent number
572  file.read(successor_str);
573  id_successor = model->GetBodyId(successor_str.c_str());
574  // If parent_int equals zero, no name has concurred
575  biorbd::utils::Error::check(model->IsBodyId(id_successor), "Wrong name in a segment");
576  } else if (!property_tag.tolower().compare("rtpredecessor")){
577  biorbd::utils::String seq("xyz");
578  biorbd::utils::Vector3d rot(0, 0, 0);
579  biorbd::utils::Vector3d trans(0, 0, 0);
580  // Transcribe the rotations
581  for (unsigned int i=0; i<3; ++i)
582  file.read(rot(i));
583  // Transcribe the angle sequence for the rotations
584  file.read(seq);
585  // Transcribe the translations
586  for (unsigned int i=0; i<3; ++i)
587  file.read(trans(i));
588  X_predecessor = biorbd::utils::RotoTrans(rot, trans, seq);
589  } else if (!property_tag.tolower().compare("rtsuccessor")){
590  biorbd::utils::String seq("xyz");
591  biorbd::utils::Vector3d rot(0, 0, 0);
592  biorbd::utils::Vector3d trans(0, 0, 0);
593  // Transcribe the rotations
594  for (unsigned int i=0; i<3; ++i)
595  file.read(rot(i));
596  // Transcribe the angle sequence for the roations
597  file.read(seq);
598  // Transcribe the translations
599  for (unsigned int i=0; i<3; ++i)
600  file.read(trans(i));
601  X_successor = biorbd::utils::RotoTrans(rot, trans, seq);
602  } else if (!property_tag.tolower().compare("axis"))
603  for (unsigned int i=0; i<axis.size(); ++i)
604  file.read(axis(i), variable);
605  else if (!property_tag.tolower().compare("stabilizationparameter"))
606  file.read(stabilizationParam, variable);
607  }
608  if (stabilizationParam > 0)
609  enableStabilization = true;
610  name = "Loop_" + predecessor_str + "_" + successor_str;
611  model->AddLoopConstraint(id_predecessor, id_successor, X_predecessor, X_successor,
612  axis, name, enableStabilization, stabilizationParam);
613  }
614  else if (!main_tag.tolower().compare("actuator")) {
615  #ifdef MODULE_ACTUATORS
616  hasActuators = true;
617  // The name of the actuator must correspond to the segment number to which it is attached
618  biorbd::utils::String name;
619  file.read(name);
620  unsigned int parent_int = model->GetBodyId(name.c_str());
621  // If parent_int equals zero, no name has concurred
622  biorbd::utils::Error::check(model->IsBodyId(parent_int), "Wrong name in a segment");
623 
624  // Declaration of all the parameters for all types
625  biorbd::utils::String type; bool isTypeSet = false;
626  unsigned int dofIdx(INT_MAX); bool isDofSet = false;
627  biorbd::utils::String str_direction;bool isDirectionSet = false;
628  int int_direction = 0;
629  double Tmax(-1); bool isTmaxSet = false;
630  double T0(-1); bool isT0Set = false;
631  double slope(-1); bool isSlopeSet = false;
632  double wmax(-1); bool iswmaxSet = false;
633  double wc(-1); bool iswcSet = false;
634  double amin(-1); bool isaminSet = false;
635  double wr(-1); bool iswrSet = false;
636  double w1(-1); bool isw1Set = false;
637  double r(-1); bool isrSet = false;
638  double qopt(-1); bool isqoptSet = false;
639  double facteur6p(-1); bool isFacteur6pSet = false;
640  double r2(-1); bool isr2Set = false;
641  double qopt2(-1); bool isqopt2Set = false;
642 
643 
644  while(file.read(property_tag) && property_tag.tolower().compare("endactuator")){
645  if (!property_tag.tolower().compare("type")){
646  file.read(type);
647  isTypeSet = true;
648  }
649  else if (!property_tag.tolower().compare("dof")){
650  biorbd::utils::String dofName;
651  file.read(dofName);
652  dofIdx = model->getDofIndex(name, dofName);
653  isDofSet = true;
654  }
655  else if (!property_tag.tolower().compare("direction")){
656  file.read(str_direction);
657  biorbd::utils::Error::check(!str_direction.tolower().compare("positive") ||
658  !str_direction.tolower().compare("negative"),
659  "Direction should be \"positive\" or \"negative\"");
660  if (!str_direction.tolower().compare("positive"))
661  int_direction = 1;
662  else
663  int_direction = -1;
664  isDirectionSet = true;
665  }
666  else if (!property_tag.tolower().compare("tmax")){
667  file.read(Tmax, variable);
668  isTmaxSet = true;
669  }
670  else if (!property_tag.tolower().compare("t0")){
671  file.read(T0, variable);
672  isT0Set = true;
673  }
674  else if (!property_tag.tolower().compare("pente") || !property_tag.tolower().compare("slope")){
675  file.read(slope, variable);
676  isSlopeSet = true;
677  }
678  else if (!property_tag.tolower().compare("wmax")){
679  file.read(wmax, variable);
680  iswmaxSet = true;
681  }
682  else if (!property_tag.tolower().compare("wc")){
683  file.read(wc, variable);
684  iswcSet = true;
685  }
686  else if (!property_tag.tolower().compare("amin")){
687  file.read(amin, variable);
688  isaminSet = true;
689  }
690  else if (!property_tag.tolower().compare("wr")){
691  file.read(wr, variable);
692  iswrSet = true;
693  }
694  else if (!property_tag.tolower().compare("w1")){
695  file.read(w1, variable);
696  isw1Set = true;
697  }
698  else if (!property_tag.tolower().compare("r")){
699  file.read(r, variable);
700  isrSet = true;
701  }
702  else if (!property_tag.tolower().compare("qopt")){
703  file.read(qopt, variable);
704  isqoptSet = true;
705  }
706  else if (!property_tag.tolower().compare("facteur")){
707  file.read(facteur6p, variable);
708  isFacteur6pSet = true;
709  }
710  else if (!property_tag.tolower().compare("r2")){
711  file.read(r2, variable);
712  isr2Set = true;
713  }
714  else if (!property_tag.tolower().compare("qopt2")){
715  file.read(qopt2, variable);
716  isqopt2Set = true;
717  }
718  }
719  // Verify that everything is there
720  biorbd::utils::Error::check(isTypeSet!=0, "Actuator type must be defined");
721  biorbd::actuator::Actuator* actuator;
722 
723  if (!type.tolower().compare("gauss3p")){
724  biorbd::utils::Error::check(isDofSet && isDirectionSet && isTmaxSet && isT0Set && iswmaxSet && iswcSet && isaminSet &&
725  iswrSet && isw1Set && isrSet && isqoptSet,
726  "Make sure all parameters are defined");
727  actuator = new biorbd::actuator::ActuatorGauss3p(int_direction,Tmax,T0,wmax,wc,amin,wr,w1,r,qopt,dofIdx,name);
728  }
729  else if (!type.tolower().compare("constant")){
730  biorbd::utils::Error::check(isDofSet && isDirectionSet && isTmaxSet,
731  "Make sure all parameters are defined");
732  actuator = new biorbd::actuator::ActuatorConstant(int_direction,Tmax,dofIdx,name);
733  }
734  else if (!type.tolower().compare("linear")){
735  biorbd::utils::Error::check(isDofSet && isDirectionSet && isSlopeSet && isT0Set,
736  "Make sure all parameters are defined");
737  actuator = new biorbd::actuator::ActuatorLinear(int_direction,T0,slope,dofIdx,name);
738  }
739  else if (!type.tolower().compare("gauss6p")){
740  biorbd::utils::Error::check(isDofSet && isDirectionSet && isTmaxSet && isT0Set && iswmaxSet && iswcSet && isaminSet &&
741  iswrSet && isw1Set && isrSet && isqoptSet && isFacteur6pSet && isr2Set && isqopt2Set,
742  "Make sure all parameters are defined");
743  actuator = new biorbd::actuator::ActuatorGauss6p(int_direction,Tmax,T0,wmax,wc,amin,wr,w1,r,qopt,facteur6p, r2, qopt2, dofIdx,name);
744  }
745  else {
746  biorbd::utils::Error::raise("Actuator do not correspond to an implemented one");
747 #ifdef _WIN32
748  actuator = new biorbd::actuator::ActuatorConstant(int_direction, Tmax, dofIdx, name); // Échec de compilation sinon
749 #endif
750  }
751 
752  model->addActuator(*actuator);
753  delete actuator;
754  #else // MODULE_ACTUATORS
755  biorbd::utils::Error::raise("Biorbd was build without the module Actuators but the model defines ones");
756  #endif // MODULE_ACTUATORS
757  } else if (!main_tag.tolower().compare("musclegroup")){
758  #ifdef MODULE_MUSCLES
759  biorbd::utils::String name;
760  file.read(name); // Name of the muscular group
761  // Declaration of the variables
762  biorbd::utils::String origin_parent_str("root");
763  biorbd::utils::String insert_parent_str("root");
764  // Read the file
765  while(file.read(property_tag) && property_tag.tolower().compare("endmusclegroup")){
766  if (!property_tag.tolower().compare("originparent")){
767  // Dynamically find the parent number
768  file.read(origin_parent_str);
769  unsigned int idx = model->GetBodyId(origin_parent_str.c_str());
770  // If parent_int still equals zero, no name has concurred
771  biorbd::utils::Error::check(model->IsBodyId(idx), "Wrong origin parent name for a muscle");
772  }
773  else if (!property_tag.tolower().compare("insertionparent")){
774  // Dynamically find the parent number
775  file.read(insert_parent_str);
776  unsigned int idx = model->GetBodyId(insert_parent_str.c_str());
777  // If parent_int still equals zero, no name has concurred
778  biorbd::utils::Error::check(model->IsBodyId(idx), "Wrong insertion parent name for a muscle");
779  }
780  }
781  model->addMuscleGroup(name, origin_parent_str, insert_parent_str);
782  #else // MODULE_MUSCLES
783  biorbd::utils::Error::raise("Biorbd was build without the module Muscles but the model defines a muscle group");
784  #endif // MODULE_MUSCLES
785  }
786  else if (!main_tag.tolower().compare("muscle")){
787  #ifdef MODULE_MUSCLES
788  biorbd::utils::String name;
789  file.read(name); // Name of the muscle
790  // Declaration of the variables
791  biorbd::muscles::MUSCLE_TYPE type(biorbd::muscles::MUSCLE_TYPE::NO_MUSCLE_TYPE);
792  biorbd::muscles::STATE_TYPE stateType(biorbd::muscles::STATE_TYPE::NO_STATE_TYPE);
793  biorbd::muscles::STATE_FATIGUE_TYPE dynamicFatigueType(biorbd::muscles::STATE_FATIGUE_TYPE::NO_FATIGUE_STATE_TYPE);
794  biorbd::utils::String muscleGroup("");
795  int idxGroup(-1);
796  biorbd::utils::Vector3d origin_pos(0,0,0);
797  biorbd::utils::Vector3d insert_pos(0,0,0);
798  double optimalLength(0);
799  double maxForce(0);
800  double tendonSlackLength(0);
801  double pennAngle(0);
802  double maxExcitation(1);
803  double maxActivation(1);
804  double PCSA(1);
805  biorbd::muscles::FatigueParameters fatigueParameters;
806 
807  // Read file
808  while(file.read(property_tag) && property_tag.tolower().compare("endmuscle")){
809  if (!property_tag.tolower().compare("musclegroup")){
810  // Dynamically find the parent number
811  file.read(muscleGroup);
812  idxGroup = model->getGroupId(muscleGroup);
813  // If parent_int is still equal to zero, no name has concurred
814  biorbd::utils::Error::check(idxGroup!=-1, "Could not find muscle group");
815  }
816  else if (!property_tag.tolower().compare("type")){
817  biorbd::utils::String tp_type;
818  file.read(tp_type);
819  if (!tp_type.tolower().compare("idealizedactuator"))
820  type = biorbd::muscles::MUSCLE_TYPE::IDEALIZED_ACTUATOR;
821  else if (!tp_type.tolower().compare("hill"))
822  type = biorbd::muscles::MUSCLE_TYPE::HILL;
823  else if (!tp_type.tolower().compare("hillthelen") || !tp_type.tolower().compare("thelen"))
824  type = biorbd::muscles::MUSCLE_TYPE::HILL_THELEN;
825  else if (!tp_type.tolower().compare("hillthelenfatigable") || !tp_type.tolower().compare("thelenfatigable"))
826  type = biorbd::muscles::MUSCLE_TYPE::HILL_THELEN_FATIGABLE;
827  else
828  biorbd::utils::Error::raise(property_tag + " is not a valid muscle type");
829  }
830  else if (!property_tag.tolower().compare("statetype")){
831  biorbd::utils::String tp_state;
832  file.read(tp_state);
833  if (!tp_state.tolower().compare("buchanan"))
834  stateType = biorbd::muscles::STATE_TYPE::BUCHANAN;
835  else if (!tp_state.tolower().compare("degroote"))
836  stateType = biorbd::muscles::STATE_TYPE::DE_GROOTE;
837  else
838  biorbd::utils::Error::raise(property_tag + " is not a valid muscle state type");
839  }
840  else if (!property_tag.tolower().compare("originposition"))
841  for (unsigned int i=0; i<3; ++i)
842  file.read(origin_pos(i), variable);
843  else if (!property_tag.tolower().compare("insertionposition"))
844  for (unsigned int i=0; i<3; ++i)
845  file.read(insert_pos(i), variable);
846  else if (!property_tag.tolower().compare("optimallength"))
847  file.read(optimalLength, variable);
848  else if (!property_tag.tolower().compare("tendonslacklength"))
849  file.read(tendonSlackLength, variable);
850  else if (!property_tag.tolower().compare("pennationangle"))
851  file.read(pennAngle, variable);
852  else if (!property_tag.tolower().compare("maximalforce"))
853  file.read(maxForce, variable);
854  else if (!property_tag.tolower().compare("maximalexcitation"))
855  file.read(maxExcitation, variable);
856  else if (!property_tag.tolower().compare("pcsa"))
857  file.read(PCSA, variable);
858  else if (!property_tag.tolower().compare("fatigueparameters")){
859  while(file.read(subproperty_tag) && subproperty_tag.tolower().compare("endfatigueparameters")){
860  if (!subproperty_tag.tolower().compare("type")){
861  biorbd::utils::String tp_fatigue_type;
862  file.read(tp_fatigue_type);
863  if (!tp_fatigue_type.tolower().compare("simple"))
864  dynamicFatigueType = biorbd::muscles::STATE_FATIGUE_TYPE::SIMPLE_STATE_FATIGUE;
865  else if (!tp_fatigue_type.tolower().compare("xia"))
866  dynamicFatigueType = biorbd::muscles::STATE_FATIGUE_TYPE::DYNAMIC_XIA;
867  else
868  biorbd::utils::Error::raise(tp_fatigue_type + " is not a value fatigue parameter type");
869  } else {
870  double param(0);
871  file.read(param);
872  if (!subproperty_tag.tolower().compare("fatiguerate"))
873  fatigueParameters.setFatigueRate(param);
874  else if (!subproperty_tag.tolower().compare("recoveryrate"))
875  fatigueParameters.setRecoveryRate(param);
876  else if (!subproperty_tag.tolower().compare("developfactor"))
877  fatigueParameters.setDevelopFactor(param);
878  else if (!subproperty_tag.tolower().compare("recoveryfactor"))
879  fatigueParameters.setRecoveryFactor(param);
880  }
881  }
882  }
883  }
884  biorbd::utils::Error::check(idxGroup!=-1, "No muscle group was provided!");
885  biorbd::muscles::Geometry geo(
886  biorbd::utils::Vector3d(origin_pos, name + "_origin", model->muscleGroup(static_cast<unsigned int>(idxGroup)).origin()),
887  biorbd::utils::Vector3d(insert_pos, name + "_insertion", model->muscleGroup(static_cast<unsigned int>(idxGroup)).insertion()));
888  biorbd::muscles::State stateMax(maxExcitation, maxActivation);
889  biorbd::muscles::Characteristics characteristics(optimalLength, maxForce, PCSA, tendonSlackLength, pennAngle, stateMax, fatigueParameters);
890  model->muscleGroup(static_cast<unsigned int>(idxGroup)).addMuscle(name,type,geo,characteristics,biorbd::muscles::PathModifiers(),stateType,dynamicFatigueType);
891  #else // MODULE_MUSCLES
892  biorbd::utils::Error::raise("Biorbd was build without the module Muscles but the model defines a muscle");
893  #endif // MODULE_MUSCLES
894  }
895  else if (!main_tag.tolower().compare("viapoint")){
896  #ifdef MODULE_MUSCLES
897  biorbd::utils::String name;
898  file.read(name); // Name of muscle... Eventually add the muscle group
899 
900  // Declaration of the variables
901  biorbd::utils::String parent("");
902  biorbd::utils::String muscle("");
903  biorbd::utils::String musclegroup("");
904  int iMuscleGroup(-1);
905  int iMuscle(-1);
906  biorbd::muscles::ViaPoint position(0,0,0);
907 
908  // Read file
909  while(file.read(property_tag) && property_tag.tolower().compare("endviapoint")){
910  if (!property_tag.tolower().compare("parent")){
911  // Dynamically find the parent number
912  file.read(parent);
913  unsigned int idx = model->GetBodyId(parent.c_str());
914  // If parent_int still equals zero, no name has concurred
915  biorbd::utils::Error::check(model->IsBodyId(idx), "Wrong origin parent name for a muscle");
916  }
917  else if (!property_tag.tolower().compare("muscle"))
918  file.read(muscle);
919  else if (!property_tag.tolower().compare("musclegroup"))
920  file.read(musclegroup);
921  else if (!property_tag.tolower().compare("position"))
922  for (unsigned int i=0; i<3; ++i)
923  file.read(position(i), variable);
924  }
925  iMuscleGroup = model->getGroupId(musclegroup);
926  biorbd::utils::Error::check(iMuscleGroup!=-1, "No muscle group was provided!");
927  iMuscle = model->muscleGroup(static_cast<unsigned int>(iMuscleGroup)).muscleID(muscle);
928  biorbd::utils::Error::check(iMuscle!=-1, "No muscle was provided!");
929  position.setName(name);
930  position.setParent(parent);
931  model->muscleGroup(static_cast<unsigned int>(iMuscleGroup))
932  .muscle(static_cast<unsigned int>(iMuscle)).addPathObject(position);
933  #else // MODULE_MUSCLES
934  biorbd::utils::Error::raise("Biorbd was build without the module Muscles but the model defines a viapoint");
935  #endif // MODULE_MUSCLES
936  }
937  else if (!main_tag.tolower().compare("wrap")){
938  #ifdef MODULE_MUSCLES
939  biorbd::utils::String name;
940  file.read(name); // Name of the wrapping
941 
942  // Declaration of the variables
943  biorbd::utils::String muscle("");
944  biorbd::utils::String musclegroup("");
945  int iMuscleGroup(-1);
946  int iMuscle(-1);
947  biorbd::utils::String parent("");
948  biorbd::utils::RotoTrans RT;
949  double dia(0);
950  double length(0);
951  int side(1);
952 
953  // Read file
954  while(file.read(property_tag) && property_tag.tolower().compare("endwrapping")){
955  if (!property_tag.tolower().compare("parent")){
956  // Dynamically find the parent number
957  file.read(parent);
958  unsigned int idx = model->GetBodyId(parent.c_str());
959  // If parent_int still equals zero, no name has concurred
960  biorbd::utils::Error::check(model->IsBodyId(idx), "Wrong origin parent name for a muscle");
961  }
962  else if (!property_tag.tolower().compare("rt")){
963  for (unsigned int i=0; i<4;++i)
964  for (unsigned int j=0; j<4; ++j)
965  file.read(RT(i,j), variable);
966  }
967  else if (!property_tag.tolower().compare("muscle"))
968  file.read(muscle);
969  else if (!property_tag.tolower().compare("musclegroup"))
970  file.read(musclegroup);
971  else if (!property_tag.tolower().compare("diameter"))
972  file.read(dia, variable);
973  else if (!property_tag.tolower().compare("length"))
974  file.read(length, variable);
975  else if (!property_tag.tolower().compare("wrappingside"))
976  file.read(side);
977  }
978  biorbd::utils::Error::check(dia != 0.0, "Diameter was not defined");
979  biorbd::utils::Error::check(length != 0.0, "Length was not defined");
980  biorbd::utils::Error::check(length < 0.0, "Side was not properly defined");
981  biorbd::utils::Error::check(parent != "", "Parent was not defined");
982  iMuscleGroup = model->getGroupId(musclegroup);
983  biorbd::utils::Error::check(iMuscleGroup!=-1, "No muscle group was provided!");
984  iMuscle = model->muscleGroup(static_cast<unsigned int>(iMuscleGroup)).muscleID(muscle);
985  biorbd::utils::Error::check(iMuscle!=-1, "No muscle was provided!");
986  biorbd::muscles::WrappingCylinder cylinder(RT,dia,length,side,name,parent);
987  model->muscleGroup(static_cast<unsigned int>(iMuscleGroup)).muscle(static_cast<unsigned int>(iMuscle)).addPathObject(cylinder);
988  #else // MODULE_MUSCLES
989  biorbd::utils::Error::raise("Biorbd was build without the module Muscles but the model defines a wrapping object");
990  #endif // MODULE_MUSCLES
991  }
992  }
993  } catch (std::runtime_error message) {
994  biorbd::utils::String error_message("Reading of file \"" + path.filename() + "." + path.extension() + "\" failed with the following error:");
995  error_message += "\n" + biorbd::utils::String(message.what()) + "\n";
996  if (name.compare(""))
997  error_message += "Element: " + main_tag + ", named: " + name + "\n";
998  if (property_tag.compare("") && property_tag.find_first_of("end") != 0)
999  error_message += "Property: " + property_tag + "\n";
1000  if (subproperty_tag.compare("") && subproperty_tag.find_first_of("end") != 0)
1001  error_message += "Subproperty: " + subproperty_tag + "\n";
1002 
1003  biorbd::utils::Error::raise(error_message);
1004  }
1005 #ifdef MODULE_ACTUATORS
1006  if (hasActuators)
1007  model->closeActuator();
1008 #endif // MODULE_ACTUATORS
1009  // Close file
1010  // std::cout << "Model file successfully loaded" << std::endl;
1011  file.close();
1012 }
1013 std::vector<std::vector<biorbd::utils::Vector3d>>
1014 biorbd::Reader::readMarkerDataFile(
1015  const biorbd::utils::Path &path){
1016  // Open file
1017  // std::cout << "Loading marker file: " << path << std::endl;
1018 #ifdef _WIN32
1019  biorbd::utils::IfStream file(
1020  biorbd::utils::Path::toWindowsFormat(
1021  path.absolutePath()).c_str(), std::ios::in);
1022 #else
1023  biorbd::utils::IfStream file(
1024  path.absolutePath().c_str(), std::ios::in);
1025 #endif
1026 
1027  // Read file
1028  biorbd::utils::String tp;
1029 
1030  // Determine the version of the file
1031  file.readSpecificTag("version", tp);
1032  unsigned int version(static_cast<unsigned int>(atoi(tp.c_str())));
1033  biorbd::utils::Error::check(version == 1, "Version not implemented yet");
1034 
1035  // Determine the number of markers
1036  file.readSpecificTag("nbmark", tp);
1037  unsigned int nbMark(static_cast<unsigned int>(atoi(tp.c_str())));
1038 
1039  // Determine the number of nodes
1040  file.readSpecificTag("nbintervals", tp);
1041  unsigned int nbIntervals(static_cast<unsigned int>(atoi(tp.c_str())));
1042 
1043  std::vector<std::vector<biorbd::utils::Vector3d>> markers;
1044  std::vector<biorbd::utils::Vector3d> position;
1045  // Scroll down until the definition of a marker
1046  for (unsigned int j=0; j<nbMark; j++){
1047  while (tp.compare("Marker")){
1048  bool check = file.read(tp);
1049  biorbd::utils::Error::check(check, "Marker file error, wrong size of marker or intervals?");
1050  }
1051 
1052  unsigned int noMarker;
1053  file.read(noMarker);
1054  for (unsigned int i=0; i<=nbIntervals; i++){ // <= because there is nbIntervals+1 values
1055  biorbd::utils::Vector3d mark(0, 0, 0);
1056  for (unsigned int j=0; j<3; ++j)
1057  file.read(mark[j]);
1058  position.push_back(mark);
1059  }
1060  markers.push_back(position);
1061  // reinitialize for the next iteration
1062  tp = "";
1063  position.clear();
1064  }
1065 
1066  // Close file
1067  // std::cout << "Marker file successfully loaded" << std::endl;
1068  file.close();
1069  return markers;
1070 }
1071 
1072 std::vector<biorbd::rigidbody::GeneralizedCoordinates>
1073 biorbd::Reader::readQDataFile(
1074  const utils::Path &path){
1075  // Open file
1076  // std::cout << "Loading kin file: " << path << std::endl;
1077 #ifdef _WIN32
1078  biorbd::utils::IfStream file(
1079  biorbd::utils::Path::toWindowsFormat(
1080  path.absolutePath()).c_str(), std::ios::in);
1081 #else
1082  biorbd::utils::IfStream file(
1083  path.absolutePath().c_str(), std::ios::in);
1084 #endif
1085 
1086  // Read file
1087  biorbd::utils::String tp;
1088 
1089  // Determine the file version
1090  file.readSpecificTag("version", tp);
1091  unsigned int version(static_cast<unsigned int>(atoi(tp.c_str())));
1092  biorbd::utils::Error::check(version == 1, "Version not implemented yet");
1093 
1094  // Determine the number of markers
1095  file.readSpecificTag("nddl", tp);
1096  unsigned int NDDL(static_cast<unsigned int>(atoi(tp.c_str())));
1097 
1098  // Determine the number of nodes
1099  file.readSpecificTag("nbintervals", tp);
1100  unsigned int nbIntervals(static_cast<unsigned int>(atoi(tp.c_str())));
1101 
1102  std::vector<biorbd::rigidbody::GeneralizedCoordinates> kinematics;
1103  // Scroll down until the definition of a marker
1104  for (unsigned int j=0; j<nbIntervals+1; j++){
1105  while (tp.compare("T")){
1106  bool check = file.read(tp);
1107  biorbd::utils::Error::check(check, "Kin file error, wrong size of NDDL or intervals?");
1108  }
1109 
1110  double time;
1111  file.read(time);
1112  biorbd::rigidbody::GeneralizedCoordinates position(NDDL);
1113  for (unsigned int i=0; i<NDDL; i++)
1114  file.read(position(i));
1115 
1116  kinematics.push_back(position);
1117  // reinitialise for the next iteration
1118  tp = "";
1119  }
1120 
1121  // Close file
1122  // std::cout << "Kin file successfully loaded" << std::endl;
1123  file.close();
1124  return kinematics;
1125 }
1126 
1127 std::vector<biorbd::utils::Vector>
1128 biorbd::Reader::readActivationDataFile(
1129  const utils::Path &path){
1130  // Open file
1131  // std::cout << "Loading kin file: " << path << std::endl;
1132 #ifdef _WIN32
1133  biorbd::utils::IfStream file(
1134  biorbd::utils::Path::toWindowsFormat(
1135  path.absolutePath()).c_str(), std::ios::in);
1136 #else
1137  biorbd::utils::IfStream file(
1138  path.absolutePath().c_str(), std::ios::in);
1139 #endif
1140 
1141  // Read file
1142  biorbd::utils::String tp;
1143 
1144  // Determine the file version
1145  file.readSpecificTag("version", tp);
1146  unsigned int version(static_cast<unsigned int>(atoi(tp.c_str())));
1147  biorbd::utils::Error::check(version == 1, "Version not implemented yet");
1148 
1149  // Determine the number of markers
1150  file.readSpecificTag("nbmuscles", tp);
1151  unsigned int nMus(static_cast<unsigned int>(atoi(tp.c_str())));
1152 
1153  // Determine the number of nodes
1154  file.readSpecificTag("nbintervals", tp);
1155  unsigned int nbIntervals(static_cast<unsigned int>(atoi(tp.c_str())));
1156 
1157  std::vector<biorbd::utils::Vector> activations;
1158  // Scroll down until the definition of a marker
1159  for (unsigned int j=0; j<nbIntervals+1; j++){
1160  while (tp.compare("T")){
1161  bool check = file.read(tp);
1162  biorbd::utils::Error::check(check, "Kin file error, wrong size of number of muscles or intervals?");
1163  }
1164 
1165  double time;
1166  file.read(time);
1167  biorbd::utils::Vector activation_tp(nMus);
1168  for (unsigned int i=0; i<nMus; i++)
1169  file.read(activation_tp(i));
1170 
1171  activations.push_back(activation_tp);
1172  // reinitialize for the next iteration
1173  tp = "";
1174  }
1175 
1176  // Close file
1177  // std::cout << "Activation file successfully loaded" << std::endl;
1178  file.close();
1179  return activations;
1180 }
1181 
1182 std::vector<biorbd::utils::Vector>
1183 biorbd::Reader::readTorqueDataFile(
1184  const utils::Path &path){
1185  // Open file
1186  // std::cout << "Loading kin file: " << path << std::endl;
1187 #ifdef _WIN32
1188  biorbd::utils::IfStream file(
1189  biorbd::utils::Path::toWindowsFormat(
1190  path.absolutePath()).c_str(), std::ios::in);
1191 #else
1192  biorbd::utils::IfStream file(
1193  path.absolutePath().c_str(), std::ios::in);
1194 #endif
1195 
1196  //Read file
1197  biorbd::utils::String tp;
1198 
1199  // Determine the file version
1200  file.readSpecificTag("version", tp);
1201  unsigned int version(static_cast<unsigned int>(atoi(tp.c_str())));
1202  biorbd::utils::Error::check(version == 1, "Version not implemented yet");
1203 
1204  // Determine the number of GeneralizedTorque
1205  file.readSpecificTag("nGeneralizedTorque", tp); //
1206  unsigned int nGeneralizedTorque(static_cast<unsigned int>(atoi(tp.c_str())));
1207 
1208  // Determine the number of nodes
1209  file.readSpecificTag("nbintervals", tp);
1210  unsigned int nbIntervals(static_cast<unsigned int>(atoi(tp.c_str())));
1211 
1212  std::vector<biorbd::utils::Vector> torque;
1213  // Scroll down until the definition of a torque
1214  for (unsigned int j=0; j<nbIntervals+1; j++){
1215  while (tp.compare("T")){
1216  bool check = file.read(tp);
1217  biorbd::utils::Error::check(check, "Kin file error, wrong size of NGeneralizedTorque or intervals?"); //
1218  }
1219 
1220  // Read the first line which represents the timestamp
1221  double time;
1222  file.read(time);
1223 
1224  // Read the vector of GeneralizedTorque associated to the timestamp
1225  biorbd::utils::Vector torque_tp(nGeneralizedTorque);
1226  for (unsigned int i=0; i<nGeneralizedTorque; i++)
1227  file.read(torque_tp(i));
1228 
1229  torque.push_back(torque_tp);
1230  // reinitialise for the next iteration
1231  tp = "";
1232  }
1233 
1234  // Close file
1235  file.close();
1236  return torque;
1237 
1238 }
1239 
1240 std::vector<biorbd::utils::Vector>
1241 biorbd::Reader::readGroundReactionForceDataFile(
1242  const utils::Path &path){
1243  // Open file
1244  // std::cout << "Loading ground reaction force file: " << path << std::endl;
1245 #ifdef _WIN32
1246  biorbd::utils::IfStream file(
1247  biorbd::utils::Path::toWindowsFormat(
1248  path.absolutePath()).c_str(), std::ios::in);
1249 #else
1250  biorbd::utils::IfStream file(
1251  path.absolutePath().c_str(), std::ios::in);
1252 #endif
1253 
1254  // Read file
1255  biorbd::utils::String tp;
1256 
1257  // Determine the file version
1258  file.readSpecificTag("version", tp);
1259  unsigned int version(static_cast<unsigned int>(atoi(tp.c_str())));
1260  biorbd::utils::Error::check(version == 1, "Version not implemented yet");
1261 
1262  // Determine the number of grf (ground reaction force)
1263  file.readSpecificTag("ngrf", tp); //
1264  unsigned int NGRF(static_cast<unsigned int>(atoi(tp.c_str())));
1265 
1266  // Determine the number of nodes
1267  file.readSpecificTag("nbintervals", tp);
1268  unsigned int nbIntervals(static_cast<unsigned int>(atoi(tp.c_str())));
1269 
1270  std::vector<biorbd::utils::Vector> grf;
1271  // Scroll down until the definition of a torque
1272  for (unsigned int j=0; j<nbIntervals+1; j++){
1273  while (tp.compare("T")){
1274  bool check = file.read(tp);
1275  biorbd::utils::Error::check(check, "Grf file error, wrong size of NR or intervals?"); //
1276  }
1277 
1278  // Read the first line that represents the timestamp
1279  double time;
1280  file.read(time);
1281 
1282  // Read the vector of GeneralizedTorque associated to the timestamp
1283  biorbd::utils::Vector grf_tp(NGRF);
1284  for (unsigned int i=0; i<NGRF; i++)
1285  file.read(grf_tp(i));
1286 
1287  grf.push_back(grf_tp);
1288  // reinitialize for the next iteration
1289  tp = "";
1290  }
1291 
1292  // Close file
1293  file.close();
1294  return grf;
1295 
1296 }
1297 
1298 void biorbd::Reader::readViconForceFile(
1299  const biorbd::utils::Path& path, // Path to the file
1300  std::vector<std::vector<unsigned int>>& frame, // Frame vector (time is frame/frequency)
1301  std::vector<unsigned int>& frequency,// Acquisition frequency
1302  std::vector<std::vector<biorbd::utils::Vector3d>>& force, // Linear forces (x,y,z)
1303  std::vector<std::vector<biorbd::utils::Vector3d>>& moment, // Moments (x,y,z)
1304  std::vector<std::vector<biorbd::utils::Vector3d>>& cop) {// Center of pressure (x,y,z) * number of pf
1305  // Open file
1306  // std::cout << "Loading force file: " << path << std::endl;
1307 #ifdef _WIN32
1308  biorbd::utils::IfStream file(
1309  biorbd::utils::Path::toWindowsFormat(
1310  path.absolutePath()).c_str(), std::ios::in);
1311 #else
1312  biorbd::utils::IfStream file(
1313  path.absolutePath().c_str(), std::ios::in);
1314 #endif
1315 
1316  frame.clear();
1317  force.clear();
1318  moment.clear();
1319  cop.clear();
1320 
1321  // Read file
1322  biorbd::utils::String tp;
1323 
1324 
1325  while(!file.eof()){
1326  // Put everything in temporary per platform (Mettre tout en temporaire par plate-forme)
1327  std::vector<unsigned int> frame1pf;
1328  std::vector<biorbd::utils::Vector3d> force1fp;
1329  std::vector<biorbd::utils::Vector3d> moment1fp;
1330  std::vector<biorbd::utils::Vector3d> cop1fp;
1331 
1332  // Get the acquisition frequency
1333  file.readSpecificTag("devices", tp);
1334  unsigned int frequency1pf(static_cast<unsigned int>(atoi(tp.c_str())));
1335 
1336  // Skip the header
1337  for (unsigned int i=0; i<4; ++i)
1338  file.getline(tp);
1339 
1340  // Transcribe the values
1341  while(!file.eof()){ // return false if it fails
1342  file.getline(tp); // take the entire line
1343  // If line is empty, it's because we are done with the forces on this platform
1344  if (!tp.compare("")){
1345  break;
1346  }
1347 
1348  // Otherwise, we save the line
1349  // now we'll use a stringstream to separate the fields out of the line (comma separated)
1350  std::stringstream ss( tp );
1351  biorbd::utils::String field;
1352  std::vector<double> data;
1353  data.clear();
1354  while (getline( ss, field, ',' )){
1355  // for each field we wish to convert it to a double
1356  // (since we require that the CSV contains nothing but floating-point values)
1357  std::stringstream fs( field );
1358  double f = 0.0; // (default value is 0.0)
1359  fs >> f;
1360 
1361  // add the newly-converted field to the end of the record
1362  data.push_back( f );
1363  }
1364  // Make sure that the line has the right number of elements(2 times, 3 cop, 3 forces, 3 moments)
1365  biorbd::utils::Error::check(data.size()==11, "Wrong number of element in a line in the force file");
1366 
1367  // Fill in the fields
1368  frame1pf.push_back(static_cast<unsigned int>(data[0])); // Frame stamp
1369  // unsigned int subFrame = static_cast<unsigned int>(data[1]); // Subframes (not interesting)
1370  biorbd::utils::Vector3d cop_tp(0, 0, 0); // Center of pressure
1371  biorbd::utils::Vector3d for_tp(0, 0, 0); // Force
1372  biorbd::utils::Vector3d M_tp(0, 0, 0); // Moment
1373  for (unsigned int i=0; i<3; ++i){
1374  cop_tp[i] = data[i+2]/1000; // from mm to m
1375  for_tp[i] = data[i+5];
1376  M_tp[i] = data[i+8]/1000; // from Nmm to Nm
1377  }
1378  cop1fp.push_back(cop_tp);
1379  force1fp.push_back(for_tp);
1380  moment1fp.push_back(M_tp);
1381  }
1382  // Push back of the values
1383  frame.push_back(frame1pf);
1384  frequency.push_back(frequency1pf);
1385  force.push_back(force1fp);
1386  moment.push_back(moment1fp);
1387  cop.push_back(cop1fp);
1388  }
1389 }
1390 
1391 std::vector<std::vector<biorbd::utils::SpatialVector>>
1392 biorbd::Reader::readViconForceFile(const biorbd::utils::String &path){
1393  // Read file
1394  std::vector<std::vector<unsigned int>> frame;
1395  std::vector<unsigned int> frequency;// Acquisition frequency
1396  std::vector<std::vector<biorbd::utils::Vector3d>> force; // Linear forces (x,y,z)
1397  std::vector<std::vector<biorbd::utils::Vector3d>> moment; // Moments (x,y,z)
1398  std::vector<std::vector<biorbd::utils::Vector3d>> cop; // Center of pressure (x,y,z)
1399  readViconForceFile(path, frame, frequency, force, moment, cop);
1400 
1401  // Redispatch the values in a vector of SpatialTransform
1402  std::vector<std::vector<biorbd::utils::SpatialVector>> st; // nb of platform per time
1403  for (unsigned int j=0; j<force.size(); ++j){// nb platform
1404  std::vector<biorbd::utils::SpatialVector> tp;
1405  for (unsigned int i=0; i<force[j].size(); ++i){ // timestamp
1406  const biorbd::utils::Vector3d& f = force[j][i]; // Linear forces (x,y,z)
1407  const biorbd::utils::Vector3d& m = moment[j][i]; // Moments (x,y,z)
1408  tp.push_back(biorbd::utils::SpatialVector(m[0], m[1], m[2], f[0], f[1], f[2]));
1409  }
1410  st.push_back(tp);
1411  }
1412  return st;
1413 }
1414 
1415 std::vector<std::vector<biorbd::utils::Vector3d>>
1416 biorbd::Reader::readViconMarkerFile(const biorbd::utils::Path& path,
1417  std::vector<biorbd::utils::String>& markOrder,
1418  int nFramesToGet) {
1419  // Read file
1420 #ifdef _WIN32
1421  biorbd::utils::IfStream file(
1422  biorbd::utils::Path::toWindowsFormat(
1423  path.absolutePath()).c_str(), std::ios::in);
1424 #else
1425  biorbd::utils::IfStream file(
1426  path.absolutePath().c_str(), std::ios::in);
1427 #endif
1428  biorbd::utils::String t;
1429 
1430 
1431  // Get the acquisition frequency
1432  // frequency = atoi(findImportantParameter(file, "trajectories").c_str());
1433 
1434  // Get the order of the markers in the file
1435  for (unsigned int i=0; i<3; ++i) // skip the header
1436  file.read(t);
1437  size_t idx_tp = 0;
1438  std::vector<unsigned int> idx_init;
1439  std::vector<unsigned int> idx_end;
1440  // Find the separators (: et ,)
1441  while (idx_tp < t.length()) {
1442  idx_tp = t.find(":", idx_tp+1);
1443  idx_init.push_back(static_cast<unsigned int>(idx_tp));
1444  idx_end.push_back(static_cast<unsigned int>(t.find(",", idx_tp+1)));
1445  }
1446  // Keep the names between the separators
1447  std::vector<biorbd::utils::String> MarkersInFile;
1448  for (unsigned int i=0; i<idx_init.size()-1; ++i){
1449  biorbd::utils::String tp;
1450  for (unsigned int j=*(idx_init.begin()+i)+1; j<*(idx_end.begin()+i); ++j)
1451  tp.push_back(t.at(j));
1452  MarkersInFile.push_back(tp);
1453  }
1454 
1455 
1456  // Compare with the given order
1457  int *ordre;
1458  ordre = new int[3*MarkersInFile.size()];
1459  for (int i=0; i<static_cast<int>(3*MarkersInFile.size()); ++i)
1460  ordre[i] = -1;
1461  for (int i=0; i<static_cast<int>(markOrder.size()); ++i){
1462  unsigned int cmp=0;
1463  biorbd::utils::String m1 = (*(markOrder.begin()+i));
1464  while (1){
1465  biorbd::utils::String m2 = (*(MarkersInFile.begin()+cmp));
1466  if (!m1.compare(m2)){
1467  ordre[3*cmp] = 3*i;
1468  ordre[3*cmp+1] = 3*i+1;
1469  ordre[3*cmp+2] = 3*i+2;
1470  break;
1471  }
1472  else
1473  ++cmp;
1474 // biorbd::utils::Error::check(cmp<MarkersInFile.size(), "The requested marker was not found in the file!");
1475  // Le marqueur demandé n'a pas été trouvé dans le fichier!
1476  if (cmp>=MarkersInFile.size())
1477  break;
1478  }
1479  }
1480 
1481  // Go to the data
1482  for (unsigned int i=0; i<4; ++i) // skip the header
1483  file.read(t);
1484 
1485  // Find the total number of frames
1486  unsigned int jumps(1);
1487  unsigned int nbFrames(0);
1488  if (nFramesToGet != -1){ // If it's all of them, the jumps are 1
1489  while(!file.eof()){
1490  file.read(t); // Get a line
1491  nbFrames++;
1492  }
1493  file.close();
1494 #ifdef _WIN32
1495  biorbd::utils::IfStream file(
1496  biorbd::utils::Path::toWindowsFormat(
1497  path.absolutePath()).c_str(), std::ios::in);
1498 #else
1499  biorbd::utils::IfStream file(
1500  path.absolutePath().c_str(), std::ios::in);
1501 #endif
1502  // Skip the header
1503  for (unsigned int i=0; i<7; ++i)
1504  file.read(t);
1505  biorbd::utils::Error::check(nFramesToGet!=0 && nFramesToGet!=1
1506  && static_cast<unsigned int>(nFramesToGet)<=nbFrames,
1507  "nNode should not be 0, 1 or greater "
1508  "than number of frame");
1509  jumps = nbFrames/static_cast<unsigned int>(nFramesToGet)+1;
1510  }
1511 
1512 
1513  std::vector<std::vector<biorbd::utils::Vector3d>> data;
1514  // now we'll use a stringstream to separate the fields out of the line (comma separated)
1515  unsigned int cmpFrames(1);
1516  while(!file.eof()){
1517  biorbd::utils::Vector data_tp = biorbd::utils::Vector(
1518  static_cast<unsigned int>(3*markOrder.size()));
1519  data_tp.setZero();
1520 
1521  std::stringstream ss( t );
1522  biorbd::utils::String field;
1523  unsigned int cmp = 0;
1524  while (getline( ss, field, ',' )){
1525  // for each field we wish to convert it to a double
1526  // (since we require that the CSV contains nothing but floating-point values)
1527  std::stringstream fs( field );
1528  double f = 0.0; // (default value is 0.0)
1529  fs >> f;
1530  if (cmp>1 && cmp<3*MarkersInFile.size()+2){ // Retirer les timespans et ne pas dépasser
1531  int idx = ordre[cmp-2];
1532  if (idx>=0)
1533  data_tp(static_cast<unsigned int>(idx)) = f;
1534  }
1535  ++cmp;
1536  }
1537  // Once the markers are in order, separate them
1538  std::vector<biorbd::utils::Vector3d> data_tp2;
1539  for (unsigned int i=0; i<static_cast<unsigned int>(data_tp.size())/3; ++i){
1540  biorbd::utils::Vector3d node(data_tp.block(3*i, 0, 3, 1)/1000);
1541  data_tp2.push_back(node);
1542  }
1543  // Stock the marker vector a that time t
1544  data.push_back(data_tp2); // Put back to meters
1545  for (unsigned int i=0; i<jumps; ++i){
1546  if (cmpFrames != nbFrames){
1547  file.read(t); // Get the line
1548  cmpFrames++;
1549  }
1550  else{
1551  file.read(t);
1552  break;
1553  }
1554  }
1555  }
1556 
1557  // Close file
1558  file.close();
1559 
1560  return data;
1561 }
1562 
1563 biorbd::rigidbody::Mesh
1564 biorbd::Reader::readMeshFileBiorbdSegments(
1565  const biorbd::utils::Path &path)
1566 {
1567  // Read a segment file
1568 
1569  // Open file
1570  // std::cout << "Loading marker file: " << path << std::endl;
1571 #ifdef _WIN32
1572  biorbd::utils::IfStream file(
1573  biorbd::utils::Path::toWindowsFormat(
1574  path.absolutePath()).c_str(), std::ios::in);
1575 #else
1576  biorbd::utils::IfStream file(
1577  path.absolutePath().c_str(), std::ios::in);
1578 #endif
1579 
1580  // Read file
1581  biorbd::utils::String tp;
1582 
1583  // Determine the file version
1584  file.readSpecificTag("version", tp);
1585  unsigned int version(static_cast<unsigned int>(atoi(tp.c_str())));
1586  biorbd::utils::Error::check(version == 1 || version == 2, "Version not implemented yet");
1587 
1588  // Know the number of points
1589  file.readSpecificTag("npoints", tp);
1590  unsigned int nPoints(static_cast<unsigned int>(atoi(tp.c_str())));
1591  file.readSpecificTag("nfaces", tp);
1592  unsigned int nFaces(static_cast<unsigned int>(atoi(tp.c_str())));
1593 
1594  biorbd::rigidbody::Mesh mesh;
1595  mesh.setPath(path);
1596  // Get all the points
1597  for (unsigned int iPoints=0; iPoints < nPoints; ++iPoints){
1598  biorbd::utils::Vector3d nodeTp(0, 0, 0);
1599  for (unsigned int i=0; i<3; ++i)
1600  file.read(nodeTp(i));
1601  mesh.addPoint(nodeTp);
1602  if (version == 2)
1603  for (unsigned int i=0; i<3; ++i){
1604  double dump; // Ignore columns 4 5 6
1605  file.read(dump);
1606  }
1607  }
1608 
1609  for (unsigned int iPoints=0; iPoints < nFaces; ++iPoints){
1610  biorbd::rigidbody::MeshFace patchTp;
1611  unsigned int nVertices;
1612  file.read(nVertices);
1613  if (nVertices != 3)
1614  biorbd::utils::Error::raise("Patches must be 3 vertices!");
1615  for (unsigned int i=0; i<nVertices; ++i)
1616  file.read(patchTp(i));
1617  mesh.addFace(patchTp);
1618  }
1619  return mesh;
1620 }
1621 
1622 
1623 biorbd::rigidbody::Mesh biorbd::Reader::readMeshFilePly(
1624  const biorbd::utils::Path &path)
1625 {
1626  // Read a bone file
1627 
1628  // Open file
1629  // std::cout << "Loading marker file: " << path << std::endl;
1630 #ifdef _WIN32
1631  biorbd::utils::IfStream file(
1632  biorbd::utils::Path::toWindowsFormat(
1633  path.absolutePath()).c_str(), std::ios::in);
1634 #else
1635  biorbd::utils::IfStream file(
1636  path.absolutePath().c_str(), std::ios::in);
1637 #endif
1638 
1639  // Read file
1640  biorbd::utils::String tp;
1641 
1642  // Know the number of points
1643  file.reachSpecificTag("element");
1644  file.readSpecificTag("vertex", tp);
1645  unsigned int nVertex(static_cast<unsigned int>(atoi(tp.c_str())));
1646  int nVertexProperties(file.countTagsInAConsecutiveLines("property"));
1647 
1648  // Find the number of columns for the vertex
1649  file.reachSpecificTag("element");
1650  file.readSpecificTag("face", tp);
1651  unsigned int nFaces(static_cast<unsigned int>(atoi(tp.c_str())));
1652  int nFacesProperties(file.countTagsInAConsecutiveLines("property"));
1653 
1654 
1655  // Trouver le nombre de ??
1656  file.reachSpecificTag("end_header");
1657 
1658  biorbd::rigidbody::Mesh mesh;
1659  mesh.setPath(path);
1660  // Get all the points
1661  for (unsigned int iPoints=0; iPoints < nVertex; ++iPoints){
1662  biorbd::utils::Vector3d nodeTp(0, 0, 0);
1663  for (unsigned int i=0; i<3; ++i)
1664  file.read(nodeTp(i));
1665  mesh.addPoint(nodeTp);
1666  // Ignore the columns post XYZ
1667  for (int i=0; i<nVertexProperties-3; ++i){
1668  double dump;
1669  file.read(dump);
1670  }
1671  }
1672 
1673  for (unsigned int iPoints=0; iPoints < nFaces; ++iPoints){
1674  biorbd::rigidbody::MeshFace patchTp;
1675  unsigned int nVertices;
1676  file.read(nVertices);
1677  if (nVertices != 3)
1678  biorbd::utils::Error::raise("Patches must be 3 vertices!");
1679  for (unsigned int i=0; i<nVertices; ++i)
1680  file.read(patchTp(i));
1681  int dump;
1682  // Remove if there are too many columns
1683  for (int i=0; i<nFacesProperties-1; ++i)
1684  file.read(dump);
1685  mesh.addFace(patchTp);
1686  }
1687  return mesh;
1688 }
1689 
1690 biorbd::rigidbody::Mesh biorbd::Reader::readMeshFileObj(
1691  const biorbd::utils::Path &path)
1692 {
1693  // Read a bone file
1694 
1695  // Open file
1696  // std::cout << "Loading marker file: " << path << std::endl;
1697 #ifdef _WIN32
1698  biorbd::utils::IfStream file(
1699  biorbd::utils::Path::toWindowsFormat(
1700  path.absolutePath()).c_str(), std::ios::in);
1701 #else
1702  biorbd::utils::IfStream file(
1703  path.absolutePath().c_str(), std::ios::in);
1704 #endif
1705 
1706  // Read file
1707  biorbd::utils::String tp;
1708 
1709  biorbd::rigidbody::Mesh mesh;
1710  mesh.setPath(path);
1711 
1712  // Get all the points
1713  biorbd::utils::Vector3d vertex;
1714  biorbd::rigidbody::MeshFace patch;
1715  biorbd::utils::String text;
1716  while (true) {
1717  // If we get to the end of file, exit loop
1718  if (file.eof())
1719  break;
1720 
1721  // Read the first element of the line
1722  file.read(text);
1723 
1724  if (!text.compare("v")) {
1725  // If first element is a v, then a vertex is found
1726  for (unsigned int i=0; i<3; ++i)
1727  file.read(vertex(i));
1728  mesh.addPoint(vertex);
1729  }
1730  else if (!text.compare("f")) {
1731  // If first element is a f, then a face is found
1732  // Face is ignore for now
1733  for (unsigned int i=0; i<3; ++i) {
1734  file.read(text);
1735  size_t idxSlash = text.find("/");
1736  biorbd::utils::String tata3(text.substr (0,idxSlash));
1737  patch(i) = (std::stoi(text.substr (0,idxSlash)) - 1);
1738  }
1739  file.getline(text); // Ignore last element if it is a 4 vertex based
1740  mesh.addFace(patch.DeepCopy());
1741  }
1742  else {
1743  // Ignore the line
1744  file.getline(text);
1745  }
1746 
1747  }
1748 
1749  return mesh;
1750 }
1751 
1752 #ifdef MODULE_VTP_FILES_READER
1753 #include "tinyxml.h"
1754 biorbd::rigidbody::Mesh biorbd::Reader::readMeshFileVtp(
1755  const biorbd::utils::Path &path) {
1756  // Read an opensim formatted mesh file
1757 
1758  // Read the file
1759 #ifdef _WIN32
1760  biorbd::utils::String filepath( biorbd::utils::Path::toWindowsFormat(
1761  path.absolutePath()).c_str());
1762 #else
1763  biorbd::utils::String filepath( path.absolutePath().c_str() );
1764 #endif
1765 
1766  TiXmlDocument doc(filepath);
1767  biorbd::utils::Error::check(doc.LoadFile(), "Failed to load file " + filepath);
1768  TiXmlHandle hDoc(&doc);
1769  biorbd::rigidbody::Mesh mesh;
1770  mesh.setPath(path);
1771 
1772  // Navigate up to VTKFile/PolyData/Piece
1773  TiXmlHandle polydata =
1774  hDoc.ChildElement("VTKFile", 0)
1775  .ChildElement("PolyData", 0)
1776  .ChildElement("Piece", 0);
1777  int numberOfPoints, NumberOfPolys;
1778  polydata.ToElement()->QueryIntAttribute("NumberOfPoints", &numberOfPoints);
1779  polydata.ToElement()->QueryIntAttribute("NumberOfPolys", &NumberOfPolys);
1780 
1781  biorbd::utils::String field;
1782 
1783  // Get the points
1784  {
1785  biorbd::utils::String points(
1786  polydata.ChildElement("Points", 0)
1787  .ChildElement("DataArray", 0).Element()->GetText());
1788  std::stringstream ss( points );
1789  for (int i = 0; i < numberOfPoints; ++i){
1790  double x, y, z;
1791  {
1792  getline( ss, field, ' ' );
1793  std::stringstream fs( field );
1794  fs >> x;
1795  }
1796  {
1797  getline( ss, field, ' ' );
1798  std::stringstream fs( field );
1799  fs >> y;
1800  }
1801  {
1802  getline( ss, field, ' ' );
1803  std::stringstream fs( field );
1804  fs >> z;
1805  }
1806  mesh.addPoint(biorbd::utils::Vector3d(x, y, z));
1807  }
1808  }
1809 
1810  // Get the patches
1811  {
1812  biorbd::utils::String polys(
1813  polydata.ChildElement("Polys", 0)
1814  .ChildElement("DataArray", 0).Element()->GetText());
1815  std::stringstream ss( polys );
1816  for (int i = 0; i < NumberOfPolys; ++i){
1817  int vertex1, vertex2, vertex3;
1818  {
1819  getline( ss, field, ' ' );
1820  std::stringstream fs( field );
1821  fs >> vertex1;
1822  }
1823  {
1824  getline( ss, field, ' ' );
1825  std::stringstream fs( field );
1826  fs >> vertex2;
1827  }
1828  {
1829  getline( ss, field, ' ' );
1830  std::stringstream fs( field );
1831  fs >> vertex3;
1832  }
1833  mesh.addFace({vertex1, vertex2, vertex3});
1834  }
1835  }
1836 
1837  return mesh;
1838 }
1839 #endif // MODULE_VTP_FILES_READER
1840 
1841 
1842 std::vector<std::vector<biorbd::utils::Vector3d>>
1843 biorbd::Reader::readViconMarkerFile(const biorbd::utils::Path &path,
1844  int nFramesToGet){
1845  // Read file
1846 #ifdef _WIN32
1847  biorbd::utils::IfStream file(
1848  biorbd::utils::Path::toWindowsFormat(
1849  path.absolutePath()).c_str(), std::ios::in);
1850 #else
1851  biorbd::utils::IfStream file(
1852  path.absolutePath().c_str(), std::ios::in);
1853 #endif
1854  biorbd::utils::String t;
1855 
1856 
1857  // Get the acquisition frequency
1858  // frequency = atoi(findImportantParameter(file, "trajectories").c_str());
1859 
1860  // Get the order of the markers in the file
1861  for (unsigned int i=0; i<3; ++i) // skip the header
1862  file.read(t);
1863  size_t idx_tp = 0;
1864  std::vector<unsigned int> idx_init;
1865  std::vector<unsigned int> idx_end;
1866  // Find the separators (: et ,)
1867  while (idx_tp < t.length()) {
1868  idx_tp = t.find(":", idx_tp+1);
1869  idx_init.push_back(static_cast<unsigned int>(idx_tp));
1870  idx_end.push_back(static_cast<unsigned int>(t.find(",", idx_tp+1)));
1871  }
1872  // Keep the names between the separators
1873  std::vector<biorbd::utils::String> MarkersInFile;
1874  for (unsigned int i=0; i<idx_init.size()-1; ++i){
1875  biorbd::utils::String tp;
1876  for (unsigned int j=*(idx_init.begin()+i)+1; j<*(idx_end.begin()+i); ++j)
1877  tp.push_back(t.at(j));
1878  MarkersInFile.push_back(tp);
1879  }
1880 
1881  // Close file
1882  file.close();
1883 
1884  return readViconMarkerFile(path, MarkersInFile, nFramesToGet);
1885 }
biorbd::rigidbody::Contacts::AddLoopConstraint
unsigned int AddLoopConstraint(unsigned int body_id_predecessor, unsigned int body_id_successor, const biorbd::utils::RotoTrans &X_predecessor, const biorbd::utils::RotoTrans &X_successor, const biorbd::utils::SpatialVector &axis, const biorbd::utils::String &name, bool enableStabilization=false, double stabilizationParam=0.1)
Add a loop constraint to the constraint set.
Definition: Contacts.cpp:71
biorbd::Reader::readViconForceFile
static void readViconForceFile(const biorbd::utils::Path &path, std::vector< std::vector< unsigned int >> &frame, std::vector< unsigned int > &frequency, std::vector< std::vector< biorbd::utils::Vector3d >> &force, std::vector< std::vector< biorbd::utils::Vector3d >> &moment, std::vector< std::vector< biorbd::utils::Vector3d >> &cop)
Read a Vicon ASCII force file.
Definition: ModelReader.cpp:1298
biorbd::rigidbody::Joints::getDofIndex
unsigned int getDofIndex(const biorbd::utils::String &SegmentName, const biorbd::utils::String &dofName)
Return the index of a DoF in a segment.
Definition: Joints.cpp:1037
biorbd::Reader::readGroundReactionForceDataFile
static std::vector< biorbd::utils::Vector > readGroundReactionForceDataFile(const biorbd::utils::Path &path)
Read a bioGRF file containing ground reaction force (GRF) data.
Definition: ModelReader.cpp:1241
biorbd::utils::Path::toWindowsFormat
static biorbd::utils::String toWindowsFormat(const biorbd::utils::String &path)
Return the path to Windows format.
Definition: Path.cpp:324
biorbd::utils::Range
Class Range.
Definition: Range.h:15
biorbd::utils::SpatialVector
Wrapper of the Eigen::Matrix<double, 6, 1> or Casadi::MX(6, 1)
Definition: SpatialVector.h:19
biorbd::muscles::WrappingCylinder
Cylinder object that makes the muscle to wrap around.
Definition: WrappingCylinder.h:13
biorbd::Reader::readActivationDataFile
static std::vector< biorbd::utils::Vector > readActivationDataFile(const biorbd::utils::Path &path)
Read a bioMus fine, containing muscle activations data.
Definition: ModelReader.cpp:1128
biorbd::utils::Vector
Wrapper of the Eigen VectorXd.
Definition: Vector.h:20
biorbd::actuator::Actuator
Class Actuator.
Definition: Actuator.h:20
biorbd::muscles::State
EMG holder to interact with the muscle.
Definition: State.h:16
biorbd::utils::Vector3d
Wrapper around Eigen Vector3d and attach it to a parent.
Definition: Vector3d.h:24
biorbd::rigidbody::Joints::AddSegment
unsigned int AddSegment(const biorbd::utils::String &segmentName, const biorbd::utils::String &parentName, const biorbd::utils::String &translationSequence, const biorbd::utils::String &rotationSequence, const std::vector< biorbd::utils::Range > &QRanges, const std::vector< biorbd::utils::Range > &QDotRanges, const std::vector< biorbd::utils::Range > &QDDotRanges, const biorbd::rigidbody::SegmentCharacteristics &characteristics, const RigidBodyDynamics::Math::SpatialTransform &centreOfRotation, int forcePlates=-1)
Add a segment to the model.
Definition: Joints.cpp:125
biorbd::utils::Path::absolutePath
biorbd::utils::String absolutePath() const
Return the absolute path relative to root.
Definition: Path.cpp:290
biorbd::muscles::FatigueParameters::setDevelopFactor
void setDevelopFactor(const biorbd::utils::Scalar &developFactor)
Set the develop factor.
Definition: FatigueParameters.cpp:52
biorbd::actuator::ActuatorConstant
Class ActuatorConstant is a joint actuator type which maximum is contant.
Definition: ActuatorConstant.h:14
biorbd::rigidbody::MeshFace::DeepCopy
biorbd::rigidbody::MeshFace DeepCopy() const
Deep copy of a MeshFace.
Definition: MeshFace.cpp:12
biorbd::rigidbody::SegmentCharacteristics
Characteristics of a segment, namely the mass, the center of mass, the inertia, the length and its me...
Definition: SegmentCharacteristics.h:27
biorbd::rigidbody::MeshFace
The face of the mesh.
Definition: MeshFace.h:19
biorbd::utils::Node::setParent
void setParent(const biorbd::utils::String &name)
Set the parent name of the node.
Definition: Node.cpp:69
biorbd::utils::Path::folder
const biorbd::utils::String & folder() const
Return the folder of the file.
Definition: Path.cpp:341
biorbd::rigidbody::GeneralizedCoordinates
Class GeneralizedCoordinates.
Definition: GeneralizedCoordinates.h:15
biorbd::Reader::readMeshFileObj
static biorbd::rigidbody::Mesh readMeshFileObj(const biorbd::utils::Path &path)
Read a OBJ file containing the meshing of a segment.
Definition: ModelReader.cpp:1690
biorbd::muscles::ViaPoint
Via point of a muscle.
Definition: ViaPoint.h:16
biorbd::utils::IfStream::getline
void getline(biorbd::utils::String &text)
Read a whole line.
Definition: IfStream.cpp:179
biorbd::utils::IfStream::read
bool read(biorbd::utils::String &text)
Read a word in the file skipping the word if it is c-like commented.
Definition: IfStream.cpp:94
biorbd::muscles::Geometry
Class Geometry of the muscle.
Definition: Geometry.h:30
biorbd::utils::RotoTransNode
A RotoTrans which is attached to a segment.
Definition: RotoTransNode.h:16
biorbd::utils::Path::relativePath
biorbd::utils::String relativePath() const
Return relative path to current working directory.
Definition: Path.cpp:190
biorbd::actuator::ActuatorGauss3p
Class ActuatorGauss3p is a joint actuator type which maximum is 3 parameter gaussian (Gauss3p)
Definition: ActuatorGauss3p.h:20
biorbd::rigidbody::Markers::addMarker
void addMarker(const biorbd::rigidbody::NodeSegment &pos, const biorbd::utils::String &name, const biorbd::utils::String &parentName, bool technical, bool anatomical, const biorbd::utils::String &axesToRemove, int id=-1)
Add a marker to the set.
Definition: Markers.cpp:47
biorbd::utils::IfStream::close
bool close()
Close the file.
Definition: IfStream.cpp:185
biorbd::utils::Error::raise
static void raise(const biorbd::utils::String &message)
Throw an error message.
Definition: Error.cpp:4
biorbd::rigidbody::Mesh
A class that holds the geometry of a segment.
Definition: Mesh.h:21
biorbd::utils::RotoTrans::fromMarkers
static biorbd::utils::RotoTrans fromMarkers(const biorbd::rigidbody::NodeSegment &origin, const std::pair< biorbd::rigidbody::NodeSegment, biorbd::rigidbody::NodeSegment > &axis1markers, const std::pair< biorbd::rigidbody::NodeSegment, biorbd::rigidbody::NodeSegment > &axis2markers, const std::pair< biorbd::utils::String, biorbd::utils::String > &axesNames, const biorbd::utils::String &axisToRecalculate)
fromMarkers Creates a system of axes from two axes and an origin defined by markers
Definition: RotoTrans.cpp:63
biorbd::rigidbody::Mesh::addPoint
void addPoint(const biorbd::utils::Vector3d &node)
Add a point to the mesh.
Definition: Mesh.cpp:51
biorbd::utils::RotoTrans::trans
biorbd::utils::Vector3d trans() const
Return the translation vector.
Definition: RotoTrans.cpp:92
biorbd::Model
The actual musculoskeletal model that holds everything in biorbd.
Definition: BiorbdModel.h:78
biorbd::utils::RotoTrans
Homogenous matrix to describe translations and rotations simultaneously.
Definition: RotoTrans.h:34
biorbd::muscles::FatigueParameters::setFatigueRate
void setFatigueRate(const biorbd::utils::Scalar &fatigueRate)
Set the fatigue rate.
Definition: FatigueParameters.cpp:32
biorbd::utils::String::tolower
static biorbd::utils::String tolower(const biorbd::utils::String &str)
Convert a string to a lower case string.
Definition: String.cpp:97
biorbd::utils::IfStream::eof
bool eof()
Return if the file is at the end.
Definition: IfStream.cpp:190
biorbd::muscles::FatigueParameters
Class FatigueParameters that holds the muscle fatigue parameters.
Definition: FatigueParameters.h:16
biorbd::Reader::readTorqueDataFile
static std::vector< biorbd::utils::Vector > readTorqueDataFile(const biorbd::utils::Path &path)
Read a bioTorque file containing generalized torques data.
Definition: ModelReader.cpp:1183
biorbd::muscles::FatigueParameters::setRecoveryRate
void setRecoveryRate(const biorbd::utils::Scalar &recoveryRate)
Set the recovery rate.
Definition: FatigueParameters.cpp:42
biorbd::muscles::PathModifiers
Holder of all the path modifiers of a muscle.
Definition: PathModifiers.h:18
biorbd::utils::Path::filename
const biorbd::utils::String & filename() const
Return the filename.
Definition: Path.cpp:352
biorbd::utils::IfStream::readSpecificTag
bool readSpecificTag(const biorbd::utils::String &tag, biorbd::utils::String &text)
Advance in the file to a specific tag.
Definition: IfStream.cpp:51
biorbd::utils::IfStream::countTagsInAConsecutiveLines
int countTagsInAConsecutiveLines(const biorbd::utils::String &tag)
Counts the number of consecutive lines starting with the same tag and then brings it back to the init...
Definition: IfStream.cpp:72
biorbd::Reader::readMarkerDataFile
static std::vector< std::vector< biorbd::utils::Vector3d > > readMarkerDataFile(const utils::Path &path)
Read a bioMark file, containing markers data.
Definition: ModelReader.cpp:1014
biorbd::Reader::readMeshFileBiorbdSegments
static biorbd::rigidbody::Mesh readMeshFileBiorbdSegments(const biorbd::utils::Path &path)
Read a bioMesh file containing the meshing of a segment.
Definition: ModelReader.cpp:1564
biorbd::utils::Path::extension
const biorbd::utils::String & extension() const
Return the extension of the file.
Definition: Path.cpp:363
biorbd::rigidbody::Markers::marker
const biorbd::rigidbody::NodeSegment & marker(unsigned int idx) const
Return the marker of index idx.
Definition: Markers.cpp:60
biorbd::Reader::readMeshFilePly
static biorbd::rigidbody::Mesh readMeshFilePly(const biorbd::utils::Path &path)
Read a PLY file containing the meshing of a segment.
Definition: ModelReader.cpp:1623
biorbd::rigidbody::RotoTransNodes::addRT
void addRT()
Add a new RT to the set.
Definition: RotoTransNodes.cpp:44
biorbd::utils::Rotation
Rotation matrix.
Definition: Rotation.h:34
biorbd::actuator::ActuatorGauss6p
Class ActuatorGauss6p is a joint actuator type which maximum is bimodal 6 parameter gaussian (Gauss6p...
Definition: ActuatorGauss6p.h:18
biorbd::utils::String
Wrapper around the std::string class with augmented functionality.
Definition: String.h:17
biorbd::actuator::ActuatorLinear
Class ActuatorLinear is a joint actuator type that linearly evolves.
Definition: ActuatorLinear.h:18
biorbd::Reader::readQDataFile
static std::vector< biorbd::rigidbody::GeneralizedCoordinates > readQDataFile(const biorbd::utils::Path &path)
Read a bioKin file, containing kinematics data.
Definition: ModelReader.cpp:1073
biorbd::rigidbody::IMUs::addIMU
void addIMU(bool technical=true, bool anatomical=true)
Add a new inertial measurement unit to the set.
Definition: IMUs.cpp:44
biorbd::rigidbody::Contacts::AddConstraint
unsigned int AddConstraint(unsigned int body_id, const biorbd::utils::Vector3d &body_point, const biorbd::utils::Vector3d &world_normal, const biorbd::utils::String &name, double acc=0)
Add a constraint to the constraint set.
Definition: Contacts.cpp:37
biorbd::utils::Path::isFileReadable
bool isFileReadable() const
Test if file exist on the computer and is accessible.
Definition: Path.cpp:108
biorbd::rigidbody::Mesh::setPath
void setPath(const biorbd::utils::Path &path)
Set the path of the underlying mesh file.
Definition: Mesh.cpp:86
biorbd::Reader::readViconMarkerFile
static std::vector< std::vector< biorbd::utils::Vector3d > > readViconMarkerFile(const biorbd::utils::Path &path, int nFramesToGet=-1)
Read a Vicon ASCII marker file (CSV formated)
Definition: ModelReader.cpp:1843
biorbd::rigidbody::Mesh::addFace
void addFace(const biorbd::rigidbody::MeshFace &face)
Add a face patch to the mesh.
Definition: Mesh.cpp:68
biorbd::rigidbody::NodeSegment
A point attached to a segment, generally speaking a skin marker.
Definition: NodeSegment.h:19
biorbd::muscles::Characteristics
Class Holds that muscle characteristics.
Definition: Characteristics.h:17
biorbd::utils::Path
Collection of methods to manipulate path.
Definition: Path.h:17
biorbd::muscles::FatigueParameters::setRecoveryFactor
void setRecoveryFactor(const biorbd::utils::Scalar &recoveryFactor)
Set the recovery factor.
Definition: FatigueParameters.cpp:62
biorbd::Reader::readModelFile
static biorbd::Model readModelFile(const biorbd::utils::Path &path)
Create a biorbd model from a bioMod file.
Definition: ModelReader.cpp:44
biorbd::utils::IfStream::reachSpecificTag
bool reachSpecificTag(const biorbd::utils::String &tag)
Advance in the file to a specific tag.
Definition: IfStream.cpp:58
biorbd::utils::Error::check
static void check(bool cond, const biorbd::utils::String &message)
Assert that raises the error message if false.
Definition: Error.cpp:10
biorbd::utils::IfStream
Wrapper for the an std::ifstream with increased capacities.
Definition: IfStream.h:17
biorbd::utils::RotoTrans::rot
biorbd::utils::Rotation rot() const
Return the rotation matrix.
Definition: RotoTrans.cpp:97
biorbd::utils::Node::setName
void setName(const biorbd::utils::String &name)
Set the name of the node.
Definition: Node.cpp:54
Generated by   doxygen 1.8.18