vpRobotSimulator.h

/****************************************************************************
 *
 * $Id: vpRobotSimulator.h 3530 2012-01-03 10:52:12Z fspindle $
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2012 by INRIA. All rights reserved.
 * 
 * This software is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * ("GPL") version 2 as published by the Free Software Foundation.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact INRIA about acquiring a ViSP Professional 
 * Edition License.
 *
 * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
 * 
 * This software was developed at:
 * INRIA Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 * http://www.irisa.fr/lagadic
 *
 * If you have questions regarding the use of this file, please contact
 * INRIA at visp@inria.fr
 * 
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 *
 * Description:
 * Basic class used to make robot simulators.
 *
 * Authors:
 * Nicolas Melchior
 *
 *****************************************************************************/

#ifndef vpRobotSimulator_HH
#define vpRobotSimulator_HH

#include <visp/vpConfig.h>



#if defined(WIN32) || defined(VISP_HAVE_PTHREAD)
#include <cmath>    // std::fabs
#include <limits>   // numeric_limits
#if defined(WIN32)
#  include <windows.h>
#elif defined(VISP_HAVE_PTHREAD)
#  include <pthread.h>
#endif

#include <visp/vpWireFrameSimulator.h>
#include <visp/vpRobot.h>
#include <visp/vpDisplayOpenCV.h>
#include <visp/vpDisplayX.h>
#include <visp/vpDisplayGTK.h>
#include <visp/vpDisplayGDI.h>
#include <visp/vpDisplayD3D.h>

class VISP_EXPORT vpRobotSimulator : protected vpWireFrameSimulator, public vpRobot
{
  public:
    vpImage<vpRGBa> I;
    
    typedef enum  
    {
      MODEL_3D,
      MODEL_DH
    } vpDisplayRobotType;
    
    
  protected:
    float samplingTime;
    double tcur;
    double tprev;
    
    Bound_scene* robotArms;
    
    unsigned int size_fMi;
    vpHomogeneousMatrix* fMi;
    
    vpColVector artCoord;
    vpColVector artVel;
    vpColVector velocity;

    #if defined(WIN32)
    HANDLE  hThread;
    HANDLE mutex_fMi;
    HANDLE mutex_artVel;
    HANDLE mutex_artCoord;
    HANDLE mutex_velocity;
    HANDLE mutex_display;
    #elif defined(VISP_HAVE_PTHREAD)
    pthread_t thread;
    pthread_attr_t attr;
    pthread_mutex_t mutex_fMi;
    pthread_mutex_t mutex_artVel;
    pthread_mutex_t mutex_artCoord;
    pthread_mutex_t mutex_velocity;
    pthread_mutex_t mutex_display;
    #endif
    
    bool displayBusy;

    bool robotStop;
    bool jointLimit;
    unsigned int jointLimitArt;
    bool singularityManagement;
    
    vpCameraParameters cameraParam;
    
    #if defined VISP_HAVE_X11
    vpDisplayX display;
    #elif defined VISP_HAVE_GDI
    vpDisplayGDI display;
    #elif defined VISP_HAVE_OPENCV
    vpDisplayOpenCV display;
    #elif defined VISP_HAVE_D3D9
    vpDisplayD3D display;
    #elif defined VISP_HAVE_GTK
    vpDisplayGTK display;
    #endif
    
    vpDisplayRobotType displayType;
    
    bool displayAllowed;
    bool constantSamplingTimeMode;

    bool setVelocityCalled;
    
  public:
    vpRobotSimulator();
    vpRobotSimulator(bool display);
     virtual ~vpRobotSimulator();
    
    /* Robot functions */
    virtual void setVelocity (const vpRobot::vpControlFrameType /*frame*/, const vpColVector & /*velocity*/) = 0;
    virtual void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q) = 0;
    
    /* vpRobot virtual methods*/
    virtual void get_eJe(vpMatrix & _eJe) = 0;
    virtual void get_fJe(vpMatrix &_fJe) = 0;
    virtual void getCameraDisplacement(vpColVector &v) = 0;
    virtual void getArticularDisplacement(vpColVector  &qdot) = 0;
    virtual void getDisplacement(const vpRobot::vpControlFrameType frame,vpColVector &q) = 0;
    void setSingularityManagement (const bool sm) {singularityManagement = sm;}
    
    
    /* Display functions */
    void initScene(vpSceneObject obj, vpSceneDesiredObject desiredObject);
    void initScene(const char* obj, const char* desiredObject);
    void initScene (vpSceneObject obj);
    void initScene(const char* obj);
    
    void setDesiredCameraPosition(const vpHomogeneousMatrix cdMo)
    {
      this->vpWireFrameSimulator::setDesiredCameraPosition(cdMo);
    }

    void setExternalCameraPosition(const vpHomogeneousMatrix camMf)
    {
      this->vpWireFrameSimulator::setExternalCameraPosition(camMf);
    }
    
    vpHomogeneousMatrix getExternalCameraPosition() const
    {
      return this->vpWireFrameSimulator::getExternalCameraPosition();
    }
    
    void setCameraColor(const vpColor col) {camColor = col;}
    
    void setCurrentViewColor(const vpColor col) {curColor = col;}
    
    void setDesiredViewColor(const vpColor col) {desColor = col;}
    
    vpCameraParameters getExternalCameraParameters() const {
      //if(px_ext != 1 && py_ext != 1)
      // we assume px_ext and py_ext > 0
      if( (std::fabs(px_ext-1.) > vpMath::maximum(px_ext,1.)*std::numeric_limits<double>::epsilon()) 
      && (std::fabs(py_ext-1) > vpMath::maximum(py_ext,1.)*std::numeric_limits<double>::epsilon()))
        return vpCameraParameters(px_ext,py_ext,I.getWidth()/2,I.getHeight()/2);
      else
      {
        unsigned int size = vpMath::minimum(I.getWidth(),I.getHeight())/2;
        return vpCameraParameters(size,size,I.getWidth()/2,I.getHeight()/2);
      }
    }
    
    void set_fMo(const vpHomogeneousMatrix &fMo) {this->fMo = fMo;}
    
    vpHomogeneousMatrix get_fMo() const {return fMo;}
    
    inline void setDisplayRobotType (const vpDisplayRobotType dispType) {displayType = dispType;}
    
    vpHomogeneousMatrix get_cMo();
    
    void getInternalView(vpImage<vpRGBa> &I);
    void getInternalView(vpImage<unsigned char> &I);


    inline void setConstantSamplingTimeMode(const bool _constantSamplingTimeMode){
      constantSamplingTimeMode = _constantSamplingTimeMode;
    }
    
    inline void setSamplingTime(const float _samplingTime){
      if(_samplingTime < static_cast<float>(vpTime::minTimeForUsleepCall)){
        samplingTime = static_cast<float>(vpTime::minTimeForUsleepCall);
      }else{
        samplingTime = _samplingTime;
      }
    }


  protected:
    #if defined(WIN32)
    static DWORD WINAPI launcher( LPVOID lpParam ) 
    {
        ((vpRobotSimulator *)lpParam)->updateArticularPosition();
        return 0;
    }
    #elif defined(VISP_HAVE_PTHREAD)
    static void* launcher(void *arg)
    {
      (reinterpret_cast<vpRobotSimulator *>(arg))->updateArticularPosition();
      // pthread_exit((void*) 0);
      return NULL;
    }    
    #endif
    
    /* Robot functions */
    void init() {;}
    virtual void updateArticularPosition() = 0;
    virtual int isInJointLimit () = 0;
    virtual void computeArticularVelocity() = 0;

    /* Display functions */
    void initDisplay() {;}
    virtual void initArms() = 0;

    #if defined(WIN32)
    vpColVector get_artCoord() {
      WaitForSingleObject(mutex_artCoord,INFINITE);
      vpColVector artCoordTmp (6);
      artCoordTmp = artCoord;
      ReleaseMutex(mutex_artCoord);
      return artCoordTmp;}
    void set_artCoord(const vpColVector &coord) {
      WaitForSingleObject(mutex_artCoord,INFINITE);
      artCoord = coord;
      ReleaseMutex(mutex_artCoord);}
    
    vpColVector get_artVel() {
      WaitForSingleObject(mutex_artVel,INFINITE);
      vpColVector artVelTmp (artVel);
      ReleaseMutex(mutex_artVel);
      return artVelTmp;}
    void set_artVel(const vpColVector &vel) {
      WaitForSingleObject(mutex_artVel,INFINITE);
      artVel = vel;
      ReleaseMutex(mutex_artVel);}
    
    vpColVector get_velocity() {
      WaitForSingleObject(mutex_velocity,INFINITE);
      vpColVector velocityTmp = velocity;
      ReleaseMutex(mutex_velocity);
      return velocityTmp;}
    void set_velocity(const vpColVector &vel) {
      WaitForSingleObject(mutex_velocity,INFINITE);
      velocity = vel;
      ReleaseMutex(mutex_velocity);}
      
    void set_displayBusy (const bool &status) {
      WaitForSingleObject(mutex_display,INFINITE);
      displayBusy = status;
      ReleaseMutex(mutex_display);}
    bool get_displayBusy () {
      WaitForSingleObject(mutex_display,INFINITE);
      bool status = displayBusy;
      if (!displayBusy) displayBusy = true;
      ReleaseMutex(mutex_display);
      return status;}

    #elif defined(VISP_HAVE_PTHREAD)
    vpColVector get_artCoord() {
      pthread_mutex_lock (&mutex_artCoord);
      vpColVector artCoordTmp (6);
      artCoordTmp = artCoord;
      pthread_mutex_unlock (&mutex_artCoord);
      return artCoordTmp;}
    void set_artCoord(const vpColVector &coord) {
      pthread_mutex_lock (&mutex_artCoord);
      artCoord = coord;
      pthread_mutex_unlock (&mutex_artCoord);}
    
    vpColVector get_artVel() {
      pthread_mutex_lock (&mutex_artVel);
      vpColVector artVelTmp (artVel);
      pthread_mutex_unlock (&mutex_artVel);
      return artVelTmp;}
    void set_artVel(const vpColVector &vel) {
      pthread_mutex_lock (&mutex_artVel);
      artVel = vel;
      pthread_mutex_unlock (&mutex_artVel);}
    
    vpColVector get_velocity() {
      pthread_mutex_lock (&mutex_velocity);
      vpColVector velocityTmp = velocity;
      pthread_mutex_unlock (&mutex_velocity);
      return velocityTmp;}
    void set_velocity(const vpColVector &vel) {
      pthread_mutex_lock (&mutex_velocity);
      velocity = vel;
      pthread_mutex_unlock (&mutex_velocity);}
      
    void set_displayBusy (const bool &status) {
      pthread_mutex_lock (&mutex_display);
      displayBusy = status;
      pthread_mutex_unlock (&mutex_display);}
    bool get_displayBusy () {
      pthread_mutex_lock (&mutex_display);
      bool status = displayBusy;
      if (!displayBusy) displayBusy = true;
      pthread_mutex_unlock (&mutex_display);
      return status;}
    #endif

    virtual void get_fMi(vpHomogeneousMatrix *fMit) = 0;
};

#endif

#endif