CGenericHapticDevice.h

Go to the documentation of this file.

//==============================================================================
/*
    Software License Agreement (BSD License)
    Copyright (c) 2003-2016, CHAI3D.
    (www.chai3d.org)

    All rights reserved.

    Redistribution and use in source and binary forms, with or without
    modification, are permitted provided that the following conditions
    are met:

    * Redistributions of source code must retain the above copyright
    notice, this list of conditions and the following disclaimer.

    * Redistributions in binary form must reproduce the above
    copyright notice, this list of conditions and the following
    disclaimer in the documentation and/or other materials provided
    with the distribution.

    * Neither the name of CHAI3D nor the names of its contributors may
    be used to endorse or promote products derived from this software
    without specific prior written permission.

    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
    COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
    POSSIBILITY OF SUCH DAMAGE. 

    \author    <http://www.chai3d.org>
    \author    Francois Conti
    \version   3.2.0 $Rev: 2141 $
*/
//==============================================================================

//------------------------------------------------------------------------------
#ifndef CGenericHapticDeviceH
#define CGenericHapticDeviceH
//------------------------------------------------------------------------------
#include "devices/CGenericDevice.h"
#include "math/CMaths.h"
#include "system/CGlobals.h"
#include "timers/CPrecisionClock.h"
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
namespace chai3d {
//------------------------------------------------------------------------------

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


//------------------------------------------------------------------------------
// GENERAL CONSTANTS
//------------------------------------------------------------------------------
const int C_DEVICE_HISTORY_SIZE = 200; // [number of samples]

const double C_DEVICE_MIN_ACQUISITION_TIME = 0.0001;   // [s]

const int C_MAX_DOF = 16;

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


//==============================================================================
//==============================================================================
enum cHapticDeviceModel
{
    C_HAPTIC_DEVICE_VIRTUAL,
    C_HAPTIC_DEVICE_DELTA_3,
    C_HAPTIC_DEVICE_DELTA_6,
    C_HAPTIC_DEVICE_OMEGA_3,
    C_HAPTIC_DEVICE_OMEGA_6,
    C_HAPTIC_DEVICE_OMEGA_7,
    C_HAPTIC_DEVICE_SIGMA_6P,
    C_HAPTIC_DEVICE_SIGMA_7,
    C_HAPTIC_DEVICE_FALCON,
    C_HAPTIC_DEVICE_XTH_1,
    C_HAPTIC_DEVICE_XTH_2,
    C_HAPTIC_DEVICE_MPR,
    C_HAPTIC_DEVICE_KSD_1,
    C_HAPTIC_DEVICE_VIC_1,
    C_HAPTIC_DEVICE_PHANTOM_TOUCH,
    C_HAPTIC_DEVICE_PHANTOM_OMNI,
    C_HAPTIC_DEVICE_PHANTOM_DESKTOP,
    C_HAPTIC_DEVICE_PHANTOM_15_6DOF,
    C_HAPTIC_DEVICE_PHANTOM_30_6DOF,
    C_HAPTIC_DEVICE_PHANTOM_OTHER,
    C_TRACKER_DEVICE_SIXENSE,
    C_TRACKER_DEVICE_LEAP,
    C_HAPTIC_DEVICE_CUSTOM
};


//------------------------------------------------------------------------------
#ifndef DOXYGEN_SHOULD_SKIP_THIS
//------------------------------------------------------------------------------

//==============================================================================
//==============================================================================
struct cTimestampValue
{
    double m_time;

    double m_value;
};


//==============================================================================
//==============================================================================
struct cTimestampPos
{
    double m_time;

    cVector3d m_pos;
};


//==============================================================================
//==============================================================================
struct cTimestampRot
{
    double m_time;

    cMatrix3d m_rot;
};


//------------------------------------------------------------------------------
#endif  // DOXYGEN_SHOULD_SKIP_THIS
//------------------------------------------------------------------------------


//==============================================================================
//==============================================================================
struct cHapticDeviceInfo
{
    cHapticDeviceModel m_model;

    std::string m_modelName;

    std::string m_manufacturerName;

    double m_maxLinearForce;

    double m_maxAngularTorque;

    double m_maxGripperForce;

    double m_maxLinearStiffness;

    double m_maxAngularStiffness;

    double m_maxGripperLinearStiffness;

    double m_maxLinearDamping;

    double m_maxAngularDamping;

    double m_maxGripperAngularDamping;

    double m_workspaceRadius;

    double m_gripperMaxAngleRad;

    bool m_sensedPosition;

    bool m_sensedRotation;

    bool m_sensedGripper;

    bool m_actuatedPosition;

    bool m_actuatedRotation;

    bool m_actuatedGripper;

    bool m_leftHand;

    bool m_rightHand;
};

//------------------------------------------------------------------------------
class cGenericHapticDevice;
typedef std::shared_ptr<cGenericHapticDevice> cGenericHapticDevicePtr;
//------------------------------------------------------------------------------

//==============================================================================
//==============================================================================
class cGenericHapticDevice : public cGenericDevice
{
    //--------------------------------------------------------------------------
    // CONSTRUCTOR & DESTRUCTOR:
    //--------------------------------------------------------------------------

public:

    cGenericHapticDevice(unsigned int a_deviceNumber = 0);

    virtual ~cGenericHapticDevice() {};

    static cGenericHapticDevicePtr create(unsigned int a_deviceNumber = 0) { return (std::make_shared<cGenericHapticDevice>(a_deviceNumber)); }


    //--------------------------------------------------------------------------
    // PUBLIC METHODS - GENERAL COMMANDS:
    //--------------------------------------------------------------------------
 
public:

    virtual bool open() { return (C_ERROR); }

    virtual bool close() { return (C_ERROR); }

    virtual bool calibrate(bool a_forceCalibration = false) { return (m_deviceReady); }

    virtual bool getPosition(cVector3d& a_position) { a_position.zero(); return (m_deviceReady); }

    virtual bool getLinearVelocity(cVector3d& a_linearVelocity) { a_linearVelocity = m_linearVelocity; return (m_deviceReady); }

    virtual bool getRotation(cMatrix3d& a_rotation) { a_rotation.identity(); return (m_deviceReady); }

    virtual bool getJointAnglesRad(double a_jointAnglesRad[C_MAX_DOF]) { return (C_ERROR); }

    virtual bool getAngularVelocity(cVector3d& a_angularVelocity) { a_angularVelocity = m_angularVelocity; return (m_deviceReady); }

    virtual bool getTransform(cTransform& a_transform);

    virtual bool getGripperAngleRad(double& a_angle);

    inline bool getGripperAngleDeg(double& a_angle) { double angle; bool result = getGripperAngleRad(angle); a_angle = cRadToDeg(angle); return (result); }

    virtual bool getGripperAngularVelocity(double& a_gripperAngularVelocity) { a_gripperAngularVelocity = m_gripperAngularVelocity; return (m_deviceReady); }

    virtual bool getForce(cVector3d& a_force) { a_force = m_prevForce; return (m_deviceReady); }

    virtual bool getTorque(cVector3d& a_torque) { a_torque = m_prevTorque; return (m_deviceReady); }

    virtual bool getGripperForce(double& a_gripperForce) { a_gripperForce = m_prevGripperForce; return (m_deviceReady); }

    virtual bool getUserSwitch(int a_switchIndex, bool& a_status);

    virtual bool getUserSwitches(unsigned int& a_userSwitches) { a_userSwitches = 0; return (m_deviceReady); }

    cHapticDeviceInfo getSpecifications() { return (m_specifications); }

    virtual void setEnableGripperUserSwitch(const bool a_status) { m_gripperUserSwitchEnabled = a_status; }

    virtual bool getEnableGripperUserSwitch() const { return (m_gripperUserSwitchEnabled); }

    bool setForce(const cVector3d& a_force) {  return (setForceAndTorqueAndGripperForce(a_force, cVector3d(0.0,0.0,0.0), 0.0)); }

    bool  setForceAndTorque(const cVector3d& a_force, const cVector3d& a_torque) { return (setForceAndTorqueAndGripperForce(a_force, a_torque, 0.0)); }

    virtual bool setForceAndTorqueAndGripperForce(const cVector3d& a_force, const cVector3d& a_torque, double a_gripperForce) { cSleepMs(1); return (m_deviceReady); }


    //--------------------------------------------------------------------------
    // PUBLIC STATIC METHODS:
    //--------------------------------------------------------------------------

public: 

    static unsigned int getNumDevices() { return (0); }


    //--------------------------------------------------------------------------
    // PUBLIC MEMBERS - GENERAL:
    //--------------------------------------------------------------------------

public:

    cHapticDeviceInfo m_specifications;


    //--------------------------------------------------------------------------
    // PROTECTED MEMBERS - CURRENT VALUES:
    //--------------------------------------------------------------------------

protected:

    cVector3d m_prevForce;

    cVector3d m_prevTorque;

    double m_prevGripperForce;

    cVector3d m_linearVelocity;

    cVector3d m_angularVelocity;

    double m_gripperAngularVelocity;


    //--------------------------------------------------------------------------
    // PROTECTED MEMBERS - VELOCITY ESTIMATION:
    //--------------------------------------------------------------------------

protected:

    cTimestampPos m_historyPos[C_DEVICE_HISTORY_SIZE];

    cTimestampRot m_historyRot[C_DEVICE_HISTORY_SIZE];

    cTimestampValue m_historyGripper[C_DEVICE_HISTORY_SIZE];

    int m_indexHistoryPos;

    int m_indexHistoryRot;

    int m_indexHistoryGripper;

    int m_indexHistoryPosWin;

    int m_indexHistoryRotWin;

    int m_indexHistoryGripperWin;

    double m_linearVelocityWindowSize;

    double m_angularVelocityWindowSize;

    double m_gripperVelocityWindowSize;

    cPrecisionClock m_clockGeneral;


    //--------------------------------------------------------------------------
    // PROTECTED MEMBERS - GRIPPER USER SWITCH:
    //--------------------------------------------------------------------------

protected:

    bool m_gripperUserSwitchEnabled;

    double m_gripperUserSwitchAngleStart;

    double m_gripperUserSwitchAngleClick;

    double m_gripperUserSwitchForceClick;

    double m_gripperUserSwitchForceEngaged;


    //--------------------------------------------------------------------------
    // PROTECTED MEMBERS - SIMULATED GRIPPER FOR DEVICES WITH USER SWITCH:
    //--------------------------------------------------------------------------

protected:

    double m_virtualGripperAngle;

    double m_virtualGripperAngleMin;

    double m_virtualGripperAngleMax;

    double m_virtualGripperAngularVelocity;

    cPrecisionClock m_virtualGripperClock;


    //--------------------------------------------------------------------------
    // PROTECTED METHODS - VELOCITY ESTIMATION:
    //--------------------------------------------------------------------------

protected:

    void estimateLinearVelocity(cVector3d& a_newPosition);

    void estimateAngularVelocity(cMatrix3d& a_newRotation);

    void estimateGripperVelocity(double a_newGripperPosition);


    //--------------------------------------------------------------------------
    // PROTECTED METHODS - GRIPPER USER SWITCH:
    //--------------------------------------------------------------------------

protected:

    double computeGripperUserSwitchForce(const double& a_gripperAngle,
                                         const double& a_gripperAngularVelocity);

    bool getGripperUserSwitch();


    //--------------------------------------------------------------------------
    // PROTECTED METHODS - DEVICE LIBRARY INITIALIZATION:
    //--------------------------------------------------------------------------

protected:

    static bool openLibraries() { return (C_SUCCESS); }

    static bool closeLibraries() { return (C_SUCCESS); }
};

//------------------------------------------------------------------------------
} // namespace chai3d
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
#endif
//------------------------------------------------------------------------------