38 #include <boost/bind/bind.hpp> 46 m_connection_feedback_given(false),
54 m_movement_state(eST_DEACTIVATED),
55 m_reset_speed_factor(0.2),
56 m_reset_timeout(reset_timeout),
62 m_serial_device(
"/dev/ttyUSB0")
64 #ifdef _IC_BUILDER_ICL_COMM_WEBSOCKET_ 69 m_ws_broadcaster->registerHintCallback(boost::bind(&SVHFingerManager::receivedHintMessage,
this,_1));
100 #ifdef _IC_BUILDER_ICL_COMM_WEBSOCKET_ 131 #ifdef _IC_BUILDER_ICL_COMM_WEBSOCKET_ 155 unsigned int retry_count=_retry_count;
167 std::vector<SVHCurrentSettings> current_settings
188 unsigned int received_count = 0;
189 unsigned int send_count = 0;
194 if (send_count == received_count)
198 <<
"Send packages = " << send_count <<
", received packages = " << received_count <<
endl);
201 LOGGING_TRACE_C(DriverSVH,
SVHFingerManager,
"Try to connect to SCHUNK five finger hand: Send packages = " << send_count <<
", received packages = " << received_count <<
endl);
208 <<
"Send packages = " << send_count <<
", received packages = " << received_count <<
endl);
209 #ifdef _IC_BUILDER_ICL_COMM_WEBSOCKET_ 224 if (received_count > 0 && retry_count >= 0)
227 LOGGING_ERROR_C(DriverSVH,
SVHFingerManager,
"Connection Failed! Send packages = " << send_count <<
", received packages = " << received_count <<
". Retrying, count: " << retry_count <<
endl);
232 LOGGING_ERROR_C(DriverSVH,
SVHFingerManager,
"Connection Failed! Send packages = " << send_count <<
", received packages = " << received_count <<
". Not Retrying anymore."<<
endl);
248 #ifdef _IC_BUILDER_ICL_COMM_WEBSOCKET_ 274 #ifdef _IC_BUILDER_ICL_COMM_WEBSOCKET_ 312 #ifdef _IC_BUILDER_ICL_COMM_WEBSOCKET_ 333 bool reset_all_success =
true;
337 size_t max_reset_counter = 3;
338 bool reset_success =
false;
339 while (!reset_success && max_reset_counter > 0)
349 reset_all_success = reset_all_success && reset_success;
353 #ifdef _IC_BUILDER_ICL_COMM_WEBSOCKET_ 363 return reset_all_success;
371 #ifdef _IC_BUILDER_ICL_COMM_WEBSOCKET_ 377 #endif // _IC_BUILDER_ICL_COMM_WEBSOCKET_ 424 bool stale_notification_sent =
false;
426 for (
size_t hit_count = 0; hit_count < 10; )
444 else if (hit_count > 0)
456 #ifdef _IC_BUILDER_ICL_COMM_WEBSOCKET_ 471 if (stale_notification_sent)
474 stale_notification_sent =
false;
479 if (!stale_notification_sent)
482 stale_notification_sent =
true;
487 control_feedback_previous = control_feedback;
511 if (
abs(position - control_feedback.
position) < 1000)
531 bool reset_all_success =
true;
534 reset_all_success == reset_all_success &&
m_is_homed[channel];
537 if (reset_all_success)
539 #ifdef _IC_BUILDER_ICL_COMM_WEBSOCKET_ 555 #ifdef _IC_BUILDER_ICL_COMM_WEBSOCKET_ 560 #endif // _IC_BUILDER_ICL_COMM_WEBSOCKET_ 609 #ifdef _IC_BUILDER_ICL_COMM_WEBSOCKET_ 614 #endif // _IC_BUILDER_ICL_COMM_WEBSOCKET_ 643 #ifdef _IC_BUILDER_ICL_COMM_WEBSOCKET_ 648 #endif // _IC_BUILDER_ICL_COMM_WEBSOCKET_ 652 bool all_disabled =
true;
714 #ifdef _IC_BUILDER_ICL_COMM_WEBSOCKET_ 715 void SVHFingerManager::receivedHintMessage(
const int &hint)
747 LOGGING_ERROR_C(DriverSVH,
SVHFingerManager,
"Special error clearing command " << hint <<
" could not be mapped. No action is taken please contact support if this happens." <<
endl);
751 #endif // _IC_BUILDER_ICL_COMM_WEBSOCKET_ 753 #ifdef _IC_BUILDER_ICL_COMM_WEBSOCKET_ 754 void SVHFingerManager::updateWebSocket()
765 if (
isHomed(static_cast<SVHChannel>(i)) &&
getPosition(static_cast<SVHChannel>(i),position))
783 #endif // _IC_BUILDER_ICL_COMM_WEBSOCKET_ 795 current = controller_feedback.
current;
816 bool reject_command =
false;
833 reject_command =
true;
935 bool all_enabled =
true;
938 all_enabled = all_enabled &&
isEnabled(static_cast<SVHChannel>(i));
971 bool all_homed =
true;
974 all_homed = all_homed &&
isHomed(static_cast<SVHChannel>(i));
975 if (!
isHomed(static_cast<SVHChannel>(i)))
999 #ifdef _IC_BUILDER_ICL_COMM_WEBSOCKET_ 1004 #endif // _IC_BUILDER_ICL_COMM_WEBSOCKET_ 1035 bool settingsAreSafe =
true;
1042 settingsAreSafe = (current_settings.
wmn >= -1800.0) &&
1043 (current_settings.
wmx <= 1800.0);
1047 settingsAreSafe = (current_settings.
wmn >= -1800.0) &&
1048 (current_settings.
wmx <= 1800.0);
1055 settingsAreSafe = (current_settings.
wmn >= -650.0) &&
1056 (current_settings.
wmx <= 650.0);
1061 settingsAreSafe = (current_settings.
wmn >= -1100.0) &&
1062 (current_settings.
wmx <= 1100.0);
1065 settingsAreSafe = (current_settings.
wmn >= -1000.0) &&
1066 (current_settings.
wmx <= 1000.0);
1070 settingsAreSafe =
true;
1074 return settingsAreSafe;
1086 LOGGING_ERROR_C(DriverSVH,
SVHFingerManager,
"WARNING!!! Current Controller Params for channel " << channel <<
" are dangerous! THIS MIGHT DAMAGE YOUR HARDWARE!!!" <<
endl);
1087 #ifdef _IC_BUILDER_ICL_COMM_WEBSOCKET_ 1199 std::vector<SVHCurrentSettings> current_settings(
eSVH_DIMENSION);
1234 return current_settings;
1242 std::vector<SVHPositionSettings> position_settings(
eSVH_DIMENSION);
1274 SVHPositionSettings pos_set_finger_index_distal (-1.0e6f, 1.0e6f, 45.0e3f, 1.00
f, 1e-3
f, -500.0
f, 500.0
f, 0.5
f, 0.0
f, 40.0
f);
1275 SVHPositionSettings pos_set_finger_index_proximal (-1.0e6f, 1.0e6f, 40.0e3f, 1.00
f, 1e-3
f, -500.0
f, 500.0
f, 0.8
f, 0.0
f, 1000.0
f);
1276 SVHPositionSettings pos_set_finger_middle_distal (-1.0e6f, 1.0e6f, 45.0e3f, 1.00
f, 1e-3
f, -500.0
f, 500.0
f, 0.5
f, 0.0
f, 10.0
f);
1277 SVHPositionSettings pos_set_finger_middle_proximal (-1.0e6f, 1.0e6f, 40.0e3f, 1.00
f, 1e-3
f, -500.0
f, 500.0
f, 0.8
f, 0.0
f, 1000.0
f);
1304 return position_settings;
1309 if ((speed>= 0.0) && (speed <= 1.0))
1315 LOGGING_ERROR_C(DriverSVH,
SVHFingerManager,
"The reset speed value given: "<< speed <<
" is not valid. Please provide a value between 0.0 and 1.0, default is 0.2"<<
endl);
1333 return target_position;
1339 int32_t cleared_position_ticks;
1350 return static_cast<double>(cleared_position_ticks *
m_ticks2rad[channel]);
SVHFirmwareInfo getFirmwareInfo()
get the latest stored Firmware information from the controller (NOT THE HARDWARE) ...
std::vector< SVHChannel > m_reset_order
vector storing the reset order of the channels
float m_reset_speed_factor
Factor for determining the finger speed during reset. Only 0.0-1.0 is allowed.
float wmx
Reference signal maximum value.
std::vector< bool > m_is_homed
vector storing reset flags for each channel
std::vector< bool > m_position_settings_given
Information about the validity of externaly given values for the position settings (easier to use thi...
float wmn
Reference signal minimum value.
MovementState
The MovementState enum indicated the overall state of the hand. Currently only used for updating the ...
void disableChannel(const SVHChannel &channel)
Disable one or all motor channels.
bool getPositionSettings(const SVHChannel &channel, SVHPositionSettings &position_settings)
returns actual position controller settings of channel
static TimeSpan createFromMSec(int64_t msec)
#define LOGGING_INFO_C(streamname, classname, arg)
std::vector< SVHCurrentSettings > m_current_settings
Vector of current controller parameters for each finger (as given by external config) ...
bool setCurrentSettings(const SVHChannel &channel, const SVHCurrentSettings ¤t_settings)
overwrite current parameters
void resetPackageCounts()
resetPackageCounts sets the sent and reveived package counts to zero
void setPositionSettings(const SVHChannel &channel, const SVHPositionSettings &position_settings)
activate a new set of position controller settings for a specific channel
SVHFeedbackPollingThread * m_feedback_thread
pointer to svh controller
void disableChannel(const SVHChannel &channel)
disable controller of channel
TimeSpan abs(const TimeSpan &span)
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 chann...
float minimumOffset
Minimum reachable tick limit, given as offset from the hard stop (soft limit)
unsigned int getSentPackageCount()
requests the number of sent packages. Request ist transferred to the serial interface that knows abou...
bool setPositionSettings(const SVHChannel &channel, const SVHPositionSettings &position_settings)
overwrite position parameters
std::vector< int32_t > m_position_max
max position vector for each channel
std::vector< double > m_ticks2rad
position conversion factor (ticks to RAD) for each channel
void setResetTimeout(const int &resetTimeout)
setResetTimeout Helper function to set the timout durind rest of fingers
boost::shared_ptr< icl_comm::websocket::WsBroadcaster > m_ws_broadcaster
Websocket handle for updating diagnostic backend (OPTIONAL)
int32_t position
Returned position value of the motor [Ticks].
int direction
Movement direction of the finger +1 or -1 home in positive or negative direction. ...
The SVHCurrentSettings save the current controller paramters for a single motor.
virtual ~SVHFingerManager()
bool getCurrent(const SVHChannel &channel, double ¤t)
returns current value of channel
void setMovementState(const MovementState &state)
setMovementState Updates the movement state of the overll hand indicating the overall status ...
int8_t m_homing_timeout
vector storing reset flags for each finger
bool getControllerFeedback(const SVHChannel &channel, SVHControllerFeedback &controller_feedback)
request the latest stored controllerfeedback (current, position) from the controller.
bool connect(const std::string &dev_name)
Open serial device connection.
bool m_connection_feedback_given
Helper variable to check if feedback was printed (will be replaced by a better solution in the future...
float idlePosition
Idle position to move the fingers to after initialization.
unsigned int getReceivedPackageCount()
request the number of correctly received packages. This number is refreshed every time the serialinte...
void requestControllerState()
Request current controller state (mainly usefull for debug purposes)
bool isConnected()
returns connected state of finger manager
#define LOGGING_WARNING_C(streamname, classname, arg)
int usleep(unsigned long useconds)
std::string m_serial_device
m_serial_device Device handle of the device to use, is overwritten if connect is called with an argum...
MovementState m_movement_state
Overall movement State to indicate what the hand is doing at the moment.
void requestControllerState()
float dwmx
Reference signal delta maximum threshold.
float resetCurrentFactor
The resetCurrentFactor indicates how much of the maximum allowed current (of the controller) must be ...
bool resetChannel(const SVHChannel &channel)
reset function for channel
bool setAllTargetPositions(const std::vector< double > &positions)
set all target positions at once
void setControllerTarget(const SVHChannel &channel, const int32_t &position)
Set new position target for finger index.
std::vector< int32_t > m_position_min
min position vector for each channel
float wmx
Reference signal maximum value.
#define LOGGING_DEBUG_C(streamname, classname, arg)
std::vector< SVHHomeSettings > m_home_settings
Vector of home settings for each finger (as given by external config)
ThreadStream & endl(ThreadStream &stream)
bool isEnabled(const SVHChannel &channel)
Check if a channel was enabled.
bool isEnabled(const SVHChannel &channel)
returns true, if current channel has been enabled
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 chann...
std::vector< SVHPositionSettings > m_position_settings
Vector of position controller parameters for each finger (as given by external config) ...
void requestControllerFeedback(const SVHChannel &channel)
request feedback (position and current) to a specific channel
std::vector< int32_t > m_position_home
home position after complete reset of each channel
void setCurrentSettings(const SVHChannel &channel, const SVHCurrentSettings ¤t_settings)
activate a new set of current controller settings for a specific channel
The SVHFirmwareInfo holds the data of a firmware response from the hardware.
The SVHControllerFeedback saves the feedback of a single motor.
void disconnect()
disconnect serial device
const TimeSpan timeout(1, 0)
bool setTargetPosition(const SVHChannel &channel, double position, double current)
set target position of a channel
bool getPositionSettings(const SVHChannel &channel, SVHPositionSettings &position_settings)
request the latest stored positionsettings from the controller
bool isHomed(const SVHChannel &channel)
returns true, if current channel has been resetted
static const char * m_channel_description[]
Description values to get the corresponding string value to a channel enum.
std::vector< bool > m_is_switched_off
vector storing information if a finger is enabled. In Case it is all request for it will be granted b...
int16_t current
Returned current value of the motor [mA].
bool getCurrentSettings(const SVHChannel &channel, SVHCurrentSettings ¤t_settings)
request the latest stored currentsettings from the controller
uint32_t m_reset_timeout
Time in seconds after which the a reset is aborted if no change in current is observable.
bool m_connected
holds the connected state
float wmn
Reference signal minimum value.
This class controls the the SCHUNK five finger hand.
void disconnect()
disconnect SCHUNK five finger hand
SVHFirmwareInfo getFirmwareInfo()
getFirmwareInfo Requests the firmware information from the harware, waits a bit and returns the last ...
void setControllerTargetAllChannels(const std::vector< int32_t > &positions)
Setting new position controller target for all fingers.
std::vector< SVHCurrentSettings > getDefaultCurrentSettings()
get default current settings. These are either values previously set from calling software or hardcod...
Thread for periodically requesting feedback messages from the SCHUNK five finger hand.
The SVHPositionSettings save the position controller paramters for a single motor.
bool enableChannel(const SVHChannel &channel)
enable controller of channel
bool requestControllerFeedback(const SVHChannel &channel)
send request controller feedback paket
void setResetSpeed(const float &speed)
setResetSpeed Set the speed percentage during reset
bool isInsideBounds(const SVHChannel &channel, const int32_t &target_position)
Check bounds of target positions.
void setDefaultHomeSettings()
initialize the homing settings with hardcoded defaults. These can be overwritten by the setHomeSettin...
float maximumOffset
Maximum reachable tick limt, given as an offset from the hard stop (soft limit)
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...
data sctructure for home positions
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 fo...
bool getPosition(const SVHChannel &channel, double &position)
returns position value of channel (will not acces hardware but return latest value) ...
bool getCurrentSettings(const SVHChannel &channel, SVHCurrentSettings ¤t_settings)
returns actual current controller settings of channel
#define LOGGING_TRACE_C(streamname, classname, arg)
SVHFingerManager(const std::vector< bool > &disable_mask=std::vector< bool >(9, false), const uint32_t &reset_timeout=5)
std::vector< bool > m_current_settings_given
Information about the validity of externaly given values for the current settings (easier to use this...
std::vector< SVHPositionSettings > getDefaultPositionSettings(const bool &reset=false)
get default position settings. These are either values previously set from calling software or hardco...
SVHChannel
Channel indicates which motor to use in command calls. WARNING: DO NOT CHANGE THE ORDER OF THESE as i...
#define LOGGING_ERROR_C(streamname, classname, arg)
bool currentSettingsAreSafe(const SVHChannel &channel, const SVHCurrentSettings ¤t_settings)
currentSettingsAreSafe helper function to check for the most important values of the current settings...
SVHController * m_controller
pointer to svh controller
void requestFirmwareInfo()
request a transmission of formware information
void enableChannel(const SVHChannel &channel)
Enable one or all motor channels.