00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
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
00233
00234
00235
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
00325
00326
00327
00328
00329
00330
00331 };
00332
00333
00334 #endif //_IKBASE_H_
00335