velocityprofile_trap.cpp
Go to the documentation of this file.
1 /***************************************************************************
2  tag: Erwin Aertbelien Mon May 10 19:10:36 CEST 2004 velocityprofile_trap.cxx
3 
4  velocityprofile_trap.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_trap.cpp,v 1.1.1.1.2.7 2003/07/24 13:26:15 psoetens Exp $
39  * $Name: $
40  ****************************************************************************/
41 
42 
43 //#include "error.h"
44 #include "velocityprofile_trap.hpp"
45 
46 namespace KDL {
47 
48 
49 VelocityProfile_Trap::VelocityProfile_Trap(double _maxvel,double _maxacc):
50  a1(0), a2(0), a3(0),
51  b1(0), b2(0), b3(0),
52  c1(0), c2(0), c3(0),
53  duration(0), t1(0), t2(0),
54  maxvel(_maxvel),maxacc(_maxacc),
55  startpos(0), endpos(0)
56 
57 {}
58  // constructs motion profile class with <maxvel> as parameter of the
59  // trajectory.
60 
61 void VelocityProfile_Trap::SetProfile(double pos1,double pos2) {
62  startpos = pos1;
63  endpos = pos2;
64  t1 = maxvel/maxacc;
65  double s = sign(endpos-startpos);
66  double deltax1 = s*maxacc*sqr(t1)/2.0;
67  double deltaT = (endpos-startpos-2.0*deltax1) / (s*maxvel);
68  if (deltaT > 0.0) {
69  // plan a complete profile :
70  duration = 2*t1+deltaT;
71  t2 = duration - t1;
72  } else {
73  // plan an incomplete profile :
75  duration = t1*2.0;
76  t2=t1;
77  }
78  a3 = s*maxacc/2.0;
79  a2 = 0;
80  a1 = startpos;
81 
82  b3 = 0;
83  b2 = a2+2*a3*t1 - 2.0*b3*t1;
84  b1 = a1+t1*(a2+a3*t1) - t1*(b2+t1*b3);
85 
86  c3 = -s*maxacc/2.0;
87  c2 = b2+2*b3*t2 - 2.0*c3*t2;
88  c1 = b1+t2*(b2+b3*t2) - t2*(c2+t2*c3);
89 }
90 
92  double pos1,double pos2,double newduration) {
93  // duration should be longer than originally planned duration
94  // Fastest :
95  SetProfile(pos1,pos2);
96  // Must be Slower :
97  double factor = duration/newduration;
98  if (factor > 1)
99  return; // do not exceed max
100  a2*=factor;
101  a3*=factor*factor;
102  b2*=factor;
103  b3*=factor*factor;
104  c2*=factor;
105  c3*=factor*factor;
106  duration = newduration;
107  t1 /= factor;
108  t2 /= factor;
109 }
110 
112  double pos1,double pos2,double newvelocity) {
113  // Max velocity
114  SetProfile(pos1,pos2);
115  // Must be Slower :
116  double factor = newvelocity; // valid = [KDL::epsilon, 1.0]
117  if (1.0 < factor) factor = 1.0;
118  if (KDL::epsilon > factor) factor = KDL::epsilon;
119  a2*=factor;
120  a3*=factor*factor;
121  b2*=factor;
122  b3*=factor*factor;
123  c2*=factor;
124  c3*=factor*factor;
125  duration = duration / factor;
126  t1 /= factor;
127  t2 /= factor;
128 }
129 
130 void VelocityProfile_Trap::SetMax(double _maxvel,double _maxacc)
131 {
132  maxvel = _maxvel; maxacc = _maxacc;
133 }
134 
136  return duration;
137 }
138 
139 double VelocityProfile_Trap::Pos(double time) const {
140  if (time < 0) {
141  return startpos;
142  } else if (time<t1) {
143  return a1+time*(a2+a3*time);
144  } else if (time<t2) {
145  return b1+time*(b2+b3*time);
146  } else if (time<=duration) {
147  return c1+time*(c2+c3*time);
148  } else {
149  return endpos;
150  }
151 }
152 double VelocityProfile_Trap::Vel(double time) const {
153  if (time < 0) {
154  return 0;
155  } else if (time<t1) {
156  return a2+2*a3*time;
157  } else if (time<t2) {
158  return b2+2*b3*time;
159  } else if (time<=duration) {
160  return c2+2*c3*time;
161  } else {
162  return 0;
163  }
164 }
165 
166 double VelocityProfile_Trap::Acc(double time) const {
167  if (time < 0) {
168  return 0;
169  } else if (time<t1) {
170  return 2*a3;
171  } else if (time<t2) {
172  return 2*b3;
173  } else if (time<=duration) {
174  return 2*c3;
175  } else {
176  return 0;
177  }
178 }
179 
182  res->SetProfileDuration( this->startpos, this->endpos, this->duration );
183  return res;
184 }
185 
187 
188 
189 void VelocityProfile_Trap::Write(std::ostream& os) const {
190  os << "TRAPEZOIDAL[" << maxvel << "," << maxacc <<"]";
191 }
192 
193 
194 
195 
196 
197 }
198 
virtual double Vel(double time) const
virtual double Pos(double time) const
double sign(double arg)
Definition: utility.h:251
VelocityProfile_Trap(double _maxvel=0, double _maxacc=0)
virtual double Acc(double time) const
INLINE Rall1d< T, V, S > sqr(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:353
virtual void Write(std::ostream &os) const
virtual void SetProfileDuration(double pos1, double pos2, double newduration)
double epsilon
default precision while comparing with Equal(..,..) functions. Initialized at 0.0000001.
Definition: utility.cxx:21
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:369
virtual void SetProfileVelocity(double pos1, double pos2, double newvelocity)
virtual void SetMax(double _maxvel, double _maxacc)
virtual void SetProfile(double pos1, double pos2)
virtual VelocityProfile * Clone() const
virtual double Duration() const


orocos_kdl
Author(s):
autogenerated on Thu Apr 13 2023 02:19:14