Visual Servoing Platform version 3.5.0
vpForceTorqueAtiNetFTSensor.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 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 http://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 * Authors:
35 * Fabien Spindler
36 *
37 *****************************************************************************/
38
39#include <stdint.h>
40
41#include <visp3/core/vpConfig.h>
42#include <visp3/core/vpException.h>
43#include <visp3/core/vpTime.h>
44#include <visp3/sensor/vpForceTorqueAtiNetFTSensor.h>
45
46// Make vpForceTorqueAtiNetFTSensor available only if inet_ntop() used to
47// communicate by UDP with the sensor through vpUDPClient is available; inet_ntop()
48// is not supported on win XP
49#ifdef VISP_HAVE_FUNC_INET_NTOP
50
51#ifndef DOXYGEN_SHOULD_SKIP_THIS
52typedef struct response_struct {
53 uint32_t rdt_sequence;
54 uint32_t ft_sequence;
55 uint32_t status;
56 int32_t FTData[6];
57} RESPONSE;
58#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
59
67 : vpUDPClient(), m_counts_per_force(1000000), m_counts_per_torque(1000000000), m_scaling_factor(1), m_ft_bias(6, 0),
68 m_data_count(0), m_data_count_prev(0), m_ft(6, 0), m_is_streaming_started(false)
69{
70}
71
78 : vpUDPClient(hostname, port), m_counts_per_force(1000000), m_counts_per_torque(1000000000), m_scaling_factor(1),
79 m_ft_bias(6, 0), m_data_count(0), m_data_count_prev(0), m_ft(6, 0), m_is_streaming_started(false)
80{
81}
82
88{
89 if (!m_is_init) {
90 throw(vpException(vpException::notInitialized, "Cannot start streaming: UDP client is not initialized"));
91 }
92
94 throw(vpException(vpException::notInitialized, "Streaming is already started"));
95 }
96
97 // Since UDP packet could be lost, retry startup 10 times before giving up
98 for (unsigned int i = 0; i < 10; ++i) {
99 int len = 8;
100 unsigned char request[8]; // The request data sent to the Net F/T
101 *(uint16_t *)&request[0] = htons(0x1234); // Standard header
102 *(uint16_t *)&request[2] = htons(0x0002); // Start high-speed streaming (see table 10.1 in Net F/T user manual)
103 *(uint32_t *)&request[4] = htonl(0); // Infinite sample (see section 10.1 in Net F/T user manual
104
105 // Send start stream
106 if (send(request, len) != len) {
107 throw(vpException(vpException::notInitialized, "UDP client is not initialized"));
108 }
109 std::cout << "wait: " << i << std::endl;
110
112 if (waitForNewData()) {
113 return true;
114 }
115 }
117 return false;
118}
119
124{
125 if (!m_is_init) {
126 throw(vpException(vpException::notInitialized, "Cannot stop streaming: UDP client is not initialized"));
127 }
128
130 throw(vpException(vpException::notInitialized, "Cannot stop streaming: streaming was not started"));
131 }
132
133 int len = 8;
134 unsigned char request[8]; // The request data sent to the Net F/T
135 *(uint16_t *)&request[0] = htons(0x1234); // Standard header
136 *(uint16_t *)&request[2] = htons(0x0000); // Stop streaming (see table 10.1 in Net F/T user manual)
137 *(uint32_t *)&request[4] = htonl(0); // Infinite sample (see section 10.1 in Net F/T user manual
138
139 // Send start stream
140 if (send(request, len) != len) {
141 throw(vpException(vpException::notInitialized, "Cannot stop streaming"));
142 }
143
145}
146
153{
156 }
157}
158
166void vpForceTorqueAtiNetFTSensor::bias(unsigned int n_counts)
167{
168 if (!m_is_init) {
169 throw(vpException(vpException::notInitialized, "Cannot bias: UDP client is not initialized"));
170 }
171
173 throw(vpException(vpException::notInitialized, "Cannot bias: streaming was not started"));
174 }
175
176 vpColVector ft_bias_tmp(6, 0);
177 m_ft_bias = 0;
178
179 if (n_counts == 0) {
181 } else {
182 for (unsigned int i = 0; i < n_counts; i++) {
183 ft_bias_tmp += getForceTorque();
185 }
186 m_ft_bias = ft_bias_tmp / n_counts;
187 }
188}
189
196
208{
209 if (!m_is_init) {
210 throw(vpException(vpException::notInitialized, "Cannot get F/T: UDP client is not initialized"));
211 }
212
214 throw(vpException(vpException::notInitialized, "Cannot get F/T: streaming was not started"));
215 }
216
218 throw(vpException(vpException::notInitialized, "Cannot get F/T: no new data available"));
219 }
220
221 return m_ft;
222}
223
230{
231 if (!m_is_init) {
232 throw(vpException(vpException::notInitialized, "Cannot wait for new data: UDP client is not initialized"));
233 }
234
236 throw(vpException(vpException::notInitialized, "Cannot wait for new data: streaming was not started"));
237 }
238
239 double t = vpTime::measureTimeMs();
241 while (vpTime::measureTimeMs() - t < static_cast<double>(timeout)) {
242 unsigned char response[36];
243 RESPONSE resp;
244 if (receive((void *)response, 36)) {
245 resp.rdt_sequence = ntohl(*(uint32_t *)&response[0]);
246 resp.ft_sequence = ntohl(*(uint32_t *)&response[4]);
247 resp.status = ntohl(*(uint32_t *)&response[8]);
248 for (int i = 0; i < 6; i++) {
249 resp.FTData[i] = ntohl(*(int32_t *)&response[12 + i * 4]);
250 }
251 // Output the response data.
252 if (resp.status) {
253 throw(vpException(vpException::notInitialized, "Cannot wait for new data: status 0x%08x is not 0x00000000",
254 resp.status));
255 }
256 double force_factor = static_cast<double>(m_scaling_factor) / static_cast<double>(m_counts_per_force);
257 double torque_factor = static_cast<double>(m_scaling_factor) / static_cast<double>(m_counts_per_torque);
258 for (int i = 0; i < 3; i++) {
259 m_ft[i] = resp.FTData[i] * force_factor;
260 }
261 for (int i = 3; i < 6; i++) {
262 m_ft[i] = resp.FTData[i] * torque_factor;
263 }
264 // Consider bias
265 m_ft -= m_ft_bias;
266
267 m_data_count++;
268 }
269
271 return true;
272 }
274 }
275
276 return false;
277}
278
279#endif
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ notInitialized
Used to indicate that a parameter is not initialized.
Definition: vpException.h:98
void bias(unsigned int n_counts=50)
bool waitForNewData(unsigned int timeout=50)
This class implements a basic (IPv4) User Datagram Protocol (UDP) client.
Definition: vpUDPClient.h:169
int receive(std::string &msg, int timeoutMs=0)
bool m_is_init
Definition: vpUDPClient.h:186
int send(const std::string &msg)
VISP_EXPORT void sleepMs(double t)
VISP_EXPORT double measureTimeMs()