vpRobotFranka.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 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:
 * Interface for the Franka robot.
 *
 * Authors:
 * Fabien Spindler
 *
 *****************************************************************************/

#include <visp3/core/vpConfig.h>

#ifdef VISP_HAVE_FRANKA

#include <visp3/robot/vpRobotException.h>
#include <visp3/robot/vpRobotFranka.h>
#include <visp3/core/vpIoTools.h>

#include "vpJointPosTrajGenerator_impl.h"
#include "vpJointVelTrajGenerator_impl.h"
#include "vpForceTorqueGenerator_impl.h"

vpRobotFranka::vpRobotFranka()
  : vpRobot(), m_handler(NULL), m_gripper(NULL), m_model(NULL), m_positionningVelocity(20.),
    m_velControlThread(), m_velControlThreadIsRunning(false), m_velControlThreadStopAsked(false),
    m_dq_des(), m_v_cart_des(),
    m_ftControlThread(), m_ftControlThreadIsRunning(false), m_ftControlThreadStopAsked(false),
    m_tau_J_des(), m_ft_cart_des(),
    m_q_min(), m_q_max(), m_dq_max(), m_ddq_max(), m_robot_state(),
    m_mutex(), m_eMc(), m_log_folder(), m_franka_address()
{
  init();
}

vpRobotFranka::vpRobotFranka(const std::string &franka_address, franka::RealtimeConfig realtime_config)
  : vpRobot(), m_handler(NULL), m_gripper(NULL), m_model(NULL), m_positionningVelocity(20.),
    m_velControlThread(), m_velControlThreadIsRunning(false), m_velControlThreadStopAsked(false),
    m_dq_des(), m_v_cart_des(),
    m_ftControlThread(), m_ftControlThreadIsRunning(false), m_ftControlThreadStopAsked(false),
    m_tau_J_des(), m_ft_cart_des(),
    m_q_min(), m_q_max(), m_dq_max(), m_ddq_max(), m_robot_state(),
    m_mutex(), m_eMc(),m_log_folder(), m_franka_address()
{
  init();
  connect(franka_address, realtime_config);
}

void vpRobotFranka::init()
{
  nDof = 7;

  m_q_min   = std::array<double, 7> {-2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973};
  m_q_max   = std::array<double, 7> {2.8973, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973};
  m_dq_max  = std::array<double, 7> {2.1750, 2.1750, 2.1750, 2.1750, 2.6100, 2.6100, 2.6100};
  m_ddq_max = std::array<double, 7> {15.0, 7.5, 10.0, 12.5, 15.0, 20.0, 20.0};
}

vpRobotFranka::~vpRobotFranka()
{
  setRobotState(vpRobot::STATE_STOP);

  if (m_handler)
    delete m_handler;

  if (m_gripper) {
    std::cout << "Grasped object, will release it now." << std::endl;
    m_gripper->stop();
    delete m_gripper;
  }

  if (m_model) {
    delete m_model;
  }
}

void vpRobotFranka::connect(const std::string &franka_address, franka::RealtimeConfig realtime_config)
{
  init();
  if(franka_address.empty()) {
    throw(vpException(vpException::fatalError, "Cannot connect Franka robot: IP/hostname is not set"));
  }
  if (m_handler)
    delete m_handler;

  m_franka_address = franka_address;
  m_handler = new franka::Robot(franka_address, realtime_config);

  std::array<double, 7> lower_torque_thresholds_nominal{
      {25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.}};
  std::array<double, 7> upper_torque_thresholds_nominal{
      {35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0}};
  std::array<double, 7> lower_torque_thresholds_acceleration{
      {25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.0}};
  std::array<double, 7> upper_torque_thresholds_acceleration{
      {35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0}};
  std::array<double, 6> lower_force_thresholds_nominal{{30.0, 30.0, 30.0, 25.0, 25.0, 25.0}};
  std::array<double, 6> upper_force_thresholds_nominal{{40.0, 40.0, 40.0, 35.0, 35.0, 35.0}};
  std::array<double, 6> lower_force_thresholds_acceleration{{30.0, 30.0, 30.0, 25.0, 25.0, 25.0}};
  std::array<double, 6> upper_force_thresholds_acceleration{{40.0, 40.0, 40.0, 35.0, 35.0, 35.0}};
  m_handler->setCollisionBehavior(
        lower_torque_thresholds_acceleration, upper_torque_thresholds_acceleration,
        lower_torque_thresholds_nominal, upper_torque_thresholds_nominal,
        lower_force_thresholds_acceleration, upper_force_thresholds_acceleration,
        lower_force_thresholds_nominal, upper_force_thresholds_nominal);

  m_handler->setJointImpedance({{3000, 3000, 3000, 2500, 2500, 2000, 2000}});
  m_handler->setCartesianImpedance({{3000, 3000, 3000, 300, 300, 300}});
#if (VISP_HAVE_FRANKA_VERSION < 0x000500)
  //  m_handler->setFilters(100, 100, 100, 100, 100);
  m_handler->setFilters(10, 10, 10, 10, 10);
#else
  // use franka::lowpassFilter() instead throw Franka::robot::control() with cutoff_frequency parameter
#endif
  if (m_model) {
    delete m_model;
  }
  m_model = new franka::Model(m_handler->loadModel());
}

void vpRobotFranka::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
{
  if (!m_handler) {
    throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
  }

  franka::RobotState robot_state = getRobotInternalState();
  vpColVector q(7);
  for (int i=0; i < 7; i++) {
    q[i] = robot_state.q[i];
  }

  switch(frame) {
  case JOINT_STATE: {
    position = q;
    break;
  }
  case END_EFFECTOR_FRAME: {
    position.resize(6);
    vpHomogeneousMatrix fMe = get_fMe(q);
    vpPoseVector fPe(fMe);
    for (size_t i=0; i < 6; i++) {
      position[i] = fPe[i];
    }
    break;
  }
  case TOOL_FRAME: { // same a CAMERA_FRAME
    position.resize(6);
    vpHomogeneousMatrix fMc = get_fMc(q);
    vpPoseVector fPc(fMc);
    for (size_t i=0; i < 6; i++) {
      position[i] = fPc[i];
    }
    break;
  }
  default: {
    throw(vpException(vpException::fatalError, "Cannot get Franka cartesian position: wrong method"));
  }
  }
}

void vpRobotFranka::getForceTorque(const vpRobot::vpControlFrameType frame, vpColVector &force)
{
  if (!m_handler) {
    throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
  }

  franka::RobotState robot_state = getRobotInternalState();

  switch(frame) {
  case JOINT_STATE: {
    force.resize(7);
    for (int i=0; i < 7; i++)
      force[i] = robot_state.tau_J[i];

    break;
  }
  case END_EFFECTOR_FRAME: {
    force.resize(6);
    for (int i=0; i < 6; i++)
      force[i] = robot_state.K_F_ext_hat_K[i];
    break;
  }
  case TOOL_FRAME: {
    // end-effector frame
    vpColVector eFe(6);
    for (int i=0; i < 6; i++)
      eFe[i] = robot_state.K_F_ext_hat_K[i];

    // Transform in tool frame
    vpHomogeneousMatrix cMe = get_eMc().inverse();
    vpForceTwistMatrix cWe( cMe  );
    force = cWe * eFe;
    break;
  }
  default: {
    throw(vpException(vpException::fatalError, "Cannot get Franka cartesian position: wrong method"));
  }
  }
}

void vpRobotFranka::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &d_position)
{
  if (!m_handler) {
    throw(vpException(vpException::fatalError, "Cannot get Franka robot velocity: robot is not connected"));
  }

  franka::RobotState robot_state = getRobotInternalState();

  switch(frame) {

  case JOINT_STATE: {
    d_position.resize(7);
    for (int i=0; i < 7; i++) {
      d_position[i]=robot_state.dq[i];
    }
    break;
  }

  default: {
    throw(vpException(vpException::fatalError, "Cannot get Franka cartesian velocity: not implemented"));
  }
  }
}

void vpRobotFranka::getCoriolis(vpColVector &coriolis)
{
  if (!m_handler) {
    throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
  }

  std::array<double, 7> coriolis_;

  franka::RobotState robot_state = getRobotInternalState();

  coriolis_ = m_model->coriolis(robot_state);

  coriolis.resize(7);
  for (int i=0; i < 7; i++) {
    coriolis[i] = coriolis_[i];
  }
}

void vpRobotFranka::getGravity(vpColVector &gravity)
{
  if (!m_handler) {
    throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
  }

  std::array<double, 7> gravity_;

  franka::RobotState robot_state = getRobotInternalState();

  gravity_ = m_model->gravity(robot_state);

  gravity.resize(7);
  for (int i=0; i < 7; i++) {
    gravity[i] = gravity_[i];
  }
}

void vpRobotFranka::getMass(vpMatrix &mass)
{
  if (!m_handler) {
    throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
  }

  std::array<double, 49> mass_;

  franka::RobotState robot_state = getRobotInternalState();

  mass_ = m_model->mass(robot_state); // column-major

  mass.resize(7, 7); // row-major
  for (size_t i = 0; i < 7; i ++) {
    for (size_t j = 0; j < 7; j ++) {
      mass[i][j] = mass_[j*7 + i];
    }
  }
}

vpHomogeneousMatrix vpRobotFranka::get_fMe(const vpColVector &q)
{
  if (!m_handler) {
    throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
  }
  if (q.size() != 7) {
    throw(vpException(vpException::fatalError, "Joint position vector [%u] is not a 7-dim vector", q.size()));
  }

  std::array< double, 7 > q_array;
  for (size_t i = 0; i < 7; i++)
    q_array[i] = q[i];

  franka::RobotState robot_state = getRobotInternalState();

  std::array<double, 16> pose_array = m_model->pose(franka::Frame::kEndEffector, q_array, robot_state.F_T_EE, robot_state.EE_T_K);
  vpHomogeneousMatrix fMe;
  for (unsigned int i=0; i< 4; i++) {
    for (unsigned int j=0; j< 4; j++) {
      fMe[i][j] = pose_array[j*4 + i];
    }
  }

  return fMe;
}

vpHomogeneousMatrix vpRobotFranka::get_fMc(const vpColVector &q)
{
  vpHomogeneousMatrix fMe = get_fMe(q);
  return (fMe * m_eMc);
}

void vpRobotFranka::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &pose)
{
  if (!m_handler) {
    throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
  }
  if (frame == JOINT_STATE) {
    throw(vpException(vpException::fatalError, "Cannot get Franka joint position as a pose vector"));
  }

  franka::RobotState robot_state = getRobotInternalState();

  std::array<double, 16> pose_array = robot_state.O_T_EE;
  vpHomogeneousMatrix fMe;
  for (unsigned int i=0; i< 4; i++) {
    for (unsigned int j=0; j< 4; j++) {
      fMe[i][j] = pose_array[j*4 + i];
    }
  }

  switch(frame) {
  case END_EFFECTOR_FRAME: {
    pose.buildFrom(fMe);
    break;
  }
  case TOOL_FRAME: {
    pose.buildFrom(fMe * m_eMc);
    break;
  }
  default: {
    throw(vpException(vpException::fatalError, "Cannot get Franka cartesian position: not implemented"));
  }
  }
}

void vpRobotFranka::get_eJe(vpMatrix &eJe_)
{
  if (!m_handler) {
    throw(vpException(vpException::fatalError, "Cannot get Franka robot eJe jacobian: robot is not connected"));
  }

  franka::RobotState robot_state = getRobotInternalState();

  std::array<double, 42> jacobian = m_model->bodyJacobian(franka::Frame::kEndEffector, robot_state); // column-major
  eJe_.resize(6, 7); // row-major
  for (size_t i = 0; i < 6; i ++) {
    for (size_t j = 0; j < 7; j ++) {
      eJe_[i][j] = jacobian[j*6 + i];
    }
  }
  // TODO check from vpRobot fJe and fJeAvailable
}

void vpRobotFranka::get_eJe(const vpColVector &q, vpMatrix &eJe_)
{
  if (!m_handler) {
    throw(vpException(vpException::fatalError, "Cannot get Franka robot eJe jacobian: robot is not connected"));
  }

  franka::RobotState robot_state = getRobotInternalState();

  std::array< double, 7 > q_array;
  for (size_t i = 0; i < 7; i++)
    q_array[i] = q[i];

  std::array<double, 42> jacobian = m_model->bodyJacobian(franka::Frame::kEndEffector, q_array, robot_state.F_T_EE, robot_state.EE_T_K); // column-major
  eJe_.resize(6, 7); // row-major
  for (size_t i = 0; i < 6; i ++) {
    for (size_t j = 0; j < 7; j ++) {
      eJe_[i][j] = jacobian[j*6 + i];
    }
  }
  // TODO check from vpRobot fJe and fJeAvailable

}

void vpRobotFranka::get_fJe(vpMatrix &fJe_)
{
  if (!m_handler) {
    throw(vpException(vpException::fatalError, "Cannot get Franka robot fJe jacobian: robot is not connected"));
  }

  franka::RobotState robot_state = getRobotInternalState();

  std::array<double, 42> jacobian = m_model->zeroJacobian(franka::Frame::kEndEffector, robot_state); // column-major
  fJe_.resize(6, 7); // row-major
  for (size_t i = 0; i < 6; i ++) {
    for (size_t j = 0; j < 7; j ++) {
      fJe_[i][j] = jacobian[j*6 + i];
    }
  }
  // TODO check from vpRobot fJe and fJeAvailable
}

void vpRobotFranka::get_fJe(const vpColVector &q, vpMatrix &fJe_)
{
  if (!m_handler) {
    throw(vpException(vpException::fatalError, "Cannot get Franka robot fJe jacobian: robot is not connected"));
  }
  if (q.size() != 7) {
    throw(vpException(vpException::fatalError, "Cannot get Franka robot fJe jacobian with an input joint position vector [%u] that is not a 7-dim vector", q.size()));
  }

  franka::RobotState robot_state = getRobotInternalState();

  std::array< double, 7 > q_array;
  for (size_t i = 0; i < 7; i++)
    q_array[i] = q[i];

  std::array<double, 42> jacobian = m_model->zeroJacobian(franka::Frame::kEndEffector, q_array, robot_state.F_T_EE, robot_state.EE_T_K); // column-major
  fJe_.resize(6, 7); // row-major
  for (size_t i = 0; i < 6; i ++) {
    for (size_t j = 0; j < 7; j ++) {
      fJe_[i][j] = jacobian[j*6 + i];
    }
  }
  // TODO check from vpRobot fJe and fJeAvailable
}

void vpRobotFranka::setLogFolder(const std::string &folder)
{
  if (!folder.empty()) {
    if (vpIoTools::checkDirectory(folder) == false) {
      try {
        vpIoTools::makeDirectory(folder);
        m_log_folder = folder;
      }
      catch(const vpException &e) {
        std::string error;
        error = "Unable to create Franka log folder: " + folder;
        throw(vpException(vpException::fatalError, error));
      }
    }
    else {
       m_log_folder = folder;
    }
  }
}

void vpRobotFranka::setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position)
{
  if (!m_handler) {
    throw(vpException(vpException::fatalError, "Cannot set Franka robot position: robot is not connected"));
  }
  if (vpRobot::STATE_POSITION_CONTROL != getRobotState()) {
    std::cout << "Robot was not in position-based control. "
                 "Modification of the robot state" << std::endl;
    setRobotState(vpRobot::STATE_POSITION_CONTROL);
  }

  if (frame == vpRobot::JOINT_STATE) {
    double speed_factor = m_positionningVelocity / 100.;

    std::array<double, 7> q_goal;
    for (size_t i = 0; i < 7; i++) {
      q_goal[i] = position[i];
    }

    vpJointPosTrajGenerator joint_pos_traj_generator(speed_factor, q_goal);

    int nbAttempts = 10;
    for (int attempt = 1; attempt <= nbAttempts; attempt++) {
      try {
        m_handler->control(joint_pos_traj_generator);
        break;
      } catch (const franka::ControlException &e) {
        std::cerr << "Warning: communication error: " << e.what() << "\nRetry attempt: " << attempt << std::endl;
        m_handler->automaticErrorRecovery();
        if (attempt == nbAttempts)
          throw;
      }
    }
  }
  else {
    throw (vpException(vpRobotException::functionNotImplementedError,
        "Cannot move the robot to a cartesian position. Only joint positionning is implemented"));
  }
}

void vpRobotFranka::setPositioningVelocity(double velocity)
{
  m_positionningVelocity = velocity;
}

vpRobot::vpRobotStateType vpRobotFranka::setRobotState(vpRobot::vpRobotStateType newState)
{
  switch (newState) {
  case vpRobot::STATE_STOP: {
    // Start primitive STOP only if the current state is velocity or force/torque
    if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState()) {
      // Stop the robot
      m_velControlThreadStopAsked = true;
      if(m_velControlThread.joinable()) {
        m_velControlThread.join();
        m_velControlThreadStopAsked = false;
        m_velControlThreadIsRunning = false;
      }
    }
    else if (vpRobot::STATE_FORCE_TORQUE_CONTROL == getRobotState()) {
      // Stop the robot
      m_ftControlThreadStopAsked = true;
      if(m_ftControlThread.joinable()) {
        m_ftControlThread.join();
        m_ftControlThreadStopAsked = false;
        m_ftControlThreadIsRunning = false;
      }
    }
    break;
  }
  case vpRobot::STATE_POSITION_CONTROL: {
    if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState()) {
      std::cout << "Change the control mode from velocity to position control.\n";
      // Stop the robot
      m_velControlThreadStopAsked = true;
      if(m_velControlThread.joinable()) {
        m_velControlThread.join();
        m_velControlThreadStopAsked = false;
        m_velControlThreadIsRunning = false;
      }
    }
    else if (vpRobot::STATE_FORCE_TORQUE_CONTROL == getRobotState()) {
      std::cout << "Change the control mode from force/torque to position control.\n";
      // Stop the robot
      m_ftControlThreadStopAsked = true;
      if(m_ftControlThread.joinable()) {
        m_ftControlThread.join();
        m_ftControlThreadStopAsked = false;
        m_ftControlThreadIsRunning = false;
      }
    }
    break;
  }
  case vpRobot::STATE_VELOCITY_CONTROL: {
    if (vpRobot::STATE_STOP == getRobotState()) {
      std::cout << "Change the control mode from stop to velocity control.\n";
    }
    else if (vpRobot::STATE_POSITION_CONTROL == getRobotState()) {
      std::cout << "Change the control mode from position to velocity control.\n";
    }
    else if (vpRobot::STATE_FORCE_TORQUE_CONTROL == getRobotState()) {
      std::cout << "Change the control mode from force/torque to velocity control.\n";
      // Stop the robot
      m_ftControlThreadStopAsked = true;
      if(m_ftControlThread.joinable()) {
        m_ftControlThread.join();
        m_ftControlThreadStopAsked = false;
        m_ftControlThreadIsRunning = false;
      }
    }
    break;
  }
  case vpRobot::STATE_FORCE_TORQUE_CONTROL: {
    if (vpRobot::STATE_STOP == getRobotState()) {
      std::cout << "Change the control mode from stop to force/torque control.\n";
    }
    else if (vpRobot::STATE_POSITION_CONTROL == getRobotState()) {
      std::cout << "Change the control mode from position to force/torque control.\n";
    }
    else if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState()) {
      std::cout << "Change the control mode from velocity to force/torque control.\n";
      // Stop the robot
      m_velControlThreadStopAsked = true;
      if(m_velControlThread.joinable()) {
        m_velControlThread.join();
        m_velControlThreadStopAsked = false;
        m_velControlThreadIsRunning = false;
      }
    }
    break;
  }

  default:
    break;
  }

  return vpRobot::setRobotState(newState);
}

void vpRobotFranka::setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
{
  if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState()) {
    throw vpRobotException(vpRobotException::wrongStateError,
                           "Cannot send a velocity to the robot. "
                           "Use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first.");
  }

  switch (frame) {
  // Saturation in joint space
  case JOINT_STATE: {
    if (vel.size() != 7) {
      throw vpRobotException(vpRobotException::wrongStateError,
                             "Joint velocity vector (%d) is not of size 7", vel.size());
    }

    vpColVector vel_max(7, getMaxRotationVelocity());

    vpColVector vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);

    for (size_t i = 0; i < m_dq_des.size(); i++) { // TODO create a function to convert
      m_dq_des[i] = vel_sat[i];
    }

    break;
  }

    // Saturation in cartesian space
  case vpRobot::TOOL_FRAME:
  case vpRobot::REFERENCE_FRAME:
  case vpRobot::END_EFFECTOR_FRAME: {
    if (vel.size() != 6) {
      throw vpRobotException(vpRobotException::wrongStateError,
                             "Cartesian velocity vector (%d) is not of size 6", 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();

    m_v_cart_des = vpRobot::saturateVelocities(vel, vel_max, true);

    break;
  }

  case vpRobot::MIXT_FRAME: {
    throw vpRobotException(vpRobotException::wrongStateError,
                           "Velocity controller not supported");
  }
  }

  if(! m_velControlThreadIsRunning) {
    m_velControlThreadIsRunning = true;
    m_velControlThread = std::thread(&vpJointVelTrajGenerator::control_thread, vpJointVelTrajGenerator(),
                                  std::ref(m_handler), std::ref(m_velControlThreadStopAsked), m_log_folder,
                                  frame, m_eMc, std::ref(m_v_cart_des), std::ref(m_dq_des),
                                  std::cref(m_q_min), std::cref(m_q_max), std::cref(m_dq_max), std::cref(m_ddq_max),
                                  std::ref(m_robot_state), std::ref(m_mutex));
  }
}

/*
  Apply a force/torque to the robot.

  \param[in] frame : Control frame in which the force/torque is applied.

  \param[in] ft : Force/torque vector. The size of this vector
  is always 6 for a cartesian force/torque skew, and 7 for joint torques.

  \param[in] filter_gain : Value in range [0:1] to filter the force/torque vector \e ft.
  To diable the filter set filter_gain to 1.
  \param[in] activate_pi_controller : Activate proportional and integral low level controller.
 */
void vpRobotFranka::setForceTorque(const vpRobot::vpControlFrameType frame, const vpColVector &ft,
                                   const double &filter_gain, const bool &activate_pi_controller)
{
  switch (frame) {
  // Saturation in joint space
  case JOINT_STATE: {
    if (ft.size() != 7) {
      throw vpRobotException(vpRobotException::wrongStateError,
                             "Joint torques vector (%d) is not of size 7", ft.size());
    }

    for (size_t i = 0; i < m_tau_J_des.size(); i++) { // TODO create a function to convert
      m_tau_J_des[i] = ft[i];
    }
    // TODO: Introduce force/torque saturation

    break;
  }

    // Saturation in cartesian space
  case vpRobot::TOOL_FRAME:
  case vpRobot::REFERENCE_FRAME:
  case vpRobot::END_EFFECTOR_FRAME: {
    if (ft.size() != 6) {
      throw vpRobotException(vpRobotException::wrongStateError,
                             "Cartesian force/torque vector (%d) is not of size 6", ft.size());
    }

    m_ft_cart_des = ft;
    // TODO: Introduce force/torque saturation

    break;
  }

  case vpRobot::MIXT_FRAME: {
    throw vpRobotException(vpRobotException::wrongStateError,
                           "Velocity controller not supported");
  }
  }

  if(! m_ftControlThreadIsRunning) {
    getRobotInternalState(); // Update m_robot_state internally
    m_ftControlThreadIsRunning = true;
    m_ftControlThread = std::thread(&vpForceTorqueGenerator::control_thread, vpForceTorqueGenerator(),
                                    std::ref(m_handler), std::ref(m_ftControlThreadStopAsked), m_log_folder,
                                    frame, std::ref(m_tau_J_des), std::ref(m_ft_cart_des), std::ref(m_robot_state),
                                    std::ref(m_mutex), filter_gain, activate_pi_controller);
  }
}

franka::RobotState vpRobotFranka::getRobotInternalState()
{
  if (!m_handler) {
    throw(vpException(vpException::fatalError, "Cannot get Franka robot state: robot is not connected"));
  }
  franka::RobotState robot_state;

  if (! m_velControlThreadIsRunning && ! m_ftControlThreadIsRunning) {
    robot_state = m_handler->readOnce();

    std::lock_guard<std::mutex> lock(m_mutex);
    m_robot_state = robot_state;
  }
  else { // robot_state is updated in the velocity control thread
    std::lock_guard<std::mutex> lock(m_mutex);
    robot_state = m_robot_state;
  }

  return robot_state;
}

vpColVector vpRobotFranka::getJointMin() const
{
  vpColVector q_min(m_q_min.size());
  for (size_t i = 0; i < m_q_min.size(); i ++)
    q_min[i] = m_q_min[i];

  return q_min;
}
vpColVector vpRobotFranka::getJointMax() const
{
  vpColVector q_max(m_q_max.size());
  for (size_t i = 0; i < m_q_max.size(); i ++)
    q_max[i] = m_q_max[i];

  return q_max;
}

vpHomogeneousMatrix vpRobotFranka::get_eMc() const
{
  return m_eMc;
}

void vpRobotFranka::set_eMc(const vpHomogeneousMatrix &eMc)
{
  m_eMc = eMc;
}

void vpRobotFranka::move(const std::string &filename, double velocity_percentage)
{
  vpColVector q;

  this->readPosFile(filename, q);
  this->setRobotState(vpRobot::STATE_POSITION_CONTROL);
  this->setPositioningVelocity(velocity_percentage);
  this->setPosition(vpRobot::JOINT_STATE, q);
}

bool vpRobotFranka::readPosFile(const std::string &filename, vpColVector &q)
{
  std::ifstream fd(filename.c_str(), std::ios::in);

  if (!fd.is_open()) {
    return false;
  }

  std::string line;
  std::string key("R:");
  std::string id("#PANDA - Joint position file");
  bool pos_found = false;
  int lineNum = 0;
  size_t njoints = 7;

  q.resize(njoints);

  while (std::getline(fd, line)) {
    lineNum++;
    if (lineNum == 1) {
      if (!(line.compare(0, id.size(), id) == 0)) { // check if Afma6 position file
        std::cout << "Error: this position file " << filename << " is not for Afma6 robot" << std::endl;
        return false;
      }
    }
    if ((line.compare(0, 1, "#") == 0)) { // skip comment
      continue;
    }
    if ((line.compare(0, key.size(), key) == 0)) { // decode position
      // check if there are at least njoint values in the line
      std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
      if (chain.size() < njoints + 1) // try to split with tab separator
        chain = vpIoTools::splitChain(line, std::string("\t"));
      if (chain.size() < njoints + 1)
        continue;

      std::istringstream ss(line);
      std::string key_;
      ss >> key_;
      for (unsigned int i = 0; i < njoints; i++)
        ss >> q[i];
      pos_found = true;
      break;
    }
  }

  // converts rotations from degrees into radians
  for (unsigned int i = 0; i < njoints; i++) {
    q[i] = vpMath::rad(q[i]);
  }

  fd.close();

  if (!pos_found) {
    std::cout << "Error: unable to find a position for Panda robot in " << filename << std::endl;
    return false;
  }

  return true;
}

bool vpRobotFranka::savePosFile(const std::string &filename, const vpColVector &q)
{

  FILE *fd;
  fd = fopen(filename.c_str(), "w");
  if (fd == NULL)
    return false;

  fprintf(fd,
        "#PANDA - Joint position file\n"
        "#\n"
        "# R: q1 q2 q3 q4 q5 q6 q7\n"
        "# with joint positions q1 to q7 expressed in degrees\n"
        "#\n");

  // Save positions in mm and deg
  fprintf(fd, "R: %lf %lf %lf %lf %lf %lf %lf\n",  vpMath::deg(q[0]), vpMath::deg(q[1]), vpMath::deg(q[2]),
      vpMath::deg(q[3]), vpMath::deg(q[4]), vpMath::deg(q[5]), vpMath::deg(q[6]));

  fclose(fd);
  return (true);
}

void vpRobotFranka::stopMotion()
{
  if (getRobotState() == vpRobot::STATE_VELOCITY_CONTROL) {
    vpColVector q(7, 0);
    setVelocity(vpRobot::JOINT_STATE, q);
  }
  setRobotState(vpRobot::STATE_STOP);
}

void vpRobotFranka::gripperHoming()
{
  if (m_franka_address.empty()) {
    throw (vpException(vpException::fatalError, "Cannot perform franka gripper homing without ip address"));
  }
  if (m_gripper == NULL)
    m_gripper = new franka::Gripper(m_franka_address);

  m_gripper->homing();
}

int vpRobotFranka::gripperMove(double width)
{
  if (m_franka_address.empty()) {
    throw (vpException(vpException::fatalError, "Cannot open franka gripper without ip address"));
  }
  if (m_gripper == NULL)
    m_gripper = new franka::Gripper(m_franka_address);

  // Check for the maximum grasping width.
  franka::GripperState gripper_state = m_gripper->readOnce();

  if (gripper_state.max_width < width) {
    std::cout << "Finger width request is too large for the current fingers on the gripper."
              << "Maximum possible width is " << gripper_state.max_width << std::endl;
    return EXIT_FAILURE;
  }

  m_gripper->move(width, 0.1);
  return EXIT_SUCCESS;
}

int vpRobotFranka::gripperClose()
{
  return gripperMove(0);
}

int vpRobotFranka::gripperOpen()
{
  if (m_franka_address.empty()) {
    throw (vpException(vpException::fatalError, "Cannot open franka gripper without ip address"));
  }
  if (m_gripper == NULL)
    m_gripper = new franka::Gripper(m_franka_address);

  // Check for the maximum grasping width.
  franka::GripperState gripper_state = m_gripper->readOnce();

  m_gripper->move(gripper_state.max_width, 0.1);
  return EXIT_SUCCESS;
}

void vpRobotFranka::gripperRelease()
{
  if (m_franka_address.empty()) {
    throw (vpException(vpException::fatalError, "Cannot release franka gripper without ip address"));
  }
  if (m_gripper == NULL)
    m_gripper = new franka::Gripper(m_franka_address);

  m_gripper->stop();
}

int vpRobotFranka::gripperGrasp(double grasping_width, double force)
{
  if (m_gripper == NULL)
    m_gripper = new franka::Gripper(m_franka_address);

  // Check for the maximum grasping width.
  franka::GripperState gripper_state = m_gripper->readOnce();
  std::cout << "Gripper max witdh: " << gripper_state.max_width << std::endl;
  if (gripper_state.max_width < grasping_width) {
    std::cout << "Object is too large for the current fingers on the gripper."
              << "Maximum possible width is " << gripper_state.max_width << std::endl;
    return EXIT_FAILURE;
  }

  // Grasp the object.
  if (!m_gripper->grasp(grasping_width, 0.1, force)) {
    std::cout << "Failed to grasp object." << std::endl;
    return EXIT_FAILURE;
  }

  return EXIT_SUCCESS;
}

#elif !defined(VISP_BUILD_SHARED_LIBS)
// Work arround to avoid warning: libvisp_robot.a(vpRobotFranka.cpp.o) has
// no symbols
void dummy_vpRobotFranka(){};
#endif