Visual Servoing Platform version 3.5.0
vpForceTorqueAtiSensor.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 <visp3/core/vpConfig.h>
40
41#if defined(VISP_HAVE_ATIDAQ) && defined(VISP_HAVE_COMEDI)
42
43#include <ftconfig.h> // atidaq private library
44
45#include <visp3/core/vpException.h>
46#include <visp3/sensor/vpForceTorqueAtiSensor.h>
47
48static Calibration *s_calibinfo = NULL;
49
54 : m_calibfile(""), m_index(1), m_num_axes(6), m_num_channels(6), m_sample_bias()
55{
56}
57
63{
64 // Open access to device
67}
68
74{
75 open();
76
77 // Get FT from device
79
81 throw vpException(vpException::fatalError, "Physical data size (%d) and number of channels (%d) doesn't match",
83
84 float *sample_bias = new float[m_num_channels];
85 for (unsigned int i = 0; i < m_num_channels; i++)
86 sample_bias[i] = m_sample_bias[i];
87
88 Bias(s_calibinfo, sample_bias);
89
90 delete[] sample_bias;
91}
92
98{
99 open();
100
101 // Get FT from device
103
104 // Reset sample bias
105 m_sample_bias = 0;
106
108 throw vpException(vpException::fatalError, "Physical data size (%d) and number of channels (%d) doesn't match",
110
111 float *sample_bias = new float[m_num_channels];
112 for (unsigned int i = 0; i < m_num_channels; i++)
113 sample_bias[i] = m_sample_bias[i];
114
115 Bias(s_calibinfo, sample_bias);
116
117 delete[] sample_bias;
118}
119
125{
126 if (s_calibinfo != NULL) {
127 // free memory allocated to calibration structure
128 destroyCalibration(s_calibinfo);
129 s_calibinfo = NULL;
130 }
132}
133
143{
145
146 if (phydata.size() != m_num_channels)
147 throw vpException(vpException::fatalError, "Physical data size (%d) and number of channels (%d) doesn't match",
148 phydata.size(), m_num_channels);
149
150 float *voltage = new float[m_num_channels];
151 float *ft = new float[m_num_axes];
152
153 for (unsigned int i = 0; i < m_num_channels; i++) {
154 voltage[i] = phydata[i];
155 }
156
157 // convert a loaded measurement into forces and torques
158 ConvertToFT(s_calibinfo, voltage, ft);
159
160 vpColVector sample(m_num_axes);
161 for (unsigned int i = 0; i < m_num_axes; i++)
162 sample[i] = ft[i];
163
164 delete[] voltage;
165 delete[] ft;
166
167 return sample;
168}
169
174{
175 std::string units(s_calibinfo->ForceUnits);
176 return units;
177}
182{
183 std::string units(s_calibinfo->TorqueUnits);
184 return units;
185}
186
191
198void vpForceTorqueAtiSensor::setCalibrationFile(const std::string &calibfile, unsigned short index)
199{
200 m_calibfile = calibfile;
201 m_index = index;
202
203 if (s_calibinfo)
204 destroyCalibration(s_calibinfo);
205
206 char file[FILENAME_MAX];
207 sprintf(file, "%s", m_calibfile.c_str());
208
209 // Create calibration struct
210 s_calibinfo = createCalibration(file, m_index);
211 if (s_calibinfo == NULL) {
212 throw vpException(vpException::fatalError, "Calibration file %s couldn't be loaded", m_calibfile.c_str());
213 }
214
215 m_num_channels = s_calibinfo->rt.NumChannels;
216 m_num_axes = s_calibinfo->rt.NumAxes;
217}
218
238std::ostream &operator<<(std::ostream &os, const vpForceTorqueAtiSensor &ati)
239{
240 if (s_calibinfo == NULL) {
241 os << "Calibration Information is not available" << std::endl;
242 return os;
243 }
244
245 // display info from calibration file
246 os << "Calibration Information for " << ati.m_calibfile << ", index #" << ati.m_index << ":" << std::endl;
247 os << " Serial: " << s_calibinfo->Serial << std::endl;
248 os << " Body Style: " << s_calibinfo->BodyStyle << std::endl;
249 os << " Calibration: " << s_calibinfo->PartNumber << std::endl;
250 os << " Calibration Date: " << s_calibinfo->CalDate << std::endl;
251 os << " Family: " << s_calibinfo->Family << std::endl;
252 os << " # Channels: " << s_calibinfo->rt.NumChannels << std::endl;
253 os << " # Axes: " << s_calibinfo->rt.NumAxes << std::endl;
254 os << " Force Units: " << s_calibinfo->ForceUnits << std::endl;
255 os << " Torque Units: " << s_calibinfo->TorqueUnits << std::endl;
256 os << "Temperature Compensation: " << (s_calibinfo->TempCompAvailable ? "Yes" : "No") << std::endl;
257
258 // print maximum loads of axes
259 os << "\nRated Loads:" << std::endl;
260 for (unsigned short i = 0; i < s_calibinfo->rt.NumAxes; i++) {
261 char *units;
262 if ((s_calibinfo->AxisNames[i])[0] == 'F') {
263 units = s_calibinfo->ForceUnits;
264 } else
265 units = s_calibinfo->TorqueUnits;
266 os << s_calibinfo->AxisNames[i] << ": " << s_calibinfo->MaxLoads[i] << " " << units << std::endl;
267 }
268
269 // print temperature compensation information, if available
270 if (s_calibinfo->TempCompAvailable) {
271 os << "\nTemperature Compensation Information:" << std::endl;
272 os << "BS: ";
273 for (unsigned short i = 0; i < s_calibinfo->rt.NumChannels - 1; i++) {
274 os << s_calibinfo->rt.bias_slopes[i] << " ";
275 }
276 os << "\nGS: ";
277 for (unsigned short i = 0; i < s_calibinfo->rt.NumChannels - 1; i++) {
278 os << s_calibinfo->rt.gain_slopes[i] << " ";
279 }
280 os << "\nTherm: " << s_calibinfo->rt.thermistor << std::endl;
281 }
282
283 return os;
284}
285
286#elif !defined(VISP_BUILD_SHARED_LIBS)
287// Work arround to avoid warning:
288// libvisp_sensor.a(vpForceTorqueAtiSensor.cpp.o) has no symbols
289void dummy_vpForceTorqueAtiSensor(){};
290#endif
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:493
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
void setChannelNumbers(const unsigned int &nchannel)
Definition: vpComedi.h:148
vpColVector getPhyData() const
Definition: vpComedi.cpp:136
void open()
Definition: vpComedi.cpp:66
void close()
Definition: vpComedi.cpp:93
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ fatalError
Fatal error.
Definition: vpException.h:96
unsigned short m_index
Index of calibration in file (default: 1)
std::string getForceUnits() const
vpColVector getForceTorque() const
unsigned short m_num_axes
Number of axis or gages available from the sensor.
std::string getTorqueUnits() const
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.