From db868e07b28be24160b2e6a100aee1f6741fa309 Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Tue, 17 Oct 2023 10:04:53 +0200 Subject: [PATCH] Fix previous commit --- CMakeLists.txt | 2 +- ChangeLog.txt | 2 +- example/servo-biclops/moveBiclops.cpp | 8 +- .../servoBiclopsPoint2DArtVelocity.cpp | 6 +- .../servoPioneerPanSegment3D.cpp | 6 +- modules/robot/include/visp3/robot/vpAfma4.h | 2 +- modules/robot/include/visp3/robot/vpBiclops.h | 48 +-- .../include/visp3/robot/vpRobotBiclops.h | 12 +- .../visp3/robot/vpRobotBiclopsController.h | 358 ++++++++---------- .../src/real-robot/biclops/vpBiclops.cpp | 6 +- .../src/real-robot/biclops/vpRobotBiclops.cpp | 4 +- .../robot/src/real-robot/viper/vpViper.cpp | 2 +- .../robot/src/real-robot/viper/vpViper650.cpp | 4 +- .../robot/src/real-robot/viper/vpViper850.cpp | 4 +- 14 files changed, 209 insertions(+), 255 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3d96273291..f187a62160 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -520,7 +520,7 @@ VP_OPTION(USE_PYLON Pylon "" "Include Pylon SDK support for Basle VP_OPTION(USE_UEYE Ueye "" "Include uEye SDK support for IDS cameras" "" ON IF NOT WINRT AND NOT IOS) VP_OPTION(USE_COMEDI Comedi "" "Include comedi (linux control and measurement device interface) support" "" ON IF NOT WINRT AND NOT IOS) VP_OPTION(USE_FTIITSDK FTIITSDK "" "Include IIT force-torque SDK support" "" ON IF NOT WINRT AND NOT IOS) -VP_OPTION(USE_BICLOPS BICLOPS "" "Include biclops support" "" ON IF NOT WINRT AND NOT IOS) +VP_OPTION(USE_BICLOPS BICLOPS "" "Include Biclops support" "" ON IF NOT WINRT AND NOT IOS) VP_OPTION(USE_PTU46 PTU46 "" "Include ptu-46 support" "" ON IF UNIX AND NOT WINRT AND NOT IOS) VP_OPTION(USE_FLIRPTUSDK FlirPtuSDK "" "Include Flir PTU SDK support" "" ON IF NOT WINRT AND NOT IOS) VP_OPTION(USE_CMU1394 CMU1394 "" "Include cmu1494 support" "" ON IF WIN32 AND NOT WINRT AND NOT IOS) diff --git a/ChangeLog.txt b/ChangeLog.txt index 33d52135de..df7a95a3df 100644 --- a/ChangeLog.txt +++ b/ChangeLog.txt @@ -957,7 +957,7 @@ ViSP 2.6.2 (released July 15, 2012) To be able to consider other user defined features the compiler should be compatible with C++ 11 standard. - Improvements - - Introduce a new Denavit Hartenberg representation of the Biclops head + - Introduce a new Denavit-Hartenberg representation of the Biclops head in vpBiclops. - Compatibility with g++ 4.7 and OpenCV 2.3.1, 2.4.0, 2.4.1 - Remove warning detected with Visual C++ /Wall flag diff --git a/example/servo-biclops/moveBiclops.cpp b/example/servo-biclops/moveBiclops.cpp index 92b5436466..d1ab30ce44 100644 --- a/example/servo-biclops/moveBiclops.cpp +++ b/example/servo-biclops/moveBiclops.cpp @@ -35,7 +35,7 @@ * \file moveBiclops.cpp * \example moveBiclops.cpp * - * Example of a real robot control, the biclops robot (pan-tilt turret) by + * Example of a real robot control, the Biclops robot (pan-tilt turret) by * Traclabs. The robot is controlled first in position, then in velocity. * * See http://www.traclabs.com/tracbiclops.htm for more details. @@ -63,7 +63,7 @@ void usage(const char *name, const char *badparam, std::string conf) { fprintf(stdout, "\n\ -Move the biclops robot\n\ +Move the Biclops robot\n\ \n\ SYNOPSIS\n\ %s [-c ] [-h]\n\ @@ -73,7 +73,7 @@ name); fprintf(stdout, "\n\ OPTIONS: Default\n\ -c %s\n\ - Sets the biclops robot configuration file.\n\n", + Sets the Biclops robot configuration file.\n\n", conf.c_str()); if (badparam) { @@ -270,7 +270,7 @@ int main(int argc, const char **argv) #else int main() { - std::cout << "You do not have an biclops PT robot connected to your computer..." << std::endl; + std::cout << "You do not have an Biclops PT robot connected to your computer..." << std::endl; return EXIT_SUCCESS; } diff --git a/example/servo-biclops/servoBiclopsPoint2DArtVelocity.cpp b/example/servo-biclops/servoBiclopsPoint2DArtVelocity.cpp index 2ef49b5a8a..dd63b1dba8 100644 --- a/example/servo-biclops/servoBiclopsPoint2DArtVelocity.cpp +++ b/example/servo-biclops/servoBiclopsPoint2DArtVelocity.cpp @@ -38,7 +38,7 @@ * \example servoBiclopsPoint2DArtVelocity.cpp * * Example of eye-in-hand control law. We control here a real robot, the - * biclops robot (pan-tilt head provided by Traclabs). The velocity is computed + * Biclops robot (pan-tilt head provided by Traclabs). The velocity is computed * in articular. The visual feature is the center of gravity of a point. */ @@ -90,7 +90,7 @@ SYNOPSIS\n\ fprintf(stdout, "\n\ OPTIONS: Default\n\ -c %s\n\ - Sets the biclops robot configuration file.\n", + Sets the Biclops robot configuration file.\n", conf.c_str()); if (badparam) { @@ -338,7 +338,7 @@ int main(int argc, const char **argv) #else int main() { - std::cout << "You do not have an biclops PT robot connected to your computer..." << std::endl; + std::cout << "You do not have an Biclops PT robot connected to your computer..." << std::endl; return EXIT_SUCCESS; } #endif diff --git a/example/servo-pioneer/servoPioneerPanSegment3D.cpp b/example/servo-pioneer/servoPioneerPanSegment3D.cpp index d00ab51c5a..e85c553c72 100644 --- a/example/servo-pioneer/servoPioneerPanSegment3D.cpp +++ b/example/servo-pioneer/servoPioneerPanSegment3D.cpp @@ -103,7 +103,7 @@ int main(int argc, char **argv) double qm_pan = 0; // Measured pan position (tilt is not handled in that example) #ifdef USE_REAL_ROBOT - // Initialize the biclops head + // Initialize the Biclops head vpRobotBiclops biclops("/usr/share/BiclopsDefault.cfg"); biclops.setDenavitHartenbergModel(vpBiclops::DH1); @@ -158,7 +158,7 @@ int main(int argc, char **argv) #endif vpPioneerPan robot_pan; // Generic robot that computes the velocities for - // the pioneer and the biclops head + // the pioneer and the Biclops head // Camera parameters. In this experiment we don't need a precise // calibration of the camera @@ -379,7 +379,7 @@ int main(int argc, char **argv) std::cout << "Send velocity to the pionner: " << v_pioneer[0] << " m/s " << vpMath::deg(v_pioneer[1]) << " deg/s" << std::endl; - std::cout << "Send velocity to the biclops head: " << vpMath::deg(v_biclops[0]) << " deg/s" << std::endl; + std::cout << "Send velocity to the Biclops head: " << vpMath::deg(v_biclops[0]) << " deg/s" << std::endl; pioneer.setVelocity(vpRobot::REFERENCE_FRAME, v_pioneer); biclops.setVelocity(vpRobot::ARTICULAR_FRAME, v_biclops); diff --git a/modules/robot/include/visp3/robot/vpAfma4.h b/modules/robot/include/visp3/robot/vpAfma4.h index 12572577fd..b857c146b7 100644 --- a/modules/robot/include/visp3/robot/vpAfma4.h +++ b/modules/robot/include/visp3/robot/vpAfma4.h @@ -137,7 +137,7 @@ class VISP_EXPORT vpAfma4 static const unsigned int njoint; ///< Number of joint. protected: - // Denavit Hartenberg parameters + // Denavit-Hartenberg parameters double _a1; // distance along x2 double _d3; // distance along z2 double _d4; // distance along z3 diff --git a/modules/robot/include/visp3/robot/vpBiclops.h b/modules/robot/include/visp3/robot/vpBiclops.h index 10d04e1657..e0af4beac1 100644 --- a/modules/robot/include/visp3/robot/vpBiclops.h +++ b/modules/robot/include/visp3/robot/vpBiclops.h @@ -49,7 +49,7 @@ * \brief Jacobian, geometric model functionalities... for Biclops, pan, tilt * head. * - * Two different Denavit Hartenberg representations of the robot are + * 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. @@ -62,7 +62,7 @@ class VISP_EXPORT vpBiclops { public: /*! - * Two different Denavit Hartenberg representations of the robot are + * 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 @@ -91,8 +91,8 @@ class VISP_EXPORT vpBiclops */ typedef enum { - DH1, //!< First Denavit Hartenberg representation. - DH2 //!< Second Denavit Hartenberg representation. + DH1, //!< First Denavit-Hartenberg representation. + DH2 //!< Second Denavit-Hartenberg representation. } DenavitHartenbergModel; public: @@ -105,12 +105,12 @@ class VISP_EXPORT vpBiclops static const float speedLimit; //!< Pan and tilt axis max velocity in rad/s to perform a displacement protected: - DenavitHartenbergModel m_dh_model; //!< Denavit Hartenberg model - vpHomogeneousMatrix m_cMe; //!< Camera frame to end-effector transformation + DenavitHartenbergModel m_dh_model; //!< Denavit-Hartenberg model + vpHomogeneousMatrix m_cMe; //!< Camera frame to PT end-effector frame transformation public: /*! - * Default constructor. Call init() that sets vpBiclops::DH1 Denavit Hartenberg model. + * Default constructor. Call init() that sets vpBiclops::DH1 Denavit-Hartenberg model. */ vpBiclops(void); @@ -124,7 +124,7 @@ class VISP_EXPORT vpBiclops /*! * Initialization. - * - By default vpBiclops::DH1 Denavit Hartenberg model is selected. + * - By default vpBiclops::DH1 Denavit-Hartenberg model is selected. * - Initialize also the default \f${^c}{\bf M}_e\f$ transformation calling set_cMe(). * \f[ * {^c}{\bf M}_e = \left( @@ -197,7 +197,7 @@ class VISP_EXPORT vpBiclops * 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 + * \param[out] 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; @@ -205,20 +205,20 @@ class VISP_EXPORT vpBiclops /*! * Compute the direct geometric model of the camera: fMc * - * \param q : Joint position for pan and tilt axis. + * \param[in] q : Joint position for pan and tilt axis. * - * \param fMc : Homogeneous matrix corresponding to the direct geometric model + * \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; /*! - * Compute the direct geometric model of the camera: fMc + * Compute the direct geometric model of the camera in terms of pose vector. * - * \param q : Joint position for pan and tilt axis. + * \param[in] q : Joint position for pan and tilt axis. * - * \param fPc : Pose vector corresponding to the direct geometric model + * \param[out] fPc : Pose vector corresponding to the direct geometric model * of the camera. Describes the transformation between the robot reference * frame (called fixed) and the camera frame. */ @@ -227,7 +227,7 @@ class VISP_EXPORT vpBiclops /*! * Return the direct geometric model of the camera: fMc * - * \param q : Joint position for pan and tilt axis. + * \param[in] q : Joint 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 @@ -238,7 +238,7 @@ class VISP_EXPORT vpBiclops /*! * Return the direct geometric model of the end effector: fMe * - * \param q : Joint position for pan and tilt axis. + * \param[in] q : Joint 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 @@ -252,9 +252,9 @@ class VISP_EXPORT vpBiclops * \warning Re is not the embedded camera frame. It corresponds to the frame * associated to the tilt axis (see also get_cMe). * - * \param q : Joint position for pan and tilt axis. + * \param[in] q : Joint position for pan and tilt axis. * - * \param eJe : Jacobian between end effector frame and end effector frame (on + * \param[out] eJe : Jacobian between end effector frame and end effector frame (on * tilt axis). */ void get_eJe(const vpColVector &q, vpMatrix &eJe) const; @@ -262,15 +262,15 @@ class VISP_EXPORT vpBiclops /*! * Get the robot jacobian expressed in the robot reference frame * - * \param q : Joint position for pan and tilt axis. + * \param[in] q : Joint position for pan and tilt axis. * - * \param fJe : Jacobian between reference frame (or fix frame) and end + * \param[out] 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. + * Return the Denavit-Hartenberg representation used to model the head. * \sa vpBiclops::DenavitHartenbergModel */ inline vpBiclops::DenavitHartenbergModel getDenavitHartenbergModel() const { return m_dh_model; } @@ -300,9 +300,9 @@ class VISP_EXPORT vpBiclops void set_cMe(const vpHomogeneousMatrix &cMe) { m_cMe = cMe; } /*! - * Set the Denavit Hartenberg representation used to model the head. + * Set the Denavit-Hartenberg representation used to model the head. * - * \param[in] dh_model : Denavit Hartenberg model. \sa vpBiclops::DenavitHartenbergModel + * \param[in] dh_model : Denavit-Hartenberg model. \sa vpBiclops::DenavitHartenbergModel */ inline void setDenavitHartenbergModel(vpBiclops::DenavitHartenbergModel dh_model = vpBiclops::DH1) { @@ -315,7 +315,7 @@ class VISP_EXPORT vpBiclops * Set output stream with Biclops parameters. * @param os : Output stream. * @param dummy : Not used. - * @return Output stream with the biclops parameters. + * @return Output stream with the Biclops parameters. */ friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpBiclops &dummy); }; diff --git a/modules/robot/include/visp3/robot/vpRobotBiclops.h b/modules/robot/include/visp3/robot/vpRobotBiclops.h index 0a26adab4e..900e7e4f5d 100644 --- a/modules/robot/include/visp3/robot/vpRobotBiclops.h +++ b/modules/robot/include/visp3/robot/vpRobotBiclops.h @@ -74,7 +74,7 @@ * See http://www.traclabs.com/biclopspt.html for more details. * * This class provide a position and a speed control interface for the Biclops - * head. To manage the biclops joint limits in speed control, a control loop is + * head. To manage the Biclops joint limits in speed control, a control loop is * running in a separate thread implemented in vpRobotBiclopsSpeedControlLoop(). * * \warning Velocity control mode is not exported from the top-level Biclops @@ -308,7 +308,7 @@ class VISP_EXPORT vpRobotBiclops : public vpBiclops, public vpRobot * \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 + * \param frame : Control frame. This Biclops head can only be controlled in * joint state. * * \param q : The joint position to set for each axis in radians. @@ -324,7 +324,7 @@ class VISP_EXPORT vpRobotBiclops : public vpBiclops, public vpRobot * \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 + * \param frame : Control frame. This Biclops head can only be controlled in * joint state. * * \param q1 : The pan joint position to set in radians. @@ -336,8 +336,8 @@ class VISP_EXPORT vpRobotBiclops : public vpBiclops, public vpRobot void setPosition(const vpRobot::vpControlFrameType frame, const double &q1, const double &q2); /*! - * Read the content of the position file and moves to head to joint - * position. + * Read the content of the position file and moves the head to joint + * positions. * * \param filename : Position filename * @@ -365,7 +365,7 @@ class VISP_EXPORT vpRobotBiclops : public vpBiclops, public vpRobot /*! * Send a velocity on each axis. * - * \param frame : Control frame. This biclops head can only be controlled in + * \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. diff --git a/modules/robot/include/visp3/robot/vpRobotBiclopsController.h b/modules/robot/include/visp3/robot/vpRobotBiclopsController.h index bb641d79cc..c2f9627fe3 100644 --- a/modules/robot/include/visp3/robot/vpRobotBiclopsController.h +++ b/modules/robot/include/visp3/robot/vpRobotBiclopsController.h @@ -30,37 +30,23 @@ * Description: * Interface for the Biclops robot. */ -<<<<<<< HEAD - ====== = #ifndef _vpRobotBiclopsController_h_ #define _vpRobotBiclopsController_h_ - >>>>>>> upstream/master #include #ifndef DOXYGEN_SHOULD_SKIP_THIS #ifdef VISP_HAVE_BICLOPS - - <<<<<<< HEAD -#include /* Classe std::ostream. */ -#include /* Classe std::ostream. */ -#include /* Classe std::ostream. */ - ====== = - /* ------------------------------------------------------------------------ */ - /* --- INCLUDES ----------------------------------------------------------- */ - /* ------------------------------------------------------------------------ */ - #include #include - >>>>>>> upstream/master #include // Contrib for Biclops sdk #include // Contrib for Biclops sdk #if defined(_WIN32) - class VISP_EXPORT Biclops; // needed for dll creation +class VISP_EXPORT Biclops; // needed for dll creation #endif /* ------------------------------------------------------------------------ */ @@ -84,210 +70,178 @@ class VISP_EXPORT vpRobotBiclopsController { public: /*! -<<<<<<< HEAD * Biclops controller status */ typedef enum { STOP, //!< Have to stop the robot. SPEED //!< Can send the desired speed. - ====== = - *Biclops head controller status. - */ - typedef enum - { - STOP, /*!< Have to stop the robot. */ - SPEED /*!< Can send the desired speed. */ - >>>>>>> upstream/master } vpControllerStatusType; - public: +public: #ifndef DOXYGEN_SHOULD_SKIP_THIS - <<<<<<< HEAD - // SHM - typedef struct /* ControllerShm_struct */ - ====== = - /*! - * Biclops head shared memory structure. - */ - typedef struct - >>>>>>> upstream/master - { - 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. - } shmType; + /*! + * 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. + } shmType; #endif /* DOXYGEN_SHOULD_SKIP_THIS */ // private: //#ifndef DOXYGEN_SHOULD_SKIP_THIS // vpRobotBiclopsController(const vpRobotBiclopsController &) - <<<<<<< HEAD // : m_biclops(), m_axisMask(0), m_panAxis(NULL), m_tiltAxis(NULL), // m_vergeAxis(NULL), // m_panProfile(), m_tiltProfile(), m_vergeProfile(), m_shm(), - ====== = - // : biclops(), m_axisMask(0), m_panAxis(NULL), m_tiltAxis(NULL), - // m_vergeAxis(NULL), - // m_panProfile(), m_tiltProfile(), m_vergeProfile(), shm(), - >>>>>>> upstream/master - // m_stopControllerThread(false) - // { - // throw vpException(vpException::functionNotImplementedError, "Not - // implemented!"); - // } - // vpRobotBiclopsController &operator=(const vpRobotBiclopsController &){ - // throw vpException(vpException::functionNotImplementedError, "Not - // implemented!"); return *this; - // } - //#endif - - public: - /*! - * Default constructor. - */ - vpRobotBiclopsController(); - - /*! - * Destructor. - */ - virtual ~vpRobotBiclopsController(); - - /*! - <<<<<<< HEAD - * Initialize the biclops by homing all axis. - * - * \param configfile : Biclops configuration file. - * - * \exception vpRobotException::notInitializedError If the biclops head cannot - ======= - * Initialize the Biclops by homing all axis. - * - * \param configfile : Biclops configuration file. - * - * \exception vpRobotException::notInitializedError If the Biclops head cannot - >>>>>>> upstream/master - * 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); - - /*! - <<<<<<< HEAD - * Set the biclops axis position. The motion of the axis is synchronized to end - ======= - * Set the Biclops axis position. The motion of the axis is synchronized to end - >>>>>>> upstream/master - * 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(); - - /*! - * 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(); - - /*! - * Return true when control thread is requested to stop. - */ - bool isStopRequested() { return m_stopControllerThread; } - - /*! - * Request to stop the control thread. - * @param stop : When true, request to stop the control thread. - */ - void stopRequest(bool stop) { m_stopControllerThread = stop; } - - private: - Biclops m_biclops; // THE interface to Biclops. - int m_axisMask; - - // Pointers to each axis (populated once controller is initialized). - PMDAxisControl *m_panAxis; - PMDAxisControl *m_tiltAxis; - PMDAxisControl *m_vergeAxis; - - PMDAxisControl::Profile m_panProfile; - PMDAxisControl::Profile m_tiltProfile; - PMDAxisControl::Profile m_vergeProfile; - - shmType m_shm; - bool m_stopControllerThread; - }; + // m_stopControllerThread(false) + // { + // throw vpException(vpException::functionNotImplementedError, "Not + // implemented!"); + // } + // vpRobotBiclopsController &operator=(const vpRobotBiclopsController &){ + // throw vpException(vpException::functionNotImplementedError, "Not + // implemented!"); return *this; + // } + //#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(); + + /*! + * 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(); + + /*! + * Return true when control thread is requested to stop. + */ + bool isStopRequested() { return m_stopControllerThread; } + + /*! + * Request to stop the control thread. + * @param stop : When true, request to stop the control thread. + */ + void stopRequest(bool stop) { m_stopControllerThread = stop; } + +private: + Biclops m_biclops; // THE interface to Biclops. + int m_axisMask; + + // Pointers to each axis (populated once controller is initialized). + PMDAxisControl *m_panAxis; + PMDAxisControl *m_tiltAxis; + PMDAxisControl *m_vergeAxis; + + PMDAxisControl::Profile m_panProfile; + PMDAxisControl::Profile m_tiltProfile; + PMDAxisControl::Profile m_vergeProfile; + + shmType m_shm; + bool m_stopControllerThread; +}; #endif /* #ifndef _vpRobotBiclopsController_h_ */ diff --git a/modules/robot/src/real-robot/biclops/vpBiclops.cpp b/modules/robot/src/real-robot/biclops/vpBiclops.cpp index d13a49b483..c61b03a148 100644 --- a/modules/robot/src/real-robot/biclops/vpBiclops.cpp +++ b/modules/robot/src/real-robot/biclops/vpBiclops.cpp @@ -86,7 +86,7 @@ vpHomogeneousMatrix vpBiclops::get_fMe(const vpColVector &q) const vpHomogeneousMatrix fMe; if (q.getRows() != 2) { - throw(vpException(vpException::dimensionError, "Bad dimension for biclops joint position vector")); + throw(vpException(vpException::dimensionError, "Bad dimension for Biclops joint position vector")); } double q1 = q[0]; // pan @@ -213,7 +213,7 @@ void vpBiclops::get_eJe(const vpColVector &q, vpMatrix &eJe) const eJe.resize(6, 2); if (q.getRows() != 2) { - throw(vpException(vpException::dimensionError, "Bad dimension for biclops joint position vector")); + throw(vpException(vpException::dimensionError, "Bad dimension for Biclops joint position vector")); } double s2 = sin(q[1]); @@ -236,7 +236,7 @@ void vpBiclops::get_eJe(const vpColVector &q, vpMatrix &eJe) const void vpBiclops::get_fJe(const vpColVector &q, vpMatrix &fJe) const { if (q.getRows() != 2) { - throw(vpException(vpException::dimensionError, "Bad dimension for biclops joint position vector")); + throw(vpException(vpException::dimensionError, "Bad dimension for Biclops joint position vector")); } fJe.resize(6, 2); diff --git a/modules/robot/src/real-robot/biclops/vpRobotBiclops.cpp b/modules/robot/src/real-robot/biclops/vpRobotBiclops.cpp index 7e2c6c5eb2..8edb4956da 100644 --- a/modules/robot/src/real-robot/biclops/vpRobotBiclops.cpp +++ b/modules/robot/src/real-robot/biclops/vpRobotBiclops.cpp @@ -116,7 +116,7 @@ void vpRobotBiclops::init() // test if the config file exists FILE *fd = fopen(m_configfile.c_str(), "r"); if (fd == NULL) { - vpCERROR << "Cannot open biclops config file: " << m_configfile << std::endl; + vpCERROR << "Cannot open Biclops config file: " << m_configfile << std::endl; throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with biclops"); } fclose(fd); @@ -528,7 +528,7 @@ void vpRobotBiclops::setPosition(const std::string &filename) { vpColVector q; if (readPositionFile(filename.c_str(), q) == false) { - vpERROR_TRACE("Cannot get biclops position from file"); + vpERROR_TRACE("Cannot get Biclops position from file"); throw vpRobotException(vpRobotException::readingParametersError, "Cannot get Biclops position from file"); } setPosition(vpRobot::JOINT_STATE, q); diff --git a/modules/robot/src/real-robot/viper/vpViper.cpp b/modules/robot/src/real-robot/viper/vpViper.cpp index 84c7f2f6fd..0019e7fc1e 100644 --- a/modules/robot/src/real-robot/viper/vpViper.cpp +++ b/modules/robot/src/real-robot/viper/vpViper.cpp @@ -64,7 +64,7 @@ vpViper::vpViper() { // Default values are initialized - // Denavit Hartenberg parameters + // Denavit-Hartenberg parameters a1 = 0.075; a2 = 0.365; a3 = 0.090; diff --git a/modules/robot/src/real-robot/viper/vpViper650.cpp b/modules/robot/src/real-robot/viper/vpViper650.cpp index 48869f951b..4557434552 100644 --- a/modules/robot/src/real-robot/viper/vpViper650.cpp +++ b/modules/robot/src/real-robot/viper/vpViper650.cpp @@ -93,13 +93,13 @@ const vpViper650::vpToolType vpViper650::defaultTool = vpViper650::TOOL_PTGREY_F /*! Default constructor. - Sets the specific parameters like the Denavit Hartenberg parameters. + Sets the specific parameters like the Denavit-Hartenberg parameters. */ vpViper650::vpViper650() : tool_current(vpViper650::defaultTool), projModel(vpCameraParameters::perspectiveProjWithoutDistortion) { - // Denavit Hartenberg parameters + // Denavit-Hartenberg parameters a1 = 0.075; a2 = 0.270; a3 = 0.090; diff --git a/modules/robot/src/real-robot/viper/vpViper850.cpp b/modules/robot/src/real-robot/viper/vpViper850.cpp index 6d1b1568d2..b0d612fe16 100644 --- a/modules/robot/src/real-robot/viper/vpViper850.cpp +++ b/modules/robot/src/real-robot/viper/vpViper850.cpp @@ -93,14 +93,14 @@ const vpViper850::vpToolType vpViper850::defaultTool = vpViper850::TOOL_PTGREY_F /*! Default constructor. - Sets the specific parameters like the Denavit Hartenberg parameters. + Sets the specific parameters like the Denavit-Hartenberg parameters. */ vpViper850::vpViper850() : tool_current(vpViper850::defaultTool), projModel(vpCameraParameters::perspectiveProjWithoutDistortion) { - // Denavit Hartenberg parameters + // Denavit-Hartenberg parameters a1 = 0.075; a2 = 0.365; a3 = 0.090;