44 #include <visp3/core/vpConfig.h>
46 #if defined(VISP_HAVE_UR_RTDE)
48 #include <visp3/robot/vpRobotUniversalRobots.h>
50 int 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]);
58 else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
59 std::cout << argv[0] <<
" [--ip " << robot_ip <<
"] [--help] [-h]"
66 std::cout <<
"-- Start test 1/3" << std::endl;
68 robot.connect(robot_ip);
70 std::shared_ptr<ur_rtde::RTDEReceiveInterface> rtde_receive_interface = robot.getRTDEReceiveInterfaceHandler();
71 std::shared_ptr<ur_rtde::DashboardClient> db_client = robot.getDashboardClientHandler();
73 std::cout <<
"Robot connected : " << (rtde_receive_interface->isConnected() ?
"yes" :
"no") << std::endl;
74 std::cout <<
"Robot mode : " << rtde_receive_interface->getRobotMode() << std::endl;
75 std::cout <<
"Robot model : " << db_client->getRobotModel() << std::endl;
76 std::cout <<
"PolyScope version: " << db_client->polyscopeVersion() << std::endl;
80 std::cout <<
"ViSP exception: " << e.
what() << std::endl;
83 catch (
const std::exception &e) {
84 std::cout <<
"ur_rtde exception: " << e.what() << std::endl;
89 std::cout <<
"-- Start test 2/3" << std::endl;
91 robot.connect(robot_ip);
96 if (robot.getRobotMode() >= 4) {
99 for (
unsigned i = 0; i < 10; i++) {
102 std::cout <<
"Joint position [deg]: " << q.
rad2deg().
t() << std::endl;
108 std::cout <<
"fMe pose vector: " << fPe.
t() << std::endl;
110 std::cout <<
"fMe pose matrix: \n" << fMe_1 << std::endl;
113 for (
size_t i = 0; i < fPe.
size(); i++) {
115 std::cout <<
"Wrong end-effector pose returned by getPosition(). Test failed" << std::endl;
120 std::cout <<
"fMe pose matrix: \n" << fMe_2 << std::endl;
121 for (
size_t i = 0; i < fMe_2.
size(); i++) {
123 std::cout <<
"Wrong end-effector pose returned by get_fMe(). Test failed" << std::endl;
131 if (robot.getRobotMode() == 7) {
133 std::cout <<
"fMe pose matrix: \n" << fMe_3 << std::endl;
134 for (
size_t i = 0; i < fMe_3.
size(); i++) {
136 std::cout <<
"Wrong end-effector forward kinematics . Test failed" << std::endl;
144 std::cout <<
"fMc pose vector: " << fPc.
t() << std::endl;
147 for (
size_t i = 0; i < fPc.
size(); i++) {
149 std::cout <<
"Wrong tool pose. Test failed" << std::endl;
155 std::cout <<
"To proceed with this test you need to power on the robot" << std::endl;
159 std::cout <<
"ViSP exception: " << e.
what() << std::endl;
162 catch (
const std::exception &e) {
163 std::cout <<
"ur_rtde exception: " << e.what() << std::endl;
168 std::cout <<
"-- Start test 3/3" << std::endl;
171 robot.connect(robot_ip);
174 for (
unsigned i = 0; i < 10; i++) {
176 std::cout <<
"End-effector force/torque: " << eFe.
t() << std::endl;
181 for (
unsigned i = 0; i < 10; i++) {
183 std::cout <<
"Camera or tool frame force/torque: " << cFc.
t() << std::endl;
188 std::cout <<
"ViSP exception: " << e.
what() << std::endl;
191 catch (
const std::exception &e) {
192 std::cout <<
"ur_rtde exception: " << e.what() << std::endl;
196 std::cout <<
"The end" << std::endl;
203 std::cout <<
"ViSP is not build with libur_rtde 3rd party used to control a robot from Universal Robots..."
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)