sdh.h

Go to the documentation of this file.

//======================================================================
//======================================================================

#ifndef SDH_h_
#define SDH_h_

#include "sdhlibrary_settings.h"

#if SDH_USE_VCC
# pragma warning(disable : 4996)
#else
// must be included first on some linuxes
extern "C" {
# include <stdint.h>
}
#endif

#include "basisdef.h"

  //----------------------------------------------------------------------
// System Includes - include with <>
//----------------------------------------------------------------------

#include <vector>
#include <string>

//----------------------------------------------------------------------
// Project Includes - include with ""
//----------------------------------------------------------------------

#include "sdhbase.h"
#include "sdhserial.h"
#include "unit_converter.h"
#include "serialbase.h"

//----------------------------------------------------------------------
// Defines, enums, unions, structs
//----------------------------------------------------------------------

#if SDH_USE_NAMESPACE

#endif

NAMESPACE_SDH_START

//----------------------------------------------------------------------
// Global variables (declarations)
//----------------------------------------------------------------------


//----------------------------------------------------------------------
// External functions and classes (declarations)
//----------------------------------------------------------------------


//----------------------------------------------------------------------
// Function prototypes (function declarations)
//----------------------------------------------------------------------


//----------------------------------------------------------------------
// Class declarations
//----------------------------------------------------------------------

#if SDH_USE_VCC
// these are needed by VCC for template instantiation into a DLL,
// see http://support.microsoft.com/default.aspx?scid=kb;EN-US;q168958
// and http://www.unknownroad.com/rtfm/VisualStudio/warningC4251.html
//
VCC_EXPORT_TEMPLATE template class VCC_EXPORT std::allocator<int>;
VCC_EXPORT_TEMPLATE template class VCC_EXPORT std::vector<int,std::allocator<int> >;

VCC_EXPORT_TEMPLATE template class VCC_EXPORT std::allocator<std::vector<int> >;
VCC_EXPORT_TEMPLATE template class VCC_EXPORT std::vector<std::vector<int>,std::allocator<std::vector<int> > >;

VCC_EXPORT_TEMPLATE template class VCC_EXPORT std::allocator<double>;
VCC_EXPORT_TEMPLATE template class VCC_EXPORT std::vector<double,std::allocator<double> >;

VCC_EXPORT_TEMPLATE template class VCC_EXPORT std::allocator<std::vector<double> >;
VCC_EXPORT_TEMPLATE template class VCC_EXPORT std::vector<std::vector<double>,std::allocator<std::vector<double> > >;
#endif

//======================================================================

class VCC_EXPORT cSDH : public cSDHBase
{
public:

    enum eMotorCurrentMode
    {
        eMCM_MOVE=0, 
        eMCM_GRIP=1, 
        eMCM_HOLD=2, 

        eMCM_DIMENSION 
    };


    enum eAxisState
    {
        eAS_IDLE = 0,            
        eAS_POSITIONING,         
        eAS_SPEED_MODE,          
        eAS_NOT_INITIALIZED,     
        eAS_CW_BLOCKED,          
        eAS_CCW_BLOCKED,         
        eAS_DISABLED,            
        eAS_LIMITS_REACHED,      

        eAS_DIMENSION 
    };


    //#######################################################################

    static cUnitConverter const uc_angle_degrees;

    static cUnitConverter const uc_angle_radians;

    static cUnitConverter const uc_time_seconds;

    static cUnitConverter const uc_time_milliseconds;

    static cUnitConverter const uc_temperature_celsius;

    static cUnitConverter const uc_temperature_fahrenheit;

    static cUnitConverter const uc_angular_velocity_degrees_per_second;

    static cUnitConverter const uc_angular_velocity_radians_per_second;

    static cUnitConverter const uc_angular_acceleration_degrees_per_second_squared;

    static cUnitConverter const uc_angular_acceleration_radians_per_second_squared;

    static cUnitConverter const uc_motor_current_ampere;

    static cUnitConverter const uc_motor_current_milliampere;

    static cUnitConverter const uc_position_millimeter;

    static cUnitConverter const uc_position_meter;


    //  end of doxygen name group sdhlibrary_cpp_sdh_h_unit_conversion_objects

protected:
    //---------------------
    // Misc member variables

    int NUMBER_OF_AXES_PER_FINGER;


    int NUMBER_OF_VIRTUAL_AXES;

    int nb_all_axes;

    std::vector<int> finger_number_of_axes;


    std::vector<std::vector<int> > finger_axis_index;

    //f_eps_v;

    std::vector<double> f_zeros_v;

    std::vector<double> f_ones_v;


    std::vector<double> zeros_v;

    std::vector<double> ones_v;


    std::vector<double> f_min_motor_current_v;

    std::vector<double> f_max_motor_current_v;



    std::vector<double> f_min_angle_v;

    std::vector<double> f_max_angle_v;

    std::vector<double> f_min_velocity_v;

    std::vector<double> f_max_velocity_v;

    std::vector<double> f_min_acceleration_v;

    std::vector<double> f_max_acceleration_v;

    double grip_max_velocity;


    double l1;

    double l2;

    // distance between center points of base joints f0<->f1, f1<->f2, f0<->f2
    double d;

    // height of center of base joints above finger base plate
    double h;

    std::vector<std::vector<double> > offset;

    cSerialBase* com;

    // !!! make this public for now (to access comm_interface->ref() / comm_interface->pos_save() for hands with missing absolute encoders)
public:
    cSDHSerial comm_interface;

    virtual void SetDebugOutput( std::ostream* debuglog )
    {
        cSDHBase::SetDebugOutput( debuglog );
        comm_interface.SetDebugOutput( debuglog );
    }

protected:

    //######################################################################
    //----------------------------------------------------------------------
    std::vector<double> SetAxisValueVector( std::vector<int> const& axes,
                                            std::vector<double> const& values,
                                            pSetFunction ll_set,
                                            pGetFunction ll_get,
                                            cUnitConverter const* uc,
                                            std::vector<double> const& min_values,
                                            std::vector<double> const& max_values,
                                            char const* name )
                                            throw (cSDHLibraryException*);


    //----------------------------------------------------------------------
    std::vector<double> GetAxisValueVector( std::vector<int> const& axes,
                                            pGetFunction ll_get,
                                            cUnitConverter const* uc,
                                            char const* name )
        throw (cSDHLibraryException*);

    //-----------------------------------------------------------------
    std::vector<int> ToIndexVector( int index, std::vector<int>& all_replacement, int maxindex, char const* name )
        throw (cSDHLibraryException*);


    //-----------------------------------------------------------------
    pSetFunction GetMotorCurrentModeFunction( eMotorCurrentMode mode )
        throw(cSDHLibraryException*);


    //-----------------------------------------------------------------
    std::vector<double> _GetFingerXYZ( int fi, std::vector<double> r_angles )
        throw(cSDHLibraryException*);


    //  end of doxygen name group sdhlibrary_cpp_sdh_h_csdh_internal
    //#####################################################################


public:
    //----------------------------------------------------------------------

    std::vector<int> all_axes;

    std::vector<int> all_real_axes;

    std::vector<int> all_fingers;

    std::vector<int> all_temperature_sensors;

    //  end of sdhlibrary_cpp_sdh_h_index_vectors
    //----------------------------------------------------------------------


    //----------------------------------------------------------------------

    const cUnitConverter* uc_angle;


    const cUnitConverter* uc_angular_velocity;


    const cUnitConverter* uc_angular_acceleration;


    const cUnitConverter* uc_time;


    const cUnitConverter* uc_temperature;


    const cUnitConverter* uc_motor_current;


    const cUnitConverter* uc_position;
    //---------------------

    // end of sdhlibrary_cpp_sdh_h_unit_conversion_ptrs
    //----------------------------------------------------------------------


    //-----------------------------------------------------------------
    cSDH( bool _use_radians=false, bool _use_fahrenheit=false, int _debug_level=0 );


    //----------------------------------------------------------------------
    virtual ~cSDH();


    //######################################################################
    //----------------------------------------------------------------------
    bool IsVirtualAxis( int iAxis )
        throw (cSDHLibraryException*);

    //-----------------------------------------------------------------
    void UseRadians( void );


    //-----------------------------------------------------------------
    void UseDegrees( void );


    //-----------------------------------------------------------------
    int GetFingerNumberOfAxes( int iFinger )
        throw (cSDHLibraryException*);


    //-----------------------------------------------------------------
    int GetFingerAxisIndex( int iFinger, int iFingerAxis )
        throw (cSDHLibraryException*);


    //-----------------------------------------------------------------
    static char const* GetLibraryRelease( void );


    //-----------------------------------------------------------------
    static char const* GetLibraryName( void );


    //-----------------------------------------------------------------
    char const* GetFirmwareRelease( void )
        throw (cSDHLibraryException*);

    char const* GetInfo( char const* what )
        throw (cSDHLibraryException*);

    //-----------------------------------------------------------------
    std::vector<double> GetTemperature( std::vector<int> const& sensors )
        throw (cSDHLibraryException*);


    //----------------------------------------------------------------------
    double GetTemperature( int iSensor )
        throw (cSDHLibraryException*);


    //  end of doxygen name group sdhlibrary_cpp_sdh_h_csdh_common
    //#####################################################################


    //######################################################################
    //-----------------------------------------------------------------
    void OpenRS232(  int _port=0, unsigned long _baudrate = 115200, double _timeout=-1, char const* _device_format_string="/dev/ttyS%d" )
        throw (cSDHLibraryException*);


    void OpenCAN_ESD(  int _net=0,  unsigned long _baudrate=1000000, double _timeout=0.0, Int32 _id_read=43, Int32 _id_write=42 )
        throw (cSDHLibraryException*);


    void OpenCAN_ESD(  tDeviceHandle _ntcan_handle, double _timeout=0.0, Int32 _id_read=43, Int32 _id_write=42 )
        throw (cSDHLibraryException*);


    void OpenCAN_PEAK( unsigned long _baudrate=1000000, double _timeout=0.0, Int32 _id_read=43, Int32 _id_write=42, const char *_device="/dev/pcanusb0" )
        throw (cSDHLibraryException*);


    void OpenCAN_PEAK(  tDeviceHandle _handle, double _timeout=0.0, Int32 _id_read=43, Int32 _id_write=42 )
        throw (cSDHLibraryException*);

    void OpenTCP( char const* _tcp_adr="192.168.1.1", int _tcp_port=23, double _timeout=0.0 )
        throw (cSDHLibraryException*);

    //-----------------------------------------------------------------
    void Close( bool leave_enabled=false )
        throw (cSDHLibraryException*);

    //-----------------------------------------------------------------
    virtual bool IsOpen( void )
        throw ();


    //  end of doxygen name group sdhlibrary_cpp_sdh_h_csdh_communication
    //#####################################################################


    //######################################################################
    //-----------------------------------------------------------------
    void EmergencyStop( void )
        throw (cSDHLibraryException*);


    //-----------------------------------------------------------------
    void Stop( void )
        throw (cSDHLibraryException*);


    //-----------------------------------------------------------------
    //-----------------------------------------------------------------
    // unimplemented from SAH:
    // def GetEmergencyStop( int* piBrakeState)

    //-----------------------------------------------------------------
    void SetController( cSDHBase::eControllerType controller )
        throw( cSDHLibraryException* );

    //-----------------------------------------------------------------
    eControllerType GetController( void  )
        throw (cSDHLibraryException*);

    //-----------------------------------------------------------------
    void SetVelocityProfile( eVelocityProfile velocity_profile )
        throw (cSDHLibraryException*);


    //-----------------------------------------------------------------
    eVelocityProfile GetVelocityProfile( void )
        throw (cSDHLibraryException*);


    //  end of doxygen name group sdhlibrary_cpp_sdh_h_csdh_auxilliary
    //#####################################################################


    //######################################################################
    //-----------------------------------------------------------------
    void SetAxisMotorCurrent( std::vector<int> const& axes, std::vector<double> const& motor_currents, eMotorCurrentMode mode=eMCM_MOVE )
        throw (cSDHLibraryException*);


    //----------------------------------------------------------------------
    void SetAxisMotorCurrent( int iAxis, double motor_current, eMotorCurrentMode mode=eMCM_MOVE )
        throw (cSDHLibraryException*);


    //-----------------------------------------------------------------
    std::vector<double> GetAxisMotorCurrent( std::vector<int> const& axes, eMotorCurrentMode mode=eMCM_MOVE )
        throw (cSDHLibraryException*);


    //----------------------------------------------------------------------
    double GetAxisMotorCurrent( int iAxis, eMotorCurrentMode mode=eMCM_MOVE )
        throw (cSDHLibraryException*);


    //-----------------------------------------------------------------
    void SetAxisEnable( std::vector<int> const& axes, std::vector<double> const& states )
        throw (cSDHLibraryException*);


    //----------------------------------------------------------------------
    void SetAxisEnable( int iAxis=All, double state=1.0 )
        throw (cSDHLibraryException*);


    //----------------------------------------------------------------------
    void SetAxisEnable( std::vector<int> const& axes, std::vector<bool> const& states )
        throw (cSDHLibraryException*);


    //----------------------------------------------------------------------
    void SetAxisEnable( int iAxis=All, bool state=true )
        throw (cSDHLibraryException*);


    //-----------------------------------------------------------------
    std::vector<double> GetAxisEnable( std::vector<int> const& axes )
        throw (cSDHLibraryException*);


    //----------------------------------------------------------------------
    double GetAxisEnable( int iAxis )
        throw (cSDHLibraryException*);


    //----------------------------------------------------------------------
    std::vector<eAxisState> GetAxisActualState( std::vector<int> const& axes )
        throw (cSDHLibraryException*);


    //----------------------------------------------------------------------
    eAxisState GetAxisActualState( int iAxis )
        throw (cSDHLibraryException*);


    //-----------------------------------------------------------------
    void WaitAxis( std::vector<int> const& axes, double timeout = -1.0 )
        throw (cSDHLibraryException*);



    //----------------------------------------------------------------------
    void WaitAxis( int iAxis, double timeout = -1.0 )
        throw (cSDHLibraryException*);


    //----------------------------------------------------------------------
    void SetAxisTargetAngle( std::vector<int> const& axes, std::vector<double> const& angles )
        throw (cSDHLibraryException*);


    //----------------------------------------------------------------------
    void SetAxisTargetAngle( int iAxis, double angle )
        throw (cSDHLibraryException*);


    //----------------------------------------------------------------------
    std::vector<double> SetAxisTargetGetAxisActualAngle( std::vector<int> const& axes, std::vector<double> const& angles )
        throw (cSDHLibraryException*);


    //-----------------------------------------------------------------
    std::vector<double> GetAxisTargetAngle( std::vector<int> const& axes )
        throw (cSDHLibraryException*);


    //----------------------------------------------------------------------
    double GetAxisTargetAngle( int iAxis )
        throw (cSDHLibraryException*);


    //-----------------------------------------------------------------
    std::vector<double> GetAxisActualAngle( std::vector<int> const& axes )
        throw (cSDHLibraryException*);


    //----------------------------------------------------------------------
    double GetAxisActualAngle( int iAxis )
        throw (cSDHLibraryException*);


    //-----------------------------------------------------------------
    void SetAxisTargetVelocity( std::vector<int> const& axes, std::vector<double> const& velocities )
        throw (cSDHLibraryException*);


    //----------------------------------------------------------------------
    void SetAxisTargetVelocity( int iAxis, double velocity )
        throw (cSDHLibraryException*);


    //----------------------------------------------------------------------
    std::vector<double> SetAxisTargetGetAxisActualVelocity( std::vector<int> const& axes, std::vector<double> const& velocities )
        throw (cSDHLibraryException*);


    //-----------------------------------------------------------------
    std::vector<double> GetAxisTargetVelocity( std::vector<int> const& axes )
        throw (cSDHLibraryException*);


    //----------------------------------------------------------------------
    double GetAxisTargetVelocity( int iAxis )
        throw (cSDHLibraryException*);


    //-----------------------------------------------------------------
    std::vector<double> GetAxisLimitVelocity( std::vector<int> const& axes )
        throw (cSDHLibraryException*);


    //----------------------------------------------------------------------
    double GetAxisLimitVelocity( int iAxis )
        throw (cSDHLibraryException*);


    //-----------------------------------------------------------------
    std::vector<double> GetAxisLimitAcceleration( std::vector<int> const& axes )
        throw (cSDHLibraryException*);


    //----------------------------------------------------------------------
    double GetAxisLimitAcceleration( int iAxis )
        throw (cSDHLibraryException*);


    //-----------------------------------------------------------------
    std::vector<double> GetAxisActualVelocity( std::vector<int>const& axes )
        throw (cSDHLibraryException*);


    //----------------------------------------------------------------------
    double GetAxisActualVelocity( int iAxis )
        throw (cSDHLibraryException*);


    //-----------------------------------------------------------------
    std::vector<double> GetAxisReferenceVelocity( std::vector<int>const& axes )
        throw (cSDHLibraryException*);


    //----------------------------------------------------------------------
    double GetAxisReferenceVelocity( int iAxis )
        throw (cSDHLibraryException*);


    //-----------------------------------------------------------------
    void SetAxisTargetAcceleration( std::vector<int>const& axes, std::vector<double>const& accelerations )
        throw (cSDHLibraryException*);


    //----------------------------------------------------------------------
    void SetAxisTargetAcceleration( int iAxis, double acceleration )
        throw (cSDHLibraryException*);


    //-----------------------------------------------------------------
    std::vector<double> GetAxisTargetAcceleration( std::vector<int>const& axes )
        throw (cSDHLibraryException*);


    //----------------------------------------------------------------------
    double GetAxisTargetAcceleration( int iAxis )
        throw (cSDHLibraryException*);


    //-----------------------------------------------------------------
    std::vector<double> GetAxisMinAngle( std::vector<int> const& axes )
        throw (cSDHLibraryException*);


    //----------------------------------------------------------------------
    double GetAxisMinAngle( int iAxis )
        throw (cSDHLibraryException*);


    //-----------------------------------------------------------------
    std::vector<double> GetAxisMaxAngle( std::vector<int> const& axes )
        throw (cSDHLibraryException*);


    //----------------------------------------------------------------------
    double GetAxisMaxAngle( int iAxis )
        throw (cSDHLibraryException*);


    //-----------------------------------------------------------------
    std::vector<double> GetAxisMaxVelocity( std::vector<int> const& axes )
        throw (cSDHLibraryException*);


    //----------------------------------------------------------------------
    double GetAxisMaxVelocity( int iAxis )
        throw (cSDHLibraryException*);


    //-----------------------------------------------------------------
    std::vector<double> GetAxisMaxAcceleration( std::vector<int> const& axes )
        throw (cSDHLibraryException*);


    //----------------------------------------------------------------------
    double GetAxisMaxAcceleration( int iAxis )
        throw (cSDHLibraryException*);


    //-----------------------------------------------------------------
    double MoveAxis( std::vector<int>const& axes, bool sequ=true )
        throw (cSDHLibraryException*);


    //----------------------------------------------------------------------
    double MoveAxis( int iAxis, bool sequ=true )
        throw (cSDHLibraryException*);


    // unimplemented from SAH:
    // def GetJointAngle( int iFinger,double* pafAngle);
    // def GetJointSpeed( int iFinger,double* pafSpeed);
    // def GetJointTorque( int iFinger,double* pafTorque);


    //  end of doxygen name group sdhlibrary_cpp_sdh_h_csdh_axis
    //#####################################################################

    //######################################################################
    //-----------------------------------------------------------------
    void SetFingerEnable( std::vector<int> const& fingers, std::vector<double> const& states )
        throw (cSDHLibraryException*);


    //----------------------------------------------------------------------
    void SetFingerEnable( int iFinger, double state=1.0 )
        throw (cSDHLibraryException*);


    //----------------------------------------------------------------------
    void SetFingerEnable( std::vector<int> const& fingers, std::vector<bool> const& states )
        throw (cSDHLibraryException*);


    //----------------------------------------------------------------------
    void SetFingerEnable( int iFinger, bool state )
        throw (cSDHLibraryException*);


    //-----------------------------------------------------------------
    std::vector<double> GetFingerEnable( std::vector<int> const& fingers )
        throw (cSDHLibraryException*);


    //----------------------------------------------------------------------
    double GetFingerEnable( int iFinger )
        throw (cSDHLibraryException*);


    //-----------------------------------------------------------------
    void SetFingerTargetAngle( int iFinger, std::vector<double> const& angles )
        throw (cSDHLibraryException*);


    //-----------------------------------------------------------------
    void SetFingerTargetAngle( int iFinger, double a0, double a1, double a2 )
        throw (cSDHLibraryException*);


    //-----------------------------------------------------------------
    std::vector<double> GetFingerTargetAngle( int iFinger )
        throw (cSDHLibraryException*);


    //-----------------------------------------------------------------
    void GetFingerTargetAngle( int iFinger, double& a0, double& a1, double& a2 )
        throw (cSDHLibraryException*);


    //-----------------------------------------------------------------
    std::vector<double> GetFingerActualAngle( int iFinger )
        throw (cSDHLibraryException*);


    //-----------------------------------------------------------------
    void GetFingerActualAngle( int iFinger, double& a0, double& a1, double& a2 )
        throw (cSDHLibraryException*);


    //-----------------------------------------------------------------
    std::vector<double> GetFingerMinAngle( int iFinger )
        throw (cSDHLibraryException*);


    //-----------------------------------------------------------------
    void GetFingerMinAngle( int iFinger, double& a0, double& a1, double& a2 )
        throw (cSDHLibraryException*);


    //-----------------------------------------------------------------
    std::vector<double> GetFingerMaxAngle( int iFinger )
        throw (cSDHLibraryException*);


    //-----------------------------------------------------------------
    void GetFingerMaxAngle( int iFinger, double& a0, double& a1, double& a2 )
        throw (cSDHLibraryException*);



    //-----------------------------------------------------------------
    std::vector<double> GetFingerXYZ( int iFinger, std::vector<double> const& angles )
        throw (cSDHLibraryException*);


    //-----------------------------------------------------------------
    std::vector<double> GetFingerXYZ( int iFinger, double a0, double a1, double a2 )
        throw (cSDHLibraryException*);


    //-----------------------------------------------------------------
    double MoveFinger( std::vector<int>const& fingers, bool sequ=true )
        throw (cSDHLibraryException*);


    //----------------------------------------------------------------------
    double MoveFinger( int iFinger, bool sequ=true )
        throw (cSDHLibraryException*);


    //-----------------------------------------------------------------
    double MoveHand( bool sequ=true )
        throw (cSDHLibraryException*);


    //  end of doxygen name group sdhlibrary_cpp_sdh_h_csdh_finger
    //#####################################################################


    //######################################################################
    //-----------------------------------------------------------------
    double GetGripMaxVelocity( void );


    //-----------------------------------------------------------------
    double GripHand( eGraspId grip, double close, double velocity, bool sequ=true )
        throw (cSDHLibraryException*);


    //  end of doxygen name group sdhlibrary_cpp_sdh_h_csdh_grip
    //#####################################################################

private:
    void UpdateSettingsFromSDH();

    void AdjustLimits( cSDHBase::eControllerType controller );


    std::string release_firmware;

    eControllerType controller_type;

    // unimplemented from SAH:
    // def GetFingerTipFT( int iFinger,double* pafFTData);

    // def GetFingerEnableState( int iFinger, int* piEnableState);

    // def GetCommandedValues( int iFinger, double* pafAngle,double* pafVelocity);
    // def GetHandConfig( int* piConfig);
    // def GetFingerLimitsStatus( int iPort,int iFinger,int* paiLimitStatus);
    // def ClearTorqueSensorOffset( int iPort,int iFinger);

    // def SetStiffnessFactor( int iFinger, double* pafStiffnessFactor);



}; // end of class cSDH
//#####################################################################

NAMESPACE_SDH_END

#endif


//======================================================================
/*
  Here are some settings for the emacs/xemacs editor (and can be safely ignored)
  (e.g. to explicitely set C++ mode for *.h header files)

  Local Variables:
  mode:C++
  mode:ELSE
  End:
*/
//======================================================================]