diff --git a/doc/image/tutorial/visual-servo/img-biclops-frames.jpg b/doc/image/tutorial/visual-servo/img-biclops-frames.jpg new file mode 100644 index 0000000000..218922584b Binary files /dev/null and b/doc/image/tutorial/visual-servo/img-biclops-frames.jpg differ diff --git a/doc/image/tutorial/visual-servo/img-biclops-frames.pptx b/doc/image/tutorial/visual-servo/img-biclops-frames.pptx new file mode 100644 index 0000000000..be04329f3a Binary files /dev/null and b/doc/image/tutorial/visual-servo/img-biclops-frames.pptx differ diff --git a/example/servo-biclops/moveBiclops.cpp b/example/servo-biclops/moveBiclops.cpp index 245afa21b1..f28b995abb 100644 --- a/example/servo-biclops/moveBiclops.cpp +++ b/example/servo-biclops/moveBiclops.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,26 +29,17 @@ * * Description: * Tests the control law - * Authors: - * -*****************************************************************************/ -/*! - \file moveBiclops.cpp - - \brief Example of a real robot control, the biclops robot (pan-tilt turret) - by Traclabs. The robot is controlled first in position, then in velocity. - - See http://www.traclabs.com/tracbiclops.htm for more details. -*/ + */ /*! - \example moveBiclops.cpp - - Example of a real robot control, the biclops robot (pan-tilt turret) by - Traclabs. The robot is controlled first in position, then in velocity. - - See http://www.traclabs.com/tracbiclops.htm for more details. -*/ + * \file moveBiclops.cpp + * \example moveBiclops.cpp + * + * Example of a real robot control, the biclops robot (pan-tilt turret) by + * Traclabs. The robot is controlled first in position, then in velocity. + * + * See http://www.traclabs.com/tracbiclops.htm for more details. + */ #include #include @@ -64,14 +54,12 @@ #define GETOPTARGS "c:h" /* - -Print the program options. - - \param name : Program name. - \param badparam : Bad parameter name. - \param conf : Biclops configuration file. - -*/ + * Print the program options. + * + * \param name : Program name. + * \param badparam : Bad parameter name. + * \param conf : Biclops configuration file. + */ void usage(const char *name, const char *badparam, std::string conf) { fprintf(stdout, "\n\ @@ -80,7 +68,7 @@ Move the biclops robot\n\ SYNOPSIS\n\ %s [-c ] [-h]\n\ ", - name); +name); fprintf(stdout, "\n\ OPTIONS: Default\n\ @@ -95,16 +83,14 @@ OPTIONS: Default\n\ } /*! - -Set the program options. - - \param argc : Command line number of parameters. - \param argv : Array of command line parameters. - \param conf : Biclops configuration file. - - \return false if the program has to be stopped, true otherwise. - -*/ + * Set the program options. + * + * \param argc : Command line number of parameters. + * \param argv : Array of command line parameters. + * \param conf : Biclops configuration file. + * + * \return false if the program has to be stopped, true otherwise. + */ bool getOptions(int argc, const char **argv, std::string &conf) { const char *optarg_; @@ -147,7 +133,7 @@ int main(int argc, const char **argv) return EXIT_FAILURE; } try { - vpRobotBiclops robot(opt_conf.c_str()); + vpRobotBiclops robot(opt_conf); vpColVector q(vpBiclops::ndof); // desired position vpColVector qdot(vpBiclops::ndof); // desired velocity @@ -160,122 +146,123 @@ int main(int argc, const char **argv) q[0] = vpMath::rad(-10); q[1] = vpMath::rad(-20); std::cout << "Set position in the articular frame: " - << " pan: " << vpMath::deg(q[0]) << " deg" - << " tilt: " << vpMath::deg(q[1]) << " deg" << std::endl; + << " pan: " << vpMath::deg(q[0]) << " deg" + << " tilt: " << vpMath::deg(q[1]) << " deg" << std::endl; robot.setPositioningVelocity(30.); - robot.setPosition(vpRobot::ARTICULAR_FRAME, q); + robot.setPosition(vpRobot::JOINT_STATE, q); - robot.getPosition(vpRobot::ARTICULAR_FRAME, qm); + robot.getPosition(vpRobot::JOINT_STATE, qm); std::cout << "Position in the articular frame: " - << " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl; - robot.getVelocity(vpRobot::ARTICULAR_FRAME, qm); + << " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl; + robot.getVelocity(vpRobot::JOINT_STATE, qm); std::cout << "Velocity in the articular frame: " - << " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl; + << " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl; q[0] = vpMath::rad(10); q[1] = vpMath::rad(20); std::cout << "Set position in the articular frame: " - << " pan: " << vpMath::deg(q[0]) << " deg" - << " tilt: " << vpMath::deg(q[1]) << " deg" << std::endl; + << " pan: " << vpMath::deg(q[0]) << " deg" + << " tilt: " << vpMath::deg(q[1]) << " deg" << std::endl; robot.setPositioningVelocity(10); - robot.setPosition(vpRobot::ARTICULAR_FRAME, q); + robot.setPosition(vpRobot::JOINT_STATE, q); - robot.getPosition(vpRobot::ARTICULAR_FRAME, qm); + robot.getPosition(vpRobot::JOINT_STATE, qm); std::cout << "Position in the articular frame: " - << " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl; - robot.getVelocity(vpRobot::ARTICULAR_FRAME, qm); + << " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl; + robot.getVelocity(vpRobot::JOINT_STATE, qm); std::cout << "Velocity in the articular frame: " - << " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl; + << " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl; std::cout << "Set STATE_VELOCITY_CONTROL" << std::endl; robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL); - robot.getPosition(vpRobot::ARTICULAR_FRAME, qm); + robot.getPosition(vpRobot::JOINT_STATE, qm); std::cout << "Position in the articular frame: " - << " pan: " << vpMath::deg(qm[0]) << " deg" - << " tilt: " << vpMath::deg(qm[1]) << " deg" << std::endl; - robot.getVelocity(vpRobot::ARTICULAR_FRAME, qm); + << " pan: " << vpMath::deg(qm[0]) << " deg" + << " tilt: " << vpMath::deg(qm[1]) << " deg" << std::endl; + robot.getVelocity(vpRobot::JOINT_STATE, qm); std::cout << "Velocity in the articular frame: " - << " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl; + << " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl; qdot = 0; // qdot[0] = vpMath::rad(0.1) ; qdot[1] = vpMath::rad(25); - std::cout << "Set articular frame velocity " - << " pan: " << vpMath::deg(qdot[0]) << " deg/s" - << " tilt: " << vpMath::deg(qdot[1]) << " deg/s" << std::endl; - robot.setVelocity(vpRobot::ARTICULAR_FRAME, qdot); + std::cout << "Set articular frame velocity for 5 sec" + << " pan: " << vpMath::deg(qdot[0]) << " deg/s" + << " tilt: " << vpMath::deg(qdot[1]) << " deg/s" << std::endl; + robot.setVelocity(vpRobot::JOINT_STATE, qdot); // waits 5000ms vpTime::wait(5000.0); - robot.getPosition(vpRobot::ARTICULAR_FRAME, qm); + robot.getPosition(vpRobot::JOINT_STATE, qm); std::cout << "Position in the articular frame: " - << " pan: " << vpMath::deg(qm[0]) << " deg" - << " tilt: " << vpMath::deg(qm[1]) << " deg" << std::endl; - robot.getVelocity(vpRobot::ARTICULAR_FRAME, qm); + << " pan: " << vpMath::deg(qm[0]) << " deg" + << " tilt: " << vpMath::deg(qm[1]) << " deg" << std::endl; + robot.getVelocity(vpRobot::JOINT_STATE, qm); std::cout << "Velocity in the articular frame: " - << " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl; + << " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl; qdot = 0; // qdot[0] = vpMath::rad(0.1) ; qdot[1] = -vpMath::rad(25); - std::cout << "Set articular frame velocity " - << " pan: " << vpMath::deg(qdot[0]) << " deg/s" - << " tilt: " << vpMath::deg(qdot[1]) << " deg/s" << std::endl; - robot.setVelocity(vpRobot::ARTICULAR_FRAME, qdot); + std::cout << "Set articular frame velocity for 3 sec" + << " pan: " << vpMath::deg(qdot[0]) << " deg/s" + << " tilt: " << vpMath::deg(qdot[1]) << " deg/s" << std::endl; + robot.setVelocity(vpRobot::JOINT_STATE, qdot); // waits 3000 ms vpTime::wait(3000.0); - robot.getPosition(vpRobot::ARTICULAR_FRAME, qm); + robot.getPosition(vpRobot::JOINT_STATE, qm); std::cout << "Position in the articular frame: " - << " pan: " << vpMath::deg(qm[0]) << " deg" - << " tilt: " << vpMath::deg(qm[1]) << " deg" << std::endl; - robot.getVelocity(vpRobot::ARTICULAR_FRAME, qm); + << " pan: " << vpMath::deg(qm[0]) << " deg" + << " tilt: " << vpMath::deg(qm[1]) << " deg" << std::endl; + robot.getVelocity(vpRobot::JOINT_STATE, qm); std::cout << "Velocity in the articular frame: " - << " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl; + << " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl; qdot = 0; // qdot[0] = vpMath::rad(0.1) ; qdot[1] = vpMath::rad(10); - std::cout << "Set articular frame velocity " - << " pan: " << vpMath::deg(qdot[0]) << " deg/s" - << " tilt: " << vpMath::deg(qdot[1]) << " deg/s" << std::endl; - robot.setVelocity(vpRobot::ARTICULAR_FRAME, qdot); + std::cout << "Set articular frame velocity for 2 sec" + << " pan: " << vpMath::deg(qdot[0]) << " deg/s" + << " tilt: " << vpMath::deg(qdot[1]) << " deg/s" << std::endl; + robot.setVelocity(vpRobot::JOINT_STATE, qdot); // waits 2000 ms vpTime::wait(2000.0); - robot.getPosition(vpRobot::ARTICULAR_FRAME, qm); + robot.getPosition(vpRobot::JOINT_STATE, qm); std::cout << "Position in the articular frame: " - << " pan: " << vpMath::deg(qm[0]) << " deg" - << " tilt: " << vpMath::deg(qm[1]) << " deg" << std::endl; - robot.getVelocity(vpRobot::ARTICULAR_FRAME, qm); + << " pan: " << vpMath::deg(qm[0]) << " deg" + << " tilt: " << vpMath::deg(qm[1]) << " deg" << std::endl; + robot.getVelocity(vpRobot::JOINT_STATE, qm); std::cout << "Velocity in the articular frame: " - << " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl; + << " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl; qdot = 0; qdot[0] = vpMath::rad(-5); // qdot[1] = vpMath::rad(-5); - std::cout << "Set articular frame velocity " - << " pan: " << vpMath::deg(qdot[0]) << " deg/s" - << " tilt: " << vpMath::deg(qdot[1]) << " deg/s" << std::endl; - robot.setVelocity(vpRobot::ARTICULAR_FRAME, qdot); + std::cout << "Set articular frame velocity for 2 sec" + << " pan: " << vpMath::deg(qdot[0]) << " deg/s" + << " tilt: " << vpMath::deg(qdot[1]) << " deg/s" << std::endl; + robot.setVelocity(vpRobot::JOINT_STATE, qdot); // waits 2000 ms vpTime::wait(2000.0); - robot.getPosition(vpRobot::ARTICULAR_FRAME, qm); + robot.getPosition(vpRobot::JOINT_STATE, qm); std::cout << "Position in the articular frame: " - << " pan: " << vpMath::deg(qm[0]) << " deg" - << " tilt: " << vpMath::deg(qm[1]) << " deg" << std::endl; - robot.getVelocity(vpRobot::ARTICULAR_FRAME, qm); + << " pan: " << vpMath::deg(qm[0]) << " deg" + << " tilt: " << vpMath::deg(qm[1]) << " deg" << std::endl; + robot.getVelocity(vpRobot::JOINT_STATE, qm); std::cout << "Velocity in the articular frame: " - << " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl; + << " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl; return EXIT_SUCCESS; - } catch (const vpException &e) { + } + catch (const vpException &e) { std::cout << "Catch an exception: " << e.getMessage() << std::endl; return EXIT_FAILURE; } diff --git a/example/servo-biclops/servoBiclopsPoint2DArtVelocity.cpp b/example/servo-biclops/servoBiclopsPoint2DArtVelocity.cpp index 7cf6e1cce1..f3214f9a87 100644 --- a/example/servo-biclops/servoBiclopsPoint2DArtVelocity.cpp +++ b/example/servo-biclops/servoBiclopsPoint2DArtVelocity.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -32,130 +31,83 @@ * tests the control law * eye-in-hand control * velocity computed in articular - * -*****************************************************************************/ - -/*! - \file servoBiclopsPoint2DArtVelocity.cpp - - \brief Example of eye-in-hand control law. We control here a real robot, the - biclops robot (pan-tilt head provided by Traclabs). The velocity is computed - in articular. The visual feature is the center of gravity of a point. - -*/ + */ /*! - \example servoBiclopsPoint2DArtVelocity.cpp - - Example of eye-in-hand control law. We control here a real robot, the - biclops robot (pan-tilt head provided by Traclabs). The velocity is computed - in articular. The visual feature is the center of gravity of a point. - -*/ + * \file servoBiclopsPoint2DArtVelocity.cpp + * \example servoBiclopsPoint2DArtVelocity.cpp + * + * Example of eye-in-hand control law. We control here a real robot, the + * biclops robot (pan-tilt head provided by Traclabs). The velocity is computed + * in articular. The visual feature is the center of gravity of a point. + */ -#include -#include #include -#include // Debug trace -#include -#if (defined(VISP_HAVE_BICLOPS) && (defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_DIRECTSHOW))) -#ifdef VISP_HAVE_PTHREAD -#include -#endif +#if defined(VISP_HAVE_BICLOPS) && defined(VISP_HAVE_APRILTAG) && defined(VISP_HAVE_REALSENSE2) #include +#include +#include #include +#include #include #include #include -#include -#include - -#include -#include -#include -#include -#include #include #include +#include #include #include #include #include -// Exception -#include - -#ifdef VISP_HAVE_PTHREAD -pthread_mutex_t mutexEndLoop = PTHREAD_MUTEX_INITIALIZER; -#endif - -void signalCtrC(int /* signumber */) -{ -#ifdef VISP_HAVE_PTHREAD - pthread_mutex_unlock(&mutexEndLoop); -#endif - vpTime::wait(10); - vpTRACE("Ctrl-C pressed..."); -} - // List of allowed command line options #define GETOPTARGS "c:d:h" /*! - - Print the program options. - - \param name : Program name. - \param badparam : Bad parameter name. - \param conf : Robot configuration file. - \param debugdir : Debug file directory. - \param user : Username. - + * Print the program options. + * + * \param name : Program name. + * \param badparam : Bad parameter name. + * \param conf : Robot configuration file. */ -void usage(const char *name, const char *badparam, std::string &conf, std::string &debugdir, std::string &user) +void usage(const char *name, const char *badparam, std::string &conf) { fprintf(stdout, "\n\ Example of eye-in-hand control law. We control here a real robot, the biclops\n\ - robot (pan-tilt head provided by Traclabs). The velocity is\n\ - computed in articular. The visual feature is the center of gravity of a\n\ - point.\n\ + robot (pan-tilt head provided by Traclabs) equipped with a Realsense camera\n\ + mounted on its end-effector. The velocity to apply to the PT head is joint\n\ + velocity. The visual feature is a point corresponding to the center of\n\ + gravity of an AprilTag. \n\ \n\ SYNOPSIS\n\ - %s [-c ] [-d ] [-h]\n", + %s [-c ] [-h]\n", name); fprintf(stdout, "\n\ OPTIONS: Default\n\ -c %s\n\ - Sets the biclops robot configuration file.\n\n\ - -d %s\n\ - Sets the debug file directory.\n\ - From this directory, creates the\"%s\"\n\ - subdirectory depending on the username, where\n\ - it writes biclops.txt file.\n", - conf.c_str(), debugdir.c_str(), user.c_str()); + Sets the biclops robot configuration file.\n", + conf.c_str()); if (badparam) { fprintf(stderr, "ERROR: \n"); fprintf(stderr, "\nBad parameter [%s]\n", badparam); } } -/*! - -Set the program options. - \param argc : Command line number of parameters. - \param argv : Array of command line parameters. - \param conf : Robot configuration file. - \param debugdir : Debug file directory. - \param user : Username. - - \return false if the program has to be stopped, true otherwise. - -*/ -bool getOptions(int argc, const char **argv, std::string &conf, std::string &debugdir, std::string &user) +/*! + * Set the program options. + * + * \param argc : Command line number of parameters. + * \param argv : Array of command line parameters. + * \param conf : Robot configuration file. + * + * \return false if the program has to be stopped, true otherwise. + * + */ +bool getOptions(int argc, const char **argv, std::string &conf) { const char *optarg_; int c; @@ -165,16 +117,13 @@ bool getOptions(int argc, const char **argv, std::string &conf, std::string &deb case 'c': conf = optarg_; break; - case 'd': - debugdir = optarg_; - break; case 'h': - usage(argv[0], NULL, conf, debugdir, user); + usage(argv[0], NULL, conf); return false; break; default: - usage(argv[0], optarg_, conf, debugdir, user); + usage(argv[0], optarg_, conf); return false; break; } @@ -182,7 +131,7 @@ bool getOptions(int argc, const char **argv, std::string &conf, std::string &deb if ((c == 1) || (c == -1)) { // standalone param or error - usage(argv[0], NULL, conf, debugdir, user); + usage(argv[0], NULL, conf); std::cerr << "ERROR: " << std::endl; std::cerr << " Bad argument " << optarg_ << std::endl << std::endl; return false; @@ -193,223 +142,192 @@ bool getOptions(int argc, const char **argv, std::string &conf, std::string &deb int main(int argc, const char **argv) { - std::cout << std::endl; - std::cout << "-------------------------------------------------------" << std::endl; - std::cout << " Test program for vpServo " << std::endl; - std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl; - std::cout << " Simulation " << std::endl; - std::cout << " task : servo a point " << std::endl; - std::cout << "-------------------------------------------------------" << std::endl; - std::cout << std::endl; - try { - -#ifdef VISP_HAVE_PTHREAD - pthread_mutex_lock(&mutexEndLoop); -#endif - signal(SIGINT, &signalCtrC); - - // default unix configuration file path + // Default unix configuration file path std::string opt_conf = "/usr/share/BiclopsDefault.cfg"; - std::string username; - std::string debugdir; - std::string opt_debugdir; - -// Set the default output path -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX - opt_debugdir = "/tmp"; -#elif defined(_WIN32) - opt_debugdir = "C:/temp"; -#endif - - // Get the user login name - vpIoTools::getUserName(username); - // Read the command line options - if (getOptions(argc, argv, opt_conf, opt_debugdir, username) == false) { + if (getOptions(argc, argv, opt_conf) == false) { return EXIT_FAILURE; } - // Get the option value - if (!opt_debugdir.empty()) - debugdir = opt_debugdir; - - // Append to the output path string, the login name of the user - std::string dirname = debugdir + "/" + username; - - // Test if the output path exist. If no try to create it - if (vpIoTools::checkDirectory(dirname) == false) { - try { - // Create the dirname - vpIoTools::makeDirectory(dirname); - } catch (...) { - usage(argv[0], NULL, opt_conf, debugdir, username); - std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr << " Cannot create " << dirname << std::endl; - std::cerr << " Check your -d " << debugdir << " option " << std::endl; - return EXIT_FAILURE; - } - } - - // Create the debug file: debugdir/$user/biclops.txt - std::string filename; - filename = debugdir + "/biclops.txt"; - FILE *fd = fopen(filename.c_str(), "w"); + // Initialize PTU + vpRobotBiclops robot(opt_conf); + + /* + * Biclops DH2 has the following axis orientation + * + * tilt + <---- (end-effector-frame) + * | + * \/ pan + + * + * The end-effector-frame from PT unit rear view is the following + * + * /\ x + * | + * (e) ----> y + * + * + * + * The camera frame attached to the PT unit is the following (rear view) + * + * (c) ----> x + * | + * \/ y + * + * The corresponding cRe (camera to end-effector rotation matrix) is then the following + * + * ( 0 1 0) + * cRe = (-1 0 0) + * ( 0 0 1) + * + * Translation cte (camera to end-effector) can be neglected + * + * (0) + * cte = (0) + * (0) + */ - vpRobotBiclops robot(opt_conf.c_str()); robot.setDenavitHartenbergModel(vpBiclops::DH2); + vpRotationMatrix cRe; + cRe[0][0] = 0; cRe[0][1] = 1; cRe[0][2] = 0; + cRe[1][0] = -1; cRe[1][1] = 0; cRe[1][2] = 0; + cRe[2][0] = 0; cRe[2][1] = 0; cRe[2][2] = 1; + vpTranslationVector cte; // By default set to 0 - { - vpColVector q(2); - q = 0; - robot.setRobotState(vpRobot::STATE_POSITION_CONTROL); - robot.setPosition(vpRobot::ARTICULAR_FRAME, q); - } - - vpImage I; - -#if defined VISP_HAVE_DC1394 - vp1394TwoGrabber g; -#elif defined VISP_HAVE_DIRECTSHOW - vpDirectShowGrabber g; -#endif + // Robot Jacobian (expressed in the end-effector frame) + vpMatrix eJe; + // Camera to end-effector frame transformation + vpHomogeneousMatrix cMe(cte, cRe); + // Velocity twist transformation to express a velocity from end-effector to camera frame + vpVelocityTwistMatrix cVe(cMe); + + // Initialize grabber + vpRealSense2 g; + rs2::config config; + config.disable_stream(RS2_STREAM_DEPTH); + config.disable_stream(RS2_STREAM_INFRARED); + config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_RGBA8, 30); + g.open(config); + + std::cout << "Read camera parameters from Realsense device" << std::endl; + vpCameraParameters cam; + cam = g.getCameraParameters(RS2_STREAM_COLOR, vpCameraParameters::perspectiveProjWithoutDistortion); - g.open(I); + vpColVector q(vpBiclops::ndof); + q = 0; + std::cout << "Move PT to initial position: " << q.t() << std::endl; + robot.setRobotState(vpRobot::STATE_POSITION_CONTROL); + robot.setPosition(vpRobot::JOINT_STATE, q); - try { - g.acquire(I); - } catch (...) { - vpERROR_TRACE(" Error caught"); - return EXIT_FAILURE; - } + vpImage I; + g.acquire(I); -// We open a window using either X11 or GTK or GDI. -// Its size is automatically defined by the image (I) size + // We open a window using either X11 or GTK or GDI. + // Its size is automatically defined by the image (I) size #if defined(VISP_HAVE_X11) vpDisplayX display(I, 100, 100, "Display X..."); #elif defined(VISP_HAVE_GTK) vpDisplayGTK display(I, 100, 100, "Display GTK..."); -#elif defined(_WIN32) +#elif defined(VISP_HAVE_GDI) vpDisplayGDI display(I, 100, 100, "Display GDI..."); #endif - try { - vpDisplay::display(I); - vpDisplay::flush(I); - } catch (...) { - vpERROR_TRACE(" Error caught"); - return EXIT_FAILURE; - } - - vpServo task; - - vpDot dot; - - try { - std::cout << "Click on a dot to initialize the tracking..." << std::endl; - dot.setGraphics(true); - dot.initTracking(I); - dot.track(I); - vpERROR_TRACE("after dot.initTracking(I) "); - } catch (...) { - vpERROR_TRACE(" Error caught"); - return EXIT_FAILURE; - } + vpDisplay::display(I); + vpDisplay::flush(I); - vpCameraParameters cam; + vpDetectorAprilTag detector; - // sets the current position of the visual feature - vpFeaturePoint p; - vpFeatureBuilder::create(p, cam, dot); // retrieve x,y and Z of the vpPoint structure + vpServo task; - p.set_Z(1); - // sets the desired position of the visual feature - vpFeaturePoint pd; + // Create current and desired point visual feature + vpFeaturePoint p, pd; + // Sets the desired position of the visual feature + // Here we set Z desired to 1 meter, and (x,y)=(0,0) to center the tag in the image pd.buildFrom(0, 0, 1); - // define the task + // Define the task // - we want an eye-in-hand control law - // - articular velocity are computed + // - joint velocities are computed + // - interaction matrix is the one at desired position task.setServo(vpServo::EYEINHAND_L_cVe_eJe); task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE); - - vpTRACE("Set the position of the end-effector frame in the camera frame"); - vpHomogeneousMatrix cMe; - // robot.get_cMe(cMe) ; - - vpVelocityTwistMatrix cVe; - robot.get_cVe(cVe); - std::cout << cVe << std::endl; task.set_cVe(cVe); - - std::cout << "Click in the image to start the servoing..." << std::endl; - vpDisplay::getClick(I); - - // Set the Jacobian (expressed in the end-effector frame) - vpMatrix eJe; - robot.get_eJe(eJe); - task.set_eJe(eJe); - - // we want to see a point on a point + // We want to see a point on a point task.addFeature(p, pd); - - // set the gain + // Set the gain task.setLambda(0.2); - // Display task information - task.print(); - robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL); - unsigned int iter = 0; - vpTRACE("\t loop"); -#ifdef VISP_HAVE_PTHREAD - while (0 != pthread_mutex_trylock(&mutexEndLoop)) -#else - for (;;) -#endif - { - std::cout << "---------------------------------------------" << iter << std::endl; + bool quit = false; + bool send_velocities = false; + vpColVector q_dot; + while (!quit) { g.acquire(I); vpDisplay::display(I); - dot.track(I); + { + std::stringstream ss; + ss << "Left click to " << (send_velocities ? "stop the robot" : "servo the robot") << ", right click to quit."; + vpDisplay::displayText(I, 20, 20, ss.str(), vpColor::red); + } - vpFeatureBuilder::create(p, cam, dot); + if (detector.detect(I)) { + // We consider the first tag only + vpImagePoint cog = detector.getCog(0); // 0 is the id of the first tag - // get the jacobian - robot.get_eJe(eJe); - task.set_eJe(eJe); + vpFeatureBuilder::create(p, cam, cog); - // std::cout << (vpMatrix)cVe*eJe << std::endl ; + // Get the jacobian + robot.get_eJe(eJe); + task.set_eJe(eJe); - vpColVector v; - v = task.computeControlLaw(); + q_dot = task.computeControlLaw(); - vpServoDisplay::display(task, cam, I); - vpDisplay::flush(I); + vpServoDisplay::display(task, cam, I); + vpDisplay::flush(I); - std::cout << "v: " << v.t(); - robot.setVelocity(vpRobot::ARTICULAR_FRAME, v); + std::cout << "q_dot: " << q_dot.t() << std::endl; - std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl; + std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl; + } + else { + q_dot = 0; + } + if (!send_velocities) { + q_dot = 0; + } - { - vpColVector s_minus_sStar(2); - s_minus_sStar = task.s - task.sStar; - fprintf(fd, "%f %f %f %f %f\n", v[0], v[1], s_minus_sStar[0], s_minus_sStar[1], (task.getError()).sumSquare()); + robot.setVelocity(vpRobot::JOINT_STATE, q_dot); + + vpDisplay::flush(I); + + vpMouseButton::vpMouseButtonType button; + if (vpDisplay::getClick(I, button, false)) { + switch (button) { + case vpMouseButton::button1: + send_velocities = !send_velocities; + break; + + case vpMouseButton::button3: + quit = true; + q_dot = 0; + break; + + default: + break; + } } } - std::cout << "Display task information " << std::endl; - task.print(); - - fclose(fd); + std::cout << "Stop the robot " << std::endl; + robot.setRobotState(vpRobot::STATE_STOP); return EXIT_SUCCESS; - } catch (const vpException &e) { + } + catch (const vpException &e) { std::cout << "Catch an exception: " << e.getMessage() << std::endl; return EXIT_FAILURE; } diff --git a/modules/robot/include/visp3/robot/vpRobotBiclops.h b/modules/robot/include/visp3/robot/vpRobotBiclops.h index aec8573df1..bf1b93c9e5 100644 --- a/modules/robot/include/visp3/robot/vpRobotBiclops.h +++ b/modules/robot/include/visp3/robot/vpRobotBiclops.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,8 +29,7 @@ * * Description: * Interface for the Biclops robot. - * -*****************************************************************************/ + */ #ifndef _vpRobotBiclops_h_ #define _vpRobotBiclops_h_ @@ -67,14 +65,18 @@ \brief Interface for the biclops, pan, tilt head control. + Two different models are proposed and can be set using vpBiclops::DenavitHartenbergModel. + The vpBiclops::DH1 and vpBiclops::DH2 model differ in the orientation of the tilt axis. + The following image gives the location of the end-effector frame and a potential camera frame. + + \image html img-biclops-frames.jpg Biclops PT models + See http://www.traclabs.com/biclopspt.html for more details. This class provide a position and a speed control interface for the biclops head. To manage the biclops joint limits in speed control, a control loop is running in a separate thread implemented in vpRobotBiclopsSpeedControlLoop(). - The control of the head is done by vpRobotBiclopsController class. - \warning Velocity control mode is not exported from the top-level Biclops API class provided by Traclabs. That means that there is no protection in this mode to prevent an axis from striking its hard limit. In position mode, @@ -86,7 +88,6 @@ speed/power can damage the unit, damage due to velocity mode commanding is under user responsibility. - */ class VISP_EXPORT vpRobotBiclops : public vpBiclops, public vpRobot {