vpRobotBiclops.cpp

/****************************************************************************
 *
 * $Id: vpRobotBiclops.cpp 4056 2013-01-05 13:04:42Z fspindle $
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2013 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 Biclops robot.
 *
 * Authors:
 * Fabien Spindler
 *
 *****************************************************************************/

#include <signal.h>
#include <errno.h>
#include <string.h>
#include <cmath>    // std::fabs
#include <limits>   // numeric_limits

#include <visp/vpTime.h>

#include <visp/vpConfig.h>

#ifdef VISP_HAVE_BICLOPS

#include <visp/vpBiclops.h>
#include <visp/vpRobotBiclops.h>
#include <visp/vpRobotException.h>
#include <visp/vpExponentialMap.h>

//#define VP_DEBUG        // Activate the debug mode
//#define VP_DEBUG_MODE 10 // Activate debug level 1 and 2
#include <visp/vpDebug.h>

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

bool vpRobotBiclops::robotAlreadyCreated = false;
const double vpRobotBiclops::defaultPositioningVelocity = 10.0;

static pthread_mutex_t vpEndThread_mutex;
static pthread_mutex_t vpShm_mutex;
static pthread_mutex_t vpMeasure_mutex;

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


vpRobotBiclops::vpRobotBiclops ()
  :
  vpRobot ()
{
  vpDEBUG_TRACE (12, "Begin default constructor.");

  vpRobotBiclops::robotAlreadyCreated = false;
  controlThreadCreated = false;
  setConfigFile("/usr/share/BiclopsDefault.cfg");

  // Initialize the mutex dedicated to she shm protection
  pthread_mutex_init (&vpShm_mutex, NULL);
  pthread_mutex_init (&vpEndThread_mutex, NULL);
  pthread_mutex_init (&vpMeasure_mutex, NULL);
}

vpRobotBiclops::vpRobotBiclops (const char * filename)
  :
  vpRobot ()
{
  vpDEBUG_TRACE (12, "Begin default constructor.");

  vpRobotBiclops::robotAlreadyCreated = false;
  controlThreadCreated = false;
  setConfigFile(filename);

  // Initialize the mutex dedicated to she shm protection
  pthread_mutex_init (&vpShm_mutex, NULL);
  pthread_mutex_init (&vpEndThread_mutex, NULL);
  pthread_mutex_init (&vpMeasure_mutex
, NULL);

  init();

  return ;
}

vpRobotBiclops::~vpRobotBiclops (void)
{

  vpDEBUG_TRACE(12, "Start vpRobotBiclops::~vpRobotBiclops()");
  setRobotState(vpRobot::STATE_STOP) ;

  vpDEBUG_TRACE (12, "Unlock mutex vpEndThread_mutex");
  pthread_mutex_unlock(&vpEndThread_mutex);

  /* wait the end of the control thread */
  int code;
  vpDEBUG_TRACE (12, "Wait end of control thread");

  if (controlThreadCreated == true) {
    code = pthread_join(control_thread, NULL);
    if (code != 0) {
      vpCERROR << "Cannot terminate the control thread: " << code
         << " strErr=" << strerror(errno)
         << " strCode=" << strerror(code)
         << std::endl;
    }
  }

  pthread_mutex_destroy (&vpShm_mutex);
  pthread_mutex_destroy (&vpEndThread_mutex);
  pthread_mutex_destroy (&vpMeasure_mutex);

  vpRobotBiclops::robotAlreadyCreated = false;

  vpDEBUG_TRACE(12, "Stop vpRobotBiclops::~vpRobotBiclops()");
  return;
}


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

void
vpRobotBiclops::setConfigFile(const char * filename)
{
  sprintf(configfile, "%s", filename);
}

void
vpRobotBiclops::init ()
{
  // test if the config file exists
  FILE *fd = fopen(configfile, "r");
  if (fd == NULL) {
    vpCERROR << "Cannot open biclops config file: " << configfile << std::endl;
    throw vpRobotException (vpRobotException::constructionError,
                "Cannot open connexion with biclops");
  }
  fclose(fd);

  // Initialize the controller
  controller.init(configfile);

  try
  {
    setRobotState(vpRobot::STATE_STOP) ;
  }
  catch(...)
  {
    vpERROR_TRACE("Error caught") ;
    throw ;
  }

  vpRobotBiclops::robotAlreadyCreated = true;

  positioningVelocity = defaultPositioningVelocity ;

  // Initialize previous articular position to manage getDisplacement()
  q_previous.resize(vpBiclops::ndof);
  q_previous = 0;

  controlThreadCreated = false;

  return ;
}

/*
  Control loop to manage the biclops joint limits in speed control.

  This control loop is running in a seperate thread in order to detect each 5
  ms joint limits during the speed control. If a joint limit is detected the
  axis should be halted.

  \warning Velocity control mode is not exported from the top-level Biclops API
  class provided by Traclabs. That means that there is no protection in this
  mode to prevent an axis from striking its hard limit. In position mode,
  Traclabs put soft limits in that keep any command from driving to a position
  too close to the hard limits. In velocity mode this protection does not exist
  in the current API.

  \warning With the understanding that hitting the hard limits at full
  speed/power can damage the unit, damage due to velocity mode commanding is
  under user responsibility.
*/
void * vpRobotBiclops::vpRobotBiclopsSpeedControlLoop (void * arg)
{
  vpRobotBiclopsController *controller = ( vpRobotBiclopsController * ) arg;

  int iter = 0;
//   PMDAxisControl *panAxis  = controller->getPanAxis();
//   PMDAxisControl *tiltAxis = controller->getTiltAxis();
  vpRobotBiclopsController::shmType shm;

  vpDEBUG_TRACE(10, "Start control loop");
  vpColVector mes_q;
  vpColVector mes_q_dot;
  vpColVector softLimit(vpBiclops::ndof);
  vpColVector q_dot(vpBiclops::ndof);
  bool *new_q_dot  = new bool [ vpBiclops::ndof ];
  bool *change_dir = new bool [ vpBiclops::ndof ]; // change of direction
  bool *force_halt = new bool [ vpBiclops::ndof ]; // force an axis to halt
  bool *enable_limit = new bool [ vpBiclops::ndof ]; // enable soft limit
  vpColVector prev_q_dot(vpBiclops::ndof); // previous desired speed
  double secure = vpMath::rad(2); // add a security angle before joint limit


  // Set the soft limits
  softLimit[0] = vpBiclops::panJointLimit  - secure;
  softLimit[1] = vpBiclops::tiltJointLimit - secure;
  vpDEBUG_TRACE(12, "soft limit pan: %f tilt: %f",
          vpMath::deg(softLimit[0]),
          vpMath::deg(softLimit[1]));

  // Initilisation
  vpDEBUG_TRACE (11, "Lock mutex vpShm_mutex");
  pthread_mutex_lock(&vpShm_mutex);

  shm = controller->readShm();

  vpDEBUG_TRACE (11, "unlock mutex vpShm_mutex");
  pthread_mutex_unlock(&vpShm_mutex);

  for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
    prev_q_dot  [i] = shm.q_dot[i];
    new_q_dot   [i] = false;
    change_dir  [i] = false;
    force_halt  [i] = false;
    enable_limit[i] = true;
  }

  // Initialize actual position and velocity
  mes_q     = controller->getActualPosition();
  mes_q_dot = controller->getActualVelocity();

  vpDEBUG_TRACE (11, "Lock mutex vpShm_mutex");
  pthread_mutex_lock(&vpShm_mutex);

  shm = controller->readShm();
  // Updates the shm
  for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
    shm.actual_q[i]     = mes_q[i];
    shm.actual_q_dot[i] = mes_q_dot[i];
  }
  // Update the actuals positions
  controller->writeShm(shm);

  vpDEBUG_TRACE (11, "unlock mutex vpShm_mutex");
  pthread_mutex_unlock(&vpShm_mutex);

  vpDEBUG_TRACE (11, "unlock mutex vpMeasure_mutex");
  pthread_mutex_unlock(&vpMeasure_mutex); // A position is available

  while (! controller->isStopRequested()) {

    // Get actual position and velocity
    mes_q     = controller->getActualPosition();
    mes_q_dot = controller->getActualVelocity();

    vpDEBUG_TRACE (11, "Lock mutex vpShm_mutex");
    pthread_mutex_lock(&vpShm_mutex);


    shm = controller->readShm();

    // Updates the shm
    for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
      shm.actual_q[i]     = mes_q[i];
      shm.actual_q_dot[i] = mes_q_dot[i];
    }

    vpDEBUG_TRACE(12, "mes pan: %f tilt: %f",
        vpMath::deg(mes_q[0]),
        vpMath::deg(mes_q[1]));
    vpDEBUG_TRACE(13, "mes pan vel: %f tilt vel: %f",
        vpMath::deg(mes_q_dot[0]),
        vpMath::deg(mes_q_dot[1]));
    vpDEBUG_TRACE(12, "desired  q_dot : %f %f",
        vpMath::deg(shm.q_dot[0]),
        vpMath::deg(shm.q_dot[1]));
    vpDEBUG_TRACE(13, "previous q_dot : %f %f",
        vpMath::deg(prev_q_dot[0]),
        vpMath::deg(prev_q_dot[1]));

    for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
      // test if joint limits are reached
      if (mes_q[i] < -softLimit[i]) {
    vpDEBUG_TRACE(12, "Axe %d in low joint limit", i);
    shm.status[i] = vpRobotBiclopsController::STOP;
    shm.jointLimit[i] = true;
      }
      else if (mes_q[i] > softLimit[i]) {
    vpDEBUG_TRACE(12, "Axe %d in hight joint limit", i);
    shm.status[i] = vpRobotBiclopsController::STOP;
    shm.jointLimit[i] = true;
      }
      else {
    shm.status[i] = vpRobotBiclopsController::SPEED;
    shm.jointLimit[i] = false;
      }

      // Test if new a speed is demanded
      //if (shm.q_dot[i] != prev_q_dot[i])
      if (std::fabs(shm.q_dot[i] - prev_q_dot[i]) > std::fabs(vpMath::maximum(shm.q_dot[i],prev_q_dot[i]))*std::numeric_limits<double>::epsilon())
    new_q_dot[i] = true;
      else
    new_q_dot[i] = false;

      // Test if desired speed change of sign
      if ((shm.q_dot[i] * prev_q_dot[i]) < 0.)
    change_dir[i] = true;
      else
    change_dir[i] = false;

    }
    vpDEBUG_TRACE(13, "status      : %d %d", shm.status[0], shm.status[1]);
    vpDEBUG_TRACE(13, "joint       : %d %d", shm.jointLimit[0], shm.jointLimit[1]);
    vpDEBUG_TRACE(13, "new q_dot   : %d %d", new_q_dot[0], new_q_dot[1]);
    vpDEBUG_TRACE(13, "new dir     : %d %d", change_dir[0], change_dir[1]);
    vpDEBUG_TRACE(13, "force halt  : %d %d", force_halt[0], force_halt[1]);
    vpDEBUG_TRACE(13, "enable limit: %d %d", enable_limit[0], enable_limit[1]);


    bool updateVelocity = false;
    for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
      // Test if a new desired speed is to apply
      if (new_q_dot[i]) {
    // A new desired speed is to apply
    if (shm.status[i] == vpRobotBiclopsController::STOP) {
      // Axis in joint limit
      if (change_dir[i] == false) {
        // New desired speed without change of direction
        // We go in the joint limit
        if (enable_limit[i] == true) { // limit detection active
          // We have to stop this axis
          // Test if this axis was stopped before
          if (force_halt[i] == false) {
        q_dot[i] = 0.;
        force_halt[i] = true; // indicate that it will be stopped
        updateVelocity = true; // We have to send this new speed
          }
        }
        else {
          // We have to apply the desired speed to go away the joint
          // Update the desired speed
          q_dot[i] = shm.q_dot[i];
          shm.status[i] = vpRobotBiclopsController::SPEED;
          force_halt[i] = false;
          updateVelocity  = true; // We have to send this new speed
        }
      }
      else {
        // New desired speed and change of direction.
        if (enable_limit[i] == true) { // limit detection active
          // Update the desired speed to go away the joint limit
          q_dot[i] = shm.q_dot[i];
          shm.status[i] = vpRobotBiclopsController::SPEED;
          force_halt[i] = false;
          enable_limit[i] = false; // Disable joint limit detection
          updateVelocity = true; // We have to send this new speed
        }
        else {
          // We have to stop this axis
          // Test if this axis was stopped before
          if (force_halt[i] == false) {
        q_dot[i] = 0.;
        force_halt[i] = true; // indicate that it will be stopped
        enable_limit[i] = true; // Joint limit detection must be active
        updateVelocity  = true; // We have to send this new speed
          }
        }
      }
    }
    else {
      // Axis not in joint limit

      // Update the desired speed
      q_dot[i] = shm.q_dot[i];
      shm.status[i] = vpRobotBiclopsController::SPEED;
      enable_limit[i] = true; // Joint limit detection must be active
      updateVelocity  = true; // We have to send this new speed
    }
      }
      else {
    // No change of the desired speed. We have to stop the robot in case of
    // joint limit
    if (shm.status[i] == vpRobotBiclopsController::STOP) {// axis limit
      if (enable_limit[i] == true)  { // limit detection active

        // Test if this axis was stopped before
        if (force_halt[i] == false) {
          // We have to stop this axis
          q_dot[i] = 0.;
          force_halt[i] = true; // indicate that it will be stopped
          updateVelocity = true; // We have to send this new speed
        }
      }
    }
    else {
      // No need to stop the robot
      enable_limit[i] = true; // Normal situation, activate limit detection
    }
      }
    }
    // Update the actuals positions
    controller->writeShm(shm);

    vpDEBUG_TRACE (11, "unlock mutex vpShm_mutex");
    pthread_mutex_unlock(&vpShm_mutex);

    if (updateVelocity) {
      vpDEBUG_TRACE(12, "apply q_dot : %f %f",
        vpMath::deg(q_dot[0]),
        vpMath::deg(q_dot[1]));

      // Apply the velocity
      controller -> setVelocity( q_dot );
    }


    // Update the previous speed for next iteration
    for (unsigned int i=0; i < vpBiclops::ndof; i ++)
      prev_q_dot[i] = shm.q_dot[i];

    vpDEBUG_TRACE(12, "iter: %d", iter);

    //wait 5 ms
    vpTime::wait(5.0);

//    if (pthread_mutex_trylock(&vpEndThread_mutex) == 0) {
//      vpDEBUG_TRACE (12, "Calling thread will end");
//      vpDEBUG_TRACE (12, "Unlock mutex vpEndThread_mutex");
//      std::cout << "Calling thread will end" << std::endl;
//      std::cout << "Unlock mutex vpEndThread_mutex" << std::endl;
//
//      pthread_mutex_unlock(&vpEndThread_mutex);
//      break;
//    }

    iter ++;
  }
  controller->stopRequest(false);
  // Stop the robot
  vpDEBUG_TRACE(10, "End of the control thread: stop the robot");
  q_dot = 0;
  controller -> setVelocity( q_dot );

  delete [] new_q_dot;
  delete [] change_dir;
  delete [] force_halt;
  delete [] enable_limit;
  vpDEBUG_TRACE(11, "unlock vpEndThread_mutex");
  pthread_mutex_unlock(&vpEndThread_mutex);

  vpDEBUG_TRACE (10, "Exit control thread ");
  //  pthread_exit(0);

  return NULL;
}


vpRobot::vpRobotStateType
vpRobotBiclops::setRobotState(vpRobot::vpRobotStateType newState)
{
  switch (newState)
  {
  case vpRobot::STATE_STOP:
    {
      if (vpRobot::STATE_STOP != getRobotState ())
      {
    stopMotion();
      }
      break;
    }
  case vpRobot::STATE_POSITION_CONTROL:
    {
      if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState ())
      {
    vpDEBUG_TRACE (12, "Speed to position control.");
    stopMotion();
      }

      break;
    }
  case vpRobot::STATE_VELOCITY_CONTROL:
    {

      if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ())
      {
    vpDEBUG_TRACE (12, "Lock mutex vpEndThread_mutex");
    pthread_mutex_lock(&vpEndThread_mutex);

    vpDEBUG_TRACE (12, "Create speed control thread");
    int code;
    code = pthread_create(&control_thread, NULL,
                  &vpRobotBiclops::vpRobotBiclopsSpeedControlLoop,
                  &controller);
    if (code != 0)  {
      vpCERROR << "Cannot create speed biclops control thread: " << code
         << " strErr=" << strerror(errno)
         << " strCode=" << strerror(code)
         << std::endl;
    }

    controlThreadCreated = true;

    vpDEBUG_TRACE (12, "Speed control thread created");
      }
      break;
    }
  default:
    break ;
  }

  return vpRobot::setRobotState (newState);
}



void
vpRobotBiclops::stopMotion(void)
{
  vpColVector q_dot(vpBiclops::ndof);
  q_dot = 0;
  controller.setVelocity(q_dot);
  //std::cout << "Request to stop the velocity controller thread...."<< std::endl;
  controller.stopRequest(true);
}

void
vpRobotBiclops::get_cVe(vpVelocityTwistMatrix &cVe)
{
  vpHomogeneousMatrix cMe ;
  cMe = vpBiclops::get_cMe() ;

  cVe.buildFrom(cMe) ;
}

void
vpRobotBiclops::get_cMe(vpHomogeneousMatrix &cMe)
{
  cMe = vpBiclops::get_cMe() ;
}


void
vpRobotBiclops::get_eJe(vpMatrix &eJe)
{
  vpColVector q(2) ;
  getPosition(vpRobot::ARTICULAR_FRAME, q) ;

  try
  {
    vpBiclops::get_eJe(q,eJe) ;
  }
  catch(...)
  {
    vpERROR_TRACE("catch exception ") ;
    throw ;
  }
}

void
vpRobotBiclops::get_fJe(vpMatrix &fJe)
{
  vpColVector q(2) ;
  getPosition(vpRobot::ARTICULAR_FRAME, q) ;

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

}

void
vpRobotBiclops::setPositioningVelocity (const double velocity)
{
  if (velocity < 0 || velocity > 100) {
    vpERROR_TRACE("Bad positionning velocity");
    throw vpRobotException (vpRobotException::constructionError,
                "Bad positionning velocity");
  }

  positioningVelocity = velocity;
}
double
vpRobotBiclops::getPositioningVelocity (void)
{
  return positioningVelocity;
}


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

  switch(frame)
  {
  case vpRobot::CAMERA_FRAME:
    vpERROR_TRACE ("Cannot move the robot in camera frame: "
         "not implemented");
    throw vpRobotException (vpRobotException::wrongStateError,
                "Cannot move the robot in camera frame: "
                "not implemented");
    break;
  case vpRobot::REFERENCE_FRAME:
    vpERROR_TRACE ("Cannot move the robot in reference frame: "
         "not implemented");
    throw vpRobotException (vpRobotException::wrongStateError,
                "Cannot move the robot in reference frame: "
                "not implemented");
    break;
  case vpRobot::MIXT_FRAME:
    vpERROR_TRACE ("Cannot move the robot in mixt frame: "
         "not implemented");
    throw vpRobotException (vpRobotException::wrongStateError,
                "Cannot move the robot in mixt frame: "
                "not implemented");
    break;
  case vpRobot::ARTICULAR_FRAME:
    break ;
  }

  // test if position reachable
//   if ( (fabs(q[0]) > vpBiclops::panJointLimit) ||
//        (fabs(q[1]) > vpBiclops::tiltJointLimit) ) {
//     vpERROR_TRACE ("Positionning error.");
//     throw vpRobotException (vpRobotException::wrongStateError,
//              "Positionning error.");
//   }

  vpDEBUG_TRACE (12, "Lock mutex vpEndThread_mutex");
  pthread_mutex_lock(&vpEndThread_mutex);
  controller.setPosition( q, positioningVelocity );
  vpDEBUG_TRACE (12, "Unlock mutex vpEndThread_mutex");
  pthread_mutex_unlock(&vpEndThread_mutex);
  return ;
}

void vpRobotBiclops::setPosition (const vpRobot::vpControlFrameType frame,
                  const double &q1, const double &q2)
{
  try{
    vpColVector q(2) ;
    q[0] = q1 ;
    q[1] = q2 ;

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

void
vpRobotBiclops::setPosition(const char *filename)
{
  vpColVector q ;
  if (readPositionFile(filename, q) == false) {
    vpERROR_TRACE ("Cannot get biclops position from file");
    throw vpRobotException (vpRobotException::readingParametersError,
                "Cannot get biclops position from file");
  }
  setPosition ( vpRobot::ARTICULAR_FRAME, q) ;
}

void
vpRobotBiclops::getPosition (const vpRobot::vpControlFrameType frame,
                 vpColVector & q)
{
  switch(frame)
  {
  case vpRobot::CAMERA_FRAME :
    vpERROR_TRACE ("Cannot get position in camera frame: not implemented");
    throw vpRobotException (vpRobotException::wrongStateError,
                "Cannot get position in camera frame: "
                "not implemented");
    break;
  case vpRobot::REFERENCE_FRAME:
    vpERROR_TRACE ("Cannot get position in reference frame: "
         "not implemented");
    throw vpRobotException (vpRobotException::wrongStateError,
                "Cannot get position in reference frame: "
                "not implemented");
    break;
  case vpRobot::MIXT_FRAME:
    vpERROR_TRACE ("Cannot get position in mixt frame: "
         "not implemented");
    throw vpRobotException (vpRobotException::wrongStateError,
                "Cannot get position in mixt frame: "
                "not implemented");
    break;
  case vpRobot::ARTICULAR_FRAME:
    break ;
  }

  vpRobot::vpRobotStateType state;
  state = vpRobot::getRobotState();

  switch (state) {
  case STATE_STOP:
  case STATE_POSITION_CONTROL:
    q = controller.getPosition();

    break;
  case STATE_VELOCITY_CONTROL:
  case STATE_ACCELERATION_CONTROL:
  default:
    q.resize(vpBiclops::ndof);

    vpDEBUG_TRACE (12, "Lock mutex vpMeasure_mutex");
    pthread_mutex_lock(&vpMeasure_mutex); // Wait until a position is available

    vpRobotBiclopsController::shmType shm;

    vpDEBUG_TRACE (12, "Lock mutex vpShm_mutex");
    pthread_mutex_lock(&vpShm_mutex);

    shm = controller.readShm();

    vpDEBUG_TRACE (12, "unlock mutex vpShm_mutex");
    pthread_mutex_unlock(&vpShm_mutex);

    for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
      q[i] = shm.actual_q[i];
    }

    vpCDEBUG(11) << "++++++++ Measure actuals: " << q.t();

    vpDEBUG_TRACE (12, "unlock mutex vpMeasure_mutex");
    pthread_mutex_unlock(&vpMeasure_mutex); // A position is available

    break;

  }
}


void
vpRobotBiclops::setVelocity (const vpRobot::vpControlFrameType frame,
                 const vpColVector & q_dot)
{
  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) ");
  }

  switch(frame)
  {
  case vpRobot::CAMERA_FRAME :
    {
      vpERROR_TRACE ("Cannot send a velocity to the robot "
           "in the camera frame: "
           "functionality not implemented");
      throw vpRobotException (vpRobotException::wrongStateError,
                  "Cannot send a velocity to the robot "
                  "in the camera frame:"
                  "functionality not implemented");
      break ;
    }
  case vpRobot::ARTICULAR_FRAME :
    {
      if ( q_dot.getRows() != 2) {
    vpERROR_TRACE ("Bad dimension fo speed vector in articular frame");
    throw vpRobotException (vpRobotException::wrongStateError,
                "Bad dimension for speed vector "
                "in articular frame");
      }
      break ;
    }
  case vpRobot::REFERENCE_FRAME :
    {
      vpERROR_TRACE ("Cannot send a velocity to the robot "
           "in the reference frame: "
           "functionality not implemented");
      throw vpRobotException (vpRobotException::wrongStateError,
                  "Cannot send a velocity to the robot "
                  "in the reference frame:"
                  "functionality not implemented");
      break ;
    }
  case vpRobot::MIXT_FRAME :
    {
      vpERROR_TRACE ("Cannot send a velocity to the robot "
           "in the mixt frame: "
           "functionality not implemented");
      throw vpRobotException (vpRobotException::wrongStateError,
                  "Cannot send a velocity to the robot "
                  "in the mixt frame:"
                  "functionality not implemented");
      break ;
    }
  default:
    {
      vpERROR_TRACE ("Error in spec of vpRobot. "
           "Case not taken in account.");
      throw vpRobotException (vpRobotException::wrongStateError,
                  "Cannot send a velocity to the robot ");
    }
  }

  vpDEBUG_TRACE (12, "Velocity limitation.");
  bool norm = false; // Flag to indicate when velocities need to be nomalized

  // Saturate articular speed
  double max = vpBiclops::speedLimit;
  vpColVector q_dot_sat(vpBiclops::ndof);

  // init q_dot_saturated
  q_dot_sat = q_dot;

  for (unsigned int i = 0 ; i < vpBiclops::ndof; ++ i) // q1 and q2
  {
    if (fabs (q_dot[i]) > max)
    {
      norm = true;
      max = fabs (q_dot[i]);
      vpERROR_TRACE ("Excess velocity: ROTATION "
             "(axe nr.%d).", i);
    }
  }
  // Rotations velocities normalisation
  if (norm == true) {
    max = vpBiclops::speedLimit / max;
    q_dot_sat = q_dot * max;
  }

  vpCDEBUG(12) << "send velocity: " << q_dot_sat.t() << std::endl;

  vpRobotBiclopsController::shmType shm;

  vpDEBUG_TRACE (12, "Lock mutex vpShm_mutex");
  pthread_mutex_lock(&vpShm_mutex);

  shm = controller.readShm();

  for (unsigned int i=0; i < vpBiclops::ndof; i ++)
    shm.q_dot[i] = q_dot[i];

  controller.writeShm(shm);

  vpDEBUG_TRACE (12, "unlock mutex vpShm_mutex");
  pthread_mutex_unlock(&vpShm_mutex);

  return;
}


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


void
vpRobotBiclops::getVelocity (const vpRobot::vpControlFrameType frame,
                 vpColVector & q_dot)
{
  switch(frame)
  {
  case vpRobot::CAMERA_FRAME :
    vpERROR_TRACE ("Cannot get position in camera frame: not implemented");
    throw vpRobotException (vpRobotException::wrongStateError,
                "Cannot get position in camera frame: "
                "not implemented");
    break;
  case vpRobot::REFERENCE_FRAME:
    vpERROR_TRACE ("Cannot get position in reference frame: "
         "not implemented");
    throw vpRobotException (vpRobotException::wrongStateError,
                "Cannot get position in reference frame: "
                "not implemented");
    break;
  case vpRobot::MIXT_FRAME:
    vpERROR_TRACE ("Cannot get position in mixt frame: "
         "not implemented");
    throw vpRobotException (vpRobotException::wrongStateError,
                "Cannot get position in mixt frame: "
                "not implemented");
    break;
  case vpRobot::ARTICULAR_FRAME:
    break ;
  }

  vpRobot::vpRobotStateType state;
  state = vpRobot::getRobotState();

  switch (state) {
  case STATE_STOP:
  case STATE_POSITION_CONTROL:
    q_dot = controller.getVelocity();

    break;
  case STATE_VELOCITY_CONTROL:
  case STATE_ACCELERATION_CONTROL:
  default:
    q_dot.resize(vpBiclops::ndof);

    vpDEBUG_TRACE (12, "Lock mutex vpMeasure_mutex");
    pthread_mutex_lock(&vpMeasure_mutex); // Wait until a position is available

    vpRobotBiclopsController::shmType shm;

    vpDEBUG_TRACE (12, "Lock mutex vpShm_mutex");
    pthread_mutex_lock(&vpShm_mutex);

    shm = controller.readShm();

    vpDEBUG_TRACE (12, "unlock mutex vpShm_mutex");
    pthread_mutex_unlock(&vpShm_mutex);

    for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
      q_dot[i] = shm.actual_q_dot[i];
    }

    vpCDEBUG(11) << "++++++++ Velocity actuals: " << q_dot.t();

    vpDEBUG_TRACE (12, "unlock mutex vpMeasure_mutex");
    pthread_mutex_unlock(&vpMeasure_mutex); // A position is available

    break;

  }
}


vpColVector
vpRobotBiclops::getVelocity (vpRobot::vpControlFrameType frame)
{
  vpColVector q_dot;
  getVelocity (frame, q_dot);

  return q_dot;
}

bool
vpRobotBiclops::readPositionFile(const char *filename, vpColVector &q)
{
  FILE * pt_f ;
  pt_f = fopen(filename,"r") ;

  if (pt_f == NULL) {
    vpERROR_TRACE ("Can not open biclops position file %s", filename);
    return false;
  }

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

  do {
    // skip lines begining with # for comments
    if (fgets (line, 100, pt_f) != NULL) {
      if ( strncmp (line, "#", 1) != 0) {
    // this line is not a comment
    if ( fscanf (pt_f, "%s", line) != EOF)   {
      if ( strcmp (line, head) == 0)
        end = true;     // robot position was found
    }
    else
      return (false); // end of file without position
      }
    }
    else {
      return (false);// end of file
    }

  }
  while ( end != true );

  double q1,q2;
  // Read positions
  if (fscanf(pt_f, "%lf %lf", &q1, &q2) == EOF) {
    std::cout << "Cannot read joint positions." << std::endl;
    return false;
  }
  q.resize(vpBiclops::ndof) ;

  q[0] = vpMath::rad(q1) ; // Rot tourelle
  q[1] = vpMath::rad(q2) ;

  fclose(pt_f) ;
  return (true);
}

void
vpRobotBiclops::getCameraDisplacement(vpColVector &d)
{
  getDisplacement(vpRobot::CAMERA_FRAME, d);

}
void vpRobotBiclops::getArticularDisplacement(vpColVector &d)
{
  getDisplacement(vpRobot::ARTICULAR_FRAME, d);
}

void
vpRobotBiclops::getDisplacement(vpRobot::vpControlFrameType frame,
                vpColVector &d)
{
  vpColVector q_current; // current position

  getPosition(vpRobot::ARTICULAR_FRAME, q_current);

  switch(frame) {
  case vpRobot::ARTICULAR_FRAME:
    d.resize(vpBiclops::ndof);
    d = q_current - q_previous;
    break ;

  case vpRobot::CAMERA_FRAME: {
    d.resize(6);
    vpHomogeneousMatrix fMc_current;
    vpHomogeneousMatrix fMc_previous;
    fMc_current  = vpBiclops::get_fMc(q_current);
    fMc_previous = vpBiclops::get_fMc(q_previous);
    vpHomogeneousMatrix c_previousMc_current;
    // fMc_c = fMc_p * c_pMc_c
    // => c_pMc_c = (fMc_p)^-1 * fMc_c
    c_previousMc_current = fMc_previous.inverse() * fMc_current;

    // Compute the instantaneous velocity from this homogeneous matrix.
    d = vpExponentialMap::inverse( c_previousMc_current );
    break ;
  }

  case vpRobot::REFERENCE_FRAME:
      vpERROR_TRACE ("Cannot get a velocity in the reference frame: "
           "functionality not implemented");
      throw vpRobotException (vpRobotException::wrongStateError,
                  "Cannot get a velocity in the reference frame:"
                  "functionality not implemented");
      break ;
  case vpRobot::MIXT_FRAME:
      vpERROR_TRACE ("Cannot get a velocity in the mixt frame: "
           "functionality not implemented");
      throw vpRobotException (vpRobotException::wrongStateError,
                  "Cannot get a velocity in the mixt frame:"
                  "functionality not implemented");
      break ;
  }


  q_previous = q_current; // Update for next call of this method

}



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

#endif