Fawkes API Fawkes Development Version
|
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 }