velocityprofile_spline.cpp
Go to the documentation of this file.
1 
2 #include <limits>
3 
5 
6 namespace KDL {
7 
8 static inline void generatePowers(int n, double x, double* powers)
9 {
10  powers[0] = 1.0;
11  for (int i=1; i<=n; i++)
12  {
13  powers[i] = powers[i-1]*x;
14  }
15  return;
16 }
17 
19 {
20  duration_ = 0.0;
21 
22  coeff_[0] = 0.0;
23  coeff_[1] = 0.0;
24  coeff_[2] = 0.0;
25  coeff_[3] = 0.0;
26  coeff_[4] = 0.0;
27  coeff_[5] = 0.0;
28 
29  return;
30 }
31 
33 {
34  duration_ = p.duration_;
35 
36  coeff_[0] = p.coeff_[0];
37  coeff_[1] = p.coeff_[1];
38  coeff_[2] = p.coeff_[2];
39  coeff_[3] = p.coeff_[3];
40  coeff_[4] = p.coeff_[4];
41  coeff_[5] = p.coeff_[5];
42 
43  return;
44 }
45 
47 {
48  return;
49 }
50 
51 void VelocityProfile_Spline::SetProfile(double pos1, double pos2)
52 {
53  return;
54 }
55 
56 void VelocityProfile_Spline::SetProfileDuration(double pos1, double pos2, double duration)
57 {
58  duration_ = duration;
59  if (duration <= std::numeric_limits<double>::epsilon() )
60  {
61  coeff_[0] = pos1;
62  coeff_[1] = 0.0;
63  coeff_[2] = 0.0;
64  coeff_[3] = 0.0;
65  coeff_[4] = 0.0;
66  coeff_[5] = 0.0;
67  } else
68  {
69  coeff_[0] = pos1;
70  coeff_[1] = (pos2 - pos1) / duration;
71  coeff_[2] = 0.0;
72  coeff_[3] = 0.0;
73  coeff_[4] = 0.0;
74  coeff_[5] = 0.0;
75  }
76 
77  return;
78 }
79 
80 void VelocityProfile_Spline::SetProfileDuration(double pos1, double vel1, double pos2, double vel2, double duration)
81 {
82  double T[4];
83  duration_ = duration;
84  generatePowers(3, duration, T);
85 
86  if (duration <= std::numeric_limits<double>::epsilon() )
87  {
88  coeff_[0] = pos2;
89  coeff_[1] = vel2;
90  coeff_[2] = 0.0;
91  coeff_[3] = 0.0;
92  coeff_[4] = 0.0;
93  coeff_[5] = 0.0;
94  } else
95  {
96  coeff_[0] = pos1;
97  coeff_[1] = vel1;
98  coeff_[2] = (-3.0*pos1 + 3.0*pos2 - 2.0*vel1*T[1] - vel2*T[1]) / T[2];
99  coeff_[3] = (2.0*pos1 - 2.0*pos2 + vel1*T[1] + vel2*T[1]) / T[3];
100  coeff_[4] = 0.0;
101  coeff_[5] = 0.0;
102  }
103 
104  return;
105 }
106 
107 void VelocityProfile_Spline::SetProfileDuration(double pos1, double vel1, double acc1, double pos2, double vel2, double acc2, double duration)
108 {
109  double T[6];
110  duration_ = duration;
111  generatePowers(5, duration, T);
112 
113  if (duration <= std::numeric_limits<double>::epsilon() )
114  {
115  coeff_[0] = pos2;
116  coeff_[1] = vel2;
117  coeff_[2] = 0.5 * acc2;
118  coeff_[3] = 0.0;
119  coeff_[4] = 0.0;
120  coeff_[5] = 0.0;
121  } else
122  {
123  coeff_[0] = pos1;
124  coeff_[1] = vel1;
125  coeff_[2] = 0.5*acc1;
126  coeff_[3] = (-20.0*pos1 + 20.0*pos2 - 3.0*acc1*T[2] + acc2*T[2] -
127  12.0*vel1*T[1] - 8.0*vel2*T[1]) / (2.0*T[3]);
128  coeff_[4] = (30.0*pos1 - 30.0*pos2 + 3.0*acc1*T[2] - 2.0*acc2*T[2] +
129  16.0*vel1*T[1] + 14.0*vel2*T[1]) / (2.0*T[4]);
130  coeff_[5] = (-12.0*pos1 + 12.0*pos2 - acc1*T[2] + acc2*T[2] -
131  6.0*vel1*T[1] - 6.0*vel2*T[1]) / (2.0*T[5]);
132  }
133 
134  return;
135 }
136 
138 {
139  return duration_;
140 }
141 
142 double VelocityProfile_Spline::Pos(double time) const
143 {
144  double t[6];
145  double position;
146  generatePowers(5, time, t);
147 
148  position = t[0]*coeff_[0] +
149  t[1]*coeff_[1] +
150  t[2]*coeff_[2] +
151  t[3]*coeff_[3] +
152  t[4]*coeff_[4] +
153  t[5]*coeff_[5];
154  return position;
155 }
156 
157 double VelocityProfile_Spline::Vel(double time) const
158 {
159  double t[5];
160  double velocity;
161  generatePowers(4, time, t);
162 
163  velocity = t[0]*coeff_[1] +
164  2.0*t[1]*coeff_[2] +
165  3.0*t[2]*coeff_[3] +
166  4.0*t[3]*coeff_[4] +
167  5.0*t[4]*coeff_[5];
168  return velocity;
169 }
170 
171 double VelocityProfile_Spline::Acc(double time) const
172 {
173  double t[4];
174  double acceleration;
175  generatePowers(3, time, t);
176 
177  acceleration = 2.0*t[0]*coeff_[2] +
178  6.0*t[1]*coeff_[3] +
179  12.0*t[2]*coeff_[4] +
180  20.0*t[3]*coeff_[5];
181  return acceleration;
182 }
183 
184 void VelocityProfile_Spline::Write(std::ostream& os) const
185 {
186  os << "coefficients : [ " << coeff_[0] << " " << coeff_[1] << " " << coeff_[2] << " " << coeff_[3] << " " << coeff_[4] << " " << coeff_[5] << " ]";
187  return;
188 }
189 
191 {
192  return new VelocityProfile_Spline(*this);
193 }
194 }
virtual double Vel(double time) const
A spline VelocityProfile trajectory interpolation.
virtual double Pos(double time) const
virtual VelocityProfile * Clone() const
virtual void Write(std::ostream &os) const
double epsilon
default precision while comparing with Equal(..,..) functions. Initialized at 0.0000001.
virtual void SetProfileDuration(double pos1, double pos2, double duration)
virtual void SetProfile(double pos1, double pos2)
virtual double Acc(double time) const
static void generatePowers(int n, double x, double *powers)


orocos_kdl
Author(s):
autogenerated on Tue Sep 22 2020 03:17:50