Visual Servoing Platform  version 3.6.1 under development (2025-02-17)
testForceTorqueAti.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
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 https://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 
43 #include <iostream>
44 
45 #include <visp3/core/vpIoTools.h>
46 #include <visp3/core/vpTime.h>
47 #include <visp3/gui/vpPlot.h>
48 #include <visp3/sensor/vpForceTorqueAtiSensor.h>
49 
50 #if defined(VISP_HAVE_THREADS)
51 
52 #include <thread>
53 #include <mutex>
54 
55 typedef enum { BIAS_DONE, UNBIAS_DONE, TO_BIAS, TO_UNBIAS } BiasState;
56 
57 #ifdef ENABLE_VISP_NAMESPACE
58 using namespace VISP_NAMESPACE_NAME;
59 #endif
60 
61 #ifndef DOXYGEN_SHOULD_SKIP_THIS
62 typedef struct
63 {
64  vpColVector ft;
65  double timestamp;
66  BiasState bias_state;
67 } t_shared_data;
68 #endif
69 
70 void scopeFunction(std::mutex &mutex_data, std::mutex &mutex_state, t_shared_data &s_shared_data, bool &s_state_stop)
71 {
72 #ifdef VISP_HAVE_DISPLAY
73  vpPlot scope(2, 700, 700, 100, 200, "ATI F/T sensor data");
74  scope.initGraph(0, 3);
75  scope.initGraph(1, 3);
76  scope.setTitle(0, "Forces (N)");
77  scope.setTitle(1, "Torques (Nm)");
78  scope.setLegend(0, 0, "x");
79  scope.setLegend(0, 1, "y");
80  scope.setLegend(0, 2, "z");
81  scope.setLegend(1, 0, "x");
82  scope.setLegend(1, 1, "y");
83  scope.setLegend(1, 2, "z");
84 #else
85  (void)mutex_data;
86  (void)s_shared_data;
87 #endif
88 
89  t_shared_data shared_data;
90 #ifdef VISP_HAVE_DISPLAY
91  bool click = false;
93 #else
94  double start_time = vpTime::measureTimeMs();
95 #endif
96  do {
97 #ifdef VISP_HAVE_DISPLAY
98  { // Get new measures to plot
99  std::lock_guard<std::mutex> lock(mutex_data);
100  shared_data.ft = s_shared_data.ft;
101  shared_data.timestamp = s_shared_data.timestamp;
102  shared_data.bias_state = s_shared_data.bias_state;
103  }
104 
105  vpColVector force = shared_data.ft.extract(0, 3);
106  vpColVector torque = shared_data.ft.extract(3, 3);
107  scope.plot(0, shared_data.timestamp, force);
108  scope.plot(1, shared_data.timestamp, torque);
109 
110  vpDisplay::displayText(scope.I, 15, 500, "Left click to quit", vpColor::red);
111  vpDisplay::displayText(scope.I, 30, 500, "Right click to bias/unbias", vpColor::red);
112  if (shared_data.bias_state == BIAS_DONE)
113  vpDisplay::displayText(scope.I, 45, 500, "Sensor is biased...", vpColor::blue);
114  else if (shared_data.bias_state == UNBIAS_DONE)
115  vpDisplay::displayText(scope.I, 45, 500, "Sensor is unbiased...", vpColor::blue);
116  else if (shared_data.bias_state == TO_BIAS)
117  vpDisplay::displayText(scope.I, 45, 500, "Sensor bias in progress...", vpColor::blue);
118  else if (shared_data.bias_state == TO_UNBIAS)
119  vpDisplay::displayText(scope.I, 45, 500, "Sensor unbias in progress...", vpColor::blue);
120  vpDisplay::flush(scope.I);
121  click = vpDisplay::getClick(scope.I, button, false);
122  if (click && button == vpMouseButton::button3) {
123  if (shared_data.bias_state == BIAS_DONE)
124  shared_data.bias_state = TO_UNBIAS;
125  else if (shared_data.bias_state == UNBIAS_DONE)
126  shared_data.bias_state = TO_BIAS;
127  { // Set new bias state
128  std::lock_guard<std::mutex> lock(mutex_data);
129  s_shared_data.bias_state = shared_data.bias_state;
130  }
131  }
132 
133 #endif
134  }
135 #ifdef VISP_HAVE_DISPLAY
136  while (!(click && button == vpMouseButton::button1)); // Stop recording by a user left click
137 #else
138  while (vpTime::measureTimeMs() - start_time < 20000); // Stop recording after 20 seconds
139 #endif
140 
141  { // Update state to stop
142  std::lock_guard<std::mutex> lock(mutex_state);
143  s_state_stop = true;
144  }
145 
146  std::cout << "End of scope thread" << std::endl;
147 }
148 
149 int main(int argc, char **argv)
150 {
151 #if defined(VISP_HAVE_ATIDAQ) && defined(VISP_HAVE_COMEDI)
152 
153 #ifdef VISP_HAVE_VIPER850_DATA
154  (void)argc;
155  (void)argv;
156  std::string calibfile = std::string(VISP_VIPER850_DATA_PATH) + std::string("/ati/FT17824.cal");
157  if (!vpIoTools::checkFilename(calibfile)) {
158  std::cout << "ATI F/T calib file \"" << calibfile << "\" doesn't exist";
159  return EXIT_FAILURE;
160  }
161 #else
162  if (argc != 2) {
163  std::cout << "Usage: " << argv[0] << " <ATI calibration file FT*.cal]>" << std::endl;
164  return EXIT_FAILURE;
165  }
166  std::string calibfile(argv[1]);
167 #endif
168 
170  ati.setCalibrationFile(calibfile);
171  ati.open();
172  std::cout << "ATI F/T sensor characteristics: \n" << ati << std::endl;
173 
174  ati.bias();
175  std::cout << "Data recording in progress..." << std::endl;
176 
177  std::mutex mutex_data;
178  std::mutex mutex_state;
179  bool state_stop = false;
180  bool s_state_stop = false;
181  t_shared_data shared_data;
182  t_shared_data s_shared_data;
183 
184  // Start scope thread
185  std::thread thread_scope(&scopeFunction, std::ref(mutex_data), std::ref(mutex_state), std::ref(s_shared_data), std::ref(s_state_stop));
186 
187  std::string file("recorded-ft-sync.txt");
188  std::ofstream f(file.c_str());
189 
190  double start_time = vpTime::measureTimeMs();
191 
192  do {
193  double loop_time = vpTime::measureTimeMs();
194  vpColVector ft = ati.getForceTorque();
195  double timestamp = loop_time - start_time;
196 
197  { // Update shared F/T measure used by the scope to plot curves
198  std::lock_guard<std::mutex> lock(mutex_data);
199  shared_data.bias_state = s_shared_data.bias_state;
200  }
201  if (shared_data.bias_state == TO_BIAS) {
202  std::cout << "Bias sensor" << std::endl;
203  ati.bias();
204  std::cout << "Unbias sensor" << std::endl;
205  shared_data.bias_state = BIAS_DONE;
206  }
207  else if (shared_data.bias_state == TO_UNBIAS) {
208  ati.unbias();
209  shared_data.bias_state = UNBIAS_DONE;
210  }
211 
212  { // Update shared F/T measure used by the scope to plot curves
213  std::lock_guard<std::mutex> lock(mutex_data);
214  s_shared_data.ft = ft;
215  s_shared_data.timestamp = timestamp;
216  s_shared_data.bias_state = shared_data.bias_state;
217  }
218  { // Get state to stop
219  std::lock_guard<std::mutex> lock(mutex_state);
220  state_stop = s_state_stop;
221  }
222 
223  f << timestamp << " " << ft.t() << std::endl;
224  vpTime::wait(loop_time, 1); // Get a new data each 1 millisecond
225  } while (!state_stop);
226 
227  // Wait until thread ends up
228  thread_scope.join();
229 
230  ati.close();
231  f.close();
232  std::cout << "Data recorded in " << file << std::endl;
233 #else
234  (void)argc;
235  (void)argv;
236  std::cout << "You should install comedi and build atidaq to enable this test..." << std::endl;
237 #endif
238  return EXIT_SUCCESS;
239 }
240 
241 #else
242 int main()
243 {
244  std::cout << "You should build this test with threading capabilities..." << std::endl;
245  return EXIT_SUCCESS;
246 }
247 #endif
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
vpColVector extract(unsigned int r, unsigned int colsize) const
Definition: vpColVector.h:410
vpRowVector t() const
static const vpColor red
Definition: vpColor.h:198
static const vpColor blue
Definition: vpColor.h:204
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void flush(const vpImage< unsigned char > &I)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
vpColVector getForceTorque() const
void setCalibrationFile(const std::string &calibfile, unsigned short index=1)
static bool checkFilename(const std::string &filename)
Definition: vpIoTools.cpp:786
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
Definition: vpPlot.h:112
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()