vpRobotViper850.cpp

/****************************************************************************
 *
 * $Id: vpRobotViper850.cpp 3697 2012-05-03 08:41:33Z fspindle $
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2012 by INRIA. All rights reserved.
 * 
 * This software is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * ("GPL") version 2 as published by the Free Software Foundation.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact INRIA about acquiring a ViSP Professional 
 * Edition License.
 *
 * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
 * 
 * This software was developed at:
 * INRIA Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 * http://www.irisa.fr/lagadic
 *
 * If you have questions regarding the use of this file, please contact
 * INRIA at visp@inria.fr
 * 
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 *
 * Description:
 * Interface for the Irisa's Viper S850 robot.
 *
 * Authors:
 * Fabien Spindler
 *
 *****************************************************************************/

#include <visp/vpConfig.h>

#ifdef VISP_HAVE_VIPER850

#include <signal.h>
#include <stdlib.h>

#include <visp/vpRobotException.h>
#include <visp/vpExponentialMap.h>
#include <visp/vpDebug.h>
#include <visp/vpVelocityTwistMatrix.h>
#include <visp/vpThetaUVector.h>
#include <visp/vpRobot.h>
#include <visp/vpRobotViper850.h>

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

bool vpRobotViper850::robotAlreadyCreated = false;

const double vpRobotViper850::defaultPositioningVelocity = 15.0;

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

void emergencyStopViper850(int signo)
{
  std::cout << "Stop the Viper850 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_Viper850();
  PrimitiveSTOP_Viper850();
  std::cout << "Robot was stopped\n";

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

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

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

vpRobotViper850::vpRobotViper850 (void)
  :
  vpViper850 (),
  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, emergencyStopViper850);
  signal(SIGBUS, emergencyStopViper850) ;
  signal(SIGSEGV, emergencyStopViper850) ;
  signal(SIGKILL, emergencyStopViper850);
  signal(SIGQUIT, emergencyStopViper850);

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

  vpRobotViper850::robotAlreadyCreated = true;

  return ;
}


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

void
vpRobotViper850::init (void)
{
  InitTry;

  // Initialise private variables used to compute the measured velocities
  q_prev_getvel.resize(6);
  q_prev_getvel = 0;
  time_prev_getvel = 0;
  first_time_getvel = true;

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


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

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

  Try( PrimitiveRESET_Viper850() );

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

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

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

  case ESTOP_MANUAL: 
    controlMode = MANUAL;
    if (HIPowerStatus == 0)
      std::cout << "Power is OFF" << std::endl;
    else
      std::cout << "Power is ON" << std::endl;
    break;
  case ESTOP_ACTIVATED: 
    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 ressources
  }
  std::cout << std::endl;

  // get real joint min/max from the MotionBlox
  Try( PrimitiveJOINT_MINMAX_Viper850(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_Viper850();
    // Free allocated ressources
    ShutDownConnection();

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

void
vpRobotViper850::init (vpViper850::vpToolType tool,
               vpCameraParameters::vpCameraParametersProjType projModel)
{

  InitTry;
  // Read the robot constants from files
  // - joint [min,max], coupl_56, long_56
  // - camera extrinsic parameters relative to eMc
  vpViper850::init(tool, projModel);

 // 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_Viper850(eMc_pose) );

  // get real joint min/max from the MotionBlox
  Try( PrimitiveJOINT_MINMAX_Viper850(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]);
//   }

  setToolType(tool);

  CatchPrint();
  return ;
}

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

vpRobotViper850::~vpRobotViper850 (void)
{
  InitTry;

  setRobotState(vpRobot::STATE_STOP) ;

  // Look if the power is on or off
  UInt32 HIPowerStatus;
  Try( PrimitiveSTATUS_Viper850(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_Viper850() );
//   }

  // Free allocated ressources
  ShutDownConnection();

  vpRobotViper850::robotAlreadyCreated = false;

  CatchPrint();
  return;
}




vpRobot::vpRobotStateType
vpRobotViper850::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_Viper850() );
    }
    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_Viper850() );
    }
    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
vpRobotViper850::stopMotion(void)
{
  if (getRobotState() != vpRobot::STATE_VELOCITY_CONTROL)
    return;

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

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

void
vpRobotViper850::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_Viper850(NULL, NULL, &EStopStatus, NULL, NULL, NULL, 
                  &HIPowerStatus));
    switch(EStopStatus) {
    case ESTOP_AUTO: controlMode = AUTO; break;
    case ESTOP_MANUAL: controlMode = MANUAL; break;
    case ESTOP_ACTIVATED:
      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 %ds  \r", nitermax-i);
      fflush(stdout);
      CAL_Wait(1);
      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 ressources
      ShutDownConnection();
      exit(0);
    }
  }

  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 Viper850 robot\n");
    fflush(stdout);
    
    Try( PrimitivePOWERON_Viper850() );
  }

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

void
vpRobotViper850::powerOff(void)
{
  InitTry;

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

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

    Try( PrimitivePOWEROFF_Viper850() );
  }

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

bool
vpRobotViper850::getPowerState(void)
{
  InitTry;
  bool status = false;
  // Look if the power is on or off
  UInt32 HIPowerStatus;
  Try( PrimitiveSTATUS_Viper850(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
vpRobotViper850::get_cVe(vpVelocityTwistMatrix &cVe)
{
  vpHomogeneousMatrix cMe ;
  vpViper850::get_cMe(cMe) ;

  cVe.buildFrom(cMe) ;
}

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


void
vpRobotViper850::get_eJe(vpMatrix &eJe)
{

  double position[6];

  InitTry;
  Try( PrimitiveACQ_POS_J_Viper850(position) );
  CatchPrint();

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

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

  double position[6];

  InitTry;
  Try( PrimitiveACQ_POS_Viper850(position) );
  CatchPrint();

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

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

void
vpRobotViper850::setPositioningVelocity (const double velocity)
{
  positioningVelocity = velocity;
}

double
vpRobotViper850::getPositioningVelocity (void)
{
  return positioningVelocity;
}


void
vpRobotViper850::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;

  InitTry;
  switch(frame) {
  case vpRobot::CAMERA_FRAME : {
    vpColVector q(njoint);
    Try( PrimitiveACQ_POS_Viper850(q.data) );

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

    // Get fMc from the inverse kinematics
    vpHomogeneousMatrix fMc;
    vpViper850::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_Viper850(destination.data, positioningVelocity) );
      Try( WaitState_Viper850(ETAT_ATTENTE_AFMA6, 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_Viper850(destination.data, positioningVelocity) );
    Try( WaitState_Viper850(ETAT_ATTENTE_AFMA6, 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_Viper850(destination.data, configuration,
                  positioningVelocity) );
    Try( WaitState_Viper850(ETAT_ATTENTE_AFMA6, 1000) );

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

  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 vpRobotViper850::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 vpRobotViper850::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
vpRobotViper850::getPosition (const vpRobot::vpControlFrameType frame,
                  vpColVector & position)
{

  InitTry;

  position.resize (6);

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

    return;
  }
  case vpRobot::REFERENCE_FRAME : {
    Try( PrimitiveACQ_POS_C_Viper850(position.data) );
    //    vpCTRACE << "Get cartesian position " << position.t() << std::endl;
    // 1=tx, 2=ty, 3=tz in meters; 4=Rz 5=Ry 6=Rz in deg
    // Convert Euler Rzyz angles from deg to rad
    for (unsigned int i=3; i <6; i++)
      position[i] = vpMath::rad(position[i]);
    // Convert Rzyz angles into Rxyz representation
    vpRzyzVector rzyz(position[3], position[4], position[5]);
    vpRotationMatrix R(rzyz);
    vpRxyzVector rxyz(R);

    // Update the position using Rxyz representation
    for (unsigned int i=0; i <3; i++)
      position[i+3] = rxyz[i];
//     vpCTRACE << "Cartesian position Rxyz (deg)" 
//       << position[0] << " " << position[1] << " " << position[2] << " "  
//       << vpMath::deg(position[3]) << " "
//       << vpMath::deg(position[4]) << " "
//       << vpMath::deg(position[5]) << std::endl;

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

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

  return;
}
void 
vpRobotViper850::getPosition (const vpRobot::vpControlFrameType frame,   
                            vpPoseVector &position)
{
  vpColVector posRxyz;
  //recupere  position en Rxyz
  this->getPosition(frame,posRxyz);
  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
vpRobotViper850::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::MIXT_FRAME : {
        vpColVector vel_max(6);

        for (int i=0; i<3; i++)
          vel_max[i] = getMaxTranslationVelocity();
        for (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);

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

        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_Viper850(vel_sat.data, REPCAM) );
        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) );
        Try( PrimitiveMOVESPEED_Viper850(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_Viper850(vel_sat.data, REPFIX) );
        break ;
      }
    case vpRobot::MIXT_FRAME : {
        //Try( PrimitiveMOVESPEED_CART_Viper850(vel_sat.data, REPMIX) );
        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_Viper850(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
vpRobotViper850::getVelocity (const vpRobot::vpControlFrameType frame,
               vpColVector & velocity)
{

  velocity.resize (6);
  velocity = 0;

  vpColVector q_cur(6);
  vpHomogeneousMatrix fMc_cur;
  vpHomogeneousMatrix cMc; // camera displacement

  InitTry;

  // Get the actual time
  double time_cur = vpTime::measureTimeSecond();

  // Get the current joint position
  Try( PrimitiveACQ_POS_J_Viper850(q_cur.data) );
  q_cur.deg2rad();

  // Get the camera pose from the direct kinematics
  vpViper850::get_fMc(q_cur, fMc_cur);

  if ( ! first_time_getvel ) {

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

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

      break ;
    }

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

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

      // Compute the velocity of the camera from this displacement
      vpColVector v;
      v = vpExponentialMap::inverse(cMc, time_cur - 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 = 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] = 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 - time_prev_getvel);
      break ;
    }
    }
  }
  else {
    first_time_getvel = false;
  }

  // Memorize the camera pose for the next call
  fMc_prev_getvel = fMc_cur;

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

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


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




vpColVector
vpRobotViper850::getVelocity (vpRobot::vpControlFrameType frame)
{
  vpColVector velocity;
  getVelocity (frame, velocity);

  return velocity;
}

bool
vpRobotViper850::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 {
      return (false);       /* fin fichier  */
    }

  }
  while ( sortie != true );

  // Lecture des positions
  q.resize(njoint);
  sscanf(line, "%s %lf %lf %lf %lf %lf %lf",
     dummy,
     &q[0], &q[1], &q[2],
     &q[3], &q[4], &q[5]);

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

  fclose(fd) ;
  return (true);
}

bool
vpRobotViper850::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
vpRobotViper850::move(const char *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
vpRobotViper850::getCameraDisplacement(vpColVector &displacement)
{
  getDisplacement(vpRobot::CAMERA_FRAME, displacement);
}
void
vpRobotViper850::getArticularDisplacement(vpColVector  &displacement)
{
  getDisplacement(vpRobot::ARTICULAR_FRAME, displacement);
}

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

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

  InitTry;

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

  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;

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

void
vpRobotViper850::biasForceTorqueSensor()
{
  InitTry;

  Try( PrimitiveTFS_BIAS_Viper850() );

  // 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
vpRobotViper850::getForceTorque(vpColVector &H)
{
  InitTry;

  H.resize (6);

  Try( PrimitiveTFS_ACQ_Viper850(H.data) );

  CatchPrint();
  if (TryStt < 0) {
    vpERROR_TRACE ("Cannot get the force/torque measures.");
    throw vpRobotException (vpRobotException::lowLevelError,
                "Cannot get force/torque measures.");
  }
}
void
vpRobotViper850::openGripper()
{
  InitTry;
  Try( PrimitiveGripper_Viper850(1) );
  std::cout << "Open the gripper..." << std::endl; 
  CatchPrint();
  if (TryStt < 0) {
    vpERROR_TRACE ("Cannot open the gripper");
    throw vpRobotException (vpRobotException::lowLevelError,
                  "Cannot open the gripper.");
  }
}

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


/*
 * Local variables:
 * c-basic-offset: 2
 * End:
 */

#endif