Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
testForceTorqueIitSensor.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 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 https://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 IIT sensor.
33 *
34*****************************************************************************/
35
42#include <iostream>
43
44#include <visp3/gui/vpPlot.h>
45#include <visp3/sensor/vpForceTorqueIitSensor.h>
46
47int main(int argc, char **argv)
48{
49#ifdef VISP_HAVE_FT_IIT_SDK
50 bool opt_no_display = false;
51 bool opt_filtered = false;
52
53 for (int i = 0; i < argc; i++) {
54 if (std::string(argv[i]) == "--no-display" || std::string(argv[i]) == "-d")
55 opt_no_display = true;
56 if (std::string(argv[i]) == "--filtered" || std::string(argv[i]) == "-f")
57 opt_filtered = true;
58 else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
59 std::cout << "\nUsage: " << argv[0] << " [--no-display] [--filtered] [--help] [-d] [-f] [-h]\n" << std::endl;
60 return 0;
61 }
62 }
63
64 std::cout << "Disable display: " << opt_no_display << std::endl;
65
67
68#if defined(VISP_HAVE_DISPLAY)
69 vpPlot *plotter = NULL;
70 if (!opt_no_display) {
71 plotter = new vpPlot(2, 700, 700, 100, 200, "Curves...");
72 plotter->initGraph(0, 3);
73 plotter->setTitle(0, "Force measurements");
74 plotter->setLegend(0, 0, "Fx");
75 plotter->setLegend(0, 1, "Fy");
76 plotter->setLegend(0, 2, "Fz");
77 plotter->initGraph(1, 3);
78 plotter->setTitle(1, "Torque measurements");
79 plotter->setLegend(1, 0, "Tx");
80 plotter->setLegend(1, 1, "Ty");
81 plotter->setLegend(1, 2, "Tz");
82 }
83#endif
84
85 if (!iit_ft.connected()) {
86 std::cout << "Unable to connect to IIT force-torque sensor" << std::endl;
87 return EXIT_SUCCESS;
88 }
89
90 iit_ft.bias();
91 iit_ft.startStreaming();
92
93 vpColVector ft;
94 bool end = false;
95 unsigned long nbacq = 0;
96 double t_start = vpTime::measureTimeMs();
97 while (!end) {
98 double t = vpTime::measureTimeMs();
99
100 ft = iit_ft.getForceTorque(opt_filtered);
101#if defined(VISP_HAVE_DISPLAY)
102 if (!opt_no_display) {
103 vpColVector force = ft.extract(0, 3);
104 vpColVector torque = ft.extract(3, 3);
105 plotter->plot(0, nbacq, force);
106 plotter->plot(1, nbacq, torque);
107 vpDisplay::displayText(plotter->I, 20, 80, "Left click to quit", vpColor::red);
109
110 if (vpDisplay::getClick(plotter->I, button, false)) {
111 if (button == vpMouseButton::button1) {
112 end = true;
113 }
114 }
115 vpDisplay::flush(plotter->I);
116 } else {
117 std::cout << "F/T: " << ft.t() << std::endl;
118 if (nbacq > 30) {
119 end = true;
120 }
121 }
122#else
123 std::cout << "F/T: " << ft.t() << std::endl;
124 if (nbacq > 30) {
125 end = true;
126 }
127#endif
128 vpTime::wait(t, 1);
129 nbacq++;
130 }
131
132 iit_ft.stopStreaming();
133 double fps = 1000. * nbacq / (vpTime::measureTimeMs() - t_start);
134 std::cout << "Mean acquisition frequency: " << fps << " Hz" << std::endl;
135
136#if defined(VISP_HAVE_DISPLAY)
137 if (plotter) {
138 delete plotter;
139 }
140#endif
141
142 std::cout << "Test succeed" << std::endl;
143
144#else
145 (void)argc;
146 (void)argv;
147 std::cout << "ViSP is not build with IIT force-torque SDK support" << std::endl;
148#endif
149 return EXIT_SUCCESS;
150}
Implementation of column vector and the associated operations.
vpColVector extract(unsigned int r, unsigned int colsize) const
vpRowVector t() const
static const vpColor red
Definition vpColor.h:211
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)
bool connected(int timeout_ms=0) const
vpColVector getForceTorque(bool filtered=false)
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 initGraph(unsigned int graphNum, unsigned int curveNbr)
Definition vpPlot.cpp:202
vpImage< unsigned char > I
Definition vpPlot.h:115
void setLegend(unsigned int graphNum, unsigned int curveNum, const std::string &legend)
Definition vpPlot.cpp:545
void plot(unsigned int graphNum, unsigned int curveNum, double x, double y)
Definition vpPlot.cpp:269
void setTitle(unsigned int graphNum, const std::string &title)
Definition vpPlot.cpp:503
VISP_EXPORT double measureTimeMs()
VISP_EXPORT int wait(double t0, double t)