Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
39
40#include <visp3/core/vpIoTools.h>
41#include <visp3/robot/vpRobotUniversalRobots.h>
42
50vpRobotUniversalRobots::vpRobotUniversalRobots() : m_rtde_receive(), m_rtde_control(), m_db_client(), m_eMc()
51{
52 init();
53}
54
59
69 : m_rtde_receive(nullptr), m_rtde_control(nullptr), m_eMc()
70{
71 init();
72 connect(ur_address);
73}
74
81void 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;
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
159
174{
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
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) {
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 } else if (frame == vpRobot::END_EFFECTOR_FRAME) {
486 double speed_factor = m_positioningVelocity / 100.;
487 std::vector<double> new_pose = position.toStdVector();
488 // Move synchronously to ensure a the blocking behaviour
489 m_rtde_control->moveL(new_pose, m_max_linear_speed * speed_factor);
490 } else if (frame == vpRobot::CAMERA_FRAME) {
491 double speed_factor = m_positioningVelocity / 100.;
492
493 vpTranslationVector f_t_c(position.extract(0, 3));
494 vpThetaUVector f_tu_c(position.extract(3, 3));
495 vpHomogeneousMatrix fMc(f_t_c, f_tu_c);
496 vpHomogeneousMatrix fMe = fMc * m_eMc.inverse();
497 vpPoseVector fPe(fMe);
498 std::vector<double> new_pose = fPe.toStdVector();
499 // Move synchronously to ensure a the blocking behaviour
500 m_rtde_control->moveL(new_pose, m_max_linear_speed * speed_factor);
501 } else {
503 "Cannot move the robot to a cartesian position. Only joint positioning is implemented"));
504 }
505}
506
595{
598 "Cannot send a velocity to the robot: robot is not in velocity control state "
599 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
600 }
601
602 vpColVector vel_sat(6);
603 m_vel_control_frame = frame;
604
605 // Velocity saturation
606 switch (frame) {
607 // saturation in cartesian space
611 case vpRobot::MIXT_FRAME: {
612 vpColVector vel_max(6);
613
614 for (unsigned int i = 0; i < 3; i++)
615 vel_max[i] = getMaxTranslationVelocity();
616 for (unsigned int i = 3; i < 6; i++)
617 vel_max[i] = getMaxRotationVelocity();
618
619 vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
620
621 break;
622 }
623 // Saturation in joint space
625 vpColVector vel_max(6);
626 vel_max = getMaxRotationVelocity();
627 vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
628 }
629 }
630
631 if (frame == vpRobot::JOINT_STATE) {
632 double dt = 1.0 / 1000; // 2ms
633 double acceleration = 0.5;
634 m_rtde_control->speedJ(vel_sat.toStdVector(), acceleration, dt);
635 } else if (frame == vpRobot::REFERENCE_FRAME) {
636 double dt = 1.0 / 1000; // 2ms
637 double acceleration = 0.25;
638 m_rtde_control->speedL(vel_sat.toStdVector(), acceleration, dt);
639 } else if (frame == vpRobot::END_EFFECTOR_FRAME) {
640 double dt = 1.0 / 1000; // 2ms
641 double acceleration = 0.25;
642 vpVelocityTwistMatrix fVe(get_fMe(), false);
643 m_rtde_control->speedL((fVe * vel_sat).toStdVector(), acceleration, dt);
644 } else if (frame == vpRobot::CAMERA_FRAME) {
645 double dt = 1.0 / 1000; // 2ms
646 double acceleration = 0.25;
648 m_rtde_control->speedL(w_v_e.toStdVector(), acceleration, dt);
649 } else {
651 "Cannot move the robot in velocity in the specified frame: not implemented"));
652 }
653}
654
663{
664 if (!m_rtde_control) {
665 throw(vpException(vpException::fatalError, "Cannot stop UR robot: robot is not connected"));
666 }
667
669}
670
679void vpRobotUniversalRobots::move(const std::string &filename, double velocity_percentage)
680{
681 vpColVector q;
682
683 readPosFile(filename, q);
685 setPositioningVelocity(velocity_percentage);
687}
688
726bool vpRobotUniversalRobots::readPosFile(const std::string &filename, vpColVector &q)
727{
728 std::ifstream fd(filename.c_str(), std::ios::in);
729
730 if (!fd.is_open()) {
731 return false;
732 }
733
734 std::string line;
735 std::string key("R:");
736 std::string id("#UR - Joint position file");
737 bool pos_found = false;
738 int lineNum = 0;
739 size_t njoints = static_cast<size_t>(nDof);
740
741 q.resize(njoints);
742
743 while (std::getline(fd, line)) {
744 lineNum++;
745 if (lineNum == 1) {
746 if (!(line.compare(0, id.size(), id) == 0)) { // check if Afma6 position file
747 std::cout << "Error: this position file " << filename << " is not for Universal Robots" << std::endl;
748 return false;
749 }
750 }
751 if ((line.compare(0, 1, "#") == 0)) { // skip comment
752 continue;
753 }
754 if ((line.compare(0, key.size(), key) == 0)) { // decode position
755 // check if there are at least njoint values in the line
756 std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
757 if (chain.size() < njoints + 1) // try to split with tab separator
758 chain = vpIoTools::splitChain(line, std::string("\t"));
759 if (chain.size() < njoints + 1)
760 continue;
761
762 std::istringstream ss(line);
763 std::string key_;
764 ss >> key_;
765 for (unsigned int i = 0; i < njoints; i++)
766 ss >> q[i];
767 pos_found = true;
768 break;
769 }
770 }
771
772 // converts rotations from degrees into radians
773 for (unsigned int i = 0; i < njoints; i++) {
774 q[i] = vpMath::rad(q[i]);
775 }
776
777 fd.close();
778
779 if (!pos_found) {
780 std::cout << "Error: unable to find a position for UR robot in " << filename << std::endl;
781 return false;
782 }
783
784 return true;
785}
786
804bool vpRobotUniversalRobots::savePosFile(const std::string &filename, const vpColVector &q)
805{
806
807 FILE *fd;
808 fd = fopen(filename.c_str(), "w");
809 if (fd == NULL)
810 return false;
811
812 fprintf(fd, "#UR - Joint position file\n"
813 "#\n"
814 "# R: q1 q2 q3 q4 q5 q6\n"
815 "# with joint positions q1 to q6 expressed in degrees\n"
816 "#\n");
817
818 // Save positions in mm and deg
819 fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n", vpMath::deg(q[0]), vpMath::deg(q[1]), vpMath::deg(q[2]),
820 vpMath::deg(q[3]), vpMath::deg(q[4]), vpMath::deg(q[5]));
821
822 fclose(fd);
823 return (true);
824}
825
832{
833 switch (newState) {
834 case vpRobot::STATE_STOP: {
835 // Start primitive STOP only if the current state is Velocity
837 if (!m_rtde_control) {
838 throw(vpException(vpException::fatalError, "Cannot stop UR robot: robot is not connected"));
839 }
840 m_rtde_control->speedStop();
841 }
842 break;
843 }
846 if (!m_rtde_control) {
847 throw(vpException(vpException::fatalError, "Cannot stop UR robot: robot is not connected"));
848 }
849 m_rtde_control->speedStop();
850 } else {
851 // std::cout << "Change the control mode from stop to position control" << std::endl;
852 }
853 break;
854 }
857 std::cout << "Change the control mode from stop to velocity control.\n";
858 }
859 break;
860 }
861 default:
862 break;
863 }
864
865 return vpRobot::setRobotState(newState);
866}
867
868#elif !defined(VISP_BUILD_SHARED_LIBS)
869// Work around to avoid warning: libvisp_robot.a(vpRobotUniversalRobots.cpp.o) has no
870// symbols
871void dummy_vpRobotUniversalRobots(){};
872#endif
unsigned int size() const
Return the number of elements of the 2D array.
Definition vpArray2D.h:292
Implementation of column vector and the associated operations.
vpColVector extract(unsigned int r, unsigned int colsize) const
std::vector< double > toStdVector() const
void resize(unsigned int i, bool flagNullify=true)
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)
static double rad(double deg)
Definition vpMath.h:116
static double deg(double rad)
Definition vpMath.h:106
Implementation of a pose vector and operations on poses.
std::vector< double > toStdVector() const
Error that can be emitted by the vpRobot class and its derivatives.
@ wrongStateError
Wrong robot state.
vpHomogeneousMatrix get_eMc() const
void setPositioningVelocity(double velocity)
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:100
virtual vpRobotStateType getRobotState(void) const
Definition vpRobot.h:142
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition vpRobot.cpp:160
vpControlFrameType
Definition vpRobot.h:73
@ REFERENCE_FRAME
Definition vpRobot.h:74
@ JOINT_STATE
Definition vpRobot.h:78
@ TOOL_FRAME
Definition vpRobot.h:82
@ MIXT_FRAME
Definition vpRobot.h:84
@ CAMERA_FRAME
Definition vpRobot.h:80
@ END_EFFECTOR_FRAME
Definition vpRobot.h:79
vpRobotStateType
Definition vpRobot.h:62
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition vpRobot.h:65
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition vpRobot.h:64
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition vpRobot.h:63
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.