SDHLibrary-CPP 0.0.2.10SCHUNK GmbH & Co. KG
C++ access library for SCHUNK Dextrous Hand SDH

SDH::cDSA Class Reference

SDH::cDSA is the end user interface class to access the DSACON32m, the tactile sensor controller of the SDH. More...

#include <dsa.h>

Collaboration diagram for SDH::cDSA:

List of all members.

Classes

struct  sContactInfo
 Structure to hold info about the contact of one sensor patch. More...
struct  sControllerInfo
 A data structure describing the controller info about the remote DSACON32m controller. More...
struct  sMatrixInfo
 A data structure describing a single sensor matrix connected to the remote DSACON32m controller. More...
struct  sResponse
 data structure for storing responses from the remote DSACON32m controller More...
struct  sSensitivityInfo
 Structure to hold info about the sensitivity settings of one sensor patch. More...
struct  sSensorInfo
 A data structure describing the sensor info about the remote DSACON32m controller. More...
struct  sTactileSensorFrame
 A data structure describing a full tactile sensor frame read from the remote DSACON32m controller. More...

Public Types

enum  eDSAErrorCode {
  E_SUCCESS, E_NOT_AVAILABLE, E_NO_SENSOR, E_NOT_INITIALIZED,
  E_ALREADY_RUNNING, E_FEATURE_NOT_SUPPORTED, E_INCONSISTENT_DATA, E_TIMEOUT,
  E_READ_ERROR, E_WRITE_ERROR, E_INSUFFICIENT_RESOURCES, E_CHECKSUM_ERROR,
  E_CMD_NOT_ENOUGH_PARAMS, E_CMD_UNKNOWN, E_CMD_FORMAT_ERROR, E_ACCESS_DENIED,
  E_ALREADY_OPEN, E_CMD_FAILED, E_CMD_ABORTED, E_INVALID_HANDLE,
  E_DEVICE_NOT_FOUND, E_DEVICE_NOT_OPENED, E_IO_ERROR, E_INVALID_PARAMETER,
  E_INDEX_OUT_OF_BOUNDS, E_CMD_PENDING, E_OVERRUN, E_RANGE_ERROR
}
 

error codes returned by the remote DSACON32m tactile sensor controller

More...
typedef UInt16 tTexel
 data type for a single 'texel' (tactile sensor element)

Public Member Functions

struct SDH::cDSA::sControllerInfo SDH__attribute__ ((__packed__))
struct SDH::cDSA::sSensorInfo SDH__attribute__ ((__packed__))
struct SDH::cDSA::sMatrixInfo SDH__attribute__ ((__packed__))
struct SDH::cDSA::sSensitivityInfo SDH__attribute__ ((__packed__))
 cDSA (int debug_level=0, int port=1, char const *device_format_string="/dev/ttyS%d")
 cDSA (int debug_level, char const *_tcp_adr, int _tcp_port=13000, double _timeout=1.0)
 ~cDSA ()
 Destructur: clean up and delete dynamically allocated memory.
sControllerInfo const & GetControllerInfo (void) const
 Return a reference to the sControllerInfo read from the remote DSACON32m controller.
sSensorInfo const & GetSensorInfo (void) const
 Return a reference to the sSensorInfo read from the remote DSACON32m controller.
sMatrixInfo const & GetMatrixInfo (int m) const
 Return a reference to the sMatrixInfo of matrix m read from the remote DSACON32m controller.
sTactileSensorFrame const & GetFrame () const
 return a reference to the latest tactile sensor frame without reading from remote DSACON32m
sTactileSensorFrame const & UpdateFrame ()
 read the tactile sensor frame from remote DSACON32m and return a reference to it. A command to query the frame (periodically) must have been sent before.
void Open (void) throw (cDSAException*, cSDHErrorCommunication*)
 (Re-)open connection to DSACON32m controller, this is called by the constructor automatically, but is still usefull to call after a call to Close()
void Close (void) throw (cDSAException*, cSDHErrorCommunication*)
 Set the framerate of the remote DSACON32m controller to 0 and close connection to it.
void SetFramerate (UInt16 framerate, bool do_RLE=true, bool do_data_acquisition=true) throw (cDSAException*, cSDHErrorCommunication*)
void SetFramerateRetries (UInt16 framerate, bool do_RLE=true, bool do_data_acquisition=true, unsigned int retries=0, bool ignore_exceptions=false) throw (cDSAException*, cSDHErrorCommunication*)
sSensitivityInfo GetMatrixSensitivity (int matrix_no) throw (cDSAException*, cSDHErrorCommunication*)
void SetMatrixSensitivity (int matrix_no, double sensitivity, bool do_all_matrices=false, bool do_reset=false, bool do_persistent=false) throw (cDSAException*, cSDHErrorCommunication*)
void SetMatrixThreshold (int matrix_no, UInt16 threshold, bool do_all_matrices=false, bool do_reset=false, bool do_persistent=false) throw (cDSAException*, cSDHErrorCommunication*)
UInt16 GetMatrixThreshold (int matrix_no) throw (cDSAException*, cSDHErrorCommunication*)
tTexel GetTexel (int m, int x, int y) const
int GetMatrixIndex (int fi, int part)
unsigned long GetAgeOfFrame (sTactileSensorFrame *frame_p)
double GetContactArea (int m)
sContactInfo GetContactInfo (int m)

Static Public Member Functions

static char const * ErrorCodeToString (eDSAErrorCode error_code)
static char const * ErrorCodeToString (UInt16 error_code)

Public Attributes

struct VCC_EXPORT
SDH::cDSA::sTactileSensorFrame 
SDH__attribute__
bool m_read_another

Protected Member Functions

struct SDH::cDSA::sResponse SDH__attribute__ ((__packed__))
void WriteCommandWithPayload (UInt8 command, UInt8 *payload, UInt16 payload_len) throw (cDSAException*, cSDHErrorCommunication*)
void WriteCommand (UInt8 command)
void ReadResponse (sResponse *response, UInt8 command_id) throw (cDSAException*, cSDHErrorCommunication*)
void ReadControllerInfo (sControllerInfo *_controller_info) throw (cDSAException*, cSDHErrorCommunication*)
void ReadSensorInfo (sSensorInfo *_sensor_info) throw (cDSAException*, cSDHErrorCommunication*)
void ReadMatrixInfo (sMatrixInfo *_matrix_info) throw (cDSAException*, cSDHErrorCommunication*)
void ReadFrame (sTactileSensorFrame *frame_p) throw (cDSAException*, cSDHErrorCommunication*)
void QueryControllerInfo (sControllerInfo *_controller_info) throw (cDSAException*, cSDHErrorCommunication*)
void QuerySensorInfo (sSensorInfo *_sensor_info) throw (cDSAException*, cSDHErrorCommunication*)
void QueryMatrixInfo (sMatrixInfo *_matrix_info, int matrix_no) throw (cDSAException*, cSDHErrorCommunication*)
void QueryMatrixInfos (void) throw (cDSAException*, cSDHErrorCommunication*)
void ParseFrame (sResponse *response, sTactileSensorFrame *frame_p) throw (cDSAException*)

Protected Attributes

cDBG dbg
 A stream object to print coloured debug messages.
cSerialBasecom
 the communication interface to use
bool do_RLE
 flag, true if data should be sent Run Length Encoded by the remote DSACON32m controller
sControllerInfo controller_info
 the controller info read from the remote DSACON32m controller
sSensorInfo sensor_info
 the sensor info read from the remote DSACON32m controller
sMatrixInfomatrix_info
 the matrix infos read from the remote DSACON32m controller
sTactileSensorFrame frame
 the latest frame received from the remote DSACON32m controller
int nb_cells
 The total number of sensor cells.
int * texel_offset
 an array with the offsets of the first texel of all matrices into the whole frame
long read_timeout_us
 default timeout used for reading in us
cSimpleTime start_pc
UInt32 start_dsa
tTexel contact_area_cell_threshold
 threshold of texel cell value for detecting contacts with GetContactArea
tTexel contact_force_cell_threshold
 threshold of texel cell value for detecting forces with GetContactForce
double force_factor
 additional calibration factor for forces in GetContactForce
double calib_pressure
double calib_voltage
 see calib_pressure

Friends

VCC_EXPORT std::ostream & operator<< (std::ostream &stream, cDSA::sResponse const &response)

Detailed Description

SDH::cDSA is the end user interface class to access the DSACON32m, the tactile sensor controller of the SDH.

Class to communicate with the tactile sensor controller DSACON32m of the SDH

Bug:
cDSAException: Checksum Error on Windows console While communicating with the tactile sensor controller a "cDSAException: Checksum Error" might be thrown once in a while. This seems to happen only when the program is started from a native windows command console (cmd.exe), but not e.g. when the program is started from a cygwin console. Strange. Workaround is to catch the exception and ignore the frame.
=> Resolved in SDHLibrary-C++ v0.0.2.1
Problem was an overrun of the windows input buffer, e.g. on heavy processor load.

Member Typedef Documentation

data type for a single 'texel' (tactile sensor element)


Member Enumeration Documentation

error codes returned by the remote DSACON32m tactile sensor controller

Enumerator:
E_SUCCESS 
E_NOT_AVAILABLE 
E_NO_SENSOR 
E_NOT_INITIALIZED 
E_ALREADY_RUNNING 
E_FEATURE_NOT_SUPPORTED 
E_INCONSISTENT_DATA 
E_TIMEOUT 
E_READ_ERROR 
E_WRITE_ERROR 
E_INSUFFICIENT_RESOURCES 
E_CHECKSUM_ERROR 
E_CMD_NOT_ENOUGH_PARAMS 
E_CMD_UNKNOWN 
E_CMD_FORMAT_ERROR 
E_ACCESS_DENIED 
E_ALREADY_OPEN 
E_CMD_FAILED 
E_CMD_ABORTED 
E_INVALID_HANDLE 
E_DEVICE_NOT_FOUND 
E_DEVICE_NOT_OPENED 
E_IO_ERROR 
E_INVALID_PARAMETER 
E_INDEX_OUT_OF_BOUNDS 
E_CMD_PENDING 
E_OVERRUN 
E_RANGE_ERROR 

Constructor & Destructor Documentation

cDSA::cDSA ( int  debug_level = 0,
int  port = 1,
char const *  device_format_string = "/dev/ttyS%d" 
)

Constructor for cDSA. This constructs a cDSA object to communicate with the remote DSACON32m controller within the SDH via RS232.

The connection is opened and established, and the sensor, controller and matrix info is queried from the remote DSACON32m controller. This initialization may take up to 9 seconds, since the DSACON32m controller needs > 8 seconds for "booting". If the SDH is already powered for some time then this will be much quicker.

Parameters:
debug_level- the level of debug messages to be printed:

  • if > 0 (1,2,3...) then debug messages of cDSA itself are printed
  • if > 1 (2,3,...) then debug messages of the low level communication interface object are printed too
port- the RS232 port to use for communication. (port 0 = ttyS0 = COM1, port 1 = ttyS1 = COM2, ...)
device_format_string- a format string (C string) for generating the device name, like "/dev/ttyS%d" (default) or "/dev/ttyUSB%d". Must contain a d where the port number should be inserted. This char array is duplicated on construction. When compiled with VCC (MS-Visual C++) then this is not used.
cDSA::cDSA ( int  debug_level,
char const *  _tcp_adr,
int  _tcp_port = 13000,
double  _timeout = 1.0 
)

Constructor for cDSA. This constructs a cDSA object to communicate with the remote DSACON32m controller within the SDH via TCP/IP (ethernet).

The connection is opened and established, and the sensor, controller and matrix info is queried from the remote DSACON32m controller. This initialization may take up to 9 seconds, since the DSACON32m controller needs > 8 seconds for "booting". If the SDH is already powered for some time then this will be much quicker.

Parameters:
debug_level- the level of debug messages to be printed:

  • if > 0 (1,2,3...) then debug messages of cDSA itself are printed
  • if > 1 (2,3,...) then debug messages of the low level communication interface object are printed too
_tcp_adr: The tcp host address of the SDH. Either a numeric IP as string or a hostname
_tcp_port: the tcp port number on the SDH to connect to
_timeout: The timeout to use:

  • <= 0 : wait forever (default)
  • T : wait for T seconds
cDSA::~cDSA ( )

Destructur: clean up and delete dynamically allocated memory.


Member Function Documentation

void cDSA::Close ( void  ) throw (cDSAException*, cSDHErrorCommunication*)

Set the framerate of the remote DSACON32m controller to 0 and close connection to it.

char const * cDSA::ErrorCodeToString ( eDSAErrorCode  error_code) [static]

Return a short string description for error_code

Parameters:
error_code- error code as returned from the remote DSACON32m tactile sensor controller
Returns:
short string description for error_code
static char const* SDH::cDSA::ErrorCodeToString ( UInt16  error_code) [inline, static]
unsigned long SDH::cDSA::GetAgeOfFrame ( sTactileSensorFrame frame_p) [inline]

return age of frame in ms (time in ms from frame sampling until now)

double cDSA::GetContactArea ( int  m)

Return contact area in mm*mm for matrix with index m

cDSA::sContactInfo cDSA::GetContactInfo ( int  m)

Return a sContactInfo struct (force,cog_x,cog_y,area) of contact force and center of gravity and contact area of that force for matrix with index m

sControllerInfo const& SDH::cDSA::GetControllerInfo ( void  ) const [inline]

Return a reference to the sControllerInfo read from the remote DSACON32m controller.

sTactileSensorFrame const& SDH::cDSA::GetFrame ( ) const [inline]

return a reference to the latest tactile sensor frame without reading from remote DSACON32m

int SDH::cDSA::GetMatrixIndex ( int  fi,
int  part 
) [inline]

return the matrix index of the sensor matrix attached to finger with index fi [1..3] and part [0,1] = [proximal,distal]

sMatrixInfo const& SDH::cDSA::GetMatrixInfo ( int  m) const [inline]

Return a reference to the sMatrixInfo of matrix m read from the remote DSACON32m controller.

cDSA::sSensitivityInfo cDSA::GetMatrixSensitivity ( int  matrix_no) throw (cDSAException*, cSDHErrorCommunication*)

Send command to get matrix sensitivity. Returns sensitivities of matrix no matrix_no.

A struct is returned containing the members error_code - see DSACON32 Command Set Reference Manual adj_flags - see DSACON32 Command Set Reference Manual cur_sens - the currently set sensitivity as float [0.0 .. 1.0] (0.0 is minimum, 1.0 is maximum sensitivity) fact_sens - the factory sensitivity as float [0.0 .. 1.0] (0.0 is minimum, 1.0 is maximum sensitivity)

Raises a cDSAException in case of invalid responses from the remote DSACON32m controller.

Bug:
With DSACON32m firmware R218 and before this did not work, instead the factory default (0.5) was always reported
=> Resolved in DSACON32m firmware R268
UInt16 cDSA::GetMatrixThreshold ( int  matrix_no) throw (cDSAException*, cSDHErrorCommunication*)

Send command to get matrix threshold. Returns threshold of matrix no matrix_no.

Returns:
threshold - the currently set threshold as integer [0 .. 4095] (0 is minimum, 4095 is maximum threshold)

Raises a cDSAException in case of invalid responses from the remote DSACON32m controller.

Remarks:
Getting the matrix threshold is only possible if the DSACON32m firmware is R268 or above.
sSensorInfo const& SDH::cDSA::GetSensorInfo ( void  ) const [inline]

Return a reference to the sSensorInfo read from the remote DSACON32m controller.

cDSA::tTexel cDSA::GetTexel ( int  m,
int  x,
int  y 
) const

Return texel value at column x row y of matrix m of the last frame

void cDSA::Open ( void  ) throw (cDSAException*, cSDHErrorCommunication*)

(Re-)open connection to DSACON32m controller, this is called by the constructor automatically, but is still usefull to call after a call to Close()

void cDSA::ParseFrame ( sResponse response,
sTactileSensorFrame frame_p 
) throw (cDSAException*) [protected]

Parse a full frame response from remote DSA

void cDSA::QueryControllerInfo ( sControllerInfo _controller_info) throw (cDSAException*, cSDHErrorCommunication*) [protected]

Send command to resquest controller info from remote DSACON32m. Read and parse the response from the remote DSACON32m.

void cDSA::QueryMatrixInfo ( sMatrixInfo _matrix_info,
int  matrix_no 
) throw (cDSAException*, cSDHErrorCommunication*) [protected]

Send command to request matrix info from remote DSACON32m. Read and parse the response from the remote DSACON32m.

void cDSA::QueryMatrixInfos ( void  ) throw (cDSAException*, cSDHErrorCommunication*) [protected]

Query all matrix infos

void cDSA::QuerySensorInfo ( sSensorInfo _sensor_info) throw (cDSAException*, cSDHErrorCommunication*) [protected]

Send command to request sensor info from remote DSACON32m. Read and parse the response from the remote DSACON32m.

void cDSA::ReadControllerInfo ( sControllerInfo _controller_info) throw (cDSAException*, cSDHErrorCommunication*) [protected]

Read and parse a controller info response from the remote DSA

void cDSA::ReadFrame ( sTactileSensorFrame frame_p) throw (cDSAException*, cSDHErrorCommunication*) [protected]

read and parse a full frame response from remote DSA

void cDSA::ReadMatrixInfo ( sMatrixInfo _matrix_info) throw (cDSAException*, cSDHErrorCommunication*) [protected]

Read and parse a matrix info response from the remote DSA

void cDSA::ReadResponse ( sResponse response,
UInt8  command_id 
) throw (cDSAException*, cSDHErrorCommunication*) [protected]

Read any response from the remote DSACON32m, expect the command_id as command id

  • tries to find the preamble within the next at most DSA_MAX_PREAMBLE_SEARCH bytes from the device
  • reads the packet id and size
  • reads all data indicated by the size read
  • if there is enough space in the payload of the response then the received data is stored there if there is not enough space in the payload of the response then the data is received but forgotten (to keep the communication line clear)
  • if sent, the CRC checksum is read and the data is checked. For invalid data an exception is thrown
void cDSA::ReadSensorInfo ( sSensorInfo _sensor_info) throw (cDSAException*, cSDHErrorCommunication*) [protected]

Read and parse a sensor info response from the remote DSA

struct SDH::cDSA::sResponse SDH::cDSA::SDH__attribute__ ( (__packed__)  ) [protected]
void cDSA::SetFramerate ( UInt16  framerate,
bool  do_RLE = true,
bool  do_data_acquisition = true 
) throw (cDSAException*, cSDHErrorCommunication*)

Set the framerate for querying full frames.

Parameters:
framerate- rate of frames.
do_RLE- flag, if true then use RLE (run length encoding) for sending frames
do_data_acquisition- flag, enable or disable data acquisition. Must be true if you want to get new frames
  • Use framerate=0 and do_data_acquisition=false to make the remote DSACON32m in SDH stop sending frames
  • Use framerate=0 and do_data_acquisition=true to read a single frame
  • Use framerate>0 and do_data_acquisition=true to make the remote DSACON32m in SDH send frames at the highest possible rate (for now: 30 FPS (frames per second)).
Bug:
SCHUNK-internal bugzilla ID: Bug 680
With DSACON32m firmware R276 and after and SDHLibrary-C++ v0.0.1.15 and before stopping of the sending did not work.
=> Resolved in SDHLibrary-C++ v0.0.1.16
Bug:
SCHUNK-internal bugzilla ID: Bug 680
With DSACON32m firmware before R276 and SDHLibrary-C++ v0.0.1.16 and before acquiring of single frames did not work
=> Resolved in SDHLibrary-C++ v0.0.1.17
Bug:
SCHUNK-internal bugzilla ID: Bug 703
With DSACON32m firmware R288 and before and SDHLibrary-C++ v0.0.2.1 and before tactile sensor frames could not be read reliably in single frame mode.
=> Resolved in DSACON32m firmware 2.9.0.0
=> Resolved in SDHLibrary-C++ v0.0.2.1 with workaround for older DSACON32m firmwares
void cDSA::SetFramerateRetries ( UInt16  framerate,
bool  do_RLE = true,
bool  do_data_acquisition = true,
unsigned int  retries = 0,
bool  ignore_exceptions = false 
) throw (cDSAException*, cSDHErrorCommunication*)

Set the framerate for querying full frames.

Parameters:
framerate- rate of frames. 0 will make the remote DSACON32m in SDH stop sending frames, any value > 0 will make the remote DSACON32m in SDH send frames at the highest possible rate (for now: 30 FPS (frames per second)).
do_RLE- flag, if true then use RLE (run length encoding) for sending frames
do_data_acquisition- flag, enable or disable data acquisition. Must be true if you want to get new frames
retries- number of times the sending will be retried in case of an error (like timeout while waiting for response)
ignore_exceptions- flag, if true then exceptions are ignored in case of error. After retries tries the function just returns even in case of an error
void cDSA::SetMatrixSensitivity ( int  matrix_no,
double  sensitivity,
bool  do_all_matrices = false,
bool  do_reset = false,
bool  do_persistent = false 
) throw (cDSAException*, cSDHErrorCommunication*)

Send command to set matrix sensitivity to sensitivity as float [0.0 .. 1.0] (0.0 is minimum, 1.0 is maximum sensitivity). If do_all_matrices is true then the sensitivity is set for all matrices. If do_reset is true then the sensitivity is reset to the factory default. If do_persistent is true then the sensitivity is saved persistently to configuration memory and will thus remain after the next power off/power on cycle and will become the new factory default value. If do_persistent is false (default) then the value will be reset to default on the next power off/power on cycle

Warning:
PLEASE NOTE: the maximum write endurance of the configuration memory is about 100.000 times!

Raises a cDSAException in case of invalid responses from the remote DSACON32m controller.

Remarks:
Setting the matrix sensitivity persistently is only possible if the DSACON32m firmware is R268 or above.
void cDSA::SetMatrixThreshold ( int  matrix_no,
UInt16  threshold,
bool  do_all_matrices = false,
bool  do_reset = false,
bool  do_persistent = false 
) throw (cDSAException*, cSDHErrorCommunication*)

Send command to set matrix threshold to threshold as integer [0 .. 4095] (0 is minimum, 4095 is maximum threshold). If do_all_matrices is true then the threshold is set for all matrices. If do_reset is true then the threshold is reset to the factory default. If do_persistent is true then the threshold is saved persistently to configuration memory and will thus remain after the next power off/power on cycle and will become the new factory default value. If do_persistent is false (default) then the value will be reset to default on the next power off/power on cycle

Warning:
PLEASE NOTE: the maximum write endurance of the configuration memory is about 100.000 times!

Raises a cDSAException in case of invalid responses from the remote DSACON32m controller.

Remarks:
Getting the matrix threshold is only possible if the DSACON32m firmware is R268 or above.
sTactileSensorFrame const& SDH::cDSA::UpdateFrame ( ) [inline]

read the tactile sensor frame from remote DSACON32m and return a reference to it. A command to query the frame (periodically) must have been sent before.

void SDH::cDSA::WriteCommand ( UInt8  command) [inline, protected]
void cDSA::WriteCommandWithPayload ( UInt8  command,
UInt8 payload,
UInt16  payload_len 
) throw (cDSAException*, cSDHErrorCommunication*) [protected]

Friends And Related Function Documentation

VCC_EXPORT std::ostream& operator<< ( std::ostream &  stream,
cDSA::sResponse const &  response 
) [friend]

Member Data Documentation

double SDH::cDSA::calib_pressure [protected]

For the voltage to pressure conversion in _VoltageToPressure() enter one pressure/voltage measurement here (from demo-dsa.py --calibration):

double SDH::cDSA::calib_voltage [protected]

see calib_pressure

the communication interface to use

threshold of texel cell value for detecting contacts with GetContactArea

threshold of texel cell value for detecting forces with GetContactForce

the controller info read from the remote DSACON32m controller

cDBG SDH::cDSA::dbg [protected]

A stream object to print coloured debug messages.

bool SDH::cDSA::do_RLE [protected]

flag, true if data should be sent Run Length Encoded by the remote DSACON32m controller

double SDH::cDSA::force_factor [protected]

additional calibration factor for forces in GetContactForce

the latest frame received from the remote DSACON32m controller

flag, if True then the ReadFrame() function will read tactile sensor frames until a timeout occurs. This will ignore intermediate frames as long as new ones are available. This was usefull some time in the past, or if you have a slow computer that cannot handle incoming frames fast enough. If False then any completely read valid frame will be reported. With new computers and fast communication like via TCP this should remain at "False".

the matrix infos read from the remote DSACON32m controller

int SDH::cDSA::nb_cells [protected]

The total number of sensor cells.

long SDH::cDSA::read_timeout_us [protected]

default timeout used for reading in us

the sensor info read from the remote DSACON32m controller

int* SDH::cDSA::texel_offset [protected]

an array with the offsets of the first texel of all matrices into the whole frame


The documentation for this class was generated from the following files: