wg06.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 #ifndef ETHERCAT_HARDWARE_WG06_H
36 #define ETHERCAT_HARDWARE_WG06_H
37 
39 
41 
42 #include <pr2_msgs/PressureState.h>
43 #include <pr2_msgs/AccelerometerState.h>
44 #include <ethercat_hardware/RawFTData.h>
45 #include <geometry_msgs/WrenchStamped.h>
46 
48 {
49  uint8_t mode_;
50  uint8_t digital_out_;
54  uint32_t timestamp_;
55  int32_t encoder_count_;
56  int32_t encoder_index_pos_;
58  uint8_t encoder_status_;
59  uint8_t unused1;
60  int32_t unused2;
61  int32_t unused3;
62  uint16_t board_temperature_;
63  uint16_t bridge_temperature_;
64  uint16_t supply_voltage_;
65  int16_t motor_voltage_;
66  uint16_t packet_count_;
67  uint8_t pad_;
68  uint8_t accel_count_;
69  uint32_t accel_[4];
70  uint8_t checksum_;
71 
72  static const unsigned SIZE=61;
73 }__attribute__ ((__packed__));
74 
75 
77 {
78  int16_t data_[6];
79  uint16_t vhalf_;
80  uint8_t sample_count_;
81  uint8_t timestamp_;
82  static const unsigned SIZE=16;
83 }__attribute__ ((__packed__));
84 
85 
87 {
88 public:
90 
91  const double &calibration_coeff(unsigned row, unsigned col) const {return calibration_coeff_[row*6 + col];}
92  double &calibration_coeff(unsigned row, unsigned col) {return calibration_coeff_[row*6 + col];}
93 
94  const double &offset(unsigned ch_num) const {return offsets_[ch_num];}
95  double &offset(unsigned ch_num) {return offsets_[ch_num];}
96 
97  const double &gain(unsigned ch_num) const {return gains_[ch_num];}
98  double &gain(unsigned ch_num) {return gains_[ch_num];}
99 
100  void print() const;
101 
103  bool getDoubleArray(XmlRpc::XmlRpcValue params, const char* name, double *results, unsigned len);
104  double calibration_coeff_[36];
105  double offsets_[6];
106  double gains_[6];
107 };
108 
109 
111 {
112  uint8_t mode_;
113  uint8_t digital_out_;
117  uint32_t timestamp_;
118  int32_t encoder_count_;
122  uint8_t unused1;
123  int32_t unused2;
124  int32_t unused3;
127  uint16_t supply_voltage_;
128  int16_t motor_voltage_;
129  uint16_t packet_count_;
130  uint8_t pad_;
131  uint8_t accel_count_;
132  uint32_t accel_[4];
133  uint8_t unused4[3];
136  uint8_t checksum_;
137 
138  static const unsigned SIZE=129;
139 }__attribute__ ((__packed__));
140 
141 
143 {
144  uint32_t timestamp_;
145  uint16_t l_finger_tip_[22];
146  uint16_t r_finger_tip_[22];
147  uint8_t pad_;
148  uint8_t checksum_;
149  static const unsigned SIZE=94;
150 } __attribute__((__packed__));
151 
152 
154 {
156  uint8_t pad_[418];
157  uint8_t checksum_;
158  static const unsigned SIZE=513;
159 } __attribute__((__packed__));
160 
161 
162 
163 class WG06 : public WG0X
164 {
165 public:
166  WG06();
167  ~WG06();
168  int initialize(pr2_hardware_interface::HardwareInterface *, bool allow_unprogrammed=true);
169  void construct(EtherCAT_SlaveHandler *sh, int &start_address);
170  void packCommand(unsigned char *buffer, bool halt, bool reset);
171  bool unpackState(unsigned char *this_buffer, unsigned char *prev_buffer);
172 
173  virtual void multiDiagnostics(vector<diagnostic_msgs::DiagnosticStatus> &vec, unsigned char *buffer);
174  enum
175  {
176  PRODUCT_CODE = 6805006
177  };
178 private:
179 
180  static const unsigned PRESSURE_PHY_ADDR = 0x2200;
181  static const unsigned BIG_PRESSURE_PHY_ADDR = 0x2600;
182 
185 
190 
191  bool unpackPressure(unsigned char* pressure_buf);
194 
196  void diagnosticsAccel(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *buffer);
199 
202 
205  unsigned pressure_size_;
206 
211 
212  static const unsigned NUM_PRESSURE_REGIONS = 22;
216 
217  void convertFTDataSampleToWrench(const FTDataSample &sample, geometry_msgs::Wrench &wrench);
218  static const unsigned MAX_FT_SAMPLES = 4;
219  static const unsigned NUM_FT_CHANNELS = 6;
220  static const int FT_VHALF_IDEAL = 32768;
221  static const int FT_VHALF_RANGE = 300;
227  uint64_t ft_sample_count_;
235 
239  //pr2_hardware_interface::AnalogIn ft_analog_in_; //!< Provides
241 
244 
252 };
253 
254 #endif /* ETHERCAT_HARDWARE_WG06_H */
WG06::diagnosticsPressure
void diagnosticsPressure(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *buffer)
Definition: wg06.cpp:891
WG06::PRODUCT_CODE
@ PRODUCT_CODE
Definition: wg06.h:176
WG06::last_pressure_time_
uint32_t last_pressure_time_
Definition: wg06.h:213
WG06::initialize
int initialize(pr2_hardware_interface::HardwareInterface *, bool allow_unprogrammed=true)
Definition: wg06.cpp:203
WG06::WG06
WG06()
Definition: wg06.cpp:56
WG06::unpackPressure
bool unpackPressure(unsigned char *pressure_buf)
Unpack pressure sensor samples from realtime data.
Definition: wg06.cpp:522
FTParamsInternal::getRosParams
bool getRosParams(ros::NodeHandle nh)
Grabs ft rosparams from a given node hande namespace.
Definition: wg06.cpp:1158
WG06StatusWithAccelAndFT::timestamp_
uint32_t timestamp_
Definition: wg06.h:117
WG06StatusWithAccelAndFT::digital_out_
uint8_t digital_out_
Definition: wg06.h:113
WG06::enable_ft_sensor_
bool enable_ft_sensor_
Definition: wg06.h:243
WG06::FT_VHALF_IDEAL
static const int FT_VHALF_IDEAL
Vhalf ADC measurement is ideally about (1<<16)/2.
Definition: wg06.h:220
WG06::pressure_checksum_error_
bool pressure_checksum_error_
Set true where checksum error on pressure data is detected, cleared on reset.
Definition: wg06.h:203
WG06StatusWithAccel::mode_
uint8_t mode_
Definition: wg06.h:81
WG06::first_publish_
bool first_publish_
Definition: wg06.h:210
pr2_hardware_interface::Accelerometer
WG06StatusWithAccel::unused1
uint8_t unused1
Definition: wg06.h:91
WG06::raw_ft_publisher_
realtime_tools::RealtimePublisher< ethercat_hardware::RawFTData > * raw_ft_publisher_
Realtime Publisher of RAW F/T data.
Definition: wg06.h:237
FTParamsInternal::gain
const double & gain(unsigned ch_num) const
Definition: wg06.h:97
WG06StatusWithAccelAndFT::mode_
uint8_t mode_
Definition: wg06.h:112
WG06Pressure
Definition: wg06.h:142
WG06StatusWithAccel::accel_
uint32_t accel_[4]
Definition: wg06.h:101
WG06::unpackAccel
bool unpackAccel(WG06StatusWithAccel *status, WG06StatusWithAccel *last_status)
Unpack 3-axis accelerometer samples from realtime data.
Definition: wg06.cpp:587
WG06StatusWithAccel::programmed_current_
int16_t programmed_current_
Definition: wg06.h:84
WG06StatusWithAccel::unused3
int32_t unused3
Definition: wg06.h:93
FTDataSample::SIZE
static const unsigned SIZE
Definition: wg06.h:82
FTParamsInternal::offset
double & offset(unsigned ch_num)
Definition: wg06.h:95
WG06StatusWithAccel::programmed_pwm_value_
int16_t programmed_pwm_value_
Definition: wg06.h:83
WG06Pressure::r_finger_tip_
uint16_t r_finger_tip_[22]
Definition: wg06.h:146
WG06::pressure_size_
unsigned pressure_size_
Size in bytes of pressure data region.
Definition: wg06.h:205
WG06Pressure::l_finger_tip_
uint16_t l_finger_tip_[22]
Definition: wg06.h:145
WG06::ft_overload_limit_
int ft_overload_limit_
Limit on raw range of F/T input.
Definition: wg06.h:222
WG06::ft_vhalf_error_
bool ft_vhalf_error_
error with Vhalf reference voltage
Definition: wg06.h:225
WG06StatusWithAccel::measured_current_
int16_t measured_current_
Definition: wg06.h:85
FTParamsInternal::offset
const double & offset(unsigned ch_num) const
Definition: wg06.h:94
WG06::ft_params_
FTParamsInternal ft_params_
Definition: wg06.h:240
WG06BigPressure::checksum_
uint8_t checksum_
Definition: wg06.h:157
WG06::unpackState
bool unpackState(unsigned char *this_buffer, unsigned char *prev_buffer)
Definition: wg06.cpp:461
WG06StatusWithAccelAndFT::unused1
uint8_t unused1
Definition: wg06.h:122
WG06StatusWithAccelAndFT::board_temperature_
uint16_t board_temperature_
Definition: wg06.h:125
__attribute__
class FTParamsInternal __attribute__
WG06StatusWithAccelAndFT::bridge_temperature_
uint16_t bridge_temperature_
Definition: wg06.h:126
WG06BigPressure::SIZE
static const unsigned SIZE
Definition: wg06.h:158
WG06::force_torque_
pr2_hardware_interface::ForceTorque force_torque_
Provides F/T data to controllers.
Definition: wg06.h:234
WG06StatusWithAccelAndFT::ft_sample_count_
uint8_t ft_sample_count_
Definition: wg06.h:134
WG06::multiDiagnostics
virtual void multiDiagnostics(vector< diagnostic_msgs::DiagnosticStatus > &vec, unsigned char *buffer)
For EtherCAT devices that publish more than one EtherCAT Status message. If sub-class implements mult...
Definition: wg06.cpp:808
WG06::accelerometer_missed_samples_
unsigned accelerometer_missed_samples_
Total of accelerometer samples that were missed.
Definition: wg06.h:208
WG06StatusWithAccel::checksum_
uint8_t checksum_
Definition: wg06.h:102
WG06::accel_publisher_
realtime_tools::RealtimePublisher< pr2_msgs::AccelerometerState > * accel_publisher_
Definition: wg06.h:215
WG06::construct
void construct(EtherCAT_SlaveHandler *sh, int &start_address)
< Construct EtherCAT device
Definition: wg06.cpp:90
pr2_hardware_interface::HardwareInterface
WG06StatusWithAccelAndFT::unused2
int32_t unused2
Definition: wg06.h:123
WG06StatusWithAccel::encoder_count_
int32_t encoder_count_
Definition: wg06.h:87
ethercat_hardware::WGSoftProcessor
Definition: wg_soft_processor.h:89
WG06Pressure::SIZE
static const unsigned SIZE
Definition: wg06.h:149
WG06StatusWithAccel::pad_
uint8_t pad_
Definition: wg06.h:99
FTDataSample::data_
int16_t data_[6]
Definition: wg06.h:78
WG06::accelerometer_
pr2_hardware_interface::Accelerometer accelerometer_
Definition: wg06.h:184
WG06StatusWithAccel::SIZE
static const unsigned SIZE
Definition: wg06.h:104
WG06::enable_pressure_sensor_
bool enable_pressure_sensor_
Definition: wg06.h:242
WG0X
Definition: wg0x.h:234
FTParamsInternal::gains_
double gains_[6]
Definition: wg06.h:106
WG06StatusWithAccelAndFT::encoder_status_
uint8_t encoder_status_
Definition: wg06.h:121
WG06Pressure::timestamp_
uint32_t timestamp_
Definition: wg06.h:144
WG06::unpackFT
bool unpackFT(WG06StatusWithAccelAndFT *status, WG06StatusWithAccelAndFT *last_status)
Unpack force/torque ADC samples from realtime data.
Definition: wg06.cpp:707
realtime_tools::RealtimePublisher< pr2_msgs::PressureState >
FTParamsInternal::offsets_
double offsets_[6]
Definition: wg06.h:105
WG06StatusWithAccel::packet_count_
uint16_t packet_count_
Definition: wg06.h:98
WG06::diagnosticsAccel
void diagnosticsAccel(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *buffer)
Definition: wg06.cpp:831
wg_soft_processor.h
WG06::pressure_checksum_error_count_
unsigned pressure_checksum_error_count_
debugging
Definition: wg06.h:204
WG06::initializeFT
bool initializeFT(pr2_hardware_interface::HardwareInterface *hw)
Definition: wg06.cpp:344
WG06::ft_missed_samples_
uint64_t ft_missed_samples_
Counts number of ft sensor samples that were missed.
Definition: wg06.h:228
WG06StatusWithAccelAndFT::unused3
int32_t unused3
Definition: wg06.h:124
WG06::PRESSURE_PHY_ADDR
static const unsigned PRESSURE_PHY_ADDR
Definition: wg06.h:180
WG06::MAX_FT_SAMPLES
static const unsigned MAX_FT_SAMPLES
Definition: wg06.h:218
WG06StatusWithAccelAndFT::num_encoder_errors_
uint16_t num_encoder_errors_
Definition: wg06.h:120
WG06StatusWithAccel::accel_count_
uint8_t accel_count_
Definition: wg06.h:100
WG06::ft_raw_analog_in_
pr2_hardware_interface::AnalogIn ft_raw_analog_in_
Definition: wg06.h:230
WG06StatusWithAccelAndFT::accel_count_
uint8_t accel_count_
Definition: wg06.h:131
WG06StatusWithAccelAndFT::encoder_count_
int32_t encoder_count_
Definition: wg06.h:118
WG06StatusWithAccelAndFT::motor_voltage_
int16_t motor_voltage_
Definition: wg06.h:128
FTParamsInternal::calibration_coeff
const double & calibration_coeff(unsigned row, unsigned col) const
Definition: wg06.h:91
WG06StatusWithAccel::timestamp_
uint32_t timestamp_
Definition: wg06.h:86
WG06::BIG_PRESSURE_PHY_ADDR
static const unsigned BIG_PRESSURE_PHY_ADDR
Definition: wg06.h:181
WG06::diagnosticsFT
void diagnosticsFT(diagnostic_updater::DiagnosticStatusWrapper &d, WG06StatusWithAccelAndFT *status)
Definition: wg06.cpp:1002
WG06
Definition: wg06.h:163
WG06::packCommand
void packCommand(unsigned char *buffer, bool halt, bool reset)
Definition: wg06.cpp:431
WG06::diag_last_ft_sample_count_
uint64_t diag_last_ft_sample_count_
F/T Sample count last time diagnostics was published.
Definition: wg06.h:229
WG06::ft_sample_count_
uint64_t ft_sample_count_
Counts number of ft sensor samples.
Definition: wg06.h:227
WG06StatusWithAccelAndFT::programmed_current_
int16_t programmed_current_
Definition: wg06.h:115
WG06StatusWithAccel::num_encoder_errors_
uint16_t num_encoder_errors_
Definition: wg06.h:89
WG06::diagnosticsWG06
void diagnosticsWG06(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *)
Definition: wg06.cpp:885
WG06::ft_sampling_rate_error_
bool ft_sampling_rate_error_
True if FT sampling rate was incorrect.
Definition: wg06.h:226
WG06Pressure::checksum_
uint8_t checksum_
Definition: wg06.h:148
WG06::convertFTDataSampleToWrench
void convertFTDataSampleToWrench(const FTDataSample &sample, geometry_msgs::Wrench &wrench)
Convert FTDataSample to Wrench using gain, offset, and coefficient matrix.
Definition: wg06.cpp:646
FTParamsInternal
Definition: wg06.h:86
WG06::NUM_FT_CHANNELS
static const unsigned NUM_FT_CHANNELS
Definition: wg06.h:219
WG06StatusWithAccel
Definition: wg06.h:47
WG06StatusWithAccelAndFT::ft_samples_
FTDataSample ft_samples_[4]
Definition: wg06.h:135
WG06::initializeAccel
bool initializeAccel(pr2_hardware_interface::HardwareInterface *hw)
Definition: wg06.cpp:324
pr2_hardware_interface::ForceTorque
WG06StatusWithAccel::bridge_temperature_
uint16_t bridge_temperature_
Definition: wg06.h:95
WG06::ft_disconnected_
bool ft_disconnected_
f/t sensor may be disconnected
Definition: wg06.h:224
FTDataSample::sample_count_
uint8_t sample_count_
Definition: wg06.h:80
FTDataSample::vhalf_
uint16_t vhalf_
Definition: wg06.h:79
WG06Pressure::pad_
uint8_t pad_
Definition: wg06.h:147
WG06StatusWithAccelAndFT::measured_current_
int16_t measured_current_
Definition: wg06.h:116
FTDataSample::timestamp_
uint8_t timestamp_
Definition: wg06.h:81
WG06::initializePressure
bool initializePressure(pr2_hardware_interface::HardwareInterface *hw)
Definition: wg06.cpp:300
WG06StatusWithAccelAndFT::unused4
uint8_t unused4[3]
Definition: wg06.h:133
WG06StatusWithAccel::motor_voltage_
int16_t motor_voltage_
Definition: wg06.h:97
WG06StatusWithAccelAndFT::encoder_index_pos_
int32_t encoder_index_pos_
Definition: wg06.h:119
WG06::ft_analog_in_
pr2_hardware_interface::AnalogIn ft_analog_in_
Provides F/T data to controllers (deprecated, use pr2_hardware_interface::ForceTorque instead)
Definition: wg06.h:232
WG06::FT_VHALF_RANGE
static const int FT_VHALF_RANGE
allow vhalf to range +/- 300 from ideal
Definition: wg06.h:221
status
uint16_t status
Definition: ethercat_device.h:2
pr2_hardware_interface::PressureSensor
wg0x.h
ros::Time
WG06StatusWithAccelAndFT::SIZE
static const unsigned SIZE
Definition: wg06.h:138
WG06StatusWithAccelAndFT::pad_
uint8_t pad_
Definition: wg06.h:130
WG06::has_accel_and_ft_
bool has_accel_and_ft_
True if device has accelerometer and force/torque sensor.
Definition: wg06.h:201
WG06::ft_publisher_
realtime_tools::RealtimePublisher< geometry_msgs::WrenchStamped > * ft_publisher_
Definition: wg06.h:238
WG06BigPressure::pad_
uint8_t pad_[418]
Definition: wg06.h:156
FTParamsInternal::FTParamsInternal
FTParamsInternal()
Definition: wg06.cpp:1077
WG06StatusWithAccelAndFT
Definition: wg06.h:110
WG06StatusWithAccelAndFT::supply_voltage_
uint16_t supply_voltage_
Definition: wg06.h:127
diagnostic_updater::DiagnosticStatusWrapper
pr2_hardware_interface::AnalogIn
WG06::soft_processor_
WGSoftProcessor soft_processor_
Definition: wg06.h:251
WG06StatusWithAccelAndFT::accel_
uint32_t accel_[4]
Definition: wg06.h:132
WG06StatusWithAccel::encoder_index_pos_
int32_t encoder_index_pos_
Definition: wg06.h:88
WG06::last_publish_time_
ros::Time last_publish_time_
Time diagnostics was last published.
Definition: wg06.h:209
WG06::pressure_sensors_
pr2_hardware_interface::PressureSensor pressure_sensors_[2]
Definition: wg06.h:183
WG06::NUM_PRESSURE_REGIONS
static const unsigned NUM_PRESSURE_REGIONS
Definition: wg06.h:212
WG06::initializeSoftProcessor
bool initializeSoftProcessor()
Definition: wg06.cpp:410
WG06StatusWithAccel::encoder_status_
uint8_t encoder_status_
Definition: wg06.h:90
WG06::ft_overload_flags_
uint8_t ft_overload_flags_
Bits 0-5 set to true if raw FT input goes beyond limit.
Definition: wg06.h:223
WG06::pressure_publisher_
realtime_tools::RealtimePublisher< pr2_msgs::PressureState > * pressure_publisher_
Definition: wg06.h:214
WG06StatusWithAccelAndFT::packet_count_
uint16_t packet_count_
Definition: wg06.h:129
FTParamsInternal::calibration_coeff_
double calibration_coeff_[36]
Definition: wg06.h:104
WG06StatusWithAccel::digital_out_
uint8_t digital_out_
Definition: wg06.h:82
WG06::~WG06
~WG06()
Definition: wg06.cpp:84
WG06::accelerometer_samples_
unsigned accelerometer_samples_
Number of accelerometer samples since last publish cycle.
Definition: wg06.h:207
WG06BigPressure
Definition: wg06.h:153
WG06StatusWithAccel::supply_voltage_
uint16_t supply_voltage_
Definition: wg06.h:96
WG06BigPressure::pressure_
WG06Pressure pressure_
Definition: wg06.h:155
FTParamsInternal::getDoubleArray
bool getDoubleArray(XmlRpc::XmlRpcValue params, const char *name, double *results, unsigned len)
Definition: wg06.cpp:1115
FTParamsInternal::print
void print() const
Definition: wg06.cpp:1094
WG06StatusWithAccelAndFT::programmed_pwm_value_
int16_t programmed_pwm_value_
Definition: wg06.h:114
WG06::enable_soft_processor_access_
bool enable_soft_processor_access_
Definition: wg06.h:250
XmlRpc::XmlRpcValue
WG06StatusWithAccelAndFT::checksum_
uint8_t checksum_
Definition: wg06.h:136
ros::NodeHandle
WG06StatusWithAccel::board_temperature_
uint16_t board_temperature_
Definition: wg06.h:94
FTDataSample
Definition: wg06.h:76
WG06StatusWithAccel::unused2
int32_t unused2
Definition: wg06.h:92


ethercat_hardware
Author(s): Rob Wheeler , Derek King
autogenerated on Thu Sep 26 2024 02:44:04