orocos_kdl
src
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
49
void
VelocityProfile_Rectangular::SetProfile
(
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
70
void
VelocityProfile_Rectangular::SetMax
(
double
vMax )
71
{
72
maxvel
= vMax;
73
}
74
75
76
void
VelocityProfile_Rectangular::
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
97
double
VelocityProfile_Rectangular::Duration
()
const
{
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
{
122
throw
Error_MotionPlanning_Incompatible();
123
return
0;
124
}
125
126
127
void
VelocityProfile_Rectangular::Write
(
std::ostream
& os)
const
{
128
os <<
"CONSTVEL["
<<
maxvel
<<
"]"
;
129
}
130
131
132
}
133
KDL::VelocityProfile_Rectangular::v
double v
Definition:
velocityprofile_rect.hpp:136
KDL::VelocityProfile_Rectangular::SetProfileDuration
virtual void SetProfileDuration(double pos1, double pos2, double duration)
Definition:
velocityprofile_rect.cpp:115
KDL::VelocityProfile_Rectangular::Acc
virtual double Acc(double time) const
Definition:
velocityprofile_rect.cpp:159
KDL::VelocityProfile_Rectangular::SetMax
void SetMax(double _maxvel)
Definition:
velocityprofile_rect.cpp:108
KDL
Definition:
kukaLWR_DHnew.cpp:25
KDL::VelocityProfile_Rectangular::Pos
virtual double Pos(double time) const
Definition:
velocityprofile_rect.cpp:139
std::ostream
velocityprofile_rect.hpp
error.h
KDL::VelocityProfile_Rectangular::Vel
virtual double Vel(double time) const
Definition:
velocityprofile_rect.cpp:149
KDL::VelocityProfile_Rectangular::maxvel
double maxvel
Definition:
velocityprofile_rect.hpp:138
KDL::VelocityProfile_Rectangular::p
double p
Definition:
velocityprofile_rect.hpp:136
KDL::VelocityProfile_Rectangular::Write
virtual void Write(std::ostream &os) const
Definition:
velocityprofile_rect.cpp:165
std::time
T time(T... args)
KDL::diff
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
KDL::VelocityProfile_Rectangular::Duration
virtual double Duration() const
Definition:
velocityprofile_rect.cpp:135
KDL::VelocityProfile_Rectangular::d
double d
Definition:
velocityprofile_rect.hpp:136
KDL::VelocityProfile_Rectangular::SetProfile
void SetProfile(double pos1, double pos2)
Definition:
velocityprofile_rect.cpp:87
Generated on Tue Nov 26 2024 04:35:25 for orocos_kdl by
1.8.17