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 MENDATORY 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 arround to avoid warning: libvisp_robot.a(vpRobotKinova.cpp.o) has
 // no symbols
void dummy_vpRobotKinova() {};
#endif