vpRobotPtu46.cpp

/****************************************************************************
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2017 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://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 ptu-46 robot.
 *
 * Authors:
 * Fabien Spindler
 *
 *****************************************************************************/

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

#include <visp3/core/vpConfig.h>
#ifdef VISP_HAVE_PTU46

/* Headers des fonctions implementees. */
#include <visp3/robot/vpPtu46.h>
#include <visp3/robot/vpRobotPtu46.h>
#include <visp3/robot/vpRobotException.h>
#include <visp3/core/vpDebug.h>
#include <visp3/core/vpIoTools.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 connection with ptu-46.");
    throw vpRobotException (vpRobotException::constructionError,
          "Cannot open connection 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");
    }
  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");
    }
  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.");
  double ptuSpeedInterface[2];

  switch(frame) {
  case vpRobot::ARTICULAR_FRAME :
  case vpRobot::CAMERA_FRAME : {
    double max = this ->maxRotationVelocity;
    bool norm = false; // Flag to indicate when velocities need to be nomalized
    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");
    }
  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");
    }
  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");
    }
  }

  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 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("#PTU-EVI - Position");
  bool pos_found = false;
  int lineNum = 0;

  q.resize(vpPtu46::ndof);

  while(std::getline(fd, line)) {
    lineNum ++;
    if (lineNum == 1) {
      if(! (line.compare(0, id.size(), id) == 0)) { // check if Ptu-46 position file
        std::cout << "Error: this position file " << filename << " is not for Ptu-46 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() < vpPtu46::ndof+1) // try to split with tab separator
        chain = vpIoTools::splitChain(line, std::string("\t"));
      if(chain.size() < vpPtu46::ndof+1)
        continue;

      std::istringstream ss(line);
      std::string key_;
      ss >> key_;
      for (unsigned int i=0; i< vpPtu46::ndof; 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 Ptu-46 robot in " << filename << std::endl;
    return false;
  }

  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");
    }
  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");
    }
  }
}

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