vpSimulatorViper850.cpp

/****************************************************************************
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2015 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://visp.inria.fr for more information.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * 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 Viper850.
 *
 * Authors:
 * Nicolas Melchior
 *
 *****************************************************************************/



#include <visp3/robot/vpSimulatorViper850.h>

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

#include <visp3/core/vpTime.h>
#include <visp3/core/vpImagePoint.h>
#include <visp3/core/vpPoint.h>
#include <visp3/core/vpMeterPixelConversion.h>
#include <visp3/core/vpIoTools.h>
#include <cmath>    // std::fabs
#include <limits>   // numeric_limits
#include <string>

const double vpSimulatorViper850::defaultPositioningVelocity = 25.0;

vpSimulatorViper850::vpSimulatorViper850()
  : vpRobotWireFrameSimulator(), vpViper850(),
    q_prev_getdis(), first_time_getdis(true), positioningVelocity(defaultPositioningVelocity),
    zeroPos(), reposPos(), toolCustom(false), arm_dir()
{
  init();
  initDisplay();
  
  tcur = vpTime::measureTimeMs();

  #if defined(_WIN32)
  mutex_fMi = CreateMutex(NULL,FALSE,NULL);
  mutex_artVel = CreateMutex(NULL,FALSE,NULL);
  mutex_artCoord = CreateMutex(NULL,FALSE,NULL);
  mutex_velocity = CreateMutex(NULL,FALSE,NULL);
  mutex_display = CreateMutex(NULL,FALSE,NULL);


  DWORD   dwThreadIdArray;
  hThread = CreateThread( 
            NULL,                   // default security attributes
            0,                      // use default stack size  
            launcher,               // thread function name
            this,                   // argument to thread function 
            0,                      // use default creation flags 
            &dwThreadIdArray);     // returns the thread identifier 
  #elif defined (VISP_HAVE_PTHREAD)
  pthread_mutex_init(&mutex_fMi, NULL);
  pthread_mutex_init(&mutex_artVel, NULL);
  pthread_mutex_init(&mutex_artCoord, NULL);
  pthread_mutex_init(&mutex_velocity, NULL);
  pthread_mutex_init(&mutex_display, NULL);
  
  pthread_attr_init(&attr);
  pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
  
  pthread_create(&thread, NULL, launcher, (void *)this);
  #endif
  
  compute_fMi();
}

vpSimulatorViper850::vpSimulatorViper850(bool do_display)
  : vpRobotWireFrameSimulator(do_display),
    q_prev_getdis(), first_time_getdis(true), positioningVelocity(defaultPositioningVelocity),
    zeroPos(), reposPos(), toolCustom(false), arm_dir()
{
  init();
  initDisplay();
    
  tcur = vpTime::measureTimeMs();
  
    #if defined(_WIN32)
  mutex_fMi = CreateMutex(NULL,FALSE,NULL);
  mutex_artVel = CreateMutex(NULL,FALSE,NULL);
  mutex_artCoord = CreateMutex(NULL,FALSE,NULL);
  mutex_velocity = CreateMutex(NULL,FALSE,NULL);
  mutex_display = CreateMutex(NULL,FALSE,NULL);


  DWORD   dwThreadIdArray;
  hThread = CreateThread( 
            NULL,                   // default security attributes
            0,                      // use default stack size  
            launcher,               // thread function name
            this,                   // argument to thread function 
            0,                      // use default creation flags 
            &dwThreadIdArray);      // returns the thread identifier 
  #elif defined(VISP_HAVE_PTHREAD)
  pthread_mutex_init(&mutex_fMi, NULL);
  pthread_mutex_init(&mutex_artVel, NULL);
  pthread_mutex_init(&mutex_artCoord, NULL);
  pthread_mutex_init(&mutex_velocity, NULL);
  pthread_mutex_init(&mutex_display, NULL);
  
  pthread_attr_init(&attr);
  pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
  
  pthread_create(&thread, NULL, launcher, (void *)this);
  #endif
  
  compute_fMi();
}

vpSimulatorViper850::~vpSimulatorViper850()
{
  robotStop = true;
  
  #if defined(_WIN32)
  WaitForSingleObject(hThread,INFINITE);
  CloseHandle(hThread);
  CloseHandle(mutex_fMi);
  CloseHandle(mutex_artVel);
  CloseHandle(mutex_artCoord);
  CloseHandle(mutex_velocity);
  CloseHandle(mutex_display);
  #elif defined(VISP_HAVE_PTHREAD)
  pthread_attr_destroy(&attr);
  pthread_join(thread, NULL);
  pthread_mutex_destroy(&mutex_fMi);
  pthread_mutex_destroy(&mutex_artVel);
  pthread_mutex_destroy(&mutex_artCoord);
  pthread_mutex_destroy(&mutex_velocity);
  pthread_mutex_destroy(&mutex_display);
  #endif
  
  if (robotArms != NULL)
  {
   // free_Bound_scene (&(camera));
    for(int i = 0; i < 6; i++)
      free_Bound_scene (&(robotArms[i]));
  }

  delete[] robotArms;
  delete[] fMi;
}

void
vpSimulatorViper850::init()
{
  // set arm_dir from #define VISP_ROBOT_ARMS_DIR if it exists
  // VISP_ROBOT_ARMS_DIR may contain multiple locations separated by ";"
  std::vector<std::string> arm_dirs = vpIoTools::splitChain(std::string(VISP_ROBOT_ARMS_DIR), std::string(";"));
  bool armDirExists = false;
  for(size_t i=0; i < arm_dirs.size(); i++)
  if (vpIoTools::checkDirectory(arm_dirs[i]) == true) { // directory exists
    arm_dir = arm_dirs[i];
    armDirExists = true;
    break;
  }
  if (! armDirExists) {
    try {
      arm_dir = vpIoTools::getenv("VISP_ROBOT_ARMS_DIR");
      std::cout << "The simulator uses data from VISP_ROBOT_ARMS_DIR=" << arm_dir << std::endl;
    }
    catch (...) {
      std::cout << "Cannot get VISP_ROBOT_ARMS_DIR environment variable" << std::endl;
    }
  }

  this->init(vpViper850::defaultTool);
  toolCustom = false;

  size_fMi = 8;
  fMi = new vpHomogeneousMatrix[8];
  artCoord.resize(njoint);
  artVel.resize(njoint);

  zeroPos.resize(njoint);
  zeroPos = 0;
  zeroPos[1] = -M_PI/2; zeroPos[2] = M_PI;
  reposPos.resize(njoint);
  reposPos = 0;
  reposPos[1] = -M_PI/2; reposPos[2] = M_PI; reposPos[4] = M_PI/2;
  
  artCoord = reposPos;
  artVel = 0;
  
  q_prev_getdis.resize(njoint);
  q_prev_getdis = 0;
  first_time_getdis = true;
  
  positioningVelocity  = defaultPositioningVelocity ;
  
  setRobotFrame (vpRobot::ARTICULAR_FRAME);
  this->setRobotState(vpRobot::STATE_STOP);
  
  // Software joint limits in radians
  //joint_min.resize(njoint);
  joint_min[0] = vpMath::rad(-50);
  joint_min[1] = vpMath::rad(-135);
  joint_min[2] = vpMath::rad(-40);
  joint_min[3] = vpMath::rad(-190);
  joint_min[4] = vpMath::rad(-110);
  joint_min[5] = vpMath::rad(-184);
  //joint_max.resize(njoint);
  joint_max[0] = vpMath::rad(50);
  joint_max[1] = vpMath::rad(-40);
  joint_max[2] = vpMath::rad(215);
  joint_max[3] = vpMath::rad(190);
  joint_max[4] = vpMath::rad(110);
  joint_max[5] = vpMath::rad(184);
}

void
vpSimulatorViper850::initDisplay()
{
  robotArms = NULL;
  robotArms = new Bound_scene[6];
  initArms();
  setExternalCameraPosition(vpHomogeneousMatrix(0.0,0.5,1.5,vpMath::rad(90),0,0));
  cameraParam.initPersProjWithoutDistortion (558.5309599, 556.055053, 320, 240);
  setExternalCameraParameters(cameraParam);
  vpCameraParameters tmp;
  getCameraParameters(tmp,640,480);
  px_int = tmp.get_px();
  py_int = tmp.get_py();
  sceneInitialized = true;
}


void
vpSimulatorViper850::init (vpViper850::vpToolType tool,
           vpCameraParameters::vpCameraParametersProjType proj_model)
{
  this->projModel = proj_model;
  
  // Use here default values of the robot constant parameters.
  switch (tool) {
  case vpViper850::TOOL_MARLIN_F033C_CAMERA: {
      erc[0] = vpMath::rad(0.07); // rx
      erc[1] = vpMath::rad(2.76); // ry
      erc[2] = vpMath::rad(-91.50); // rz
      etc[0] = -0.0453; // tx
      etc[1] =  0.0005; // ty
      etc[2] =  0.0728; // tz
      
      setCameraParameters(vpCameraParameters(1232.0, 1233.0, 320, 240));
      break;
    }
  case vpViper850::TOOL_PTGREY_FLEA2_CAMERA: {
      erc[0] = vpMath::rad(0.1527764261); // rx
      erc[1] = vpMath::rad(1.285092455); // ry
      erc[2] = vpMath::rad(-90.75777848); // rz
      etc[0] = -0.04558630174; // tx
      etc[1] = -0.00134326752; // ty
      etc[2] =  0.001000828017; // tz
      
      setCameraParameters(vpCameraParameters(868.0, 869.0, 320, 240));
      break;
    }
  case vpViper850::TOOL_SCHUNK_GRIPPER_CAMERA:
  case vpViper850::TOOL_GENERIC_CAMERA: {
      std::cout << "This tool is not handled in vpSimulatorViper850.cpp" << std::endl;
      break;
    }
  }
  
  vpRotationMatrix eRc(erc);
  this->eMc.buildFrom(etc, eRc);
  
  setToolType(tool);
  return ;
}

void
vpSimulatorViper850::getCameraParameters (vpCameraParameters &cam,
                 const unsigned int &image_width,
                 const unsigned int &image_height)
{
  if (toolCustom)
  {
    vpCameraParameters(px_int,py_int,image_width/2,image_height/2);
  }
  // Set default parameters
  switch (getToolType()) {
  case vpViper850::TOOL_MARLIN_F033C_CAMERA: {
    // Set default intrinsic camera parameters for 640x480 images
    if (image_width == 640 && image_height == 480) 
    {
      std::cout << "Get default camera parameters for camera \"" 
        << vpViper850::CONST_MARLIN_F033C_CAMERA_NAME << "\"" << std::endl;
    cam.initPersProjWithoutDistortion(1232.0, 1233.0, 320, 240);
    }
    else {
      vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
    }
    break;
  }
  case vpViper850::TOOL_PTGREY_FLEA2_CAMERA: {
    // Set default intrinsic camera parameters for 640x480 images
    if (image_width == 640 && image_height == 480) {
      std::cout << "Get default camera parameters for camera \"" 
        << vpViper850::CONST_PTGREY_FLEA2_CAMERA_NAME << "\"" << std::endl;
    cam.initPersProjWithoutDistortion(868.0, 869.0, 320, 240);
    }
    else {
      vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
    }
    break;
  }
  case vpViper850::TOOL_SCHUNK_GRIPPER_CAMERA:
  case vpViper850::TOOL_GENERIC_CAMERA: {
      std::cout << "This tool is not handled in vpSimulatorViper850.cpp" << std::endl;
      break;
    }
  }
  return;
}

void
vpSimulatorViper850::getCameraParameters (vpCameraParameters &cam,
                                          const vpImage<unsigned char> &I_)
{
  getCameraParameters(cam,I_.getWidth(),I_.getHeight());
}

void
vpSimulatorViper850::getCameraParameters (vpCameraParameters &cam,
                                          const vpImage<vpRGBa> &I_)
{
  getCameraParameters(cam,I_.getWidth(),I_.getHeight());
}


void
vpSimulatorViper850::setCameraParameters(const vpCameraParameters &cam)
{
  px_int = cam.get_px();
  py_int = cam.get_py();
  toolCustom = true;
}


void
vpSimulatorViper850::updateArticularPosition()
{
  double tcur_1 = tcur;// temporary variable used to store the last time since the last command 

  while (!robotStop)
  {
    //Get current time
    tprev = tcur_1;
    tcur = vpTime::measureTimeMs();
    
    
    if(setVelocityCalled || !constantSamplingTimeMode){
      setVelocityCalled = false;
      computeArticularVelocity();
      
      double ellapsedTime = (tcur - tprev) * 1e-3;
      if(constantSamplingTimeMode){//if we want a constant velocity, we force the ellapsed time to the given samplingTime
        ellapsedTime = getSamplingTime(); // in second
      }
      
      vpColVector articularCoordinates = get_artCoord();
      articularCoordinates = get_artCoord();
      vpColVector articularVelocities = get_artVel();
      
      if (jointLimit)
      {
        double art = articularCoordinates[jointLimitArt-1] + ellapsedTime*articularVelocities[jointLimitArt-1];
        if (art <= joint_min[jointLimitArt-1] || art >= joint_max[jointLimitArt-1]) {
          if (verbose_) {
            std::cout << "Joint " << jointLimitArt-1
                    << " reaches a limit: " << vpMath::deg(joint_min[jointLimitArt-1]) << " < " << vpMath::deg(art) << " < " << vpMath::deg(joint_max[jointLimitArt-1]) << std::endl;
          }
          articularVelocities = 0.0;
        }
        else
          jointLimit = false;
      }
      
      articularCoordinates[0] = articularCoordinates[0] + ellapsedTime*articularVelocities[0];
      articularCoordinates[1] = articularCoordinates[1] + ellapsedTime*articularVelocities[1];
      articularCoordinates[2] = articularCoordinates[2] + ellapsedTime*articularVelocities[2];
      articularCoordinates[3] = articularCoordinates[3] + ellapsedTime*articularVelocities[3];
      articularCoordinates[4] = articularCoordinates[4] + ellapsedTime*articularVelocities[4];
      articularCoordinates[5] = articularCoordinates[5] + ellapsedTime*articularVelocities[5];
      
      int jl = isInJointLimit();
      
      if (jl != 0 && jointLimit == false)
      {
        if (jl < 0)
          ellapsedTime = (joint_min[(unsigned int)(-jl-1)] - articularCoordinates[(unsigned int)(-jl-1)])/(articularVelocities[(unsigned int)(-jl-1)]);
        else
          ellapsedTime = (joint_max[(unsigned int)(jl-1)] - articularCoordinates[(unsigned int)(jl-1)])/(articularVelocities[(unsigned int)(jl-1)]);
        
        for (unsigned int i = 0; i < 6; i++)
          articularCoordinates[i] = articularCoordinates[i] + ellapsedTime*articularVelocities[i];
        
        jointLimit = true;
        jointLimitArt = (unsigned int)fabs((double)jl);
      }

      set_artCoord(articularCoordinates);
      set_artVel(articularVelocities);
      
      compute_fMi();
     
      if (displayAllowed)
      {
        vpDisplay::display(I);
        vpDisplay::displayFrame(I,getExternalCameraPosition (),cameraParam,0.2,vpColor::none, thickness_);
        vpDisplay::displayFrame(I,getExternalCameraPosition ()*fMi[7],cameraParam,0.1,vpColor::none, thickness_);
      }
      
      if (displayType == MODEL_3D && displayAllowed)
      {
        while (get_displayBusy()) vpTime::wait(2);
        vpSimulatorViper850::getExternalImage(I);
        set_displayBusy(false);
      }
        
      
      if (displayType == MODEL_DH && displayAllowed)
      {
        vpHomogeneousMatrix fMit[8];
        get_fMi(fMit);
      
      //vpDisplay::displayFrame(I,getExternalCameraPosition ()*fMi[6],cameraParam,0.2,vpColor::none);

        vpImagePoint iP, iP_1;
        vpPoint pt(0,0,0);
      
        pt.track(getExternalCameraPosition ());
        vpMeterPixelConversion::convertPoint (cameraParam, pt.get_x(), pt.get_y(), iP_1);
        pt.track(getExternalCameraPosition ()*fMit[0]);
        vpMeterPixelConversion::convertPoint (cameraParam, pt.get_x(), pt.get_y(), iP);
        vpDisplay::displayLine(I, iP_1, iP, vpColor::green, thickness_);
        for (int k = 1; k < 7; k++)
        {
          pt.track(getExternalCameraPosition ()*fMit[k-1]);
          vpMeterPixelConversion::convertPoint (cameraParam, pt.get_x(), pt.get_y(), iP_1);
        
          pt.track(getExternalCameraPosition ()*fMit[k]);
          vpMeterPixelConversion::convertPoint (cameraParam, pt.get_x(), pt.get_y(), iP);
        
          vpDisplay::displayLine(I,iP_1,iP,vpColor::green, thickness_);
        }
        vpDisplay::displayCamera(I,getExternalCameraPosition ()*fMit[7],cameraParam,0.1,vpColor::green, thickness_);
      }
      
      vpDisplay::flush(I);      
      
      vpTime::wait( tcur, 1000 * getSamplingTime() );
      tcur_1 = tcur;
    }else{
      vpTime::wait(tcur, vpTime::getMinTimeForUsleepCall());
    }
  }
}

void 
vpSimulatorViper850::compute_fMi()
{
  //vpColVector q = get_artCoord();
  vpColVector q(6);//; = get_artCoord();
  q = get_artCoord();
  
  vpHomogeneousMatrix fMit[8];
  
  double q1 = q[0];
  double q2 = q[1];
  double q3 = q[2];
  double q4 = q[3];
  double q5 = q[4];
  double q6 = q[5];
  
  double c1 = cos(q1);
  double s1 = sin(q1);
  double c2 = cos(q2);
  double s2 = sin(q2);
  double c23 = cos(q2+q3);
  double s23 = sin(q2+q3);
  double c4 = cos(q4);
  double s4 = sin(q4);
  double c5 = cos(q5);
  double s5 = sin(q5);
  double c6 = cos(q6);
  double s6 = sin(q6);
  
  fMit[0][0][0] = c1;
  fMit[0][1][0] = s1;
  fMit[0][2][0] = 0;
  fMit[0][0][1] = 0;
  fMit[0][1][1] = 0;
  fMit[0][2][1] = -1;
  fMit[0][0][2] = -s1;
  fMit[0][1][2] = c1;
  fMit[0][2][2] = 0;
  fMit[0][0][3] = a1*c1;
  fMit[0][1][3] = a1*s1;
  fMit[0][2][3] = d1;
  
  fMit[1][0][0] = c1*c2;
  fMit[1][1][0] = s1*c2;
  fMit[1][2][0] = -s2;
  fMit[1][0][1] = -c1*s2;
  fMit[1][1][1] = -s1*s2;
  fMit[1][2][1] = -c2;
  fMit[1][0][2] = -s1;
  fMit[1][1][2] = c1;
  fMit[1][2][2] = 0;
  fMit[1][0][3] = c1*(a2*c2+a1);
  fMit[1][1][3] = s1*(a2*c2+a1);
  fMit[1][2][3] = d1-a2*s2;
  
  double quickcomp1 = a3*c23-a2*c2-a1;
  
  fMit[2][0][0] = -c1*c23;
  fMit[2][1][0] = -s1*c23;
  fMit[2][2][0] = s23;
  fMit[2][0][1] = s1;
  fMit[2][1][1] = -c1;
  fMit[2][2][1] = 0;
  fMit[2][0][2] = c1*s23;
  fMit[2][1][2] = s1*s23;
  fMit[2][2][2] = c23;
  fMit[2][0][3] = -c1*quickcomp1;
  fMit[2][1][3] = -s1*quickcomp1;
  fMit[2][2][3] = a3*s23-a2*s2+d1;
  
  double quickcomp2 = c1*(s23*d4 - quickcomp1);
  double quickcomp3 = s1*(s23*d4 - quickcomp1);
  
  fMit[3][0][0] = -c1*c23*c4+s1*s4;
  fMit[3][1][0] = -s1*c23*c4-c1*s4;
  fMit[3][2][0] = s23*c4;
  fMit[3][0][1] = c1*s23;
  fMit[3][1][1] = s1*s23;
  fMit[3][2][1] = c23;
  fMit[3][0][2] = -c1*c23*s4-s1*c4;
  fMit[3][1][2] = -s1*c23*s4+c1*c4;
  fMit[3][2][2] = s23*s4;
  fMit[3][0][3] = quickcomp2;
  fMit[3][1][3] = quickcomp3;
  fMit[3][2][3] = c23*d4+a3*s23-a2*s2+d1;
  
  fMit[4][0][0] = c1*(s23*s5-c5*c23*c4)+s1*c5*s4;
  fMit[4][1][0] = s1*(s23*s5-c5*c23*c4)-c1*c5*s4;
  fMit[4][2][0] = s23*c4*c5+c23*s5;
  fMit[4][0][1] = c1*c23*s4+s1*c4;
  fMit[4][1][1] = s1*c23*s4-c1*c4;
  fMit[4][2][1] = -s23*s4;
  fMit[4][0][2] = c1*(s23*c5+s5*c23*c4)-s1*s5*s4;
  fMit[4][1][2] = s1*(s23*c5+s5*c23*c4)+c1*s5*s4;
  fMit[4][2][2] = -s23*c4*s5+c23*c5;
  fMit[4][0][3] = quickcomp2;
  fMit[4][1][3] = quickcomp3;
  fMit[4][2][3] = c23*d4+a3*s23-a2*s2+d1;
  
  fMit[5][0][0] = c1*(c23*(c4*c5*c6-s4*s6)-s23*s5*c6)-s1*(s4*c5*c6+c4*s6);
  fMit[5][1][0] = -s1*(c23*(-c4*c5*c6+s4*s6)+s23*s5*c6)+c1*(s4*c5*c6+c4*s6);
  fMit[5][2][0] = s23*(s4*s6-c4*c5*c6)-c23*s5*c6;
  fMit[5][0][1] = -c1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)+s1*(s4*c5*s6-c4*c6);
  fMit[5][1][1] = -s1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)-c1*(s4*c5*s6-c4*c6);
  fMit[5][2][1] = s23*(c4*c5*s6+s4*c6)+c23*s5*s6;
  fMit[5][0][2] = c1*(c23*c4*s5+s23*c5)-s1*s4*s5;
  fMit[5][1][2] = s1*(c23*c4*s5+s23*c5)+c1*s4*s5;
  fMit[5][2][2] = -s23*c4*s5+c23*c5;
  fMit[5][0][3] = quickcomp2;
  fMit[5][1][3] = quickcomp3;
  fMit[5][2][3] = s23*a3+c23*d4-a2*s2+d1;
  
  fMit[6][0][0] = c1*(c23*(c4*c5*c6-s4*s6)-s23*s5*c6)-s1*(s4*c5*c6+c4*s6);
  fMit[6][1][0] = -s1*(c23*(-c4*c5*c6+s4*s6)+s23*s5*c6)+c1*(s4*c5*c6+c4*s6);
  fMit[6][2][0] = s23*(s4*s6-c4*c5*c6)-c23*s5*c6;
  fMit[6][0][1] = -c1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)+s1*(s4*c5*s6-c4*c6);
  fMit[6][1][1] = -s1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)-c1*(s4*c5*s6-c4*c6);
  fMit[6][2][1] = s23*(c4*c5*s6+s4*c6)+c23*s5*s6;
  fMit[6][0][2] = c1*(c23*c4*s5+s23*c5)-s1*s4*s5;
  fMit[6][1][2] = s1*(c23*c4*s5+s23*c5)+c1*s4*s5;
  fMit[6][2][2] = -s23*c4*s5+c23*c5;
  fMit[6][0][3] = c1*(c23*(c4*s5*d6-a3)+s23*(c5*d6+d4)+a1+a2*c2)-s1*s4*s5*d6;
  fMit[6][1][3] = s1*(c23*(c4*s5*d6-a3)+s23*(c5*d6+d4)+a1+a2*c2)+c1*s4*s5*d6;
  fMit[6][2][3] = s23*(a3-c4*s5*d6)+c23*(c5*d6+d4)-a2*s2+d1;
  
  vpHomogeneousMatrix cMe;
  get_cMe(cMe);
  cMe = cMe.inverse();
//   fMit[7] = fMit[6] * cMe;
  vpViper::get_fMc(q,fMit[7]);
  
  #if defined(_WIN32)
  WaitForSingleObject(mutex_fMi,INFINITE);
  for (int i = 0; i < 8; i++)
    fMi[i] = fMit[i];
  ReleaseMutex(mutex_fMi);
  #elif defined(VISP_HAVE_PTHREAD)
  pthread_mutex_lock (&mutex_fMi);
  for (int i = 0; i < 8; i++)
    fMi[i] = fMit[i];
  pthread_mutex_unlock (&mutex_fMi);
  #endif
}


vpRobot::vpRobotStateType
vpSimulatorViper850::setRobotState(vpRobot::vpRobotStateType newState)
{
  switch (newState) {
  case vpRobot::STATE_STOP: {
    // Start primitive STOP only if the current state is Velocity
    if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState ()) {
      stopMotion();
    }
    break;
  }
  case vpRobot::STATE_POSITION_CONTROL: {
    if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState ()) {
      std::cout << "Change the control mode from velocity to position control.\n";
      stopMotion();
    }
    else {
      //std::cout << "Change the control mode from stop to position control.\n";
    }
    break;
  }
  case vpRobot::STATE_VELOCITY_CONTROL: {
    if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
      std::cout << "Change the control mode from stop to velocity control.\n";
    }
    break;
  }
  case vpRobot::STATE_ACCELERATION_CONTROL:
  default:
    break ;
  }

  return vpRobot::setRobotState (newState);
}

void
vpSimulatorViper850::setVelocity (const vpRobot::vpControlFrameType frame, const vpColVector & vel)
{
  if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
    vpERROR_TRACE ("Cannot send a velocity to the robot "
           "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
    throw vpRobotException (vpRobotException::wrongStateError,
                "Cannot send a velocity to the robot "
                "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
  }
  
  vpColVector vel_sat(6);

  double scale_trans_sat = 1;
  double scale_rot_sat   = 1;
  double scale_sat       = 1;
  double vel_trans_max = getMaxTranslationVelocity();
  double vel_rot_max   = getMaxRotationVelocity();
  
  double vel_abs; // Absolute value
  
  // Velocity saturation
  switch(frame)
  {
    // saturation in cartesian space
    case vpRobot::CAMERA_FRAME :
    case vpRobot::REFERENCE_FRAME :
    {
      if (vel.getRows() != 6)
      {
    vpERROR_TRACE ("The velocity vector must have a size of 6 !!!!");
    throw;
      }
      
      for (unsigned int i = 0 ; i < 3; ++ i)
      {
        vel_abs = fabs (vel[i]);
        if (vel_abs > vel_trans_max && !jointLimit)
        {
      vel_trans_max = vel_abs;
      vpERROR_TRACE ("Excess velocity %g m/s in TRANSLATION "
                 "(axis nr. %d).", vel[i], i+1);
        }
      
        vel_abs = fabs (vel[i+3]);
        if (vel_abs > vel_rot_max && !jointLimit) {
      vel_rot_max = vel_abs;
      vpERROR_TRACE ("Excess velocity %g rad/s in ROTATION "
               "(axis nr. %d).", vel[i+3], i+4);
        }
      }
    
      if (vel_trans_max > getMaxTranslationVelocity())                     
        scale_trans_sat = getMaxTranslationVelocity() / vel_trans_max;
    
      if (vel_rot_max > getMaxRotationVelocity())
        scale_rot_sat = getMaxRotationVelocity() / vel_rot_max; 
    
      if ( (scale_trans_sat < 1) || (scale_rot_sat < 1) )
      {
        if (scale_trans_sat < scale_rot_sat)  
      scale_sat = scale_trans_sat;                    
        else                        
      scale_sat = scale_rot_sat;
      }
      break;
    }
    
    // saturation in joint space
    case vpRobot::ARTICULAR_FRAME :
    {
      if (vel.getRows() != 6)
      {
    vpERROR_TRACE ("The velocity vector must have a size of 6 !!!!");
    throw;
      }
      for (unsigned int i = 0 ; i < 6; ++ i)
      {
        vel_abs = fabs (vel[i]);
        if (vel_abs > vel_rot_max && !jointLimit)
        {
      vel_rot_max = vel_abs;
      vpERROR_TRACE ("Excess velocity %g rad/s in ROTATION "
               "(axis nr. %d).", vel[i], i+1);
        }
      }
      if (vel_rot_max > getMaxRotationVelocity())
        scale_rot_sat = getMaxRotationVelocity() / vel_rot_max; 
      if ( scale_rot_sat < 1 ) 
        scale_sat = scale_rot_sat;
      break;
    }
    case vpRobot::MIXT_FRAME :
    {
      break;
    }
  }
  
  set_velocity (vel * scale_sat);
  setRobotFrame (frame);
  setVelocityCalled = true;
}


void
vpSimulatorViper850::computeArticularVelocity()
{
  vpRobot::vpControlFrameType frame = getRobotFrame();
  
  double scale_rot_sat   = 1;
  double scale_sat       = 1;
  double vel_rot_max   = getMaxRotationVelocity();
  double vel_abs;
  
  vpColVector articularCoordinates = get_artCoord();
  vpColVector velocityframe = get_velocity();
  vpColVector articularVelocity;
  
  switch(frame)
  {
    case vpRobot::CAMERA_FRAME :
    {
      vpMatrix eJe_;
      vpVelocityTwistMatrix eVc(eMc);
      vpViper850::get_eJe(articularCoordinates,eJe_);
      eJe_ = eJe_.pseudoInverse();
      if (singularityManagement)
        singularityTest(articularCoordinates,eJe_);
      articularVelocity = eJe_*eVc*velocityframe;
      set_artVel (articularVelocity);
      break;
    }
    case vpRobot::REFERENCE_FRAME :
    {
      vpMatrix fJe_;
      vpViper850::get_fJe(articularCoordinates,fJe_);
      fJe_ = fJe_.pseudoInverse();
      if (singularityManagement)
        singularityTest(articularCoordinates,fJe_);
      articularVelocity = fJe_*velocityframe;
      set_artVel (articularVelocity);
      break;
    }
    case vpRobot::ARTICULAR_FRAME :
    {
      articularVelocity = velocityframe;
      set_artVel (articularVelocity);
      break;
    }
    case vpRobot::MIXT_FRAME :
    {
      break;
    }
  }
  
  
  
  switch(frame)
  {
    case vpRobot::CAMERA_FRAME :
    case vpRobot::REFERENCE_FRAME :
    {
      for (unsigned int i = 0 ; i < 6; ++ i)
      {
        vel_abs = fabs (articularVelocity[i]);
        if (vel_abs > vel_rot_max && !jointLimit)
        {
          vel_rot_max = vel_abs;
          vpERROR_TRACE ("Excess velocity %g rad/s in ROTATION "
             "(axis nr. %d).", articularVelocity[i], i+1);
        }
      }
      if (vel_rot_max > getMaxRotationVelocity())
        scale_rot_sat = getMaxRotationVelocity() / vel_rot_max; 
      if ( scale_rot_sat < 1 ) 
        scale_sat = scale_rot_sat;
    
      set_artVel(articularVelocity * scale_sat);
      break;
    }
    case vpRobot::ARTICULAR_FRAME :
    case vpRobot::MIXT_FRAME :
    {
      break;
    }
  }
}


void
vpSimulatorViper850::getVelocity (const vpRobot::vpControlFrameType frame, vpColVector & vel)
{
  vel.resize(6);
  
  vpColVector articularCoordinates = get_artCoord();
  vpColVector articularVelocity = get_artVel();
  
  switch(frame)
  {
    case vpRobot::CAMERA_FRAME : 
    {
      vpMatrix eJe_;
      vpVelocityTwistMatrix cVe(eMc);
      vpViper850::get_eJe(articularCoordinates,eJe_);
      vel = cVe*eJe_*articularVelocity;
      break ;
    }
    case vpRobot::ARTICULAR_FRAME : 
    {
      vel = articularVelocity;
      break ;
    }
    case vpRobot::REFERENCE_FRAME : 
    {
      vpMatrix fJe_;
      vpViper850::get_fJe(articularCoordinates,fJe_);
      vel = fJe_*articularVelocity;
      break ;
    }
    case vpRobot::MIXT_FRAME : 
    {
      break ;
    }
    default: 
    {
      vpERROR_TRACE ("Error in spec of vpRobot. "
           "Case not taken in account.");
      return;
    }
  }
}

void
vpSimulatorViper850::getVelocity (const vpRobot::vpControlFrameType frame, vpColVector & vel, double &timestamp)
{
  timestamp = vpTime::measureTimeSecond();
  getVelocity(frame, vel);
}

vpColVector
vpSimulatorViper850::getVelocity (vpRobot::vpControlFrameType frame)
{
  vpColVector vel(6);
  getVelocity (frame, vel);

  return vel;
}

vpColVector
vpSimulatorViper850::getVelocity (vpRobot::vpControlFrameType frame, double &timestamp)
{
  timestamp = vpTime::measureTimeSecond();
  vpColVector vel(6);
  getVelocity (frame, vel);

  return vel;
}

void 
vpSimulatorViper850::findHighestPositioningSpeed(vpColVector &q)
{
  double vel_rot_max   = getMaxRotationVelocity();
  double velmax =  fabs(q[0]);
  for (unsigned int i = 1; i < 6; i++)
  {
    if (velmax < fabs(q[i]))
      velmax =  fabs(q[i]);
  }
  
  double alpha = (getPositioningVelocity() * vel_rot_max) / (velmax*100);
  q = q * alpha;
}

void 
vpSimulatorViper850::setPosition(const vpRobot::vpControlFrameType frame,const vpColVector &q)
{
  if (vpRobot::STATE_POSITION_CONTROL != getRobotState ())
  {
    vpERROR_TRACE ("Robot was not in position-based control\n"
             "Modification of the robot state");
   // setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
  }
  
  vpColVector articularCoordinates = get_artCoord();
  
  vpColVector error(6);
  double errsqr = 0;
  switch(frame)
  {
    case vpRobot::CAMERA_FRAME : 
    {
      unsigned int nbSol;
      vpColVector qdes(6);

      vpTranslationVector txyz;
      vpRxyzVector rxyz;
      for (unsigned int i=0; i < 3; i++)
      {
        txyz[i] = q[i];
        rxyz[i] = q[i+3];
      }

      vpRotationMatrix cRc2(rxyz);
      vpHomogeneousMatrix cMc2(txyz, cRc2);

      vpHomogeneousMatrix fMc_;
      vpViper850::get_fMc(articularCoordinates, fMc_);
    
      vpHomogeneousMatrix fMc2 = fMc_ * cMc2;
    
      do
      {
        articularCoordinates = get_artCoord();
        qdes = articularCoordinates;
        nbSol = getInverseKinematics(fMc2, qdes, verbose_);
        setVelocityCalled = true;
        if (nbSol > 0)
        {
          error = qdes - articularCoordinates;
          errsqr = error.sumSquare();
          //findHighestPositioningSpeed(error);
          set_artVel(error);
          if (errsqr < 1e-4)
          {
            set_artCoord (qdes);
            error = 0;
            set_artVel(error);
            set_velocity(error);
            break;
          }
        }
        else
        {
          vpERROR_TRACE ("Positionning error.");
          throw vpRobotException (vpRobotException::positionOutOfRangeError,
                "Position out of range.");
        }
      }while (errsqr > 1e-8 && nbSol > 0);

      break ;
    }
      
    case vpRobot::ARTICULAR_FRAME:
    {
      do
      {
        articularCoordinates = get_artCoord();
        error = q - articularCoordinates;
        errsqr = error.sumSquare();
        //findHighestPositioningSpeed(error);
        set_artVel(error);
        setVelocityCalled = true;
        if (errsqr < 1e-4)
        {
          set_artCoord (q);
          error = 0;
          set_artVel(error);
          set_velocity(error);
          break;
        }
      }while (errsqr > 1e-8);
      break ;
    }
      
    case vpRobot::REFERENCE_FRAME:
    {
      unsigned int nbSol;
      vpColVector qdes(6);

      vpTranslationVector txyz;
      vpRxyzVector rxyz;
      for (unsigned int i=0; i < 3; i++)
      {
        txyz[i] = q[i];
        rxyz[i] = q[i+3];
      }

      vpRotationMatrix fRc(rxyz);
      vpHomogeneousMatrix fMc_(txyz, fRc);

      do
      {
        articularCoordinates = get_artCoord();
        qdes = articularCoordinates;
        nbSol = getInverseKinematics(fMc_, qdes, verbose_);
        if (nbSol > 0)
        {
          error = qdes - articularCoordinates;
          errsqr = error.sumSquare();
          //findHighestPositioningSpeed(error);
          set_artVel(error);
          setVelocityCalled = true;
          if (errsqr < 1e-4)
          {
            set_artCoord (qdes);
            error = 0;
            set_artVel(error);
            set_velocity(error);
            break;
          }
        }
        else
          vpERROR_TRACE ("Positionning error. Position unreachable");
      }while (errsqr > 1e-8 && nbSol > 0);
      break ;
    }
    case vpRobot::MIXT_FRAME:
    {
      vpERROR_TRACE ("Positionning error. Mixt frame not implemented");
      throw vpRobotException (vpRobotException::lowLevelError,
                  "Positionning error: "
                  "Mixt frame not implemented.");
      break ;
    }
  }
}

void vpSimulatorViper850::setPosition (const vpRobot::vpControlFrameType frame,
                   const double pos1,
                   const double pos2,
                   const double pos3,
                   const double pos4,
                   const double pos5,
                   const double pos6)
{
  try{
    vpColVector position(6) ;
    position[0] = pos1 ;
    position[1] = pos2 ;
    position[2] = pos3 ;
    position[3] = pos4 ;
    position[4] = pos5 ;
    position[5] = pos6 ;

    setPosition(frame, position) ;
  }
  catch(...)
    {
      vpERROR_TRACE("Error caught");
      throw ;
    }
}

void vpSimulatorViper850::setPosition(const char *filename)
{
  vpColVector q;
  bool ret;

  ret = this->readPosFile(filename, q);

  if (ret == false) {
    vpERROR_TRACE ("Bad position in \"%s\"", filename);
    throw vpRobotException (vpRobotException::lowLevelError,
                "Bad position in filename.");
  }
  this->setRobotState(vpRobot::STATE_POSITION_CONTROL);
  this->setPosition(vpRobot::ARTICULAR_FRAME, q);
}

void 
vpSimulatorViper850::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
{
  q.resize(6);
  
  switch(frame)
  {
    case vpRobot::CAMERA_FRAME : 
    {
      q = 0;
      break ;
    }
      
    case vpRobot::ARTICULAR_FRAME:
    {
      q = get_artCoord();
      break ;
    }
      
    case vpRobot::REFERENCE_FRAME:
    {
      vpHomogeneousMatrix fMc_;
      vpViper::get_fMc (get_artCoord(), fMc_);
      
      vpRotationMatrix fRc;
      fMc_.extract(fRc);
      vpRxyzVector rxyz(fRc);
      
      vpTranslationVector txyz;
      fMc_.extract(txyz);
      
      for (unsigned int i=0; i <3; i++)
      {
        q[i] = txyz[i];
        q[i+3] = rxyz[i];
      }
      break ;
    }
    
    case vpRobot::MIXT_FRAME:
    {
      vpERROR_TRACE ("Positionning error. Mixt frame not implemented");
      throw vpRobotException (vpRobotException::lowLevelError,
                  "Positionning error: "
                  "Mixt frame not implemented.");
      break ;
    }
  }
}

void
vpSimulatorViper850::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q, double &timestamp)
{
  timestamp = vpTime::measureTimeSecond();
  getPosition(frame, q);
}


void 
vpSimulatorViper850::getPosition (const vpRobot::vpControlFrameType frame,   
                                  vpPoseVector &position)
{
  vpColVector posRxyz;
  //recupere  position en Rxyz
  this->getPosition(frame,posRxyz);
  
  //recupere le vecteur thetaU correspondant
  vpThetaUVector RtuVect(vpRxyzVector(posRxyz[3],posRxyz[4],posRxyz[5]));

  //remplit le vpPoseVector avec translation et rotation ThetaU
  for(unsigned int j=0;j<3;j++)
  {
    position[j]=posRxyz[j];
    position[j+3]=RtuVect[j];
  }
}

void
vpSimulatorViper850::getPosition(const vpRobot::vpControlFrameType frame,
                                 vpPoseVector &position, double &timestamp)
{
  timestamp = vpTime::measureTimeSecond();
  getPosition(frame, position);
}

void 
vpSimulatorViper850::setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
{
  if (limitMin.getRows() != 6 || limitMax.getRows() != 6)
  {
    vpTRACE("Joint limit vector has not a size of 6 !");
    return;
  }
  
  joint_min[0] = limitMin[0];
  joint_min[1] = limitMin[1];
  joint_min[2] = limitMin[2];
  joint_min[3] = limitMin[3];
  joint_min[4] = limitMin[4];
  joint_min[5] = limitMin[5];

  joint_max[0] = limitMax[0];
  joint_max[1] = limitMax[1];
  joint_max[2] = limitMax[2];
  joint_max[3] = limitMax[3];
  joint_max[4] = limitMax[4];
  joint_max[5] = limitMax[5];

}

bool
vpSimulatorViper850::singularityTest(const vpColVector q, vpMatrix &J)
{
  double q2 = q[1];
  double q3 = q[2];
  double q5 = q[4];

  double c2 = cos(q2);
  double c3 = cos(q3);
  double s3 = sin(q3);
  double c23 = cos(q2+q3);
  double s23 = sin(q2+q3);
  double s5 = sin(q5);
  
  bool cond1 = fabs(s5) < 1e-1;
  bool cond2 = fabs(a3*s3+c3*d4) < 1e-1;
  bool cond3 = fabs(a2*c2-a3*c23+s23*d4+a1) < 1e-1;
  
  if(cond1)
  {
    J[3][0] = 0;
    J[5][0] = 0;
    J[3][1] = 0;
    J[5][1] = 0;
    J[3][2] = 0;
    J[5][2] = 0;
    J[3][3] = 0;
    J[5][3] = 0;
    J[3][4] = 0;
    J[5][4] = 0;
    J[3][5] = 0;
    J[5][5] = 0;
    return true;
  }
  
  if(cond2)
  {
    J[1][0] = 0; J[2][0] = 0; J[3][0] = 0; J[4][0] = 0; J[5][0] = 0;
    J[1][1] = 0; J[2][1] = 0; J[3][1] = 0; J[4][1] = 0; J[5][1] = 0;
    J[1][2] = 0; J[2][2] = 0; J[3][2] = 0; J[4][2] = 0; J[5][2] = 0;
    return true;
  }
  
  if(cond3)
  {
    J[0][0] = 0; J[3][0] = 0; J[4][0] = 0; J[5][0] = 0;
    J[0][1] = 0; J[3][1] = 0; J[4][1] = 0; J[5][1] = 0;
  }
  
  return false;
}

int
vpSimulatorViper850::isInJointLimit ()
{
  int artNumb = 0;
  double diff = 0;
  double difft = 0;
  
  vpColVector articularCoordinates = get_artCoord();
  
  for (unsigned int i = 0; i < 6; i++)
  {
    if (articularCoordinates[i] <= joint_min[i])
    {
      difft = joint_min[i] - articularCoordinates[i];
      if (difft > diff)
      {
    diff = difft;
    artNumb = -(int)i-1;
      }
    }
  }
  
  for (unsigned int i = 0; i < 6; i++)
  {
    if (articularCoordinates[i] >= joint_max[i])
    {
      difft = articularCoordinates[i] - joint_max[i];
      if (difft > diff)
      {
    diff = difft;
    artNumb = (int)(i+1);
      }
    }
  }
  
  if (artNumb != 0)
    std::cout << "\nWarning: Velocity control stopped: axis " << fabs((float)artNumb) << " on joint limit!" <<std::endl;
  
  return artNumb;
}

void
vpSimulatorViper850::getCameraDisplacement(vpColVector &displacement)
{
  getDisplacement(vpRobot::CAMERA_FRAME, displacement);
}

void
vpSimulatorViper850::getArticularDisplacement(vpColVector  &displacement)
{
  getDisplacement(vpRobot::ARTICULAR_FRAME, displacement);
}

void
vpSimulatorViper850::getDisplacement(vpRobot::vpControlFrameType frame,
                 vpColVector &displacement)
{
  displacement.resize (6);
  displacement = 0;
  vpColVector q_cur(6);

  q_cur = get_artCoord();

  if ( ! first_time_getdis ) 
  {
    switch (frame) 
    {
      case vpRobot::CAMERA_FRAME: 
      {
        std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
        return;
        break ;
      }

      case vpRobot::ARTICULAR_FRAME: 
      {
        displacement = q_cur - q_prev_getdis;
        break ;
      }

      case vpRobot::REFERENCE_FRAME: 
      {
        std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
        return;
        break ;
      }

      case vpRobot::MIXT_FRAME: 
      {
        std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
        return;
        break ;
      }
    }
  }
  else 
  {
    first_time_getdis = false;
  }

  // Memorize the joint position for the next call
  q_prev_getdis = q_cur;
}

bool
vpSimulatorViper850::readPosFile(const char *filename, vpColVector &q)
{
  FILE * fd ;
  fd = fopen(filename, "r") ;
  if (fd == NULL)
    return false;

  char line[FILENAME_MAX];
  char dummy[FILENAME_MAX];
  char head[] = "R:";
  bool sortie = false;

  do {
    // Saut des lignes commencant par #
    if (fgets (line, FILENAME_MAX, fd) != NULL) {
      if ( strncmp (line, "#", 1) != 0) {
        // La ligne n'est pas un commentaire
        if ( strncmp (line, head, sizeof(head)-1) == 0) {
          sortie = true;    // Position robot trouvee.
        }
        //  else
        //    return (false); // fin fichier sans position robot.
      }
    }
    else {
      fclose(fd) ;
      return (false);       /* fin fichier  */
    }
  }
  while ( sortie != true );

  // Lecture des positions
  q.resize(njoint);
  int ret = sscanf(line, "%s %lf %lf %lf %lf %lf %lf",
                   dummy,
                   &q[0], &q[1], &q[2], &q[3], &q[4], &q[5]);
  if (ret != 7) {
    fclose(fd) ;
    return false;
  }

  // converts rotations from degrees into radians
  q.deg2rad();

  fclose(fd) ;
  return (true);
}

bool
vpSimulatorViper850::savePosFile(const char *filename, const vpColVector &q)
{

  FILE * fd ;
  fd = fopen(filename, "w") ;
  if (fd == NULL)
    return false;

  fprintf(fd, "\
#Viper - Position - Version 1.0\n\
#\n\
# R: A B C D E F\n\
# Joint position in degrees\n\
#\n\
#\n\n");

  // Save positions in mm and deg
  fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n",
      vpMath::deg(q[0]),
      vpMath::deg(q[1]),
      vpMath::deg(q[2]),
      vpMath::deg(q[3]),
      vpMath::deg(q[4]),
      vpMath::deg(q[5]));

  fclose(fd) ;
  return (true);
}

void
vpSimulatorViper850::move(const char *filename)
{
  vpColVector q;

  try
  {
    this->readPosFile(filename, q);
    this->setRobotState(vpRobot::STATE_POSITION_CONTROL);
    this->setPosition ( vpRobot::ARTICULAR_FRAME,  q);
  }
  catch(...) 
  {
    throw;
  }
}

void
vpSimulatorViper850::get_cMe(vpHomogeneousMatrix &cMe)
{
  vpViper850::get_cMe(cMe) ;
}

void
vpSimulatorViper850::get_cVe(vpVelocityTwistMatrix &cVe)
{
  vpHomogeneousMatrix cMe ;
  vpViper850::get_cMe(cMe) ;

  cVe.buildFrom(cMe) ;
}

void
vpSimulatorViper850::get_eJe(vpMatrix &eJe_)
{
  try
  {
    vpViper850::get_eJe(get_artCoord(), eJe_) ;
  }
  catch(...)
  {
    vpERROR_TRACE("catch exception ") ;
    throw ;
  }
}

void
vpSimulatorViper850::get_fJe(vpMatrix &fJe_)
{
  try
  {
    vpColVector articularCoordinates = get_artCoord();
    vpViper850::get_fJe(articularCoordinates, fJe_) ;
  }
  catch(...)
  {
    vpERROR_TRACE("Error caught");
    throw ;
  }
}

void 
vpSimulatorViper850::stopMotion()
{
  if (getRobotState() != vpRobot::STATE_VELOCITY_CONTROL)
    return;
  
  vpColVector stop(6);
  stop = 0;
  set_artVel(stop);
  set_velocity(stop);
  vpRobot::setRobotState (vpRobot::STATE_STOP);
}



/**********************************************************************************/
/**********************************************************************************/
/**********************************************************************************/
/**********************************************************************************/

void
vpSimulatorViper850::initArms()
{
  // set scene_dir from #define VISP_SCENE_DIR if it exists
  // VISP_SCENES_DIR may contain multiple locations separated by ";"
  std::string scene_dir_;
  std::vector<std::string> scene_dirs = vpIoTools::splitChain(std::string(VISP_SCENES_DIR), std::string(";"));
  bool sceneDirExists = false;
  for(size_t i=0; i < scene_dirs.size(); i++)
  if (vpIoTools::checkDirectory(scene_dirs[i]) == true) { // directory exists
    scene_dir_ = scene_dirs[i];
    sceneDirExists = true;
    break;
  }
  if (! sceneDirExists) {
    try {
      scene_dir_ = vpIoTools::getenv("VISP_SCENES_DIR");
      std::cout << "The simulator uses data from VISP_SCENES_DIR=" << scene_dir_ << std::endl;
    }
    catch (...) {
      std::cout << "Cannot get VISP_SCENES_DIR environment variable" << std::endl;
    }
  }

  unsigned int name_length = 30; // the size of this kind of string "/viper850_arm2.bnd"
  if (scene_dir_.size() > FILENAME_MAX)
    throw vpException (vpException::dimensionError, "Cannot initialize Viper850 simulator");

  unsigned int full_length = (unsigned int)scene_dir_.size() + name_length;
  if (full_length > FILENAME_MAX)
    throw vpException (vpException::dimensionError, "Cannot initialize Viper850 simulator");

  char *name_cam = new char [full_length];

  strcpy(name_cam, scene_dir_.c_str());
  strcat(name_cam,"/camera.bnd");
  set_scene(name_cam,&camera,cameraFactor);
  
  if (arm_dir.size() > FILENAME_MAX)
    throw vpException (vpException::dimensionError, "Cannot initialize Viper850 simulator");
  full_length = (unsigned int)arm_dir.size() + name_length;
  if (full_length > FILENAME_MAX)
    throw vpException (vpException::dimensionError, "Cannot initialize Viper850 simulator");

  char *name_arm = new char [full_length];
  strcpy(name_arm, arm_dir.c_str());
  strcat(name_arm,"/viper850_arm1.bnd");
  set_scene(name_arm, robotArms, 1.0);
  strcpy(name_arm, arm_dir.c_str());
  strcat(name_arm,"/viper850_arm2.bnd");
  set_scene(name_arm, robotArms+1, 1.0);
  strcpy(name_arm, arm_dir.c_str());
  strcat(name_arm,"/viper850_arm3.bnd");
  set_scene(name_arm, robotArms+2, 1.0);
  strcpy(name_arm, arm_dir.c_str());
  strcat(name_arm,"/viper850_arm4.bnd");
  set_scene(name_arm, robotArms+3, 1.0);
  strcpy(name_arm, arm_dir.c_str());
  strcat(name_arm,"/viper850_arm5.bnd");
  set_scene(name_arm, robotArms+4, 1.0);
  strcpy(name_arm, arm_dir.c_str());
  strcat(name_arm,"/viper850_arm6.bnd");
  set_scene(name_arm, robotArms+5, 1.0);
  
//   set_scene("./arm2.bnd", robotArms+1, 1.0);
//   set_scene("./arm3.bnd", robotArms+2, 1.0);
//   set_scene("./arm4.bnd", robotArms+3, 1.0);
//   set_scene("./arm5.bnd", robotArms+4, 1.0);
//   set_scene("./arm6.bnd", robotArms+5, 1.0);
  
  add_rfstack(IS_BACK);

  add_vwstack ("start","depth", 0.0, 100.0);
  add_vwstack ("start","window", -0.1,0.1,-0.1,0.1);
  add_vwstack ("start","type", PERSPECTIVE);
// 
//   sceneInitialized = true;
//   displayObject = true;
  displayCamera = true;

  delete [] name_cam;
  delete [] name_arm;
}


void 
vpSimulatorViper850::getExternalImage(vpImage<vpRGBa> &I_)
{
  bool changed = false;
  vpHomogeneousMatrix displacement = navigation(I_,changed);

  //if (displacement[2][3] != 0)
  if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon())
    camMf2 = camMf2*displacement;

  f2Mf = camMf2.inverse()*camMf;

  camMf = camMf2* displacement * f2Mf;

  double u;
  double v;
  //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()))
  {
    u = (double)I_.getWidth()/(2*px_ext);
    v = (double)I_.getHeight()/(2*py_ext);
  }
  else
  {
    u = (double)I_.getWidth()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
    v = (double)I_.getHeight()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
  }

  float w44o[4][4],w44cext[4][4],x,y,z;

  vp2jlc_matrix(camMf.inverse(),w44cext);

  add_vwstack ("start","cop", w44cext[3][0],w44cext[3][1],w44cext[3][2]);
  x = w44cext[2][0] + w44cext[3][0];
  y = w44cext[2][1] + w44cext[3][1];
  z = w44cext[2][2] + w44cext[3][2];
  add_vwstack ("start","vrp", x,y,z);
  add_vwstack ("start","vpn", w44cext[2][0],w44cext[2][1],w44cext[2][2]);
  add_vwstack ("start","vup", w44cext[1][0],w44cext[1][1],w44cext[1][2]);
  add_vwstack ("start","window", -u, u, -v, v);
  
  vpHomogeneousMatrix fMit[8];
  get_fMi(fMit);
  
  vp2jlc_matrix(vpHomogeneousMatrix(0,0,0,0,0,0),w44o);
  display_scene(w44o,robotArms[0],I_, curColor);
  
  vp2jlc_matrix(fMit[0],w44o);
  display_scene(w44o,robotArms[1],I_, curColor);
  
  vp2jlc_matrix(fMit[1],w44o);
  display_scene(w44o,robotArms[2],I_, curColor);
  
  vp2jlc_matrix(fMit[2],w44o);
  display_scene(w44o,robotArms[3],I_, curColor);
  
  vp2jlc_matrix(fMit[3],w44o);
  display_scene(w44o,robotArms[4],I_, curColor);
  
  vp2jlc_matrix(fMit[6],w44o);
  display_scene(w44o,robotArms[5],I_, curColor);

  if (displayCamera)
  {
    vpHomogeneousMatrix cMe;
    get_cMe(cMe);
    cMe = cMe.inverse();
    cMe = fMit[6] * cMe;
    vp2jlc_matrix(cMe,w44o);
    display_scene(w44o,camera, I_, camColor);
  }
  
  if (displayObject)
  {
    vp2jlc_matrix(fMo,w44o);
    display_scene(w44o,scene,I_, curColor);
  }
}

bool
vpSimulatorViper850::initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo_)
{
  vpColVector stop(6);
  bool status = true;
  stop = 0;
  set_artVel(stop);
  set_velocity(stop);
  vpHomogeneousMatrix fMc_;
  fMc_ = fMo * cMo_.inverse();
  
  vpColVector articularCoordinates = get_artCoord();
  unsigned int nbSol = getInverseKinematics(fMc_, articularCoordinates, verbose_);
  
  if (nbSol == 0) {
    status = false;
    vpERROR_TRACE ("Positionning error. Position unreachable");
  }

  if (verbose_)
    std::cout << "Used joint coordinates (rad): " << articularCoordinates.t() << std::endl;

  set_artCoord(articularCoordinates);
  
  compute_fMi();

  return status;
}

void 
vpSimulatorViper850::initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo_)
{
  vpColVector stop(6);
  stop = 0;
  set_artVel(stop);
  set_velocity(stop);
  vpHomogeneousMatrix fMit[8];
  get_fMi(fMit);
  fMo = fMit[7] * cMo_;
}

#elif !defined(VISP_BUILD_SHARED_LIBS)
// Work arround to avoid warning: libvisp_robot.a(vpSimulatorViper850.cpp.o) has no symbols
void dummy_vpSimulatorViper850() {};
#endif