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