vpRobotViper650.cpp

/****************************************************************************
 *
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2019 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 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:
 * Interface for the Irisa's Viper S650 robot.
 *
 * Authors:
 * Fabien Spindler
 *
 *****************************************************************************/

#include <visp3/core/vpConfig.h>

#ifdef VISP_HAVE_VIPER650

#include <signal.h>
#include <stdlib.h>
#include <sys/types.h>
#include <unistd.h>

#include <visp3/core/vpDebug.h>
#include <visp3/core/vpExponentialMap.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpThetaUVector.h>
#include <visp3/core/vpVelocityTwistMatrix.h>
#include <visp3/robot/vpRobot.h>
#include <visp3/robot/vpRobotException.h>
#include <visp3/robot/vpRobotViper650.h>

/* ---------------------------------------------------------------------- */
/* --- STATIC ----------------------------------------------------------- */
/* ---------------------------------------------------------------------- */

bool vpRobotViper650::m_robotAlreadyCreated = false;

const double vpRobotViper650::m_defaultPositioningVelocity = 15.0;

/* ---------------------------------------------------------------------- */
/* --- EMERGENCY STOP --------------------------------------------------- */
/* ---------------------------------------------------------------------- */

void emergencyStopViper650(int signo)
{
  std::cout << "Stop the Viper650 application by signal (" << signo << "): " << (char)7;
  switch (signo) {
  case SIGINT:
    std::cout << "SIGINT (stop by ^C) " << std::endl;
    break;
  case SIGBUS:
    std::cout << "SIGBUS (stop due to a bus error) " << std::endl;
    break;
  case SIGSEGV:
    std::cout << "SIGSEGV (stop due to a segmentation fault) " << std::endl;
    break;
  case SIGKILL:
    std::cout << "SIGKILL (stop by CTRL \\) " << std::endl;
    break;
  case SIGQUIT:
    std::cout << "SIGQUIT " << std::endl;
    break;
  default:
    std::cout << signo << std::endl;
  }
  // std::cout << "Emergency stop called\n";
  //  PrimitiveESTOP_Viper650();
  PrimitiveSTOP_Viper650();
  std::cout << "Robot was stopped\n";

  // Free allocated resources
  //  ShutDownConnection(); // Some times cannot exit here when Ctrl-C

  fprintf(stdout, "Application ");
  fflush(stdout);
  kill(getpid(), SIGKILL);
  exit(1);
}

/* ---------------------------------------------------------------------- */
/* --- CONSTRUCTOR ------------------------------------------------------ */
/* ---------------------------------------------------------------------- */

vpRobotViper650::vpRobotViper650(bool verbose) : vpViper650(), vpRobot()
{

  /*
    #define SIGHUP  1   // hangup
    #define SIGINT  2   // interrupt (rubout)
    #define SIGQUIT 3   // quit (ASCII FS)
    #define SIGILL  4   // illegal instruction (not reset when caught)
    #define SIGTRAP 5   // trace trap (not reset when caught)
    #define SIGIOT  6   // IOT instruction
    #define SIGABRT 6   // used by abort, replace SIGIOT in the future
    #define SIGEMT  7   // EMT instruction
    #define SIGFPE  8   // floating point exception
    #define SIGKILL 9   // kill (cannot be caught or ignored)
    #define SIGBUS  10  // bus error
    #define SIGSEGV 11  // segmentation violation
    #define SIGSYS  12  // bad argument to system call
    #define SIGPIPE 13  // write on a pipe with no one to read it
    #define SIGALRM 14  // alarm clock
    #define SIGTERM 15  // software termination signal from kill
  */

  signal(SIGINT, emergencyStopViper650);
  signal(SIGBUS, emergencyStopViper650);
  signal(SIGSEGV, emergencyStopViper650);
  signal(SIGKILL, emergencyStopViper650);
  signal(SIGQUIT, emergencyStopViper650);

  setVerbose(verbose);
  if (verbose_)
    std::cout << "Open communication with MotionBlox.\n";
  try {
    this->init();
    this->setRobotState(vpRobot::STATE_STOP);
  } catch (...) {
    //  vpERROR_TRACE("Error caught") ;
    throw;
  }
  m_positioningVelocity = m_defaultPositioningVelocity;

  m_maxRotationVelocity_joint6 = maxRotationVelocity;

  vpRobotViper650::m_robotAlreadyCreated = true;

  return;
}

/* ------------------------------------------------------------------------ */
/* --- INITIALISATION ----------------------------------------------------- */
/* ------------------------------------------------------------------------ */

void vpRobotViper650::init(void)
{
  InitTry;

  // Initialise private variables used to compute the measured velocities
  m_q_prev_getvel.resize(6);
  m_q_prev_getvel = 0;
  m_time_prev_getvel = 0;
  m_first_time_getvel = true;

  // Initialise private variables used to compute the measured displacement
  m_q_prev_getdis.resize(6);
  m_q_prev_getdis = 0;
  m_first_time_getdis = true;

  // Initialize the firewire connection
  Try(InitializeConnection(verbose_));

  // Connect to the servoboard using the servo board GUID
  Try(InitializeNode_Viper650());

  Try(PrimitiveRESET_Viper650());

  // Enable the joint limits on axis 6
  Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper650(0));

  // Update the eMc matrix in the low level controller
  init(vpViper650::defaultTool);

  // Look if the power is on or off
  UInt32 HIPowerStatus;
  UInt32 EStopStatus;
  Try(PrimitiveSTATUS_Viper650(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
  CAL_Wait(0.1);

  // Print the robot status
  if (verbose_) {
    std::cout << "Robot status: ";
    switch (EStopStatus) {
    case ESTOP_AUTO:
      m_controlMode = AUTO;
      if (HIPowerStatus == 0)
        std::cout << "Power is OFF" << std::endl;
      else
        std::cout << "Power is ON" << std::endl;
      break;

    case ESTOP_MANUAL:
      m_controlMode = MANUAL;
      if (HIPowerStatus == 0)
        std::cout << "Power is OFF" << std::endl;
      else
        std::cout << "Power is ON" << std::endl;
      break;
    case ESTOP_ACTIVATED:
      m_controlMode = ESTOP;
      std::cout << "Emergency stop is activated" << std::endl;
      break;
    default:
      std::cout << "Sorry there is an error on the emergency chain." << std::endl;
      std::cout << "You have to call Adept for maintenance..." << std::endl;
      // Free allocated resources
    }
    std::cout << std::endl;
  }
  // get real joint min/max from the MotionBlox
  Try(PrimitiveJOINT_MINMAX_Viper650(joint_min.data, joint_max.data));
  // Convert units from degrees to radians
  joint_min.deg2rad();
  joint_max.deg2rad();

  //   for (unsigned int i=0; i < njoint; i++) {
  //     printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
  //     joint_max[i]);
  //   }

  // If an error occur in the low level controller, goto here
  // CatchPrint();
  Catch();

  // Test if an error occurs
  if (TryStt == -20001)
    printf("No connection detected. Check if the robot is powered on \n"
           "and if the firewire link exist between the MotionBlox and this "
           "computer.\n");
  else if (TryStt == -675)
    printf(" Timeout enabling power...\n");

  if (TryStt < 0) {
    // Power off the robot
    PrimitivePOWEROFF_Viper650();
    // Free allocated resources
    ShutDownConnection();

    std::cout << "Cannot open connection with the motionblox..." << std::endl;
    throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with the motionblox");
  }
  return;
}

void vpRobotViper650::init(vpViper650::vpToolType tool, vpCameraParameters::vpCameraParametersProjType projModel)
{
  // Read the robot constants from files
  // - joint [min,max], coupl_56, long_56
  // - camera extrinsic parameters relative to eMc
  vpViper650::init(tool, projModel);

  InitTry;

  // Get real joint min/max from the MotionBlox
  Try(PrimitiveJOINT_MINMAX_Viper650(joint_min.data, joint_max.data));
  // Convert units from degrees to radians
  joint_min.deg2rad();
  joint_max.deg2rad();

  //   for (unsigned int i=0; i < njoint; i++) {
  //     printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
  //     joint_max[i]);
  //   }

  // Set the camera constant (eMc pose) in the MotionBlox
  double eMc_pose[6];
  for (unsigned int i = 0; i < 3; i++) {
    eMc_pose[i] = etc[i];     // translation in meters
    eMc_pose[i + 3] = erc[i]; // rotation in rad
  }
  // Update the eMc pose in the low level controller
  Try(PrimitiveCONST_Viper650(eMc_pose));

  CatchPrint();
  return;
}

void vpRobotViper650::init(vpViper650::vpToolType tool, const std::string &filename)
{
  vpViper650::init(tool, filename);

  InitTry;

  // Get real joint min/max from the MotionBlox
  Try(PrimitiveJOINT_MINMAX_Viper650(joint_min.data, joint_max.data));
  // Convert units from degrees to radians
  joint_min.deg2rad();
  joint_max.deg2rad();

  //   for (unsigned int i=0; i < njoint; i++) {
  //     printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
  //     joint_max[i]);
  //   }

  // Set the camera constant (eMc pose) in the MotionBlox
  double eMc_pose[6];
  for (unsigned int i = 0; i < 3; i++) {
    eMc_pose[i] = etc[i];     // translation in meters
    eMc_pose[i + 3] = erc[i]; // rotation in rad
  }
  // Update the eMc pose in the low level controller
  Try(PrimitiveCONST_Viper650(eMc_pose));

  CatchPrint();
  return;
}

void vpRobotViper650::init(vpViper650::vpToolType tool, const vpHomogeneousMatrix &eMc_)
{
  vpViper650::init(tool, eMc_);

  InitTry;

  // Get real joint min/max from the MotionBlox
  Try(PrimitiveJOINT_MINMAX_Viper650(joint_min.data, joint_max.data));
  // Convert units from degrees to radians
  joint_min.deg2rad();
  joint_max.deg2rad();

  //   for (unsigned int i=0; i < njoint; i++) {
  //     printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
  //     joint_max[i]);
  //   }

  // Set the camera constant (eMc pose) in the MotionBlox
  double eMc_pose[6];
  for (unsigned int i = 0; i < 3; i++) {
    eMc_pose[i] = etc[i];     // translation in meters
    eMc_pose[i + 3] = erc[i]; // rotation in rad
  }
  // Update the eMc pose in the low level controller
  Try(PrimitiveCONST_Viper650(eMc_pose));

  CatchPrint();
  return;
}

void vpRobotViper650::set_eMc(const vpHomogeneousMatrix &eMc_)
{
  this->vpViper650::set_eMc(eMc_);

  InitTry;

  // Set the camera constant (eMc pose) in the MotionBlox
  double eMc_pose[6];
  for (unsigned int i = 0; i < 3; i++) {
    eMc_pose[i] = etc[i];     // translation in meters
    eMc_pose[i + 3] = erc[i]; // rotation in rad
  }
  // Update the eMc pose in the low level controller
  Try(PrimitiveCONST_Viper650(eMc_pose));

  CatchPrint();

  return;
}

void vpRobotViper650::set_eMc(const vpTranslationVector &etc_, const vpRxyzVector &erc_)
{
  this->vpViper650::set_eMc(etc_, erc_);

  InitTry;

  // Set the camera constant (eMc pose) in the MotionBlox
  double eMc_pose[6];
  for (unsigned int i = 0; i < 3; i++) {
    eMc_pose[i] = etc[i];     // translation in meters
    eMc_pose[i + 3] = erc[i]; // rotation in rad
  }
  // Update the eMc pose in the low level controller
  Try(PrimitiveCONST_Viper650(eMc_pose));

  CatchPrint();

  return;
}

/* ------------------------------------------------------------------------ */
/* --- DESTRUCTOR --------------------------------------------------------- */
/* ------------------------------------------------------------------------ */

vpRobotViper650::~vpRobotViper650(void)
{
  InitTry;

  setRobotState(vpRobot::STATE_STOP);

  // Look if the power is on or off
  UInt32 HIPowerStatus;
  Try(PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
  CAL_Wait(0.1);

  //   if (HIPowerStatus == 1) {
  //     fprintf(stdout, "Power OFF the robot\n");
  //     fflush(stdout);

  //     Try( PrimitivePOWEROFF_Viper650() );
  //   }

  // Free allocated resources
  ShutDownConnection();

  vpRobotViper650::m_robotAlreadyCreated = false;

  CatchPrint();
  return;
}

vpRobot::vpRobotStateType vpRobotViper650::setRobotState(vpRobot::vpRobotStateType newState)
{
  InitTry;

  switch (newState) {
  case vpRobot::STATE_STOP: {
    // Start primitive STOP only if the current state is Velocity
    if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState()) {
      Try(PrimitiveSTOP_Viper650());
      vpTime::sleepMs(100); // needed to ensure velocity task ends up on low level
    }
    break;
  }
  case vpRobot::STATE_POSITION_CONTROL: {
    if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState()) {
      std::cout << "Change the control mode from velocity to position control.\n";
      Try(PrimitiveSTOP_Viper650());
    } else {
      // std::cout << "Change the control mode from stop to position
      // control.\n";
    }
    this->powerOn();
    break;
  }
  case vpRobot::STATE_VELOCITY_CONTROL: {
    if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState()) {
      std::cout << "Change the control mode from stop to velocity control.\n";
    }
    this->powerOn();
    break;
  }
  default:
    break;
  }

  CatchPrint();

  return vpRobot::setRobotState(newState);
}

/* ------------------------------------------------------------------------ */
/* --- STOP --------------------------------------------------------------- */
/* ------------------------------------------------------------------------ */

void vpRobotViper650::stopMotion(void)
{
  if (getRobotState() != vpRobot::STATE_VELOCITY_CONTROL)
    return;

  InitTry;
  Try(PrimitiveSTOP_Viper650());
  setRobotState(vpRobot::STATE_STOP);

  CatchPrint();
  if (TryStt < 0) {
    vpERROR_TRACE("Cannot stop robot motion");
    throw vpRobotException(vpRobotException::lowLevelError, "Cannot stop robot motion.");
  }
}

void vpRobotViper650::powerOn(void)
{
  InitTry;

  // Look if the power is on or off
  UInt32 HIPowerStatus;
  UInt32 EStopStatus;
  bool firsttime = true;
  unsigned int nitermax = 10;

  for (unsigned int i = 0; i < nitermax; i++) {
    Try(PrimitiveSTATUS_Viper650(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
    if (EStopStatus == ESTOP_AUTO) {
      m_controlMode = AUTO;
      break; // exit for loop
    } else if (EStopStatus == ESTOP_MANUAL) {
      m_controlMode = MANUAL;
      break; // exit for loop
    } else if (EStopStatus == ESTOP_ACTIVATED) {
      m_controlMode = ESTOP;
      if (firsttime) {
        std::cout << "Emergency stop is activated! \n"
                  << "Check the emergency stop button and push the yellow "
                     "button before continuing."
                  << std::endl;
        firsttime = false;
      }
      fprintf(stdout, "Remaining time %us  \r", nitermax - i);
      fflush(stdout);
      CAL_Wait(1);
    } else {
      std::cout << "Sorry there is an error on the emergency chain." << std::endl;
      std::cout << "You have to call Adept for maintenance..." << std::endl;
      // Free allocated resources
      ShutDownConnection();
      throw(vpRobotException(vpRobotException::lowLevelError, "Error on the emergency chain"));
    }
  }

  if (EStopStatus == ESTOP_ACTIVATED)
    std::cout << std::endl;

  if (EStopStatus == ESTOP_ACTIVATED) {
    std::cout << "Sorry, cannot power on the robot." << std::endl;
    throw(vpRobotException(vpRobotException::lowLevelError, "Cannot power on the robot."));
  }

  if (HIPowerStatus == 0) {
    fprintf(stdout, "Power ON the Viper650 robot\n");
    fflush(stdout);

    Try(PrimitivePOWERON_Viper650());
  }

  CatchPrint();
  if (TryStt < 0) {
    vpERROR_TRACE("Cannot power on the robot");
    throw(vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot."));
  }
}

void vpRobotViper650::powerOff(void)
{
  InitTry;

  // Look if the power is on or off
  UInt32 HIPowerStatus;
  Try(PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
  CAL_Wait(0.1);

  if (HIPowerStatus == 1) {
    fprintf(stdout, "Power OFF the Viper650 robot\n");
    fflush(stdout);

    Try(PrimitivePOWEROFF_Viper650());
  }

  CatchPrint();
  if (TryStt < 0) {
    vpERROR_TRACE("Cannot power off the robot");
    throw vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot.");
  }
}

bool vpRobotViper650::getPowerState(void) const
{
  InitTry;
  bool status = false;
  // Look if the power is on or off
  UInt32 HIPowerStatus;
  Try(PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
  CAL_Wait(0.1);

  if (HIPowerStatus == 1) {
    status = true;
  }

  CatchPrint();
  if (TryStt < 0) {
    vpERROR_TRACE("Cannot get the power status");
    throw vpRobotException(vpRobotException::lowLevelError, "Cannot get the power status.");
  }
  return status;
}

void vpRobotViper650::get_cVe(vpVelocityTwistMatrix &cVe) const
{
  vpHomogeneousMatrix cMe;
  vpViper650::get_cMe(cMe);

  cVe.buildFrom(cMe);
}

void vpRobotViper650::get_cMe(vpHomogeneousMatrix &cMe) const { vpViper650::get_cMe(cMe); }

void vpRobotViper650::get_eJe(vpMatrix &eJe)
{

  double position[6];
  double timestamp;

  InitTry;
  Try(PrimitiveACQ_POS_J_Viper650(position, &timestamp));
  CatchPrint();

  vpColVector q(6);
  for (unsigned int i = 0; i < njoint; i++)
    q[i] = vpMath::rad(position[i]);

  try {
    vpViper650::get_eJe(q, eJe);
  } catch (...) {
    vpERROR_TRACE("catch exception ");
    throw;
  }
}
void vpRobotViper650::get_fJe(vpMatrix &fJe)
{

  double position[6];
  double timestamp;

  InitTry;
  Try(PrimitiveACQ_POS_Viper650(position, &timestamp));
  CatchPrint();

  vpColVector q(6);
  for (unsigned int i = 0; i < njoint; i++)
    q[i] = position[i];

  try {
    vpViper650::get_fJe(q, fJe);
  } catch (...) {
    vpERROR_TRACE("Error caught");
    throw;
  }
}

void vpRobotViper650::setPositioningVelocity(double velocity) { m_positioningVelocity = velocity; }

double vpRobotViper650::getPositioningVelocity(void) const { return m_positioningVelocity; }

void vpRobotViper650::setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position)
{

  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 destination(njoint);
  int error = 0;
  double timestamp;

  InitTry;
  switch (frame) {
  case vpRobot::CAMERA_FRAME: {
    vpColVector q(njoint);
    Try(PrimitiveACQ_POS_Viper650(q.data, &timestamp));

    // Convert degrees into rad
    q.deg2rad();

    // Get fMc from the inverse kinematics
    vpHomogeneousMatrix fMc;
    vpViper650::get_fMc(q, fMc);

    // Set cMc from the input position
    vpTranslationVector txyz;
    vpRxyzVector rxyz;
    for (unsigned int i = 0; i < 3; i++) {
      txyz[i] = position[i];
      rxyz[i] = position[i + 3];
    }

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

    // Compute the new position to reach: fMc*cMc2
    vpHomogeneousMatrix fMc2 = fMc * cMc2;

    // Compute the corresponding joint position from the inverse kinematics
    unsigned int solution = this->getInverseKinematics(fMc2, q);
    if (solution) { // Position is reachable
      destination = q;
      // convert rad to deg requested for the low level controller
      destination.rad2deg();
      Try(PrimitiveMOVE_J_Viper650(destination.data, m_positioningVelocity));
      Try(WaitState_Viper650(ETAT_ATTENTE_VIPER650, 1000));
    } else {
      // Cartesian position is out of range
      error = -1;
    }

    break;
  }
  case vpRobot::ARTICULAR_FRAME: {
    destination = position;
    // convert rad to deg requested for the low level controller
    destination.rad2deg();

    // std::cout << "Joint destination (deg): " << destination.t() <<
    // std::endl;
    Try(PrimitiveMOVE_J_Viper650(destination.data, m_positioningVelocity));
    Try(WaitState_Viper650(ETAT_ATTENTE_VIPER650, 1000));
    break;
  }
  case vpRobot::REFERENCE_FRAME: {
    // Convert angles from Rxyz representation to Rzyz representation
    vpRxyzVector rxyz(position[3], position[4], position[5]);
    vpRotationMatrix R(rxyz);
    vpRzyzVector rzyz(R);

    for (unsigned int i = 0; i < 3; i++) {
      destination[i] = position[i];
      destination[i + 3] = vpMath::deg(rzyz[i]); // convert also angles in deg
    }
    int configuration = 0; // keep the actual configuration

    // std::cout << "Base frame destination Rzyz (deg): " << destination.t()
    // << std::endl;
    Try(PrimitiveMOVE_C_Viper650(destination.data, configuration, m_positioningVelocity));
    Try(WaitState_Viper650(ETAT_ATTENTE_VIPER650, 1000));

    break;
  }
  case vpRobot::MIXT_FRAME: {
    throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
                                                            "Mixt frame not implemented.");
  }
  case vpRobot::END_EFFECTOR_FRAME: {
    throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
                                                            "Mixt frame not implemented.");
  }
  }

  CatchPrint();
  if (TryStt == InvalidPosition || TryStt == -1023)
    std::cout << " : Position out of range.\n";

  else if (TryStt == -3019) {
    if (frame == vpRobot::ARTICULAR_FRAME)
      std::cout << " : Joint position out of range.\n";
    else
      std::cout << " : Cartesian position leads to a joint position out of "
                   "range.\n";
  } else if (TryStt < 0)
    std::cout << " : Unknown error (see Fabien).\n";
  else if (error == -1)
    std::cout << "Position out of range.\n";

  if (TryStt < 0 || error < 0) {
    vpERROR_TRACE("Positionning error.");
    throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range.");
  }

  return;
}

void vpRobotViper650::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 vpRobotViper650::setPosition(const std::string &filename)
{
  vpColVector q;
  bool ret;

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

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

void vpRobotViper650::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position, double &timestamp)
{

  InitTry;

  position.resize(6);

  switch (frame) {
  case vpRobot::CAMERA_FRAME: {
    position = 0;
    return;
  }
  case vpRobot::ARTICULAR_FRAME: {
    Try(PrimitiveACQ_POS_J_Viper650(position.data, &timestamp));
    // vpCTRACE << "Get joint position (deg)" << position.t() << std::endl;
    position.deg2rad();

    return;
  }
  case vpRobot::REFERENCE_FRAME: {
    vpColVector q(njoint);
    Try(PrimitiveACQ_POS_J_Viper650(q.data, &timestamp));

    // Compute fMc
    vpHomogeneousMatrix fMc = vpViper650::get_fMc(q);

    // From fMc extract the pose
    vpRotationMatrix fRc;
    fMc.extract(fRc);
    vpRxyzVector rxyz;
    rxyz.buildFrom(fRc);

    for (unsigned int i = 0; i < 3; i++) {
      position[i] = fMc[i][3];   // translation x,y,z
      position[i + 3] = rxyz[i]; // Euler rotation x,y,z
    }

    break;
  }
  case vpRobot::MIXT_FRAME: {
    throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in mixt frame: "
                                                            "not implemented");
  }
  case vpRobot::END_EFFECTOR_FRAME: {
    throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in end-effector frame: "
                                                            "not implemented");
  }
  }

  CatchPrint();
  if (TryStt < 0) {
    vpERROR_TRACE("Cannot get position.");
    throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position.");
  }

  return;
}

void vpRobotViper650::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
{
  double timestamp;
  getPosition(frame, position, timestamp);
}

void vpRobotViper650::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double &timestamp)
{
  vpColVector posRxyz;
  // recupere  position en Rxyz
  this->getPosition(frame, posRxyz, timestamp);
  vpRxyzVector RxyzVect;
  for (unsigned int j = 0; j < 3; j++)
    RxyzVect[j] = posRxyz[j + 3];
  // recupere le vecteur thetaU correspondant
  vpThetaUVector RtuVect(RxyzVect);

  // 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 vpRobotViper650::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position)
{
  double timestamp;
  getPosition(frame, position, timestamp);
}

double vpRobotViper650::getTime() const
{
  double timestamp;
  PrimitiveACQ_TIME_Viper650(&timestamp);
  return timestamp;
}

void vpRobotViper650::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);

  // Velocity saturation
  switch (frame) {
  // saturation in cartesian space
  case vpRobot::CAMERA_FRAME:
  case vpRobot::REFERENCE_FRAME:
  case vpRobot::END_EFFECTOR_FRAME:
  case vpRobot::MIXT_FRAME: {
    vpColVector vel_max(6);

    for (unsigned int i = 0; i < 3; i++)
      vel_max[i] = getMaxTranslationVelocity();
    for (unsigned int i = 3; i < 6; i++)
      vel_max[i] = getMaxRotationVelocity();

    vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);

    break;
  }
  // saturation in joint space
  case vpRobot::ARTICULAR_FRAME: {
    vpColVector vel_max(6);

    // if (getMaxRotationVelocity() == getMaxRotationVelocityJoint6()) {
    if (std::fabs(getMaxRotationVelocity() - getMaxRotationVelocityJoint6()) < std::numeric_limits<double>::epsilon()) {
      for (unsigned int i = 0; i < 6; i++)
        vel_max[i] = getMaxRotationVelocity();
    } else {
      for (unsigned int i = 0; i < 5; i++)
        vel_max[i] = getMaxRotationVelocity();
      vel_max[5] = getMaxRotationVelocityJoint6();
    }

    vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
  }
  }

  InitTry;

  switch (frame) {
  case vpRobot::CAMERA_FRAME: {
    // Send velocities in m/s and rad/s
    // std::cout << "Vitesse cam appliquee: " << vel_sat.t();
    Try(PrimitiveMOVESPEED_CART_Viper650(vel_sat.data, REPCAM_VIPER650));
    break;
  }
  case vpRobot::END_EFFECTOR_FRAME: {
    // Transform in camera frame
    vpHomogeneousMatrix cMe;
    this->get_cMe(cMe);
    vpVelocityTwistMatrix cVe(cMe);
    vpColVector v_c = cVe * vel_sat;
    // Send velocities in m/s and rad/s
    Try(PrimitiveMOVESPEED_CART_Viper650(v_c.data, REPCAM_VIPER650));
    break;
  }
  case vpRobot::ARTICULAR_FRAME: {
    // Convert all the velocities from rad/s into deg/s
    vel_sat.rad2deg();
    // std::cout << "Vitesse appliquee: " << vel_sat.t();
    // Try( PrimitiveMOVESPEED_CART(vel_sat.data, REPART_VIPER650) );
    Try(PrimitiveMOVESPEED_Viper650(vel_sat.data));
    break;
  }
  case vpRobot::REFERENCE_FRAME: {
    // Send velocities in m/s and rad/s
    //std::cout << "Vitesse ref appliquee: " << vel_sat.t();
    Try(PrimitiveMOVESPEED_CART_Viper650(vel_sat.data, REPFIX_VIPER650));
    break;
  }
  case vpRobot::MIXT_FRAME: {
    // Try( PrimitiveMOVESPEED_CART_Viper650(vel_sat.data, REPMIX_VIPER650) );
    break;
  }
  default: {
    vpERROR_TRACE("Error in spec of vpRobot. "
                  "Case not taken in account.");
    return;
  }
  }

  Catch();
  if (TryStt < 0) {
    if (TryStt == VelStopOnJoint) {
      UInt32 axisInJoint[njoint];
      PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
      for (unsigned int i = 0; i < njoint; i++) {
        if (axisInJoint[i])
          std::cout << "\nWarning: Velocity control stopped: axis " << i + 1 << " on joint limit!" << std::endl;
      }
    } else {
      printf("\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
      if (TryString != NULL) {
        // The statement is in TryString, but we need to check the validity
        printf(" Error sentence %s\n", TryString); // Print the TryString
      } else {
        printf("\n");
      }
    }
  }

  return;
}

/* ------------------------------------------------------------------------ */
/* --- GET ---------------------------------------------------------------- */
/* ------------------------------------------------------------------------ */

void vpRobotViper650::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity, double &timestamp)
{
  velocity.resize(6);
  velocity = 0;

  vpColVector q_cur(6);
  vpHomogeneousMatrix fMe_cur, fMc_cur;
  vpHomogeneousMatrix eMe, cMc; // camera displacement
  double time_cur;

  InitTry;

  // Get the current joint position
  Try(PrimitiveACQ_POS_J_Viper650(q_cur.data, &timestamp));
  time_cur = timestamp;
  q_cur.deg2rad();

  // Get the end-effector pose from the direct kinematics
  vpViper650::get_fMe(q_cur, fMe_cur);
  // Get the camera pose from the direct kinematics
  vpViper650::get_fMc(q_cur, fMc_cur);

  if (!m_first_time_getvel) {

    switch (frame) {
    case vpRobot::END_EFFECTOR_FRAME: {
      // Compute the displacement of the end-effector since the previous call
      eMe = m_fMe_prev_getvel.inverse() * fMe_cur;

      // Compute the velocity of the end-effector from this displacement
      velocity = vpExponentialMap::inverse(eMe, time_cur - m_time_prev_getvel);

      break;
    }

    case vpRobot::CAMERA_FRAME: {
      // Compute the displacement of the camera since the previous call
      cMc = m_fMc_prev_getvel.inverse() * fMc_cur;

      // Compute the velocity of the camera from this displacement
      velocity = vpExponentialMap::inverse(cMc, time_cur - m_time_prev_getvel);

      break;
    }

    case vpRobot::ARTICULAR_FRAME: {
      velocity = (q_cur - m_q_prev_getvel) / (time_cur - m_time_prev_getvel);
      break;
    }

    case vpRobot::REFERENCE_FRAME: {
      // Compute the displacement of the camera since the previous call
      cMc = m_fMc_prev_getvel.inverse() * fMc_cur;

      // Compute the velocity of the camera from this displacement
      vpColVector v;
      v = vpExponentialMap::inverse(cMc, time_cur - m_time_prev_getvel);

      // Express this velocity in the reference frame
      vpVelocityTwistMatrix fVc(fMc_cur);
      velocity = fVc * v;

      break;
    }

    case vpRobot::MIXT_FRAME: {
      // Compute the displacement of the camera since the previous call
      cMc = m_fMc_prev_getvel.inverse() * fMc_cur;

      // Compute the ThetaU representation for the rotation
      vpRotationMatrix cRc;
      cMc.extract(cRc);
      vpThetaUVector thetaU;
      thetaU.buildFrom(cRc);

      for (unsigned int i = 0; i < 3; i++) {
        // Compute the translation displacement in the reference frame
        velocity[i] = m_fMc_prev_getvel[i][3] - fMc_cur[i][3];
        // Update the rotation displacement in the camera frame
        velocity[i + 3] = thetaU[i];
      }

      // Compute the velocity
      velocity /= (time_cur - m_time_prev_getvel);
      break;
    }
    default: {
      throw(vpException(vpException::functionNotImplementedError,
                        "vpRobotViper650::getVelocity() not implemented in end-effector"));
    }
    }
  } else {
    m_first_time_getvel = false;
  }

  // Memorize the end-effector pose for the next call
  m_fMe_prev_getvel = fMe_cur;
  // Memorize the camera pose for the next call
  m_fMc_prev_getvel = fMc_cur;

  // Memorize the joint position for the next call
  m_q_prev_getvel = q_cur;

  // Memorize the time associated to the joint position for the next call
  m_time_prev_getvel = time_cur;

  CatchPrint();
  if (TryStt < 0) {
    vpERROR_TRACE("Cannot get velocity.");
    throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
  }
}

void vpRobotViper650::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
{
  double timestamp;
  getVelocity(frame, velocity, timestamp);
}

vpColVector vpRobotViper650::getVelocity(vpRobot::vpControlFrameType frame, double &timestamp)
{
  vpColVector velocity;
  getVelocity(frame, velocity, timestamp);

  return velocity;
}

vpColVector vpRobotViper650::getVelocity(vpRobot::vpControlFrameType frame)
{
  vpColVector velocity;
  double timestamp;
  getVelocity(frame, velocity, timestamp);

  return velocity;
}

bool vpRobotViper650::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("#Viper650 - 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 Viper650 position file
        std::cout << "Error: this position file " << filename << " is not for Viper650 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 Viper650 robot in " << filename << std::endl;
    return false;
  }

  return true;
}

bool vpRobotViper650::savePosFile(const std::string &filename, const vpColVector &q)
{

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

  fprintf(fd, "\
#Viper650 - Position - Version 1.00\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 vpRobotViper650::move(const std::string &filename)
{
  vpColVector q;

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

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

  double q[6];
  vpColVector q_cur(6);
  double timestamp;

  InitTry;

  // Get the current joint position
  Try(PrimitiveACQ_POS_Viper650(q, &timestamp));
  for (unsigned int i = 0; i < njoint; i++) {
    q_cur[i] = q[i];
  }

  if (!m_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 - m_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 {
    m_first_time_getdis = false;
  }

  // Memorize the joint position for the next call
  m_q_prev_getdis = q_cur;

  CatchPrint();
  if (TryStt < 0) {
    vpERROR_TRACE("Cannot get velocity.");
    throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
  }
}

void vpRobotViper650::biasForceTorqueSensor() const
{
  InitTry;

  Try(PrimitiveTFS_BIAS_Viper650());

  // Wait 500 ms to be sure the next measures take into account the bias
  vpTime::wait(500);

  CatchPrint();
  if (TryStt < 0) {
    vpERROR_TRACE("Cannot bias the force/torque sensor.");
    throw vpRobotException(vpRobotException::lowLevelError, "Cannot bias the force/torque sensor.");
  }
}

void vpRobotViper650::getForceTorque(vpColVector &H) const
{
  InitTry;

  H.resize(6);

  Try(PrimitiveTFS_ACQ_Viper650(H.data));

  CatchPrint();
  if (TryStt < 0) {
    vpERROR_TRACE("Cannot get the force/torque measures.");
    throw vpRobotException(vpRobotException::lowLevelError, "Cannot get force/torque measures.");
  }
}

vpColVector vpRobotViper650::getForceTorque() const
{
  InitTry;

  vpColVector H(6);

  Try(PrimitiveTFS_ACQ_Viper650(H.data));

  return H;

  CatchPrint();
  if (TryStt < 0) {
    vpERROR_TRACE("Cannot get the force/torque measures.");
    throw vpRobotException(vpRobotException::lowLevelError, "Cannot get force/torque measures.");
  }
  return H; // Here to avoid a warning, but should never be called
}

void vpRobotViper650::enableJoint6Limits() const
{
  InitTry;
  Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper650(0));
  std::cout << "Enable joint limits on axis 6..." << std::endl;
  CatchPrint();
  if (TryStt < 0) {
    vpERROR_TRACE("Cannot enable joint limits on axis 6");
    throw vpRobotException(vpRobotException::lowLevelError, "Cannot enable joint limits on axis 6.");
  }
}

void vpRobotViper650::disableJoint6Limits() const
{
  InitTry;
  Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper650(1));
  std::cout << "Warning: Disable joint limits on axis 6..." << std::endl;
  CatchPrint();
  if (TryStt < 0) {
    vpERROR_TRACE("Cannot disable joint limits on axis 6");
    throw vpRobotException(vpRobotException::lowLevelError, "Cannot disable joint limits on axis 6.");
  }
}

void vpRobotViper650::setMaxRotationVelocity(double w_max)
{
  vpRobot::setMaxRotationVelocity(w_max);
  setMaxRotationVelocityJoint6(w_max);

  return;
}

void vpRobotViper650::setMaxRotationVelocityJoint6(double w6_max)
{
  m_maxRotationVelocity_joint6 = w6_max;
  return;
}

double vpRobotViper650::getMaxRotationVelocityJoint6() const { return m_maxRotationVelocity_joint6; }

void vpRobotViper650::openGripper()
{
  InitTry;
  Try(PrimitivePneumaticGripper_Viper650(1));
  std::cout << "Open the pneumatic gripper..." << std::endl;
  CatchPrint();
  if (TryStt < 0) {
    throw vpRobotException(vpRobotException::lowLevelError, "Cannot open the gripper.");
  }
}

void vpRobotViper650::closeGripper() const
{
  InitTry;
  Try(PrimitivePneumaticGripper_Viper650(0));
  std::cout << "Close the pneumatic gripper..." << std::endl;
  CatchPrint();
  if (TryStt < 0) {
    throw vpRobotException(vpRobotException::lowLevelError, "Cannot close the gripper.");
  }
}

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