Visual Servoing Platform  version 3.5.1 under development (2022-12-04)
vpRobotUniversalRobots.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2022 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Interface for Universal Robots.
33  *
34  * Authors:
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
39 #include <visp3/core/vpConfig.h>
40 
41 #if defined(VISP_HAVE_UR_RTDE) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
42 
43 #include <visp3/core/vpIoTools.h>
44 #include <visp3/robot/vpRobotUniversalRobots.h>
45 
53 vpRobotUniversalRobots::vpRobotUniversalRobots() : m_rtde_receive(), m_rtde_control(), m_db_client(), m_eMc()
54 {
55  init();
56 }
57 
62 
71 vpRobotUniversalRobots::vpRobotUniversalRobots(const std::string &ur_address)
72  : m_rtde_receive(nullptr), m_rtde_control(nullptr), m_eMc()
73 {
74  init();
75  connect(ur_address);
76 }
77 
84 void vpRobotUniversalRobots::connect(const std::string &ur_address)
85 {
86  if (!m_rtde_receive) {
87  m_rtde_receive = std::make_shared<ur_rtde::RTDEReceiveInterface>(ur_address);
88  }
89  if (!m_rtde_control) {
90  m_rtde_control = std::make_shared<ur_rtde::RTDEControlInterface>(ur_address);
91  }
92  if (!m_db_client) {
93  m_db_client = std::make_shared<ur_rtde::DashboardClient>(ur_address);
94  }
95  if (!m_rtde_receive->isConnected()) {
96  m_rtde_receive->reconnect();
97  if (!m_rtde_receive->isConnected()) {
98  throw(vpException(vpException::fatalError, "Cannot connect UR robot to receive interface"));
99  }
100  }
101  if (!m_rtde_control->isConnected()) {
102  m_rtde_control->reconnect();
103  if (!m_rtde_control->isConnected()) {
104  throw(vpException(vpException::fatalError, "Cannot connect UR robot to control interface"));
105  }
106  }
107  if (!m_db_client->isConnected()) {
108  m_db_client->connect();
109  if (!m_db_client->isConnected()) {
110  throw(vpException(vpException::fatalError, "Cannot connect UR robot to dashboard client"));
111  }
112  }
113 }
114 
119 {
120  if (m_rtde_receive && m_rtde_receive->isConnected()) {
121  m_rtde_receive->disconnect();
122  }
123  if (m_rtde_control && m_rtde_control->isConnected()) {
124  m_rtde_control->disconnect();
125  }
126  if (m_db_client && m_db_client->isConnected()) {
127  m_db_client->disconnect();
128  }
129 }
130 
135 {
136  nDof = 6;
138  m_max_joint_speed = vpMath::rad(180.); // deg/s
139  m_max_joint_acceleration = vpMath::rad(800.); // deg/s^2
140  m_max_linear_speed = 0.5; // m/s
141  m_max_linear_acceleration = 2.5; // m/s^2
143 }
144 
157 {
158  vpPoseVector fPe;
160  return vpHomogeneousMatrix(fPe);
161 }
162 
177 {
178  if (!m_rtde_control || !m_rtde_receive) {
179  throw(vpException(vpException::fatalError, "Cannot get UR robot forward kinematics: robot is not connected"));
180  }
181  if (q.size() != 6) {
183  "Cannot get UR robot forward kinematics: joint position vector is not 6-dim (%d)", q.size()));
184  }
185  // Robot modes:
186  // https://sdurobotics.gitlab.io/ur_rtde/api/api.html#_CPPv4N7ur_rtde20RTDEReceiveInterface12getRobotModeEv
187  if (m_rtde_receive->getRobotMode() != 7) {
188  throw(vpException(vpException::fatalError, "Cannot get UR robot forward kinematics: brakes are not released"));
189  }
190 
191  const std::vector<double> q_std = q.toStdVector();
192  std::vector<double> tcp_pose = m_rtde_control->getForwardKinematics(q_std);
193 
194  vpPoseVector f_P_e;
195  for (size_t i = 0; i < 6; i++) {
196  f_P_e[i] = tcp_pose[i];
197  }
198  vpHomogeneousMatrix fMe(f_P_e);
199  return fMe;
200 }
201 
216 {
217  vpPoseVector fPc;
219  return vpHomogeneousMatrix(fPc);
220 }
221 
235 
247 
258 {
259  if (!m_rtde_receive) {
260  throw(vpException(vpException::fatalError, "Cannot get UR robot force/torque: robot is not connected"));
261  }
262 
263  std::vector<double> eFe = m_rtde_receive->getActualTCPForce();
264 
265  switch (frame) {
266  case JOINT_STATE: {
267  throw(vpException(vpException::fatalError, "Cannot get UR force/torque in joint space"));
268  break;
269  }
270  case END_EFFECTOR_FRAME: {
271  force = eFe;
272  break;
273  }
274  case TOOL_FRAME: {
275  // Transform in tool frame
277  vpForceTwistMatrix cWe(cMe);
278  force = cWe * eFe;
279  break;
280  }
281  default: {
282  throw(vpException(vpException::fatalError, "Cannot get UR force/torque: frame not supported"));
283  }
284  }
285 }
286 
291 {
292  if (!m_db_client) {
293  throw(vpException(vpException::fatalError, "Cannot get UR robot PolyScope verson: robot is not connected"));
294  }
295  return m_db_client->polyscopeVersion();
296 }
297 
313 {
314  if (!m_rtde_receive) {
315  throw(vpException(vpException::fatalError, "Cannot get UR robot position: robot is not connected"));
316  }
317  // Robot modes:
318  // https://sdurobotics.gitlab.io/ur_rtde/api/api.html#_CPPv4N7ur_rtde20RTDEReceiveInterface12getRobotModeEv
319  if (m_rtde_receive->getRobotMode() < 4) {
320  throw(vpException(vpException::fatalError, "Cannot get UR robot position: robot is not powered on"));
321  }
322 
323  switch (frame) {
324  case JOINT_STATE: {
325  position = m_rtde_receive->getActualQ();
326  break;
327  }
328  case END_EFFECTOR_FRAME: {
329  std::vector<double> tcp_pose = m_rtde_receive->getActualTCPPose();
330  position.resize(6);
331  for (size_t i = 0; i < tcp_pose.size(); i++) {
332  position[i] = tcp_pose[i];
333  }
334 
335  break;
336  }
337  case TOOL_FRAME: { // same a CAMERA_FRAME
338  std::vector<double> tcp_pose = m_rtde_receive->getActualTCPPose();
339  vpPoseVector fPe;
340  for (size_t i = 0; i < tcp_pose.size(); i++) {
341  fPe[i] = tcp_pose[i];
342  }
343 
344  vpHomogeneousMatrix fMe(fPe);
345  vpHomogeneousMatrix fMc = fMe * m_eMc;
346  vpPoseVector fPc(fMc);
347  position.resize(6);
348  for (size_t i = 0; i < 6; i++) {
349  position[i] = fPc[i];
350  }
351  break;
352  }
353  default: {
354  throw(vpException(vpException::fatalError, "Cannot get UR cartesian position: wrong method"));
355  }
356  }
357 }
358 
369 {
370  if (!m_rtde_receive) {
371  throw(vpException(vpException::fatalError, "Cannot get UR robot position: robot is not connected"));
372  }
373  if (frame == JOINT_STATE) {
374  throw(vpException(vpException::fatalError, "Cannot get UR joint position as a pose vector"));
375  }
376 
377  switch (frame) {
378  case END_EFFECTOR_FRAME:
379  case TOOL_FRAME: {
380  vpColVector position;
381  getPosition(frame, position);
382  for (size_t i = 0; i < 6; i++) {
383  pose[i] = position[i];
384  }
385  break;
386  }
387  default: {
388  throw(vpException(vpException::fatalError, "Cannot get Franka cartesian position: not implemented"));
389  }
390  }
391 }
392 
397 {
398  if (!m_db_client) {
399  throw(vpException(vpException::fatalError, "Cannot get UR robot model: robot is not connected"));
400  }
401  return m_db_client->getRobotModel();
402 }
408 {
409  if (!m_rtde_receive) {
410  throw(vpException(vpException::fatalError, "Cannot get UR robot mode: robot is not connected"));
411  }
412  // Robot modes:
413  // https://sdurobotics.gitlab.io/ur_rtde/api/api.html#_CPPv4N7ur_rtde20RTDEReceiveInterface12getRobotModeEv
414  return (m_rtde_receive->getRobotMode());
415 }
416 
423 
438 {
439  if (frame == vpRobot::JOINT_STATE) {
440  throw(vpException(vpException::fatalError, "Cannot set UR robot cartesian position: joint state is specified"));
441  }
442  vpColVector position(pose);
443  setPosition(frame, position);
444 }
445 
461 {
462  if (!m_rtde_control) {
463  throw(vpException(vpException::fatalError, "Cannot set UR robot position: robot is not connected"));
464  }
465 
466  // Robot modes:
467  // https://sdurobotics.gitlab.io/ur_rtde/api/api.html#_CPPv4N7ur_rtde20RTDEReceiveInterface12getRobotModeEv
468  if (m_rtde_receive->getRobotMode() != 7) {
469  throw(vpException(vpException::fatalError, "Cannot set UR robot position: brakes are not released"));
470  }
471 
472  if (position.size() != 6) {
474  "Cannot set UR robot position: position vector is not a 6-dim vector (%d)", position.size()));
475  }
476 
478  std::cout << "Robot is not in position-based control. "
479  "Modification of the robot state"
480  << std::endl;
482  }
483 
484  if (frame == vpRobot::JOINT_STATE) {
485  double speed_factor = m_positionningVelocity / 100.;
486  std::vector<double> new_q = position.toStdVector();
487  m_rtde_control->moveJ(new_q, m_max_joint_speed * speed_factor);
488  } else if (frame == vpRobot::END_EFFECTOR_FRAME) {
489  double speed_factor = m_positionningVelocity / 100.;
490  std::vector<double> new_pose = position.toStdVector();
491  // Move synchronously to ensure a the blocking behaviour
492  m_rtde_control->moveL(new_pose, m_max_linear_speed * speed_factor);
493  } else if (frame == vpRobot::CAMERA_FRAME) {
494  double speed_factor = m_positionningVelocity / 100.;
495 
496  vpTranslationVector f_t_c(position.extract(0, 3));
497  vpThetaUVector f_tu_c(position.extract(3, 3));
498  vpHomogeneousMatrix fMc(f_t_c, f_tu_c);
499  vpHomogeneousMatrix fMe = fMc * m_eMc.inverse();
500  vpPoseVector fPe(fMe);
501  std::vector<double> new_pose = fPe.toStdVector();
502  // Move synchronously to ensure a the blocking behaviour
503  m_rtde_control->moveL(new_pose, m_max_linear_speed * speed_factor);
504  } else {
506  "Cannot move the robot to a cartesian position. Only joint positionning is implemented"));
507  }
508 }
509 
598 {
601  "Cannot send a velocity to the robot: robot is not in velocity control state "
602  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
603  }
604 
605  vpColVector vel_sat(6);
606  m_vel_control_frame = frame;
607 
608  // Velocity saturation
609  switch (frame) {
610  // saturation in cartesian space
614  case vpRobot::MIXT_FRAME: {
615  vpColVector vel_max(6);
616 
617  for (unsigned int i = 0; i < 3; i++)
618  vel_max[i] = getMaxTranslationVelocity();
619  for (unsigned int i = 3; i < 6; i++)
620  vel_max[i] = getMaxRotationVelocity();
621 
622  vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
623 
624  break;
625  }
626  // Saturation in joint space
627  case vpRobot::JOINT_STATE: {
628  vpColVector vel_max(6);
629  vel_max = getMaxRotationVelocity();
630  vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
631  }
632  }
633 
634  if (frame == vpRobot::JOINT_STATE) {
635  double dt = 1.0 / 1000; // 2ms
636  double acceleration = 0.5;
637  m_rtde_control->speedJ(vel_sat.toStdVector(), acceleration, dt);
638  } else if (frame == vpRobot::REFERENCE_FRAME) {
639  double dt = 1.0 / 1000; // 2ms
640  double acceleration = 0.25;
641  m_rtde_control->speedL(vel_sat.toStdVector(), acceleration, dt);
642  } else if (frame == vpRobot::END_EFFECTOR_FRAME) {
643  double dt = 1.0 / 1000; // 2ms
644  double acceleration = 0.25;
645  vpVelocityTwistMatrix fVe(get_fMe(), false);
646  m_rtde_control->speedL((fVe * vel_sat).toStdVector(), acceleration, dt);
647  } else if (frame == vpRobot::CAMERA_FRAME) {
648  double dt = 1.0 / 1000; // 2ms
649  double acceleration = 0.25;
651  m_rtde_control->speedL(w_v_e.toStdVector(), acceleration, dt);
652  } else {
654  "Cannot move the robot in velocity in the specified frame: not implemented"));
655  }
656 }
657 
666 {
667  if (!m_rtde_control) {
668  throw(vpException(vpException::fatalError, "Cannot stop UR robot: robot is not connected"));
669  }
670 
672 }
673 
682 void vpRobotUniversalRobots::move(const std::string &filename, double velocity_percentage)
683 {
684  vpColVector q;
685 
686  readPosFile(filename, q);
688  setPositioningVelocity(velocity_percentage);
690 }
691 
729 bool vpRobotUniversalRobots::readPosFile(const std::string &filename, vpColVector &q)
730 {
731  std::ifstream fd(filename.c_str(), std::ios::in);
732 
733  if (!fd.is_open()) {
734  return false;
735  }
736 
737  std::string line;
738  std::string key("R:");
739  std::string id("#UR - Joint position file");
740  bool pos_found = false;
741  int lineNum = 0;
742  size_t njoints = static_cast<size_t>(nDof);
743 
744  q.resize(njoints);
745 
746  while (std::getline(fd, line)) {
747  lineNum++;
748  if (lineNum == 1) {
749  if (!(line.compare(0, id.size(), id) == 0)) { // check if Afma6 position file
750  std::cout << "Error: this position file " << filename << " is not for Universal Robots" << std::endl;
751  return false;
752  }
753  }
754  if ((line.compare(0, 1, "#") == 0)) { // skip comment
755  continue;
756  }
757  if ((line.compare(0, key.size(), key) == 0)) { // decode position
758  // check if there are at least njoint values in the line
759  std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
760  if (chain.size() < njoints + 1) // try to split with tab separator
761  chain = vpIoTools::splitChain(line, std::string("\t"));
762  if (chain.size() < njoints + 1)
763  continue;
764 
765  std::istringstream ss(line);
766  std::string key_;
767  ss >> key_;
768  for (unsigned int i = 0; i < njoints; i++)
769  ss >> q[i];
770  pos_found = true;
771  break;
772  }
773  }
774 
775  // converts rotations from degrees into radians
776  for (unsigned int i = 0; i < njoints; i++) {
777  q[i] = vpMath::rad(q[i]);
778  }
779 
780  fd.close();
781 
782  if (!pos_found) {
783  std::cout << "Error: unable to find a position for UR robot in " << filename << std::endl;
784  return false;
785  }
786 
787  return true;
788 }
789 
807 bool vpRobotUniversalRobots::savePosFile(const std::string &filename, const vpColVector &q)
808 {
809 
810  FILE *fd;
811  fd = fopen(filename.c_str(), "w");
812  if (fd == NULL)
813  return false;
814 
815  fprintf(fd, "#UR - Joint position file\n"
816  "#\n"
817  "# R: q1 q2 q3 q4 q5 q6\n"
818  "# with joint positions q1 to q6 expressed in degrees\n"
819  "#\n");
820 
821  // Save positions in mm and deg
822  fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n", vpMath::deg(q[0]), vpMath::deg(q[1]), vpMath::deg(q[2]),
823  vpMath::deg(q[3]), vpMath::deg(q[4]), vpMath::deg(q[5]));
824 
825  fclose(fd);
826  return (true);
827 }
828 
835 {
836  switch (newState) {
837  case vpRobot::STATE_STOP: {
838  // Start primitive STOP only if the current state is Velocity
840  if (!m_rtde_control) {
841  throw(vpException(vpException::fatalError, "Cannot stop UR robot: robot is not connected"));
842  }
843  m_rtde_control->speedStop();
844  }
845  break;
846  }
849  if (!m_rtde_control) {
850  throw(vpException(vpException::fatalError, "Cannot stop UR robot: robot is not connected"));
851  }
852  m_rtde_control->speedStop();
853  } else {
854  // std::cout << "Change the control mode from stop to position control" << std::endl;
855  }
856  break;
857  }
860  std::cout << "Change the control mode from stop to velocity control.\n";
861  }
862  break;
863  }
864  default:
865  break;
866  }
867 
868  return vpRobot::setRobotState(newState);
869 }
870 
871 #elif !defined(VISP_BUILD_SHARED_LIBS)
872 // Work around to avoid warning: libvisp_robot.a(vpRobotUniversalRobots.cpp.o) has no
873 // symbols
874 void dummy_vpRobotUniversalRobots(){};
875 #endif
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:293
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
vpColVector extract(unsigned int r, unsigned int colsize) const
Definition: vpColVector.h:222
std::vector< double > toStdVector() const
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:314
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ functionNotImplementedError
Function not implemented.
Definition: vpException.h:90
@ fatalError
Fatal error.
Definition: vpException.h:96
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
Definition: vpIoTools.cpp:1926
static double rad(double deg)
Definition: vpMath.h:117
static double deg(double rad)
Definition: vpMath.h:110
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:152
std::vector< double > toStdVector() const
Error that can be emited by the vpRobot class and its derivates.
vpHomogeneousMatrix get_eMc() const
void setPositioningVelocity(double velocity)
std::string getRobotModel() const
vpHomogeneousMatrix get_fMc()
vpHomogeneousMatrix get_fMe()
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
void move(const std::string &filename, double velocity_percentage=10.)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void connect(const std::string &ur_address)
bool savePosFile(const std::string &filename, const vpColVector &q)
void set_eMc(const vpHomogeneousMatrix &eMc)
std::shared_ptr< ur_rtde::DashboardClient > m_db_client
vpRobot::vpControlFrameType m_vel_control_frame
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position)
void getForceTorque(const vpRobot::vpControlFrameType frame, vpColVector &force)
std::shared_ptr< ur_rtde::RTDEReceiveInterface > m_rtde_receive
std::shared_ptr< ur_rtde::RTDEControlInterface > m_rtde_control
bool readPosFile(const std::string &filename, vpColVector &q)
int nDof
number of degrees of freedom
Definition: vpRobot.h:103
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:145
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition: vpRobot.cpp:163
vpControlFrameType
Definition: vpRobot.h:76
@ REFERENCE_FRAME
Definition: vpRobot.h:77
@ JOINT_STATE
Definition: vpRobot.h:81
@ TOOL_FRAME
Definition: vpRobot.h:85
@ MIXT_FRAME
Definition: vpRobot.h:87
@ CAMERA_FRAME
Definition: vpRobot.h:83
@ END_EFFECTOR_FRAME
Definition: vpRobot.h:82
vpRobotStateType
Definition: vpRobot.h:65
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition: vpRobot.h:68
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:67
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:66
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:273
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:201
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:251
Implementation of a rotation vector as axis-angle minimal representation.
Class that consider the case of a translation vector.