KatanaNativeInterface  $VERSION$
lmBase.h
Go to the documentation of this file.
1 /*
2  * Katana Native Interface - A C++ interface to the robot arm Katana.
3  * Copyright (C) 2005 Neuronics AG
4  * Check out the AUTHORS file for detailed contact information.
5  *
6  * This program is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  *
11  * This program is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with this program; if not, write to the Free Software
18  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19  */
20 
21 /********************************************************************************/
22 #ifndef _LMBASE_H_
23 #define _LMBASE_H_
24 /********************************************************************************/
25 
26 #include "KNI_InvKin/ikBase.h"
27 #include "common/exception.h"
28 #include <vector>
29 /********************************************************************************/
30 
31 
32 //------------------------------------------------------------------------------//
33 
36 struct TLM_points {
37  double pos;
38  double time;
39 };
40 
43 struct TLMtrajectory {
44  double* arr_actpos;
45  double* arr_tarpos;
46  int distance;
47  double time;
48  double dt;
51  short** motors;
52  double** derivatives;
53  double*** coefficients;
54  short*** parameters;
55 };
56 
59 struct TMLMIP {
61 };
62 
63 
67 struct TPoint6D {
68  double X;
69  double Y;
70  double Z;
71  double Al;
72  double Be;
73  double Ga;
74 };
75 
76 struct TPoint3D {
77  double X;
78  double Y;
79  double Z;
80 };
81 
82 
83 struct TBlendtrace {
94  double tA;
95  double tB;
96  double distBA;
97 };
98 
99 struct TSplinepoint {
101  double time;
102 };
103 
115  double tEnd;
116 
117 };
118 
119 //---------------------------------------------------------------------------//
120 
125 
126 
130 public:
132  Exception("Joint speed too high", -70) {}
133 };
134 
138 public:
140  Exception("Wait parameter set to false", -71) {}
141 };
142 
146 
147 //---------------------------------------------------------------------------//
148 
153 class DLLDIR_LM CLMBase : public CikBase {
154 
155  private:
159 
160 
163 
164 
165 
166  void fillPoints(double vmax);
169 
170  void calcParameters(double* arr_actpos, double* arr_tarpos, double vmax);
171 
172 
183  double totalTime(double distance, double acc, double dec, double vmax);
184 
196  double relPosition(double reltime, double distance, double acc, double dec,
197  double vmax);
198 
215  void splineCoefficients(int steps, double *timearray, double *encoderarray,
216  double *arr_p1, double *arr_p2, double *arr_p3, double *arr_p4);
217 
229  bool checkJointSpeed(std::vector<int> lastsolution,
230  std::vector<int> solution, double time);
231 
232 // TRetBLEND blend_calculate_blendtraces(double amax, double vmax, long tightness);
233 // TRetBLEND blend_fill_splinepoints(double kappa, double amax, double vmax);
234 // double blend_Ivontacc(double tp, double dist, double amax, double vmax);
235 // TRetBLEND blend_Ivontdec(double tp, double dist, double amax, double vmax, double* I);
236 
237  public:
238 
239  CLMBase() : _maximumVelocity(20), _activatePositionController(true), _isInitialized(false) {}
240 
244  void initLM();
245 
247  void movLM(double X, double Y, double Z,
248  double Al, double Be, double Ga,
249  bool exactflag, double vmax, bool wait=true, int tolerance = 100, long timeout = TM_ENDLESS);
250 
254  void movLM2PwithL(double X1, double Y1, double Z1, double Al1, double Be1,
255  double Ga1, double X2, double Y2, double Z2, double Al2, double Be2,
256  double Ga2, bool exactflag, double vmax, bool wait=false,
257  int tolerance = 100, long timeout = TM_ENDLESS);
258 
259  void movLM2P4D(double X1, double Y1, double Z1,
260  double Al1, double Be1, double Ga1,
261  double X2, double Y2, double Z2,
262  double Al2, double Be2, double Ga2,
263  bool exactflag, double vmax, bool wait=false, int tolerance = 100, long timeout = TM_ENDLESS);
264 
291  void movLM2P(double X1, double Y1, double Z1, double Al1, double Be1,
292  double Ga1, double X2, double Y2, double Z2, double Al2, double Be2,
293  double Ga2, bool exactflag, double vmax, bool wait=true,
294  int tolerance = 100, long timeout = TM_ENDLESS);
295 
296  void setMaximumLinearVelocity(double maximumVelocity);
297  double getMaximumLinearVelocity() const;
298 
302  void setActivatePositionController(bool activate);
303 
307 
310  double x, double y, double z,
311  double phi, double theta, double psi,
312  bool waitUntilReached = true,
313  int waitTimeout = TM_ENDLESS);
314 
320  std::vector<double> coordinates,
321  bool waitUntilReached = true,
322  int waitTimeout = TM_ENDLESS);
323 
324 // TRetBLEND movLMBlend(double X1, double Y1, double Z1,
325 // double Al1, double Be1, double Ga1,
326 // double X2, double Y2, double Z2,
327 // double Al2, double Be2, double Ga2,
328 // double vmax, long tightness);
329 
330 
331 };
332 
333 /********************************************************************************/
334 #endif //_IKBASE_H_
335 /********************************************************************************/
Linear movement Class.
Definition: lmBase.h:153
void initLM()
Initialize the parameters for the linear movements.
void polCoefficients()
bool _activatePositionController
Definition: lmBase.h:157
TBLENDtrajectory blendtrajectory
Definition: lmBase.h:162
double _maximumVelocity
Definition: lmBase.h:156
double relPosition(double reltime, double distance, double acc, double dec, double vmax)
Calculates the relative position reached after the relative time given.
void setActivatePositionController(bool activate)
Re-Activate the position controller after the linear movement.
void movLM2PwithL(double X1, double Y1, double Z1, double Al1, double Be1, double Ga1, double X2, double Y2, double Z2, double Al2, double Be2, double Ga2, bool exactflag, double vmax, bool wait=false, int tolerance=100, long timeout=TM_ENDLESS)
Old version of movLM2P which uses L-Command (only 4 splines)
void movLM(double X, double Y, double Z, double Al, double Be, double Ga, bool exactflag, double vmax, bool wait=true, int tolerance=100, long timeout=TM_ENDLESS)
bool checkJointSpeed(std::vector< int > lastsolution, std::vector< int > solution, double time)
Checks if the joint speeds are below speed limit.
void movLM2P(double X1, double Y1, double Z1, double Al1, double Be1, double Ga1, double X2, double Y2, double Z2, double Al2, double Be2, double Ga2, bool exactflag, double vmax, bool wait=true, int tolerance=100, long timeout=TM_ENDLESS)
New version of movLM2P with multiple splines.
TLMtrajectory trajectory
Definition: lmBase.h:161
void movLM2P4D(double X1, double Y1, double Z1, double Al1, double Be1, double Ga1, double X2, double Y2, double Z2, double Al2, double Be2, double Ga2, bool exactflag, double vmax, bool wait=false, int tolerance=100, long timeout=TM_ENDLESS)
CLMBase()
Definition: lmBase.h:239
void calcParameters(double *arr_actpos, double *arr_tarpos, double vmax)
bool _isInitialized
Definition: lmBase.h:158
void setMaximumLinearVelocity(double maximumVelocity)
bool getActivatePositionController()
Check if the position controller will be activated after the linear movement.
double getMaximumLinearVelocity() const
void splineCoefficients(int steps, double *timearray, double *encoderarray, double *arr_p1, double *arr_p2, double *arr_p3, double *arr_p4)
Calculates the spline coefficient and stores them in arr_p1 - arr_p4.
void moveRobotLinearTo(double x, double y, double z, double phi, double theta, double psi, bool waitUntilReached=true, int waitTimeout=TM_ENDLESS)
void polDeviratives()
void fillPoints(double vmax)
double totalTime(double distance, double acc, double dec, double vmax)
Calculates time needed for movement over a distance.
void moveRobotLinearTo(std::vector< double > coordinates, bool waitUntilReached=true, int waitTimeout=TM_ENDLESS)
This method does the same as the one above and is mainly provided for convenience.
Definition: ikBase.h:44
Joint speed too high.
Definition: lmBase.h:129
Wait parameter set to false.
Definition: lmBase.h:137
#define DLLDIR_LM
Definition: dllexport.h:32
#define TM_ENDLESS
timeout symbol for 'endless' waiting
Definition: kmlBase.h:51
[LMBLEND] Trajectory points
Definition: lmBase.h:107
short number_of_blends
Definition: lmBase.h:111
short number_of_referencepoints
Definition: lmBase.h:109
short number_of_splines
Definition: lmBase.h:113
TSplinepoint * splinepoints
Definition: lmBase.h:112
short number_of_splinepoints
Definition: lmBase.h:114
TPoint6D * referencepoints
Definition: lmBase.h:108
double tEnd
Definition: lmBase.h:115
TBlendtrace * blendtrace
Definition: lmBase.h:110
double tB
Definition: lmBase.h:95
TPoint3D p2p3n
Definition: lmBase.h:85
TPoint3D b2
Definition: lmBase.h:91
TPoint3D p1p2n
Definition: lmBase.h:84
TPoint3D P1A
Definition: lmBase.h:88
double tA
Definition: lmBase.h:94
TPoint3D b1
Definition: lmBase.h:90
TPoint3D V1A
Definition: lmBase.h:86
double distBA
Definition: lmBase.h:96
TPoint3D m2
Definition: lmBase.h:93
TPoint3D m1
Definition: lmBase.h:92
TPoint3D P1B
Definition: lmBase.h:89
TPoint3D V1B
Definition: lmBase.h:87
[LM] linear movement: points to be interpolated
Definition: lmBase.h:36
double pos
position of one point to be interpolated (% refer to the total trajectory)
Definition: lmBase.h:37
double time
time that it takes to reach the point (from starting position)
Definition: lmBase.h:38
[LM] linear movement: parameters
Definition: lmBase.h:43
short number_of_points
number of points to interpolate
Definition: lmBase.h:49
int distance
distance between target and current position
Definition: lmBase.h:46
double *** coefficients
coefficients of the polinomes that join the points
Definition: lmBase.h:53
short ** motors
motor position in each point to be interpolated
Definition: lmBase.h:51
TLM_points * points
points to be interpolated
Definition: lmBase.h:50
double time
time that it takes from current position to target position
Definition: lmBase.h:47
short *** parameters
parameters to be sent in the command 'L' packet
Definition: lmBase.h:54
double * arr_actpos
current position in cartesian units
Definition: lmBase.h:44
double * arr_tarpos
target position in cartesian units
Definition: lmBase.h:45
double dt
time elapsed between one step and the next one
Definition: lmBase.h:48
double ** derivatives
second order derivatives of the polinomes that join the points, in the points
Definition: lmBase.h:52
[LM] Store intermediate targets for multiple linear movements
Definition: lmBase.h:59
short mlm_intermediate_pos[5]
current position in cartesian units
Definition: lmBase.h:60
double Y
Definition: lmBase.h:78
double Z
Definition: lmBase.h:79
double X
Definition: lmBase.h:77
[LMBLEND] Standard coordinates for a point in space
Definition: lmBase.h:67
double Z
Definition: lmBase.h:70
double Y
Definition: lmBase.h:69
double Be
Definition: lmBase.h:72
double Ga
Definition: lmBase.h:73
double Al
Definition: lmBase.h:71
double X
Definition: lmBase.h:68
TPoint6D point
Definition: lmBase.h:100
double time
Definition: lmBase.h:101