diff --git a/CMakeLists.txt b/CMakeLists.txt index a30a64bbef..3d96273291 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -549,14 +549,14 @@ if(SOQT_FOUND) # SoQt < 1.6.0 that depends on Qt4 was found. We need an explicit endif() VP_OPTION(USE_SOXT SOXT "" "Include Coin/SoXt support" "" OFF IF USE_COIN3D AND NOT WINRT AND NOT IOS) set(THREADS_PREFER_PTHREAD_FLAG ON) -VP_OPTION(USE_PTHREAD Threads "" "Include pthread support" "" ON IF NOT (WIN32 OR MINGW)) +VP_OPTION(USE_THREADS Threads "" "Include std::thread support" "" ON IF NOT (WIN32 OR MINGW)) # We need threads with c++11 -if((VISP_CXX_STANDARD GREATER VISP_CXX_STANDARD_98) AND NOT USE_PTHREAD) +if((VISP_CXX_STANDARD GREATER VISP_CXX_STANDARD_98) AND NOT USE_THREADS) if(Threads_FOUND) - message(WARNING "We need threads. Turn USE_PTHREAD=ON.") - unset(USE_PTHREAD) - set(USE_PTHREAD ON CACHE BOOL "Include pthread support" FORCE) + message(WARNING "We need std::thread. We turn USE_THREADS=ON.") + unset(USE_THREADS) + set(USE_THREADS ON CACHE BOOL "Include std::thread support" FORCE) endif() endif() @@ -658,9 +658,9 @@ endif() # ---------------------------------------------------------------------------- # Build-in 3rd parties. Should be after c++ standard potential modification # ---------------------------------------------------------------------------- -VP_OPTION(WITH_PTHREAD "" "" "Build pthread as built-in library" "" ON IF (NOT USE_PTHREAD) AND (WIN32 OR MINGW) AND (NOT WINRT_8_1) AND (NOT WINRT_8_0)) +VP_OPTION(WITH_PTHREAD "" "" "Build pthread as built-in library" "" ON IF (NOT USE_THREADS) AND (WIN32 OR MINGW) AND (NOT WINRT_8_1) AND (NOT WINRT_8_0)) # Since C99 is not supported by MSVC 2010 or prior, we disable apriltag if MSVC < 2012 -VP_OPTION(WITH_APRILTAG "" "" "Build AprilTag as built-in library" "" ON IF (USE_PTHREAD OR WITH_PTHREAD) AND (NOT WINRT_8_1) AND (NOT WINRT_8_0) AND (NOT MSVC_VERSION LESS 1700)) +VP_OPTION(WITH_APRILTAG "" "" "Build AprilTag as built-in library" "" ON IF (USE_THREADS OR WITH_PTHREAD) AND (NOT WINRT_8_1) AND (NOT WINRT_8_0) AND (NOT MSVC_VERSION LESS 1700)) VP_OPTION(WITH_APRILTAG_BIG_FAMILY "" "" "Build AprilTag big family (41h12, 48h12, 49h12, 52h13)" "" OFF IF WITH_APRILTAG) VP_OPTION(WITH_ATIDAQ "" "" "Build atidaq-c as built-in library" "" ON IF USE_COMEDI AND NOT WINRT) VP_OPTION(WITH_CLIPPER "" "" "Build clipper as built-in library" "" ON IF USE_OPENCV) @@ -890,7 +890,8 @@ VP_SET(VISP_HAVE_LAPACK_GSL TRUE IF (BUILD_MODULE_visp_core AND USE_GSL)) VP_SET(VISP_HAVE_LAPACK_MKL TRUE IF (BUILD_MODULE_visp_core AND USE_MKL)) VP_SET(VISP_HAVE_LAPACK_NETLIB TRUE IF (BUILD_MODULE_visp_core AND USE_NETLIB)) VP_SET(VISP_HAVE_LAPACK_OPENBLAS TRUE IF (BUILD_MODULE_visp_core AND USE_OPENBLAS)) -VP_SET(VISP_HAVE_PTHREAD TRUE IF (BUILD_MODULE_visp_core AND USE_PTHREAD)) +VP_SET(VISP_HAVE_PTHREAD TRUE IF (BUILD_MODULE_visp_core AND USE_THREADS)) # Keep for the momment for compat +VP_SET(VISP_HAVE_THREADS TRUE IF (BUILD_MODULE_visp_core AND USE_THREADS)) VP_SET(VISP_HAVE_XML2 TRUE IF (BUILD_MODULE_visp_core AND USE_XML2)) VP_SET(VISP_HAVE_PCL TRUE IF (BUILD_MODULE_visp_core AND USE_PCL)) VP_SET(VISP_HAVE_TENSORRT TRUE IF (BUILD_MODULE_visp_core AND USE_TENSORRT)) @@ -915,14 +916,14 @@ VP_SET(VISP_HAVE_UR_RTDE TRUE IF (BUILD_MODULE_visp_robot AND USE_UR_RTDE)) VP_SET(VISP_HAVE_FRANKA TRUE IF (BUILD_MODULE_visp_robot AND USE_FRANKA)) VP_SET(VISP_HAVE_JACOSDK TRUE IF (BUILD_MODULE_visp_robot AND USE_JACOSDK)) VP_SET(VISP_HAVE_MAVSDK TRUE IF (BUILD_MODULE_visp_robot AND USE_MAVSDK)) -VP_SET(VISP_HAVE_BICLOPS TRUE IF (BUILD_MODULE_visp_robot AND USE_BICLOPS)) +VP_SET(VISP_HAVE_BICLOPS TRUE IF (BUILD_MODULE_visp_robot AND USE_BICLOPS AND USE_THREADS)) VP_SET(VISP_HAVE_PTU46 TRUE IF (BUILD_MODULE_visp_robot AND USE_PTU46)) VP_SET(VISP_HAVE_FLIR_PTU_SDK TRUE IF (BUILD_MODULE_visp_robot AND USE_FLIRPTUSDK)) VP_SET(VISP_HAVE_ARSDK TRUE IF (BUILD_MODULE_visp_robot AND USE_ARSDK)) VP_SET(VISP_HAVE_FFMPEG TRUE IF (BUILD_MODULE_visp_robot AND USE_FFMPEG)) #VP_SET(VISP_HAVE_PIONEER TRUE IF (BUILD_MODULE_visp_robot AND USE_ARIA)) if(BUILD_MODULE_visp_robot AND USE_ARIA) - if(UNIX AND USE_PTHREAD AND RT_FOUND AND DL_FOUND) + if(UNIX AND USE_THREADS AND RT_FOUND AND DL_FOUND) set(VISP_HAVE_PIONEER TRUE) elseif(NOT UNIX) set(VISP_HAVE_PIONEER TRUE) @@ -930,7 +931,7 @@ if(BUILD_MODULE_visp_robot AND USE_ARIA) endif() #VP_SET(VISP_HAVE_VIRTUOSE TRUE IF (BUILD_MODULE_visp_robot AND USE_VIRTUOSE)) if(BUILD_MODULE_visp_robot AND USE_VIRTUOSE) - if(UNIX AND USE_PTHREAD AND RT_FOUND AND DL_FOUND) + if(UNIX AND USE_THREADS AND RT_FOUND AND DL_FOUND) set(VISP_HAVE_VIRTUOSE TRUE) elseif(NOT UNIX) set(VISP_HAVE_VIRTUOSE TRUE) @@ -970,9 +971,9 @@ VP_SET(VISP_HAVE_D3D9 TRUE IF USE_DIRECT3D) # for header vpConfig.h VP_SET(VISP_HAVE_GTK TRUE IF USE_GTK2) # for header vpConfig.h VP_SET(VISP_HAVE_XRANDR TRUE IF XRANDR) # for header vpConfig.h -# Check if libfreenect dependencies (ie libusb-1.0 and libpthread) are available -if(USE_LIBFREENECT AND USE_LIBUSB_1 AND USE_PTHREAD) - if(LIBFREENECT_FOUND AND LIBUSB_1_FOUND AND PTHREAD_FOUND) +# Check if libfreenect dependencies (ie libusb-1.0 and threads) are available +if(USE_LIBFREENECT AND USE_LIBUSB_1 AND USE_THREADS) + if(LIBFREENECT_FOUND AND LIBUSB_1_FOUND) set(VISP_HAVE_LIBFREENECT_AND_DEPENDENCIES TRUE) # The material is found. Check if libfreenect is an old version @@ -1092,7 +1093,7 @@ endif() # libraries for Pioneer mobile robots if(USE_ARIA AND UNIX) - if(ARIA_FOUND AND USE_PTHREAD AND RT_FOUND AND DL_FOUND) + if(ARIA_FOUND AND USE_THREADS AND RT_FOUND AND DL_FOUND) set(VISP_HAVE_PIONEER TRUE) # for header vpConfig.h endif() elseif(USE_ARIA AND NOT UNIX) @@ -1571,10 +1572,10 @@ status(" RGB-D sensors: ") status(" Use Realsense:" USE_REALSENSE THEN "yes (ver ${REALSENSE_VERSION})" ELSE "no") status(" Use Realsense2:" USE_REALSENSE2 THEN "yes (ver ${REALSENSE2_VERSION})" ELSE "no") status(" Use Occipital Structure:" USE_OCCIPITAL_STRUCTURE THEN "yes" ELSE "no") -status(" Use Kinect:" USE_LIBFREENECT AND USE_LIBUSB_1 AND USE_PTHREAD THEN "yes" ELSE "no") +status(" Use Kinect:" USE_LIBFREENECT AND USE_LIBUSB_1 AND USE_THREADS THEN "yes" ELSE "no") status(" \\- Use libfreenect:" USE_LIBFREENECT THEN "yes (ver ${LIBFREENECT_VERSION})" ELSE "no") status(" \\- Use libusb-1:" USE_LIBUSB_1 THEN "yes (ver ${LIBUSB_1_VERSION})" ELSE "no") -status(" \\- Use pthread:" USE_PTHREAD THEN "yes" ELSE "no") +status(" \\- Use std::thread:" USE_THREADS THEN "yes" ELSE "no") status(" Use PCL:" USE_PCL THEN "yes (ver ${PCL_VERSION})" ELSE "no") status(" \\- Use VTK:" VTK_FOUND THEN "yes (ver ${VTK_VERSION})" ELSE "no") status("") @@ -1601,7 +1602,7 @@ status(" Use json (nlohmann):" USE_NLOHMANN_JSON THEN "yes (ver ${nlohman status("") status(" Optimization: ") status(" Use OpenMP:" USE_OPENMP THEN "yes" ELSE "no") -status(" Use pthread:" USE_PTHREAD THEN "yes" ELSE "no") +status(" Use st::thread:" USE_THREADS THEN "yes" ELSE "no") status(" Use pthread (built-in):" WITH_PTHREAD THEN "yes (ver ${PTHREADS_VERSION})" ELSE "no") status(" Use Simd (built-in):" "yes (ver ${SIMD_VERSION})") status("") diff --git a/CTestConfig.cmake b/CTestConfig.cmake index 10adf905ff..32fb240079 100644 --- a/CTestConfig.cmake +++ b/CTestConfig.cmake @@ -281,8 +281,8 @@ if(VISP_HAVE_XML2) set(BUILDNAME "${BUILDNAME}-xml") endif() # PThread -if(VISP_HAVE_PTHREAD) - set(BUILDNAME "${BUILDNAME}-pthread") +if(VISP_HAVE_THREADS) + set(BUILDNAME "${BUILDNAME}-threads") endif() # OpenMP if(VISP_HAVE_OPENMP) diff --git a/ChangeLog.txt b/ChangeLog.txt index 09295ae08b..33d52135de 100644 --- a/ChangeLog.txt +++ b/ChangeLog.txt @@ -1037,7 +1037,7 @@ ViSP 2.6.1 (released October 20th, 2011) vpRobotSimulator, vpFFMPEG, vpBSpline - It is now possible to grab images from an usb webcam with the vpV4l2Grabber class that is based on the video 4 linux 2 driver. - - When it was possible without braking the backward compatibilty + - When it was possible without braking the backward compatibility std::list was introduced instead of vpList. Some of the functions that use vpList are now deprecated. - Better detection of image or video file types when used in vpImageIo, diff --git a/cmake/AddExtraCompilationFlags.cmake b/cmake/AddExtraCompilationFlags.cmake index 95f15d0d29..03061fbc03 100644 --- a/cmake/AddExtraCompilationFlags.cmake +++ b/cmake/AddExtraCompilationFlags.cmake @@ -113,7 +113,7 @@ if(USE_OPENMP) add_extra_compiler_option("${OpenMP_CXX_FLAGS}") endif() -if(USE_PTHREAD) +if(USE_THREADS) if(THREADS_HAVE_PTHREAD_ARG) add_extra_compiler_option("-pthread") endif() diff --git a/cmake/templates/VISPConfig.cmake.in b/cmake/templates/VISPConfig.cmake.in index c77a73224b..0ed3aaaf2e 100644 --- a/cmake/templates/VISPConfig.cmake.in +++ b/cmake/templates/VISPConfig.cmake.in @@ -259,6 +259,7 @@ set(VISP_HAVE_SOWIN "@VISP_HAVE_SOWIN@") set(VISP_HAVE_SOXT "@VISP_HAVE_SOXT@") set(VISP_HAVE_TAKKTILE2 "@VISP_HAVE_TAKKTILE2@") set(VISP_HAVE_TENSORRT "@VISP_HAVE_TENSORRT@") +set(VISP_HAVE_THREADS "@VISP_HAVE_THREADS@") set(VISP_HAVE_UEYE "@VISP_HAVE_UEYE@") set(VISP_HAVE_UR_RTDE "@VISP_HAVE_UR_RTDE@") set(VISP_HAVE_V4L2 "@VISP_HAVE_V4L2@") diff --git a/cmake/templates/vpConfig.h.in b/cmake/templates/vpConfig.h.in index 67f9f61b93..8190f0c319 100644 --- a/cmake/templates/vpConfig.h.in +++ b/cmake/templates/vpConfig.h.in @@ -149,6 +149,9 @@ // Defined if pthread library available. #cmakedefine VISP_HAVE_PTHREAD +// Defined if std::thread available. +#cmakedefine VISP_HAVE_THREADS + // Defined if YARP available. #cmakedefine VISP_HAVE_YARP diff --git a/doc/config-doxygen.in b/doc/config-doxygen.in index bb4dffd702..7b531402fe 100644 --- a/doc/config-doxygen.in +++ b/doc/config-doxygen.in @@ -2220,6 +2220,7 @@ PREDEFINED = @DOXYGEN_SHOULD_SKIP_THIS@ \ VISP_HAVE_SOXT \ VISP_HAVE_TAKKTILE2 \ VISP_HAVE_TENSORRT \ + VISP_HAVE_THREADS \ VISP_HAVE_UEYE \ VISP_HAVE_UR_RTDE \ VISP_HAVE_V4L2 \ diff --git a/example/servo-biclops/moveBiclops.cpp b/example/servo-biclops/moveBiclops.cpp index f28b995abb..92b5436466 100644 --- a/example/servo-biclops/moveBiclops.cpp +++ b/example/servo-biclops/moveBiclops.cpp @@ -41,14 +41,14 @@ * See http://www.traclabs.com/tracbiclops.htm for more details. */ -#include -#include -#include -#include -#include +#include + +#include + #ifdef VISP_HAVE_BICLOPS #include +#include // List of allowed command line options #define GETOPTARGS "c:h" @@ -145,49 +145,49 @@ int main(int argc, const char **argv) q = 0; q[0] = vpMath::rad(-10); q[1] = vpMath::rad(-20); - std::cout << "Set position in the articular frame: " + std::cout << "Set Joint position " << " pan: " << vpMath::deg(q[0]) << " deg" << " tilt: " << vpMath::deg(q[1]) << " deg" << std::endl; robot.setPositioningVelocity(30.); robot.setPosition(vpRobot::JOINT_STATE, q); robot.getPosition(vpRobot::JOINT_STATE, qm); - std::cout << "Position in the articular frame: " + std::cout << " Joint position " << " 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: " + std::cout << " Joint velocity " << " 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: " + std::cout << "Set Joint position " << " pan: " << vpMath::deg(q[0]) << " deg" << " tilt: " << vpMath::deg(q[1]) << " deg" << std::endl; robot.setPositioningVelocity(10); robot.setPosition(vpRobot::JOINT_STATE, q); robot.getPosition(vpRobot::JOINT_STATE, qm); - std::cout << "Position in the articular frame: " + std::cout << " Joint position: " << " 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: " + std::cout << " Joint velocity: " << " 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::JOINT_STATE, qm); - std::cout << "Position in the articular frame: " + std::cout << " Joint position: " << " 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: " + std::cout << " Joint velocity " << " 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 for 5 sec" + std::cout << "Set joint 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); @@ -196,17 +196,17 @@ int main(int argc, const char **argv) vpTime::wait(5000.0); robot.getPosition(vpRobot::JOINT_STATE, qm); - std::cout << "Position in the articular frame: " + std::cout << " Joint position " << " 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: " + std::cout << " Joint velocity " << " 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 for 3 sec" + std::cout << "Set joint 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); @@ -215,17 +215,17 @@ int main(int argc, const char **argv) vpTime::wait(3000.0); robot.getPosition(vpRobot::JOINT_STATE, qm); - std::cout << "Position in the articular frame: " + std::cout << " Joint position " << " 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: " + std::cout << " Joint velocity " << " 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 for 2 sec" + std::cout << "Set joint 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); @@ -234,18 +234,18 @@ int main(int argc, const char **argv) vpTime::wait(2000.0); robot.getPosition(vpRobot::JOINT_STATE, qm); - std::cout << "Position in the articular frame: " + std::cout << " Joint position " << " 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: " + std::cout << " Joint velocity " << " 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 for 2 sec" + std::cout << "Set joint 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); @@ -254,11 +254,11 @@ int main(int argc, const char **argv) vpTime::wait(2000.0); robot.getPosition(vpRobot::JOINT_STATE, qm); - std::cout << "Position in the articular frame: " + std::cout << " Joint position " << " 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: " + std::cout << " Joint velocity " << " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl; return EXIT_SUCCESS; } diff --git a/example/servo-ptu46/servoPtu46Point2DArtVelocity.cpp b/example/servo-ptu46/servoPtu46Point2DArtVelocity.cpp index def4c02299..7b5011991f 100644 --- a/example/servo-ptu46/servoPtu46Point2DArtVelocity.cpp +++ b/example/servo-ptu46/servoPtu46Point2DArtVelocity.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -32,8 +31,7 @@ * tests the control law * eye-in-hand control * velocity computed in articular - * -*****************************************************************************/ + */ /*! \file servoPtu46Point2DArtVelocity.cpp @@ -61,11 +59,9 @@ #endif #include -#if (defined(VISP_HAVE_PTU46) & defined(VISP_HAVE_DC1394)) +#if (defined(VISP_HAVE_PTU46) & defined(VISP_HAVE_DC1394) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)) -#ifdef VISP_HAVE_PTHREAD -#include -#endif +#include #include #include @@ -87,16 +83,12 @@ #include -#ifdef VISP_HAVE_PTHREAD -pthread_mutex_t mutexEndLoop = PTHREAD_MUTEX_INITIALIZER; -#endif +std::mutex mutexEndLoop; void signalCtrC(int signumber) { (void)(signumber); -#ifdef VISP_HAVE_PTHREAD - pthread_mutex_unlock(&mutexEndLoop); -#endif + mutexEndLoop.unlock(); usleep(1000 * 10); vpTRACE("Ctrl-C pressed..."); } @@ -113,10 +105,7 @@ int main() std::cout << std::endl; try { - -#ifdef VISP_HAVE_PTHREAD - pthread_mutex_lock(&mutexEndLoop); -#endif + mutexEndLoop.lock(); signal(SIGINT, &signalCtrC); vpRobotPtu46 robot; @@ -135,7 +124,8 @@ int main() try { g.acquire(I); - } catch (...) { + } + catch (...) { vpERROR_TRACE(" Error caught"); return EXIT_FAILURE; } @@ -146,7 +136,8 @@ int main() try { vpDisplay::display(I); vpDisplay::flush(I); - } catch (...) { + } + catch (...) { vpERROR_TRACE(" Error caught"); return EXIT_FAILURE; } @@ -164,7 +155,8 @@ int main() // dot.initTracking(I) ; dot.track(I); vpERROR_TRACE("after dot.initTracking(I) "); - } catch (...) { + } + catch (...) { vpERROR_TRACE(" Error caught "); return EXIT_FAILURE; } @@ -215,12 +207,7 @@ int main() unsigned int iter = 0; vpTRACE("\t loop"); -#ifdef VISP_HAVE_PTHREAD - while (0 != pthread_mutex_trylock(&mutexEndLoop)) -#else - for (;;) -#endif - { + while (0 != mutexEndLoop.trylock()) { std::cout << "---------------------------------------------" << iter << std::endl; g.acquire(I); @@ -249,7 +236,8 @@ int main() vpTRACE("Display task information "); task.print(); - } catch (const vpException &e) { + } + catch (const vpException &e) { std::cout << "Sorry PtU46 not available. Got exception: " << e << std::endl; return EXIT_FAILURE } diff --git a/modules/core/CMakeLists.txt b/modules/core/CMakeLists.txt index 2734dad673..d464b4d544 100644 --- a/modules/core/CMakeLists.txt +++ b/modules/core/CMakeLists.txt @@ -226,7 +226,7 @@ if(USE_XML2) list(APPEND opt_incs ${XML2_INCLUDE_DIRS}) list(APPEND opt_libs ${XML2_LIBRARIES}) endif() -if(USE_PTHREAD) +if(USE_THREADS) if(CMAKE_THREAD_LIBS_INIT) list(APPEND opt_libs "${CMAKE_THREAD_LIBS_INIT}") endif() diff --git a/modules/core/include/visp3/core/vpMatrix.h b/modules/core/include/visp3/core/vpMatrix.h index 2b6b65bdf6..a1f28fd036 100644 --- a/modules/core/include/visp3/core/vpMatrix.h +++ b/modules/core/include/visp3/core/vpMatrix.h @@ -998,7 +998,7 @@ vpMatrix M(R); */ //@{ /*! - \deprecated Only provided for compatibilty with ViSP previous releases. + \deprecated Only provided for compatibility with ViSP previous releases. This function does nothing. */ vp_deprecated void init() { } diff --git a/modules/robot/CMakeLists.txt b/modules/robot/CMakeLists.txt index b376dc5517..d402d398bd 100644 --- a/modules/robot/CMakeLists.txt +++ b/modules/robot/CMakeLists.txt @@ -49,7 +49,7 @@ if(USE_VIRTUOSE) list(APPEND opt_incs ${VIRTUOSE_INCLUDE_DIRS}) list(APPEND opt_libs ${VIRTUOSE_LIBRARIES}) - if(USE_PTHREAD AND RT_FOUND AND DL_FOUND) + if(USE_THREADS AND RT_FOUND AND DL_FOUND) if(CMAKE_THREAD_LIBS_INIT) list(APPEND opt_libs "${CMAKE_THREAD_LIBS_INIT}") endif() diff --git a/modules/robot/include/visp3/robot/vpAfma6.h b/modules/robot/include/visp3/robot/vpAfma6.h index f004056f77..d1e698cb98 100644 --- a/modules/robot/include/visp3/robot/vpAfma6.h +++ b/modules/robot/include/visp3/robot/vpAfma6.h @@ -78,7 +78,7 @@ class VISP_EXPORT vpAfma6 #ifdef VISP_HAVE_AFMA6_DATA //! File where constant parameters in relation with the robot are stored: //! joint max, min, coupling factor between 4 ant 5 joint, distance between - //! 5 and 6 joint, tranformation eMc between end-effector and camera frame. + //! 5 and 6 joint, transformation eMc between end-effector and camera frame. static const std::string CONST_AFMA6_FILENAME; static const std::string CONST_EMC_CCMOP_WITHOUT_DISTORTION_FILENAME; static const std::string CONST_EMC_CCMOP_WITH_DISTORTION_FILENAME; diff --git a/modules/robot/include/visp3/robot/vpBiclops.h b/modules/robot/include/visp3/robot/vpBiclops.h index 0851327974..7ae339b8d5 100644 --- a/modules/robot/include/visp3/robot/vpBiclops.h +++ b/modules/robot/include/visp3/robot/vpBiclops.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 _vpBiclops_h_ #define _vpBiclops_h_ @@ -53,135 +51,273 @@ #include /*! - - \class vpBiclops - - \ingroup group_robot_real_ptu - - \brief Jacobian, geometric model functionalities... for biclops, pan, tilt - head. - - Two different Denavit Hartenberg representations of the robot are - implemented. As mentionned in vpBiclops::DenavitHartenbergModel they differ - in the orientation of the tilt axis. Use setDenavitHartenbergModel() to - select the representation. - - See http://www.traclabs.com/tracbiclops.htm for more details concerning the - hardware. - -*/ - + * \class vpBiclops + * + * \ingroup group_robot_real_ptu + * + * \brief Jacobian, geometric model functionalities... for Biclops, pan, tilt + * head. + * + * Two different Denavit Hartenberg representations of the robot are + * implemented. As mentioned in vpBiclops::DenavitHartenbergModel they differ + * in the orientation of the tilt axis. Use setDenavitHartenbergModel() to + * select the representation. + * + * See http://www.traclabs.com/tracbiclops.htm for more details concerning the + * hardware. + * + */ class VISP_EXPORT vpBiclops { public: /*! - Two different Denavit Hartenberg representations of the robot are - implemented. They differ in the orientation of the tilt axis. - - The first representation, vpBiclops::DH1 is given by: - - \f[ - \begin{tabular}{|c|c|c|c|c|} - \hline - Joint & $a_i$ & $d_i$ & $\alpha_i$ & $\theta_i$ \\ - \hline - 1 & 0 & 0 & $-\pi/2$ & $q_1^*$ \\ - 2 & 0 & 0 & $ \pi/2$ & $q_2^* + \pi/2$ \\ - \hline - \end{tabular} - \f] - - The second one, vpBiclops::DH2 is given by: - - \f[ - \begin{tabular}{|c|c|c|c|c|} - \hline - Joint & $a_i$ & $d_i$ & $\alpha_i$ & $\theta_i$ \\ - \hline - 1 & 0 & 0 & $ \pi/2$ & $q_1^*$ \\ - 2 & 0 & 0 & $ -\pi/2$ & $q_2^* - \pi/2$ \\ - \hline - \end{tabular} - \f] - - where \f$q_1^*, q_2^*\f$ are respectively the pan and tilt joint - positions. - - In those representations, the pan is oriented from left to right, while - the tilt is oriented - - in vpBiclops::DH1 from down to top, - - in vpBiclops::DH2 from top to down. - - - */ - typedef enum { - DH1, /*!< First Denavit Hartenberg representation. */ - DH2 /*!< Second Denavit Hartenberg representation. */ + * Two different Denavit Hartenberg representations of the robot are + * implemented. As you can see in the next image, they differ in the orientation of the tilt axis. + * + * \image html img-biclops-frames.jpg Biclops PT models + * + * The first representation, vpBiclops::DH1 is given by: + * + * | Joint | \f$a_i\f$ | \f$d_i\f$ | \f$\alpha_i\f$ | \f$\theta_i\f$ | + * | :---: | :-------: | :-------: | -------------: | ----------------: | + * | 1 | 0 | 0 | \f$-\pi/2\f$ | \f$q_1\f$ | + * | 2 | 0 | 0 | \f$ \pi/2\f$ | \f$q_2 + \pi/2\f$ | + * + * The second one, vpBiclops::DH2 is given by: + * + * | Joint | \f$a_i\f$ | \f$d_i\f$ | \f$\alpha_i\f$ | \f$\theta_i\f$ | + * | :---: | :-------: | :-------: | -------------: | ----------------: | + * | 1 | 0 | 0 | \f$ \pi/2\f$ | \f$q_1\f$ | + * | 2 | 0 | 0 | \f$-\pi/2\f$ | \f$q_2 - \pi/2\f$ | + * + * where \f$q_1, q_2\f$ are respectively the pan and tilt joint + * positions. + * + * In those representations, the pan is oriented from left to right, while + * the tilt is oriented + * - in vpBiclops::DH1 from down to top, + * - in vpBiclops::DH2 from top to down. + */ + typedef enum + { + DH1, //!< First Denavit Hartenberg representation. + DH2 //!< Second Denavit Hartenberg representation. } DenavitHartenbergModel; public: /* Constants */ - static const unsigned int ndof; /*!< Number of dof */ + static const unsigned int ndof; //!< Number of dof /* Geometric model */ - static const float h; + static const float h; //!< Vertical distance between camera and pan/tilt end-effector (see init()) - static const float panJointLimit; - static const float tiltJointLimit; - static const float speedLimit; + static const float panJointLimit; //!< Pan +/- joint limit + static const float tiltJointLimit; //!< Tilt +/- joint limit + static const float speedLimit; //!< Pan/tilt +/- max joint velocity protected: - DenavitHartenbergModel dh_model_; - vpHomogeneousMatrix cMe_; // Camera frame to mobile platform frame + DenavitHartenbergModel m_dh_model; //!< Denavit-Hartenberg model + vpHomogeneousMatrix m_cMe; // Camera frame to PT end-effector frame transformation public: + /*! + * Default constructor. Call init(). + */ vpBiclops(void); - /*! Destructor that does nothing. */ - virtual ~vpBiclops(){}; + + /*! + * Destructor that does nothing. + */ + virtual ~vpBiclops() { }; /** @name Inherited functionalities from vpBiclops */ //@{ + /*! + * Initialize the default \f${^c}{\bf M}_e\f$ transformation calling set_cMe(). + * + * \f[ + * {^c}{\bf M}_e = \left( + * \begin{matrix} + * 0 & 1 & 0 & 0 \\ + * -1 & 0 & 0 & h \\ + * 0 & 0 & 1 & 0 \\ + * 0 & 0 & 0 & 1 + * \end{matrix} + * \right) + * \f] + */ void init(void); + /*! + * Compute the direct geometric model of the camera: fMc + * + * \warning Provided for compatibility with previous versions. Use rather + * get_fMc(const vpColVector &, vpHomogeneousMatrix &). + * + * \param q : Articular position for pan and tilt axis. + * + * \param fMc : Homogeneous matrix corresponding to the direct geometric model + * of the camera. Describes the transformation between the robot reference + * frame (called fixed) and the camera frame. + * + * \sa get_fMc(const vpColVector &, vpHomogeneousMatrix &) + */ void computeMGD(const vpColVector &q, vpHomogeneousMatrix &fMc) const; + + /*! + * Return the direct geometric model of the camera: fMc + * + * \warning Provided for compatibility with previous versions. Use rather + * get_fMc(const vpColVector &). + * + * \param q : Articular position for pan and tilt axis. + * + * \return fMc, the homogeneous matrix corresponding to the direct geometric + * model of the camera. Describes the transformation between the robot + * reference frame (called fixed) and the camera frame. + * + * \sa get_fMc(const vpColVector &) + */ vpHomogeneousMatrix computeMGD(const vpColVector &q) const; - void computeMGD(const vpColVector &q, vpPoseVector &fvc) const; /*! - Return the tranformation \f${^c}{\bf M}_e\f$ between the camera frame and - the end effector frame. - */ - vpHomogeneousMatrix get_cMe() const { return cMe_; } + * Compute the direct geometric model of the camera in terms of pose vector. + * + * \warning Provided for compatibility with previous versions. Use rather + * get_fMc(const vpColVector &, vpPoseVector &). + * + * \param q : Articular position for pan and tilt axis. + * + * \param fPc : Pose vector corresponding to the transformation between the + * robot reference frame (called fixed) and the camera frame. + * + * \sa get_fMc(const vpColVector &, vpPoseVector &) + */ + void computeMGD(const vpColVector &q, vpPoseVector &fPc) const; + + /*! + * Return the transformation \f${^c}{\bf M}_e\f$ between the camera frame and + * the end effector frame. + */ + vpHomogeneousMatrix get_cMe() const { return m_cMe; } + + /*! + * Get the twist matrix corresponding to the transformation between the + * camera frame and the end effector frame. The end effector frame is located + * on the tilt axis. + * + * \param cVe : Twist transformation between camera and end effector frame to + * express a velocity skew from end effector frame in camera frame. + */ + void get_cVe(vpVelocityTwistMatrix &cVe) const; - void get_cVe(vpVelocityTwistMatrix &_cVe) const; + /*! + * Compute the direct geometric model of the camera: fMc + * + * \param[in] q : Articular position for pan and tilt axis. + * + * \param[out] fMc : Homogeneous matrix corresponding to the direct geometric model + * of the camera. Describes the transformation between the robot reference + * frame (called fixed) and the camera frame. + */ void get_fMc(const vpColVector &q, vpHomogeneousMatrix &fMc) const; - void get_fMc(const vpColVector &q, vpPoseVector &fvc) const; + + /*! + * Compute the direct geometric model of the camera in terms of pose vector. + * + * \param[in] q : Articular position for pan and tilt axis. + * + * \param[out] fPc : Pose vector corresponding to the transformation between the + * robot reference frame (called fixed) and the camera frame. + */ + void get_fMc(const vpColVector &q, vpPoseVector &fPc) const; + + /*! + * Return the direct geometric model of the camera: fMc + * + * \param q : Articular position for pan and tilt axis. + * + * \return fMc, the homogeneous matrix corresponding to the direct geometric + * model of the camera. Describes the transformation between the robot + * reference frame (called fixed) and the camera frame. + */ vpHomogeneousMatrix get_fMc(const vpColVector &q) const; + + /*! + * Return the direct geometric model of the end effector: fMe + * + * \param q : Articular position for pan and tilt axis. + * + * \return fMe, the homogeneous matrix corresponding to the direct geometric + * model of the end effector. Describes the transformation between the robot + * reference frame (called fixed) and the end effector frame. + */ vpHomogeneousMatrix get_fMe(const vpColVector &q) const; + /*! + * Get the robot jacobian expressed in the end-effector frame. + * + * \warning Re is not the embedded camera frame. It corresponds to the frame + * associated to the tilt axis (see also get_cMe). + * + * \param q : Articular position for pan and tilt axis. + * + * \param eJe : Jacobian between end effector frame and end effector frame (on + * tilt axis). + */ void get_eJe(const vpColVector &q, vpMatrix &eJe) const; + + /*! + * Get the robot jacobian expressed in the robot reference frame + * + * \param q : Articular position for pan and tilt axis. + * + * \param fJe : Jacobian between reference frame (or fix frame) and end + * effector frame (on tilt axis). + */ void get_fJe(const vpColVector &q, vpMatrix &fJe) const; /*! - Return the Denavit Hartenberg representation used to model the head. - \sa vpBiclops::DenavitHartenbergModel - */ - inline vpBiclops::DenavitHartenbergModel getDenavitHartenbergModel() const { return dh_model_; } + * Return the Denavit Hartenberg representation used to model the head. + * \sa vpBiclops::DenavitHartenbergModel + */ + inline vpBiclops::DenavitHartenbergModel getDenavitHartenbergModel() const { return m_dh_model; } + /*! + * Set the default homogeneous matrix corresponding to the transformation + * between the camera frame and the end effector frame. The end effector frame + * is located on the tilt axis. + * + * \f[ + * {^c}{\bf M}_e = \left( + * \begin{matrix} + * 0 & 1 & 0 & 0 \\ + * -1 & 0 & 0 & h \\ + * 0 & 0 & 1 & 0 \\ + * 0 & 0 & 0 & 1 + * \end{matrix} + * \right) + * \f] + */ void set_cMe(); + /*! - Set the transformation between the camera frame and the end effector - frame. - */ - void set_cMe(const vpHomogeneousMatrix &cMe) { cMe_ = cMe; } + * Set the transformation between the camera frame and the end effector + * frame. + */ + void set_cMe(const vpHomogeneousMatrix &cMe) { m_cMe = cMe; } /*! - Set the Denavit Hartenberg representation used to model the head. - - \sa vpBiclops::DenavitHartenbergModel - */ - inline void setDenavitHartenbergModel(vpBiclops::DenavitHartenbergModel m = vpBiclops::DH1) { dh_model_ = m; } + * Set the Denavit Hartenberg representation used to model the head. + * + * \sa vpBiclops::DenavitHartenbergModel + */ + inline void setDenavitHartenbergModel(vpBiclops::DenavitHartenbergModel m = vpBiclops::DH1) { m_dh_model = m; } //@} + + /*! + * Update Biclops PT head constants to output stream. + */ friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpBiclops &constant); }; diff --git a/modules/robot/include/visp3/robot/vpRobotBiclops.h b/modules/robot/include/visp3/robot/vpRobotBiclops.h index bf1b93c9e5..1d904948fb 100644 --- a/modules/robot/include/visp3/robot/vpRobotBiclops.h +++ b/modules/robot/include/visp3/robot/vpRobotBiclops.h @@ -44,7 +44,7 @@ /* --- GENERAL --- */ #include -#include +#include #include /* --- ViSP --- */ @@ -94,55 +94,342 @@ class VISP_EXPORT vpRobotBiclops : public vpBiclops, public vpRobot public: static const double defaultPositioningVelocity; + /*! + * Default constructor. + * + * Does nothing more than setting the default configuration file + * to `/usr/share/BiclopsDefault.cfg`. + * + * As shown in the following example, the turret need to be initialized + * using init() function. + * + * \code + * #include + * + * int main() + * { + * #ifdef VISP_HAVE_BICLOPS + * vpRobotBiclops robot; // Use the default config file in /usr/share/BiclopsDefault.cfg + * + * // Initialize the head + * robot.init(); + * + * // Move the robot to a specified pan and tilt + * robot.setRobotState(vpRobot::STATE_POSITION_CONTROL); + * vpColVector q(2); + * q[0] = vpMath::rad(20); // pan + * q[1] = vpMath::rad(40); // tilt + * robot.setPosition(vpRobot::ARTICULAR_FRAME, q); + * #endif + * return 0; + * } + * \endcode + */ vpRobotBiclops(); + + /*! + * Constructor that initialize the Biclops pan, tilt head by reading the + * configuration file provided by Traclabs + * and do the homing sequence. + * + * The following example shows how to use the constructor. + * + * \code + * #include + * + * int main() + * { + * #ifdef VISP_HAVE_BICLOPS + * // Specify the config file location and initialize the turret + * vpRobotBiclops robot("/usr/share/BiclopsDefault.cfg"); + * + * // Move the robot to a specified pan and tilt + * robot.setRobotState(vpRobot::STATE_POSITION_CONTROL); + * + * vpColVector q(2); + * q[0] = vpMath::rad(-20); // pan + * q[1] = vpMath::rad(10); // tilt + * robot.setPosition(vpRobot::ARTICULAR_FRAME, q); + * #endif + * return 0; + * } + * \endcode + */ explicit vpRobotBiclops(const std::string &filename); + + /*! + * Destructor. + * Wait the end of the control thread. + */ virtual ~vpRobotBiclops(); + /*! + * Set the Biclops config filename. + * Check if the config file exists and initialize the head. + * + * \exception vpRobotException::constructionError If the config file cannot be + * opened. + */ void init(); - void get_cMe(vpHomogeneousMatrix &_cMe) const; - void get_cVe(vpVelocityTwistMatrix &_cVe) const; - void get_eJe(vpMatrix &_eJe); - void get_fJe(vpMatrix &_fJe); - + /*! + * Get the homogeneous matrix corresponding to the transformation between the + * camera frame and the end effector frame. The end effector frame is located + * on the tilt axis. + * + * \param cMe : Homogeneous matrix between camera and end effector frame. + */ + void get_cMe(vpHomogeneousMatrix &cMe) const; + + /*! + * Get the twist matrix corresponding to the transformation between the + * camera frame and the end effector frame. The end effector frame is located + * on the tilt axis. + * + * \param cVe : Twist transformation between camera and end effector frame to + * express a velocity skew from end effector frame in camera frame. + */ + void get_cVe(vpVelocityTwistMatrix &cVe) const; + + /*! + * Get the robot jacobian expressed in the end-effector frame. + * + * \warning Re is not the embedded camera frame. It corresponds to the frame + * associated to the tilt axis (see also get_cMe). + * + * \param eJe : Jacobian between end effector frame and end effector frame (on + * tilt axis). + */ + void get_eJe(vpMatrix &eJe); + + /*! + * Get the robot jacobian expressed in the robot reference frame + * + * \param fJe : Jacobian between reference frame (or fix frame) and end + * effector frame (on tilt axis). + */ + void get_fJe(vpMatrix &fJe); + + /*! + * Get the robot displacement since the last call of this method. + * + * \warning The first call of this method gives not a good value for the + * displacement. + * + * \param frame The frame in which the measured displacement is expressed. + * + * \param d The displacement: + * + * - In joint state, the dimension of q is 2 (the number of axis of the robot) + * with respectively d[0] (pan displacement), d[1] (tilt displacement). + * + * - In camera frame, the dimension of d is 6 (tx, ty, ty, tux, tuy, tuz). + * Translations are expressed in meters, rotations in radians with the theta U + * representation. + * + * \exception vpRobotException::wrongStateError If a not supported frame type + * is given. + */ void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &d); + + /*! + * Return the position of each axis. + * - In positioning control mode, call vpRobotBiclopsController::getPosition() + * - In speed control mode, call vpRobotBiclopsController::getActualPosition() + * + * \param frame : Control frame. This Biclops head can only be controlled in + * joint state. + * + * \param q : The position of the axis in radians. + * + * \exception vpRobotException::wrongStateError : If a not supported frame type + * is given. + */ void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q); + + /*! + * Get the velocity in % used for a position control. + * + * \return Positioning velocity in [0, 100.0]. The + * maximum positioning velocity is given vpBiclops::speedLimit. + */ double getPositioningVelocity(void); + + /*! + * Get the joint velocity. + * + * \param frame : Control frame. This head can only be controlled in joint state. + * + * \param q_dot : The measured joint velocity in rad/s. + * + * \exception vpRobotException::wrongStateError : If a not supported frame type + * is given. + */ void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q_dot); + + /*! + * Return the joint velocity. + * + * \param frame : Control frame. This head can only be controlled in joint state. + * + * \return The measured joint velocity in rad/s. + * + * \exception vpRobotException::wrongStateError : If a not supported frame type + * is given. + */ vpColVector getVelocity(const vpRobot::vpControlFrameType frame); + /*! + * Get joint positions from the position file. + * + * \param filename : Position file. + * + * \param q : The joint positions read in the file. + * + * \code + * # Example of Biclops position file + * # The axis positions must be preceeded by R: + * # First value : pan joint position in degrees + * # Second value: tilt joint position in degrees + * R: 15.0 5.0 + * \endcode + * + * \return true if a position was found, false otherwise. + */ bool readPositionFile(const std::string &filename, vpColVector &q); + /*! + * Set the Biclops config filename. + */ void setConfigFile(const std::string &filename = "/usr/share/BiclopsDefault.cfg"); + + /*! + * Move the robot in position control. + * + * \warning This method is blocking. That mean that it waits the end of the + * positioning. + * + * \param frame : Control frame. This Biclops head can only be controlled in + * joint state. + * + * \param q : The position to set for each axis in radians. + * + * \exception vpRobotException::wrongStateError : If a not supported frame + * type is given. + */ void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q); + + /*! + * Move the robot in position control. + * + * \warning This method is blocking. That mean that it wait the end of the + * positioning. + * + * \param frame : Control frame. This Biclops head can only be controlled in + * joint state. + * + * \param q1 : The pan position to set in radians. + * \param q2 : The tilt position to set in radians. + * + * \exception vpRobotException::wrongStateError : If a not supported frame + * type is given. + */ void setPosition(const vpRobot::vpControlFrameType frame, const double &q1, const double &q2); - void setPosition(const char *filename); + + /*! + * Read the content of the position file and moves to head to joint + * positions. + * + * \param filename : Position filename + * + * \exception vpRobotException::readingParametersError : If the joint + * positions cannot be read from file. + * + * \sa readPositionFile() + */ + void setPosition(const std::string &filename); + + /*! + * Set the velocity used for a position control. + * + * \param velocity : Velocity in % of the maximum velocity between [0,100]. The + * maximum velocity is given vpBiclops::speedLimit. + */ void setPositioningVelocity(double velocity); + + /*! + * Change the state of the robot either to stop them, or to set position or + * speed control. + */ vpRobot::vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState); + + /*! + * Send a velocity on each axis. + * + * \param frame : Control frame. This Biclops head can only be controlled in + * joint state. Be aware, the camera frame (vpRobot::CAMERA_FRAME), the reference + * frame (vpRobot::REFERENCE_FRAME), end-effector frame (vpRobot::END_EFFECTOR_FRAME) + * and the mixt frame (vpRobot::MIXT_FRAME) are not implemented. + * + * \param q_dot : The desired joint velocities for each axis in rad/s. \f$ \dot + * {r} = [\dot{q}_1, \dot{q}_2]^t \f$ with \f$ \dot{q}_1 \f$ the pan of the + * camera and \f$ \dot{q}_2\f$ the tilt of the camera. + * + * \exception vpRobotException::wrongStateError : If a the robot is not + * configured to handle a velocity. The robot can handle a velocity only if the + * velocity control mode is set. For that, call setRobotState( + * vpRobot::STATE_VELOCITY_CONTROL) before setVelocity(). + * + * \exception vpRobotException::wrongStateError : If a not supported frame type + * (vpRobot::CAMERA_FRAME, vpRobot::REFERENCE_FRAME, vpRobot::END_EFFECTOR_FRAME + * or vpRobot::MIXT_FRAME) is given. + * + * \warning Velocities could be saturated if one of them exceed the maximal + * authorized speed (see vpRobot::maxRotationVelocity). + */ void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &q_dot); + /*! + * Halt all the axis. + */ void stopMotion(); + /* + * Control loop to manage the Biclops joint limits in speed control. + * + * This control loop is running in a separate thread in order to detect each 5 + * ms joint limits during the speed control. If a joint limit is detected the + * axis should be halted. + * + * \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, + * Traclabs put soft limits in that keep any command from driving to a position + * too close to the hard limits. In velocity mode this protection does not + * exist in the current API. + * + * \warning With the understanding that hitting the hard limits at full + * speed/power can damage the unit, damage due to velocity mode commanding is + * under user responsibility. + */ static void *vpRobotBiclopsSpeedControlLoop(void *arg); private: - static bool robotAlreadyCreated; - pthread_t control_thread; + std::thread m_control_thread; - std::string configfile; // Biclops config file + std::string m_configfile; // Biclops config file - vpRobotBiclopsController controller; + vpRobotBiclopsController m_controller; - double positioningVelocity; - vpColVector q_previous; - bool controlThreadCreated; + double m_positioningVelocity; + vpColVector m_q_previous; // private: //#ifndef DOXYGEN_SHOULD_SKIP_THIS // /*! \brief No copy constructor allowed. */ // vpRobotBiclops(const vpRobotBiclops &) - // : vpBiclops(), vpRobot(), control_thread(), controller(), - // positioningVelocity(0), q_previous(), controlThreadCreated(false) + // : vpBiclops(), vpRobot(), m_control_thread(), m_controller(), + // m_positioningVelocity(0), m_q_previous() // { // throw vpException(vpException::functionNotImplementedError, "Not // implemented!"); diff --git a/modules/robot/include/visp3/robot/vpRobotBiclopsController.h b/modules/robot/include/visp3/robot/vpRobotBiclopsController.h index 16694dba7d..9264798643 100644 --- a/modules/robot/include/visp3/robot/vpRobotBiclopsController.h +++ b/modules/robot/include/visp3/robot/vpRobotBiclopsController.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,25 +29,23 @@ * * Description: * Interface for the Biclops robot. - * -*****************************************************************************/ + */ + +#ifndef _vpRobotBiclopsController_h_ +#define _vpRobotBiclopsController_h_ #include #ifndef DOXYGEN_SHOULD_SKIP_THIS #ifdef VISP_HAVE_BICLOPS -#ifndef _vpRobotBiclopsController_h_ -#define _vpRobotBiclopsController_h_ /* ------------------------------------------------------------------------ */ /* --- INCLUDES ----------------------------------------------------------- */ /* ------------------------------------------------------------------------ */ -/* --- GENERAL --- */ -#include /* Classe std::ostream. */ -#include /* Classe std::ostream. */ -#include /* Classe std::ostream. */ +#include +#include #include "Biclops.h" // Contrib for Biclops robot #include "PMDUtils.h" // Contrib for Biclops robot @@ -79,30 +76,37 @@ class VISP_EXPORT Biclops; // needed for dll creation class VISP_EXPORT vpRobotBiclopsController { public: - typedef enum { + /*! + * Biclops head controller status. + */ + typedef enum + { STOP, /*!< Have to stop the robot. */ SPEED /*!< Can send the desired speed. */ } vpControllerStatusType; public: #ifndef DOXYGEN_SHOULD_SKIP_THIS - // SHM - typedef struct /* ControllerShm_struct */ { + /*! + * Biclops head shared memory structure. + */ + typedef struct + { vpControllerStatusType status[2]; - double q_dot[2]; /*!< Desired speed. */ - double actual_q[2]; /*!< Current measured position of each axes. */ - double actual_q_dot[2]; /*!< Current measured velocity of each axes. */ - bool jointLimit[2]; /*!< Indicates if an axe is in joint limit. */ + double q_dot[2]; //!< Desired speed. + double actual_q[2]; //!< Current measured position of each axes. + double actual_q_dot[2]; //!< Current measured velocity of each axes. + bool jointLimit[2]; //!< Indicates if an axe is in joint limit. } shmType; #endif /* DOXYGEN_SHOULD_SKIP_THIS */ // private: //#ifndef DOXYGEN_SHOULD_SKIP_THIS // vpRobotBiclopsController(const vpRobotBiclopsController &) - // : biclops(), axisMask(0), panAxis(NULL), tiltAxis(NULL), - // vergeAxis(NULL), - // panProfile(), tiltProfile(), vergeProfile(), shm(), - // stopControllerThread_(false) + // : biclops(), m_axisMask(0), m_panAxis(NULL), m_tiltAxis(NULL), + // m_vergeAxis(NULL), + // m_panProfile(), m_tiltProfile(), m_vergeProfile(), shm(), + // m_stopControllerThread(false) // { // throw vpException(vpException::functionNotImplementedError, "Not // implemented!"); @@ -114,39 +118,135 @@ class VISP_EXPORT vpRobotBiclopsController //#endif public: + /*! + * Default constructor. + */ vpRobotBiclopsController(); + + /*! + * Destructor. + */ virtual ~vpRobotBiclopsController(); + + /*! + * Initialize the Biclops by homing all axis. + * + * \param configfile : Biclops configuration file. + * + * \exception vpRobotException::notInitializedError If the Biclops head cannot + * be initialized. The initialization can failed, + * - if the head is not powered on, + * - if the head is not connected to your computer throw a serial cable, + * - if you try to open a bad serial port. Check you config file to verify + * which is the used serial port. + */ void init(const std::string &configfile); + + /*! + * Set the Biclops axis position. The motion of the axis is synchronized to end + * on the same time. + * + * \warning Wait the end of the positioning. + * + * \param q : The position to set for each axis. + * + * \param percentVelocity : The velocity displacement to reach the new position + * in the range [0: 100.0]. 100 % corresponds to the maximal admissible + * speed. The maximal admissible speed is given by vpBiclops::speedLimit. + */ void setPosition(const vpColVector &q, double percentVelocity); + + /*! + * Apply a velocity to each axis of the Biclops robot. + * + * \warning This method is non blocking. + * + * \param q_dot : Velocity to apply. + */ void setVelocity(const vpColVector &q_dot); + + /*! + * Get the Biclops joint positions. + * + * \return The axis joint positions in radians. + */ vpColVector getPosition(); + + /*! + * Get the Biclops actual joint positions. + * + * \return The axis actual joint positions in radians. + */ vpColVector getActualPosition(); + + /*! + * Get the Biclops joint velocities. + * + * \return The axis joint velocities in rad/s. + */ vpColVector getVelocity(); + + /*! + * Get the Biclops actual joint velocities. + * + * \return The axis actual joint velocities in rad/s. + */ vpColVector getActualVelocity(); - PMDAxisControl *getPanAxis() { return panAxis; }; - PMDAxisControl *getTiltAxis() { return tiltAxis; }; - PMDAxisControl *getVergeAxis() { return vergeAxis; }; + + /*! + * Return a pointer to the PMD pan axis. + */ + PMDAxisControl *getPanAxis() { return m_panAxis; }; + + /*! + * Return a pointer to the PMD tilt axis. + */ + PMDAxisControl *getTiltAxis() { return m_tiltAxis; }; + + /*! + * Return a pointer to the PMD verge axis. + */ + PMDAxisControl *getVergeAxis() { return m_vergeAxis; }; + + /*! + * Update the shared memory. + * + * \param shm : Content to write in the shared memory. + */ void writeShm(shmType &shm); + + /*! + * Get a copy of the shared memory. + * + * \return A copy of the shared memory. + */ shmType readShm(); - bool isStopRequested() { return stopControllerThread_; } - void stopRequest(bool stop) { stopControllerThread_ = stop; } + /*! + * Return true when control thread is requested to stop. + */ + bool isStopRequested() { return m_stopControllerThread; } + + /*! + * Send a control thread stop request. + */ + void stopRequest(bool stop) { m_stopControllerThread = stop; } private: - Biclops biclops; // THE interface to Biclops. - int axisMask; + Biclops m_biclops; // THE interface to Biclops. + int m_axisMask; // Pointers to each axis (populated once controller is initialized). - PMDAxisControl *panAxis; - PMDAxisControl *tiltAxis; - PMDAxisControl *vergeAxis; + PMDAxisControl *m_panAxis; + PMDAxisControl *m_tiltAxis; + PMDAxisControl *m_vergeAxis; - PMDAxisControl::Profile panProfile; - PMDAxisControl::Profile tiltProfile; - PMDAxisControl::Profile vergeProfile; + PMDAxisControl::Profile m_panProfile; + PMDAxisControl::Profile m_tiltProfile; + PMDAxisControl::Profile m_vergeProfile; - shmType shm; - bool stopControllerThread_; + shmType m_shm; + bool m_stopControllerThread; }; #endif /* #ifndef _vpRobotBiclopsController_h_ */ diff --git a/modules/robot/include/visp3/robot/vpUnicycle.h b/modules/robot/include/visp3/robot/vpUnicycle.h index 01ce9023f0..9fa31000e1 100644 --- a/modules/robot/include/visp3/robot/vpUnicycle.h +++ b/modules/robot/include/visp3/robot/vpUnicycle.h @@ -65,7 +65,7 @@ class VISP_EXPORT vpUnicycle /** @name Inherited functionalities from vpUnicycle */ //@{ /*! - Return the tranformation \f${^c}{\bf M}_e\f$ between the camera frame + Return the transformation \f${^c}{\bf M}_e\f$ between the camera frame and the mobile robot end effector frame. */ vpHomogeneousMatrix get_cMe() const { return cMe_; } diff --git a/modules/robot/include/visp3/robot/vpViper650.h b/modules/robot/include/visp3/robot/vpViper650.h index 05f3d037d5..c4eedb66a3 100644 --- a/modules/robot/include/visp3/robot/vpViper650.h +++ b/modules/robot/include/visp3/robot/vpViper650.h @@ -100,7 +100,7 @@ class VISP_EXPORT vpViper650 : public vpViper { public: #ifdef VISP_HAVE_VIPER650_DATA - //! Files where constant tranformation between end-effector and camera frame + //! Files where constant transformation between end-effector and camera frame //! are stored. static const std::string CONST_EMC_MARLIN_F033C_WITHOUT_DISTORTION_FILENAME; static const std::string CONST_EMC_MARLIN_F033C_WITH_DISTORTION_FILENAME; diff --git a/modules/robot/include/visp3/robot/vpViper850.h b/modules/robot/include/visp3/robot/vpViper850.h index d61d6b4d47..0c94fd44c9 100644 --- a/modules/robot/include/visp3/robot/vpViper850.h +++ b/modules/robot/include/visp3/robot/vpViper850.h @@ -101,7 +101,7 @@ class VISP_EXPORT vpViper850 : public vpViper { public: #ifdef VISP_HAVE_VIPER850_DATA - //! Files where constant tranformation between end-effector and camera frame + //! Files where constant transformation between end-effector and camera frame //! are stored. static const std::string CONST_EMC_MARLIN_F033C_WITHOUT_DISTORTION_FILENAME; static const std::string CONST_EMC_MARLIN_F033C_WITH_DISTORTION_FILENAME; diff --git a/modules/robot/src/real-robot/afma4/vpRobotAfma4.cpp b/modules/robot/src/real-robot/afma4/vpRobotAfma4.cpp index 9497e53e5a..f5cc6d3f3f 100644 --- a/modules/robot/src/real-robot/afma4/vpRobotAfma4.cpp +++ b/modules/robot/src/real-robot/afma4/vpRobotAfma4.cpp @@ -1101,7 +1101,7 @@ void vpRobotAfma4::getPosition(const vpRobot::vpControlFrameType frame, vpColVec vpRobot::STATE_VELOCITY_CONTROL) before setVelocity(). \warning Velocities could be saturated if one of them exceed the - maximal autorized speed (see vpRobot::maxTranslationVelocity and + maximal authorized speed (see vpRobot::maxTranslationVelocity and vpRobot::maxRotationVelocity). To change these values use setMaxTranslationVelocity() and setMaxRotationVelocity(). diff --git a/modules/robot/src/real-robot/afma6/vpRobotAfma6.cpp b/modules/robot/src/real-robot/afma6/vpRobotAfma6.cpp index 61d8985d35..24ed3560b5 100644 --- a/modules/robot/src/real-robot/afma6/vpRobotAfma6.cpp +++ b/modules/robot/src/real-robot/afma6/vpRobotAfma6.cpp @@ -1587,7 +1587,7 @@ void vpRobotAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVe vpRobot::STATE_VELOCITY_CONTROL) before setVelocity(). \warning Velocities could be saturated if one of them exceed the - maximal autorized speed (see vpRobot::maxTranslationVelocity and + maximal authorized speed (see vpRobot::maxTranslationVelocity and vpRobot::maxRotationVelocity). To change these values use setMaxTranslationVelocity() and setMaxRotationVelocity(). diff --git a/modules/robot/src/real-robot/biclops/vpBiclops.cpp b/modules/robot/src/real-robot/biclops/vpBiclops.cpp index b259201663..617ed32018 100644 --- a/modules/robot/src/real-robot/biclops/vpBiclops.cpp +++ b/modules/robot/src/real-robot/biclops/vpBiclops.cpp @@ -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. - * -*****************************************************************************/ + */ /* ----------------------------------------------------------------------- */ /* --- INCLUDE ----------------------------------------------------------- */ @@ -55,64 +53,26 @@ const float vpBiclops::tiltJointLimit = (float)(M_PI / 4.5); /*!< Tilt range (in const float vpBiclops::speedLimit = (float)(M_PI / 3.0); /*!< Maximum speed (in rad/s) to perform a displacement */ -/*! - Compute the direct geometric model of the camera: fMc - - \warning Provided for compatibilty with previous versions. Use rather - get_fMc(const vpColVector &, vpHomogeneousMatrix &). - - \param q : Articular position for pan and tilt axis. - - \param fMc : Homogeneous matrix corresponding to the direct geometric model - of the camera. Describes the transformation between the robot reference - frame (called fixed) and the camera frame. - - \sa get_fMc(const vpColVector &, vpHomogeneousMatrix &) -*/ void vpBiclops::computeMGD(const vpColVector &q, vpHomogeneousMatrix &fMc) const { vpHomogeneousMatrix fMe = get_fMe(q); - fMc = fMe * cMe_.inverse(); + fMc = fMe * m_cMe.inverse(); vpCDEBUG(6) << "camera position: " << std::endl << fMc; return; } -/*! - Compute the direct geometric model of the camera: fMc - - \param q : Articular position for pan and tilt axis. - - \param fMc : Homogeneous matrix corresponding to the direct geometric model - of the camera. Describes the transformation between the robot reference - frame (called fixed) and the camera frame. - -*/ void vpBiclops::get_fMc(const vpColVector &q, vpHomogeneousMatrix &fMc) const { vpHomogeneousMatrix fMe = get_fMe(q); - fMc = fMe * cMe_.inverse(); + fMc = fMe * m_cMe.inverse(); vpCDEBUG(6) << "camera position: " << std::endl << fMc; return; } -/*! - Return the direct geometric model of the camera: fMc - - \warning Provided for compatibilty with previous versions. Use rather - get_fMc(const vpColVector &). - - \param q : Articular position for pan and tilt axis. - - \return fMc, the homogeneous matrix corresponding to the direct geometric - model of the camera. Describes the transformation between the robot - reference frame (called fixed) and the camera frame. - - \sa get_fMc(const vpColVector &) -*/ vpHomogeneousMatrix vpBiclops::computeMGD(const vpColVector &q) const { vpHomogeneousMatrix fMc; @@ -122,16 +82,6 @@ vpHomogeneousMatrix vpBiclops::computeMGD(const vpColVector &q) const return fMc; } -/*! - Return the direct geometric model of the camera: fMc - - \param q : Articular position for pan and tilt axis. - - \return fMc, the homogeneous matrix corresponding to the direct geometric - model of the camera. Discribes the transformation between the robot - reference frame (called fixed) and the camera frame. - -*/ vpHomogeneousMatrix vpBiclops::get_fMc(const vpColVector &q) const { vpHomogeneousMatrix fMc; @@ -141,16 +91,6 @@ vpHomogeneousMatrix vpBiclops::get_fMc(const vpColVector &q) const return fMc; } -/*! - Return the direct geometric model of the end effector: fMe - - \param q : Articular position for pan and tilt axis. - - \return fMe, the homogeneous matrix corresponding to the direct geometric - model of the end effector. Describes the transformation between the robot - reference frame (called fixed) and the end effector frame. - -*/ vpHomogeneousMatrix vpBiclops::get_fMe(const vpColVector &q) const { vpHomogeneousMatrix fMe; @@ -168,7 +108,7 @@ vpHomogeneousMatrix vpBiclops::get_fMe(const vpColVector &q) const double c2 = cos(q2); double s2 = sin(q2); - if (dh_model_ == DH1) { + if (m_dh_model == DH1) { fMe[0][0] = -c1 * s2; fMe[0][1] = -s1; fMe[0][2] = c1 * c2; @@ -188,7 +128,8 @@ vpHomogeneousMatrix vpBiclops::get_fMe(const vpColVector &q) const fMe[3][1] = 0; fMe[3][2] = 0; fMe[3][3] = 1; - } else { + } + else { fMe[0][0] = c1 * s2; fMe[0][1] = -s1; fMe[0][2] = c1 * c2; @@ -213,46 +154,22 @@ vpHomogeneousMatrix vpBiclops::get_fMe(const vpColVector &q) const return fMe; } -/*! - Compute the direct geometric model of the camera in terms of pose vector. - - \warning Provided for compatibilty with previous versions. Use rather - get_fMc(const vpColVector &, vpPoseVector &). - - \param q : Articular position for pan and tilt axis. - - \param fvc : Pose vector corresponding to the transformation between the - robot reference frame (called fixed) and the camera frame. - - \sa get_fMc(const vpColVector &, vpPoseVector &) -*/ - -void vpBiclops::computeMGD(const vpColVector &q, vpPoseVector &fvc) const +void vpBiclops::computeMGD(const vpColVector &q, vpPoseVector &fPc) const { vpHomogeneousMatrix fMc; get_fMc(q, fMc); - fvc.buildFrom(fMc.inverse()); + fPc.buildFrom(fMc.inverse()); return; } -/*! - Compute the direct geometric model of the camera in terms of pose vector. - - \param q : Articular position for pan and tilt axis. - - \param fvc : Pose vector corresponding to the transformation between the - robot reference frame (called fixed) and the camera frame. - -*/ - -void vpBiclops::get_fMc(const vpColVector &q, vpPoseVector &fvc) const +void vpBiclops::get_fMc(const vpColVector &q, vpPoseVector &fPc) const { vpHomogeneousMatrix fMc; get_fMc(q, fMc); - fvc.buildFrom(fMc.inverse()); + fPc.buildFrom(fMc.inverse()); return; } @@ -261,24 +178,11 @@ void vpBiclops::get_fMc(const vpColVector &q, vpPoseVector &fvc) const /* --- CONSTRUCTOR ------------------------------------------------------ */ /* ---------------------------------------------------------------------- */ -/*! - - Default construtor. Call init(). - -*/ -vpBiclops::vpBiclops(void) : dh_model_(DH1), cMe_() { init(); } -/* ---------------------------------------------------------------------- */ -/* --- PRIVATE ---------------------------------------------------------- */ -/* ---------------------------------------------------------------------- */ - -/*! - Initialization. - Set the default \f${^c}{\bf M}_e\f$ transformation. +vpBiclops::vpBiclops(void) : m_dh_model(DH1), m_cMe() { init(); } -*/ void vpBiclops::init() { - dh_model_ = DH1; + m_dh_model = DH1; set_cMe(); return; } @@ -290,70 +194,39 @@ void vpBiclops::init() VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpBiclops & /*constant*/) { os << "Geometric parameters: " << std::endl - << "h: " - << "\t" << vpBiclops::h << std::endl; + << "h: " + << "\t" << vpBiclops::h << std::endl; return os; } -/*! +void vpBiclops::get_cVe(vpVelocityTwistMatrix &cVe) const { cVe.buildFrom(m_cMe); } - Get the twist matrix corresponding to the transformation between the - camera frame and the end effector frame. The end effector frame is located - on the tilt axis. - - \param cVe : Twist transformation between camera and end effector frame to - expess a velocity skew from end effector frame in camera frame. - -*/ -void vpBiclops::get_cVe(vpVelocityTwistMatrix &cVe) const { cVe.buildFrom(cMe_); } - -/*! - - Set the default homogeneous matrix corresponding to the transformation - between the camera frame and the end effector frame. The end effector frame - is located on the tilt axis. - -*/ void vpBiclops::set_cMe() { - vpHomogeneousMatrix eMc; - - eMc[0][0] = 0; - eMc[0][1] = -1; - eMc[0][2] = 0; - eMc[0][3] = h; - - eMc[1][0] = 1; - eMc[1][1] = 0; - eMc[1][2] = 0; - eMc[1][3] = 0; - - eMc[2][0] = 0; - eMc[2][1] = 0; - eMc[2][2] = 1; - eMc[2][3] = 0; - - eMc[3][0] = 0; - eMc[3][1] = 0; - eMc[3][2] = 0; - eMc[3][3] = 1; - - cMe_ = eMc.inverse(); + vpHomogeneousMatrix cMe; + + m_cMe[0][0] = 0; + m_cMe[0][1] = 1; + m_cMe[0][2] = 0; + m_cMe[0][3] = 0; + + m_cMe[1][0] = -1; + m_cMe[1][1] = 0; + m_cMe[1][2] = 0; + m_cMe[1][3] = h; + + m_cMe[2][0] = 0; + m_cMe[2][1] = 0; + m_cMe[2][2] = 1; + m_cMe[2][3] = 0; + + m_cMe[3][0] = 0; + m_cMe[3][1] = 0; + m_cMe[3][2] = 0; + m_cMe[3][3] = 1; } -/*! - Get the robot jacobian expressed in the end-effector frame. - - \warning Re is not the embedded camera frame. It corresponds to the frame - associated to the tilt axis (see also get_cMe). - - \param q : Articular position for pan and tilt axis. - - \param eJe : Jacobian between end effector frame and end effector frame (on - tilt axis). - -*/ void vpBiclops::get_eJe(const vpColVector &q, vpMatrix &eJe) const { eJe.resize(6, 2); @@ -368,25 +241,17 @@ void vpBiclops::get_eJe(const vpColVector &q, vpMatrix &eJe) const eJe = 0; - if (dh_model_ == DH1) { + if (m_dh_model == DH1) { eJe[3][0] = -c2; eJe[4][1] = 1; eJe[5][0] = -s2; - } else { + } + else { eJe[3][0] = -c2; eJe[4][1] = -1; eJe[5][0] = s2; } } -/*! - Get the robot jacobian expressed in the robot reference frame - - \param q : Articular position for pan and tilt axis. - - \param fJe : Jacobian between reference frame (or fix frame) and end - effector frame (on tilt axis). - -*/ void vpBiclops::get_fJe(const vpColVector &q, vpMatrix &fJe) const { @@ -402,11 +267,12 @@ void vpBiclops::get_fJe(const vpColVector &q, vpMatrix &fJe) const fJe = 0; - if (dh_model_ == DH1) { + if (m_dh_model == DH1) { fJe[3][1] = -s1; fJe[4][1] = c1; fJe[5][0] = 1; - } else { + } + else { fJe[3][1] = s1; fJe[4][1] = -c1; fJe[5][0] = 1; diff --git a/modules/robot/src/real-robot/biclops/vpRobotBiclops.cpp b/modules/robot/src/real-robot/biclops/vpRobotBiclops.cpp index dbe18ed09d..4e355e6d45 100644 --- a/modules/robot/src/real-robot/biclops/vpRobotBiclops.cpp +++ b/modules/robot/src/real-robot/biclops/vpRobotBiclops.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,18 +29,18 @@ * * Description: * Interface for the Biclops robot. - * -*****************************************************************************/ + */ + +#include + +#ifdef VISP_HAVE_BICLOPS #include // std::fabs #include #include // numeric_limits #include #include - -#include - -#ifdef VISP_HAVE_BICLOPS +#include #include #include @@ -58,127 +57,39 @@ /* --- STATIC ------------------------------------------------------------ */ /* ------------------------------------------------------------------------ */ -bool vpRobotBiclops::robotAlreadyCreated = false; const double vpRobotBiclops::defaultPositioningVelocity = 10.0; -static pthread_mutex_t vpEndThread_mutex; -static pthread_mutex_t vpShm_mutex; -static pthread_mutex_t vpMeasure_mutex; + +static std::mutex m_mutex_end_thread; +static std::mutex m_mutex_shm; +static std::mutex m_mutex_measure; /* ----------------------------------------------------------------------- */ /* --- CONSTRUCTOR ------------------------------------------------------ */ /* ---------------------------------------------------------------------- */ -/*! - - Default constructor. - - Does nothing more than setting the default configuration file - to `/usr/share/BiclopsDefault.cfg`. - - As shown in the following example, the turret need to be initialized - using init() function. - - \code -#include - -int main() -{ -#ifdef VISP_HAVE_BICLOPS - vpRobotBiclops robot; // Use the default config file in -/usr/share/BiclopsDefault.cfg" - - // Specify the config file location - robot.setConfigFile("/usr/share/BiclopsDefault.cfg"); // Not mandatory since the file is the default one - - // Initialize the head - robot.init(); - - // Move the robot to a specified pan and tilt - robot.setRobotState(vpRobot::STATE_POSITION_CONTROL) ; - vpColVector q(2); - q[0] = vpMath::rad(20); // pan - q[1] = vpMath::rad(40); // tilt - robot.setPosition(vpRobot::ARTICULAR_FRAME, q); -#endif - return 0; -} - \endcode - -*/ vpRobotBiclops::vpRobotBiclops() - : vpBiclops(), vpRobot(), control_thread(), controller(), positioningVelocity(defaultPositioningVelocity), - q_previous(), controlThreadCreated(false) + : vpBiclops(), vpRobot(), m_control_thread(), m_controller(), m_positioningVelocity(defaultPositioningVelocity), + m_q_previous() { vpDEBUG_TRACE(12, "Begin default constructor."); - vpRobotBiclops::robotAlreadyCreated = false; setConfigFile("/usr/share/BiclopsDefault.cfg"); - - // Initialize the mutex dedicated to she shm protection - pthread_mutex_init(&vpShm_mutex, NULL); - pthread_mutex_init(&vpEndThread_mutex, NULL); - pthread_mutex_init(&vpMeasure_mutex, NULL); - - control_thread = 0; -} - -/*! - - Constructor that initialize the biclops pan, tilt head by reading the - configuration file provided by Traclabs - and do the homing sequence. - - The following example shows how to use the constructor. - - \code -#include - -int main() -{ -#ifdef VISP_HAVE_BICLOPS - // Specify the config file location and initialize the turret - vpRobotBiclops robot("/usr/share/BiclopsDefault.cfg"); - - // Move the robot to a specified pan and tilt - robot.setRobotState(vpRobot::STATE_POSITION_CONTROL) ; - - vpColVector q(2); - q[0] = vpMath::rad(-20); // pan - q[1] = vpMath::rad(10); // tilt - robot.setPosition(vpRobot::ARTICULAR_FRAME, q); -#endif - return 0; } - \endcode -*/ vpRobotBiclops::vpRobotBiclops(const std::string &filename) - : vpBiclops(), vpRobot(), control_thread(), controller(), positioningVelocity(defaultPositioningVelocity), - q_previous(), controlThreadCreated(false) + : vpBiclops(), vpRobot(), m_control_thread(), m_controller(), m_positioningVelocity(defaultPositioningVelocity), + m_q_previous() { vpDEBUG_TRACE(12, "Begin default constructor."); - vpRobotBiclops::robotAlreadyCreated = false; setConfigFile(filename); - // Initialize the mutex dedicated to she shm protection - pthread_mutex_init(&vpShm_mutex, NULL); - pthread_mutex_init(&vpEndThread_mutex, NULL); - pthread_mutex_init(&vpMeasure_mutex, NULL); - init(); return; } -/*! - - Destructor. - Wait the end of the control thread. - -*/ - vpRobotBiclops::~vpRobotBiclops() { @@ -186,108 +97,60 @@ vpRobotBiclops::~vpRobotBiclops() setRobotState(vpRobot::STATE_STOP); vpDEBUG_TRACE(12, "Unlock mutex vpEndThread_mutex"); - pthread_mutex_unlock(&vpEndThread_mutex); + m_mutex_end_thread.unlock(); /* wait the end of the control thread */ vpDEBUG_TRACE(12, "Wait end of control thread"); - if (controlThreadCreated == true) { - int code = pthread_join(control_thread, NULL); - if (code != 0) { - vpCERROR << "Cannot terminate the control thread: " << code << " strErr=" << strerror(errno) - << " strCode=" << strerror(code) << std::endl; - } + if (m_control_thread.joinable()) { + m_control_thread.join(); } - pthread_mutex_destroy(&vpShm_mutex); - pthread_mutex_destroy(&vpEndThread_mutex); - pthread_mutex_destroy(&vpMeasure_mutex); - - vpRobotBiclops::robotAlreadyCreated = false; - vpDEBUG_TRACE(12, "Stop vpRobotBiclops::~vpRobotBiclops()"); return; } -/* ------------------------------------------------------------------------- - */ -/* --- INITIALISATION ------------------------------------------------------ - */ -/* ------------------------------------------------------------------------- - */ - -/*! - - Set the Biclops config filename. - -*/ -void vpRobotBiclops::setConfigFile(const std::string &filename) { this->configfile = filename; } - -/*! +/* -------------------------------------------------------------------------*/ +/* --- INITIALISATION ------------------------------------------------------*/ +/* -------------------------------------------------------------------------*/ - Set the Biclops config filename. - Check if the config file exists and initialize the head. +void vpRobotBiclops::setConfigFile(const std::string &filename) { this->m_configfile = filename; } - \exception vpRobotException::constructionError If the config file cannot be - oppened. - -*/ void vpRobotBiclops::init() { // test if the config file exists - FILE *fd = fopen(configfile.c_str(), "r"); + FILE *fd = fopen(m_configfile.c_str(), "r"); if (fd == NULL) { - vpCERROR << "Cannot open biclops config file: " << configfile << std::endl; + vpCERROR << "Cannot open biclops config file: " << m_configfile << std::endl; throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with biclops"); } fclose(fd); // Initialize the controller - controller.init(configfile); + m_controller.init(m_configfile); try { setRobotState(vpRobot::STATE_STOP); - } catch (...) { + } + catch (...) { vpERROR_TRACE("Error caught"); throw; } - vpRobotBiclops::robotAlreadyCreated = true; - // Initialize previous articular position to manage getDisplacement() - q_previous.resize(vpBiclops::ndof); - q_previous = 0; - - controlThreadCreated = false; + m_q_previous.resize(vpBiclops::ndof); + m_q_previous = 0; return; } -/* - Control loop to manage the biclops joint limits in speed control. - - This control loop is running in a separate thread in order to detect each 5 - ms joint limits during the speed control. If a joint limit is detected the - axis should be halted. - - \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, - Traclabs put soft limits in that keep any command from driving to a position - too close to the hard limits. In velocity mode this protection does not - exist in the current API. - - \warning With the understanding that hitting the hard limits at full - speed/power can damage the unit, damage due to velocity mode commanding is - under user responsibility. -*/ void *vpRobotBiclops::vpRobotBiclopsSpeedControlLoop(void *arg) { - vpRobotBiclopsController *controller = static_cast(arg); + vpRobotBiclopsController *m_controller = static_cast(arg); int iter = 0; - // PMDAxisControl *panAxis = controller->getPanAxis(); - // PMDAxisControl *tiltAxis = controller->getTiltAxis(); + // PMDAxisControl *m_panAxis = m_controller->getPanAxis(); + // PMDAxisControl *m_tiltAxis = m_controller->getTiltAxis(); vpRobotBiclopsController::shmType shm; vpDEBUG_TRACE(10, "Start control loop"); @@ -307,14 +170,14 @@ void *vpRobotBiclops::vpRobotBiclopsSpeedControlLoop(void *arg) softLimit[1] = vpBiclops::tiltJointLimit - secure; vpDEBUG_TRACE(12, "soft limit pan: %f tilt: %f", vpMath::deg(softLimit[0]), vpMath::deg(softLimit[1])); - // Initilisation + // Initialization vpDEBUG_TRACE(11, "Lock mutex vpShm_mutex"); - pthread_mutex_lock(&vpShm_mutex); + m_mutex_shm.lock(); - shm = controller->readShm(); + shm = m_controller->readShm(); vpDEBUG_TRACE(11, "unlock mutex vpShm_mutex"); - pthread_mutex_unlock(&vpShm_mutex); + m_mutex_shm.unlock(); for (unsigned int i = 0; i < vpBiclops::ndof; i++) { prev_q_dot[i] = shm.q_dot[i]; @@ -325,37 +188,37 @@ void *vpRobotBiclops::vpRobotBiclopsSpeedControlLoop(void *arg) } // Initialize actual position and velocity - mes_q = controller->getActualPosition(); - mes_q_dot = controller->getActualVelocity(); + mes_q = m_controller->getActualPosition(); + mes_q_dot = m_controller->getActualVelocity(); vpDEBUG_TRACE(11, "Lock mutex vpShm_mutex"); - pthread_mutex_lock(&vpShm_mutex); + m_mutex_shm.lock(); - shm = controller->readShm(); + shm = m_controller->readShm(); // Updates the shm for (unsigned int i = 0; i < vpBiclops::ndof; i++) { shm.actual_q[i] = mes_q[i]; shm.actual_q_dot[i] = mes_q_dot[i]; } - // Update the actuals positions - controller->writeShm(shm); + // Update current positions + m_controller->writeShm(shm); vpDEBUG_TRACE(11, "unlock mutex vpShm_mutex"); - pthread_mutex_unlock(&vpShm_mutex); + m_mutex_shm.unlock(); vpDEBUG_TRACE(11, "unlock mutex vpMeasure_mutex"); - pthread_mutex_unlock(&vpMeasure_mutex); // A position is available + m_mutex_measure.unlock(); // A position is available - while (!controller->isStopRequested()) { + while (!m_controller->isStopRequested()) { // Get actual position and velocity - mes_q = controller->getActualPosition(); - mes_q_dot = controller->getActualVelocity(); + mes_q = m_controller->getActualPosition(); + mes_q_dot = m_controller->getActualVelocity(); vpDEBUG_TRACE(11, "Lock mutex vpShm_mutex"); - pthread_mutex_lock(&vpShm_mutex); + m_mutex_shm.lock(); - shm = controller->readShm(); + shm = m_controller->readShm(); // Updates the shm for (unsigned int i = 0; i < vpBiclops::ndof; i++) { @@ -374,11 +237,13 @@ void *vpRobotBiclops::vpRobotBiclopsSpeedControlLoop(void *arg) vpDEBUG_TRACE(12, "Axe %d in low joint limit", i); shm.status[i] = vpRobotBiclopsController::STOP; shm.jointLimit[i] = true; - } else if (mes_q[i] > softLimit[i]) { + } + else if (mes_q[i] > softLimit[i]) { vpDEBUG_TRACE(12, "Axe %d in hight joint limit", i); shm.status[i] = vpRobotBiclopsController::STOP; shm.jointLimit[i] = true; - } else { + } + else { shm.status[i] = vpRobotBiclopsController::SPEED; shm.jointLimit[i] = false; } @@ -422,7 +287,8 @@ void *vpRobotBiclops::vpRobotBiclopsSpeedControlLoop(void *arg) force_halt[i] = true; // indicate that it will be stopped updateVelocity = true; // We have to send this new speed } - } else { + } + else { // We have to apply the desired speed to go away the joint // Update the desired speed q_dot[i] = shm.q_dot[i]; @@ -430,7 +296,8 @@ void *vpRobotBiclops::vpRobotBiclopsSpeedControlLoop(void *arg) force_halt[i] = false; updateVelocity = true; // We have to send this new speed } - } else { + } + else { // New desired speed and change of direction. if (enable_limit[i] == true) { // limit detection active // Update the desired speed to go away the joint limit @@ -439,7 +306,8 @@ void *vpRobotBiclops::vpRobotBiclopsSpeedControlLoop(void *arg) force_halt[i] = false; enable_limit[i] = false; // Disable joint limit detection updateVelocity = true; // We have to send this new speed - } else { + } + else { // We have to stop this axis // Test if this axis was stopped before if (force_halt[i] == false) { @@ -450,7 +318,8 @@ void *vpRobotBiclops::vpRobotBiclopsSpeedControlLoop(void *arg) } } } - } else { + } + else { // Axis not in joint limit // Update the desired speed @@ -459,7 +328,8 @@ void *vpRobotBiclops::vpRobotBiclopsSpeedControlLoop(void *arg) enable_limit[i] = true; // Joint limit detection must be active updateVelocity = true; // We have to send this new speed } - } else { + } + else { // No change of the desired speed. We have to stop the robot in case // of joint limit if (shm.status[i] == vpRobotBiclopsController::STOP) { // axis limit @@ -473,23 +343,24 @@ void *vpRobotBiclops::vpRobotBiclopsSpeedControlLoop(void *arg) updateVelocity = true; // We have to send this new speed } } - } else { + } + else { // No need to stop the robot enable_limit[i] = true; // Normal situation, activate limit detection } } } - // Update the actuals positions - controller->writeShm(shm); + // Update the actual positions + m_controller->writeShm(shm); vpDEBUG_TRACE(11, "unlock mutex vpShm_mutex"); - pthread_mutex_unlock(&vpShm_mutex); + m_mutex_shm.unlock(); if (updateVelocity) { vpDEBUG_TRACE(12, "apply q_dot : %f %f", vpMath::deg(q_dot[0]), vpMath::deg(q_dot[1])); // Apply the velocity - controller->setVelocity(q_dot); + m_controller->setVelocity(q_dot); } // Update the previous speed for next iteration @@ -501,43 +372,24 @@ void *vpRobotBiclops::vpRobotBiclopsSpeedControlLoop(void *arg) // wait 5 ms vpTime::wait(5.0); - // if (pthread_mutex_trylock(&vpEndThread_mutex) == 0) { - // vpDEBUG_TRACE (12, "Calling thread will end"); - // vpDEBUG_TRACE (12, "Unlock mutex vpEndThread_mutex"); - // std::cout << "Calling thread will end" << std::endl; - // std::cout << "Unlock mutex vpEndThread_mutex" << std::endl; - // - // pthread_mutex_unlock(&vpEndThread_mutex); - // break; - // } - iter++; } - controller->stopRequest(false); + m_controller->stopRequest(false); // Stop the robot vpDEBUG_TRACE(10, "End of the control thread: stop the robot"); q_dot = 0; - controller->setVelocity(q_dot); + m_controller->setVelocity(q_dot); delete[] new_q_dot; delete[] change_dir; delete[] force_halt; delete[] enable_limit; vpDEBUG_TRACE(11, "unlock vpEndThread_mutex"); - pthread_mutex_unlock(&vpEndThread_mutex); - - vpDEBUG_TRACE(10, "Exit control thread "); - // pthread_exit(0); + m_mutex_end_thread.unlock(); return NULL; } -/*! - - Change the state of the robot either to stop them, or to set position or - speed control. - -*/ vpRobot::vpRobotStateType vpRobotBiclops::setRobotState(vpRobot::vpRobotStateType newState) { switch (newState) { @@ -559,17 +411,11 @@ vpRobot::vpRobotStateType vpRobotBiclops::setRobotState(vpRobot::vpRobotStateTyp if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState()) { vpDEBUG_TRACE(12, "Lock mutex vpEndThread_mutex"); - pthread_mutex_lock(&vpEndThread_mutex); + m_mutex_end_thread.lock(); vpDEBUG_TRACE(12, "Create speed control thread"); - int code; - code = pthread_create(&control_thread, NULL, &vpRobotBiclops::vpRobotBiclopsSpeedControlLoop, &controller); - if (code != 0) { - vpCERROR << "Cannot create speed biclops control thread: " << code << " strErr=" << strerror(errno) - << " strCode=" << strerror(code) << std::endl; - } - - controlThreadCreated = true; + m_control_thread = std::thread(&vpRobotBiclops::vpRobotBiclopsSpeedControlLoop, &m_controller); + vpTime::wait(100.0); vpDEBUG_TRACE(12, "Speed control thread created"); } @@ -582,31 +428,16 @@ vpRobot::vpRobotStateType vpRobotBiclops::setRobotState(vpRobot::vpRobotStateTyp return vpRobot::setRobotState(newState); } -/*! - - Halt all the axis. - -*/ void vpRobotBiclops::stopMotion(void) { vpColVector q_dot(vpBiclops::ndof); q_dot = 0; - controller.setVelocity(q_dot); + m_controller.setVelocity(q_dot); // std::cout << "Request to stop the velocity controller thread...."<< // std::endl; - controller.stopRequest(true); + m_controller.stopRequest(true); } -/*! - - Get the twist matrix corresponding to the transformation between the - camera frame and the end effector frame. The end effector frame is located - on the tilt axis. - - \param cVe : Twist transformation between camera and end effector frame to - expess a velocity skew from end effector frame in camera frame. - -*/ void vpRobotBiclops::get_cVe(vpVelocityTwistMatrix &cVe) const { vpHomogeneousMatrix cMe; @@ -615,67 +446,24 @@ void vpRobotBiclops::get_cVe(vpVelocityTwistMatrix &cVe) const cVe.buildFrom(cMe); } -/*! - - Get the homogeneous matrix corresponding to the transformation between the - camera frame and the end effector frame. The end effector frame is located - on the tilt axis. - - \param cMe : Homogeneous matrix between camera and end effector frame. - -*/ void vpRobotBiclops::get_cMe(vpHomogeneousMatrix &cMe) const { cMe = vpBiclops::get_cMe(); } -/*! - Get the robot jacobian expressed in the end-effector frame. - - \warning Re is not the embedded camera frame. It corresponds to the frame - associated to the tilt axis (see also get_cMe). - - \param _eJe : Jacobian between end effector frame and end effector frame (on - tilt axis). - -*/ -void vpRobotBiclops::get_eJe(vpMatrix &_eJe) +void vpRobotBiclops::get_eJe(vpMatrix &eJe) { vpColVector q(2); getPosition(vpRobot::ARTICULAR_FRAME, q); - try { - vpBiclops::get_eJe(q, _eJe); - } catch (...) { - vpERROR_TRACE("catch exception "); - throw; - } + vpBiclops::get_eJe(q, eJe); } -/*! - Get the robot jacobian expressed in the robot reference frame - - \param _fJe : Jacobian between reference frame (or fix frame) and end - effector frame (on tilt axis). - -*/ -void vpRobotBiclops::get_fJe(vpMatrix &_fJe) +void vpRobotBiclops::get_fJe(vpMatrix &fJe) { vpColVector q(2); getPosition(vpRobot::ARTICULAR_FRAME, q); - try { - vpBiclops::get_fJe(q, _fJe); - } catch (...) { - vpERROR_TRACE("Error caught"); - throw; - } + vpBiclops::get_fJe(q, fJe); } -/*! - - Set the velocity used for a position control. - - \param velocity : Velocity in % of the maximum velocity between [0,100]. The - maximum velocity is given vpBiclops::speedLimit. -*/ void vpRobotBiclops::setPositioningVelocity(double velocity) { if (velocity < 0 || velocity > 100) { @@ -683,32 +471,11 @@ void vpRobotBiclops::setPositioningVelocity(double velocity) throw vpRobotException(vpRobotException::constructionError, "Bad positioning velocity"); } - positioningVelocity = velocity; + m_positioningVelocity = velocity; } -/*! - Get the velocity in % used for a position control. - - \return Positioning velocity in [0, 100.0]. The - maximum positioning velocity is given vpBiclops::speedLimit. - -*/ -double vpRobotBiclops::getPositioningVelocity(void) { return positioningVelocity; } -/*! - Move the robot in position control. +double vpRobotBiclops::getPositioningVelocity(void) { return m_positioningVelocity; } - \warning This method is blocking. That mean that it waits the end of the - positioning. - - \param frame : Control frame. This biclops head can only be controlled in - articular. - - \param q : The position to set for each axis in radians. - - \exception vpRobotException::wrongStateError : If a not supported frame - type is given. - -*/ void vpRobotBiclops::setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q) { @@ -740,29 +507,13 @@ void vpRobotBiclops::setPosition(const vpRobot::vpControlFrameType frame, const } vpDEBUG_TRACE(12, "Lock mutex vpEndThread_mutex"); - pthread_mutex_lock(&vpEndThread_mutex); - controller.setPosition(q, positioningVelocity); + m_mutex_end_thread.lock(); + m_controller.setPosition(q, m_positioningVelocity); vpDEBUG_TRACE(12, "Unlock mutex vpEndThread_mutex"); - pthread_mutex_unlock(&vpEndThread_mutex); + m_mutex_end_thread.unlock(); return; } -/*! - Move the robot in position control. - - \warning This method is blocking. That mean that it wait the end of the - positioning. - - \param frame : Control frame. This biclops head can only be controlled in - articular. - - \param q1 : The pan position to set in radians. - \param q2 : The tilt position to set in radians. - - \exception vpRobotException::wrongStateError : If a not supported frame - type is given. - -*/ void vpRobotBiclops::setPosition(const vpRobot::vpControlFrameType frame, const double &q1, const double &q2) { try { @@ -771,50 +522,23 @@ void vpRobotBiclops::setPosition(const vpRobot::vpControlFrameType frame, const q[1] = q2; setPosition(frame, q); - } catch (...) { + } + catch (...) { vpERROR_TRACE("Error caught"); throw; } } -/*! - - Read the content of the position file and moves to head to articular - position. - - \param filename : Position filename - - \exception vpRobotException::readingParametersError : If the articular - position cannot be read from file. - - \sa readPositionFile() - -*/ -void vpRobotBiclops::setPosition(const char *filename) +void vpRobotBiclops::setPosition(const std::string &filename) { vpColVector q; if (readPositionFile(filename, q) == false) { vpERROR_TRACE("Cannot get biclops position from file"); - throw vpRobotException(vpRobotException::readingParametersError, "Cannot get biclops position from file"); + throw vpRobotException(vpRobotException::readingParametersError, "Cannot get Biclops position from file"); } setPosition(vpRobot::ARTICULAR_FRAME, q); } -/*! - - Return the position of each axis. - - In positioning control mode, call vpRobotBiclopsController::getPosition() - - In speed control mode, call vpRobotBiclopsController::getActualPosition() - - \param frame : Control frame. This biclops head can only be controlled in - articular. - - \param q : The position of the axis in radians. - - \exception vpRobotException::wrongStateError : If a not supported frame type - is given. - -*/ void vpRobotBiclops::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q) { switch (frame) { @@ -844,7 +568,7 @@ void vpRobotBiclops::getPosition(const vpRobot::vpControlFrameType frame, vpColV switch (state) { case STATE_STOP: case STATE_POSITION_CONTROL: - q = controller.getPosition(); + q = m_controller.getPosition(); break; case STATE_VELOCITY_CONTROL: @@ -853,17 +577,17 @@ void vpRobotBiclops::getPosition(const vpRobot::vpControlFrameType frame, vpColV q.resize(vpBiclops::ndof); vpDEBUG_TRACE(12, "Lock mutex vpMeasure_mutex"); - pthread_mutex_lock(&vpMeasure_mutex); // Wait until a position is available + m_mutex_measure.lock(); // Wait until a position is available vpRobotBiclopsController::shmType shm; vpDEBUG_TRACE(12, "Lock mutex vpShm_mutex"); - pthread_mutex_lock(&vpShm_mutex); + m_mutex_shm.lock(); - shm = controller.readShm(); + shm = m_controller.readShm(); vpDEBUG_TRACE(12, "unlock mutex vpShm_mutex"); - pthread_mutex_unlock(&vpShm_mutex); + m_mutex_shm.unlock(); for (unsigned int i = 0; i < vpBiclops::ndof; i++) { q[i] = shm.actual_q[i]; @@ -872,38 +596,12 @@ void vpRobotBiclops::getPosition(const vpRobot::vpControlFrameType frame, vpColV vpCDEBUG(11) << "++++++++ Measure actuals: " << q.t(); vpDEBUG_TRACE(12, "unlock mutex vpMeasure_mutex"); - pthread_mutex_unlock(&vpMeasure_mutex); // A position is available + m_mutex_measure.unlock(); // A position is available break; } } -/*! - - Send a velocity on each axis. - - \param frame : Control frame. This biclops head can only be controlled in - articular. Be aware, the camera frame (vpRobot::CAMERA_FRAME), the reference - frame (vpRobot::REFERENCE_FRAME), end-effector frame (vpRobot::END_EFFECTOR_FRAME) - and the mixt frame (vpRobot::MIXT_FRAME) are not implemented. - - \param q_dot : The desired articular velocity of the axis in rad/s. \f$ \dot - {r} = [\dot{q}_1, \dot{q}_2]^t \f$ with \f$ \dot{q}_1 \f$ the pan of the - camera and \f$ \dot{q}_2\f$ the tilt of the camera. - - \exception vpRobotException::wrongStateError : If a the robot is not - configured to handle a velocity. The robot can handle a velocity only if the - velocity control mode is set. For that, call setRobotState( - vpRobot::STATE_VELOCITY_CONTROL) before setVelocity(). - - \exception vpRobotException::wrongStateError : If a not supported frame type - (vpRobot::CAMERA_FRAME, vpRobot::REFERENCE_FRAME, vpRobot::END_EFFECTOR_FRAME - or vpRobot::MIXT_FRAME) is given. - - \warning Velocities could be saturated if one of them exceed the maximal - autorized speed (see vpRobot::maxRotationVelocity). - -*/ void vpRobotBiclops::setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &q_dot) { if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState()) { @@ -982,39 +680,25 @@ void vpRobotBiclops::setVelocity(const vpRobot::vpControlFrameType frame, const vpRobotBiclopsController::shmType shm; vpDEBUG_TRACE(12, "Lock mutex vpShm_mutex"); - pthread_mutex_lock(&vpShm_mutex); + m_mutex_shm.lock(); - shm = controller.readShm(); + shm = m_controller.readShm(); for (unsigned int i = 0; i < vpBiclops::ndof; i++) shm.q_dot[i] = q_dot[i]; - controller.writeShm(shm); + m_controller.writeShm(shm); vpDEBUG_TRACE(12, "unlock mutex vpShm_mutex"); - pthread_mutex_unlock(&vpShm_mutex); + m_mutex_shm.unlock(); return; } -/* ------------------------------------------------------------------------- - */ -/* --- GET ----------------------------------------------------------------- - */ -/* ------------------------------------------------------------------------- - */ - -/*! - - Get the articular velocity. - - \param frame : Control frame. This head can only be controlled in articular. +/* ------------------------------------------------------------------------- */ +/* --- GET ----------------------------------------------------------------- */ +/* ------------------------------------------------------------------------- */ - \param q_dot : The measured articular velocity in rad/s. - - \exception vpRobotException::wrongStateError : If a not supported frame type - is given. -*/ void vpRobotBiclops::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q_dot) { switch (frame) { @@ -1044,7 +728,7 @@ void vpRobotBiclops::getVelocity(const vpRobot::vpControlFrameType frame, vpColV switch (state) { case STATE_STOP: case STATE_POSITION_CONTROL: - q_dot = controller.getVelocity(); + q_dot = m_controller.getVelocity(); break; case STATE_VELOCITY_CONTROL: @@ -1053,17 +737,17 @@ void vpRobotBiclops::getVelocity(const vpRobot::vpControlFrameType frame, vpColV q_dot.resize(vpBiclops::ndof); vpDEBUG_TRACE(12, "Lock mutex vpMeasure_mutex"); - pthread_mutex_lock(&vpMeasure_mutex); // Wait until a position is available + m_mutex_measure.lock(); // Wait until a position is available vpRobotBiclopsController::shmType shm; vpDEBUG_TRACE(12, "Lock mutex vpShm_mutex"); - pthread_mutex_lock(&vpShm_mutex); + m_mutex_shm.lock(); - shm = controller.readShm(); + shm = m_controller.readShm(); vpDEBUG_TRACE(12, "unlock mutex vpShm_mutex"); - pthread_mutex_unlock(&vpShm_mutex); + m_mutex_shm.unlock(); for (unsigned int i = 0; i < vpBiclops::ndof; i++) { q_dot[i] = shm.actual_q_dot[i]; @@ -1072,23 +756,12 @@ void vpRobotBiclops::getVelocity(const vpRobot::vpControlFrameType frame, vpColV vpCDEBUG(11) << "++++++++ Velocity actuals: " << q_dot.t(); vpDEBUG_TRACE(12, "unlock mutex vpMeasure_mutex"); - pthread_mutex_unlock(&vpMeasure_mutex); // A position is available + m_mutex_measure.unlock(); // A position is available break; } } -/*! - - Return the articular velocity. - - \param frame : Control frame. This head can only be controlled in articular. - - \return The measured articular velocity in rad/s. - - \exception vpRobotException::wrongStateError : If a not supported frame type - is given. -*/ vpColVector vpRobotBiclops::getVelocity(vpRobot::vpControlFrameType frame) { vpColVector q_dot; @@ -1097,25 +770,6 @@ vpColVector vpRobotBiclops::getVelocity(vpRobot::vpControlFrameType frame) return q_dot; } -/*! - - Get an articular position from the position file. - - \param filename : Position file. - - \param q : The articular position read in the file. - - \code - # Example of biclops position file - # The axis positions must be preceed by R: - # First value : pan articular position in degrees - # Second value: tilt articular position in degrees - R: 15.0 5.0 - \endcode - - \return true if a position was found, false otherwise. - -*/ bool vpRobotBiclops::readPositionFile(const std::string &filename, vpColVector &q) { std::ifstream fd(filename.c_str(), std::ios::in); @@ -1174,28 +828,6 @@ bool vpRobotBiclops::readPositionFile(const std::string &filename, vpColVector & return true; } -/*! - - Get the robot displacement since the last call of this method. - - \warning The first call of this method gives not a good value for the - displacement. - - \param frame The frame in which the measured displacement is expressed. - - \param d The displacement: - - - In articular, the dimension of q is 2 (the number of axis of the robot) - with respectively d[0] (pan displacement), d[1] (tilt displacement). - - - In camera frame, the dimension of d is 6 (tx, ty, ty, tux, tuy, tuz). - Translations are expressed in meters, rotations in radians with the theta U - representation. - - \exception vpRobotException::wrongStateError If a not supported frame type - is given. - -*/ void vpRobotBiclops::getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &d) { vpColVector q_current; // current position @@ -1205,7 +837,7 @@ void vpRobotBiclops::getDisplacement(vpRobot::vpControlFrameType frame, vpColVec switch (frame) { case vpRobot::ARTICULAR_FRAME: d.resize(vpBiclops::ndof); - d = q_current - q_previous; + d = q_current - m_q_previous; break; case vpRobot::CAMERA_FRAME: { @@ -1213,7 +845,7 @@ void vpRobotBiclops::getDisplacement(vpRobot::vpControlFrameType frame, vpColVec vpHomogeneousMatrix fMc_current; vpHomogeneousMatrix fMc_previous; fMc_current = vpBiclops::get_fMc(q_current); - fMc_previous = vpBiclops::get_fMc(q_previous); + fMc_previous = vpBiclops::get_fMc(m_q_previous); vpHomogeneousMatrix c_previousMc_current; // fMc_c = fMc_p * c_pMc_c // => c_pMc_c = (fMc_p)^-1 * fMc_c @@ -1238,11 +870,11 @@ void vpRobotBiclops::getDisplacement(vpRobot::vpControlFrameType frame, vpColVec break; } - q_previous = q_current; // Update for next call of this method + m_q_previous = q_current; // Update for next call of this method } #elif !defined(VISP_BUILD_SHARED_LIBS) // Work around to avoid warning: libvisp_robot.a(vpRobotBiclops.cpp.o) has no // symbols -void dummy_vpRobotBiclops(){}; +void dummy_vpRobotBiclops() { }; #endif diff --git a/modules/robot/src/real-robot/biclops/vpRobotBiclopsController.cpp b/modules/robot/src/real-robot/biclops/vpRobotBiclopsController.cpp index 47690bab1e..b0b2fcf36b 100644 --- a/modules/robot/src/real-robot/biclops/vpRobotBiclopsController.cpp +++ b/modules/robot/src/real-robot/biclops/vpRobotBiclopsController.cpp @@ -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. - * -*****************************************************************************/ + */ #include #include @@ -57,96 +55,77 @@ /* --- CONSTRUCTOR ------------------------------------------------------ */ /* ---------------------------------------------------------------------- */ -/*! - Default constructor. -*/ vpRobotBiclopsController::vpRobotBiclopsController() - : biclops(), axisMask(0), panAxis(NULL), tiltAxis(NULL), vergeAxis(NULL), panProfile(), tiltProfile(), vergeProfile(), - shm(), stopControllerThread_(false) + : m_biclops(), m_axisMask(0), m_panAxis(NULL), m_tiltAxis(NULL), m_vergeAxis(NULL), m_panProfile(), m_tiltProfile(), + m_vergeProfile(), m_shm(), m_stopControllerThread(false) { - axisMask = Biclops::PanMask + Biclops::TiltMask - /*+ Biclops::VergeMask*/; // add this if you want verge. + m_axisMask = Biclops::PanMask + Biclops::TiltMask + /*+ Biclops::VergeMask*/; // add this if you want verge. // Set Debug level depending on how much info you want to see about // the inner workings of the API. Level 2 is highest with 0 being // the default (i.e., no messages). - biclops.SetDebugLevel(0); + m_biclops.SetDebugLevel(0); // initialize the shared data structure for (unsigned int i = 0; i < vpBiclops::ndof; i++) { - shm.status[i] = STOP; - shm.q_dot[i] = 0.; - shm.actual_q[i] = 0.; - shm.jointLimit[i] = false; - shm.status[i] = STOP; + m_shm.status[i] = STOP; + m_shm.q_dot[i] = 0.; + m_shm.actual_q[i] = 0.; + m_shm.jointLimit[i] = false; + m_shm.status[i] = STOP; } } -/*! - - Destructor. - -*/ -vpRobotBiclopsController::~vpRobotBiclopsController() {} - -/*! +vpRobotBiclopsController::~vpRobotBiclopsController() { } - Initialize the biclops by homing all axis. - - \param configfile : Biclops configuration file. - - \exception vpRobotException::notInitializedError If the biclops head connot - be initialized. The initialization can failed, - - if the head is not powered on, - - if the head is not connected to your computer throw a serial cable, - - if you try to open a bad serial port. Check you config file to verify - which is the used serial port. -*/ void vpRobotBiclopsController::init(const std::string &configfile) { - vpDEBUG_TRACE(12, "Initialize biclops."); + vpDEBUG_TRACE(12, "Initialize Biclops."); bool binit = false; for (int i = 0; i < 1; i++) { try { - std::cout << "Try to initialize biclops head " << std::endl; - binit = biclops.Initialize(configfile.c_str()); + std::cout << "Try to initialize Biclops head " << std::endl; + binit = m_biclops.Initialize(configfile.c_str()); usleep(100000); if (binit) { // Initialization completed successfully. Close the config file. std::cout << "Initialization succeed...\n"; break; - } else { + } + else { std::cout << "Initialization failed...\n"; } - } catch (...) { + } + catch (...) { std::cout << "Initialization failed..." << std::endl; } } if (!binit) { - std::cout << "Cannot initialize biclops head. " << std::endl; + std::cout << "Cannot initialize Biclops head. " << std::endl; std::cout << "Check if the serial cable is connected." << std::endl; std::cout << "Check if the robot is powered on." << std::endl; std::cout << "Check if you try to open the good serial port." << std::endl; std::cout << "Try to power off/on and restart..." << std::endl; - throw vpRobotException(vpRobotException::notInitializedError, "Cannot initialize biclops head."); + throw vpRobotException(vpRobotException::notInitializedError, "Cannot initialize Biclops head."); } vpDEBUG_TRACE(12, "Biclops initialized"); // Get shortcut references to each axis. - panAxis = biclops.GetAxis(Biclops::Pan); - tiltAxis = biclops.GetAxis(Biclops::Tilt); - if ((axisMask & Biclops::VergeMask) != 0) - vergeAxis = biclops.GetAxis(Biclops::Verge); + m_panAxis = m_biclops.GetAxis(Biclops::Pan); + m_tiltAxis = m_biclops.GetAxis(Biclops::Tilt); + if ((m_axisMask & Biclops::VergeMask) != 0) + m_vergeAxis = m_biclops.GetAxis(Biclops::Verge); #ifdef VISP_HAVE_BICLOPS_AND_GET_HOMED_STATE_FUNCTION // new API - if (!panAxis->GetHomedState() || !tiltAxis->GetHomedState()) { + if (!m_panAxis->GetHomedState() || !m_tiltAxis->GetHomedState()) { vpDEBUG_TRACE(12, "Biclops is not homed"); } #else // old API - if (!panAxis->IsAlreadyHomed() || !tiltAxis->IsAlreadyHomed()) { + if (!m_panAxis->IsAlreadyHomed() || !m_tiltAxis->IsAlreadyHomed()) { vpDEBUG_TRACE(12, "Biclops is not homed"); } #endif @@ -154,36 +133,21 @@ void vpRobotBiclopsController::init(const std::string &configfile) // Execute the homing sequence for all axes. vpDEBUG_TRACE(12, "Execute the homing sequence for all axes"); vpDEBUG_TRACE(12, "Execute the homing sequence for all axes"); - if (biclops.HomeAxes(axisMask)) + if (m_biclops.HomeAxes(m_axisMask)) vpDEBUG_TRACE(12, "Homing sequence succeed."); else { vpERROR_TRACE("Homing sequence failed. Program is stopped"); - throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with biclops"); + throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with Biclops"); } // Get the currently defined (default) motion profiles. - // PMDAxisControl::Profile panProfile,tiltProfile,vergeProfile; - panAxis->GetProfile(panProfile); - tiltAxis->GetProfile(tiltProfile); - if ((axisMask & Biclops::VergeMask) != 0) - vergeAxis->GetProfile(vergeProfile); + // PMDAxisControl::Profile m_panProfile,m_tiltProfile,m_vergeProfile; + m_panAxis->GetProfile(m_panProfile); + m_tiltAxis->GetProfile(m_tiltProfile); + if ((m_axisMask & Biclops::VergeMask) != 0) + m_vergeAxis->GetProfile(m_vergeProfile); } -/*! - - Set the biclops axis position. The motion of the axis is synchronized to end - on the same time. - - \warning Wait the end of the positioning. - - \param q : The position to set for each axis. - - \param percentVelocity : The velocity displacement to reach the new position - in the range [0: 100.0]. 100 % corresponds to the maximal admissible - speed. The maximal admissible speed is given by vpBiclops::speedLimit. - -*/ - void vpRobotBiclopsController::setPosition(const vpColVector &q, double percentVelocity) { if (q.getRows() != vpBiclops::ndof) { @@ -191,32 +155,32 @@ void vpRobotBiclopsController::setPosition(const vpColVector &q, double percentV throw vpRobotException(vpRobotException::lowLevelError, "Bad dimension for positioning vector."); } - panAxis->SetProfileMode(PMDTrapezoidalProfile); - tiltAxis->SetProfileMode(PMDTrapezoidalProfile); + m_panAxis->SetProfileMode(PMDTrapezoidalProfile); + m_tiltAxis->SetProfileMode(PMDTrapezoidalProfile); // Create the list of axes we want to coordinate PMDUtils::AxisList axisList; - axisList.push_back(panAxis); - axisList.push_back(tiltAxis); + axisList.push_back(m_panAxis); + axisList.push_back(m_tiltAxis); #ifdef VISP_HAVE_BICLOPS_AND_GET_HOMED_STATE_FUNCTION // new API // Get the currently defined (default) motion profiles. - // PMDAxisControl::Profile panProfile,tiltProfile; - panAxis->GetProfile(panProfile); - tiltAxis->GetProfile(tiltProfile); + // PMDAxisControl::Profile m_panProfile,m_tiltProfile; + m_panAxis->GetProfile(m_panProfile); + m_tiltAxis->GetProfile(m_tiltProfile); // Set a position to move to by modifying the respective profiles. // NOTE: profile values are in revolutions, so here we convert // from degrees (divide by 360) for readability. - panProfile.pos = PMDUtils::RadsToRevs(q[0]); - panProfile.vel = PMDUtils::RadsToRevs(vpBiclops::speedLimit * percentVelocity / 100.); + m_panProfile.pos = PMDUtils::RadsToRevs(q[0]); + m_panProfile.vel = PMDUtils::RadsToRevs(vpBiclops::speedLimit * percentVelocity / 100.); - tiltProfile.pos = PMDUtils::RadsToRevs(q[1]); - tiltProfile.vel = PMDUtils::RadsToRevs(vpBiclops::speedLimit * percentVelocity / 100.); + m_tiltProfile.pos = PMDUtils::RadsToRevs(q[1]); + m_tiltProfile.vel = PMDUtils::RadsToRevs(vpBiclops::speedLimit * percentVelocity / 100.); // Inform the controller of the new desired position. - panAxis->SetProfile(panProfile); - tiltAxis->SetProfile(tiltProfile); + m_panAxis->SetProfile(m_panProfile); + m_tiltAxis->SetProfile(m_tiltProfile); #else // old API @@ -225,44 +189,35 @@ void vpRobotBiclopsController::setPosition(const vpColVector &q, double percentV // Set a position to move to by modifying the respective profiles. // NOTE: profile values are in revolutions, so here we convert // from degrees (divide by 360) for readability. - panProfile.pos = PMDUtils::RadsToRevs(q[0]); - panProfile.vel = PMDUtils::RadsToRevs(vpBiclops::speedLimit * percentVelocity / 100.); + m_panProfile.pos = PMDUtils::RadsToRevs(q[0]); + m_panProfile.vel = PMDUtils::RadsToRevs(vpBiclops::speedLimit * percentVelocity / 100.); vpDEBUG_TRACE(12, "Speed percent: %lf", vpBiclops::speedLimit * percentVelocity / 100.); - panAxis->ProfileToCounts(panProfile, desired_profile); + m_panAxis->ProfileToCounts(m_panProfile, desired_profile); vpCDEBUG(12) << "desired_profile.pos: " << desired_profile.pos << std::endl; vpCDEBUG(12) << "desired_profile.vel: " << desired_profile.vel << std::endl; - panAxis->SetProfile(desired_profile); + m_panAxis->SetProfile(desired_profile); // Set a position to move to by modifying the respective profiles. // NOTE: profile values are in revolutions, so here we convert // from degrees (divide by 360) for readability. - tiltProfile.pos = PMDUtils::RadsToRevs(q[1]); - tiltProfile.vel = PMDUtils::RadsToRevs(vpBiclops::speedLimit * percentVelocity / 100.); + m_tiltProfile.pos = PMDUtils::RadsToRevs(q[1]); + m_tiltProfile.vel = PMDUtils::RadsToRevs(vpBiclops::speedLimit * percentVelocity / 100.); - tiltAxis->ProfileToCounts(tiltProfile, desired_profile); + m_tiltAxis->ProfileToCounts(m_tiltProfile, desired_profile); vpCDEBUG(12) << "desired_profile.pos: " << desired_profile.pos << std::endl; vpCDEBUG(12) << "desired_profile.vel: " << desired_profile.vel << std::endl; - tiltAxis->SetProfile(desired_profile); + m_tiltAxis->SetProfile(desired_profile); #endif // Coordinate motion PMDUtils::Coordinate(axisList); - biclops.Move(Biclops::PanMask + Biclops::TiltMask /*, 0*/); // + m_biclops.Move(Biclops::PanMask + Biclops::TiltMask /*, 0*/); // } -/*! - - Apply a velocity to each axis of the biclops robot. - - \warning This method is non blocking. - - \param q_dot : Velocity to apply. - -*/ void vpRobotBiclopsController::setVelocity(const vpColVector &q_dot) { if (q_dot.getRows() != vpBiclops::ndof) { @@ -272,66 +227,59 @@ void vpRobotBiclopsController::setVelocity(const vpColVector &q_dot) #ifdef VISP_HAVE_BICLOPS_AND_GET_HOMED_STATE_FUNCTION // new API // Get the currently defined (default) motion profiles. - // PMDAxisControl::Profile panProfile, tiltProfile; - panAxis->GetProfile(panProfile); - tiltAxis->GetProfile(tiltProfile); + // PMDAxisControl::Profile m_panProfile, m_tiltProfile; + m_panAxis->GetProfile(m_panProfile); + m_tiltAxis->GetProfile(m_tiltProfile); // Set a position to move to by modifying the respective profiles. // NOTE: profile values are in revolutions, so here we convert // from degrees (divide by 360) for readability. - panProfile.vel = PMDUtils::RadsToRevs(q_dot[0]); - tiltProfile.vel = PMDUtils::RadsToRevs(q_dot[1]); + m_panProfile.vel = PMDUtils::RadsToRevs(q_dot[0]); + m_tiltProfile.vel = PMDUtils::RadsToRevs(q_dot[1]); // Inform the controller of the new desired position. - panAxis->SetProfile(panProfile); - tiltAxis->SetProfile(tiltProfile); + m_panAxis->SetProfile(m_panProfile); + m_tiltAxis->SetProfile(m_tiltProfile); - panAxis->SetProfileMode(PMDVelocityContouringProfile); - tiltAxis->SetProfileMode(PMDVelocityContouringProfile); + m_panAxis->SetProfileMode(PMDVelocityContouringProfile); + m_tiltAxis->SetProfileMode(PMDVelocityContouringProfile); #else // old API - panAxis->SetProfileMode(PMDVelocityContouringProfile); - tiltAxis->SetProfileMode(PMDVelocityContouringProfile); + m_panAxis->SetProfileMode(PMDVelocityContouringProfile); + m_tiltAxis->SetProfileMode(PMDVelocityContouringProfile); PMDAxisControl::CountsProfile desired_profile; // Set a position to move to by modifying the respective profiles. // NOTE: profile values are in revolutions, so here we convert // from degrees (divide by 360) for readability. - panProfile.vel = PMDUtils::RadsToRevs(q_dot[0]); + m_panProfile.vel = PMDUtils::RadsToRevs(q_dot[0]); - panAxis->ProfileToCounts(panProfile, desired_profile); - panAxis->SetProfile(desired_profile); + m_panAxis->ProfileToCounts(m_panProfile, desired_profile); + m_panAxis->SetProfile(desired_profile); // Set a position to move to by modifying the respective profiles. // NOTE: profile values are in revolutions, so here we convert // from degrees (divide by 360) for readability. - tiltProfile.vel = PMDUtils::RadsToRevs(q_dot[1]); + m_tiltProfile.vel = PMDUtils::RadsToRevs(q_dot[1]); - tiltAxis->ProfileToCounts(tiltProfile, desired_profile); - tiltAxis->SetProfile(desired_profile); + m_tiltAxis->ProfileToCounts(m_tiltProfile, desired_profile); + m_tiltAxis->SetProfile(desired_profile); #endif // Coordinate motion - biclops.Move(Biclops::PanMask + Biclops::TiltMask, 0); // + m_biclops.Move(Biclops::PanMask + Biclops::TiltMask, 0); // } -/*! - - Get the biclops articular position. - - \return The axis articular position in radians. - -*/ vpColVector vpRobotBiclopsController::getPosition() { vpDEBUG_TRACE(12, "Start vpRobotBiclopsController::getPosition() "); vpColVector q(vpBiclops::ndof); PMDint32 panpos, tiltpos; - panAxis->GetPosition(panpos); - tiltAxis->GetPosition(tiltpos); + m_panAxis->GetPosition(panpos); + m_tiltAxis->GetPosition(tiltpos); - q[0] = PMDUtils::RevsToRads(panAxis->CountsToUnits(panpos)); - q[1] = PMDUtils::RevsToRads(tiltAxis->CountsToUnits(tiltpos)); + q[0] = PMDUtils::RevsToRads(m_panAxis->CountsToUnits(panpos)); + q[1] = PMDUtils::RevsToRads(m_tiltAxis->CountsToUnits(tiltpos)); vpCDEBUG(11) << "++++++++ Mesure : " << q.t(); vpDEBUG_TRACE(12, "End vpRobotBiclopsController::getPosition()"); @@ -339,101 +287,68 @@ vpColVector vpRobotBiclopsController::getPosition() return q; } -/*! - - Get the biclops actual articular position. - - \return The axis actual articular position in radians. - -*/ vpColVector vpRobotBiclopsController::getActualPosition() { vpColVector q(vpBiclops::ndof); PMDint32 panpos, tiltpos; - panAxis->GetActualPosition(panpos); - tiltAxis->GetActualPosition(tiltpos); + m_panAxis->GetActualPosition(panpos); + m_tiltAxis->GetActualPosition(tiltpos); - q[0] = PMDUtils::RevsToRads(panAxis->CountsToUnits(panpos)); - q[1] = PMDUtils::RevsToRads(tiltAxis->CountsToUnits(tiltpos)); + q[0] = PMDUtils::RevsToRads(m_panAxis->CountsToUnits(panpos)); + q[1] = PMDUtils::RevsToRads(m_tiltAxis->CountsToUnits(tiltpos)); return q; } -/*! - - Get the biclops articular velocity. - - \return The axis articular velocity in rad/s. - -*/ vpColVector vpRobotBiclopsController::getVelocity() { vpColVector q_dot(vpBiclops::ndof); PMDint32 pan_vel, tilt_vel; - panAxis->GetVelocity(pan_vel); - tiltAxis->GetVelocity(tilt_vel); + m_panAxis->GetVelocity(pan_vel); + m_tiltAxis->GetVelocity(tilt_vel); - q_dot[0] = PMDUtils::RevsToRads(panAxis->CountsToUnits(pan_vel)); - q_dot[1] = PMDUtils::RevsToRads(tiltAxis->CountsToUnits(tilt_vel)); + q_dot[0] = PMDUtils::RevsToRads(m_panAxis->CountsToUnits(pan_vel)); + q_dot[1] = PMDUtils::RevsToRads(m_tiltAxis->CountsToUnits(tilt_vel)); return q_dot; } -/*! - - Get the biclops actual articular velocity. - - \return The axis actual articular velocity in rad/s. - -*/ vpColVector vpRobotBiclopsController::getActualVelocity() { vpColVector q_dot(vpBiclops::ndof); PMDint32 pan_vel, tilt_vel; - panAxis->GetActualVelocity(pan_vel); - tiltAxis->GetActualVelocity(tilt_vel); + m_panAxis->GetActualVelocity(pan_vel); + m_tiltAxis->GetActualVelocity(tilt_vel); - q_dot[0] = PMDUtils::RevsToRads(panAxis->CountsToUnits(pan_vel)); - q_dot[1] = PMDUtils::RevsToRads(tiltAxis->CountsToUnits(tilt_vel)); + q_dot[0] = PMDUtils::RevsToRads(m_panAxis->CountsToUnits(pan_vel)); + q_dot[1] = PMDUtils::RevsToRads(m_tiltAxis->CountsToUnits(tilt_vel)); return q_dot; } -/*! - - Update the shared memory. - - \param shm_ : Content to write in the shared memory. -*/ -void vpRobotBiclopsController::writeShm(shmType &shm_) +void vpRobotBiclopsController::writeShm(shmType &shm) { for (unsigned int i = 0; i < vpBiclops::ndof; i++) { - vpDEBUG_TRACE(13, "q_dot[%d]=%f", i, shm_.q_dot[i]); + vpDEBUG_TRACE(13, "q_dot[%d]=%f", i, shm.q_dot[i]); } - memcpy(&this->shm, &shm_, sizeof(shmType)); + memcpy(&this->m_shm, &shm, sizeof(shmType)); // this->shm = shm_; for (unsigned int i = 0; i < vpBiclops::ndof; i++) { - vpDEBUG_TRACE(13, "shm.q_dot[%d]=%f", i, shm.q_dot[i]); + vpDEBUG_TRACE(13, "shm.q_dot[%d]=%f", i, m_shm.q_dot[i]); } } -/*! - - Get a copy of the shared memory. - - \return A copy of the shared memory. -*/ vpRobotBiclopsController::shmType vpRobotBiclopsController::readShm() { shmType tmp_shm; for (unsigned int i = 0; i < vpBiclops::ndof; i++) { - vpDEBUG_TRACE(13, "shm.q_dot[%d]=%f", i, shm.q_dot[i]); + vpDEBUG_TRACE(13, "shm.q_dot[%d]=%f", i, m_shm.q_dot[i]); } - memcpy(&tmp_shm, &this->shm, sizeof(shmType)); + memcpy(&tmp_shm, &this->m_shm, sizeof(shmType)); // tmp_shm = shm; for (unsigned int i = 0; i < vpBiclops::ndof; i++) { vpDEBUG_TRACE(13, "tmp_shm.q_dot[%d]=%f", i, tmp_shm.q_dot[i]); @@ -445,7 +360,7 @@ vpRobotBiclopsController::shmType vpRobotBiclopsController::readShm() #elif !defined(VISP_BUILD_SHARED_LIBS) // Work around to avoid warning: // libvisp_robot.a(vpRobotBiclopsController.cpp.o) has no symbols -void dummy_vpRobotBiclopsController(){}; +void dummy_vpRobotBiclopsController() { }; #endif #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS diff --git a/modules/robot/src/real-robot/franka/vpRobotFranka.cpp b/modules/robot/src/real-robot/franka/vpRobotFranka.cpp index b39dbc0553..5579d4348e 100644 --- a/modules/robot/src/real-robot/franka/vpRobotFranka.cpp +++ b/modules/robot/src/real-robot/franka/vpRobotFranka.cpp @@ -800,7 +800,7 @@ vpRobot::vpRobotStateType vpRobotFranka::setRobotState(vpRobot::vpRobotStateType vpRobot::STATE_VELOCITY_CONTROL) before setVelocity(). \warning Velocities could be saturated if one of them exceed the - maximal autorized speed (see vpRobot::maxTranslationVelocity and + maximal authorized speed (see vpRobot::maxTranslationVelocity and vpRobot::maxRotationVelocity). To change these values use setMaxTranslationVelocity() and setMaxRotationVelocity(). */ diff --git a/modules/robot/src/real-robot/ptu46/vpPtu46.cpp b/modules/robot/src/real-robot/ptu46/vpPtu46.cpp index 9d904eea5c..9a2807b945 100644 --- a/modules/robot/src/real-robot/ptu46/vpPtu46.cpp +++ b/modules/robot/src/real-robot/ptu46/vpPtu46.cpp @@ -148,7 +148,7 @@ void vpPtu46::computeMGD(const vpColVector &q, vpPoseVector &r) const /*! - Default construtor. Call init(). + Default constructor. Call init(). */ vpPtu46::vpPtu46(void) { init(); } @@ -184,7 +184,7 @@ VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpPtu46 & /* consta on the tilt axis. \param cVe : Twist transformation between camera and end effector frame to - expess a velocity skew from end effector frame in camera frame. + express a velocity skew from end effector frame in camera frame. */ void vpPtu46::get_cVe(vpVelocityTwistMatrix &cVe) const diff --git a/modules/robot/src/real-robot/ptu46/vpRobotPtu46.cpp b/modules/robot/src/real-robot/ptu46/vpRobotPtu46.cpp index 3fb718d738..586cc70dc0 100644 --- a/modules/robot/src/real-robot/ptu46/vpRobotPtu46.cpp +++ b/modules/robot/src/real-robot/ptu46/vpRobotPtu46.cpp @@ -125,7 +125,7 @@ vpRobotPtu46::~vpRobotPtu46(void) \exception vpRobotException::constructionError : If the device cannot be - oppened. + opened. */ void vpRobotPtu46::init() @@ -195,7 +195,7 @@ void vpRobotPtu46::stopMotion(void) on the tilt axis. \param cVe : Twist transformation between camera and end effector frame to - expess a velocity skew from end effector frame in camera frame. + express a velocity skew from end effector frame in camera frame. */ void vpRobotPtu46::get_cVe(vpVelocityTwistMatrix &cVe) const @@ -466,7 +466,7 @@ void vpRobotPtu46::getPosition(const vpRobot::vpControlFrameType frame, vpColVec (vpRobot::REFERENCE_FRAME, vpRobot::END_EFFECTOR_FRAME, vpRobot::MIXT_FRAME) is given. \warning Velocities could be saturated if one of them exceed the maximal - autorized speed (see vpRobot::maxRotationVelocity). + authorized speed (see vpRobot::maxRotationVelocity). */ void vpRobotPtu46::setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &v) @@ -639,7 +639,7 @@ vpColVector vpRobotPtu46::getVelocity(vpRobot::vpControlFrameType frame) \code # Example of ptu-46 position file - # The axis positions must be preceed by R: + # The axis positions must be preceeded by R: # First value : pan articular position in degrees # Second value: tilt articular position in degrees R: 15.0 5.0 diff --git a/modules/robot/src/real-robot/universal-robots/vpRobotUniversalRobots.cpp b/modules/robot/src/real-robot/universal-robots/vpRobotUniversalRobots.cpp index 78e77a812b..73329e7ac6 100644 --- a/modules/robot/src/real-robot/universal-robots/vpRobotUniversalRobots.cpp +++ b/modules/robot/src/real-robot/universal-robots/vpRobotUniversalRobots.cpp @@ -552,7 +552,7 @@ void vpRobotUniversalRobots::setPosition(const vpRobot::vpControlFrameType frame * vpRobot::STATE_VELOCITY_CONTROL) before setVelocity(). * * \warning Velocities could be saturated if one of them exceed the - * maximal autorized speed (see vpRobot::maxTranslationVelocity and + * maximal authorized speed (see vpRobot::maxTranslationVelocity and * vpRobot::maxRotationVelocity). To change these values use * setMaxTranslationVelocity() and setMaxRotationVelocity(). * diff --git a/modules/robot/src/real-robot/viper/vpRobotViper650.cpp b/modules/robot/src/real-robot/viper/vpRobotViper650.cpp index f62aecad44..b252cc2ed4 100644 --- a/modules/robot/src/real-robot/viper/vpRobotViper650.cpp +++ b/modules/robot/src/real-robot/viper/vpRobotViper650.cpp @@ -1596,7 +1596,7 @@ reference frame in m/s and rotations \f$ ^{c} \omega_x, ^{c} \omega_y, ^{c} vpRobot::STATE_VELOCITY_CONTROL) before setVelocity(). \warning Velocities could be saturated if one of them exceed the - maximal autorized speed (see vpRobot::maxTranslationVelocity and + maximal authorized speed (see vpRobot::maxTranslationVelocity and vpRobot::maxRotationVelocity). To change these values use setMaxTranslationVelocity() and setMaxRotationVelocity(). diff --git a/modules/robot/src/real-robot/viper/vpRobotViper850.cpp b/modules/robot/src/real-robot/viper/vpRobotViper850.cpp index 6ebdf0cf3e..c22d028a18 100644 --- a/modules/robot/src/real-robot/viper/vpRobotViper850.cpp +++ b/modules/robot/src/real-robot/viper/vpRobotViper850.cpp @@ -1625,7 +1625,7 @@ reference frame in m/s and rotations \f$ ^{c} \omega_x, ^{c} \omega_y, ^{c} vpRobot::STATE_VELOCITY_CONTROL) before setVelocity(). \warning Velocities could be saturated if one of them exceed the - maximal autorized speed (see vpRobot::maxTranslationVelocity and + maximal authorized speed (see vpRobot::maxTranslationVelocity and vpRobot::maxRotationVelocity). To change these values use setMaxTranslationVelocity() and setMaxRotationVelocity(). diff --git a/modules/robot/src/robot-simulator/vpSimulatorAfma6.cpp b/modules/robot/src/robot-simulator/vpSimulatorAfma6.cpp index 1714b3eae0..d6c1d38888 100644 --- a/modules/robot/src/robot-simulator/vpSimulatorAfma6.cpp +++ b/modules/robot/src/robot-simulator/vpSimulatorAfma6.cpp @@ -816,7 +816,7 @@ reference frame in m/s and rotations \f$ ^{c} \omega_x, ^{c} \omega_y, ^{c} vpRobot::STATE_VELOCITY_CONTROL) before setVelocity(). \warning Velocities could be saturated if one of them exceed the - maximal autorized speed (see vpRobot::maxTranslationVelocity and + maximal authorized speed (see vpRobot::maxTranslationVelocity and vpRobot::maxRotationVelocity). To change these values use setMaxTranslationVelocity() and setMaxRotationVelocity(). diff --git a/modules/robot/src/robot-simulator/vpSimulatorViper850.cpp b/modules/robot/src/robot-simulator/vpSimulatorViper850.cpp index 3ad470f5e0..3274b0a639 100644 --- a/modules/robot/src/robot-simulator/vpSimulatorViper850.cpp +++ b/modules/robot/src/robot-simulator/vpSimulatorViper850.cpp @@ -753,7 +753,7 @@ reference frame in m/s and rotations \f$ ^{c} \omega_x, ^{c} \omega_y, ^{c} vpRobot::STATE_VELOCITY_CONTROL) before setVelocity(). \warning Velocities could be saturated if one of them exceed the - maximal autorized speed (see vpRobot::maxTranslationVelocity and + maximal authorized speed (see vpRobot::maxTranslationVelocity and vpRobot::maxRotationVelocity). To change these values use setMaxTranslationVelocity() and setMaxRotationVelocity(). diff --git a/modules/vision/src/pose-estimation/vpLevenbergMarquartd.cpp b/modules/vision/src/pose-estimation/vpLevenbergMarquartd.cpp index 677f57d808..6ce6cb6e5d 100644 --- a/modules/vision/src/pose-estimation/vpLevenbergMarquartd.cpp +++ b/modules/vision/src/pose-estimation/vpLevenbergMarquartd.cpp @@ -557,7 +557,7 @@ int qrfac(int m, int n, double *a, int lda, int *pivot, int *ipvt, int /* lipvt ipvt[i] = i; } /* - * reduction de "a" en "r" avec les tranformations de Householder. + * reduction de "a" en "r" avec les transformations de Householder. */ minmn = vpMath::minimum(m, n); for (i = 0; i < minmn; i++) { @@ -599,7 +599,7 @@ int qrfac(int m, int n, double *a, int lda, int *pivot, int *ipvt, int /* lipvt *MIJ(a, i, i, lda) += 1.0; /* - * application de la tranformation aux lignes + * application de la transformation aux lignes * restantes et mise a jour des normes. */ ip1 = i + 1; @@ -770,7 +770,7 @@ int qrsolv(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, doub wa[k] = temp; /* - * accumulation des tranformations dans + * accumulation des transformations dans * les lignes de s. */