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 /********************************************************************************/
CLMBase::relPosition
double relPosition(double reltime, double distance, double acc, double dec, double vmax)
Calculates the relative position reached after the relative time given.
CLMBase::polDeviratives
void polDeviratives()
TLM_points::time
double time
time that it takes to reach the point (from starting position)
Definition: lmBase.h:38
TBlendtrace::m1
TPoint3D m1
Definition: lmBase.h:92
CLMBase::trajectory
TLMtrajectory trajectory
Definition: lmBase.h:161
TLMtrajectory
[LM] linear movement: parameters
Definition: lmBase.h:43
CLMBase::CLMBase
CLMBase()
Definition: lmBase.h:239
TPoint3D::X
double X
Definition: lmBase.h:77
WaitParameterException::WaitParameterException
WaitParameterException()
Definition: lmBase.h:139
CLMBase::movLM2P
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.
TBlendtrace::P1A
TPoint3D P1A
Definition: lmBase.h:88
TPoint3D::Y
double Y
Definition: lmBase.h:78
TBLENDtrajectory::splinepoints
TSplinepoint * splinepoints
Definition: lmBase.h:112
TPoint6D::Al
double Al
Definition: lmBase.h:71
CLMBase::movLM2P4D
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)
TPoint6D::Z
double Z
Definition: lmBase.h:70
TMLMIP::mlm_intermediate_pos
short mlm_intermediate_pos[5]
current position in cartesian units
Definition: lmBase.h:60
TLMtrajectory::points
TLM_points * points
points to be interpolated
Definition: lmBase.h:50
TLMtrajectory::dt
double dt
time elapsed between one step and the next one
Definition: lmBase.h:48
TBLENDtrajectory::blendtrace
TBlendtrace * blendtrace
Definition: lmBase.h:110
TPoint6D::Be
double Be
Definition: lmBase.h:72
TLMtrajectory::arr_tarpos
double * arr_tarpos
target position in cartesian units
Definition: lmBase.h:45
CLMBase::getMaximumLinearVelocity
double getMaximumLinearVelocity() const
TBLENDtrajectory::number_of_splines
short number_of_splines
Definition: lmBase.h:113
CLMBase::calcParameters
void calcParameters(double *arr_actpos, double *arr_tarpos, double vmax)
TM_ENDLESS
#define TM_ENDLESS
timeout symbol for 'endless' waiting
Definition: kmlBase.h:51
CLMBase::initLM
void initLM()
Initialize the parameters for the linear movements.
TBlendtrace::b1
TPoint3D b1
Definition: lmBase.h:90
TLMtrajectory::distance
int distance
distance between target and current position
Definition: lmBase.h:46
CLMBase::moveRobotLinearTo
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.
TBlendtrace::P1B
TPoint3D P1B
Definition: lmBase.h:89
CLMBase::totalTime
double totalTime(double distance, double acc, double dec, double vmax)
Calculates time needed for movement over a distance.
TPoint6D
[LMBLEND] Standard coordinates for a point in space
Definition: lmBase.h:67
TBlendtrace::p1p2n
TPoint3D p1p2n
Definition: lmBase.h:84
CikBase
Definition: ikBase.h:44
TBlendtrace::V1A
TPoint3D V1A
Definition: lmBase.h:86
TBlendtrace::tA
double tA
Definition: lmBase.h:94
CLMBase::moveRobotLinearTo
void moveRobotLinearTo(double x, double y, double z, double phi, double theta, double psi, bool waitUntilReached=true, int waitTimeout=TM_ENDLESS)
CLMBase::getActivatePositionController
bool getActivatePositionController()
Check if the position controller will be activated after the linear movement.
CLMBase::polCoefficients
void polCoefficients()
TSplinepoint::time
double time
Definition: lmBase.h:101
CLMBase::_isInitialized
bool _isInitialized
Definition: lmBase.h:158
TBlendtrace::distBA
double distBA
Definition: lmBase.h:96
TLMtrajectory::derivatives
double ** derivatives
second order derivatives of the polinomes that join the points, in the points
Definition: lmBase.h:52
WaitParameterException
Wait parameter set to false.
Definition: lmBase.h:137
TBlendtrace::V1B
TPoint3D V1B
Definition: lmBase.h:87
ikBase.h
TBLENDtrajectory::number_of_blends
short number_of_blends
Definition: lmBase.h:111
CLMBase::setMaximumLinearVelocity
void setMaximumLinearVelocity(double maximumVelocity)
Exception
Definition: exception.h:79
TLM_points
[LM] linear movement: points to be interpolated
Definition: lmBase.h:36
CLMBase::movLM2PwithL
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)
TPoint3D
Definition: lmBase.h:76
TPoint6D::X
double X
Definition: lmBase.h:68
TBlendtrace
Definition: lmBase.h:83
CLMBase
Linear movement Class.
Definition: lmBase.h:153
TSplinepoint::point
TPoint6D point
Definition: lmBase.h:100
TLM_points::pos
double pos
position of one point to be interpolated (% refer to the total trajectory)
Definition: lmBase.h:37
TBLENDtrajectory::number_of_referencepoints
short number_of_referencepoints
Definition: lmBase.h:109
TPoint6D::Y
double Y
Definition: lmBase.h:69
CLMBase::_activatePositionController
bool _activatePositionController
Definition: lmBase.h:157
CLMBase::setActivatePositionController
void setActivatePositionController(bool activate)
Re-Activate the position controller after the linear movement.
JointSpeedException
Joint speed too high.
Definition: lmBase.h:129
TLMtrajectory::motors
short ** motors
motor position in each point to be interpolated
Definition: lmBase.h:51
TSplinepoint
Definition: lmBase.h:99
CLMBase::fillPoints
void fillPoints(double vmax)
DLLDIR_LM
#define DLLDIR_LM
Definition: dllexport.h:32
TBLENDtrajectory::tEnd
double tEnd
Definition: lmBase.h:115
TBLENDtrajectory
[LMBLEND] Trajectory points
Definition: lmBase.h:107
TPoint3D::Z
double Z
Definition: lmBase.h:79
TLMtrajectory::parameters
short *** parameters
parameters to be sent in the command 'L' packet
Definition: lmBase.h:54
JointSpeedException::JointSpeedException
JointSpeedException()
Definition: lmBase.h:131
exception.h
TBlendtrace::m2
TPoint3D m2
Definition: lmBase.h:93
CLMBase::movLM
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)
CLMBase::blendtrajectory
TBLENDtrajectory blendtrajectory
Definition: lmBase.h:162
CLMBase::checkJointSpeed
bool checkJointSpeed(std::vector< int > lastsolution, std::vector< int > solution, double time)
Checks if the joint speeds are below speed limit.
TBLENDtrajectory::referencepoints
TPoint6D * referencepoints
Definition: lmBase.h:108
TBlendtrace::p2p3n
TPoint3D p2p3n
Definition: lmBase.h:85
TBLENDtrajectory::number_of_splinepoints
short number_of_splinepoints
Definition: lmBase.h:114
TLMtrajectory::time
double time
time that it takes from current position to target position
Definition: lmBase.h:47
TBlendtrace::b2
TPoint3D b2
Definition: lmBase.h:91
CLMBase::splineCoefficients
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.
TBlendtrace::tB
double tB
Definition: lmBase.h:95
CLMBase::_maximumVelocity
double _maximumVelocity
Definition: lmBase.h:156
TLMtrajectory::arr_actpos
double * arr_actpos
current position in cartesian units
Definition: lmBase.h:44
TLMtrajectory::number_of_points
short number_of_points
number of points to interpolate
Definition: lmBase.h:49
TMLMIP
[LM] Store intermediate targets for multiple linear movements
Definition: lmBase.h:59
TLMtrajectory::coefficients
double *** coefficients
coefficients of the polinomes that join the points
Definition: lmBase.h:53
TPoint6D::Ga
double Ga
Definition: lmBase.h:73