Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpForceTorqueAtiSensor.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 <visp3/core/vpConfig.h>
37
38#if defined(VISP_HAVE_ATIDAQ) && defined(VISP_HAVE_COMEDI)
39
40#include <ftconfig.h> // atidaq private library
41
42#include <visp3/core/vpException.h>
43#include <visp3/sensor/vpForceTorqueAtiSensor.h>
44
45static Calibration *s_calibinfo = NULL;
46
51 : m_calibfile(""), m_index(1), m_num_axes(6), m_num_channels(6), m_sample_bias()
52{
53}
54
60{
61 // Open access to device
64}
65
71{
72 open();
73
74 // Get FT from device
76
78 throw vpException(vpException::fatalError, "Physical data size (%d) and number of channels (%d) doesn't match",
80
81 float *sample_bias = new float[m_num_channels];
82 for (unsigned int i = 0; i < m_num_channels; i++)
83 sample_bias[i] = m_sample_bias[i];
84
85 Bias(s_calibinfo, sample_bias);
86
87 delete[] sample_bias;
88}
89
95{
96 open();
97
98 // Get FT from device
100
101 // Reset sample bias
102 m_sample_bias = 0;
103
105 throw vpException(vpException::fatalError, "Physical data size (%d) and number of channels (%d) doesn't match",
107
108 float *sample_bias = new float[m_num_channels];
109 for (unsigned int i = 0; i < m_num_channels; i++)
110 sample_bias[i] = m_sample_bias[i];
111
112 Bias(s_calibinfo, sample_bias);
113
114 delete[] sample_bias;
115}
116
122{
123 if (s_calibinfo != NULL) {
124 // free memory allocated to calibration structure
125 destroyCalibration(s_calibinfo);
126 s_calibinfo = NULL;
127 }
129}
130
140{
142
143 if (phydata.size() != m_num_channels)
144 throw vpException(vpException::fatalError, "Physical data size (%d) and number of channels (%d) doesn't match",
145 phydata.size(), m_num_channels);
146
147 float *voltage = new float[m_num_channels];
148 float *ft = new float[m_num_axes];
149
150 for (unsigned int i = 0; i < m_num_channels; i++) {
151 voltage[i] = phydata[i];
152 }
153
154 // convert a loaded measurement into forces and torques
155 ConvertToFT(s_calibinfo, voltage, ft);
156
157 vpColVector sample(m_num_axes);
158 for (unsigned int i = 0; i < m_num_axes; i++)
159 sample[i] = ft[i];
160
161 delete[] voltage;
162 delete[] ft;
163
164 return sample;
165}
166
171{
172 std::string units(s_calibinfo->ForceUnits);
173 return units;
174}
179{
180 std::string units(s_calibinfo->TorqueUnits);
181 return units;
182}
183
188
195void vpForceTorqueAtiSensor::setCalibrationFile(const std::string &calibfile, unsigned short index)
196{
197 m_calibfile = calibfile;
198 m_index = index;
199
200 if (s_calibinfo)
201 destroyCalibration(s_calibinfo);
202
203 // Create calibration struct
204 s_calibinfo = createCalibration(m_calibfile.c_str(), m_index);
205 if (s_calibinfo == NULL) {
206 throw vpException(vpException::fatalError, "Calibration file %s couldn't be loaded", m_calibfile.c_str());
207 }
208
209 m_num_channels = s_calibinfo->rt.NumChannels;
210 m_num_axes = s_calibinfo->rt.NumAxes;
211}
212
232std::ostream &operator<<(std::ostream &os, const vpForceTorqueAtiSensor &ati)
233{
234 if (s_calibinfo == NULL) {
235 os << "Calibration Information is not available" << std::endl;
236 return os;
237 }
238
239 // display info from calibration file
240 os << "Calibration Information for " << ati.m_calibfile << ", index #" << ati.m_index << ":" << std::endl;
241 os << " Serial: " << s_calibinfo->Serial << std::endl;
242 os << " Body Style: " << s_calibinfo->BodyStyle << std::endl;
243 os << " Calibration: " << s_calibinfo->PartNumber << std::endl;
244 os << " Calibration Date: " << s_calibinfo->CalDate << std::endl;
245 os << " Family: " << s_calibinfo->Family << std::endl;
246 os << " # Channels: " << s_calibinfo->rt.NumChannels << std::endl;
247 os << " # Axes: " << s_calibinfo->rt.NumAxes << std::endl;
248 os << " Force Units: " << s_calibinfo->ForceUnits << std::endl;
249 os << " Torque Units: " << s_calibinfo->TorqueUnits << std::endl;
250 os << "Temperature Compensation: " << (s_calibinfo->TempCompAvailable ? "Yes" : "No") << std::endl;
251
252 // print maximum loads of axes
253 os << "\nRated Loads:" << std::endl;
254 for (unsigned short i = 0; i < s_calibinfo->rt.NumAxes; i++) {
255 char *units;
256 if ((s_calibinfo->AxisNames[i])[0] == 'F') {
257 units = s_calibinfo->ForceUnits;
258 } else
259 units = s_calibinfo->TorqueUnits;
260 os << s_calibinfo->AxisNames[i] << ": " << s_calibinfo->MaxLoads[i] << " " << units << std::endl;
261 }
262
263 // print temperature compensation information, if available
264 if (s_calibinfo->TempCompAvailable) {
265 os << "\nTemperature Compensation Information:" << std::endl;
266 os << "BS: ";
267 for (unsigned short i = 0; i < s_calibinfo->rt.NumChannels - 1; i++) {
268 os << s_calibinfo->rt.bias_slopes[i] << " ";
269 }
270 os << "\nGS: ";
271 for (unsigned short i = 0; i < s_calibinfo->rt.NumChannels - 1; i++) {
272 os << s_calibinfo->rt.gain_slopes[i] << " ";
273 }
274 os << "\nTherm: " << s_calibinfo->rt.thermistor << std::endl;
275 }
276
277 return os;
278}
279
280#elif !defined(VISP_BUILD_SHARED_LIBS)
281// Work around to avoid warning:
282// libvisp_sensor.a(vpForceTorqueAtiSensor.cpp.o) has no symbols
283void dummy_vpForceTorqueAtiSensor(){};
284#endif
unsigned int size() const
Return the number of elements of the 2D array.
Definition vpArray2D.h:292
Implementation of column vector and the associated operations.
void setChannelNumbers(const unsigned int &nchannel)
Definition vpComedi.h:145
vpColVector getPhyData() const
Definition vpComedi.cpp:133
void open()
Definition vpComedi.cpp:63
void close()
Definition vpComedi.cpp:90
error that can be emitted by ViSP classes.
Definition vpException.h:59
@ fatalError
Fatal error.
Definition vpException.h:84
unsigned short m_index
Index of calibration in file (default: 1)
unsigned short m_num_axes
Number of axis or gages available from the sensor.
std::string m_calibfile
ATI calibration file FT*.cal.
vpColVector m_sample_bias
Sample value used for bias.
void setCalibrationFile(const std::string &calibfile, unsigned short index=1)
unsigned short m_num_channels
Number of channels available from the sensor.