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);
167  void polDeviratives();
168  void polCoefficients();
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 
306  bool getActivatePositionController();
307 
309  void moveRobotLinearTo(
310  double x, double y, double z,
311  double phi, double theta, double psi,
312  bool waitUntilReached = true,
313  int waitTimeout = TM_ENDLESS);
314 
319  void moveRobotLinearTo(
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 /********************************************************************************/
TPoint3D b1
Definition: lmBase.h:90
double tA
Definition: lmBase.h:94
double dt
time elapsed between one step and the next one
Definition: lmBase.h:48
TPoint3D b2
Definition: lmBase.h:91
CLMBase()
Definition: lmBase.h:239
[LM] linear movement: parameters
Definition: lmBase.h:43
TPoint3D p1p2n
Definition: lmBase.h:84
double ** derivatives
second order derivatives of the polinomes that join the points, in the points
Definition: lmBase.h:52
short *** parameters
parameters to be sent in the command &#39;L&#39; packet
Definition: lmBase.h:54
double Ga
Definition: lmBase.h:73
short number_of_splines
Definition: lmBase.h:113
double pos
position of one point to be interpolated (% refer to the total trajectory)
Definition: lmBase.h:37
TSplinepoint * splinepoints
Definition: lmBase.h:112
double X
Definition: lmBase.h:77
TLM_points * points
points to be interpolated
Definition: lmBase.h:50
#define TM_ENDLESS
timeout symbol for &#39;endless&#39; waiting
Definition: kmlBase.h:51
TBlendtrace * blendtrace
Definition: lmBase.h:110
double Y
Definition: lmBase.h:78
TPoint3D V1A
Definition: lmBase.h:86
int distance
distance between target and current position
Definition: lmBase.h:46
double _maximumVelocity
Definition: lmBase.h:156
TPoint3D m1
Definition: lmBase.h:92
Wait parameter set to false.
Definition: lmBase.h:137
TBLENDtrajectory blendtrajectory
Definition: lmBase.h:162
TPoint3D V1B
Definition: lmBase.h:87
#define DLLDIR_LM
Definition: dllexport.h:32
[LMBLEND] Standard coordinates for a point in space
Definition: lmBase.h:67
short number_of_blends
Definition: lmBase.h:111
double Y
Definition: lmBase.h:69
TPoint6D point
Definition: lmBase.h:100
double Be
Definition: lmBase.h:72
Definition: ikBase.h:44
double * arr_actpos
current position in cartesian units
Definition: lmBase.h:44
TPoint3D p2p3n
Definition: lmBase.h:85
double distBA
Definition: lmBase.h:96
double time
time that it takes from current position to target position
Definition: lmBase.h:47
Joint speed too high.
Definition: lmBase.h:129
double tB
Definition: lmBase.h:95
double Z
Definition: lmBase.h:79
[LM] Store intermediate targets for multiple linear movements
Definition: lmBase.h:59
double Al
Definition: lmBase.h:71
TLMtrajectory trajectory
Definition: lmBase.h:161
double *** coefficients
coefficients of the polinomes that join the points
Definition: lmBase.h:53
TPoint3D m2
Definition: lmBase.h:93
Linear movement Class.
Definition: lmBase.h:153
[LM] linear movement: points to be interpolated
Definition: lmBase.h:36
short number_of_referencepoints
Definition: lmBase.h:109
double Z
Definition: lmBase.h:70
short ** motors
motor position in each point to be interpolated
Definition: lmBase.h:51
short number_of_points
number of points to interpolate
Definition: lmBase.h:49
double time
Definition: lmBase.h:101
double tEnd
Definition: lmBase.h:115
TPoint3D P1B
Definition: lmBase.h:89
TPoint3D P1A
Definition: lmBase.h:88
bool _isInitialized
Definition: lmBase.h:158
double * arr_tarpos
target position in cartesian units
Definition: lmBase.h:45
short number_of_splinepoints
Definition: lmBase.h:114
double time
time that it takes to reach the point (from starting position)
Definition: lmBase.h:38
TPoint6D * referencepoints
Definition: lmBase.h:108
double X
Definition: lmBase.h:68
short mlm_intermediate_pos[5]
current position in cartesian units
Definition: lmBase.h:60
[LMBLEND] Trajectory points
Definition: lmBase.h:107
bool _activatePositionController
Definition: lmBase.h:157