vpRobotPtu46.cpp

/****************************************************************************
 *
 * $Id: vpRobotPtu46.cpp 4574 2014-01-09 08:48:51Z fspindle $
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2014 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 ptu-46 robot.
 *
 * Authors:
 * Fabien Spindler
 *
 *****************************************************************************/

#include <signal.h>
#include <string.h>

#include <visp/vpConfig.h>
#ifdef VISP_HAVE_PTU46

/* Headers des fonctions implementees. */
#include <visp/vpPtu46.h>
#include <visp/vpRobotPtu46.h>
#include <visp/vpRobotException.h>
#include <visp/vpDebug.h>

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

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

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


vpRobotPtu46::vpRobotPtu46 (const char *device)
  :
  vpRobot ()
{
  this->device = new char [FILENAME_MAX];

  sprintf(this->device, "%s", device);

  vpDEBUG_TRACE (12, "Open communication with Ptu-46.");
  try
  {
    init();
  }
  catch(...)
  {
    delete [] this->device;
    vpERROR_TRACE("Error caught") ;
    throw ;
  }

  try
  {
    setRobotState(vpRobot::STATE_STOP) ;
  }
  catch(...)
  {
    delete [] this->device;
    vpERROR_TRACE("Error caught") ;
    throw ;
  }
  positioningVelocity = defaultPositioningVelocity ;
  return ;
}

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

vpRobotPtu46::~vpRobotPtu46 (void)
{

  setRobotState(vpRobot::STATE_STOP) ;

  if (0 != ptu.close())
  {
    vpERROR_TRACE ("Error while closing communications with the robot ptu-46.");
  }

  vpRobotPtu46::robotAlreadyCreated = false;

  delete [] device;

  return;
}


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

void
vpRobotPtu46::init ()
{

  vpDEBUG_TRACE (12, "Open connection Ptu-46.");
  if (0 != ptu.init(device) )
  {
    vpERROR_TRACE ("Cannot open connexion with ptu-46.");
    throw vpRobotException (vpRobotException::constructionError,
                "Cannot open connexion with ptu-46");
  }

  return ;
}


vpRobot::vpRobotStateType
vpRobotPtu46::setRobotState(vpRobot::vpRobotStateType newState)
{
  switch (newState)
  {
  case vpRobot::STATE_STOP:
    {
      if (vpRobot::STATE_STOP != getRobotState ())
      {
    ptu.stop();
      }
      break;
    }
  case vpRobot::STATE_POSITION_CONTROL:
    {
      if (vpRobot::STATE_VELOCITY_CONTROL  == getRobotState ())
      {
    vpDEBUG_TRACE (12, "Passage vitesse -> position.");
    ptu.stop();
      }
      else
      {
    vpDEBUG_TRACE (1, "Passage arret -> position.");
      }
      break;
    }
  case vpRobot::STATE_VELOCITY_CONTROL:
    {
      if (vpRobot::STATE_POSITION_CONTROL != getRobotState ())
      {
    vpDEBUG_TRACE (10, "Arret du robot...");
    ptu.stop();
      }
      break;
    }
  default:
    break ;
  }

  return vpRobot::setRobotState (newState);
}

void
vpRobotPtu46::stopMotion(void)
{
  ptu.stop();
  setRobotState (vpRobot::STATE_STOP);
}


void
vpRobotPtu46::get_cVe(vpVelocityTwistMatrix &cVe) const
{
  vpHomogeneousMatrix cMe ;
  vpPtu46::get_cMe(cMe) ;

  cVe.buildFrom(cMe) ;
}

void
vpRobotPtu46::get_cMe(vpHomogeneousMatrix &cMe) const
{
  vpPtu46::get_cMe(cMe) ;
}

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

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

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

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

}


void
vpRobotPtu46::setPositioningVelocity (const double velocity)
{
  positioningVelocity = velocity;
}
double
vpRobotPtu46::getPositioningVelocity (void)
{
  return positioningVelocity;
}


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

  // Interface for the controller
  double artpos[2];

  artpos[0] = q[0];
  artpos[1] = q[1];

  if (0 != ptu.move(artpos, positioningVelocity, PTU_ABSOLUTE_MODE) )
  {
    vpERROR_TRACE ("Positionning error.");
    throw vpRobotException (vpRobotException::lowLevelError,
                "Positionning error.");
  }

  return ;
}


void
vpRobotPtu46::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
vpRobotPtu46::setPosition(const char *filename)
{
  vpColVector q ;
  if (readPositionFile(filename, q) == false) {
    vpERROR_TRACE ("Cannot get ptu-46 position from file");
    throw vpRobotException (vpRobotException::readingParametersError,
                "Cannot get ptu-46 position from file");
  }
  setPosition ( vpRobot::ARTICULAR_FRAME, q) ;
}

void
vpRobotPtu46::getPosition (const vpRobot::vpControlFrameType frame,
               vpColVector & q)
{
  vpDEBUG_TRACE (9, "# Entree.");

  switch(frame)
  {
  case vpRobot::CAMERA_FRAME :
    vpERROR_TRACE ("Cannot get position in camera frame: not implemented");
    throw vpRobotException (vpRobotException::lowLevelError,
                "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::lowLevelError,
                "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::lowLevelError,
                "Cannot get position in mixt frame: "
                "not implemented");
    break;
  case vpRobot::ARTICULAR_FRAME:
    break ;
  }

  double artpos[2];

  if (0 != ptu.getCurrentPosition( artpos ) )
  {
    vpERROR_TRACE ("Error when calling  recup_posit_Afma4.");
    throw vpRobotException (vpRobotException::lowLevelError,
                "Error when calling  recup_posit_Afma4.");
  }

  q.resize (vpPtu46::ndof);

  q[0] = artpos[0];
  q[1] = artpos[1];
}


void
vpRobotPtu46::setVelocity (const vpRobot::vpControlFrameType frame,
               const vpColVector & v)
{
  TPtuFrame ptuFrameInterface;

  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 :
    {
      ptuFrameInterface = PTU_CAMERA_FRAME;
      if ( v.getRows() != 2) {
    vpERROR_TRACE ("Bad dimension fo speed vector in camera frame");
    throw vpRobotException (vpRobotException::wrongStateError,
                "Bad dimension for speed vector "
                "in camera frame");
      }
      break ;
    }
  case vpRobot::ARTICULAR_FRAME :
    {
      ptuFrameInterface = PTU_ARTICULAR_FRAME;
      if ( v.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
  double ptuSpeedInterface[2];

  switch(frame) {
  case vpRobot::ARTICULAR_FRAME :
  case vpRobot::CAMERA_FRAME : {
    double max = this ->maxRotationVelocity;
    for (unsigned int i = 0 ; i < 2; ++ i) // rx and ry of the camera
    {
      if (fabs (v[i]) > max)
      {
    norm = true;
    max = fabs (v[i]);
    vpERROR_TRACE ("Excess velocity: ROTATION "
             "(axe nr.%d).", i);
      }
    }
    // Rotations velocities normalisation
    if (norm == true) {
      max =  this ->maxRotationVelocity / max;
      for (unsigned int i = 0 ; i < 2; ++ i)
    ptuSpeedInterface [i] = v[i]*max;
    }
    break;
  }
  default:
    // Should never occur
    break;

  }

  vpCDEBUG(12) << "v: " << ptuSpeedInterface[0]
         << " " << ptuSpeedInterface[1] << std::endl;
  ptu.move(ptuSpeedInterface, ptuFrameInterface);
  return;
}


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


void
vpRobotPtu46::getVelocity (const vpRobot::vpControlFrameType frame,
               vpColVector & q_dot)
{

  TPtuFrame ptuFrameInterface = PTU_ARTICULAR_FRAME;

  switch (frame)
  {
  case vpRobot::CAMERA_FRAME:
    {
      vpERROR_TRACE ("Cannot get a velocity in the camera frame: "
           "functionality not implemented");
      throw vpRobotException (vpRobotException::wrongStateError,
                  "Cannot get a velocity in the camera frame:"
                  "functionality not implemented");
       break ;
    }
  case vpRobot::ARTICULAR_FRAME:
    {
      ptuFrameInterface = PTU_ARTICULAR_FRAME;
      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_dot.resize(vpPtu46::ndof);
  double ptuSpeedInterface[2];

  ptu.getCurrentSpeed(ptuSpeedInterface, ptuFrameInterface);

  q_dot[0] = ptuSpeedInterface[0];
  q_dot[1] = ptuSpeedInterface[1];

}


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

  return q_dot;
}

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

  if (pt_f == NULL) {
    vpERROR_TRACE ("Can not open ptu-46 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
  fscanf(pt_f, "%lf %lf", &q1, &q2);
  q.resize(vpPtu46::ndof) ;

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

  fclose(pt_f) ;
  return (true);
}

void
vpRobotPtu46::getCameraDisplacement(vpColVector &v)
{
  getDisplacement(vpRobot::CAMERA_FRAME, v);

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

void
vpRobotPtu46::getDisplacement(vpRobot::vpControlFrameType frame,
                  vpColVector &d)
{
  double d_[6];

  switch (frame)
  {
  case vpRobot::CAMERA_FRAME:
    {
      d.resize (6);
      ptu.measureDpl(d_, PTU_CAMERA_FRAME);
      d[0]=d_[0];
      d[1]=d_[1];
      d[2]=d_[2];
      d[3]=d_[3];
      d[4]=d_[4];
      d[5]=d_[5];
      break ;
    }
  case vpRobot::ARTICULAR_FRAME:
    {
      ptu.measureDpl(d_, PTU_ARTICULAR_FRAME);
      d.resize (vpPtu46::ndof);
      d[0]=d_[0];  // pan
      d[1]=d_[1];  // tilt
      break ;
    }
  case vpRobot::REFERENCE_FRAME:
    {
      vpERROR_TRACE ("Cannot get a displacement in the reference frame: "
           "functionality not implemented");
      throw vpRobotException (vpRobotException::wrongStateError,
                  "Cannot get a displacement in the reference frame:"
                  "functionality not implemented");
      break ;
    }
  case vpRobot::MIXT_FRAME:
    {
      vpERROR_TRACE ("Cannot get a displacement in the mixt frame: "
           "functionality not implemented");
      throw vpRobotException (vpRobotException::wrongStateError,
                  "Cannot get a displacement in the reference frame:"
                  "functionality not implemented");

      break ;
    }
  }
}

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

#endif