44#include <visp3/core/vpConfig.h>
46#if defined(VISP_HAVE_UR_RTDE) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
48#include <visp3/robot/vpRobotUniversalRobots.h>
50int main(
int argc,
char **argv)
52 std::string robot_ip =
"192.168.0.100";
54 for (
int i = 1; i < argc; i++) {
55 if (std::string(argv[i]) ==
"--ip" && i + 1 < argc) {
56 robot_ip = std::string(argv[i + 1]);
57 }
else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
58 std::cout << argv[0] <<
" [--ip " << robot_ip <<
"] [--help] [-h]"
65 std::cout <<
"-- Start test 1/3" << std::endl;
67 robot.connect(robot_ip);
69 std::shared_ptr<ur_rtde::RTDEReceiveInterface> rtde_receive_interface = robot.getRTDEReceiveInterfaceHandler();
70 std::shared_ptr<ur_rtde::DashboardClient> db_client = robot.getDashboardClientHandler();
72 std::cout <<
"Robot connected : " << (rtde_receive_interface->isConnected() ?
"yes" :
"no") << std::endl;
73 std::cout <<
"Robot mode : " << rtde_receive_interface->getRobotMode() << std::endl;
74 std::cout <<
"Robot model : " << db_client->getRobotModel() << std::endl;
75 std::cout <<
"PolyScope version: " << db_client->polyscopeVersion() << std::endl;
78 std::cout <<
"ViSP exception: " << e.
what() << std::endl;
80 }
catch (
const std::exception &e) {
81 std::cout <<
"ur_rtde exception: " << e.what() << std::endl;
86 std::cout <<
"-- Start test 2/3" << std::endl;
88 robot.connect(robot_ip);
93 if (robot.getRobotMode() >= 4) {
96 for (
unsigned i = 0; i < 10; i++) {
99 std::cout <<
"Joint position [deg]: " << q.
rad2deg().
t() << std::endl;
105 std::cout <<
"fMe pose vector: " << fPe.
t() << std::endl;
107 std::cout <<
"fMe pose matrix: \n" << fMe_1 << std::endl;
110 for (
size_t i = 0; i < fPe.
size(); i++) {
112 std::cout <<
"Wrong end-effector pose returned by getPosition(). Test failed" << std::endl;
117 std::cout <<
"fMe pose matrix: \n" << fMe_2 << std::endl;
118 for (
size_t i = 0; i < fMe_2.
size(); i++) {
120 std::cout <<
"Wrong end-effector pose returned by get_fMe(). Test failed" << std::endl;
128 if (robot.getRobotMode() == 7) {
130 std::cout <<
"fMe pose matrix: \n" << fMe_3 << std::endl;
131 for (
size_t i = 0; i < fMe_3.
size(); i++) {
133 std::cout <<
"Wrong end-effector forward kinematics . Test failed" << std::endl;
141 std::cout <<
"fMc pose vector: " << fPc.
t() << std::endl;
144 for (
size_t i = 0; i < fPc.
size(); i++) {
146 std::cout <<
"Wrong tool pose. Test failed" << std::endl;
151 std::cout <<
"To proceed with this test you need to power on the robot" << std::endl;
154 std::cout <<
"ViSP exception: " << e.
what() << std::endl;
156 }
catch (
const std::exception &e) {
157 std::cout <<
"ur_rtde exception: " << e.what() << std::endl;
162 std::cout <<
"-- Start test 3/3" << std::endl;
165 robot.connect(robot_ip);
168 for (
unsigned i = 0; i < 10; i++) {
170 std::cout <<
"End-effector force/torque: " << eFe.
t() << std::endl;
175 for (
unsigned i = 0; i < 10; i++) {
177 std::cout <<
"Camera or tool frame force/torque: " << cFc.
t() << std::endl;
181 std::cout <<
"ViSP exception: " << e.
what() << std::endl;
183 }
catch (
const std::exception &e) {
184 std::cout <<
"ur_rtde exception: " << e.what() << std::endl;
188 std::cout <<
"The end" << std::endl;
195#if !defined(VISP_HAVE_UR_RTDE)
196 std::cout <<
"ViSP is not build with libur_rtde 3rd party used to control a robot from Universal Robots..."
199#if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
200 std::cout <<
"Build ViSP with c++11 or higher compiler flag (cmake -DUSE_CXX_STANDARD=11)." << std::endl;
Type * data
Address of the first element of the data array.
unsigned int size() const
Return the number of elements of the 2D array.
Implementation of column vector and the associated operations.
error that can be emitted by ViSP classes.
const char * what() const
Implementation of an homogeneous matrix and operations on such kind of matrices.
static bool equal(double x, double y, double threshold=0.001)
Implementation of a pose vector and operations on poses.
VISP_EXPORT int wait(double t0, double t)