orocos_kdl
velocityprofile_traphalf.cpp
Go to the documentation of this file.
1 /***************************************************************************
2  tag: Erwin Aertbelien Mon May 10 19:10:36 CEST 2004 velocityprofile_traphalf.cxx
3 
4  velocityprofile_traphalf.cxx - description
5  -------------------
6  begin : Mon May 10 2004
7  copyright : (C) 2004 Erwin Aertbelien
8  email : erwin.aertbelien@mech.kuleuven.ac.be
9 
10  ***************************************************************************
11  * This library is free software; you can redistribute it and/or *
12  * modify it under the terms of the GNU Lesser General Public *
13  * License as published by the Free Software Foundation; either *
14  * version 2.1 of the License, or (at your option) any later version. *
15  * *
16  * This library is distributed in the hope that it will be useful, *
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU *
19  * Lesser General Public License for more details. *
20  * *
21  * You should have received a copy of the GNU Lesser General Public *
22  * License along with this library; if not, write to the Free Software *
23  * Foundation, Inc., 59 Temple Place, *
24  * Suite 330, Boston, MA 02111-1307 USA *
25  * *
26  ***************************************************************************/
27 /*****************************************************************************
28  * \author
29  * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
30  *
31  * \version
32  * ORO_Geometry V0.2
33  *
34  * \par History
35  * - $log$
36  *
37  * \par Release
38  * $Id: velocityprofile_traphalf.cpp,v 1.1.1.1.2.5 2003/07/24 13:26:15 psoetens Exp $
39  * $Name: $
40  ****************************************************************************/
41 
42 
43 //#include "error.h"
45 #include <algorithm>
46 
47 namespace KDL {
48 
49 
50 VelocityProfile_TrapHalf::VelocityProfile_TrapHalf(double _maxvel,double _maxacc,bool _starting):
51  maxvel(_maxvel),maxacc(_maxacc),starting(_starting) {}
52 
53 void VelocityProfile_TrapHalf::SetMax(double _maxvel,double _maxacc, bool _starting)
54 {
55  maxvel = _maxvel; maxacc = _maxacc; starting = _starting;
56 }
57 
58 void VelocityProfile_TrapHalf::PlanProfile1(double v,double a) {
59  a3 = 0;
60  a2 = 0;
61  a1 = startpos;
62  b3 = a/2.0;
63  b2 = -a*t1;
64  b1 = startpos + a*t1*t1/2.0;
65  c3 = 0;
66  c2 = v;
67  c1 = endpos - v*duration;
68 }
69 
70 void VelocityProfile_TrapHalf::PlanProfile2(double v,double a) {
71  a3 = 0;
72  a2 = v;
73  a1 = startpos;
74  b3 = -a/2.0;
75  b2 = a*t2;
76  b1 = endpos - a*t2*t2/2.0;
77  c3 = 0;
78  c2 = 0;
79  c1 = endpos;
80 }
81 
82 void VelocityProfile_TrapHalf::SetProfile(double pos1,double pos2) {
83  startpos = pos1;
84  endpos = pos2;
85  double s = sign(endpos-startpos);
86  // check that the maxvel is obtainable
87  double vel = std::min(maxvel, sqrt(2.0*s*(endpos-startpos)*maxacc));
88  duration = s*(endpos-startpos)/vel+vel/maxacc/2.0;
89  if (starting) {
90  t1 = 0;
91  t2 = vel/maxacc;
92  PlanProfile1(vel*s,maxacc*s);
93  } else {
94  t1 = duration-vel/maxacc;
95  t2 = duration;
96  PlanProfile2(s*vel,s*maxacc);
97  }
98 }
99 
101  double pos1,double pos2,double newduration)
102 {
103  SetProfile(pos1,pos2);
104  double factor = duration/newduration;
105 
106  if ( factor > 1 )
107  return;
108 
109  double s = sign(endpos-startpos);
110  double tmp = 2.0*s*(endpos-startpos)/maxvel;
111  double v = s*maxvel;
112  duration = newduration;
113  if (starting) {
114  if (tmp > duration) {
115  t1 = 0;
116  double a = v*v/2.0/(v*duration-(endpos-startpos));
117  t2 = v/a;
118  PlanProfile1(v,a);
119  } else {
121  double a = v*v/2.0/(endpos-startpos);
122  t1 = t2-v/a;
123  PlanProfile1(v,a);
124  }
125  } else {
126  if (tmp > duration) {
127  t2 = duration;
128  double a = v*v/2.0/(v*duration-(endpos-startpos));
129  t1 = t2-v/a;
130  PlanProfile2(v,a);
131  } else {
132  double a = v*v/2.0/(endpos-startpos);
133  t1 = 0;
134  t2 = v/a;
135  PlanProfile2(v,a);
136  }
137  }
138 }
139 
140 double VelocityProfile_TrapHalf::Duration() const {
141  return duration;
142 }
143 
144 double VelocityProfile_TrapHalf::Pos(double time) const {
145  if (time < 0) {
146  return startpos;
147  } else if (time<t1) {
148  return a1+time*(a2+a3*time);
149  } else if (time<t2) {
150  return b1+time*(b2+b3*time);
151  } else if (time<=duration) {
152  return c1+time*(c2+c3*time);
153  } else {
154  return endpos;
155  }
156 }
157 double VelocityProfile_TrapHalf::Vel(double time) const {
158  if (time < 0) {
159  return 0;
160  } else if (time<t1) {
161  return a2+2*a3*time;
162  } else if (time<t2) {
163  return b2+2*b3*time;
164  } else if (time<=duration) {
165  return c2+2*c3*time;
166  } else {
167  return 0;
168  }
169 }
170 
171 double VelocityProfile_TrapHalf::Acc(double time) const {
172  if (time < 0) {
173  return 0;
174  } else if (time<t1) {
175  return 2*a3;
176  } else if (time<t2) {
177  return 2*b3;
178  } else if (time<=duration) {
179  return 2*c3;
180  } else {
181  return 0;
182  }
183 }
184 
185 VelocityProfile* VelocityProfile_TrapHalf::Clone() const {
187  res->SetProfileDuration( this->startpos, this->endpos, this->duration );
188  return res;
189 }
190 
192 
193 
195  os << "TRAPEZOIDALHALF[" << maxvel << "," << maxacc << "," << starting << "]";
196 }
197 
198 
199 
200 
201 
202 }
203 
KDL::VelocityProfile_TrapHalf::Pos
virtual double Pos(double time) const
Definition: velocityprofile_traphalf.cpp:182
KDL::VelocityProfile_TrapHalf::duration
double duration
Definition: velocityprofile_traphalf.hpp:149
KDL::VelocityProfile_TrapHalf::SetProfile
virtual void SetProfile(double pos1, double pos2)
Definition: velocityprofile_traphalf.cpp:120
KDL::VelocityProfile_TrapHalf::b1
double b1
Definition: velocityprofile_traphalf.hpp:147
KDL::VelocityProfile_TrapHalf::VelocityProfile_TrapHalf
VelocityProfile_TrapHalf(double _maxvel=0, double _maxacc=0, bool _starting=true)
Definition: velocityprofile_traphalf.cpp:88
KDL::VelocityProfile_TrapHalf::starting
bool starting
Definition: velocityprofile_traphalf.hpp:158
KDL::VelocityProfile_TrapHalf::c1
double c1
Definition: velocityprofile_traphalf.hpp:148
KDL::sqrt
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:391
KDL::VelocityProfile_TrapHalf::~VelocityProfile_TrapHalf
virtual ~VelocityProfile_TrapHalf()
Definition: velocityprofile_traphalf.cpp:229
KDL::VelocityProfile_TrapHalf::Acc
virtual double Acc(double time) const
Definition: velocityprofile_traphalf.cpp:209
KDL::VelocityProfile_TrapHalf::Duration
virtual double Duration() const
Definition: velocityprofile_traphalf.cpp:178
KDL::VelocityProfile_TrapHalf::Clone
virtual VelocityProfile * Clone() const
Definition: velocityprofile_traphalf.cpp:223
algorithm
KDL::VelocityProfile_TrapHalf::SetMax
void SetMax(double _maxvel, double _maxacc, bool _starting)
Definition: velocityprofile_traphalf.cpp:91
KDL
Definition: kukaLWR_DHnew.cpp:25
KDL::VelocityProfile_TrapHalf::a2
double a2
Definition: velocityprofile_traphalf.hpp:146
KDL::VelocityProfile_TrapHalf::endpos
double endpos
Definition: velocityprofile_traphalf.hpp:153
KDL::VelocityProfile_TrapHalf::Write
virtual void Write(std::ostream &os) const
Definition: velocityprofile_traphalf.cpp:232
KDL::VelocityProfile_TrapHalf::maxvel
double maxvel
Definition: velocityprofile_traphalf.hpp:156
KDL::VelocityProfile_TrapHalf::PlanProfile1
void PlanProfile1(double v, double a)
Definition: velocityprofile_traphalf.cpp:96
KDL::VelocityProfile_TrapHalf::startpos
double startpos
Definition: velocityprofile_traphalf.hpp:152
std::ostream
KDL::VelocityProfile_TrapHalf::PlanProfile2
void PlanProfile2(double v, double a)
Definition: velocityprofile_traphalf.cpp:108
KDL::VelocityProfile_TrapHalf::Vel
virtual double Vel(double time) const
Definition: velocityprofile_traphalf.cpp:195
KDL::VelocityProfile_TrapHalf::b2
double b2
Definition: velocityprofile_traphalf.hpp:147
KDL::sign
double sign(double arg)
Definition: utility.h:251
KDL::VelocityProfile_TrapHalf::a3
double a3
Definition: velocityprofile_traphalf.hpp:146
KDL::VelocityProfile_TrapHalf::t1
double t1
Definition: velocityprofile_traphalf.hpp:150
std::min
T min(T... args)
KDL::VelocityProfile_TrapHalf::c2
double c2
Definition: velocityprofile_traphalf.hpp:148
KDL::VelocityProfile_TrapHalf::maxacc
double maxacc
Definition: velocityprofile_traphalf.hpp:157
KDL::VelocityProfile_TrapHalf::b3
double b3
Definition: velocityprofile_traphalf.hpp:147
KDL::VelocityProfile_TrapHalf::t2
double t2
Definition: velocityprofile_traphalf.hpp:150
KDL::VelocityProfile_TrapHalf::c3
double c3
Definition: velocityprofile_traphalf.hpp:148
std::time
T time(T... args)
KDL::VelocityProfile_TrapHalf::SetProfileDuration
virtual void SetProfileDuration(double pos1, double pos2, double newduration)
Definition: velocityprofile_traphalf.cpp:138
velocityprofile_traphalf.hpp
KDL::VelocityProfile_TrapHalf::a1
double a1
Definition: velocityprofile_traphalf.hpp:146