ViSP  2.6.2
vpServoData.cpp
1 /****************************************************************************
2  *
3  * $Id: vpServoData.cpp 3616 2012-03-09 14:31:52Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2012 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:289
#define vpERROR_TRACE
Definition: vpDebug.h:379
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:440
double sumSquare() const
return sum of the Aij^2 (for all i, for all j)
Definition: vpMatrix.cpp:758
static void makeDirectory(const char *dirname)
Definition: vpIoTools.cpp:358
vpColVector getError() const
Definition: vpServo.h:298
vpRowVector t() const
transpose of Vector
vpColVector s
Definition: vpServo.h:428
static double deg(double rad)
Definition: vpMath.h:93
vpColVector sStar
Definition: vpServo.h:431
Class required to compute the visual servoing control law.
Definition: vpServo.h:150
void setMeterRad()
velocity output in meter and deg (default)
Definition: vpServoData.cpp:88