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