vpSimulatorViper850.cpp

/****************************************************************************
 *
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2023 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 as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * 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 https://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.
 *
*****************************************************************************/
 
#include <visp3/robot/vpSimulatorViper850.h>
 
#if defined(VISP_HAVE_MODULE_GUI) && ((defined(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_PTHREAD))
 
#include <cmath>  // std::fabs
#include <limits> // numeric_limits
#include <string>
#include <visp3/core/vpImagePoint.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpMeterPixelConversion.h>
#include <visp3/core/vpPoint.h>
#include <visp3/core/vpTime.h>
 
#include "../wireframe-simulator/vpBound.h"
#include "../wireframe-simulator/vpRfstack.h"
#include "../wireframe-simulator/vpScene.h"
#include "../wireframe-simulator/vpVwstack.h"
 
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)
 
  DWORD dwThreadIdArray;
  hThread = CreateThread(nullptr,              // 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_attr_init(&attr);
  pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
 
  pthread_create(&thread, nullptr, 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)
  DWORD dwThreadIdArray;
  hThread = CreateThread(nullptr,              // 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_attr_init(&attr);
  pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
 
  pthread_create(&thread, nullptr, launcher, (void *)this);
#endif
 
  compute_fMi();
}
 
vpSimulatorViper850::~vpSimulatorViper850()
{
  m_mutex_robotStop.lock();
  robotStop = true;
  m_mutex_robotStop.unlock();
 
#if defined(_WIN32)
#if defined(WINRT_8_1)
  WaitForSingleObjectEx(hThread, INFINITE, FALSE);
#else // pure win32
  WaitForSingleObject(hThread, INFINITE);
#endif
  CloseHandle(hThread);
#elif defined(VISP_HAVE_PTHREAD)
  pthread_attr_destroy(&attr);
  pthread_join(thread, nullptr);
#endif
 
  if (robotArms != nullptr) {
    // 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 = nullptr;
  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_CUSTOM:
  case vpViper850::TOOL_GENERIC_CAMERA: {
    std::cout << "This tool is not handled in vpSimulatorViper850.cpp" << std::endl;
    break;
  }
  }
 
  vpRotationMatrix eRc(erc);
 
  m_mutex_eMc.lock();
  this->eMc.buildFrom(etc, eRc);
  m_mutex_eMc.unlock();
 
  setToolType(tool);
  return;
}
 
void vpSimulatorViper850::getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width,
                                              const unsigned int &image_height)
{
  if (toolCustom) {
    cam.initPersProjWithoutDistortion(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_CUSTOM:
  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
 
  bool stop = false;
  bool setVelocityCalled_ = false;
  while (!stop) {
    // Get current time
    tprev = tcur_1;
    tcur = vpTime::measureTimeMs();
 
    m_mutex_setVelocityCalled.lock();
    setVelocityCalled_ = setVelocityCalled;
    m_mutex_setVelocityCalled.unlock();
 
    if (setVelocityCalled_ || !constantSamplingTimeMode) {
      m_mutex_setVelocityCalled.lock();
      setVelocityCalled = false;
      m_mutex_setVelocityCalled.unlock();
      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();
      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());
    }
    m_mutex_robotStop.lock();
    stop = robotStop;
    m_mutex_robotStop.unlock();
  }
}
 
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;
  m_mutex_eMc.lock();
  vpViper::get_fMc(q, fMit[7]);
  m_mutex_eMc.unlock();
 
  m_mutex_fMi.lock();
  for (int i = 0; i < 8; i++) {
    fMi[i] = fMit[i];
  }
  m_mutex_fMi.unlock();
}
 
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_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);
      }
    }
 
    double scale_trans_sat = 1;
    double scale_rot_sat = 1;
    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);
      }
    }
    double scale_rot_sat = 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::END_EFFECTOR_FRAME:
  case vpRobot::MIXT_FRAME: {
    break;
  }
  }
 
  set_velocity(vel * scale_sat);
 
  m_mutex_frame.lock();
  setRobotFrame(frame);
  m_mutex_frame.unlock();
 
  m_mutex_setVelocityCalled.lock();
  setVelocityCalled = true;
  m_mutex_setVelocityCalled.unlock();
}
 
void vpSimulatorViper850::computeArticularVelocity()
{
  vpRobot::vpControlFrameType frame;
 
  m_mutex_frame.lock();
  frame = getRobotFrame();
  m_mutex_frame.unlock();
 
  double vel_rot_max = getMaxRotationVelocity();
 
  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::END_EFFECTOR_FRAME:
  case vpRobot::MIXT_FRAME: {
    break;
  }
  }
 
  switch (frame) {
  case vpRobot::CAMERA_FRAME:
  case vpRobot::REFERENCE_FRAME: {
    for (unsigned int i = 0; i < 6; ++i) {
      double 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);
      }
    }
    double scale_rot_sat = 1;
    double scale_sat = 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::END_EFFECTOR_FRAME:
  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::END_EFFECTOR_FRAME:
  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_);
      m_mutex_setVelocityCalled.lock();
      setVelocityCalled = true;
      m_mutex_setVelocityCalled.unlock();
      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("Positioning 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);
      m_mutex_setVelocityCalled.lock();
      setVelocityCalled = true;
      m_mutex_setVelocityCalled.unlock();
      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);
        m_mutex_setVelocityCalled.lock();
        setVelocityCalled = true;
        m_mutex_setVelocityCalled.unlock();
        if (errsqr < 1e-4) {
          set_artCoord(qdes);
          error = 0;
          set_artVel(error);
          set_velocity(error);
          break;
        }
      } else
        vpERROR_TRACE("Positioning error. Position unreachable");
    } while (errsqr > 1e-8 && nbSol > 0);
    break;
  }
  case vpRobot::MIXT_FRAME: {
    throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
                                                            "Mixt frame not implemented.");
  }
  case vpRobot::END_EFFECTOR_FRAME: {
    throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
                                                            "End-effector frame not implemented.");
  }
  }
}
 
void vpSimulatorViper850::setPosition(const vpRobot::vpControlFrameType frame, double pos1, double pos2, double pos3,
                                      double pos4, double pos5, 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("Positioning error. Mixt frame not implemented");
    throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
                                                            "Mixt frame not implemented.");
  }
  case vpRobot::END_EFFECTOR_FRAME: {
    vpERROR_TRACE("Positioning error. Mixt frame not implemented");
    throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
                                                            "End-effector frame not implemented.");
  }
  }
}
 
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::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;
    }
 
    case vpRobot::ARTICULAR_FRAME: {
      displacement = q_cur - q_prev_getdis;
      break;
    }
 
    case vpRobot::REFERENCE_FRAME: {
      std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
      return;
    }
 
    case vpRobot::MIXT_FRAME: {
      std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
      return;
    }
    case vpRobot::END_EFFECTOR_FRAME: {
      std::cout << "getDisplacement() END_EFFECTOR_FRAME not implemented\n";
      return;
    }
    }
  } else {
    first_time_getdis = false;
  }
 
  // Memorize the joint position for the next call
  q_prev_getdis = q_cur;
}
 
bool vpSimulatorViper850::readPosFile(const std::string &filename, vpColVector &q)
{
  std::ifstream fd(filename.c_str(), std::ios::in);
 
  if (!fd.is_open()) {
    return false;
  }
 
  std::string line;
  std::string key("R:");
  std::string id("#Viper850 - Position");
  bool pos_found = false;
  int lineNum = 0;
 
  q.resize(njoint);
 
  while (std::getline(fd, line)) {
    lineNum++;
    if (lineNum == 1) {
      if (!(line.compare(0, id.size(), id) == 0)) { // check if Viper850 position file
        std::cout << "Error: this position file " << filename << " is not for Viper850 robot" << std::endl;
        return false;
      }
    }
    if ((line.compare(0, 1, "#") == 0)) { // skip comment
      continue;
    }
    if ((line.compare(0, key.size(), key) == 0)) { // decode position
      // check if there are at least njoint values in the line
      std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
      if (chain.size() < njoint + 1) // try to split with tab separator
        chain = vpIoTools::splitChain(line, std::string("\t"));
      if (chain.size() < njoint + 1)
        continue;
 
      std::istringstream ss(line);
      std::string key_;
      ss >> key_;
      for (unsigned int i = 0; i < njoint; i++)
        ss >> q[i];
      pos_found = true;
      break;
    }
  }
 
  // converts rotations from degrees into radians
  q.deg2rad();
 
  fd.close();
 
  if (!pos_found) {
    std::cout << "Error: unable to find a position for Viper850 robot in " << filename << std::endl;
    return false;
  }
 
  return true;
}
 
bool vpSimulatorViper850::savePosFile(const std::string &filename, const vpColVector &q)
{
 
  FILE *fd;
  fd = fopen(filename.c_str(), "w");
  if (fd == nullptr)
    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_)
{
  m_mutex_scene.lock();
  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);
  }
  m_mutex_scene.unlock();
}
 
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("Positioning 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;
  m_mutex_scene.lock();
  set_artVel(stop);
  set_velocity(stop);
  vpHomogeneousMatrix fMit[8];
  get_fMi(fMit);
  fMo = fMit[7] * cMo_;
  m_mutex_scene.unlock();
}
 
#elif !defined(VISP_BUILD_SHARED_LIBS)
// Work around to avoid warning: libvisp_robot.a(vpSimulatorViper850.cpp.o)
// has no symbols
void dummy_vpSimulatorViper850(){};
#endif