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.
Definition: utility.cxx:21
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 Thu Apr 13 2023 02:19:14