#include <SVHFingerManager.h>
Classes | |
struct | DiagnosticState |
Public Types | |
enum | Hints { HT_DEVICE_NOT_FOUND, HT_CONNECTION_FAILED, HT_NOT_RESETTED, HT_NOT_CONNECTED, HT_RESET_FAILED, HT_CHANNEL_SWITCHED_OF, HT_DANGEROUS_CURRENTS, HT_DIMENSION } |
The Hints enum provides mapping to hints that can be sent to the web-diagnostic interface. More... | |
Public Member Functions | |
bool | connect (const std::string &dev_name="/dev/ttyUSB0", const unsigned int &retry_count=3) |
Open connection to SCHUNK five finger hand. Wait until expected return packages are received. More... | |
double | convertmAtoN (const SVHChannel &channel, const int16_t ¤t) |
Converts joint currents of a specific channel from current [mA] to force [N] factoring the effort_constants of the channels. More... | |
void | disableChannel (const SVHChannel &channel) |
disable controller of channel More... | |
void | disconnect () |
disconnect SCHUNK five finger hand More... | |
bool | enableChannel (const SVHChannel &channel) |
enable controller of channel More... | |
bool | getCurrent (const SVHChannel &channel, double ¤t) |
returns current value of channel More... | |
bool | getCurrentSettings (const SVHChannel &channel, SVHCurrentSettings ¤t_settings) |
returns actual current controller settings of channel More... | |
std::vector< SVHCurrentSettings > | getDefaultCurrentSettings () |
get default current settings. These are either values previously set from calling software or hardcoded defaults More... | |
std::vector< SVHPositionSettings > | getDefaultPositionSettings (const bool &reset=false) |
get default position settings. These are either values previously set from calling software or hardcoded defaults \parm reset true if the Positions settins are to be used during reset (reduced speed) More... | |
bool | getDiagnosticStatus (const SVHChannel &channel, struct DiagnosticState &diagnostic_status) |
returns actual diagnostic status of channel More... | |
SVHFirmwareInfo | getFirmwareInfo (const std::string &dev_name="/dev/ttyUSB0", const unsigned int &retry_count=3) |
getFirmwareInfo Requests the firmware information from the harware, waits a bit and returns the last one read. More... | |
bool | getHomeSettings (const SVHChannel &channel, SVHHomeSettings &home_settings) |
returns actual home settings of channel More... | |
bool | getPosition (const SVHChannel &channel, double &position) |
returns position value of channel (will not acces hardware but return latest value) More... | |
bool | getPositionSettings (const SVHChannel &channel, SVHPositionSettings &position_settings) |
returns actual position controller settings of channel More... | |
bool | isConnected () |
returns connected state of finger manager More... | |
bool | isEnabled (const SVHChannel &channel) |
returns true, if current channel has been enabled More... | |
bool | isHomed (const SVHChannel &channel) |
returns true, if current channel has been resetted More... | |
bool | requestControllerFeedback (const SVHChannel &channel) |
send request controller feedback paket More... | |
bool | requestControllerFeedbackAllChannels () |
sends request controller feedback packet for all channels More... | |
void | requestControllerState () |
bool | resetChannel (const SVHChannel &channel) |
reset function for channel More... | |
bool | resetDiagnosticData (const SVHChannel &channel) |
resetDiagnosticData reset the diagnostic data vectors More... | |
bool | setAllTargetPositions (const std::vector< double > &positions) |
set all target positions at once More... | |
bool | setCurrentSettings (const SVHChannel &channel, const SVHCurrentSettings ¤t_settings) |
overwrite current parameters More... | |
void | setDefaultHomeSettings () |
initialize the homing settings with hardcoded defaults. These can be overwritten by the setHomeSettings function More... | |
float | setForceLimit (const SVHChannel &channel, float force_limit) |
setForceLimit set the force limit for the given channel More... | |
bool | setHomeSettings (const SVHChannel &channel, const SVHHomeSettings &home_settings) |
setHomeSettings set the home Settings which are maily used doring reset and provide the soft limit for the fingers More... | |
bool | setMaxForce (float max_force) |
setMaxForce set the max force / current as a persentage of the maximum possible current More... | |
bool | setPositionSettings (const SVHChannel &channel, const SVHPositionSettings &position_settings) |
overwrite position parameters More... | |
void | setResetSpeed (const float &speed) |
setResetSpeed Set the speed percentage during reset More... | |
void | setResetTimeout (const int &reset_timeout) |
setResetTimeout Helper function to set the timout durind rest of fingers More... | |
bool | setTargetPosition (const SVHChannel &channel, double position, double current) |
set target position of a channel More... | |
SVHFingerManager (const std::vector< bool > &disable_mask=std::vector< bool >(9, false), const uint32_t &reset_timeout=5) | |
virtual | ~SVHFingerManager () |
Private Member Functions | |
uint16_t | convertNtomA (const SVHChannel &channel, const double &effort) |
Converts joint efforts of a specific channel from force [N] to current [mA] factoring the effort_constants of the channels. More... | |
int32_t | convertRad2Ticks (const SVHChannel &channel, const double &position) |
Converts joint positions of a specific channel from RAD to ticks factoring in the offset of the channels. More... | |
double | convertTicks2Rad (const SVHChannel &channel, const int32_t &ticks) |
Converts joint positions of a specific channel from ticks to RAD factoring in the offset of the channels. More... | |
bool | currentSettingsAreSafe (const SVHChannel &channel, const SVHCurrentSettings ¤t_settings) |
currentSettingsAreSafe helper function to check for the most important values of the current settings More... | |
bool | isInsideBounds (const SVHChannel &channel, const int32_t &target_position) |
Check bounds of target positions. More... | |
void | pollFeedback () |
Periodically poll feedback from the hardware. More... | |
Private Attributes | |
bool | m_connected |
holds the connected state More... | |
bool | m_connection_feedback_given |
SVHController * | m_controller |
pointer to svh controller More... | |
std::vector< SVHCurrentSettings > | m_current_settings |
Vector of current controller parameters for each finger (as given by external config) More... | |
std::vector< bool > | m_current_settings_given |
SVHControllerFeedback | m_debug_feedback |
std::vector< double > | m_diagnostic_current_maximum |
vectors storing diagnostic information, current maximum More... | |
std::vector< double > | m_diagnostic_current_minimum |
vectors storing diagnostic information, current minimum More... | |
std::vector< bool > | m_diagnostic_current_state |
vectors storing diagnostic information, current state More... | |
std::vector< double > | m_diagnostic_deadlock |
vectors storing diagnostic information, diagnostics deadlock More... | |
std::vector< bool > | m_diagnostic_encoder_state |
vectors storing diagnostic information, encoder state More... | |
std::vector< double > | m_diagnostic_position_maximum |
vectors storing diagnostic information, position maximum More... | |
std::vector< double > | m_diagnostic_position_minimum |
vectors storing diagnostic information, position minimum More... | |
std::thread | m_feedback_thread |
Thread for polling periodic feedback from the hardware. More... | |
SVHFirmwareInfo | m_firmware_info |
Firmware info of the connected Hand. More... | |
std::vector< SVHHomeSettings > | m_home_settings |
Vector of home settings for each finger (as given by external config) More... | |
std::chrono::seconds | m_homing_timeout |
Timeout for homing. More... | |
std::vector< bool > | m_is_homed |
vector storing reset flags for each channel More... | |
std::vector< bool > | m_is_switched_off |
float | m_max_current_percentage |
limit the maximum of the force / current of the finger as a percentage of the possible maximum More... | |
std::atomic< bool > | m_poll_feedback |
Flag whether to poll feedback periodically in the feedback thread. More... | |
std::vector< int32_t > | m_position_home |
home position after complete reset of each channel More... | |
std::vector< int32_t > | m_position_max |
max position vector for each channel More... | |
std::vector< int32_t > | m_position_min |
min position vector for each channel More... | |
std::vector< SVHPositionSettings > | m_position_settings |
Vector of position controller parameters for each finger (as given by external config) More... | |
std::vector< bool > | m_position_settings_given |
std::vector< double > | m_reset_current_factor |
Vector containing factors for the currents at reset. Vector containing factors for the currents at reset. A hard stop is found if the maxCurrent (first 2 CurrentSettingsValues) x the reset factor was reached. 0.75 by default Beware. Setting this value very high might result in damage to the motors during reset. More... | |
std::vector< SVHChannel > | m_reset_order |
vector storing the reset order of the channels More... | |
float | m_reset_speed_factor |
Factor for determining the finger speed during reset. Only 0.0-1.0 is allowed. More... | |
std::chrono::seconds | m_reset_timeout |
Time in seconds after which the a reset is aborted if no change in current is observable. More... | |
std::string | m_serial_device |
m_serial_device Device handle of the device to use, is overwritten if connect is called with an argument More... | |
std::vector< double > | m_ticks2rad |
position conversion factor (ticks to RAD) for each channel More... | |
This class manages controller parameters and the finger reset.
Definition at line 55 of file SVHFingerManager.h.
The Hints enum provides mapping to hints that can be sent to the web-diagnostic interface.
Enumerator | |
---|---|
HT_DEVICE_NOT_FOUND | |
HT_CONNECTION_FAILED | |
HT_NOT_RESETTED | |
HT_NOT_CONNECTED | |
HT_RESET_FAILED | |
HT_CHANNEL_SWITCHED_OF | |
HT_DANGEROUS_CURRENTS | |
HT_DIMENSION |
Definition at line 62 of file SVHFingerManager.h.
driver_svh::SVHFingerManager::SVHFingerManager | ( | const std::vector< bool > & | disable_mask = std::vector<bool>(9, false) , |
const uint32_t & | reset_timeout = 5 |
||
) |
Constructs a finger manager for the SCHUNK five finger hand.
autostart | if set to true, the driver will immediately connect to the hardware and try to reset all fingers |
dev_name | the dev to use for autostart. Default is /dev/ttyUSB0 |
Definition at line 52 of file SVHFingerManager.cpp.
|
virtual |
Definition at line 120 of file SVHFingerManager.cpp.
bool driver_svh::SVHFingerManager::connect | ( | const std::string & | dev_name = "/dev/ttyUSB0" , |
const unsigned int & | retry_count = 3 |
||
) |
Open connection to SCHUNK five finger hand. Wait until expected return packages are received.
dev_name | file handle of the serial device e.g. "/dev/ttyUSB0" |
_retry_count | The number of times a connection is tried to be established if at least one package was received |
Definition at line 134 of file SVHFingerManager.cpp.
double driver_svh::SVHFingerManager::convertmAtoN | ( | const SVHChannel & | channel, |
const int16_t & | current | ||
) |
Converts joint currents of a specific channel from current [mA] to force [N] factoring the effort_constants of the channels.
channel | Channel to Convert for (each one has different constants) |
current | Current in [mA] |
Definition at line 1482 of file SVHFingerManager.cpp.
|
private |
Converts joint efforts of a specific channel from force [N] to current [mA] factoring the effort_constants of the channels.
channel | Channel to Convert for (each one has different constants) |
efforts | Effort in [N] |
Definition at line 1461 of file SVHFingerManager.cpp.
|
private |
Converts joint positions of a specific channel from RAD to ticks factoring in the offset of the channels.
channel | Channel to Convert for (each one has different offset) |
position | The desired position given in RAD |
Definition at line 1427 of file SVHFingerManager.cpp.
|
private |
Converts joint positions of a specific channel from ticks to RAD factoring in the offset of the channels.
channel | Channel to Convert for (each one has different offset) |
ticks | The current position in ticks |
Definition at line 1444 of file SVHFingerManager.cpp.
|
private |
currentSettingsAreSafe helper function to check for the most important values of the current settings
channel | the channel the settings are meant for |
current_settings | the settings to evaluate |
Definition at line 1033 of file SVHFingerManager.cpp.
void driver_svh::SVHFingerManager::disableChannel | ( | const SVHChannel & | channel | ) |
disable controller of channel
channel | channel to disable |
Definition at line 672 of file SVHFingerManager.cpp.
void driver_svh::SVHFingerManager::disconnect | ( | ) |
disconnect SCHUNK five finger hand
Definition at line 275 of file SVHFingerManager.cpp.
bool driver_svh::SVHFingerManager::enableChannel | ( | const SVHChannel & | channel | ) |
enable controller of channel
channel | channel to enable |
Definition at line 635 of file SVHFingerManager.cpp.
bool driver_svh::SVHFingerManager::getCurrent | ( | const SVHChannel & | channel, |
double & | current | ||
) |
returns current value of channel
channel | channel to get the current of |
current | current of the given channel in [mA] |
Definition at line 754 of file SVHFingerManager.cpp.
bool driver_svh::SVHFingerManager::getCurrentSettings | ( | const SVHChannel & | channel, |
SVHCurrentSettings & | current_settings | ||
) |
returns actual current controller settings of channel
channel | channel to get the current controller settings for |
current_settings | settings currently active for the current controller |
Definition at line 986 of file SVHFingerManager.cpp.
std::vector< SVHCurrentSettings > driver_svh::SVHFingerManager::getDefaultCurrentSettings | ( | ) |
get default current settings. These are either values previously set from calling software or hardcoded defaults
Definition at line 1254 of file SVHFingerManager.cpp.
std::vector< SVHPositionSettings > driver_svh::SVHFingerManager::getDefaultPositionSettings | ( | const bool & | reset = false | ) |
get default position settings. These are either values previously set from calling software or hardcoded defaults \parm reset true if the Positions settins are to be used during reset (reduced speed)
returns parameters for position settings either the default ones or parameters that have been set from outside
Definition at line 1316 of file SVHFingerManager.cpp.
bool driver_svh::SVHFingerManager::getDiagnosticStatus | ( | const SVHChannel & | channel, |
struct DiagnosticState & | diagnostic_status | ||
) |
returns actual diagnostic status of channel
channel | channel to get the position settings for |
diagnostic_status | diagnostic data of motor and encoder |
Definition at line 610 of file SVHFingerManager.cpp.
SVHFirmwareInfo driver_svh::SVHFingerManager::getFirmwareInfo | ( | const std::string & | dev_name = "/dev/ttyUSB0" , |
const unsigned int & | retry_count = 3 |
||
) |
getFirmwareInfo Requests the firmware information from the harware, waits a bit and returns the last one read.
dev_name | file handle of the serial device e.g. "/dev/ttyUSB0" |
_retry_count | The number of times a connection is tried to be established if at least one package was received |
Definition at line 1565 of file SVHFingerManager.cpp.
bool driver_svh::SVHFingerManager::getHomeSettings | ( | const SVHChannel & | channel, |
SVHHomeSettings & | home_settings | ||
) |
returns actual home settings of channel
channel | channel to get the position settings for |
home_settings | settings indicating the movement range of a finger, its reset direction and idle position |
Definition at line 1018 of file SVHFingerManager.cpp.
bool driver_svh::SVHFingerManager::getPosition | ( | const SVHChannel & | channel, |
double & | position | ||
) |
returns position value of channel (will not acces hardware but return latest value)
channel | channel to get the position of |
position | position the given channel ist at |
Definition at line 714 of file SVHFingerManager.cpp.
bool driver_svh::SVHFingerManager::getPositionSettings | ( | const SVHChannel & | channel, |
SVHPositionSettings & | position_settings | ||
) |
returns actual position controller settings of channel
channel | channel to get the position settings for |
position_settings | settings currently active for the position controller |
Definition at line 1002 of file SVHFingerManager.cpp.
|
inline |
returns connected state of finger manager
Definition at line 111 of file SVHFingerManager.h.
bool driver_svh::SVHFingerManager::isEnabled | ( | const SVHChannel & | channel | ) |
returns true, if current channel has been enabled
channel | channel to check if it is enabled |
Definition at line 912 of file SVHFingerManager.cpp.
bool driver_svh::SVHFingerManager::isHomed | ( | const SVHChannel & | channel | ) |
returns true, if current channel has been resetted
channel |
Definition at line 953 of file SVHFingerManager.cpp.
|
private |
Check bounds of target positions.
channel | |
target_position |
Definition at line 1494 of file SVHFingerManager.cpp.
|
private |
Periodically poll feedback from the hardware.
The hardware will send data only in response to special requests. We realize constant joint position feedback for all fingers through sending these requests periodically in this function.
Definition at line 1627 of file SVHFingerManager.cpp.
bool driver_svh::SVHFingerManager::requestControllerFeedback | ( | const SVHChannel & | channel | ) |
send request controller feedback paket
channel | channel to request the feedback for |
Definition at line 699 of file SVHFingerManager.cpp.
bool driver_svh::SVHFingerManager::requestControllerFeedbackAllChannels | ( | ) |
sends request controller feedback packet for all channels
void driver_svh::SVHFingerManager::requestControllerState | ( | ) |
requests the current controller state to be updated
Definition at line 1515 of file SVHFingerManager.cpp.
bool driver_svh::SVHFingerManager::resetChannel | ( | const SVHChannel & | channel | ) |
reset function for channel
reset function for a single finger
channel | Channel to reset |
Definition at line 298 of file SVHFingerManager.cpp.
bool driver_svh::SVHFingerManager::resetDiagnosticData | ( | const SVHChannel & | channel | ) |
resetDiagnosticData reset the diagnostic data vectors
channel | channel to reset the data vector for |
Definition at line 1173 of file SVHFingerManager.cpp.
bool driver_svh::SVHFingerManager::setAllTargetPositions | ( | const std::vector< double > & | positions | ) |
set all target positions at once
positions | Vector of positions to set as targets given in [rad]. Only the first eSVH_CHANNEL_DIMENSION (9) values will be considered. If less values are given all others are set to zero |
Definition at line 771 of file SVHFingerManager.cpp.
bool driver_svh::SVHFingerManager::setCurrentSettings | ( | const SVHChannel & | channel, |
const SVHCurrentSettings & | current_settings | ||
) |
overwrite current parameters
channel | channel to set the current settings for |
current_settings | settings of the current controller for a specific channel |
Definition at line 1074 of file SVHFingerManager.cpp.
void driver_svh::SVHFingerManager::setDefaultHomeSettings | ( | ) |
initialize the homing settings with hardcoded defaults. These can be overwritten by the setHomeSettings function
Definition at line 1216 of file SVHFingerManager.cpp.
float driver_svh::SVHFingerManager::setForceLimit | ( | const SVHChannel & | channel, |
float | force_limit | ||
) |
setForceLimit set the force limit for the given channel
channel | channel to set the force limit for |
force_limit | force limit to set |
Definition at line 1542 of file SVHFingerManager.cpp.
bool driver_svh::SVHFingerManager::setHomeSettings | ( | const SVHChannel & | channel, |
const SVHHomeSettings & | home_settings | ||
) |
setHomeSettings set the home Settings which are maily used doring reset and provide the soft limit for the fingers
channel | channel to set the home settings for |
home_settings | settings indicating the movement range of a finger, its reset direction and idle position |
Definition at line 1138 of file SVHFingerManager.cpp.
bool driver_svh::SVHFingerManager::setMaxForce | ( | float | max_force | ) |
setMaxForce set the max force / current as a persentage of the maximum possible current
max_force | in percent [0,1] return if valid max_force was given |
Definition at line 1526 of file SVHFingerManager.cpp.
bool driver_svh::SVHFingerManager::setPositionSettings | ( | const SVHChannel & | channel, |
const SVHPositionSettings & | position_settings | ||
) |
overwrite position parameters
channel | channel to set the positoon settings for |
position_settings | settings of the position controller to be used |
Definition at line 1111 of file SVHFingerManager.cpp.
void driver_svh::SVHFingerManager::setResetSpeed | ( | const float & | speed | ) |
setResetSpeed Set the speed percentage during reset
speed | percent of the normal speed used during reset Allowed values 0.0-1.0 |
Definition at line 1411 of file SVHFingerManager.cpp.
void driver_svh::SVHFingerManager::setResetTimeout | ( | const int & | reset_timeout | ) |
setResetTimeout Helper function to set the timout durind rest of fingers
resetTimeout | timeout in Seconds. Values smaler than 0 will be interpreted as 0 |
Definition at line 1520 of file SVHFingerManager.cpp.
bool driver_svh::SVHFingerManager::setTargetPosition | ( | const SVHChannel & | channel, |
double | position, | ||
double | current | ||
) |
set target position of a channel
channel | channel to set the target position for |
position | target position in [rad] |
current | max current to use for that position |
Definition at line 837 of file SVHFingerManager.cpp.
|
private |
holds the connected state
Definition at line 341 of file SVHFingerManager.h.
|
private |
Helper variable to check if feedback was printed (will be replaced by a better solution in the future)
Definition at line 345 of file SVHFingerManager.h.
|
private |
pointer to svh controller
Definition at line 332 of file SVHFingerManager.h.
|
private |
Vector of current controller parameters for each finger (as given by external config)
Definition at line 401 of file SVHFingerManager.h.
|
private |
Information about the validity of externaly given values for the current settings (easier to use this way)
Definition at line 405 of file SVHFingerManager.h.
|
private |
Definition at line 486 of file SVHFingerManager.h.
|
private |
vectors storing diagnostic information, current maximum
Definition at line 380 of file SVHFingerManager.h.
|
private |
vectors storing diagnostic information, current minimum
Definition at line 383 of file SVHFingerManager.h.
|
private |
vectors storing diagnostic information, current state
Definition at line 377 of file SVHFingerManager.h.
|
private |
vectors storing diagnostic information, diagnostics deadlock
Definition at line 392 of file SVHFingerManager.h.
|
private |
vectors storing diagnostic information, encoder state
Definition at line 374 of file SVHFingerManager.h.
|
private |
vectors storing diagnostic information, position maximum
Definition at line 386 of file SVHFingerManager.h.
|
private |
vectors storing diagnostic information, position minimum
Definition at line 389 of file SVHFingerManager.h.
|
private |
Thread for polling periodic feedback from the hardware.
Definition at line 338 of file SVHFingerManager.h.
|
private |
Firmware info of the connected Hand.
Definition at line 417 of file SVHFingerManager.h.
|
private |
Vector of home settings for each finger (as given by external config)
Definition at line 414 of file SVHFingerManager.h.
|
private |
Timeout for homing.
Definition at line 348 of file SVHFingerManager.h.
|
private |
vector storing reset flags for each channel
Definition at line 367 of file SVHFingerManager.h.
|
private |
vector storing information if a finger is enabled. In Case it is all request for it will be granted but not executed on hardware
Definition at line 371 of file SVHFingerManager.h.
|
private |
limit the maximum of the force / current of the finger as a percentage of the possible maximum
Definition at line 352 of file SVHFingerManager.h.
|
private |
Flag whether to poll feedback periodically in the feedback thread.
Definition at line 335 of file SVHFingerManager.h.
|
private |
home position after complete reset of each channel
Definition at line 364 of file SVHFingerManager.h.
|
private |
max position vector for each channel
Definition at line 361 of file SVHFingerManager.h.
|
private |
min position vector for each channel
Definition at line 358 of file SVHFingerManager.h.
|
private |
Vector of position controller parameters for each finger (as given by external config)
Definition at line 408 of file SVHFingerManager.h.
|
private |
Information about the validity of externaly given values for the position settings (easier to use this way)
Definition at line 411 of file SVHFingerManager.h.
|
private |
Vector containing factors for the currents at reset. Vector containing factors for the currents at reset. A hard stop is found if the maxCurrent (first 2 CurrentSettingsValues) x the reset factor was reached. 0.75 by default Beware. Setting this value very high might result in damage to the motors during reset.
Definition at line 435 of file SVHFingerManager.h.
|
private |
vector storing the reset order of the channels
Definition at line 426 of file SVHFingerManager.h.
|
private |
Factor for determining the finger speed during reset. Only 0.0-1.0 is allowed.
Definition at line 395 of file SVHFingerManager.h.
|
private |
Time in seconds after which the a reset is aborted if no change in current is observable.
Definition at line 398 of file SVHFingerManager.h.
|
private |
m_serial_device Device handle of the device to use, is overwritten if connect is called with an argument
Definition at line 423 of file SVHFingerManager.h.
|
private |
position conversion factor (ticks to RAD) for each channel
Definition at line 355 of file SVHFingerManager.h.