Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
vpServoData.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Save data during the task execution.
33  *
34  * Authors:
35  * Eric Marchand
36  *
37  *****************************************************************************/
38 
44 // Servo
45 #include <visp3/vs/vpServo.h>
46 
47 #include <visp3/core/vpIoException.h>
48 #include <visp3/core/vpIoTools.h>
49 #include <visp3/vs/vpServoData.h>
50 
51 void vpServoData::open(const char *directory)
52 {
53  try {
54  if (vpIoTools::checkDirectory(directory) == false)
55  vpIoTools::makeDirectory(directory);
56 
57  char s[FILENAME_MAX];
58 
59  sprintf(s, "%s/vel.dat", directory);
60  velocityFile.open(s);
61  sprintf(s, "%s/error.dat", directory);
62  errorFile.open(s);
63  sprintf(s, "%s/errornorm.dat", directory);
64  errorNormFile.open(s);
65  sprintf(s, "%s/s.dat", directory);
66  sFile.open(s);
67  sprintf(s, "%s/sStar.dat", directory);
68  sStarFile.open(s);
69 
70  } catch (...) {
71  vpERROR_TRACE("Error caught");
72  throw;
73  }
74 }
75 
76 void vpServoData::setCmDeg() { cmDeg = true; }
77 void vpServoData::setMeterRad() { cmDeg = false; }
78 void vpServoData::save(const vpServo &task)
79 {
80  if (cmDeg == false)
81  velocityFile << task.q_dot.t();
82  else {
83  for (unsigned int i = 0; i < 3; i++)
84  velocityFile << task.q_dot[i] * 100 << " ";
85  for (unsigned int i = 4; i < 6; i++)
86  velocityFile << vpMath::deg(task.q_dot[i]) << " ";
87  velocityFile << std::endl;
88  }
89  errorFile << (task.getError()).t();
90  errorNormFile << (task.getError()).sumSquare() << std::endl;
91  vNormFile << task.q_dot.sumSquare() << std::endl;
92 
93  sFile << task.s.t();
94  sStarFile << task.sStar.t();
95 }
96 
98 {
99  velocityFile.close();
100  errorFile.close();
101  errorNormFile.close();
102  sFile.close();
103  sStarFile.close();
104 }
105 
106 /*
107  * Local variables:
108  * c-basic-offset: 2
109  * End:
110  */
static void makeDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:570
void open(const char *baseDirectory)
Definition: vpServoData.cpp:51
void save(const vpServo &task)
Definition: vpServoData.cpp:78
void setCmDeg()
velocity output in cm and deg
Definition: vpServoData.cpp:76
#define vpERROR_TRACE
Definition: vpDebug.h:393
vpRowVector t() const
vpColVector q_dot
Articular velocity.
Definition: vpServo.h:570
static bool checkDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:420
void close()
Definition: vpServoData.cpp:97
vpColVector s
Definition: vpServo.h:558
static double deg(double rad)
Definition: vpMath.h:103
double sumSquare() const
vpColVector sStar
Definition: vpServo.h:562
vpColVector getError() const
Definition: vpServo.h:278
void setMeterRad()
velocity output in meter and deg (default)
Definition: vpServoData.cpp:77