vpRobotUniversalRobots.cpp

/*
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2025 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 Universal Robots.
 */
 
#include <visp3/core/vpConfig.h>
 
#if defined(VISP_HAVE_UR_RTDE)
 
#include <visp3/core/vpIoTools.h>
#include <visp3/robot/vpRobotUniversalRobots.h>
 
BEGIN_VISP_NAMESPACE
  vpRobotUniversalRobots::vpRobotUniversalRobots() : m_rtde_receive(), m_rtde_control(), m_db_client(), m_eMc()
{
  init();
}

vpRobotUniversalRobots::~vpRobotUniversalRobots() { setRobotState(vpRobot::STATE_STOP); }

vpRobotUniversalRobots::vpRobotUniversalRobots(const std::string &ur_address)
  : m_rtde_receive(nullptr), m_rtde_control(nullptr), m_eMc()
{
  init();
  connect(ur_address);
}

void vpRobotUniversalRobots::connect(const std::string &ur_address)
{
  if (!m_rtde_receive) {
    m_rtde_receive = std::make_shared<ur_rtde::RTDEReceiveInterface>(ur_address);
  }
  if (!m_rtde_control) {
    m_rtde_control = std::make_shared<ur_rtde::RTDEControlInterface>(ur_address);
  }
  if (!m_db_client) {
    m_db_client = std::make_shared<ur_rtde::DashboardClient>(ur_address);
  }
  if (!m_rtde_receive->isConnected()) {
    m_rtde_receive->reconnect();
    if (!m_rtde_receive->isConnected()) {
      throw(vpException(vpException::fatalError, "Cannot connect UR robot to receive interface"));
    }
  }
  if (!m_rtde_control->isConnected()) {
    m_rtde_control->reconnect();
    if (!m_rtde_control->isConnected()) {
      throw(vpException(vpException::fatalError, "Cannot connect UR robot to control interface"));
    }
  }
  if (!m_db_client->isConnected()) {
    m_db_client->connect();
    if (!m_db_client->isConnected()) {
      throw(vpException(vpException::fatalError, "Cannot connect UR robot to dashboard client"));
    }
  }
}

void vpRobotUniversalRobots::disconnect()
{
  if (m_rtde_receive && m_rtde_receive->isConnected()) {
    m_rtde_receive->disconnect();
  }
  if (m_rtde_control && m_rtde_control->isConnected()) {
    m_rtde_control->disconnect();
  }
  if (m_db_client && m_db_client->isConnected()) {
    m_db_client->disconnect();
  }
}

void vpRobotUniversalRobots::init()
{
  nDof = 6;
  m_positioningVelocity = 20.;
  m_max_joint_speed = vpMath::rad(180.);        // deg/s
  m_max_joint_acceleration = vpMath::rad(800.); // deg/s^2
  m_max_linear_speed = 0.5;                     // m/s
  m_max_linear_acceleration = 2.5;              // m/s^2
  m_vel_control_frame = vpRobot::JOINT_STATE;
}

vpHomogeneousMatrix vpRobotUniversalRobots::get_fMe()
{
  vpPoseVector fPe;
  getPosition(vpRobot::END_EFFECTOR_FRAME, fPe);
  return vpHomogeneousMatrix(fPe);
}

vpHomogeneousMatrix vpRobotUniversalRobots::get_fMe(const vpColVector &q)
{
  if (!m_rtde_control || !m_rtde_receive) {
    throw(vpException(vpException::fatalError, "Cannot get UR robot forward kinematics: robot is not connected"));
  }
  if (q.size() != 6) {
    throw(vpException(vpException::fatalError,
                      "Cannot get UR robot forward kinematics: joint position vector is not 6-dim (%d)", q.size()));
  }
  // Robot modes:
  // https://sdurobotics.gitlab.io/ur_rtde/api/api.html#_CPPv4N7ur_rtde20RTDEReceiveInterface12getRobotModeEv
  if (m_rtde_receive->getRobotMode() != 7) {
    throw(vpException(vpException::fatalError, "Cannot get UR robot forward kinematics: brakes are not released"));
  }
 
  const std::vector<double> q_std = q.toStdVector();
  std::vector<double> tcp_pose = m_rtde_control->getForwardKinematics(q_std);
 
  vpPoseVector f_P_e;
  for (size_t i = 0; i < 6; i++) {
    f_P_e[i] = tcp_pose[i];
  }
  vpHomogeneousMatrix fMe(f_P_e);
  return fMe;
}

vpHomogeneousMatrix vpRobotUniversalRobots::get_fMc()
{
  vpPoseVector fPc;
  getPosition(vpRobot::CAMERA_FRAME, fPc);
  return vpHomogeneousMatrix(fPc);
}

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

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

void vpRobotUniversalRobots::getForceTorque(const vpRobot::vpControlFrameType frame, vpColVector &force)
{
  if (!m_rtde_receive) {
    throw(vpException(vpException::fatalError, "Cannot get UR robot force/torque: robot is not connected"));
  }
 
  std::vector<double> eFe = m_rtde_receive->getActualTCPForce();
 
  switch (frame) {
  case JOINT_STATE: {
    throw(vpException(vpException::fatalError, "Cannot get UR force/torque in joint space"));
    break;
  }
  case END_EFFECTOR_FRAME: {
    force = eFe;
    break;
  }
  case TOOL_FRAME: {
    // Transform in tool frame
    vpHomogeneousMatrix cMe = get_eMc().inverse();
    vpForceTwistMatrix cWe(cMe);
    force = cWe * eFe;
    break;
  }
  default: {
    throw(vpException(vpException::fatalError, "Cannot get UR force/torque: frame not supported"));
  }
  }
}

std::string vpRobotUniversalRobots::getPolyScopeVersion()
{
  if (!m_db_client) {
    throw(vpException(vpException::fatalError, "Cannot get UR robot PolyScope verson: robot is not connected"));
  }
  return m_db_client->polyscopeVersion();
}

void vpRobotUniversalRobots::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
{
  if (!m_rtde_receive) {
    throw(vpException(vpException::fatalError, "Cannot get UR robot position: robot is not connected"));
  }
  // Robot modes:
  // https://sdurobotics.gitlab.io/ur_rtde/api/api.html#_CPPv4N7ur_rtde20RTDEReceiveInterface12getRobotModeEv
  if (m_rtde_receive->getRobotMode() < 4) {
    throw(vpException(vpException::fatalError, "Cannot get UR robot position: robot is not powered on"));
  }
 
  switch (frame) {
  case JOINT_STATE: {
    position = m_rtde_receive->getActualQ();
    break;
  }
  case END_EFFECTOR_FRAME: {
    std::vector<double> tcp_pose = m_rtde_receive->getActualTCPPose();
    position.resize(6);
    for (size_t i = 0; i < tcp_pose.size(); i++) {
      position[i] = tcp_pose[i];
    }
 
    break;
  }
  case TOOL_FRAME: { // same a CAMERA_FRAME
    std::vector<double> tcp_pose = m_rtde_receive->getActualTCPPose();
    vpPoseVector fPe;
    for (size_t i = 0; i < tcp_pose.size(); i++) {
      fPe[i] = tcp_pose[i];
    }
 
    vpHomogeneousMatrix fMe(fPe);
    vpHomogeneousMatrix fMc = fMe * m_eMc;
    vpPoseVector fPc(fMc);
    position.resize(6);
    for (size_t i = 0; i < 6; i++) {
      position[i] = fPc[i];
    }
    break;
  }
  default: {
    throw(vpException(vpException::fatalError, "Cannot get UR cartesian position: wrong method"));
  }
  }
}

void vpRobotUniversalRobots::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &pose)
{
  if (!m_rtde_receive) {
    throw(vpException(vpException::fatalError, "Cannot get UR robot position: robot is not connected"));
  }
  if (frame == JOINT_STATE) {
    throw(vpException(vpException::fatalError, "Cannot get UR joint position as a pose vector"));
  }
 
  switch (frame) {
  case END_EFFECTOR_FRAME:
  case TOOL_FRAME: {
    vpColVector position;
    getPosition(frame, position);
    for (size_t i = 0; i < 6; i++) {
      pose[i] = position[i];
    }
    break;
  }
  default: {
    throw(vpException(vpException::fatalError, "Cannot get Franka cartesian position: not implemented"));
  }
  }
}

std::string vpRobotUniversalRobots::getRobotModel() const
{
  if (!m_db_client) {
    throw(vpException(vpException::fatalError, "Cannot get UR robot model: robot is not connected"));
  }
  return m_db_client->getRobotModel();
}

int vpRobotUniversalRobots::getRobotMode() const
{
  if (!m_rtde_receive) {
    throw(vpException(vpException::fatalError, "Cannot get UR robot mode: robot is not connected"));
  }
  // Robot modes:
  // https://sdurobotics.gitlab.io/ur_rtde/api/api.html#_CPPv4N7ur_rtde20RTDEReceiveInterface12getRobotModeEv
  return (m_rtde_receive->getRobotMode());
}

void vpRobotUniversalRobots::setPositioningVelocity(double velocity) { m_positioningVelocity = velocity; }

void vpRobotUniversalRobots::setPosition(const vpRobot::vpControlFrameType frame, const vpPoseVector &pose)
{
  if (frame == vpRobot::JOINT_STATE) {
    throw(vpException(vpException::fatalError, "Cannot set UR robot cartesian position: joint state is specified"));
  }
  vpColVector position(pose);
  setPosition(frame, position);
}

void vpRobotUniversalRobots::setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position)
{
  if (!m_rtde_control) {
    throw(vpException(vpException::fatalError, "Cannot set UR robot position: robot is not connected"));
  }
 
  // Robot modes:
  // https://sdurobotics.gitlab.io/ur_rtde/api/api.html#_CPPv4N7ur_rtde20RTDEReceiveInterface12getRobotModeEv
  if (m_rtde_receive->getRobotMode() != 7) {
    throw(vpException(vpException::fatalError, "Cannot set UR robot position: brakes are not released"));
  }
 
  if (position.size() != 6) {
    throw(vpException(vpException::fatalError,
                      "Cannot set UR robot position: position vector is not a 6-dim vector (%d)", position.size()));
  }
 
  if (vpRobot::STATE_POSITION_CONTROL != getRobotState()) {
    std::cout << "Robot is 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::vector<double> new_q = position.toStdVector();
    m_rtde_control->moveJ(new_q, m_max_joint_speed * speed_factor);
  }
  else if (frame == vpRobot::END_EFFECTOR_FRAME) {
    double speed_factor = m_positioningVelocity / 100.;
    std::vector<double> new_pose = position.toStdVector();
    // Move synchronously to ensure a the blocking behaviour
    m_rtde_control->moveL(new_pose, m_max_linear_speed * speed_factor);
  }
  else if (frame == vpRobot::CAMERA_FRAME) {
    double speed_factor = m_positioningVelocity / 100.;
 
    vpTranslationVector f_t_c(position.extract(0, 3));
    vpThetaUVector f_tu_c(position.extract(3, 3));
    vpHomogeneousMatrix fMc(f_t_c, f_tu_c);
    vpHomogeneousMatrix fMe = fMc * m_eMc.inverse();
    vpPoseVector fPe(fMe);
    std::vector<double> new_pose = fPe.toStdVector();
    // Move synchronously to ensure a the blocking behaviour
    m_rtde_control->moveL(new_pose, m_max_linear_speed * speed_factor);
  }
  else {
    throw(vpException(vpRobotException::functionNotImplementedError,
                      "Cannot move the robot to a cartesian position. Only joint positioning is implemented"));
  }
}

void vpRobotUniversalRobots::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: robot is not in velocity control state "
                           "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
  }
 
  vpColVector vel_sat(6);
  m_vel_control_frame = frame;
 
  // Velocity saturation
  switch (frame) {
  // saturation in cartesian space
  case vpRobot::CAMERA_FRAME:
  case vpRobot::REFERENCE_FRAME:
  case vpRobot::END_EFFECTOR_FRAME:
  case vpRobot::MIXT_FRAME: {
    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);
 
    break;
  }
  // Saturation in joint space
  case vpRobot::JOINT_STATE: {
    vpColVector vel_max(6);
    vel_max = getMaxRotationVelocity();
    vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
  }
  }
 
  if (frame == vpRobot::JOINT_STATE) {
    double dt = 1.0 / 1000; // 2ms
    double acceleration = 0.5;
    m_rtde_control->speedJ(vel_sat.toStdVector(), acceleration, dt);
  }
  else if (frame == vpRobot::REFERENCE_FRAME) {
    double dt = 1.0 / 1000; // 2ms
    double acceleration = 0.25;
    m_rtde_control->speedL(vel_sat.toStdVector(), acceleration, dt);
  }
  else if (frame == vpRobot::END_EFFECTOR_FRAME) {
    double dt = 1.0 / 1000; // 2ms
    double acceleration = 0.25;
    vpVelocityTwistMatrix fVe(get_fMe(), false);
    m_rtde_control->speedL((fVe * vel_sat).toStdVector(), acceleration, dt);
  }
  else if (frame == vpRobot::CAMERA_FRAME) {
    double dt = 1.0 / 1000; // 2ms
    double acceleration = 0.25;
    vpColVector w_v_e = vpVelocityTwistMatrix(get_fMe(), false) * vpVelocityTwistMatrix(m_eMc) * vel_sat;
    m_rtde_control->speedL(w_v_e.toStdVector(), acceleration, dt);
  }
  else {
    throw(vpException(vpRobotException::functionNotImplementedError,
                      "Cannot move the robot in velocity in the specified frame: not implemented"));
  }
}

void vpRobotUniversalRobots::stopMotion()
{
  if (!m_rtde_control) {
    throw(vpException(vpException::fatalError, "Cannot stop UR robot: robot is not connected"));
  }
 
  setRobotState(vpRobot::STATE_STOP);
}

void vpRobotUniversalRobots::move(const std::string &filename, double velocity_percentage)
{
  vpColVector q;
 
  readPosFile(filename, q);
  setRobotState(vpRobot::STATE_POSITION_CONTROL);
  setPositioningVelocity(velocity_percentage);
  setPosition(vpRobot::JOINT_STATE, q);
}

bool vpRobotUniversalRobots::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("#UR - Joint position file");
  bool pos_found = false;
  int lineNum = 0;
  size_t njoints = static_cast<size_t>(nDof);
 
  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 Universal Robots" << 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 UR robot in " << filename << std::endl;
    return false;
  }
 
  return true;
}

bool vpRobotUniversalRobots::savePosFile(const std::string &filename, const vpColVector &q)
{
 
  FILE *fd;
  fd = fopen(filename.c_str(), "w");
  if (fd == nullptr)
    return false;
 
  fprintf(fd, "#UR - Joint position file\n"
              "#\n"
              "# R: q1 q2 q3 q4 q5 q6\n"
              "# with joint positions q1 to q6 expressed in degrees\n"
              "#\n");
 
  // Save positions in mm and deg
  fprintf(fd, "R: %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]));
 
  fclose(fd);
  return (true);
}

vpRobot::vpRobotStateType vpRobotUniversalRobots::setRobotState(vpRobot::vpRobotStateType newState)
{
  switch (newState) {
  case vpRobot::STATE_STOP: {
    // Start primitive STOP only if the current state is Velocity
    if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState()) {
      if (!m_rtde_control) {
        throw(vpException(vpException::fatalError, "Cannot stop UR robot: robot is not connected"));
      }
      m_rtde_control->speedStop();
    }
    break;
  }
  case vpRobot::STATE_POSITION_CONTROL: {
    if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState()) {
      if (!m_rtde_control) {
        throw(vpException(vpException::fatalError, "Cannot stop UR robot: robot is not connected"));
      }
      m_rtde_control->speedStop();
    }
    else {
   // std::cout << "Change the control mode from stop to position control" << std::endl;
    }
    break;
  }
  case vpRobot::STATE_VELOCITY_CONTROL: {
    if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState()) {
      std::cout << "Change the control mode from stop to velocity control.\n";
    }
    break;
  }
  default:
    break;
  }
 
  return vpRobot::setRobotState(newState);
}
END_VISP_NAMESPACE
#elif !defined(VISP_BUILD_SHARED_LIBS)
// Work around to avoid warning: libvisp_robot.a(vpRobotUniversalRobots.cpp.o) has no symbols
void dummy_vpRobotUniversalRobots() { }
#endif