Visual Servoing Platform  version 3.0.1
testForceTorqueAti.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Test force/torque ATI sensor.
32  *
33  * Authors:
34  * Fabien Spindler
35  *
36  *****************************************************************************/
37 
46 #include <iostream>
47 
48 #include <visp3/core/vpIoTools.h>
49 #include <visp3/core/vpMutex.h>
50 #include <visp3/core/vpTime.h>
51 #include <visp3/core/vpThread.h>
52 #include <visp3/sensor/vpForceTorqueAtiSensor.h>
53 #include <visp3/gui/vpPlot.h>
54 
55 #if defined(VISP_HAVE_PTHREAD)
56 
57 typedef enum {
58  BIAS_DONE,
59  UNBIAS_DONE,
60  TO_BIAS,
61  TO_UNBIAS
62 } BiasState;
63 
64 vpMutex s_mutex_data;
65 
66 #ifndef DOXYGEN_SHOULD_SKIP_THIS
67 typedef struct {
68  vpColVector ft;
69  double timestamp;
70  BiasState bias_state;
71 } t_shared_data;
72 #endif
73 
74 t_shared_data s_shared_data;
75 
76 vpMutex s_mutex_state;
77 bool s_state_stop = false;
78 
79 vpThread::Return scopeFunction(vpThread::Args args)
80 {
81  (void)args; // Avoid warning: unused parameter args
82 
83 #ifdef VISP_HAVE_DISPLAY
84  vpPlot scope(2, 700, 700, 100, 200, "ATI F/T sensor data");
85  scope.initGraph(0,3);
86  scope.initGraph(1,3);
87  scope.setTitle(0, "Forces (N)");
88  scope.setTitle(1, "Torques (Nm)");
89  scope.setLegend(0, 0, "x");
90  scope.setLegend(0, 1, "y");
91  scope.setLegend(0, 2, "z");
92  scope.setLegend(1, 0, "x");
93  scope.setLegend(1, 1, "y");
94  scope.setLegend(1, 2, "z");
95 #endif
96 
97  t_shared_data shared_data;
98 #ifdef VISP_HAVE_DISPLAY
99  bool click = false;
101 #else
102  double start_time = vpTime::measureTimeMs();
103 #endif
104  do {
105 #ifdef VISP_HAVE_DISPLAY
106  { // Get new measures to plot
107  vpMutex::vpScopedLock lock(s_mutex_data);
108  shared_data.ft = s_shared_data.ft;
109  shared_data.timestamp = s_shared_data.timestamp;
110  shared_data.bias_state = s_shared_data.bias_state;
111  }
112 
113  vpColVector force = shared_data.ft.extract(0, 3);
114  vpColVector torque = shared_data.ft.extract(3, 3);
115  scope.plot(0, shared_data.timestamp, force);
116  scope.plot(1, shared_data.timestamp, torque);
117 
118  vpDisplay::displayText(scope.I, 15, 500, "Left click to quit", vpColor::red);
119  vpDisplay::displayText(scope.I, 30, 500, "Right click to bias/unbias", vpColor::red);
120  if (shared_data.bias_state == BIAS_DONE)
121  vpDisplay::displayText(scope.I, 45, 500, "Sensor is biased...", vpColor::blue);
122  else if (shared_data.bias_state == UNBIAS_DONE)
123  vpDisplay::displayText(scope.I, 45, 500, "Sensor is unbiased...", vpColor::blue);
124  else if (shared_data.bias_state == TO_BIAS)
125  vpDisplay::displayText(scope.I, 45, 500, "Sensor bias in progress...", vpColor::blue);
126  else if (shared_data.bias_state == TO_UNBIAS)
127  vpDisplay::displayText(scope.I, 45, 500, "Sensor unbias in progress...", vpColor::blue);
128  vpDisplay::flush(scope.I);
129  click = vpDisplay::getClick(scope.I, button, false);
130  if (click && button == vpMouseButton::button3) {
131  if (shared_data.bias_state == BIAS_DONE) shared_data.bias_state = TO_UNBIAS;
132  else if (shared_data.bias_state == UNBIAS_DONE) shared_data.bias_state = TO_BIAS;
133  { // Set new bias state
134  vpMutex::vpScopedLock lock(s_mutex_data);
135  s_shared_data.bias_state = shared_data.bias_state;
136  }
137  }
138 
139 #endif
140  }
141 #ifdef VISP_HAVE_DISPLAY
142  while(! (click && button == vpMouseButton::button1)); // Stop recording by a user left click
143 #else
144  while(vpTime::measureTimeMs() - start_time < 20000); // Stop recording after 20 seconds
145 #endif
146 
147  { // Update state to stop
148  vpMutex::vpScopedLock lock(s_mutex_state);
149  s_state_stop = true;
150  }
151 
152  std::cout << "End of scope thread" << std::endl;
153  return 0;
154 }
155 
156 int main(int argc, char** argv)
157 {
158 #if defined(VISP_HAVE_ATIDAQ) && defined(VISP_HAVE_COMEDI)
159 
160 #ifdef VISP_HAVE_VIPER850_DATA
161  (void)argc;
162  (void)argv;
163  std::string calibfile = std::string(VISP_VIPER850_DATA_PATH) + std::string("/ati/FT17824.cal");
164  if (! vpIoTools::checkFilename(calibfile)) {
165  std::cout << "ATI F/T calib file \"" << calibfile << "\" doesn't exist";
166  return 0;
167  }
168 #else
169  if(argc != 2) {
170  std::cout << "Usage: " << argv[0] << " <ATI calibration file FT*.cal]>" << std::endl;
171  return -1;
172  }
173  std::string calibfile(argv[1]);
174 #endif
175 
177  ati.setCalibrationFile(calibfile);
178  ati.open();
179  std::cout << "ATI F/T sensor characteristics: \n" << ati << std::endl;
180 
181  ati.bias();
182  std::cout << "Data recording in progress..." << std::endl;
183 
184  // Start scope thread
185  vpThread thread_scope(scopeFunction);
186 
187  std::string file("recorded-ft-sync.txt");
188  std::ofstream f(file.c_str());
189  bool state_stop;
190  t_shared_data shared_data;
191 
192  double start_time = vpTime::measureTimeMs();
193 
194  do {
195  double loop_time = vpTime::measureTimeMs();
196  vpColVector ft = ati.getForceTorque();
197  double timestamp = loop_time - start_time;
198 
199  { // Update shared F/T measure used by the scope to plot curves
200  vpMutex::vpScopedLock lock(s_mutex_data);
201  shared_data.bias_state = s_shared_data.bias_state;
202  }
203  if (shared_data.bias_state == TO_BIAS) {
204  std::cout << "Bias sensor" << std::endl;
205  ati.bias();
206  std::cout << "Unbias sensor" << std::endl;
207  shared_data.bias_state = BIAS_DONE;
208  }
209  else if (shared_data.bias_state == TO_UNBIAS) {
210  ati.unbias();
211  shared_data.bias_state = UNBIAS_DONE;
212  }
213 
214  { // Update shared F/T measure used by the scope to plot curves
215  vpMutex::vpScopedLock lock(s_mutex_data);
216  s_shared_data.ft = ft;
217  s_shared_data.timestamp = timestamp;
218  s_shared_data.bias_state = shared_data.bias_state;
219  }
220  { // Get state to stop
221  vpMutex::vpScopedLock lock(s_mutex_state);
222  state_stop = s_state_stop;
223  }
224 
225  f << timestamp << " " << ft.t() << std::endl;
226  vpTime::wait(loop_time, 1); // Get a new data each 1 millisecond
227  }
228  while(! state_stop);
229 
230  // Wait until thread ends up
231  thread_scope.join();
232 
233  ati.close();
234  f.close();
235  std::cout << "Data recorded in " << file << std::endl;
236 #else
237  (void)argc;
238  (void)argv;
239  std::cout << "You should install comedi and build atidaq to enable this test..." << std::endl;
240 #endif
241 }
242 
243 #else
244 int main()
245 {
246  std::cout << "You should build this test with threading capabilities..." << std::endl;
247 }
248 #endif
Class that allows protection by mutex.
Definition: vpMutex.h:163
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:157
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
void * Return
Definition: vpThread.h:36
vpColVector getForceTorque() const
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
vpColVector extract(unsigned int r, unsigned int colsize) const
Definition: vpColVector.h:149
vpRowVector t() const
static void flush(const vpImage< unsigned char > &I)
VISP_EXPORT double measureTimeMs()
Definition: vpTime.cpp:93
static const vpColor red
Definition: vpColor.h:163
static bool checkFilename(const char *filename)
Definition: vpIoTools.cpp:508
void * Args
Definition: vpThread.h:35
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
Definition: vpPlot.h:113
void setCalibrationFile(const std::string &calibfile, unsigned short index=1)
static const vpColor blue
Definition: vpColor.h:169