vpRobotKinova.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:
 * Interface for Kinova Jaco robot.
 *
 * Authors:
 * Fabien Spindler
 *
 *****************************************************************************/
 
#include <visp3/core/vpConfig.h>
 
#ifdef VISP_HAVE_JACOSDK
 
#include <visp3/core/vpIoTools.h>
#include <visp3/robot/vpRobotException.h>
 
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/robot/vpRobotKinova.h>
 
vpRobotKinova::vpRobotKinova()
  : m_eMc(), m_plugin_location("./"), m_verbose(false), m_plugin_loaded(false), m_devices_count(0),
    m_devices_list(NULL), m_active_device(-1), m_command_layer(CMD_LAYER_UNSET), m_command_layer_handle()
{
  init();
}
 
vpRobotKinova::~vpRobotKinova()
{
  closePlugin();
 
  if (m_devices_list) {
    delete[] m_devices_list;
  }
}
 
void vpRobotKinova::setDoF(unsigned int dof)
{
  if (dof == 4 || dof == 6 || dof == 7) {
    nDof = dof;
  } else {
    throw(vpException(vpException::fatalError,
                      "Unsupported Kinova Jaco degrees of freedom: %d. Possible values are 4, 6 or 7.", dof));
  }
}
 
void vpRobotKinova::init()
{
  // If you want to control the robot in Cartesian in a tool frame, set the corresponding transformation in m_eMc
  // that is set to identity by default in the constructor.
 
  maxRotationVelocity = maxRotationVelocityDefault;
  maxTranslationVelocity = maxTranslationVelocityDefault;
 
  // Set here the robot degrees of freedom number
  nDof = 6; // Jaco2 default dof = 6
 
  m_devices_list = new KinovaDevice[MAX_KINOVA_DEVICE];
}
 
/*
 
  At least one of these function has to be implemented to control the robot with a
  Cartesian velocity:
  - get_eJe()
  - get_fJe()
 
*/
 
void vpRobotKinova::get_eJe(vpMatrix &eJe)
{
  if (!m_plugin_loaded) {
    throw(vpException(vpException::fatalError, "Jaco SDK plugin not loaded"));
  }
  if (!m_devices_count) {
    throw(vpException(vpException::fatalError, "No Kinova robot found"));
  }
 
  (void)eJe;
  std::cout << "Not implemented ! " << std::endl;
}
 
void vpRobotKinova::get_fJe(vpMatrix &fJe)
{
  if (!m_plugin_loaded) {
    throw(vpException(vpException::fatalError, "Jaco SDK plugin not loaded"));
  }
  if (!m_devices_count) {
    throw(vpException(vpException::fatalError, "No Kinova robot found"));
  }
 
  (void)fJe;
  std::cout << "Not implemented ! " << std::endl;
}
 
/*
 
  At least one of these function has to be implemented to control the robot:
  - setCartVelocity()
  - setJointVelocity()
 
*/
 
void vpRobotKinova::setCartVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &v)
{
  if (v.size() != 6) {
    throw(vpException(vpException::fatalError,
                      "Cannot send a velocity twist vector in tool frame that is not 6-dim (%d)", v.size()));
  }
 
  vpColVector v_e;   // This is the velocity that the robot is able to apply in the end-effector frame
  vpColVector v_c;   // This is the velocity that the robot is able to apply in the camera frame
  vpColVector v_mix; // This is the velocity that the robot is able to apply in the mix frame
  switch (frame) {
  case vpRobot::CAMERA_FRAME: {
    // We have to transform the requested velocity in the end-effector frame.
    // Knowing that the constant transformation between the tool frame and the end-effector frame obtained
    // by extrinsic calibration is set in m_eMc we can compute the velocity twist matrix eVc that transform
    // a velocity twist from tool (or camera) frame into end-effector frame
    vpVelocityTwistMatrix eVc(m_eMc); // GET IT FROM CAMERA EXTRINSIC CALIBRATION FILE
 
    // Input velocity is expressed in camera or tool frame
    v_c = v;
 
    // Tranform velocity in end-effector
    v_e = eVc * v_c;
 
    // Convert end-effector translation velocity in base frame, rotation velocity is unchanged
    vpColVector p_e;
    getPosition(vpRobot::END_EFFECTOR_FRAME, p_e);
    vpRxyzVector bre(p_e[3], p_e[4], p_e[5]);
    vpRotationMatrix bRe(bre);
    vpMatrix bVe(6, 6, 0);
    bVe.eye();
    bVe.insert(bRe, 0, 0);
    v_mix = bVe * v_e;
 
    TrajectoryPoint pointToSend;
    pointToSend.InitStruct();
    // We specify that this point will be used an angular (joint by joint) velocity vector
    pointToSend.Position.Type = CARTESIAN_VELOCITY;
    pointToSend.Position.HandMode = HAND_NOMOVEMENT;
 
    pointToSend.Position.CartesianPosition.X = static_cast<float>(v_mix[0]);
    pointToSend.Position.CartesianPosition.Y = static_cast<float>(v_mix[1]);
    pointToSend.Position.CartesianPosition.Z = static_cast<float>(v_mix[2]);
    pointToSend.Position.CartesianPosition.ThetaX = static_cast<float>(v_mix[3]);
    pointToSend.Position.CartesianPosition.ThetaY = static_cast<float>(v_mix[4]);
    pointToSend.Position.CartesianPosition.ThetaZ = static_cast<float>(v_mix[5]);
 
    KinovaSetCartesianControl(); // Not sure that this function is useful here
 
    KinovaSendBasicTrajectory(pointToSend);
    break;
  }
 
  case vpRobot::END_EFFECTOR_FRAME: {
    // Input velocity is expressed in end-effector
    v_e = v;
 
    // Convert end-effector translation velocity in base frame, rotation velocity is unchanged
    vpColVector p_e;
    getPosition(vpRobot::END_EFFECTOR_FRAME, p_e);
    vpRxyzVector bre(p_e[3], p_e[4], p_e[5]);
    vpRotationMatrix bRe(bre);
    vpMatrix bVe(6, 6, 0);
    bVe.eye();
    bVe.insert(bRe, 0, 0);
    v_mix = bVe * v_e;
 
    TrajectoryPoint pointToSend;
    pointToSend.InitStruct();
    // We specify that this point will be used an angular (joint by joint) velocity vector
    pointToSend.Position.Type = CARTESIAN_VELOCITY;
    pointToSend.Position.HandMode = HAND_NOMOVEMENT;
 
    pointToSend.Position.CartesianPosition.X = static_cast<float>(v_mix[0]);
    pointToSend.Position.CartesianPosition.Y = static_cast<float>(v_mix[1]);
    pointToSend.Position.CartesianPosition.Z = static_cast<float>(v_mix[2]);
    pointToSend.Position.CartesianPosition.ThetaX = static_cast<float>(v_mix[3]);
    pointToSend.Position.CartesianPosition.ThetaY = static_cast<float>(v_mix[4]);
    pointToSend.Position.CartesianPosition.ThetaZ = static_cast<float>(v_mix[5]);
 
    KinovaSetCartesianControl(); // Not sure that this function is useful here
 
    KinovaSendBasicTrajectory(pointToSend);
    break;
  }
 
  case vpRobot::MIXT_FRAME: {
 
    // Convert end-effector translation velocity in base frame, rotation velocity is unchanged
    vpColVector p_e;
    getPosition(vpRobot::END_EFFECTOR_FRAME, p_e);
    vpRxyzVector bre(p_e[3], p_e[4], p_e[5]);
    vpRotationMatrix bRe(bre);
    std::cout << "rotation matrix from base to endeffector is bRe : " << std::endl;
    std::cout << "bRe:\n" << bRe << std::endl;
    vpMatrix bVe(6, 6, 0);
    bVe.eye();
    bVe.insert(bRe, 0, 0);
    v_e = v;
    // vpColVector bVe;
    vpColVector v_mix = bVe * v_e;
 
    TrajectoryPoint pointToSend;
    pointToSend.InitStruct();
    // We specify that this point will be used an angular (joint by joint) velocity vector
    pointToSend.Position.Type = CARTESIAN_VELOCITY;
    pointToSend.Position.HandMode = HAND_NOMOVEMENT;
 
    pointToSend.Position.CartesianPosition.X = static_cast<float>(v_mix[0]);
    pointToSend.Position.CartesianPosition.Y = static_cast<float>(v_mix[1]);
    pointToSend.Position.CartesianPosition.Z = static_cast<float>(v_mix[2]);
    pointToSend.Position.CartesianPosition.ThetaX = static_cast<float>(v_e[3]);
    pointToSend.Position.CartesianPosition.ThetaY = static_cast<float>(v_e[4]);
    pointToSend.Position.CartesianPosition.ThetaZ = static_cast<float>(v_e[5]);
 
    KinovaSetCartesianControl(); // Not sure that this function is useful here
    KinovaSendBasicTrajectory(pointToSend);
    break;
  }
  case vpRobot::REFERENCE_FRAME:
  case vpRobot::JOINT_STATE:
    // Out of the scope
    break;
  }
}
 
void vpRobotKinova::setJointVelocity(const vpColVector &qdot)
{
  if (qdot.size() != static_cast<unsigned int>(nDof)) {
    throw(vpException(vpException::dimensionError,
                      "Cannot apply a %d-dim joint velocity vector to the Jaco robot configured with %d DoF",
                      qdot.size(), nDof));
  }
  TrajectoryPoint pointToSend;
  pointToSend.InitStruct();
  // We specify that this point will be used an angular (joint by joint) velocity vector
  pointToSend.Position.Type = ANGULAR_VELOCITY;
  pointToSend.Position.HandMode = HAND_NOMOVEMENT;
  switch (nDof) {
  case 7: {
    pointToSend.Position.Actuators.Actuator7 = static_cast<float>(vpMath::deg(qdot[6]));
    pointToSend.Position.Actuators.Actuator6 = static_cast<float>(vpMath::deg(qdot[5]));
    pointToSend.Position.Actuators.Actuator5 = static_cast<float>(vpMath::deg(qdot[4]));
    pointToSend.Position.Actuators.Actuator4 = static_cast<float>(vpMath::deg(qdot[3]));
    pointToSend.Position.Actuators.Actuator3 = static_cast<float>(vpMath::deg(qdot[2]));
    pointToSend.Position.Actuators.Actuator2 = static_cast<float>(vpMath::deg(qdot[1]));
    pointToSend.Position.Actuators.Actuator1 = static_cast<float>(vpMath::deg(qdot[0]));
    break;
  }
  case 6: {
    pointToSend.Position.Actuators.Actuator6 = static_cast<float>(vpMath::deg(qdot[5]));
    pointToSend.Position.Actuators.Actuator5 = static_cast<float>(vpMath::deg(qdot[4]));
    pointToSend.Position.Actuators.Actuator4 = static_cast<float>(vpMath::deg(qdot[3]));
    pointToSend.Position.Actuators.Actuator3 = static_cast<float>(vpMath::deg(qdot[2]));
    pointToSend.Position.Actuators.Actuator2 = static_cast<float>(vpMath::deg(qdot[1]));
    pointToSend.Position.Actuators.Actuator1 = static_cast<float>(vpMath::deg(qdot[0]));
    break;
  }
  case 4: {
    pointToSend.Position.Actuators.Actuator4 = static_cast<float>(vpMath::deg(qdot[3]));
    pointToSend.Position.Actuators.Actuator3 = static_cast<float>(vpMath::deg(qdot[2]));
    pointToSend.Position.Actuators.Actuator2 = static_cast<float>(vpMath::deg(qdot[1]));
    pointToSend.Position.Actuators.Actuator1 = static_cast<float>(vpMath::deg(qdot[0]));
    break;
  }
  default:
    throw(vpException(vpException::fatalError, "Jaco robot non supported %d DoF", nDof));
  }
 
  KinovaSetAngularControl(); // Not sure that this function is useful here
 
  KinovaSendBasicTrajectory(pointToSend);
}
 
void vpRobotKinova::setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
{
  if (!m_plugin_loaded) {
    throw(vpException(vpException::fatalError, "Jaco SDK plugin not loaded"));
  }
  if (!m_devices_count) {
    throw(vpException(vpException::fatalError, "No Kinova robot found"));
  }
  if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState()) {
    throw vpRobotException(vpRobotException::wrongStateError,
                           "Cannot send a velocity to the robot. "
                           "Call setRobotState(vpRobot::STATE_VELOCITY_CONTROL) once before "
                           "entering your control loop.");
  }
 
  vpColVector vel_sat(6);
 
  // Velocity saturation
  switch (frame) {
  // Saturation in cartesian space
  case vpRobot::TOOL_FRAME:
  case vpRobot::REFERENCE_FRAME:
  case vpRobot::END_EFFECTOR_FRAME:
  case vpRobot::MIXT_FRAME: {
    if (vel.size() != 6) {
      throw(vpException(vpException::dimensionError,
                        "Cannot apply a Cartesian velocity that is not a 6-dim vector (%d)", vel.size()));
    }
    vpColVector vel_max(6);
 
    for (unsigned int i = 0; i < 3; i++)
      vel_max[i] = getMaxTranslationVelocity();
    for (unsigned int i = 3; i < 6; i++)
      vel_max[i] = getMaxRotationVelocity();
 
    vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
 
    setCartVelocity(frame, vel_sat);
    break;
  }
 
  // Saturation in joint space
  case vpRobot::JOINT_STATE: {
    if (vel.size() != static_cast<size_t>(nDof)) {
      throw(vpException(vpException::dimensionError, "Cannot apply a joint velocity that is not a %-dim vector (%d)",
                        nDof, vel.size()));
    }
    vpColVector vel_max(vel.size());
 
    // Since the robot has only rotation axis all the joint max velocities are set to getMaxRotationVelocity()
    vel_max = getMaxRotationVelocity();
 
    vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
 
    setJointVelocity(vel_sat);
  }
  }
}
 
/*
 
  THESE FUNCTIONS ARE NOT MANDATORY BUT ARE USUALLY USEFUL
 
*/
 
void vpRobotKinova::getJointPosition(vpColVector &q)
{
  AngularPosition currentCommand;
 
  // We get the actual angular command of the robot. Values are in deg
  KinovaGetAngularCommand(currentCommand);
 
  q.resize(nDof);
  switch (nDof) {
  case 7: {
    q[6] = vpMath::rad(currentCommand.Actuators.Actuator7);
    q[5] = vpMath::rad(currentCommand.Actuators.Actuator6);
    q[4] = vpMath::rad(currentCommand.Actuators.Actuator5);
    q[3] = vpMath::rad(currentCommand.Actuators.Actuator4);
    q[2] = vpMath::rad(currentCommand.Actuators.Actuator3);
    q[1] = vpMath::rad(currentCommand.Actuators.Actuator2);
    q[0] = vpMath::rad(currentCommand.Actuators.Actuator1);
    break;
  }
  case 6: {
    q[5] = vpMath::rad(currentCommand.Actuators.Actuator6);
    q[4] = vpMath::rad(currentCommand.Actuators.Actuator5);
    q[3] = vpMath::rad(currentCommand.Actuators.Actuator4);
    q[2] = vpMath::rad(currentCommand.Actuators.Actuator3);
    q[1] = vpMath::rad(currentCommand.Actuators.Actuator2);
    q[0] = vpMath::rad(currentCommand.Actuators.Actuator1);
    break;
  }
  case 4: {
    q[3] = vpMath::rad(currentCommand.Actuators.Actuator4);
    q[2] = vpMath::rad(currentCommand.Actuators.Actuator3);
    q[1] = vpMath::rad(currentCommand.Actuators.Actuator2);
    q[0] = vpMath::rad(currentCommand.Actuators.Actuator1);
    break;
  }
  default:
    throw(vpException(vpException::fatalError, "Jaco robot non supported %d DoF", nDof));
  }
}
 
void vpRobotKinova::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
{
  if (!m_plugin_loaded) {
    throw(vpException(vpException::fatalError, "Jaco SDK plugin not loaded"));
  }
  if (!m_devices_count) {
    throw(vpException(vpException::fatalError, "No Kinova robot found"));
  }
 
  if (frame == JOINT_STATE) {
    getJointPosition(position);
  } else if (frame == END_EFFECTOR_FRAME) {
    CartesianPosition currentCommand;
    // We get the actual cartesian position of the robot
    KinovaGetCartesianCommand(currentCommand);
    position.resize(6);
    position[0] = currentCommand.Coordinates.X;
    position[1] = currentCommand.Coordinates.Y;
    position[2] = currentCommand.Coordinates.Z;
    position[3] = currentCommand.Coordinates.ThetaX;
    position[4] = currentCommand.Coordinates.ThetaY;
    position[5] = currentCommand.Coordinates.ThetaZ;
  } else {
    std::cout << "Not implemented ! " << std::endl;
  }
}
 
void vpRobotKinova::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &pose)
{
  if (frame == JOINT_STATE) {
    throw(vpException(vpException::fatalError, "Cannot get Jaco joint position as a pose vector"));
  }
 
  vpColVector position;
  getPosition(frame, position);
 
  vpRxyzVector rxyz; // reference frame to end-effector frame rotations
  // Update the transformation between reference frame and end-effector frame
  for (unsigned int i = 0; i < 3; i++) {
    pose[i] = position[i];     // tx, ty, tz
    rxyz[i] = position[i + 3]; // ry, ry, rz
  }
  vpThetaUVector tu(rxyz);
  for (unsigned int i = 0; i < 3; i++) {
    pose[i + 3] = tu[i]; // tux, tuy, tuz
  }
}
 
void vpRobotKinova::setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)
{
  if (!m_plugin_loaded) {
    throw(vpException(vpException::fatalError, "Jaco SDK plugin not loaded"));
  }
  if (!m_devices_count) {
    throw(vpException(vpException::fatalError, "No Kinova robot found"));
  }
  if (frame == vpRobot::JOINT_STATE) {
    if (static_cast<int>(q.size()) != nDof) {
      throw(vpException(vpException::fatalError,
                        "Cannot move robot to a joint position of dim %u that is not a %d-dim vector", q.size(), nDof));
    }
    TrajectoryPoint pointToSend;
    pointToSend.InitStruct();
    // We specify that this point will be an angular(joint by joint) position.
    pointToSend.Position.Type = ANGULAR_POSITION;
    pointToSend.Position.HandMode = HAND_NOMOVEMENT;
    switch (nDof) {
    case 7: {
      pointToSend.Position.Actuators.Actuator7 = static_cast<float>(vpMath::deg(q[6]));
      pointToSend.Position.Actuators.Actuator6 = static_cast<float>(vpMath::deg(q[5]));
      pointToSend.Position.Actuators.Actuator5 = static_cast<float>(vpMath::deg(q[4]));
      pointToSend.Position.Actuators.Actuator4 = static_cast<float>(vpMath::deg(q[3]));
      pointToSend.Position.Actuators.Actuator3 = static_cast<float>(vpMath::deg(q[2]));
      pointToSend.Position.Actuators.Actuator2 = static_cast<float>(vpMath::deg(q[1]));
      pointToSend.Position.Actuators.Actuator1 = static_cast<float>(vpMath::deg(q[0]));
      break;
    }
    case 6: {
      pointToSend.Position.Actuators.Actuator6 = static_cast<float>(vpMath::deg(q[5]));
      pointToSend.Position.Actuators.Actuator5 = static_cast<float>(vpMath::deg(q[4]));
      pointToSend.Position.Actuators.Actuator4 = static_cast<float>(vpMath::deg(q[3]));
      pointToSend.Position.Actuators.Actuator3 = static_cast<float>(vpMath::deg(q[2]));
      pointToSend.Position.Actuators.Actuator2 = static_cast<float>(vpMath::deg(q[1]));
      pointToSend.Position.Actuators.Actuator1 = static_cast<float>(vpMath::deg(q[0]));
      break;
    }
    case 4: {
      pointToSend.Position.Actuators.Actuator4 = static_cast<float>(vpMath::deg(q[3]));
      pointToSend.Position.Actuators.Actuator3 = static_cast<float>(vpMath::deg(q[2]));
      pointToSend.Position.Actuators.Actuator2 = static_cast<float>(vpMath::deg(q[1]));
      pointToSend.Position.Actuators.Actuator1 = static_cast<float>(vpMath::deg(q[0]));
      break;
    }
    default:
      throw(vpException(vpException::fatalError, "Jaco robot non supported %d DoF", nDof));
    }
 
    KinovaSetAngularControl(); // Not sure that this function is useful here
 
    if (m_verbose) {
      std::cout << "Move robot to joint position [rad rad rad rad rad rad]: " << q.t() << std::endl;
    }
    KinovaSendBasicTrajectory(pointToSend);
  } else if (frame == vpRobot::END_EFFECTOR_FRAME) {
    if (q.size() != 6) {
      throw(vpException(vpException::fatalError,
                        "Cannot move robot to cartesian position of dim %d that is not a 6-dim vector", q.size()));
    }
    TrajectoryPoint pointToSend;
    pointToSend.InitStruct();
    // We specify that this point will be an angular(joint by joint) position.
    pointToSend.Position.Type = CARTESIAN_POSITION;
    pointToSend.Position.HandMode = HAND_NOMOVEMENT;
    pointToSend.Position.CartesianPosition.X = static_cast<float>(q[0]);
    pointToSend.Position.CartesianPosition.Y = static_cast<float>(q[1]);
    pointToSend.Position.CartesianPosition.Z = static_cast<float>(q[2]);
    pointToSend.Position.CartesianPosition.ThetaX = static_cast<float>(q[3]);
    pointToSend.Position.CartesianPosition.ThetaY = static_cast<float>(q[4]);
    pointToSend.Position.CartesianPosition.ThetaZ = static_cast<float>(q[5]);
 
    KinovaSetCartesianControl(); // Not sure that this function is useful here
 
    if (m_verbose) {
      std::cout << "Move robot to cartesian position [m m m rad rad rad]: " << q.t() << std::endl;
    }
    KinovaSendBasicTrajectory(pointToSend);
 
  } else {
    throw(vpException(vpException::fatalError,
                      "Cannot move robot to a cartesian position. Only joint positioning is implemented"));
  }
}
 
void vpRobotKinova::getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &q)
{
  if (!m_plugin_loaded) {
    throw(vpException(vpException::fatalError, "Jaco SDK plugin not loaded"));
  }
  if (!m_devices_count) {
    throw(vpException(vpException::fatalError, "No Kinova robot found"));
  }
 
  (void)frame;
  (void)q;
  std::cout << "Not implemented ! " << std::endl;
}
 
void vpRobotKinova::loadPlugin()
{
  if (m_command_layer == CMD_LAYER_UNSET) {
    throw(vpException(vpException::fatalError, "Kinova robot command layer unset"));
  }
  if (m_plugin_loaded) {
    closePlugin();
  }
#ifdef __linux__
  // We load the API
  std::string plugin_name = (m_command_layer == CMD_LAYER_USB) ? std::string("Kinova.API.USBCommandLayerUbuntu.so")
                                                               : std::string("Kinova.API.EthCommandLayerUbuntu.so");
  std::string plugin = vpIoTools::createFilePath(m_plugin_location, plugin_name);
  if (m_verbose) {
    std::cout << "Load plugin: \"" << plugin << "\"" << std::endl;
  }
  m_command_layer_handle = dlopen(plugin.c_str(), RTLD_NOW | RTLD_GLOBAL);
 
  std::string prefix = (m_command_layer == CMD_LAYER_USB) ? std::string("") : std::string("Ethernet_");
  // We load the functions from the library
  KinovaCloseAPI = (int (*)())dlsym(m_command_layer_handle, (prefix + std::string("CloseAPI")).c_str());
  KinovaGetAngularCommand =
      (int (*)(AngularPosition &))dlsym(m_command_layer_handle, (prefix + std::string("GetAngularCommand")).c_str());
  KinovaGetCartesianCommand = (int (*)(CartesianPosition &))dlsym(
      m_command_layer_handle, (prefix + std::string("GetCartesianCommand")).c_str());
  KinovaGetDevices = (int (*)(KinovaDevice devices[MAX_KINOVA_DEVICE], int &result))dlsym(
      m_command_layer_handle, (prefix + std::string("GetDevices")).c_str());
  KinovaInitAPI = (int (*)())dlsym(m_command_layer_handle, (prefix + std::string("InitAPI")).c_str());
  KinovaInitFingers = (int (*)())dlsym(m_command_layer_handle, (prefix + std::string("InitFingers")).c_str());
  KinovaMoveHome = (int (*)())dlsym(m_command_layer_handle, (prefix + std::string("MoveHome")).c_str());
  KinovaSendBasicTrajectory =
      (int (*)(TrajectoryPoint))dlsym(m_command_layer_handle, (prefix + std::string("SendBasicTrajectory")).c_str());
  KinovaSetActiveDevice =
      (int (*)(KinovaDevice devices))dlsym(m_command_layer_handle, (prefix + std::string("SetActiveDevice")).c_str());
  KinovaSetAngularControl =
      (int (*)())dlsym(m_command_layer_handle, (prefix + std::string("SetAngularControl")).c_str());
  KinovaSetCartesianControl =
      (int (*)())dlsym(m_command_layer_handle, (prefix + std::string("SetCartesianControl")).c_str());
#elif _WIN32
  // We load the API.
  std::string plugin_name = (m_command_layer == CMD_LAYER_USB) ? std::string("CommandLayerWindows.dll")
                                                               : std::string("CommandLayerEthernet.dll");
  std::string plugin = vpIoTools::createFilePath(m_plugin_location, plugin_name);
  if (m_verbose) {
    std::cout << "Load plugin: \"" << plugin << "\"" << std::endl;
  }
  m_command_layer_handle = LoadLibrary(TEXT(plugin.c_str()));
 
  // We load the functions from the library
  KinovaCloseAPI = (int (*)())GetProcAddress(m_command_layer_handle, "CloseAPI");
  KinovaGetAngularCommand = (int (*)(AngularPosition &))GetProcAddress(m_command_layer_handle, "GetAngularCommand");
  KinovaGetCartesianCommand =
      (int (*)(CartesianPosition &))GetProcAddress(m_command_layer_handle, "GetCartesianCommand");
  KinovaGetDevices =
      (int (*)(KinovaDevice[MAX_KINOVA_DEVICE], int &))GetProcAddress(m_command_layer_handle, "GetDevices");
  KinovaInitAPI = (int (*)())GetProcAddress(m_command_layer_handle, "InitAPI");
  KinovaInitFingers = (int (*)())GetProcAddress(m_command_layer_handle, "InitFingers");
  KinovaMoveHome = (int (*)())GetProcAddress(m_command_layer_handle, "MoveHome");
  KinovaSendBasicTrajectory = (int (*)(TrajectoryPoint))GetProcAddress(m_command_layer_handle, "SendBasicTrajectory");
  KinovaSetActiveDevice = (int (*)(KinovaDevice))GetProcAddress(m_command_layer_handle, "SetActiveDevice");
  KinovaSetAngularControl = (int (*)())GetProcAddress(m_command_layer_handle, "SetAngularControl");
  KinovaSetCartesianControl = (int (*)())GetProcAddress(m_command_layer_handle, "SetCartesianControl");
#endif
 
  // Verify that all functions has been loaded correctly
  if ((KinovaCloseAPI == NULL) || (KinovaGetAngularCommand == NULL) || (KinovaGetAngularCommand == NULL) ||
      (KinovaGetCartesianCommand == NULL) || (KinovaGetDevices == NULL) || (KinovaInitAPI == NULL) ||
      (KinovaInitFingers == NULL) || (KinovaMoveHome == NULL) || (KinovaSendBasicTrajectory == NULL) ||
      (KinovaSetActiveDevice == NULL) || (KinovaSetAngularControl == NULL) || (KinovaSetCartesianControl == NULL)) {
    throw(vpException(vpException::fatalError, "Cannot load plugin from \"%s\" folder", m_plugin_location.c_str()));
  }
  if (m_verbose) {
    std::cout << "Plugin successfully loaded" << std::endl;
  }
 
  m_plugin_loaded = true;
}
 
void vpRobotKinova::closePlugin()
{
  if (m_plugin_loaded) {
    if (m_verbose) {
      std::cout << "Close plugin" << std::endl;
    }
#ifdef __linux__
    dlclose(m_command_layer_handle);
#elif _WIN32
    FreeLibrary(m_command_layer_handle);
#endif
    m_plugin_loaded = false;
  }
}
 
void vpRobotKinova::homing()
{
  if (!m_plugin_loaded) {
    throw(vpException(vpException::fatalError, "Jaco SDK plugin not loaded"));
  }
  if (!m_devices_count) {
    throw(vpException(vpException::fatalError, "No Kinova robot found"));
  }
 
  if (m_verbose) {
    std::cout << "Move the robot to home position" << std::endl;
  }
  KinovaMoveHome();
}
 
int vpRobotKinova::connect()
{
  loadPlugin();
  if (!m_plugin_loaded) {
    throw(vpException(vpException::fatalError, "Jaco SDK plugin not loaded"));
  }
 
  int result = (*KinovaInitAPI)();
 
  if (m_verbose) {
    std::cout << "Initialization's result: " << result << std::endl;
  }
 
  m_devices_count = KinovaGetDevices(m_devices_list, result);
 
  if (m_verbose) {
    std::cout << "Found " << m_devices_count << " devices" << std::endl;
  }
 
  // By default set the first device as active
  setActiveDevice(0);
 
  // Initialize fingers
  KinovaInitFingers();
 
  return m_devices_count;
}
 
void vpRobotKinova::setActiveDevice(int device)
{
  if (!m_plugin_loaded) {
    throw(vpException(vpException::fatalError, "Jaco SDK plugin not loaded"));
  }
  if (!m_devices_count) {
    throw(vpException(vpException::fatalError, "No Kinova robot found"));
  }
  if (device < 0 || device >= m_devices_count) {
    throw(vpException(vpException::badValue, "Cannot set Kinova active device %d. Value should be in range [0, %d]",
                      device, m_devices_count - 1));
  }
  if (device != m_active_device) {
    m_active_device = device;
    KinovaSetActiveDevice(m_devices_list[m_active_device]);
  }
}
 
#elif !defined(VISP_BUILD_SHARED_LIBS)
// Work around to avoid warning: libvisp_robot.a(vpRobotKinova.cpp.o) has
// no symbols
void dummy_vpRobotKinova(){};
#endif