Visual Servoing Platform  version 3.6.1 under development (2024-06-21)
testUniversalRobotsGetData.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Test Universal Robots behavior
32  */
33 
40 #include <iostream>
41 
42 #include <visp3/core/vpConfig.h>
43 
44 #if defined(VISP_HAVE_UR_RTDE)
45 
46 #include <visp3/robot/vpRobotUniversalRobots.h>
47 
48 int main(int argc, char **argv)
49 {
50 #ifdef ENABLE_VISP_NAMESPACE
51  using namespace VISP_NAMESPACE_NAME;
52 #endif
53  std::string robot_ip = "192.168.0.100";
54 
55  for (int i = 1; i < argc; i++) {
56  if (std::string(argv[i]) == "--ip" && i + 1 < argc) {
57  robot_ip = std::string(argv[i + 1]);
58  }
59  else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
60  std::cout << argv[0] << " [--ip " << robot_ip << "] [--help] [-h]"
61  << "\n";
62  return EXIT_SUCCESS;
63  }
64  }
65 
66  try {
67  std::cout << "-- Start test 1/3" << std::endl;
69  robot.connect(robot_ip);
70 
71  std::shared_ptr<ur_rtde::RTDEReceiveInterface> rtde_receive_interface = robot.getRTDEReceiveInterfaceHandler();
72  std::shared_ptr<ur_rtde::DashboardClient> db_client = robot.getDashboardClientHandler();
73 
74  std::cout << "Robot connected : " << (rtde_receive_interface->isConnected() ? "yes" : "no") << std::endl;
75  std::cout << "Robot mode : " << rtde_receive_interface->getRobotMode() << std::endl;
76  std::cout << "Robot model : " << db_client->getRobotModel() << std::endl;
77  std::cout << "PolyScope version: " << db_client->polyscopeVersion() << std::endl;
78  robot.disconnect();
79  }
80  catch (const vpException &e) {
81  std::cout << "ViSP exception: " << e.what() << std::endl;
82  return EXIT_FAILURE;
83  }
84  catch (const std::exception &e) {
85  std::cout << "ur_rtde exception: " << e.what() << std::endl;
86  return EXIT_FAILURE;
87  }
88 
89  try {
90  std::cout << "-- Start test 2/3" << std::endl;
92  robot.connect(robot_ip);
93 
94  // Next test is only done when robot powered on
95  // See robot mode doc:
96  // https://sdurobotics.gitlab.io/ur_rtde/api/api.html#_CPPv4N7ur_rtde20RTDEReceiveInterface12getRobotModeEv
97  if (robot.getRobotMode() >= 4) {
98  vpColVector q, q_init;
99 
100  for (unsigned i = 0; i < 10; i++) {
101  robot.getPosition(vpRobot::JOINT_STATE, q_init);
102  q = q_init;
103  std::cout << "Joint position [deg]: " << q.rad2deg().t() << std::endl;
104  vpTime::wait(10);
105  }
106 
107  vpPoseVector fPe;
108  robot.getPosition(vpRobot::END_EFFECTOR_FRAME, fPe);
109  std::cout << "fMe pose vector: " << fPe.t() << std::endl;
110  vpHomogeneousMatrix fMe_1(fPe);
111  std::cout << "fMe pose matrix: \n" << fMe_1 << std::endl;
112  vpColVector position;
113  robot.getPosition(vpRobot::END_EFFECTOR_FRAME, position);
114  for (size_t i = 0; i < fPe.size(); i++) {
115  if (!vpMath::equal(fPe[i], position[i])) {
116  std::cout << "Wrong end-effector pose returned by getPosition(). Test failed" << std::endl;
117  return EXIT_FAILURE;
118  }
119  }
120  vpHomogeneousMatrix fMe_2 = robot.get_fMe();
121  std::cout << "fMe pose matrix: \n" << fMe_2 << std::endl;
122  for (size_t i = 0; i < fMe_2.size(); i++) {
123  if (!vpMath::equal(fMe_1.data[i], fMe_2.data[i])) {
124  std::cout << "Wrong end-effector pose returned by get_fMe(). Test failed" << std::endl;
125  return EXIT_FAILURE;
126  }
127  }
128 
129  // Next test is only done when brakes are released
130  // See robot mode doc:
131  // https://sdurobotics.gitlab.io/ur_rtde/api/api.html#_CPPv4N7ur_rtde20RTDEReceiveInterface12getRobotModeEv
132  if (robot.getRobotMode() == 7) {
133  vpHomogeneousMatrix fMe_3 = robot.get_fMe(q_init);
134  std::cout << "fMe pose matrix: \n" << fMe_3 << std::endl;
135  for (size_t i = 0; i < fMe_3.size(); i++) {
136  if (!vpMath::equal(fMe_2.data[i], fMe_3.data[i])) {
137  std::cout << "Wrong end-effector forward kinematics . Test failed" << std::endl;
138  return EXIT_FAILURE;
139  }
140  }
141  }
142 
143  vpPoseVector fPc;
144  robot.getPosition(vpRobot::TOOL_FRAME, fPc);
145  std::cout << "fMc pose vector: " << fPc.t() << std::endl;
146  std::cout << "fMc pose matrix: \n" << vpHomogeneousMatrix(fPc) << std::endl;
147  robot.getPosition(vpRobot::TOOL_FRAME, position);
148  for (size_t i = 0; i < fPc.size(); i++) {
149  if (!vpMath::equal(fPc[i], position[i])) {
150  std::cout << "Wrong tool pose. Test failed" << std::endl;
151  return EXIT_FAILURE;
152  }
153  }
154  }
155  else {
156  std::cout << "To proceed with this test you need to power on the robot" << std::endl;
157  }
158  }
159  catch (const vpException &e) {
160  std::cout << "ViSP exception: " << e.what() << std::endl;
161  return EXIT_FAILURE;
162  }
163  catch (const std::exception &e) {
164  std::cout << "ur_rtde exception: " << e.what() << std::endl;
165  return EXIT_FAILURE;
166  }
167 
168  try {
169  std::cout << "-- Start test 3/3" << std::endl;
170  vpRobotUniversalRobots robot(robot_ip);
171  robot.disconnect();
172  robot.connect(robot_ip);
173 
174  vpColVector eFe;
175  for (unsigned i = 0; i < 10; i++) {
176  robot.getForceTorque(vpRobot::END_EFFECTOR_FRAME, eFe);
177  std::cout << "End-effector force/torque: " << eFe.t() << std::endl;
178  vpTime::wait(10);
179  }
180 
181  vpColVector cFc;
182  for (unsigned i = 0; i < 10; i++) {
183  robot.getForceTorque(vpRobot::TOOL_FRAME, cFc);
184  std::cout << "Camera or tool frame force/torque: " << cFc.t() << std::endl;
185  vpTime::wait(10);
186  }
187  }
188  catch (const vpException &e) {
189  std::cout << "ViSP exception: " << e.what() << std::endl;
190  return EXIT_FAILURE;
191  }
192  catch (const std::exception &e) {
193  std::cout << "ur_rtde exception: " << e.what() << std::endl;
194  return EXIT_FAILURE;
195  }
196 
197  std::cout << "The end" << std::endl;
198  return EXIT_SUCCESS;
199 }
200 
201 #else
202 int main()
203 {
204  std::cout << "ViSP is not build with libur_rtde 3rd party used to control a robot from Universal Robots..."
205  << std::endl;
206 
207 }
208 #endif
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:130
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:331
Implementation of column vector and the associated operations.
Definition: vpColVector.h:171
vpColVector & rad2deg()
Definition: vpColVector.h:980
vpRowVector t() const
error that can be emitted by ViSP classes.
Definition: vpException.h:60
const char * what() const
Definition: vpException.cpp:71
Implementation of an homogeneous matrix and operations on such kind of matrices.
static bool equal(double x, double y, double threshold=0.001)
Definition: vpMath.h:456
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:187
vpRowVector t() const
@ JOINT_STATE
Definition: vpRobot.h:82
@ TOOL_FRAME
Definition: vpRobot.h:86
@ END_EFFECTOR_FRAME
Definition: vpRobot.h:83
VISP_EXPORT int wait(double t0, double t)