vpVirtuose.cpp

/****************************************************************************
 *
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2023 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 https://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.
 *
*****************************************************************************/
 
#include <visp3/core/vpException.h>
#include <visp3/robot/vpVirtuose.h>
 
#ifdef VISP_HAVE_VIRTUOSE
BEGIN_VISP_NAMESPACE
  vpVirtuose::vpVirtuose()
  : m_virtContext(nullptr), 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 != nullptr) {
    virtClose(m_virtContext);
    m_virtContext = nullptr;
  }
}
 
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 == nullptr) {
      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;
}
END_VISP_NAMESPACE
#else
// Work around to avoid warning
void dummy_vpVirtuose() { }
#endif