KatanaNativeInterface $VERSION$
|
00001 /* 00002 * Katana Native Interface - A C++ interface to the robot arm Katana. 00003 * Copyright (C) 2005 Neuronics AG 00004 * Check out the AUTHORS file for detailed contact information. 00005 * 00006 * This program is free software; you can redistribute it and/or modify 00007 * it under the terms of the GNU General Public License as published by 00008 * the Free Software Foundation; either version 2 of the License, or 00009 * (at your option) any later version. 00010 * 00011 * This program is distributed in the hope that it will be useful, 00012 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00013 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00014 * GNU General Public License for more details. 00015 * 00016 * You should have received a copy of the GNU General Public License 00017 * along with this program; if not, write to the Free Software 00018 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 00019 */ 00020 00021 /********************************************************************************/ 00022 #ifndef _LMBASE_H_ 00023 #define _LMBASE_H_ 00024 /********************************************************************************/ 00025 00026 #include "KNI_InvKin/ikBase.h" 00027 #include "common/exception.h" 00028 #include <vector> 00029 /********************************************************************************/ 00030 00031 00032 //------------------------------------------------------------------------------// 00033 00036 struct TLM_points { 00037 double pos; 00038 double time; 00039 }; 00040 00043 struct TLMtrajectory { 00044 double* arr_actpos; 00045 double* arr_tarpos; 00046 int distance; 00047 double time; 00048 double dt; 00049 short number_of_points; 00050 TLM_points* points; 00051 short** motors; 00052 double** derivatives; 00053 double*** coefficients; 00054 short*** parameters; 00055 }; 00056 00059 struct TMLMIP { 00060 short mlm_intermediate_pos[5]; 00061 }; 00062 00063 00067 struct TPoint6D { 00068 double X; 00069 double Y; 00070 double Z; 00071 double Al; 00072 double Be; 00073 double Ga; 00074 }; 00075 00076 struct TPoint3D { 00077 double X; 00078 double Y; 00079 double Z; 00080 }; 00081 00082 00083 struct TBlendtrace { 00084 TPoint3D p1p2n; 00085 TPoint3D p2p3n; 00086 TPoint3D V1A; 00087 TPoint3D V1B; 00088 TPoint3D P1A; 00089 TPoint3D P1B; 00090 TPoint3D b1; 00091 TPoint3D b2; 00092 TPoint3D m1; 00093 TPoint3D m2; 00094 double tA; 00095 double tB; 00096 double distBA; 00097 }; 00098 00099 struct TSplinepoint { 00100 TPoint6D point; 00101 double time; 00102 }; 00103 00107 struct TBLENDtrajectory { 00108 TPoint6D* referencepoints; 00109 short number_of_referencepoints; 00110 TBlendtrace* blendtrace; 00111 short number_of_blends; 00112 TSplinepoint* splinepoints; 00113 short number_of_splines; 00114 short number_of_splinepoints; 00115 double tEnd; 00116 00117 }; 00118 00119 //---------------------------------------------------------------------------// 00120 00125 00126 00129 class JointSpeedException : public Exception { 00130 public: 00131 JointSpeedException() throw(): 00132 Exception("Joint speed too high", -70) {} 00133 }; 00134 00137 class WaitParameterException : public Exception { 00138 public: 00139 WaitParameterException() throw(): 00140 Exception("Wait parameter set to false", -71) {} 00141 }; 00142 00146 00147 //---------------------------------------------------------------------------// 00148 00153 class DLLDIR_LM CLMBase : public CikBase { 00154 00155 private: 00156 double _maximumVelocity; 00157 bool _activatePositionController; 00158 bool _isInitialized; 00159 00160 00161 TLMtrajectory trajectory; 00162 TBLENDtrajectory blendtrajectory; 00163 00164 00165 00166 void fillPoints(double vmax); 00167 void polDeviratives(); 00168 void polCoefficients(); 00169 00170 void calcParameters(double* arr_actpos, double* arr_tarpos, double vmax); 00171 00172 00183 double totalTime(double distance, double acc, double dec, double vmax); 00184 00196 double relPosition(double reltime, double distance, double acc, double dec, 00197 double vmax); 00198 00215 void splineCoefficients(int steps, double *timearray, double *encoderarray, 00216 double *arr_p1, double *arr_p2, double *arr_p3, double *arr_p4); 00217 00229 bool checkJointSpeed(std::vector<int> lastsolution, 00230 std::vector<int> solution, double time); 00231 00232 // TRetBLEND blend_calculate_blendtraces(double amax, double vmax, long tightness); 00233 // TRetBLEND blend_fill_splinepoints(double kappa, double amax, double vmax); 00234 // double blend_Ivontacc(double tp, double dist, double amax, double vmax); 00235 // TRetBLEND blend_Ivontdec(double tp, double dist, double amax, double vmax, double* I); 00236 00237 public: 00238 00239 CLMBase() : _maximumVelocity(20), _activatePositionController(true), _isInitialized(false) {} 00240 00244 void initLM(); 00245 00247 void movLM(double X, double Y, double Z, 00248 double Al, double Be, double Ga, 00249 bool exactflag, double vmax, bool wait=true, int tolerance = 100, long timeout = TM_ENDLESS); 00250 00254 void movLM2PwithL(double X1, double Y1, double Z1, double Al1, double Be1, 00255 double Ga1, double X2, double Y2, double Z2, double Al2, double Be2, 00256 double Ga2, bool exactflag, double vmax, bool wait=false, 00257 int tolerance = 100, long timeout = TM_ENDLESS); 00258 00259 void movLM2P4D(double X1, double Y1, double Z1, 00260 double Al1, double Be1, double Ga1, 00261 double X2, double Y2, double Z2, 00262 double Al2, double Be2, double Ga2, 00263 bool exactflag, double vmax, bool wait=false, int tolerance = 100, long timeout = TM_ENDLESS); 00264 00291 void movLM2P(double X1, double Y1, double Z1, double Al1, double Be1, 00292 double Ga1, double X2, double Y2, double Z2, double Al2, double Be2, 00293 double Ga2, bool exactflag, double vmax, bool wait=true, 00294 int tolerance = 100, long timeout = TM_ENDLESS); 00295 00296 void setMaximumLinearVelocity(double maximumVelocity); 00297 double getMaximumLinearVelocity() const; 00298 00302 void setActivatePositionController(bool activate); 00303 00306 bool getActivatePositionController(); 00307 00309 void moveRobotLinearTo( 00310 double x, double y, double z, 00311 double phi, double theta, double psi, 00312 bool waitUntilReached = true, 00313 int waitTimeout = TM_ENDLESS); 00314 00319 void moveRobotLinearTo( 00320 std::vector<double> coordinates, 00321 bool waitUntilReached = true, 00322 int waitTimeout = TM_ENDLESS); 00323 00324 // TRetBLEND movLMBlend(double X1, double Y1, double Z1, 00325 // double Al1, double Be1, double Ga1, 00326 // double X2, double Y2, double Z2, 00327 // double Al2, double Be2, double Ga2, 00328 // double vmax, long tightness); 00329 00330 00331 }; 00332 00333 /********************************************************************************/ 00334 #endif //_IKBASE_H_ 00335 /********************************************************************************/