vpSimulatorAfma6.h

/****************************************************************************
 *
 * $Id: vpSimulatorAfma6.h 2598 2010-06-02 09:20:22Z nmelchio $
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2014 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/vpRobotWireFrameSimulator.h>
#include <visp/vpAfma6.h>

#include <string>

#if defined(_WIN32) || defined(VISP_HAVE_PTHREAD)

class VISP_EXPORT vpSimulatorAfma6 : public vpRobotWireFrameSimulator, 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 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 getDisplacement(const vpRobot::vpControlFrameType frame,
                         vpColVector &displacement);
    void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q);
    void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q, double &timestamp);
    void getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position);
    void getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double &timestamp);
    double getPositioningVelocity (void){return positioningVelocity;}
    void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q);
    void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q, double &timestamp);
    vpColVector getVelocity (const vpRobot::vpControlFrameType frame);
    vpColVector getVelocity (const vpRobot::vpControlFrameType frame, double &timestamp);

    void get_cMe(vpHomogeneousMatrix &cMe);
    void get_cVe(vpVelocityTwistMatrix &cVe);
    void get_eJe(vpMatrix &eJe);
    void get_fJe(vpMatrix &fJe);

    void init (vpAfma6::vpAfma6ToolType tool, vpCameraParameters::vpCameraParametersProjType projModel=vpCameraParameters::perspectiveProjWithoutDistortion);
    bool initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo);
    void initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo);

    void move(const char *filename) ;

    static bool readPosFile(const char *filename, vpColVector &q);
    static bool savePosFile(const char *filename, const vpColVector &q);
    void setCameraParameters(const vpCameraParameters &cam) ;
    void setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax);

    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);
    void setPositioningVelocity (const double vel) {positioningVelocity = vel;}
    bool setPosition(const vpHomogeneousMatrix &cdMo, vpImage<unsigned char> *Iint=NULL, const double &errMax = 0.001);
    vpRobot::vpRobotStateType setRobotState (const vpRobot::vpRobotStateType newState);
    
    void setVelocity (const vpRobot::vpControlFrameType frame, const vpColVector & velocity);

    void stopMotion();
    
    
protected:
    void computeArticularVelocity();
    void compute_fMi();
    void findHighestPositioningSpeed(vpColVector &q);
    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 init();
    void initArms();
    void initDisplay();
    int isInJointLimit (void);
    bool singularityTest(const vpColVector q, vpMatrix &J);
    void updateArticularPosition();
    
private:
    void getCameraDisplacement(vpColVector &displacement);
    void getArticularDisplacement(vpColVector &displacement);
};

#endif

#endif