Fawkes API Fawkes Development Version

goto_thread.cpp

00001 
00002 /***************************************************************************
00003  *  goto_thread.cpp - Katana goto one-time thread
00004  *
00005  *  Created: Wed Jun 10 11:45:31 2009
00006  *  Copyright  2006-2009  Tim Niemueller [www.niemueller.de]
00007  *
00008  ****************************************************************************/
00009 
00010 /*  This program is free software; you can redistribute it and/or modify
00011  *  it under the terms of the GNU General Public License as published by
00012  *  the Free Software Foundation; either version 2 of the License, or
00013  *  (at your option) any later version.
00014  *
00015  *  This program is distributed in the hope that it will be useful,
00016  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018  *  GNU Library General Public License for more details.
00019  *
00020  *  Read the full text in the LICENSE.GPL file in the doc directory.
00021  */
00022 
00023 #include "goto_thread.h"
00024 
00025 #include <cstdlib>
00026 #include <kniBase.h>
00027 
00028 /** @class KatanaGotoThread "goto_thread.h"
00029  * Katana linear goto thread.
00030  * This thread moves the arm into a specified position.
00031  * @author Tim Niemueller
00032  */
00033 
00034 /** Constructor.
00035  * @param katana katana linear motion base class
00036  * @param logger logger
00037  * @param poll_interval_ms interval in ms between two checks if the
00038  * final position has been reached
00039  */
00040 KatanaGotoThread::KatanaGotoThread(fawkes::RefPtr<CLMBase> katana,
00041                                    fawkes::Logger *logger,
00042                                    unsigned int poll_interval_ms)
00043   : KatanaMotionThread("KatanaGotoThread", katana, logger)
00044 {
00045   __poll_interval_usec = poll_interval_ms * 1000;
00046 }
00047 
00048 
00049 /** Set target position.
00050  * @param x X coordinate relative to base
00051  * @param y Y coordinate relative to base
00052  * @param z Z coordinate relative to base
00053  * @param phi Phi Euler angle of tool
00054  * @param theta Theta Euler angle of tool
00055  * @param psi Psi Euler angle of tool
00056  */
00057 void
00058 KatanaGotoThread::set_target(float x, float y, float z,
00059                              float phi, float theta, float psi)
00060 {
00061   __x     = x;
00062   __y     = y;
00063   __z     = z;
00064   __phi   = phi;
00065   __theta = theta;
00066   __psi   = psi;
00067 }
00068 
00069 
00070 void
00071 KatanaGotoThread::once()
00072 {
00073   try {
00074     // non-blocking call to KNI
00075     _katana->moveRobotTo(__x, __y, __z, __phi, __theta, __psi);
00076   } catch (KNI::NoSolutionException &e) {
00077     _logger->log_warn("KatanaGotoThread", "Initiating goto failed (no solution, ignoring): %s", e.what());
00078     _finished = true;
00079     _error_code = fawkes::KatanaInterface::ERROR_NO_SOLUTION;
00080     return;
00081   } catch (/*KNI*/::Exception &e) {
00082     _logger->log_warn("KatanaGotoThread", "Initiating goto failed (ignoring): %s", e.what());
00083     _finished = true;
00084     _error_code = fawkes::KatanaInterface::ERROR_CMD_START_FAILED;
00085     return;
00086   }
00087 
00088   // Check for finished motion
00089   bool final = false;
00090   short num_motors = _katana->getNumberOfMotors();
00091   CKatBase *base = _katana->GetBase();
00092   const TKatMOT *mot = base->GetMOT();
00093   short num_errors  = 0;
00094 
00095   while (! final) {
00096       usleep(__poll_interval_usec);
00097       final = true;
00098       try {
00099         base->GetSCT()->arr[0].recvDAT(); // update sensor values
00100         base->recvMPS(); // get position for all motors
00101         base->recvGMS(); // get status flags for all motors
00102       } catch (/*KNI*/::Exception &e) {
00103         if (++num_errors <= 10) {
00104           _logger->log_warn("KatanaGripperThread", "Receiving MPS/GMS failed, retrying");
00105           continue;
00106         } else {
00107           _logger->log_warn("KatanaGripperThread", "Receiving MPS/GMS failed too often, aborting");
00108           _error_code = fawkes::KatanaInterface::ERROR_COMMUNICATION;
00109           break;
00110         }
00111       }
00112 
00113       for (int i=0; i < num_motors; ++i) {
00114         if (mot->arr[i].GetPVP()->msf == MSF_MOTCRASHED) {
00115           _error_code = fawkes::KatanaInterface::ERROR_MOTOR_CRASHED;
00116           break;
00117         }
00118         
00119         final &= std::abs(mot->arr[i].GetTPS()->tarpos - mot->arr[i].GetPVP()->pos) < 100;
00120       }
00121     }
00122 
00123     _logger->log_debug(name(), "Position (%f,%f,%f, %f,%f,%f) reached",
00124                        __x, __y, __z, __phi, __theta, __psi);
00125 
00126   _finished = true;
00127 }
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends