From ccce0ba33d83777946826a8709cca62dc42ead56 Mon Sep 17 00:00:00 2001 From: Ugo Pattacini Date: Sat, 26 Nov 2022 12:27:56 +0100 Subject: [PATCH] clean up of cartesian and gaze interfaces --- doc/anyrobot_cartesian_interface.dox | 14 +- doc/cartesian_interface.dox | 37 +- doc/doc.dox | 1 - doc/gaze_interface.dox | 20 +- src/CMakeLists.txt | 1 - src/motorControlAdvanced/CMakeLists.txt | 19 - .../tutorial_cartesian_interface.cpp | 277 -------------- .../tutorial_gaze_interface.cpp | 345 ------------------ 8 files changed, 21 insertions(+), 693 deletions(-) delete mode 100644 src/motorControlAdvanced/CMakeLists.txt delete mode 100644 src/motorControlAdvanced/tutorial_cartesian_interface.cpp delete mode 100644 src/motorControlAdvanced/tutorial_gaze_interface.cpp diff --git a/doc/anyrobot_cartesian_interface.dox b/doc/anyrobot_cartesian_interface.dox index 784dcdd..7aeb4f5 100644 --- a/doc/anyrobot_cartesian_interface.dox +++ b/doc/anyrobot_cartesian_interface.dox @@ -41,7 +41,7 @@ performances. Here follow the details. \n as much as possible since a lag in the control loop will inevitably cause for instance unwanted overshoots in the response. As final suggestion, consider placing the server physically close to the robot, that is where fast ethernet link ends (and yarp too) and the internal robot bus begins (e.g. CAN); for iCub indeed, the server runs aboard the - PC104. + icub-head. - The client simply lives inside the user code, making queries to the server through yarp ports, thus it never speaks to the solver. There are no special needs for it: the location of the client depends only on the requirements of the user code; a program that opens up a client may run on a shuffle PC so as on a powerful machine or even aboard the @@ -50,8 +50,8 @@ performances. Here follow the details. \n \section sec_customcart_dependencies Dependencies -Besides the "classical" dependecies you need to have in order to compile all the components (Ipopt, -\ref iKin, \ref servercartesiancontroller, \ref clientcartesiancontroller; you should know them very well by now :), +Besides the "classical" dependecies you need to have in order to compile all the components (Ipopt, yarp, icub-main, +iKin, servercartesiancontroller, clientcartesiancontroller; you should know them very well by now :), you are required to accomplish a preliminary job: it can be tedious (I realize it when I had to write the "fake" stuff :) but is mandatory. You have to provide some low-level yarp motor interfaces that are necessary for the cartesian components to start and operate correctly. They are only three: IControlLimits, IEncoders, IVelocityControl. @@ -65,7 +65,7 @@ Moreover, as enhanced option, the server is also capable of automatically detect interface, through which the low-level velocity control is replaced by the faster and more accurate streaming position control. In this latter modality, also the standard IPositionControl interface is required to be implemented. \n This is not the right place where to tell about how to deal with these interfaces, but please find out more on the topic -of making a new device in yarp here. +of making a new device in yarp. \section sec_customcart_custom_example Example of Customization @@ -108,9 +108,9 @@ link_0 (A 1.0) (D 0.0) (alpha 0.0) (offset 0.0) (min -180.0) (max 180.0) link_1 (A 1.0) (D 0.0) (alpha 0.0) (offset 0.0) (min -180.0) (max 180.0) link_2 (A 1.0) (D 0.0) (alpha 0.0) (offset 0.0) (min -180.0) (max 180.0) \endcode -The meaning of these parameters is described in the \ref iKinLink class; at any rate they are easily recognized as the link length -(A in [m]), the link offset (D in [m]), the link twist (alpha in [rad]) and so on: please visit the wiki -page on kinematics for a deeper insight. \n +The meaning of these parameters is described in the iKinLink class; at any rate they are easily recognized as the link length +(A in [m]), the link offset (D in [m]), the link twist (alpha in [rad]) and so on: please visit the +documentation on kinematics for a deeper insight. \n A careful reader should have not missed the expression for the joints limits, which here can definitely take whatever values for all the three joints, much different from the bounds assigned to the real joints and hard-coded within the fake motor device. Do not worry about that: at start-up the solver will query the low-level interface (namely IControlLimits) about the actual diff --git a/doc/cartesian_interface.dox b/doc/cartesian_interface.dox index fe035ea..f65564a 100644 --- a/doc/cartesian_interface.dox +++ b/doc/cartesian_interface.dox @@ -24,23 +24,21 @@ Alternatively, the client can act as a pure wrapper over the solver coping with meeting some specific requirements. However, also in this configuration, the server part needs to be launched (even if not employed) as a gateway to let the client communicate with the solver. -\note A paper that describes how the controller works can be downloaded here. -If you're going to use this controller for your work, please quote it within any resulting publication: U. Pattacini, F. Nori, L. Natale, G. Metta, G. Sandini, -"An Experimental Evaluation of a Novel Minimum-Jerk Cartesian Controller for Humanoid Robots", IEEE/RSJ International Conference on Intelligent Robots and Systems, -Taipei, Taiwan, 2010. +\note If you're going to use this controller for your work, please quote it within any resulting publication: +U. Pattacini, F. Nori, L. Natale, G. Metta, G. Sandini, "An Experimental Evaluation of a Novel Minimum-Jerk Cartesian Controller for Humanoid Robots", +IEEE/RSJ International Conference on Intelligent Robots and Systems, Taipei, Taiwan, 2010. \section sec_cart_dependencies Dependencies In order to use the Cartesian Interface, make sure that the following steps are done: \n -[Note that the term cluster refers to the set of computers directly connected to the robot network, whereas PC104 +[Note that the term cluster refers to the set of computers directly connected to the robot network, whereas icub-head indicates the hub board mounted on the robot] --# Update YARP and iCub repositories and compile them (always a good practice). --# Install Ipopt on the cluster. +-# Install the required software dependencies. -# On the cluster: compile the repository with the switch ENABLE_icubmod_cartesiancontrollerclient enabled. This will make the client part of the interface available. --# On PC104: compile the repository with the switch ENABLE_icubmod_cartesiancontrollerserver enabled. This will make the +-# On icub-head: compile the repository with the switch ENABLE_icubmod_cartesiancontrollerserver enabled. This will make the server part of the interface available on the hub. @@ -49,7 +47,7 @@ indicates the hub board mounted on the robot] Henceforth rely on the installed copy of $ICUB_ROOT/main/app/iCubStartup/scripts/iCubStartup.xml.template application to launch both the Cartesian solvers (and other useful tools as well). \n The \ref iKinCartesianSolver "Cartesian Solvers" are required to invert the kinematics using Ipopt and they are meant to run -on the cluster to avoid overloading the PC104 core. Make sure that the machines where the solvers will be running are configured +on the cluster to avoid overloading the icub-head core. Make sure that the machines where the solvers will be running are configured properly to work with the specific robot (e.g. the YARP_ROBOT_NAME correctly points to the name of the robot) in order to load at start-up the robot dependent kinematics. \n The solvers need the robotInterface to connect to the robot to get useful data to operate (e.g. the joints bounds) and also the @@ -101,7 +99,7 @@ Central for a correct usage of the Cartesian controller is the knowledge of the that shall be adopted to describe any desired limb pose. Positions (in meters) and orientation refer to the root frame attached to the waist as in the -wiki page. \n +documentation. \n The root x-axis points backward, the y-axis points rightward, while the z-axis points upward. To specify a target orientation of the end-effector, you have to provide three components for the rotation @@ -147,7 +145,7 @@ Vector o=yarp::math::dcm2axis(R); \section sec_cart_useinterface Using the Cartesian Interface -The YARP Cartesian Interface is fully documented here. +The YARP documentation contains a full description of Cartesian Interface. Moreover, some examples are also reported hereafter to gain a deeper insight into the Cartesian device. Imagine that we want to get the actual arm pose from within our code. What we have to write is just: @@ -479,19 +477,4 @@ public: } }; \endcode - - -\section sec_cart_siminterface The Simulator and the Cartesian Interface - -To have the Cartesian Interface fully operative together with the robot simulator, follow these steps: - --# Launch the \ref icub_Simulation "iCub Simulator". --# Deploy the Cartesian servers: yarprobotinterface \--context simCartesianControl --# Launch the \ref iKinCartesianSolver "Cartesian Solvers" for the required limbs: have a look to the template located in - the directory icub-main/app/simCartesianControl/scripts. - -Note that you will need to compile the server part of the interface also on the cluster where the simulator is supposed to run. - -A working example is available under src/motorControlAdvanced/tutorial_cartesian_interface.cpp which makes the left hand follow -a circular trajectory located in front of the simulated robot. -*/ +*/ \ No newline at end of file diff --git a/doc/doc.dox b/doc/doc.dox index 3e3675e..738498e 100644 --- a/doc/doc.dox +++ b/doc/doc.dox @@ -19,7 +19,6 @@ A list of tutorials on various topics from basic compilation, visual processing, - \ref icub_motor_control_tutorial - a tutorial on how to use the motor interfaces - \ref icub_impedance_control_tutorial - a tutorial on how to use the joint level torque/impedance interface - \ref icub_basic_image_processing - a tutorial on a basic image processing - - \ref icub_periodic_thread - a tutorial on how to write a control loop using threads - \ref icub_cartesian_interface - a tutorial on how to control a robot's limb in the operational space - \ref icub_gaze_interface - a tutorial on how to control the robot gaze through a YARP interface diff --git a/doc/gaze_interface.dox b/doc/gaze_interface.dox index ae9b65f..e135bb4 100644 --- a/doc/gaze_interface.dox +++ b/doc/gaze_interface.dox @@ -24,8 +24,7 @@ Roncone A., Pattacini U., Metta G. & Natale L., "A Cartesian 6-DoF Gaze Controll In order to use the Gaze Interface, make sure that the following steps are done: --# Update YARP and iCub repositories and compile them (always a good practice). --# Install Ipopt. +-# Install the required software dependencies. -# Compile the iCub repository with the switch ENABLE_icubmod_gazecontrollerclient enabled: this will make the client part of the interface available. The server part is represented by the module \ref iKinGazeCtrl itself. @@ -70,12 +69,12 @@ clientGazeCtrl.close(); \section sec_gaze_useinterface Using the Gaze Interface -The YARP Gaze Interface is fully documented here. +The YARP documentation contains a full description of Gaze Interface. It makes use of three different coordinate systems that enable the user to control the robot's gaze as detailed hereafter. \subsection subsec_gaze_cartcoorsystem Expressing the fixation point in Cartesian Coordinates This coordinate system is the same as the one the \ref icub_cartesian_interface "Cartesian Interface" relies on and complies with the -wiki specifications; it lets the user to give the target +documentation; it lets the user to give the target location where to gaze at with respect to the root reference frame attached to the robot's waist. The following snippet of code shows how to command the gaze in the Cartesian space and to retrieve the current configuration. @@ -355,15 +354,4 @@ public: } }; \endcode - - -\section sec_gaze_siminterface The Simulator and the Gaze Interface - -The Gaze Interface is already fully operative with the robot simulator: you only need to select the proper configuration file. \n -To do that, type the following: -\code -iKinGazeCtrl --from configSim.ini -\endcode - -A working example is available here: src/motorControlAdvanced/tutorial_gaze_interface.cpp -*/ +*/ \ No newline at end of file diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index fe5d939..ffbd99d 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -17,7 +17,6 @@ include(CTest) # go by subdirectories add_subdirectory(imageProcessing) add_subdirectory(motorControlBasic) -add_subdirectory(motorControlAdvanced) add_subdirectory(motorControlImpedance) add_subdirectory(periodicThread) add_subdirectory(ctrlLib) diff --git a/src/motorControlAdvanced/CMakeLists.txt b/src/motorControlAdvanced/CMakeLists.txt deleted file mode 100644 index 195db5a..0000000 --- a/src/motorControlAdvanced/CMakeLists.txt +++ /dev/null @@ -1,19 +0,0 @@ -# Copyright: 2012 iCub Facility, Istituto Italiano di Tecnologia -# Author: Lorenzo Natale -# CopyPolicy: Released under the terms of the GNU GPL v2.0. -# - -cmake_minimum_required(VERSION 3.5) -project(motorControlAdvanced) - -find_package(YARP) - -add_executable(tutorial_cartesian_interface tutorial_cartesian_interface.cpp) -target_compile_definitions(tutorial_cartesian_interface PRIVATE _USE_MATH_DEFINES) -target_link_libraries(tutorial_cartesian_interface ${YARP_LIBRARIES}) -install(TARGETS tutorial_cartesian_interface DESTINATION bin) - -add_executable(tutorial_gaze_interface tutorial_gaze_interface.cpp) -target_compile_definitions(tutorial_gaze_interface PRIVATE _USE_MATH_DEFINES) -target_link_libraries(tutorial_gaze_interface ${YARP_LIBRARIES}) -install(TARGETS tutorial_gaze_interface DESTINATION bin) diff --git a/src/motorControlAdvanced/tutorial_cartesian_interface.cpp b/src/motorControlAdvanced/tutorial_cartesian_interface.cpp deleted file mode 100644 index 657c3d4..0000000 --- a/src/motorControlAdvanced/tutorial_cartesian_interface.cpp +++ /dev/null @@ -1,277 +0,0 @@ -// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- -// -// A tutorial on how to use the Cartesian Interface to control a limb -// in the operational space. -// -// Author: Ugo Pattacini - - -#include -#include - -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#define CTRL_THREAD_PER 0.02 // [s] -#define PRINT_STATUS_PER 1.0 // [s] -#define MAX_TORSO_PITCH 30.0 // [deg] - -using namespace std; -using namespace yarp::os; -using namespace yarp::dev; -using namespace yarp::sig; -using namespace yarp::math; - - -class CtrlThread: public PeriodicThread, - public CartesianEvent -{ -protected: - PolyDriver client; - ICartesianControl *icart; - - Vector xd; - Vector od; - - int startup_context_id; - - double t; - double t0; - double t1; - - // the event callback attached to the "motion-ongoing" - virtual void cartesianEventCallback() - { - fprintf(stdout,"20%% of trajectory attained\n"); - } - -public: - CtrlThread(const double period) : PeriodicThread(period) - { - // we wanna raise an event each time the arm is at 20% - // of the trajectory (or 80% far from the target) - cartesianEventParameters.type="motion-ongoing"; - cartesianEventParameters.motionOngoingCheckPoint=0.2; - } - - virtual bool threadInit() - { - // open a client interface to connect to the cartesian server of the simulator - // we suppose that: - // - // 1 - the iCub simulator is running - // (launch: iCub_SIM) - // - // 2 - the cartesian server is running - // (launch: yarprobotinterface --context simCartesianControl) - // - // 3 - the cartesian solver for the left arm is running too - // (launch: iKinCartesianSolver --context simCartesianControl --part left_arm) - // - Property option("(device cartesiancontrollerclient)"); - option.put("remote","/icubSim/cartesianController/left_arm"); - option.put("local","/cartesian_client/left_arm"); - - if (!client.open(option)) - return false; - - // open the view - client.view(icart); - - // latch the controller context in order to preserve - // it after closing the module - // the context contains the dofs status, the tracking mode, - // the resting positions, the limits and so on. - icart->storeContext(&startup_context_id); - - // set trajectory time - icart->setTrajTime(1.0); - - // get the torso dofs - Vector newDof, curDof; - icart->getDOF(curDof); - newDof=curDof; - - // enable the torso yaw and pitch - // disable the torso roll - newDof[0]=1; - newDof[1]=0; - newDof[2]=1; - - // send the request for dofs reconfiguration - icart->setDOF(newDof,curDof); - - // impose some restriction on the torso pitch - limitTorsoPitch(); - - // print out some info about the controller - Bottle info; - icart->getInfo(info); - fprintf(stdout,"info = %s\n",info.toString().c_str()); - - // register the event, attaching the callback - icart->registerEvent(*this); - - xd.resize(3); - od.resize(4); - - return true; - } - - virtual void afterStart(bool s) - { - if (s) - fprintf(stdout,"Thread started successfully\n"); - else - fprintf(stdout,"Thread did not start\n"); - - t=t0=t1=Time::now(); - } - - virtual void run() - { - t=Time::now(); - - generateTarget(); - - // go to the target :) - // (in streaming) - icart->goToPose(xd,od); - - // some verbosity - printStatus(); - } - - virtual void threadRelease() - { - // we require an immediate stop - // before closing the client for safety reason - icart->stopControl(); - - // it's a good rule to restore the controller - // context as it was before opening the module - icart->restoreContext(startup_context_id); - - client.close(); - } - - void generateTarget() - { - // translational target part: a circular trajectory - // in the yz plane centered in [-0.3,-0.1,0.1] with radius=0.1 m - // and frequency 0.1 Hz - xd[0]=-0.3; - xd[1]=-0.1+0.1*cos(2.0*M_PI*0.1*(t-t0)); - xd[2]=+0.1+0.1*sin(2.0*M_PI*0.1*(t-t0)); - - // we keep the orientation of the left arm constant: - // we want the middle finger to point forward (end-effector x-axis) - // with the palm turned down (end-effector y-axis points leftward); - // to achieve that it is enough to rotate the root frame of pi around z-axis - od[0]=0.0; od[1]=0.0; od[2]=1.0; od[3]=M_PI; - } - - void limitTorsoPitch() - { - int axis=0; // pitch joint - double min, max; - - // sometimes it may be helpful to reduce - // the range of variability of the joints; - // for example here we don't want the torso - // to lean out more than 30 degrees forward - - // we keep the lower limit - icart->getLimits(axis,&min,&max); - icart->setLimits(axis,min,MAX_TORSO_PITCH); - } - - void printStatus() - { - if (t-t1>=PRINT_STATUS_PER) - { - Vector x,o,xdhat,odhat,qdhat; - - // we get the current arm pose in the - // operational space - icart->getPose(x,o); - - // we get the final destination of the arm - // as found by the solver: it differs a bit - // from the desired pose according to the tolerances - icart->getDesired(xdhat,odhat,qdhat); - - double e_x=norm(xdhat-x); - double e_o=norm(odhat-o); - - fprintf(stdout,"+++++++++\n"); - fprintf(stdout,"xd [m] = %s\n",xd.toString().c_str()); - fprintf(stdout,"xdhat [m] = %s\n",xdhat.toString().c_str()); - fprintf(stdout,"x [m] = %s\n",x.toString().c_str()); - fprintf(stdout,"od [rad] = %s\n",od.toString().c_str()); - fprintf(stdout,"odhat [rad] = %s\n",odhat.toString().c_str()); - fprintf(stdout,"o [rad] = %s\n",o.toString().c_str()); - fprintf(stdout,"norm(e_x) [m] = %g\n",e_x); - fprintf(stdout,"norm(e_o) [rad] = %g\n",e_o); - fprintf(stdout,"---------\n\n"); - - t1=t; - } - } -}; - - - -class CtrlModule: public RFModule -{ -protected: - CtrlThread *thr; - -public: - virtual bool configure(ResourceFinder &rf) - { - thr=new CtrlThread(CTRL_THREAD_PER); - if (!thr->start()) - { - delete thr; - return false; - } - - return true; - } - - virtual bool close() - { - thr->stop(); - delete thr; - - return true; - } - - virtual double getPeriod() { return 1.0; } - virtual bool updateModule() { return true; } -}; - - - -int main() -{ - Network yarp; - if (!yarp.checkNetwork()) - { - fprintf(stdout,"Error: yarp server does not seem available\n"); - return 1; - } - - CtrlModule mod; - - ResourceFinder rf; - return mod.runModule(rf); -} diff --git a/src/motorControlAdvanced/tutorial_gaze_interface.cpp b/src/motorControlAdvanced/tutorial_gaze_interface.cpp deleted file mode 100644 index b86872d..0000000 --- a/src/motorControlAdvanced/tutorial_gaze_interface.cpp +++ /dev/null @@ -1,345 +0,0 @@ -// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- -// -// A tutorial on how to use the Gaze Interface. -// -// Author: Ugo Pattacini - - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#define CTRL_THREAD_PER 0.02 // [s] -#define PRINT_STATUS_PER 1.0 // [s] -#define STORE_POI_PER 2.0 // [s] -#define SWITCH_STATE_PER 10.0 // [s] - -#define STATE_TRACK 0 -#define STATE_RECALL 1 -#define STATE_WAIT 2 -#define STATE_STILL 3 - -using namespace std; -using namespace yarp::os; -using namespace yarp::dev; -using namespace yarp::sig; -using namespace yarp::math; - - -class CtrlThread: public PeriodicThread, - public GazeEvent -{ -protected: - PolyDriver clientGaze; - PolyDriver clientTorso; - IGazeControl *igaze; - IEncoders *ienc; - IPositionControl *ipos; - - int state; - int startup_context_id; - - Vector fp; - - deque poiList; - mutex mtx; - - double t; - double t0; - double t1; - double t2; - double t3; - - // the motion-done callback - virtual void gazeEventCallback() - { - lock_guard lg(mtx); - - Vector ang; - igaze->getAngles(ang); - - fprintf(stdout,"Actual gaze configuration: (%s) [deg]\n", - ang.toString(3,3).c_str()); - - fprintf(stdout,"Moving the torso; check if the gaze is compensated ...\n"); - - // move the torso yaw - double val; - ienc->getEncoder(0,&val); - ipos->positionMove(0,val>0.0?-30.0:30.0); - - // detach the callback - igaze->unregisterEvent(*this); - - // switch state - state=STATE_STILL; - } - -public: - CtrlThread(const double period) : PeriodicThread(period) - { - // here we specify that the event we are interested in is - // of type "motion-done" - gazeEventParameters.type="motion-done"; - } - - virtual bool threadInit() - { - // open a client interface to connect to the gaze server - // we suppose that: - // 1 - the iCub simulator is running; - // 2 - the gaze server iKinGazeCtrl is running and - // launched with the following options: "--from configSim.ini" - Property optGaze("(device gazecontrollerclient)"); - optGaze.put("remote","/iKinGazeCtrl"); - optGaze.put("local","/gaze_client"); - - if (!clientGaze.open(optGaze)) - return false; - - // open the view - clientGaze.view(igaze); - - // latch the controller context in order to preserve - // it after closing the module - // the context contains the tracking mode, the neck limits and so on. - igaze->storeContext(&startup_context_id); - - // set trajectory time: - igaze->setNeckTrajTime(0.8); - igaze->setEyesTrajTime(0.4); - - // put the gaze in tracking mode, so that - // when the torso moves, the gaze controller - // will compensate for it - igaze->setTrackingMode(true); - - // print out some info about the controller - Bottle info; - igaze->getInfo(info); - fprintf(stdout,"info = %s\n",info.toString().c_str()); - - Property optTorso("(device remote_controlboard)"); - optTorso.put("remote","/icubSim/torso"); - optTorso.put("local","/torso_client"); - - if (!clientTorso.open(optTorso)) - return false; - - // open the view - clientTorso.view(ienc); - clientTorso.view(ipos); - ipos->setRefSpeed(0,10.0); - - fp.resize(3); - - state=STATE_TRACK; - - t=t0=t1=t2=t3=Time::now(); - - return true; - } - - virtual void afterStart(bool s) - { - if (s) - fprintf(stdout,"Thread started successfully\n"); - else - fprintf(stdout,"Thread did not start\n"); - } - - virtual void run() - { - lock_guard lg(mtx); - - t=Time::now(); - generateTarget(); - - if (state==STATE_TRACK) - { - // look at the target (streaming) - igaze->lookAtFixationPoint(fp); - - // some verbosity - printStatus(); - - // we collect from time to time - // some interesting points (POI) - // where to look at soon afterwards - storeInterestingPoint(); - - if (t-t2>=SWITCH_STATE_PER) - { - // switch state - state=STATE_RECALL; - } - } - - if (state==STATE_RECALL) - { - // pick up the first POI - // and clear the list - Vector ang=poiList.front(); - poiList.clear(); - - fprintf(stdout,"Retrieving POI #0 ... (%s) [deg]\n", - ang.toString(3,3).c_str()); - - // register the motion-done event, attaching the callback - // that will be executed as soon as the gaze is accomplished - igaze->registerEvent(*this); - - // look at the chosen POI - igaze->lookAtAbsAngles(ang); - - // switch state - state=STATE_WAIT; - } - - if (state==STATE_STILL) - { - // check if the torso movement is complete - bool done; - ipos->checkMotionDone(0,&done); - if (done) - { - fprintf(stdout,"done\n"); - - t1=t2=t3=t; - - // switch state - state=STATE_TRACK; - } - } - } - - virtual void threadRelease() - { - // we require an immediate stop - // before closing the client for safety reason - igaze->stopControl(); - - // it's a good rule to restore the controller - // context as it was before opening the module - igaze->restoreContext(startup_context_id); - - clientGaze.close(); - clientTorso.close(); - } - - void generateTarget() - { - // translational target part: a circular trajectory - // in the yz plane centered in [-0.5,0.0,0.3] with radius=0.1 m - // and frequency 0.1 Hz - fp[0]=-0.5; - fp[1]=+0.0+0.1*cos(2.0*M_PI*0.1*(t-t0)); - fp[2]=+0.3+0.1*sin(2.0*M_PI*0.1*(t-t0)); - } - - void storeInterestingPoint() - { - if (t-t3>=STORE_POI_PER) - { - Vector ang; - - // we store the current azimuth, elevation - // and vergence wrt the absolute reference frame - // The absolute reference frame for the azimuth/elevation couple - // is head-centered with the robot in rest configuration - // (i.e. torso and head angles zeroed). - igaze->getAngles(ang); - - int numPOI=(int)poiList.size(); - fprintf(stdout,"Storing POI #%d: (%s) [deg]\n", - numPOI,ang.toString(3,3).c_str()); - - poiList.push_back(ang); - - t3=t; - } - } - - void printStatus() - { - if (t-t1>=PRINT_STATUS_PER) - { - // we get the current fixation point in the - // operational space - Vector x; - igaze->getFixationPoint(x); - - fprintf(stdout,"+++++++++\n"); - fprintf(stdout,"fp [m] = (%s)\n",fp.toString(3,3).c_str()); - fprintf(stdout,"x [m] = (%s)\n",x.toString(3,3).c_str()); - fprintf(stdout,"norm(fp-x) [m] = %g\n",norm(fp-x)); - fprintf(stdout,"---------\n\n"); - - t1=t; - } - } -}; - - - -class CtrlModule: public RFModule -{ -protected: - CtrlThread *thr; - -public: - virtual bool configure(ResourceFinder &rf) - { - thr=new CtrlThread(CTRL_THREAD_PER); - if (!thr->start()) - { - delete thr; - return false; - } - - return true; - } - - virtual bool close() - { - thr->stop(); - delete thr; - - return true; - } - - virtual double getPeriod() { return 1.0; } - virtual bool updateModule() { return true; } -}; - - - -int main(int argc, char *argv[]) -{ - Network yarp; - if (!yarp.checkNetwork()) - { - fprintf(stdout,"Error: yarp server does not seem available\n"); - return 1; - } - - CtrlModule mod; - - ResourceFinder rf; - return mod.runModule(rf); -} - - -