Visual Servoing Platform version 3.5.0
testForceTorqueAti.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 * Test force/torque ATI sensor.
33 *
34 * Authors:
35 * Fabien Spindler
36 *
37 *****************************************************************************/
38
48#include <iostream>
49
50#include <visp3/core/vpIoTools.h>
51#include <visp3/core/vpMutex.h>
52#include <visp3/core/vpThread.h>
53#include <visp3/core/vpTime.h>
54#include <visp3/gui/vpPlot.h>
55#include <visp3/sensor/vpForceTorqueAtiSensor.h>
56
57#if defined(VISP_HAVE_PTHREAD)
58
59typedef enum { BIAS_DONE, UNBIAS_DONE, TO_BIAS, TO_UNBIAS } BiasState;
60
61vpMutex s_mutex_data;
62
63#ifndef DOXYGEN_SHOULD_SKIP_THIS
64typedef struct {
65 vpColVector ft;
66 double timestamp;
67 BiasState bias_state;
68} t_shared_data;
69#endif
70
71t_shared_data s_shared_data;
72
73vpMutex s_mutex_state;
74bool s_state_stop = false;
75
76vpThread::Return scopeFunction(vpThread::Args args)
77{
78 (void)args; // Avoid warning: unused parameter args
79
80#ifdef VISP_HAVE_DISPLAY
81 vpPlot scope(2, 700, 700, 100, 200, "ATI F/T sensor data");
82 scope.initGraph(0, 3);
83 scope.initGraph(1, 3);
84 scope.setTitle(0, "Forces (N)");
85 scope.setTitle(1, "Torques (Nm)");
86 scope.setLegend(0, 0, "x");
87 scope.setLegend(0, 1, "y");
88 scope.setLegend(0, 2, "z");
89 scope.setLegend(1, 0, "x");
90 scope.setLegend(1, 1, "y");
91 scope.setLegend(1, 2, "z");
92#endif
93
94 t_shared_data shared_data;
95#ifdef VISP_HAVE_DISPLAY
96 bool click = false;
98#else
99 double start_time = vpTime::measureTimeMs();
100#endif
101 do {
102#ifdef VISP_HAVE_DISPLAY
103 { // Get new measures to plot
104 vpMutex::vpScopedLock lock(s_mutex_data);
105 shared_data.ft = s_shared_data.ft;
106 shared_data.timestamp = s_shared_data.timestamp;
107 shared_data.bias_state = s_shared_data.bias_state;
108 }
109
110 vpColVector force = shared_data.ft.extract(0, 3);
111 vpColVector torque = shared_data.ft.extract(3, 3);
112 scope.plot(0, shared_data.timestamp, force);
113 scope.plot(1, shared_data.timestamp, torque);
114
115 vpDisplay::displayText(scope.I, 15, 500, "Left click to quit", vpColor::red);
116 vpDisplay::displayText(scope.I, 30, 500, "Right click to bias/unbias", vpColor::red);
117 if (shared_data.bias_state == BIAS_DONE)
118 vpDisplay::displayText(scope.I, 45, 500, "Sensor is biased...", vpColor::blue);
119 else if (shared_data.bias_state == UNBIAS_DONE)
120 vpDisplay::displayText(scope.I, 45, 500, "Sensor is unbiased...", vpColor::blue);
121 else if (shared_data.bias_state == TO_BIAS)
122 vpDisplay::displayText(scope.I, 45, 500, "Sensor bias in progress...", vpColor::blue);
123 else if (shared_data.bias_state == TO_UNBIAS)
124 vpDisplay::displayText(scope.I, 45, 500, "Sensor unbias in progress...", vpColor::blue);
125 vpDisplay::flush(scope.I);
126 click = vpDisplay::getClick(scope.I, button, false);
127 if (click && button == vpMouseButton::button3) {
128 if (shared_data.bias_state == BIAS_DONE)
129 shared_data.bias_state = TO_UNBIAS;
130 else if (shared_data.bias_state == UNBIAS_DONE)
131 shared_data.bias_state = TO_BIAS;
132 { // Set new bias state
133 vpMutex::vpScopedLock lock(s_mutex_data);
134 s_shared_data.bias_state = shared_data.bias_state;
135 }
136 }
137
138#endif
139 }
140#ifdef VISP_HAVE_DISPLAY
141 while (!(click && button == vpMouseButton::button1)); // Stop recording by a user left click
142#else
143 while (vpTime::measureTimeMs() - start_time < 20000); // Stop recording after 20 seconds
144#endif
145
146 { // Update state to stop
147 vpMutex::vpScopedLock lock(s_mutex_state);
148 s_state_stop = true;
149 }
150
151 std::cout << "End of scope thread" << std::endl;
152 return 0;
153}
154
155int main(int argc, char **argv)
156{
157#if defined(VISP_HAVE_ATIDAQ) && defined(VISP_HAVE_COMEDI)
158
159#ifdef VISP_HAVE_VIPER850_DATA
160 (void)argc;
161 (void)argv;
162 std::string calibfile = std::string(VISP_VIPER850_DATA_PATH) + std::string("/ati/FT17824.cal");
163 if (!vpIoTools::checkFilename(calibfile)) {
164 std::cout << "ATI F/T calib file \"" << calibfile << "\" doesn't exist";
165 return 0;
166 }
167#else
168 if (argc != 2) {
169 std::cout << "Usage: " << argv[0] << " <ATI calibration file FT*.cal]>" << std::endl;
170 return -1;
171 }
172 std::string calibfile(argv[1]);
173#endif
174
176 ati.setCalibrationFile(calibfile);
177 ati.open();
178 std::cout << "ATI F/T sensor characteristics: \n" << ati << std::endl;
179
180 ati.bias();
181 std::cout << "Data recording in progress..." << std::endl;
182
183 // Start scope thread
184 vpThread thread_scope(scopeFunction);
185
186 std::string file("recorded-ft-sync.txt");
187 std::ofstream f(file.c_str());
188 bool state_stop;
189 t_shared_data shared_data;
190
191 double start_time = vpTime::measureTimeMs();
192
193 do {
194 double loop_time = vpTime::measureTimeMs();
195 vpColVector ft = ati.getForceTorque();
196 double timestamp = loop_time - start_time;
197
198 { // Update shared F/T measure used by the scope to plot curves
199 vpMutex::vpScopedLock lock(s_mutex_data);
200 shared_data.bias_state = s_shared_data.bias_state;
201 }
202 if (shared_data.bias_state == TO_BIAS) {
203 std::cout << "Bias sensor" << std::endl;
204 ati.bias();
205 std::cout << "Unbias sensor" << std::endl;
206 shared_data.bias_state = BIAS_DONE;
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 vpMutex::vpScopedLock lock(s_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 vpMutex::vpScopedLock lock(s_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}
239
240#else
241int main() { std::cout << "You should build this test with threading capabilities..." << std::endl; }
242#endif
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
vpColVector extract(unsigned int r, unsigned int colsize) const
Definition: vpColVector.h:220
vpRowVector t() const
static const vpColor red
Definition: vpColor.h:217
static const vpColor blue
Definition: vpColor.h:223
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:802
Class that allows protection by mutex.
Definition: vpMutex.h:171
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
Definition: vpPlot.h:116
void * Return
Definition: vpThread.h:78
void * Args
Definition: vpThread.h:77
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()