vpRobotFranka.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:
 * Interface for the Franka robot.
 *
*****************************************************************************/
 
#include <visp3/core/vpConfig.h>
 
#ifdef VISP_HAVE_FRANKA
 
#include <visp3/core/vpIoTools.h>
#include <visp3/robot/vpRobotException.h>
#include <visp3/robot/vpRobotFranka.h>
 
#include "vpForceTorqueGenerator_impl.h"
#include "vpJointPosTrajGenerator_impl.h"
#include "vpJointVelTrajGenerator_impl.h"
 
vpRobotFranka::vpRobotFranka()
  : vpRobot(), m_handler(nullptr), m_gripper(nullptr), m_model(nullptr), m_positioningVelocity(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(nullptr), m_gripper(nullptr), m_model(nullptr), m_positioningVelocity(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 force/torque: frame not supported"));
  }
  }
}
 
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_positioningVelocity / 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 positioning is implemented"));
  }
}
 
void vpRobotFranka::setPositioningVelocity(double velocity) { m_positioningVelocity = 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 == nullptr)
    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 == nullptr)
    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 == nullptr)
    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 == nullptr)
    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 == nullptr)
    m_gripper = new franka::Gripper(m_franka_address);
 
  m_gripper->stop();
}
 
int vpRobotFranka::gripperGrasp(double grasping_width, double force)
{
  if (m_gripper == nullptr)
    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 around to avoid warning: libvisp_robot.a(vpRobotFranka.cpp.o) has
// no symbols
void dummy_vpRobotFranka(){};
#endif