velocityprofile_rect.cpp
Go to the documentation of this file.
1 /***************************************************************************
2  tag: Erwin Aertbelien Mon May 10 19:10:36 CEST 2004 velocityprofile_rect.cxx
3 
4  velocityprofile_rect.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_rect.cpp,v 1.1.1.1.2.5 2003/07/24 13:26:15 psoetens Exp $
39  * $Name: $
40  ****************************************************************************/
41 
42 
43 #include "utilities/error.h"
44 #include "velocityprofile_rect.hpp"
45 
46 namespace KDL {
47 
48 
50  double pos1,
51  double pos2
52  )
53 {
54  double diff;
55  diff = pos2-pos1; // increment per sec.
56  if ( diff != 0 )
57  {
58  v = (diff>0)?maxvel:-maxvel;
59  p = pos1; // start pos
60  d = diff/v;
61  }
62  else
63  {
64  v = 0;
65  p = pos1;
66  d = 0;
67  }
68 }
69 
71  {
72  maxvel = vMax;
73  }
74 
75 
77  SetProfileDuration(double pos1,double pos2,double duration)
78 {
79  double diff;
80  diff = pos2-pos1; // increment per sec.
81  if ( diff != 0 )
82  {
83  v = diff/duration;
84  if (v > maxvel || duration==0 ) // safety.
85  v=maxvel;
86  p = pos1; // start pos
87  d = diff/v;
88  }
89  else
90  {
91  v = 0;
92  p = pos1;
93  d = duration;
94  }
95 }
96 
98  return d;
99 }
100 
101 double VelocityProfile_Rectangular::Pos(double time) const {
102  if (time < 0) {
103  return p;
104  } else if (time>d) {
105  return v*d+p;
106  } else {
107  return v*time+p;
108  }
109 }
110 
111 double VelocityProfile_Rectangular::Vel(double time) const {
112  if (time < 0) {
113  return 0;
114  } else if (time>d) {
115  return 0;
116  } else {
117  return v;
118  }
119 }
120 
121 double VelocityProfile_Rectangular::Acc(double time) const {
123  return 0;
124 }
125 
126 
127 void VelocityProfile_Rectangular::Write(std::ostream& os) const {
128  os << "CONSTVEL[" << maxvel << "]";
129 }
130 
131 
132 }
133 
void SetProfile(double pos1, double pos2)
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
Definition: frames.hpp:1130
virtual double Vel(double time) const
virtual double Acc(double time) const
virtual void SetProfileDuration(double pos1, double pos2, double duration)
virtual void Write(std::ostream &os) const
virtual double Pos(double time) const


orocos_kdl
Author(s):
autogenerated on Fri Mar 12 2021 03:05:44