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
36struct TLM_points {
37 double pos;
38 double time;
39};
40
44 double* arr_actpos;
45 double* arr_tarpos;
47 double time;
48 double dt;
51 short** motors;
52 double** derivatives;
53 double*** coefficients;
54 short*** parameters;
55};
56
59struct TMLMIP {
61};
62
63
67struct TPoint6D {
68 double X;
69 double Y;
70 double Z;
71 double Al;
72 double Be;
73 double Ga;
74};
75
76struct TPoint3D {
77 double X;
78 double Y;
79 double Z;
80};
81
82
94 double tA;
95 double tB;
96 double distBA;
97};
98
101 double time;
102};
103
115 double tEnd;
116
117};
118
119//---------------------------------------------------------------------------//
120
125
126
130public:
132 Exception("Joint speed too high", -70) {}
133};
134
138public:
140 Exception("Wait parameter set to false", -71) {}
141};
142
146
147//---------------------------------------------------------------------------//
148
153class 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);
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