Visual Servoing Platform
version 3.6.0
Loading...
Searching...
No Matches
vpForceTorqueIitSensor.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
* Wrapper over IIT force-torque sensor.
33
*
34
* Authors:
35
* Alexander Oliva
36
*
37
*****************************************************************************/
38
44
#include <visp3/sensor/vpForceTorqueIitSensor.h>
45
46
#ifdef VISP_HAVE_FT_IIT_SDK
47
53
vpForceTorqueIitSensor::vpForceTorqueIitSensor
()
54
: m_ftLib(), m_numSensorsInLib(0), m_ft(6, 0), m_ft_filt(6, 0), m_ftSensorsData(), m_acquisitionEnabled(false),
55
m_dataValid(false), m_connected(false), m_acquisitionThread(), m_timeCur(), m_timePrev(), m_mutex(),
56
m_warmupMilliseconds(500)
57
{
58
// Get number of connected in library sensors
59
m_numSensorsInLib
=
m_ftLib
._getNumberOfConnectedSensors();
60
61
/*
62
* Initialize Communication with sensor(-s):
63
*
64
* streaming is configured with "storeOption=0",
65
* which means that data will be stored in the library's main thread
66
* and will be given in an output file.
67
*
68
* "storeDataFlag" is initialized as "false", so that the
69
* recording will not start with the streaming thread, but
70
* the moment for when the recording will start & stop.
71
*/
72
if
(
m_ftLib
._configureStreaming(
false
, 0) == 0) {
73
// Start the main acquisition thread
74
m_ftLib._startStreamingThread();
75
76
m_connected = true;
77
}
78
}
79
83
void
vpForceTorqueIitSensor::close
()
84
{
85
m_ftLib
._stopStreamingThread();
86
m_acquisitionEnabled
=
false
;
87
m_dataValid
=
false
;
88
}
89
93
vpForceTorqueIitSensor::~vpForceTorqueIitSensor
() { this->
close
(); }
94
98
void
vpForceTorqueIitSensor::join
()
99
{
100
if
(
m_acquisitionThread
.joinable()) {
101
m_acquisitionThread
.join();
102
}
103
}
104
108
void
vpForceTorqueIitSensor::acquisitionLoop
()
109
{
110
m_timePrev
=
m_timeCur
= std::chrono::system_clock::now();
111
112
// Main thread
113
auto
time_init = std::chrono::system_clock::now();
114
while
(
m_acquisitionEnabled
) {
115
116
// Get time passed since the last acquired sample
117
m_timeCur
= std::chrono::system_clock::now();
118
119
// Calculate delta time
120
auto
elapsed_milliseconds = std::chrono::duration_cast<std::chrono::milliseconds>(
m_timeCur
-
m_timePrev
).count();
121
122
if
(elapsed_milliseconds >= 1) {
123
/*
124
* Once a new aquisition started,
125
* reset loop timers to keep a relatively fixed sampling time
126
*/
127
// Update previous time
128
m_timePrev
=
m_timeCur
;
129
130
/*
131
* get all connected sensors' data:
132
* call to a mutex method allowing to access the ftsensors' data
133
* in streaming mode (1 sample / packet / sensor)
134
*/
135
m_ftSensorsData
=
m_ftLib
._getFTSensorsData();
136
137
// Warm up the sensor. At the beginning we experienced that values returned in m_ftSensorsData.ftSensor->ft
138
// are completly wrong like the following:
139
// 2.237378396e+11 207.3293304 14291.07715 1.479413346e+19 13.26593399 3380.078613
140
auto
warmup_milliseconds = std::chrono::duration_cast<std::chrono::milliseconds>(
m_timeCur
- time_init).count();
141
if
(warmup_milliseconds >
m_warmupMilliseconds
) {
142
m_dataValid
=
true
;
143
}
else
{
144
continue
;
145
}
146
147
const
std::lock_guard<std::mutex> lock(
m_mutex
);
148
for
(
unsigned
int
i = 0; i < 6; i++) {
149
m_ft
[i] =
m_ftSensorsData
.ftSensor->ft[i];
150
m_ft_filt
[i] =
m_ftSensorsData
.ftSensor->filt_ft[i];
151
}
152
}
153
}
154
}
155
159
void
vpForceTorqueIitSensor::bias
() {
m_ftLib
._biasAllFTSensorsTCP(); }
160
172
bool
vpForceTorqueIitSensor::connected
(
int
timeout_ms)
const
173
{
174
vpChrono
chrono;
175
chrono.
start
();
176
while
(!
m_connected
&& chrono.
getDurationMs
() < timeout_ms) {
177
vpTime::sleepMs
(1);
178
}
179
180
return
m_connected
;
181
}
182
205
vpColVector
vpForceTorqueIitSensor::getForceTorque
(
bool
filtered)
206
{
207
const
std::lock_guard<std::mutex> lock(
m_mutex
);
208
if
(filtered) {
209
return
m_ft_filt
;
210
}
else
{
211
return
m_ft
;
212
}
213
}
214
218
void
vpForceTorqueIitSensor::startStreaming
()
219
{
220
m_acquisitionEnabled
=
true
;
221
m_acquisitionThread
= std::thread([
this
] { this->
acquisitionLoop
(); });
222
223
while
(!
m_dataValid
) {
224
vpTime::wait
(10);
225
}
226
}
227
231
void
vpForceTorqueIitSensor::stopStreaming
()
232
{
233
m_acquisitionEnabled
=
false
;
234
if
(
m_acquisitionThread
.joinable()) {
235
m_acquisitionThread
.join();
236
}
237
}
238
239
#else
240
// Work around to avoid warning:
241
// libvisp_sensor.a(vpForceTorqueIitSensor.cpp.o) has no symbols
242
void
dummy_vpForceTorqueIitSensor(){};
243
#endif
vpChrono
Definition
vpTime.h:85
vpChrono::start
void start(bool reset=true)
Definition
vpTime.cpp:397
vpChrono::getDurationMs
double getDurationMs()
Definition
vpTime.cpp:386
vpColVector
Implementation of column vector and the associated operations.
Definition
vpColVector.h:167
vpForceTorqueIitSensor::vpForceTorqueIitSensor
vpForceTorqueIitSensor()
Definition
vpForceTorqueIitSensor.cpp:53
vpForceTorqueIitSensor::acquisitionLoop
void acquisitionLoop()
Definition
vpForceTorqueIitSensor.cpp:108
vpForceTorqueIitSensor::m_connected
bool m_connected
Definition
vpForceTorqueIitSensor.h:175
vpForceTorqueIitSensor::m_timePrev
std::chrono::time_point< std::chrono::system_clock > m_timePrev
Definition
vpForceTorqueIitSensor.h:178
vpForceTorqueIitSensor::m_ft
vpColVector m_ft
Definition
vpForceTorqueIitSensor.h:168
vpForceTorqueIitSensor::close
void close()
Definition
vpForceTorqueIitSensor.cpp:83
vpForceTorqueIitSensor::m_numSensorsInLib
int m_numSensorsInLib
Definition
vpForceTorqueIitSensor.h:166
vpForceTorqueIitSensor::~vpForceTorqueIitSensor
~vpForceTorqueIitSensor()
Definition
vpForceTorqueIitSensor.cpp:93
vpForceTorqueIitSensor::m_dataValid
std::atomic< bool > m_dataValid
Definition
vpForceTorqueIitSensor.h:174
vpForceTorqueIitSensor::connected
bool connected(int timeout_ms=0) const
Definition
vpForceTorqueIitSensor.cpp:172
vpForceTorqueIitSensor::startStreaming
void startStreaming()
Definition
vpForceTorqueIitSensor.cpp:218
vpForceTorqueIitSensor::m_ftLib
ftSensorLib m_ftLib
Definition
vpForceTorqueIitSensor.h:165
vpForceTorqueIitSensor::bias
void bias()
Definition
vpForceTorqueIitSensor.cpp:159
vpForceTorqueIitSensor::stopStreaming
void stopStreaming()
Definition
vpForceTorqueIitSensor.cpp:231
vpForceTorqueIitSensor::m_warmupMilliseconds
int m_warmupMilliseconds
Definition
vpForceTorqueIitSensor.h:181
vpForceTorqueIitSensor::m_timeCur
std::chrono::time_point< std::chrono::system_clock > m_timeCur
Definition
vpForceTorqueIitSensor.h:178
vpForceTorqueIitSensor::m_acquisitionThread
std::thread m_acquisitionThread
Definition
vpForceTorqueIitSensor.h:177
vpForceTorqueIitSensor::join
void join()
Definition
vpForceTorqueIitSensor.cpp:98
vpForceTorqueIitSensor::m_ft_filt
vpColVector m_ft_filt
Definition
vpForceTorqueIitSensor.h:169
vpForceTorqueIitSensor::m_ftSensorsData
ftSensorsConnected m_ftSensorsData
Definition
vpForceTorqueIitSensor.h:171
vpForceTorqueIitSensor::getForceTorque
vpColVector getForceTorque(bool filtered=false)
Definition
vpForceTorqueIitSensor.cpp:205
vpForceTorqueIitSensor::m_mutex
std::mutex m_mutex
Definition
vpForceTorqueIitSensor.h:180
vpForceTorqueIitSensor::m_acquisitionEnabled
std::atomic< bool > m_acquisitionEnabled
Definition
vpForceTorqueIitSensor.h:173
vpTime::wait
VISP_EXPORT int wait(double t0, double t)
vpTime::sleepMs
VISP_EXPORT void sleepMs(double t)
modules
sensor
src
force-torque
vpForceTorqueIitSensor.cpp
Generated by
1.12.0