Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
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 
48 static 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  }
131  vpComedi::close();
132 }
133 
143 {
144  vpColVector phydata = vpComedi::getPhyData();
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 
198 void 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 
238 std::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
289 void dummy_vpForceTorqueAtiSensor(){};
290 #endif
void open()
Definition: vpComedi.cpp:66
void setChannelNumbers(const unsigned int &nchannel)
Definition: vpComedi.h:148
unsigned short m_index
Index of calibration in file (default: 1)
vpColVector getForceTorque() const
error that can be emited by ViSP classes.
Definition: vpException.h:71
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:158
std::string m_calibfile
ATI calibration file FT*.cal.
vpColVector m_sample_bias
Sample value used for bias.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
std::string getForceUnits() const
void setCalibrationFile(const std::string &calibfile, unsigned short index=1)
unsigned short m_num_axes
Number of axis or gages available from the sensor.
unsigned short m_num_channels
Number of channels available from the sensor.
vpColVector getPhyData() const
Definition: vpComedi.cpp:136
std::string getTorqueUnits() const
void close()
Definition: vpComedi.cpp:93
friend VISP_EXPORT std::ostream & operator<<(std::ostream &os, const vpForceTorqueAtiSensor &ati)