Visual Servoing Platform  version 3.6.1 under development (2024-10-09)
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 
43 BEGIN_VISP_NAMESPACE
51  vpRobotUniversalRobots::vpRobotUniversalRobots() : m_rtde_receive(), m_rtde_control(), m_db_client(), m_eMc()
52 {
53  init();
54 }
55 
60 
69 vpRobotUniversalRobots::vpRobotUniversalRobots(const std::string &ur_address)
70  : m_rtde_receive(nullptr), m_rtde_control(nullptr), m_eMc()
71 {
72  init();
73  connect(ur_address);
74 }
75 
82 void vpRobotUniversalRobots::connect(const std::string &ur_address)
83 {
84  if (!m_rtde_receive) {
85  m_rtde_receive = std::make_shared<ur_rtde::RTDEReceiveInterface>(ur_address);
86  }
87  if (!m_rtde_control) {
88  m_rtde_control = std::make_shared<ur_rtde::RTDEControlInterface>(ur_address);
89  }
90  if (!m_db_client) {
91  m_db_client = std::make_shared<ur_rtde::DashboardClient>(ur_address);
92  }
93  if (!m_rtde_receive->isConnected()) {
94  m_rtde_receive->reconnect();
95  if (!m_rtde_receive->isConnected()) {
96  throw(vpException(vpException::fatalError, "Cannot connect UR robot to receive interface"));
97  }
98  }
99  if (!m_rtde_control->isConnected()) {
100  m_rtde_control->reconnect();
101  if (!m_rtde_control->isConnected()) {
102  throw(vpException(vpException::fatalError, "Cannot connect UR robot to control interface"));
103  }
104  }
105  if (!m_db_client->isConnected()) {
106  m_db_client->connect();
107  if (!m_db_client->isConnected()) {
108  throw(vpException(vpException::fatalError, "Cannot connect UR robot to dashboard client"));
109  }
110  }
111 }
112 
117 {
118  if (m_rtde_receive && m_rtde_receive->isConnected()) {
119  m_rtde_receive->disconnect();
120  }
121  if (m_rtde_control && m_rtde_control->isConnected()) {
122  m_rtde_control->disconnect();
123  }
124  if (m_db_client && m_db_client->isConnected()) {
125  m_db_client->disconnect();
126  }
127 }
128 
133 {
134  nDof = 6;
135  m_positioningVelocity = 20.;
136  m_max_joint_speed = vpMath::rad(180.); // deg/s
137  m_max_joint_acceleration = vpMath::rad(800.); // deg/s^2
138  m_max_linear_speed = 0.5; // m/s
139  m_max_linear_acceleration = 2.5; // m/s^2
141 }
142 
155 {
156  vpPoseVector fPe;
158  return vpHomogeneousMatrix(fPe);
159 }
160 
175 {
176  if (!m_rtde_control || !m_rtde_receive) {
177  throw(vpException(vpException::fatalError, "Cannot get UR robot forward kinematics: robot is not connected"));
178  }
179  if (q.size() != 6) {
181  "Cannot get UR robot forward kinematics: joint position vector is not 6-dim (%d)", q.size()));
182  }
183  // Robot modes:
184  // https://sdurobotics.gitlab.io/ur_rtde/api/api.html#_CPPv4N7ur_rtde20RTDEReceiveInterface12getRobotModeEv
185  if (m_rtde_receive->getRobotMode() != 7) {
186  throw(vpException(vpException::fatalError, "Cannot get UR robot forward kinematics: brakes are not released"));
187  }
188 
189  const std::vector<double> q_std = q.toStdVector();
190  std::vector<double> tcp_pose = m_rtde_control->getForwardKinematics(q_std);
191 
192  vpPoseVector f_P_e;
193  for (size_t i = 0; i < 6; i++) {
194  f_P_e[i] = tcp_pose[i];
195  }
196  vpHomogeneousMatrix fMe(f_P_e);
197  return fMe;
198 }
199 
214 {
215  vpPoseVector fPc;
217  return vpHomogeneousMatrix(fPc);
218 }
219 
233 
245 
256 {
257  if (!m_rtde_receive) {
258  throw(vpException(vpException::fatalError, "Cannot get UR robot force/torque: robot is not connected"));
259  }
260 
261  std::vector<double> eFe = m_rtde_receive->getActualTCPForce();
262 
263  switch (frame) {
264  case JOINT_STATE: {
265  throw(vpException(vpException::fatalError, "Cannot get UR force/torque in joint space"));
266  break;
267  }
268  case END_EFFECTOR_FRAME: {
269  force = eFe;
270  break;
271  }
272  case TOOL_FRAME: {
273  // Transform in tool frame
275  vpForceTwistMatrix cWe(cMe);
276  force = cWe * eFe;
277  break;
278  }
279  default: {
280  throw(vpException(vpException::fatalError, "Cannot get UR force/torque: frame not supported"));
281  }
282  }
283 }
284 
289 {
290  if (!m_db_client) {
291  throw(vpException(vpException::fatalError, "Cannot get UR robot PolyScope verson: robot is not connected"));
292  }
293  return m_db_client->polyscopeVersion();
294 }
295 
311 {
312  if (!m_rtde_receive) {
313  throw(vpException(vpException::fatalError, "Cannot get UR robot position: robot is not connected"));
314  }
315  // Robot modes:
316  // https://sdurobotics.gitlab.io/ur_rtde/api/api.html#_CPPv4N7ur_rtde20RTDEReceiveInterface12getRobotModeEv
317  if (m_rtde_receive->getRobotMode() < 4) {
318  throw(vpException(vpException::fatalError, "Cannot get UR robot position: robot is not powered on"));
319  }
320 
321  switch (frame) {
322  case JOINT_STATE: {
323  position = m_rtde_receive->getActualQ();
324  break;
325  }
326  case END_EFFECTOR_FRAME: {
327  std::vector<double> tcp_pose = m_rtde_receive->getActualTCPPose();
328  position.resize(6);
329  for (size_t i = 0; i < tcp_pose.size(); i++) {
330  position[i] = tcp_pose[i];
331  }
332 
333  break;
334  }
335  case TOOL_FRAME: { // same a CAMERA_FRAME
336  std::vector<double> tcp_pose = m_rtde_receive->getActualTCPPose();
337  vpPoseVector fPe;
338  for (size_t i = 0; i < tcp_pose.size(); i++) {
339  fPe[i] = tcp_pose[i];
340  }
341 
342  vpHomogeneousMatrix fMe(fPe);
343  vpHomogeneousMatrix fMc = fMe * m_eMc;
344  vpPoseVector fPc(fMc);
345  position.resize(6);
346  for (size_t i = 0; i < 6; i++) {
347  position[i] = fPc[i];
348  }
349  break;
350  }
351  default: {
352  throw(vpException(vpException::fatalError, "Cannot get UR cartesian position: wrong method"));
353  }
354  }
355 }
356 
367 {
368  if (!m_rtde_receive) {
369  throw(vpException(vpException::fatalError, "Cannot get UR robot position: robot is not connected"));
370  }
371  if (frame == JOINT_STATE) {
372  throw(vpException(vpException::fatalError, "Cannot get UR joint position as a pose vector"));
373  }
374 
375  switch (frame) {
376  case END_EFFECTOR_FRAME:
377  case TOOL_FRAME: {
378  vpColVector position;
379  getPosition(frame, position);
380  for (size_t i = 0; i < 6; i++) {
381  pose[i] = position[i];
382  }
383  break;
384  }
385  default: {
386  throw(vpException(vpException::fatalError, "Cannot get Franka cartesian position: not implemented"));
387  }
388  }
389 }
390 
395 {
396  if (!m_db_client) {
397  throw(vpException(vpException::fatalError, "Cannot get UR robot model: robot is not connected"));
398  }
399  return m_db_client->getRobotModel();
400 }
406 {
407  if (!m_rtde_receive) {
408  throw(vpException(vpException::fatalError, "Cannot get UR robot mode: robot is not connected"));
409  }
410  // Robot modes:
411  // https://sdurobotics.gitlab.io/ur_rtde/api/api.html#_CPPv4N7ur_rtde20RTDEReceiveInterface12getRobotModeEv
412  return (m_rtde_receive->getRobotMode());
413 }
414 
421 
436 {
437  if (frame == vpRobot::JOINT_STATE) {
438  throw(vpException(vpException::fatalError, "Cannot set UR robot cartesian position: joint state is specified"));
439  }
440  vpColVector position(pose);
441  setPosition(frame, position);
442 }
443 
459 {
460  if (!m_rtde_control) {
461  throw(vpException(vpException::fatalError, "Cannot set UR robot position: robot is not connected"));
462  }
463 
464  // Robot modes:
465  // https://sdurobotics.gitlab.io/ur_rtde/api/api.html#_CPPv4N7ur_rtde20RTDEReceiveInterface12getRobotModeEv
466  if (m_rtde_receive->getRobotMode() != 7) {
467  throw(vpException(vpException::fatalError, "Cannot set UR robot position: brakes are not released"));
468  }
469 
470  if (position.size() != 6) {
472  "Cannot set UR robot position: position vector is not a 6-dim vector (%d)", position.size()));
473  }
474 
476  std::cout << "Robot is not in position-based control. "
477  "Modification of the robot state"
478  << std::endl;
480  }
481 
482  if (frame == vpRobot::JOINT_STATE) {
483  double speed_factor = m_positioningVelocity / 100.;
484  std::vector<double> new_q = position.toStdVector();
485  m_rtde_control->moveJ(new_q, m_max_joint_speed * speed_factor);
486  }
487  else if (frame == vpRobot::END_EFFECTOR_FRAME) {
488  double speed_factor = m_positioningVelocity / 100.;
489  std::vector<double> new_pose = position.toStdVector();
490  // Move synchronously to ensure a the blocking behaviour
491  m_rtde_control->moveL(new_pose, m_max_linear_speed * speed_factor);
492  }
493  else if (frame == vpRobot::CAMERA_FRAME) {
494  double speed_factor = m_positioningVelocity / 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  }
505  else {
507  "Cannot move the robot to a cartesian position. Only joint positioning is implemented"));
508  }
509 }
510 
603 {
606  "Cannot send a velocity to the robot: robot is not in velocity control state "
607  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
608  }
609 
610  vpColVector vel_sat(6);
611  m_vel_control_frame = frame;
612 
613  // Velocity saturation
614  switch (frame) {
615  // saturation in cartesian space
619  case vpRobot::MIXT_FRAME: {
620  vpColVector vel_max(6);
621 
622  for (unsigned int i = 0; i < 3; i++)
623  vel_max[i] = getMaxTranslationVelocity();
624  for (unsigned int i = 3; i < 6; i++)
625  vel_max[i] = getMaxRotationVelocity();
626 
627  vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
628 
629  break;
630  }
631  // Saturation in joint space
632  case vpRobot::JOINT_STATE: {
633  vpColVector vel_max(6);
634  vel_max = getMaxRotationVelocity();
635  vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
636  }
637  }
638 
639  if (frame == vpRobot::JOINT_STATE) {
640  double dt = 1.0 / 1000; // 2ms
641  double acceleration = 0.5;
642  m_rtde_control->speedJ(vel_sat.toStdVector(), acceleration, dt);
643  }
644  else if (frame == vpRobot::REFERENCE_FRAME) {
645  double dt = 1.0 / 1000; // 2ms
646  double acceleration = 0.25;
647  m_rtde_control->speedL(vel_sat.toStdVector(), acceleration, dt);
648  }
649  else if (frame == vpRobot::END_EFFECTOR_FRAME) {
650  double dt = 1.0 / 1000; // 2ms
651  double acceleration = 0.25;
652  vpVelocityTwistMatrix fVe(get_fMe(), false);
653  m_rtde_control->speedL((fVe * vel_sat).toStdVector(), acceleration, dt);
654  }
655  else if (frame == vpRobot::CAMERA_FRAME) {
656  double dt = 1.0 / 1000; // 2ms
657  double acceleration = 0.25;
659  m_rtde_control->speedL(w_v_e.toStdVector(), acceleration, dt);
660  }
661  else {
663  "Cannot move the robot in velocity in the specified frame: not implemented"));
664  }
665 }
666 
675 {
676  if (!m_rtde_control) {
677  throw(vpException(vpException::fatalError, "Cannot stop UR robot: robot is not connected"));
678  }
679 
681 }
682 
691 void vpRobotUniversalRobots::move(const std::string &filename, double velocity_percentage)
692 {
693  vpColVector q;
694 
695  readPosFile(filename, q);
697  setPositioningVelocity(velocity_percentage);
699 }
700 
738 bool vpRobotUniversalRobots::readPosFile(const std::string &filename, vpColVector &q)
739 {
740  std::ifstream fd(filename.c_str(), std::ios::in);
741 
742  if (!fd.is_open()) {
743  return false;
744  }
745 
746  std::string line;
747  std::string key("R:");
748  std::string id("#UR - Joint position file");
749  bool pos_found = false;
750  int lineNum = 0;
751  size_t njoints = static_cast<size_t>(nDof);
752 
753  q.resize(njoints);
754 
755  while (std::getline(fd, line)) {
756  lineNum++;
757  if (lineNum == 1) {
758  if (!(line.compare(0, id.size(), id) == 0)) { // check if Afma6 position file
759  std::cout << "Error: this position file " << filename << " is not for Universal Robots" << std::endl;
760  return false;
761  }
762  }
763  if ((line.compare(0, 1, "#") == 0)) { // skip comment
764  continue;
765  }
766  if ((line.compare(0, key.size(), key) == 0)) { // decode position
767  // check if there are at least njoint values in the line
768  std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
769  if (chain.size() < njoints + 1) // try to split with tab separator
770  chain = vpIoTools::splitChain(line, std::string("\t"));
771  if (chain.size() < njoints + 1)
772  continue;
773 
774  std::istringstream ss(line);
775  std::string key_;
776  ss >> key_;
777  for (unsigned int i = 0; i < njoints; i++)
778  ss >> q[i];
779  pos_found = true;
780  break;
781  }
782  }
783 
784  // converts rotations from degrees into radians
785  for (unsigned int i = 0; i < njoints; i++) {
786  q[i] = vpMath::rad(q[i]);
787  }
788 
789  fd.close();
790 
791  if (!pos_found) {
792  std::cout << "Error: unable to find a position for UR robot in " << filename << std::endl;
793  return false;
794  }
795 
796  return true;
797 }
798 
816 bool vpRobotUniversalRobots::savePosFile(const std::string &filename, const vpColVector &q)
817 {
818 
819  FILE *fd;
820  fd = fopen(filename.c_str(), "w");
821  if (fd == nullptr)
822  return false;
823 
824  fprintf(fd, "#UR - Joint position file\n"
825  "#\n"
826  "# R: q1 q2 q3 q4 q5 q6\n"
827  "# with joint positions q1 to q6 expressed in degrees\n"
828  "#\n");
829 
830  // Save positions in mm and deg
831  fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n", vpMath::deg(q[0]), vpMath::deg(q[1]), vpMath::deg(q[2]),
832  vpMath::deg(q[3]), vpMath::deg(q[4]), vpMath::deg(q[5]));
833 
834  fclose(fd);
835  return (true);
836 }
837 
844 {
845  switch (newState) {
846  case vpRobot::STATE_STOP: {
847  // Start primitive STOP only if the current state is Velocity
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  }
854  break;
855  }
858  if (!m_rtde_control) {
859  throw(vpException(vpException::fatalError, "Cannot stop UR robot: robot is not connected"));
860  }
861  m_rtde_control->speedStop();
862  }
863  else {
864  // std::cout << "Change the control mode from stop to position control" << std::endl;
865  }
866  break;
867  }
870  std::cout << "Change the control mode from stop to velocity control.\n";
871  }
872  break;
873  }
874  default:
875  break;
876  }
877 
878  return vpRobot::setRobotState(newState);
879 }
880 END_VISP_NAMESPACE
881 #elif !defined(VISP_BUILD_SHARED_LIBS)
882 // Work around to avoid warning: libvisp_robot.a(vpRobotUniversalRobots.cpp.o) has no symbols
883 void dummy_vpRobotUniversalRobots() { };
884 #endif
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:349
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
vpColVector extract(unsigned int r, unsigned int colsize) const
Definition: vpColVector.h:405
std::vector< double > toStdVector() const
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1143
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ functionNotImplementedError
Function not implemented.
Definition: vpException.h:66
@ fatalError
Fatal error.
Definition: vpException.h:72
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:1661
static double rad(double deg)
Definition: vpMath.h:129
static double deg(double rad)
Definition: vpMath.h:119
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:203
std::vector< double > toStdVector() const
Error that can be emitted by the vpRobot class and its derivatives.
@ wrongStateError
Wrong robot state.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel) VP_OVERRIDE
vpHomogeneousMatrix get_eMc() const
void setPositioningVelocity(double velocity)
std::string getRobotModel() const
vpHomogeneousMatrix get_fMc()
vpHomogeneousMatrix get_fMe()
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position) VP_OVERRIDE
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
void move(const std::string &filename, double velocity_percentage=10.)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position) 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:104
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:155
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition: vpRobot.cpp:164
vpControlFrameType
Definition: vpRobot.h:77
@ REFERENCE_FRAME
Definition: vpRobot.h:78
@ JOINT_STATE
Definition: vpRobot.h:82
@ TOOL_FRAME
Definition: vpRobot.h:86
@ MIXT_FRAME
Definition: vpRobot.h:88
@ CAMERA_FRAME
Definition: vpRobot.h:84
@ END_EFFECTOR_FRAME
Definition: vpRobot.h:83
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:274
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:202
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:252
Implementation of a rotation vector as axis-angle minimal representation.
Class that consider the case of a translation vector.