Visual Servoing Platform  version 3.6.1 under development (2024-04-22)
vpRobotUniversalRobots.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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 *****************************************************************************/
35 
36 #include <visp3/core/vpConfig.h>
37 
38 #if defined(VISP_HAVE_UR_RTDE)
39 
40 #include <visp3/core/vpIoTools.h>
41 #include <visp3/robot/vpRobotUniversalRobots.h>
42 
50 vpRobotUniversalRobots::vpRobotUniversalRobots() : m_rtde_receive(), m_rtde_control(), m_db_client(), m_eMc()
51 {
52  init();
53 }
54 
59 
68 vpRobotUniversalRobots::vpRobotUniversalRobots(const std::string &ur_address)
69  : m_rtde_receive(nullptr), m_rtde_control(nullptr), m_eMc()
70 {
71  init();
72  connect(ur_address);
73 }
74 
81 void vpRobotUniversalRobots::connect(const std::string &ur_address)
82 {
83  if (!m_rtde_receive) {
84  m_rtde_receive = std::make_shared<ur_rtde::RTDEReceiveInterface>(ur_address);
85  }
86  if (!m_rtde_control) {
87  m_rtde_control = std::make_shared<ur_rtde::RTDEControlInterface>(ur_address);
88  }
89  if (!m_db_client) {
90  m_db_client = std::make_shared<ur_rtde::DashboardClient>(ur_address);
91  }
92  if (!m_rtde_receive->isConnected()) {
93  m_rtde_receive->reconnect();
94  if (!m_rtde_receive->isConnected()) {
95  throw(vpException(vpException::fatalError, "Cannot connect UR robot to receive interface"));
96  }
97  }
98  if (!m_rtde_control->isConnected()) {
99  m_rtde_control->reconnect();
100  if (!m_rtde_control->isConnected()) {
101  throw(vpException(vpException::fatalError, "Cannot connect UR robot to control interface"));
102  }
103  }
104  if (!m_db_client->isConnected()) {
105  m_db_client->connect();
106  if (!m_db_client->isConnected()) {
107  throw(vpException(vpException::fatalError, "Cannot connect UR robot to dashboard client"));
108  }
109  }
110 }
111 
116 {
117  if (m_rtde_receive && m_rtde_receive->isConnected()) {
118  m_rtde_receive->disconnect();
119  }
120  if (m_rtde_control && m_rtde_control->isConnected()) {
121  m_rtde_control->disconnect();
122  }
123  if (m_db_client && m_db_client->isConnected()) {
124  m_db_client->disconnect();
125  }
126 }
127 
132 {
133  nDof = 6;
134  m_positioningVelocity = 20.;
135  m_max_joint_speed = vpMath::rad(180.); // deg/s
136  m_max_joint_acceleration = vpMath::rad(800.); // deg/s^2
137  m_max_linear_speed = 0.5; // m/s
138  m_max_linear_acceleration = 2.5; // m/s^2
140 }
141 
154 {
155  vpPoseVector fPe;
157  return vpHomogeneousMatrix(fPe);
158 }
159 
174 {
175  if (!m_rtde_control || !m_rtde_receive) {
176  throw(vpException(vpException::fatalError, "Cannot get UR robot forward kinematics: robot is not connected"));
177  }
178  if (q.size() != 6) {
180  "Cannot get UR robot forward kinematics: joint position vector is not 6-dim (%d)", q.size()));
181  }
182  // Robot modes:
183  // https://sdurobotics.gitlab.io/ur_rtde/api/api.html#_CPPv4N7ur_rtde20RTDEReceiveInterface12getRobotModeEv
184  if (m_rtde_receive->getRobotMode() != 7) {
185  throw(vpException(vpException::fatalError, "Cannot get UR robot forward kinematics: brakes are not released"));
186  }
187 
188  const std::vector<double> q_std = q.toStdVector();
189  std::vector<double> tcp_pose = m_rtde_control->getForwardKinematics(q_std);
190 
191  vpPoseVector f_P_e;
192  for (size_t i = 0; i < 6; i++) {
193  f_P_e[i] = tcp_pose[i];
194  }
195  vpHomogeneousMatrix fMe(f_P_e);
196  return fMe;
197 }
198 
213 {
214  vpPoseVector fPc;
216  return vpHomogeneousMatrix(fPc);
217 }
218 
232 
244 
255 {
256  if (!m_rtde_receive) {
257  throw(vpException(vpException::fatalError, "Cannot get UR robot force/torque: robot is not connected"));
258  }
259 
260  std::vector<double> eFe = m_rtde_receive->getActualTCPForce();
261 
262  switch (frame) {
263  case JOINT_STATE: {
264  throw(vpException(vpException::fatalError, "Cannot get UR force/torque in joint space"));
265  break;
266  }
267  case END_EFFECTOR_FRAME: {
268  force = eFe;
269  break;
270  }
271  case TOOL_FRAME: {
272  // Transform in tool frame
274  vpForceTwistMatrix cWe(cMe);
275  force = cWe * eFe;
276  break;
277  }
278  default: {
279  throw(vpException(vpException::fatalError, "Cannot get UR force/torque: frame not supported"));
280  }
281  }
282 }
283 
288 {
289  if (!m_db_client) {
290  throw(vpException(vpException::fatalError, "Cannot get UR robot PolyScope verson: robot is not connected"));
291  }
292  return m_db_client->polyscopeVersion();
293 }
294 
310 {
311  if (!m_rtde_receive) {
312  throw(vpException(vpException::fatalError, "Cannot get UR robot position: robot is not connected"));
313  }
314  // Robot modes:
315  // https://sdurobotics.gitlab.io/ur_rtde/api/api.html#_CPPv4N7ur_rtde20RTDEReceiveInterface12getRobotModeEv
316  if (m_rtde_receive->getRobotMode() < 4) {
317  throw(vpException(vpException::fatalError, "Cannot get UR robot position: robot is not powered on"));
318  }
319 
320  switch (frame) {
321  case JOINT_STATE: {
322  position = m_rtde_receive->getActualQ();
323  break;
324  }
325  case END_EFFECTOR_FRAME: {
326  std::vector<double> tcp_pose = m_rtde_receive->getActualTCPPose();
327  position.resize(6);
328  for (size_t i = 0; i < tcp_pose.size(); i++) {
329  position[i] = tcp_pose[i];
330  }
331 
332  break;
333  }
334  case TOOL_FRAME: { // same a CAMERA_FRAME
335  std::vector<double> tcp_pose = m_rtde_receive->getActualTCPPose();
336  vpPoseVector fPe;
337  for (size_t i = 0; i < tcp_pose.size(); i++) {
338  fPe[i] = tcp_pose[i];
339  }
340 
341  vpHomogeneousMatrix fMe(fPe);
342  vpHomogeneousMatrix fMc = fMe * m_eMc;
343  vpPoseVector fPc(fMc);
344  position.resize(6);
345  for (size_t i = 0; i < 6; i++) {
346  position[i] = fPc[i];
347  }
348  break;
349  }
350  default: {
351  throw(vpException(vpException::fatalError, "Cannot get UR cartesian position: wrong method"));
352  }
353  }
354 }
355 
366 {
367  if (!m_rtde_receive) {
368  throw(vpException(vpException::fatalError, "Cannot get UR robot position: robot is not connected"));
369  }
370  if (frame == JOINT_STATE) {
371  throw(vpException(vpException::fatalError, "Cannot get UR joint position as a pose vector"));
372  }
373 
374  switch (frame) {
375  case END_EFFECTOR_FRAME:
376  case TOOL_FRAME: {
377  vpColVector position;
378  getPosition(frame, position);
379  for (size_t i = 0; i < 6; i++) {
380  pose[i] = position[i];
381  }
382  break;
383  }
384  default: {
385  throw(vpException(vpException::fatalError, "Cannot get Franka cartesian position: not implemented"));
386  }
387  }
388 }
389 
394 {
395  if (!m_db_client) {
396  throw(vpException(vpException::fatalError, "Cannot get UR robot model: robot is not connected"));
397  }
398  return m_db_client->getRobotModel();
399 }
405 {
406  if (!m_rtde_receive) {
407  throw(vpException(vpException::fatalError, "Cannot get UR robot mode: robot is not connected"));
408  }
409  // Robot modes:
410  // https://sdurobotics.gitlab.io/ur_rtde/api/api.html#_CPPv4N7ur_rtde20RTDEReceiveInterface12getRobotModeEv
411  return (m_rtde_receive->getRobotMode());
412 }
413 
420 
435 {
436  if (frame == vpRobot::JOINT_STATE) {
437  throw(vpException(vpException::fatalError, "Cannot set UR robot cartesian position: joint state is specified"));
438  }
439  vpColVector position(pose);
440  setPosition(frame, position);
441 }
442 
458 {
459  if (!m_rtde_control) {
460  throw(vpException(vpException::fatalError, "Cannot set UR robot position: robot is not connected"));
461  }
462 
463  // Robot modes:
464  // https://sdurobotics.gitlab.io/ur_rtde/api/api.html#_CPPv4N7ur_rtde20RTDEReceiveInterface12getRobotModeEv
465  if (m_rtde_receive->getRobotMode() != 7) {
466  throw(vpException(vpException::fatalError, "Cannot set UR robot position: brakes are not released"));
467  }
468 
469  if (position.size() != 6) {
471  "Cannot set UR robot position: position vector is not a 6-dim vector (%d)", position.size()));
472  }
473 
475  std::cout << "Robot is not in position-based control. "
476  "Modification of the robot state"
477  << std::endl;
479  }
480 
481  if (frame == vpRobot::JOINT_STATE) {
482  double speed_factor = m_positioningVelocity / 100.;
483  std::vector<double> new_q = position.toStdVector();
484  m_rtde_control->moveJ(new_q, m_max_joint_speed * speed_factor);
485  }
486  else if (frame == vpRobot::END_EFFECTOR_FRAME) {
487  double speed_factor = m_positioningVelocity / 100.;
488  std::vector<double> new_pose = position.toStdVector();
489  // Move synchronously to ensure a the blocking behaviour
490  m_rtde_control->moveL(new_pose, m_max_linear_speed * speed_factor);
491  }
492  else if (frame == vpRobot::CAMERA_FRAME) {
493  double speed_factor = m_positioningVelocity / 100.;
494 
495  vpTranslationVector f_t_c(position.extract(0, 3));
496  vpThetaUVector f_tu_c(position.extract(3, 3));
497  vpHomogeneousMatrix fMc(f_t_c, f_tu_c);
498  vpHomogeneousMatrix fMe = fMc * m_eMc.inverse();
499  vpPoseVector fPe(fMe);
500  std::vector<double> new_pose = fPe.toStdVector();
501  // Move synchronously to ensure a the blocking behaviour
502  m_rtde_control->moveL(new_pose, m_max_linear_speed * speed_factor);
503  }
504  else {
506  "Cannot move the robot to a cartesian position. Only joint positioning 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  }
639  else if (frame == vpRobot::REFERENCE_FRAME) {
640  double dt = 1.0 / 1000; // 2ms
641  double acceleration = 0.25;
642  m_rtde_control->speedL(vel_sat.toStdVector(), acceleration, dt);
643  }
644  else if (frame == vpRobot::END_EFFECTOR_FRAME) {
645  double dt = 1.0 / 1000; // 2ms
646  double acceleration = 0.25;
647  vpVelocityTwistMatrix fVe(get_fMe(), false);
648  m_rtde_control->speedL((fVe * vel_sat).toStdVector(), acceleration, dt);
649  }
650  else if (frame == vpRobot::CAMERA_FRAME) {
651  double dt = 1.0 / 1000; // 2ms
652  double acceleration = 0.25;
654  m_rtde_control->speedL(w_v_e.toStdVector(), acceleration, dt);
655  }
656  else {
658  "Cannot move the robot in velocity in the specified frame: not implemented"));
659  }
660 }
661 
670 {
671  if (!m_rtde_control) {
672  throw(vpException(vpException::fatalError, "Cannot stop UR robot: robot is not connected"));
673  }
674 
676 }
677 
686 void vpRobotUniversalRobots::move(const std::string &filename, double velocity_percentage)
687 {
688  vpColVector q;
689 
690  readPosFile(filename, q);
692  setPositioningVelocity(velocity_percentage);
694 }
695 
733 bool vpRobotUniversalRobots::readPosFile(const std::string &filename, vpColVector &q)
734 {
735  std::ifstream fd(filename.c_str(), std::ios::in);
736 
737  if (!fd.is_open()) {
738  return false;
739  }
740 
741  std::string line;
742  std::string key("R:");
743  std::string id("#UR - Joint position file");
744  bool pos_found = false;
745  int lineNum = 0;
746  size_t njoints = static_cast<size_t>(nDof);
747 
748  q.resize(njoints);
749 
750  while (std::getline(fd, line)) {
751  lineNum++;
752  if (lineNum == 1) {
753  if (!(line.compare(0, id.size(), id) == 0)) { // check if Afma6 position file
754  std::cout << "Error: this position file " << filename << " is not for Universal Robots" << std::endl;
755  return false;
756  }
757  }
758  if ((line.compare(0, 1, "#") == 0)) { // skip comment
759  continue;
760  }
761  if ((line.compare(0, key.size(), key) == 0)) { // decode position
762  // check if there are at least njoint values in the line
763  std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
764  if (chain.size() < njoints + 1) // try to split with tab separator
765  chain = vpIoTools::splitChain(line, std::string("\t"));
766  if (chain.size() < njoints + 1)
767  continue;
768 
769  std::istringstream ss(line);
770  std::string key_;
771  ss >> key_;
772  for (unsigned int i = 0; i < njoints; i++)
773  ss >> q[i];
774  pos_found = true;
775  break;
776  }
777  }
778 
779  // converts rotations from degrees into radians
780  for (unsigned int i = 0; i < njoints; i++) {
781  q[i] = vpMath::rad(q[i]);
782  }
783 
784  fd.close();
785 
786  if (!pos_found) {
787  std::cout << "Error: unable to find a position for UR robot in " << filename << std::endl;
788  return false;
789  }
790 
791  return true;
792 }
793 
811 bool vpRobotUniversalRobots::savePosFile(const std::string &filename, const vpColVector &q)
812 {
813 
814  FILE *fd;
815  fd = fopen(filename.c_str(), "w");
816  if (fd == nullptr)
817  return false;
818 
819  fprintf(fd, "#UR - Joint position file\n"
820  "#\n"
821  "# R: q1 q2 q3 q4 q5 q6\n"
822  "# with joint positions q1 to q6 expressed in degrees\n"
823  "#\n");
824 
825  // Save positions in mm and deg
826  fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n", vpMath::deg(q[0]), vpMath::deg(q[1]), vpMath::deg(q[2]),
827  vpMath::deg(q[3]), vpMath::deg(q[4]), vpMath::deg(q[5]));
828 
829  fclose(fd);
830  return (true);
831 }
832 
839 {
840  switch (newState) {
841  case vpRobot::STATE_STOP: {
842  // Start primitive STOP only if the current state is Velocity
844  if (!m_rtde_control) {
845  throw(vpException(vpException::fatalError, "Cannot stop UR robot: robot is not connected"));
846  }
847  m_rtde_control->speedStop();
848  }
849  break;
850  }
853  if (!m_rtde_control) {
854  throw(vpException(vpException::fatalError, "Cannot stop UR robot: robot is not connected"));
855  }
856  m_rtde_control->speedStop();
857  }
858  else {
859  // std::cout << "Change the control mode from stop to position control" << std::endl;
860  }
861  break;
862  }
865  std::cout << "Change the control mode from stop to velocity control.\n";
866  }
867  break;
868  }
869  default:
870  break;
871  }
872 
873  return vpRobot::setRobotState(newState);
874 }
875 
876 #elif !defined(VISP_BUILD_SHARED_LIBS)
877 // Work around to avoid warning: libvisp_robot.a(vpRobotUniversalRobots.cpp.o) has no symbols
878 void dummy_vpRobotUniversalRobots() { };
879 #endif
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:339
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
vpColVector extract(unsigned int r, unsigned int colsize) const
Definition: vpColVector.h:367
std::vector< double > toStdVector() const
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1056
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ functionNotImplementedError
Function not implemented.
Definition: vpException.h:78
@ fatalError
Fatal error.
Definition: vpException.h:84
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:2425
static double rad(double deg)
Definition: vpMath.h:127
static double deg(double rad)
Definition: vpMath.h:117
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:189
std::vector< double > toStdVector() const
Error that can be emitted by the vpRobot class and its derivatives.
@ wrongStateError
Wrong robot state.
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position) vp_override
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position) vp_override
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) vp_override
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 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:102
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:153
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition: vpRobot.cpp:160
vpControlFrameType
Definition: vpRobot.h:75
@ REFERENCE_FRAME
Definition: vpRobot.h:76
@ JOINT_STATE
Definition: vpRobot.h:80
@ TOOL_FRAME
Definition: vpRobot.h:84
@ MIXT_FRAME
Definition: vpRobot.h:86
@ CAMERA_FRAME
Definition: vpRobot.h:82
@ END_EFFECTOR_FRAME
Definition: vpRobot.h:81
vpRobotStateType
Definition: vpRobot.h:63
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition: vpRobot.h:66
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:65
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:64
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:270
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:198
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:248
Implementation of a rotation vector as axis-angle minimal representation.
Class that consider the case of a translation vector.