vpVirtuose.cpp

/****************************************************************************
 *
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2019 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 as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * 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_port("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), m_njoints(6)
{
  virtAPIVersion(&m_apiMajorVersion, &m_apiMinorVersion);
  std::cout << "API version: " << m_apiMajorVersion << "." << m_apiMinorVersion << std::endl;
}

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

vpVirtuose::~vpVirtuose()
{
  close();
}

void vpVirtuose::setIpAddressAndPort(const std::string &ip, int port)
{
  std::stringstream ss;
  ss << ip << "#" << port;

  m_ip_port = ss.str();
}

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


  float articular_position_[20] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};

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

  vpColVector articularPosition(m_njoints, 0);
  for (unsigned int i = 0; i < m_njoints; 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()."));
  }

  float articular_velocity_[20] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};

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

  vpColVector articularVelocity(m_njoints, 0);

  for (unsigned int i = 0; i < m_njoints; 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; }

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

  return m_njoints;
}


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_port.c_str());

    if (m_virtContext == NULL) {
      int err = virtGetErrorCode(m_virtContext);
      throw(vpException(vpException::fatalError, "Cannot open communication with haptic device using %s: %s. Check ip and port values",
                        m_ip_port.c_str(), 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));
    }

    // Update number of joints
    float articular_position_[20] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};

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

    m_njoints = 6; // At least 6 joints
    for (unsigned int i=m_njoints; i < 20; i++) {
      m_njoints = i;
      if (std::fabs(articular_position_[i]) <= std::numeric_limits<float>::epsilon()) {
        break;
      }
    }

    m_is_init = true;
  }
}

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

  if (articularForce.size() != m_njoints) {
    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 = new float [m_njoints];
  for (unsigned int i = 0; i < m_njoints; i++)
    articular_force[i] = (float)articularForce[i];

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

  delete [] articular_force;
}

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

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

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

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

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

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

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

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

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