47 #include <visp3/core/vpConfig.h> 49 #if defined(VISP_HAVE_UR_RTDE) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) 51 #include <visp3/robot/vpRobotUniversalRobots.h> 53 int main(
int argc,
char **argv)
55 std::string robot_ip =
"192.168.0.100";
57 for (
int i = 1; i < argc; i++) {
58 if (std::string(argv[i]) ==
"--ip" && i + 1 < argc) {
59 robot_ip = std::string(argv[i + 1]);
60 }
else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
61 std::cout << argv[0] <<
" [--ip " << robot_ip <<
"] [--help] [-h]" 68 std::cout <<
"-- Start test 1/3" << std::endl;
75 std::cout <<
"Robot connected : " << (rtde_receive_interface->isConnected() ?
"yes" :
"no") << std::endl;
76 std::cout <<
"Robot mode : " << rtde_receive_interface->getRobotMode() << std::endl;
77 std::cout <<
"Robot model : " << db_client->getRobotModel() << std::endl;
78 std::cout <<
"PolyScope version: " << db_client->polyscopeVersion() << std::endl;
81 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;
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;
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;
154 std::cout <<
"To proceed with this test you need to power on the robot" << std::endl;
157 std::cout <<
"ViSP exception: " << e.
what() << std::endl;
159 }
catch (
const std::exception &e) {
160 std::cout <<
"ur_rtde exception: " << e.what() << std::endl;
165 std::cout <<
"-- Start test 3/3" << std::endl;
171 for (
unsigned i = 0; i < 10; i++) {
173 std::cout <<
"End-effector force/torque: " << eFe.
t() << std::endl;
178 for (
unsigned i = 0; i < 10; i++) {
180 std::cout <<
"Camera or tool frame force/torque: " << cFc.
t() << std::endl;
184 std::cout <<
"ViSP exception: " << e.
what() << std::endl;
186 }
catch (
const std::exception &e) {
187 std::cout <<
"ur_rtde exception: " << e.what() << std::endl;
191 std::cout <<
"The end" << std::endl;
198 #if !defined(VISP_HAVE_UR_RTDE) 199 std::cout <<
"ViSP is not build with libur_rtde 3rd party used to control a robot from Universal Robots..." 202 #if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11) 203 std::cout <<
"Build ViSP with c++11 or higher compiler flag (cmake -DUSE_CXX_STANDARD=11)." << std::endl;
VISP_EXPORT int wait(double t0, double t)
void connect(const std::string &ur_address)
void getForceTorque(const vpRobot::vpControlFrameType frame, vpColVector &force)
Implementation of an homogeneous matrix and operations on such kind of matrices.
static bool equal(double x, double y, double s=0.001)
error that can be emited by ViSP classes.
Type * data
Address of the first element of the data array.
unsigned int size() const
Return the number of elements of the 2D array.
vpHomogeneousMatrix get_fMe()
std::shared_ptr< ur_rtde::RTDEReceiveInterface > getRTDEReceiveInterfaceHandler() const
const char * what() const
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
Implementation of column vector and the associated operations.
Implementation of a pose vector and operations on poses.
std::shared_ptr< ur_rtde::DashboardClient > getDashboardClientHandler() const