Visual Servoing Platform  version 3.6.1 under development (2024-10-13)
testForceTorqueAti.cpp

This example shows how to retrieve data from an ATI F/T sensor connected to a DAQ board. He we have 2 threads:

/*
* ViSP, open source Visual Servoing Platform software.
* Copyright (C) 2005 - 2024 by Inria. All rights reserved.
*
* This software is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
* For using ViSP with software that can not be combined with the GNU
* GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
* See https://visp.inria.fr for more information.
*
* This software was developed at:
* Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
*
* If you have questions regarding the use of this file, please contact
* Inria at visp@inria.fr
*
* This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
* Description:
* Test force/torque ATI sensor.
*/
#include <iostream>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpTime.h>
#include <visp3/gui/vpPlot.h>
#include <visp3/sensor/vpForceTorqueAtiSensor.h>
#if defined(VISP_HAVE_THREADS)
#include <thread>
#include <mutex>
typedef enum { BIAS_DONE, UNBIAS_DONE, TO_BIAS, TO_UNBIAS } BiasState;
#ifdef ENABLE_VISP_NAMESPACE
using namespace VISP_NAMESPACE_NAME;
#endif
#ifndef DOXYGEN_SHOULD_SKIP_THIS
typedef struct
{
double timestamp;
BiasState bias_state;
} t_shared_data;
#endif
void scopeFunction(std::mutex &mutex_data, std::mutex &mutex_state, t_shared_data &s_shared_data, bool &s_state_stop)
{
#ifdef VISP_HAVE_DISPLAY
vpPlot scope(2, 700, 700, 100, 200, "ATI F/T sensor data");
scope.initGraph(0, 3);
scope.initGraph(1, 3);
scope.setTitle(0, "Forces (N)");
scope.setTitle(1, "Torques (Nm)");
scope.setLegend(0, 0, "x");
scope.setLegend(0, 1, "y");
scope.setLegend(0, 2, "z");
scope.setLegend(1, 0, "x");
scope.setLegend(1, 1, "y");
scope.setLegend(1, 2, "z");
#else
(void)mutex_data;
(void)s_shared_data;
#endif
t_shared_data shared_data;
#ifdef VISP_HAVE_DISPLAY
bool click = false;
#else
double start_time = vpTime::measureTimeMs();
#endif
do {
#ifdef VISP_HAVE_DISPLAY
{ // Get new measures to plot
std::lock_guard<std::mutex> lock(mutex_data);
shared_data.ft = s_shared_data.ft;
shared_data.timestamp = s_shared_data.timestamp;
shared_data.bias_state = s_shared_data.bias_state;
}
vpColVector force = shared_data.ft.extract(0, 3);
vpColVector torque = shared_data.ft.extract(3, 3);
scope.plot(0, shared_data.timestamp, force);
scope.plot(1, shared_data.timestamp, torque);
vpDisplay::displayText(scope.I, 15, 500, "Left click to quit", vpColor::red);
vpDisplay::displayText(scope.I, 30, 500, "Right click to bias/unbias", vpColor::red);
if (shared_data.bias_state == BIAS_DONE)
vpDisplay::displayText(scope.I, 45, 500, "Sensor is biased...", vpColor::blue);
else if (shared_data.bias_state == UNBIAS_DONE)
vpDisplay::displayText(scope.I, 45, 500, "Sensor is unbiased...", vpColor::blue);
else if (shared_data.bias_state == TO_BIAS)
vpDisplay::displayText(scope.I, 45, 500, "Sensor bias in progress...", vpColor::blue);
else if (shared_data.bias_state == TO_UNBIAS)
vpDisplay::displayText(scope.I, 45, 500, "Sensor unbias in progress...", vpColor::blue);
vpDisplay::flush(scope.I);
click = vpDisplay::getClick(scope.I, button, false);
if (click && button == vpMouseButton::button3) {
if (shared_data.bias_state == BIAS_DONE)
shared_data.bias_state = TO_UNBIAS;
else if (shared_data.bias_state == UNBIAS_DONE)
shared_data.bias_state = TO_BIAS;
{ // Set new bias state
std::lock_guard<std::mutex> lock(mutex_data);
s_shared_data.bias_state = shared_data.bias_state;
}
}
#endif
}
#ifdef VISP_HAVE_DISPLAY
while (!(click && button == vpMouseButton::button1)); // Stop recording by a user left click
#else
while (vpTime::measureTimeMs() - start_time < 20000); // Stop recording after 20 seconds
#endif
{ // Update state to stop
std::lock_guard<std::mutex> lock(mutex_state);
s_state_stop = true;
}
std::cout << "End of scope thread" << std::endl;
}
int main(int argc, char **argv)
{
#if defined(VISP_HAVE_ATIDAQ) && defined(VISP_HAVE_COMEDI)
#ifdef VISP_HAVE_VIPER850_DATA
(void)argc;
(void)argv;
std::string calibfile = std::string(VISP_VIPER850_DATA_PATH) + std::string("/ati/FT17824.cal");
if (!vpIoTools::checkFilename(calibfile)) {
std::cout << "ATI F/T calib file \"" << calibfile << "\" doesn't exist";
return EXIT_FAILURE;
}
#else
if (argc != 2) {
std::cout << "Usage: " << argv[0] << " <ATI calibration file FT*.cal]>" << std::endl;
return EXIT_FAILURE;
}
std::string calibfile(argv[1]);
#endif
ati.setCalibrationFile(calibfile);
ati.open();
std::cout << "ATI F/T sensor characteristics: \n" << ati << std::endl;
ati.bias();
std::cout << "Data recording in progress..." << std::endl;
std::mutex mutex_data;
std::mutex mutex_state;
bool state_stop = false;
bool s_state_stop = false;
t_shared_data shared_data;
t_shared_data s_shared_data;
// Start scope thread
std::thread thread_scope(&scopeFunction, std::ref(mutex_data), std::ref(mutex_state), std::ref(s_shared_data), std::ref(s_state_stop));
std::string file("recorded-ft-sync.txt");
std::ofstream f(file.c_str());
double start_time = vpTime::measureTimeMs();
do {
double loop_time = vpTime::measureTimeMs();
double timestamp = loop_time - start_time;
{ // Update shared F/T measure used by the scope to plot curves
std::lock_guard<std::mutex> lock(mutex_data);
shared_data.bias_state = s_shared_data.bias_state;
}
if (shared_data.bias_state == TO_BIAS) {
std::cout << "Bias sensor" << std::endl;
ati.bias();
std::cout << "Unbias sensor" << std::endl;
shared_data.bias_state = BIAS_DONE;
}
else if (shared_data.bias_state == TO_UNBIAS) {
ati.unbias();
shared_data.bias_state = UNBIAS_DONE;
}
{ // Update shared F/T measure used by the scope to plot curves
std::lock_guard<std::mutex> lock(mutex_data);
s_shared_data.ft = ft;
s_shared_data.timestamp = timestamp;
s_shared_data.bias_state = shared_data.bias_state;
}
{ // Get state to stop
std::lock_guard<std::mutex> lock(mutex_state);
state_stop = s_state_stop;
}
f << timestamp << " " << ft.t() << std::endl;
vpTime::wait(loop_time, 1); // Get a new data each 1 millisecond
} while (!state_stop);
// Wait until thread ends up
thread_scope.join();
ati.close();
f.close();
std::cout << "Data recorded in " << file << std::endl;
#else
(void)argc;
(void)argv;
std::cout << "You should install comedi and build atidaq to enable this test..." << std::endl;
#endif
return EXIT_SUCCESS;
}
#else
int main()
{
std::cout << "You should build this test with threading capabilities..." << std::endl;
return EXIT_SUCCESS;
}
#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:405
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: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()