vpVirtuose.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: Class which enables to project an image in the 3D space
 * and get the view of a virtual camera.
 *
 * Authors:
 * Fabien Spindler
 *
 *****************************************************************************/

#include <visp3/core/vpException.h>
#include <visp3/robot/vpVirtuose.h>

#ifdef VISP_HAVE_VIRTUOSE

vpVirtuose::vpVirtuose()
  : m_virtContext(NULL), m_ip("localhost#5000"), m_verbose(false),
    m_apiMajorVersion(0), m_apiMinorVersion(0),
    m_ctrlMajorVersion(0), m_ctrlMinorVersion(0),
    m_typeCommand(COMMAND_TYPE_IMPEDANCE), m_indexType(INDEXING_ALL),
    m_is_init(false), m_period(0.001f)
{
  virtAPIVersion(&m_apiMajorVersion, &m_apiMinorVersion);
  std::cout << "API version: " << m_apiMajorVersion << "." << m_apiMinorVersion << std::endl;
}

vpVirtuose::~vpVirtuose()
{
  if (m_virtContext != NULL) {
    virtClose(m_virtContext);
    m_virtContext = NULL;
  }
}

void vpVirtuose::addForce(vpColVector &force)
{
  if (force.size() != 6) {
    throw(vpException(vpException::dimensionError,
                      "Cannot apply a force feedback (dim %d) to the haptic device that is not 6-dimension",
                      force.size()));
  }

  init();

  float virtforce[6];
  for(unsigned int i=0; i<6; i++)
    virtforce[i] = (float)force[i];

  if (virtAddForce(m_virtContext, virtforce)) {
    int err = virtGetErrorCode(m_virtContext);
    throw(vpException(vpException::fatalError,
                      "Error calling virtAddForce: error code %d", err));
  }
}

void vpVirtuose::enableForceFeedback(int enable)
{
  init();

  if (virtEnableForceFeedback(m_virtContext, enable)) {
    int err = virtGetErrorCode(m_virtContext);
    throw(vpException(vpException::fatalError,
                      "Error calling virtEnableForceFeedback(): error code %d", err));
  }
}

vpColVector vpVirtuose::getArticularPosition() const
{
  if (!m_is_init) {
    throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
  }

  vpColVector articularPosition(6,0);

  float articular_position_[6];
  if (virtGetArticularPosition(m_virtContext, articular_position_)) {
    int err = virtGetErrorCode(m_virtContext);
    throw(vpException(vpException::fatalError,
                      "Error calling virtGetArticularPosition(): error code %d", err));
  }

  for(unsigned int i=0; i<6; i++)
    articularPosition[i] = articular_position_[i];

  return articularPosition;
}


vpColVector vpVirtuose::getArticularVelocity() const
{
  if (!m_is_init) {
    throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
  }

  vpColVector articularVelocity(6,0);
  float articular_velocity_[6];
  if (virtGetArticularSpeed(m_virtContext, articular_velocity_)) {
    int err = virtGetErrorCode(m_virtContext);
    throw(vpException(vpException::fatalError,
                      "Error calling virtGetArticularSpeed: error code %d", err));
  }

  for(unsigned int i=0; i<6; i++)
    articularVelocity[i] = articular_velocity_[i];

  return articularVelocity;

}

vpPoseVector vpVirtuose::getAvatarPosition() const
{
  if (!m_is_init) {
    throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
  }

  float position_[7];
  vpPoseVector position;
  vpTranslationVector translation;
  vpQuaternionVector quaternion;

  if (virtGetAvatarPosition(m_virtContext, position_)) {
    int err = virtGetErrorCode(m_virtContext);
    throw(vpException(vpException::fatalError,
                      "Error calling virtGetAvatarPosition: error code %d", err));
  }
  else
  {
    for (int i=0; i<3; i++)
      translation[i] = position_[i];
    for (int i=0; i<4; i++)
      quaternion[i] = position_[3+i];

    vpThetaUVector thetau(quaternion);

    position.buildFrom(translation, thetau);

    return position;
  }
}

vpPoseVector vpVirtuose::getBaseFrame() const
{
  if (!m_is_init) {
    throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
  }

  vpPoseVector position;
  float position_[7];
  vpTranslationVector translation;
  vpQuaternionVector quaternion;

  if (virtGetBaseFrame(m_virtContext, position_)) {
    int err = virtGetErrorCode(m_virtContext);
    throw(vpException(vpException::fatalError,
                      "Error calling virtGetBaseFrame: error code %d", err));
  }
  else
  {
    for (int i=0; i<3; i++)
      translation[i] = position_[i];
    for (int i=0; i<4; i++)
      quaternion[i] = position_[3+i];

    vpThetaUVector thetau(quaternion);

    position.buildFrom(translation, thetau);

    return position;
  }
}

VirtCommandType vpVirtuose::getCommandType() const
{
  if (!m_is_init) {
    throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
  }

  VirtCommandType type;

  if (virtGetCommandType(m_virtContext, &type)) {
    int err = virtGetErrorCode(m_virtContext);
    throw(vpException(vpException::fatalError,
                      "Error calling virtGetCommandType: error code %d", err));
  }
  return type;
}

bool vpVirtuose::getDeadMan() const
{
  if (!m_is_init) {
    throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
  }

  int deadman;
  if (virtGetDeadMan(m_virtContext, &deadman)) {
    int err = virtGetErrorCode(m_virtContext);
    throw(vpException(vpException::fatalError,
                      "Error calling virtGetDeadMan: error code %d", err));
  }
  return (deadman ? true : false);
}

bool vpVirtuose::getEmergencyStop() const
{
  if (!m_is_init) {
    throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
  }

  int emergencyStop;
  if (virtGetEmergencyStop(m_virtContext, &emergencyStop)) {
    int err = virtGetErrorCode(m_virtContext);
    throw(vpException(vpException::fatalError,
                      "Error calling virtGetEmergencyStop: error code %d", err));
  }
  return (emergencyStop ? true : false);
}

vpColVector vpVirtuose::getForce() const
{
  if (!m_is_init) {
    throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
  }

  vpColVector force(6,0);
  float force_[6];
  if (virtGetForce(m_virtContext, force_)) {
    int err = virtGetErrorCode(m_virtContext);
    throw(vpException(vpException::fatalError,
                      "Error calling virtGetForce: error code %d", err));
  }

  for(unsigned int i=0; i<6; i++)
    force[i] = force_[i];
  return force;
}

VirtContext vpVirtuose::getHandler() {
  return m_virtContext;
}

vpPoseVector vpVirtuose::getObservationFrame () const
{
  if (!m_is_init) {
    throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
  }

  vpPoseVector position;
  float position_[7];
  vpTranslationVector translation;
  vpQuaternionVector quaternion;

  if (virtGetObservationFrame(m_virtContext, position_)) {
    int err = virtGetErrorCode(m_virtContext);
    throw(vpException(vpException::fatalError,
                      "Error calling virtGetObservationFrame: error code %d", err));
  }
  else
  {
    for (int i=0; i<3; i++)
      translation[i] = position_[i];
    for (int i=0; i<4; i++)
      quaternion[i] = position_[3+i];

    vpThetaUVector thetau(quaternion);

    position.buildFrom(translation, thetau);
  }
  return position;
}

vpPoseVector vpVirtuose::getPhysicalPosition() const
{
  if (!m_is_init) {
    throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
  }

  vpPoseVector position;
  float position_[7];
  vpTranslationVector translation;
  vpQuaternionVector quaternion;

  if (virtGetPhysicalPosition(m_virtContext, position_)) {
    int err = virtGetErrorCode(m_virtContext);
    throw(vpException(vpException::fatalError,
                      "Error calling virtGetPhysicalPosition: error code %d", err));
  }
  else
  {
    for (int i=0; i<3; i++)
      translation[i] = position_[i];
    for (int i=0; i<4; i++)
      quaternion[i] = position_[3+i];

    vpThetaUVector thetau(quaternion);

    position.buildFrom(translation, thetau);
  }
  return position;
}

vpColVector vpVirtuose::getPhysicalVelocity() const
{
  if (!m_is_init) {
    throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
  }

  vpColVector vel(6,0);
  float speed[6];
  if (virtGetPhysicalSpeed(m_virtContext, speed)) {
    int err = virtGetErrorCode(m_virtContext);
    throw(vpException(vpException::fatalError, "Error calling virtGetPhysicalSpeed: error code %s",
                      virtGetErrorMessage(err)));
  }
  for(unsigned int i=0; i<6; i++)
    vel[i] = speed[i];
  return vel;
}

vpPoseVector vpVirtuose::getPosition() const
{
  if (!m_is_init) {
    throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
  }

  vpPoseVector position;
  float position_[7];
  vpTranslationVector translation;
  vpQuaternionVector quaternion;

  if (virtGetPosition(m_virtContext, position_)) {
    int err = virtGetErrorCode(m_virtContext);
    throw(vpException(vpException::fatalError,
                      "Error calling virtGetPosition: error code %d", err));
  }
  else
  {
    for (int i=0; i<3; i++)
      translation[i] = position_[i];
    for (int i=0; i<4; i++)
      quaternion[i] = position_[3+i];

    vpThetaUVector thetau(quaternion);

    position.buildFrom(translation, thetau);
  }
  return position;
}

bool vpVirtuose::getPower () const
{
  if (!m_is_init) {
    throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
  }

  int power;
  virtGetPowerOn(m_virtContext, &power);
  return (power ? true : false);
}

vpColVector vpVirtuose::getVelocity() const
{
  if (!m_is_init) {
    throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
  }

  vpColVector vel(6,0);
  float speed[6];
  if (virtGetSpeed(m_virtContext, speed)) {
    int err = virtGetErrorCode(m_virtContext);
    throw(vpException(vpException::fatalError, "Cannot get haptic device velocity: %s",
                      virtGetErrorMessage(err)));
  }
  for(unsigned int i=0; i<6; i++)
    vel[i] = speed[i];
  return vel;
}

void vpVirtuose::init()
{
  if (! m_is_init) {
    m_virtContext = virtOpen(m_ip.c_str());

    if (m_virtContext == NULL) {
      int err = virtGetErrorCode(m_virtContext);
      throw(vpException(vpException::fatalError, "Cannot open haptic device: %s",
                        virtGetErrorMessage(err)));
    }

    if (virtGetControlerVersion(m_virtContext, &m_ctrlMajorVersion, &m_ctrlMinorVersion)) {
      int err = virtGetErrorCode(m_virtContext);
      throw(vpException(vpException::fatalError, "Cannot get haptic device controller version: %s",
                        virtGetErrorMessage(err)));
    }

    if (m_verbose) {
      std::cout << "Controller version: " << m_ctrlMajorVersion << "." << m_ctrlMinorVersion << std::endl;
    }

    if (virtSetCommandType(m_virtContext, m_typeCommand)) {
      int err = virtGetErrorCode(m_virtContext);
      throw(vpException(vpException::fatalError, "Cannot set haptic device command type: %s",
                        virtGetErrorMessage(err)));
    }

    if (virtSetTimeStep(m_virtContext, m_period)) {
      int err = virtGetErrorCode(m_virtContext);
      throw(vpException(vpException::fatalError,
                        "Error calling virtSetTimeStep: error code %d", err));
    }

    m_is_init = true;
  }
}

void vpVirtuose::setArticularForce (const vpColVector &articularForce)
{
  init();

  if (articularForce.size() != 6) {
    throw(vpException(vpException::dimensionError,
                      "Cannot apply an articular force feedback (dim %d) to the haptic device that is not 6-dimension",
                      articularForce.size()));
  }

  float articular_force[6];
  for(unsigned int i=0; i<6; i++)
    articular_force[i] = (float)articularForce[i];

  if (virtSetArticularForce(m_virtContext, articular_force)) {
    int err = virtGetErrorCode(m_virtContext);
    throw(vpException(vpException::fatalError,
                      "Error calling virtSetArticularForce: error code %d", err));
  }
}

void vpVirtuose::setArticularPosition (const vpColVector &articularPosition)
{
  init();

  if (articularPosition.size() != 6) {
    throw(vpException(vpException::dimensionError,
                      "Cannot send an articular position command (dim %d) to the haptic device that is not 6-dimension",
                      articularPosition.size()));
  }

  float articular_position[6];
  for(unsigned int i=0; i<6; i++)
    articular_position[i] = (float)articularPosition[i];

  if (virtSetArticularPosition(m_virtContext, articular_position)) {
    int err = virtGetErrorCode(m_virtContext);
    throw(vpException(vpException::fatalError,
                      "Error calling virtSetArticularPosition: error code %d", err));
  }
}

void vpVirtuose::setArticularVelocity (const vpColVector &articularVelocity)
{
  init();

  if (articularVelocity.size() != 6) {
    throw(vpException(vpException::dimensionError,
                      "Cannot send an articular velocity command (dim %d) to the haptic device that is not 6-dimension",
                      articularVelocity.size()));
  }

  float articular_velocity[6];
  for(unsigned int i=0; i<6; i++)
    articular_velocity[i] = (float)articularVelocity[i];

  if (virtSetArticularSpeed(m_virtContext, articular_velocity)) {
    int err = virtGetErrorCode(m_virtContext);
    throw(vpException(vpException::fatalError,
                      "Error calling virtSetArticularVelocity: error code %d", err));
  }
}

void vpVirtuose::setBaseFrame (const vpPoseVector &position)
{
  init();

  float position_[7];
  vpTranslationVector translation;
  vpQuaternionVector quaternion;

  position.extract(translation);
  position.extract(quaternion);

  for (int i=0; i<3; i++)
    position_[i] = (float)translation[i];
  for (int i=0; i<4; i++)
    position_[3 + i] = (float)quaternion[i];

  if (virtSetBaseFrame(m_virtContext, position_)) {
    int err = virtGetErrorCode(m_virtContext);
    throw(vpException(vpException::fatalError,
                      "Error calling virtSetBaseFrame: error code %d", err));
  }
}

void vpVirtuose::setCommandType(const VirtCommandType &type)
{
  init();

  if (m_typeCommand != type) {
    m_typeCommand = type;

    if (virtSetCommandType(m_virtContext, m_typeCommand)) {
      int err = virtGetErrorCode(m_virtContext);
      throw(vpException(vpException::fatalError,
                        "Error calling virtSetCommandType: error code %d", err));
    }
  }
}

void vpVirtuose::setForce (const vpColVector &force)
{
  init();

  if (force.size() != 6) {
    throw(vpException(vpException::dimensionError,
                      "Cannot apply a force feedback (dim %d) to the haptic device that is not 6-dimension",
                      force.size()));
  }

  float virtforce[6];
  for(unsigned int i=0; i<6; i++)
    virtforce[i] = (float)force[i];

  if (virtSetForce(m_virtContext, virtforce)) {
    int err = virtGetErrorCode(m_virtContext);
    throw(vpException(vpException::fatalError,
                      "Error calling virtSetForce: error code %d", err));
  }
}

void vpVirtuose::setForceFactor (const float &forceFactor)
{
  init();

  if (virtSetForceFactor(m_virtContext, forceFactor)) {
    int err = virtGetErrorCode(m_virtContext);
    throw(vpException(vpException::fatalError,
                      "Error calling virtSetForceFactor: error code %d", err));
  }
}

void vpVirtuose::setIndexingMode (const VirtIndexingType &type)
{
  init();

  if (m_indexType != type){
    m_indexType = type;

    if (virtSetIndexingMode(m_virtContext, m_indexType)) {
      int err = virtGetErrorCode(m_virtContext);
      throw(vpException(vpException::fatalError,
                        "Error calling setIndexingMode: error code %d", err));
    }
  }
}

void vpVirtuose::setObservationFrame (const vpPoseVector &position)
{
  init();

  float position_[7];
  vpTranslationVector translation;
  vpQuaternionVector quaternion;

  position.extract(translation);
  position.extract(quaternion);

  for (int i=0; i<3; i++)
    position_[i] = (float)translation[i];
  for (int i=0; i<4; i++)
    position_[3 + i] = (float)quaternion[i];

  if (virtSetObservationFrame(m_virtContext, position_)) {
    int err = virtGetErrorCode(m_virtContext);
    throw(vpException(vpException::fatalError,
                      "Error calling virtSetObservationFrame: error code %d", err));
  }
}

void vpVirtuose::setPeriodicFunction(VirtPeriodicFunction CallBackVirt)
{
  init();

  if (virtSetPeriodicFunction(m_virtContext, CallBackVirt, &m_period, this)) {
    int err = virtGetErrorCode(m_virtContext);
    throw(vpException(vpException::fatalError,
                      "Error calling virtSetPeriodicFunction: error code %d", err));
  }
}

void vpVirtuose::setPosition (vpPoseVector &position)
{
  init();

  float position_[7];
  vpTranslationVector translation;
  vpQuaternionVector quaternion;

  position.extract(translation);
  position.extract(quaternion);

  for (int i=0; i<3; i++)
    position_[i] = (float)translation[i];
  for (int i=0; i<4; i++)
    position_[3+i] = (float)quaternion[i];

  if (virtSetPosition(m_virtContext, position_)) {
    int err = virtGetErrorCode(m_virtContext);
    throw(vpException(vpException::fatalError,
                      "Error calling virtSetPosition: error code %d", err));
  }
}


void vpVirtuose::setPowerOff ()
{
  init();

  if (virtSetPowerOn(m_virtContext, 0)) {
    int err = virtGetErrorCode(m_virtContext);
    throw(vpException(vpException::fatalError,
                      "Error calling virtSetPowerOff: error code %d", err));
  }
}

void vpVirtuose::setPowerOn ()
{
  init();

  if (virtSetPowerOn(m_virtContext, 1)) {
    int err = virtGetErrorCode(m_virtContext);
    throw(vpException(vpException::fatalError,
                      "Error calling virtSetPowerOn: error code %d", err));
  }
}

void vpVirtuose::setSaturation(const float &forceLimit, const float &torqueLimit)
{
  init();

  if (virtSaturateTorque(m_virtContext, forceLimit, torqueLimit)) {
    int err = virtGetErrorCode(m_virtContext);
    throw(vpException(vpException::fatalError,
                      "Error calling virtSaturateTorque: error code %d", err));
  }
}

void vpVirtuose::setTimeStep (const float &timeStep)
{
  init();

  if (m_period != timeStep){
    m_period = timeStep;

    if (virtSetTimeStep(m_virtContext, m_period)) {
      int err = virtGetErrorCode(m_virtContext);
      throw(vpException(vpException::fatalError,
                        "Error calling virtSetTimeStep: error code %d", err));
    }
  }
}

void vpVirtuose::setVelocity (vpColVector &velocity)
{
  init();

  if (velocity.size() != 6) {
    throw(vpException(vpException::dimensionError,
                      "Cannot set a velocity vector (dim %d) that is not 6-dimension",
                      velocity.size()));
  }

  float speed[6];
  for(unsigned int i=0; i<6; i++)
    speed[i] = (float)velocity[i];

  if (virtSetSpeed(m_virtContext, speed)) {
    int err = virtGetErrorCode(m_virtContext);
    throw(vpException(vpException::fatalError,
                      "Error calling virtSetSpeed: error code %d", err));
  }
}

void vpVirtuose::setVelocityFactor (const float &velocityFactor)
{
  init();

  if (virtSetSpeedFactor(m_virtContext, velocityFactor)) {
    int err = virtGetErrorCode(m_virtContext);
    throw(vpException(vpException::fatalError, "Error calling setVelocityFactor: error code %d",
                      err));
  }
}

void vpVirtuose::startPeriodicFunction()
{
  init();

  if (virtStartLoop(m_virtContext)) {
    int err = virtGetErrorCode(m_virtContext);
    throw(vpException(vpException::fatalError, "Error calling startLoop: error code %d",
                      err));
  }
  else
    std::cout << "Haptic loop open." << std::endl;
}

void vpVirtuose::stopPeriodicFunction()
{
  init();

  if (virtStopLoop(m_virtContext)) {
    int err = virtGetErrorCode(m_virtContext);
    throw(vpException(vpException::fatalError, "Error calling stopLoop: error code %d",
                      err));
  }
  else
    std::cout << "Haptic loop closed." << std::endl;
}

#else
// Work around to avoid warning
void dummy_vpVirtuose() {};
#endif