Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpForceTorqueAtiNetFTSensor.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 * ATI Force torque interface.
33 *
34*****************************************************************************/
35
36#include <stdint.h>
37
38#include <visp3/core/vpConfig.h>
39#include <visp3/core/vpException.h>
40#include <visp3/core/vpTime.h>
41#include <visp3/sensor/vpForceTorqueAtiNetFTSensor.h>
42
43// Make vpForceTorqueAtiNetFTSensor available only if inet_ntop() used to
44// communicate by UDP with the sensor through vpUDPClient is available; inet_ntop()
45// is not supported on win XP
46#ifdef VISP_HAVE_FUNC_INET_NTOP
47
48#ifndef DOXYGEN_SHOULD_SKIP_THIS
49typedef struct response_struct {
50 uint32_t rdt_sequence;
51 uint32_t ft_sequence;
52 uint32_t status;
53 int32_t FTData[6];
54} RESPONSE;
55#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
56
64 : vpUDPClient(), m_counts_per_force(1000000), m_counts_per_torque(1000000000), m_scaling_factor(1), m_ft_bias(6, 0),
65 m_data_count(0), m_data_count_prev(0), m_ft(6, 0), m_is_streaming_started(false)
66{
67}
68
75 : vpUDPClient(hostname, port), m_counts_per_force(1000000), m_counts_per_torque(1000000000), m_scaling_factor(1),
76 m_ft_bias(6, 0), m_data_count(0), m_data_count_prev(0), m_ft(6, 0), m_is_streaming_started(false)
77{
78}
79
85{
86 if (!m_is_init) {
87 throw(vpException(vpException::notInitialized, "Cannot start streaming: UDP client is not initialized"));
88 }
89
91 throw(vpException(vpException::notInitialized, "Streaming is already started"));
92 }
93
94 // Since UDP packet could be lost, retry startup 10 times before giving up
95 for (unsigned int i = 0; i < 10; ++i) {
96 int len = 8;
97 unsigned char request[8]; // The request data sent to the Net F/T
98 *(uint16_t *)&request[0] = htons(0x1234); // Standard header
99 *(uint16_t *)&request[2] = htons(0x0002); // Start high-speed streaming (see table 10.1 in Net F/T user manual)
100 *(uint32_t *)&request[4] = htonl(0); // Infinite sample (see section 10.1 in Net F/T user manual
101
102 // Send start stream
103 if (send(request, len) != len) {
104 throw(vpException(vpException::notInitialized, "UDP client is not initialized"));
105 }
106 std::cout << "wait: " << i << std::endl;
107
109 if (waitForNewData()) {
110 return true;
111 }
112 }
114 return false;
115}
116
121{
122 if (!m_is_init) {
123 throw(vpException(vpException::notInitialized, "Cannot stop streaming: UDP client is not initialized"));
124 }
125
127 throw(vpException(vpException::notInitialized, "Cannot stop streaming: streaming was not started"));
128 }
129
130 int len = 8;
131 unsigned char request[8]; // The request data sent to the Net F/T
132 *(uint16_t *)&request[0] = htons(0x1234); // Standard header
133 *(uint16_t *)&request[2] = htons(0x0000); // Stop streaming (see table 10.1 in Net F/T user manual)
134 *(uint32_t *)&request[4] = htonl(0); // Infinite sample (see section 10.1 in Net F/T user manual
135
136 // Send start stream
137 if (send(request, len) != len) {
138 throw(vpException(vpException::notInitialized, "Cannot stop streaming"));
139 }
140
142}
143
155
163void vpForceTorqueAtiNetFTSensor::bias(unsigned int n_counts)
164{
165 if (!m_is_init) {
166 throw(vpException(vpException::notInitialized, "Cannot bias: UDP client is not initialized"));
167 }
168
170 throw(vpException(vpException::notInitialized, "Cannot bias: streaming was not started"));
171 }
172
173 vpColVector ft_bias_tmp(6, 0);
174 m_ft_bias = 0;
175
176 if (n_counts == 0) {
178 } else {
179 for (unsigned int i = 0; i < n_counts; i++) {
180 ft_bias_tmp += getForceTorque();
182 }
183 m_ft_bias = ft_bias_tmp / n_counts;
184 }
185}
186
193
205{
206 if (!m_is_init) {
207 throw(vpException(vpException::notInitialized, "Cannot get F/T: UDP client is not initialized"));
208 }
209
211 throw(vpException(vpException::notInitialized, "Cannot get F/T: streaming was not started"));
212 }
213
215 throw(vpException(vpException::notInitialized, "Cannot get F/T: no new data available"));
216 }
217
218 return m_ft;
219}
220
227{
228 if (!m_is_init) {
229 throw(vpException(vpException::notInitialized, "Cannot wait for new data: UDP client is not initialized"));
230 }
231
233 throw(vpException(vpException::notInitialized, "Cannot wait for new data: streaming was not started"));
234 }
235
236 double t = vpTime::measureTimeMs();
238 while (vpTime::measureTimeMs() - t < static_cast<double>(timeout)) {
239 unsigned char response[36];
240 RESPONSE resp;
241 if (receive((void *)response, 36)) {
242 resp.rdt_sequence = ntohl(*(uint32_t *)&response[0]);
243 resp.ft_sequence = ntohl(*(uint32_t *)&response[4]);
244 resp.status = ntohl(*(uint32_t *)&response[8]);
245 for (int i = 0; i < 6; i++) {
246 resp.FTData[i] = ntohl(*(int32_t *)&response[12 + i * 4]);
247 }
248 // Output the response data.
249 if (resp.status) {
250 throw(vpException(vpException::notInitialized, "Cannot wait for new data: status 0x%08x is not 0x00000000",
251 resp.status));
252 }
253 double force_factor = static_cast<double>(m_scaling_factor) / static_cast<double>(m_counts_per_force);
254 double torque_factor = static_cast<double>(m_scaling_factor) / static_cast<double>(m_counts_per_torque);
255 for (int i = 0; i < 3; i++) {
256 m_ft[i] = resp.FTData[i] * force_factor;
257 }
258 for (int i = 3; i < 6; i++) {
259 m_ft[i] = resp.FTData[i] * torque_factor;
260 }
261 // Consider bias
262 m_ft -= m_ft_bias;
263
264 m_data_count++;
265 }
266
268 return true;
269 }
271 }
272
273 return false;
274}
275
276#endif
Implementation of column vector and the associated operations.
error that can be emitted by ViSP classes.
Definition vpException.h:59
@ notInitialized
Used to indicate that a parameter is not initialized.
Definition vpException.h:86
void bias(unsigned int n_counts=50)
bool waitForNewData(unsigned int timeout=50)
This class implements a basic (IPv4) User Datagram Protocol (UDP) client.
int receive(std::string &msg, int timeoutMs=0)
int send(const std::string &msg)
VISP_EXPORT double measureTimeMs()
VISP_EXPORT void sleepMs(double t)