vpSimulatorAfma6.h

/****************************************************************************
 *
 * $Id: vpSimulatorAfma6.h 2598 2010-06-02 09:20:22Z nmelchio $
 *
 * 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:
 * Class which provides a simulator for the robot Afma6.
 *
 * Authors:
 * Nicolas Melchior
 *
 *****************************************************************************/

#ifndef vpSimulatorAfma6_HH
#define vpSimulatorAfma6_HH

#include <visp/vpRobotSimulator.h>
#include <visp/vpAfma6.h>

#include <string>

#if defined(WIN32) || defined(VISP_HAVE_PTHREAD)

class VISP_EXPORT vpSimulatorAfma6 : public vpRobotSimulator, public vpAfma6
{
  public:
    static const double defaultPositioningVelocity;
    
  private:
    vpColVector q_prev_getdis;
    bool first_time_getdis;
    
    double positioningVelocity;
    
    vpColVector zeroPos;
    vpColVector reposPos;
    
    bool toolCustom;
    std::string arm_dir;
 
  public:
    vpSimulatorAfma6();
    vpSimulatorAfma6(bool display);
    virtual ~vpSimulatorAfma6();
    
    void init (vpAfma6::vpAfma6ToolType tool, vpCameraParameters::vpCameraParametersProjType projModel=vpCameraParameters::perspectiveProjWithoutDistortion);
    
    vpRobot::vpRobotStateType setRobotState (const vpRobot::vpRobotStateType newState);
    
    void getCameraParameters(vpCameraParameters &cam,
               const unsigned int &image_width,
               const unsigned int &image_height);
    void getCameraParameters(vpCameraParameters &cam,
               const vpImage<unsigned char> &I);
    void getCameraParameters(vpCameraParameters &cam, const vpImage<vpRGBa> &I);
    void setCameraParameters(const vpCameraParameters cam) ;
    
    void setVelocity (const vpRobot::vpControlFrameType frame, const vpColVector & velocity);
    void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q);
    vpColVector getVelocity (const vpRobot::vpControlFrameType frame);
    
    void setPositioningVelocity (const double velocity) {positioningVelocity = velocity;}
    void setPosition(const vpRobot::vpControlFrameType frame,const vpColVector &q);
    void setPosition (const vpRobot::vpControlFrameType frame,
                   const double pos1,
                   const double pos2,
                   const double pos3,
                   const double pos4,
                   const double pos5,
                   const double pos6);
    void setPosition(const char *filename);
    bool setPosition(const vpHomogeneousMatrix &cdMo, vpImage<unsigned char> *Iint=NULL, const double &errMax = 0.001);
    
    void setJointLimit(vpColVector limitMin, vpColVector limitMax);
    
    double getPositioningVelocity (void){return positioningVelocity;}
    
    void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q);
    void getPosition (const vpRobot::vpControlFrameType frame,
            vpPoseVector &position);

    void getCameraDisplacement(vpColVector &displacement);
    void getArticularDisplacement(vpColVector &displacement);
    void getDisplacement(const vpRobot::vpControlFrameType frame, 
               vpColVector &displacement);
    
    static bool readPosFile(const char *filename, vpColVector &q);
    static bool savePosFile(const char *filename, const vpColVector &q);
    void move(const char *filename) ;
    
    void get_cMe(vpHomogeneousMatrix &cMe);
    void get_cVe(vpVelocityTwistMatrix &cVe);
    void get_eJe(vpMatrix &eJe);
    void get_fJe(vpMatrix &fJe);
    
    void stopMotion();
    
    void initialiseCameraRelativeToObject(vpHomogeneousMatrix cMo);
    void initialiseObjectRelativeToCamera(vpHomogeneousMatrix cMo);
    
  protected:
    void updateArticularPosition();
    void computeArticularVelocity();
    void findHighestPositioningSpeed(vpColVector &q);
    void init();
    bool singularityTest(const vpColVector q, vpMatrix &J);
    int isInJointLimit (void);
    
    void initDisplay();
    void initArms();
    void getExternalImage(vpImage<vpRGBa> &I);
    
    inline void get_fMi(vpHomogeneousMatrix *fMit) {
    #if defined(WIN32)
      WaitForSingleObject(mutex_fMi,INFINITE);
      for (int i = 0; i < 8; i++)
        fMit[i] = fMi[i];
      ReleaseMutex(mutex_fMi);
    #elif defined(VISP_HAVE_PTHREAD)
      pthread_mutex_lock (&mutex_fMi);
      for (int i = 0; i < 8; i++)
        fMit[i] = fMi[i];
      pthread_mutex_unlock (&mutex_fMi);
    #endif
    }
      
    void compute_fMi();
};

#endif

#endif