moveit2
The MoveIt Motion Planning Framework for ROS 2.
velocity_profile_atrap.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2018 Pilz GmbH & Co. KG
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Pilz GmbH & Co. KG nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
36 
38 {
39 VelocityProfileATrap::VelocityProfileATrap(double max_vel, double max_acc, double max_dec)
40  : max_vel_(fabs(max_vel))
41  , max_acc_(fabs(max_acc))
42  , max_dec_(fabs(max_dec))
43  , start_pos_(0)
44  , end_pos_(0)
45  , start_vel_(0)
46  , a1_(0)
47  , a2_(0)
48  , a3_(0)
49  , b1_(0)
50  , b2_(0)
51  , b3_(0)
52  , c1_(0)
53  , c2_(0)
54  , c3_(0)
55  , t_a_(0)
56  , t_b_(0)
57  , t_c_(0)
58 {
59 }
60 
61 void VelocityProfileATrap::SetProfile(double pos1, double pos2)
62 {
63  start_pos_ = pos1;
64  end_pos_ = pos2;
65  start_vel_ = 0.0;
66 
67  if (start_pos_ == end_pos_)
68  {
69  // goal already reached, set everything to zero
70  setEmptyProfile();
71  return;
72  }
73  else
74  {
75  // get the sign
76  double s = ((end_pos_ - start_pos_) > 0.0) - ((end_pos_ - start_pos_) < 0.0);
77 
78  double dis = fabs(end_pos_ - start_pos_);
79  double min_dis_max_vel = 0.5 * max_vel_ * max_vel_ / max_acc_ + 0.5 * max_vel_ * max_vel_ / max_dec_;
80 
81  // max_vel can be reached
82  if (dis > min_dis_max_vel)
83  {
84  // acceleration phase
85  a1_ = start_pos_;
86  a2_ = 0.0;
87  a3_ = s * max_acc_ / 2.0;
88  t_a_ = max_vel_ / max_acc_;
89 
90  // constant phase
91  b1_ = a1_ + a3_ * t_a_ * t_a_;
92  b2_ = s * max_vel_;
93  b3_ = 0;
94  t_b_ = (dis - min_dis_max_vel) / max_vel_;
95 
96  // deceleration phase
97  c1_ = b1_ + b2_ * (t_b_);
98  c2_ = s * max_vel_;
99  c3_ = -s * max_dec_ / 2.0;
100  t_c_ = max_vel_ / max_dec_;
101  }
102  // max_vel cannot be reached, no constant velocity phase
103  else
104  {
105  // compute the new velocity of constant phase
106  double new_vel = s * sqrt(2.0 * dis * max_acc_ * max_dec_ / (max_acc_ + max_dec_));
107 
108  // acceleration phase
109  a1_ = start_pos_;
110  a2_ = 0.0;
111  a3_ = s * max_acc_ / 2.0;
112  t_a_ = fabs(new_vel) / max_acc_;
113 
114  // constant phase
115  b1_ = a1_ + a3_ * t_a_ * t_a_;
116  b2_ = new_vel;
117  b3_ = 0;
118  t_b_ = 0.0;
119 
120  // deceleration phase
121  c1_ = b1_;
122  c2_ = new_vel;
123  c3_ = -s * max_dec_ / 2.0;
124  t_c_ = fabs(new_vel) / max_dec_;
125  }
126  }
127 }
128 
129 void VelocityProfileATrap::SetProfileDuration(double pos1, double pos2, double duration)
130 {
131  // compute the fastest case
132  SetProfile(pos1, pos2);
133 
134  // cannot be faster
135  if (Duration() > duration)
136  {
137  return;
138  }
139 
140  double ratio = Duration() / duration;
141  a2_ *= ratio;
142  a3_ *= ratio * ratio;
143  b2_ *= ratio;
144  b3_ *= ratio * ratio;
145  c2_ *= ratio;
146  c3_ *= ratio * ratio;
147  t_a_ /= ratio;
148  t_b_ /= ratio;
149  t_c_ /= ratio;
150 }
151 
152 bool VelocityProfileATrap::setProfileAllDurations(double pos1, double pos2, double duration1, double duration2,
153  double duration3)
154 {
155  // compute the fastest case
156  SetProfile(pos1, pos2);
157 
158  assert(duration1 > 0);
159  assert(duration3 > 0);
160 
161  // cannot be faster
162  if (Duration() - (duration1 + duration2 + duration3) > KDL::epsilon)
163  {
164  return false;
165  }
166 
167  // get the sign
168  double s = ((end_pos_ - start_pos_) > 0.0) - ((end_pos_ - start_pos_) < 0.0);
169  // compute the new velocity/acceleration/decel4eration
170  double dis = fabs(end_pos_ - start_pos_);
171  double new_vel = s * dis / (duration2 + duration1 / 2.0 + duration3 / 2.0);
172  double new_acc = new_vel / duration1;
173  double new_dec = -new_vel / duration3;
174  if ((fabs(new_vel) - max_vel_ > KDL::epsilon) || (fabs(new_acc) - max_acc_ > KDL::epsilon) ||
175  (fabs(new_dec) - max_dec_ > KDL::epsilon))
176  {
177  return false;
178  }
179  else
180  {
181  // set profile
182  start_pos_ = pos1;
183  end_pos_ = pos2;
184 
185  // acceleration phase
186  a1_ = start_pos_;
187  a2_ = 0.0;
188  a3_ = new_acc / 2.0;
189  t_a_ = duration1;
190 
191  // constant phase
192  b1_ = a1_ + a3_ * t_a_ * t_a_;
193  b2_ = new_vel;
194  b3_ = 0;
195  t_b_ = duration2;
196 
197  // deceleration phase
198  c1_ = b1_ + b2_ * (t_b_);
199  c2_ = new_vel;
200  c3_ = new_dec / 2.0;
201  t_c_ = duration3;
202 
203  return true;
204  }
205 }
206 
207 bool VelocityProfileATrap::setProfileStartVelocity(double pos1, double pos2, double vel1)
208 {
209  if (vel1 == 0)
210  {
211  SetProfile(pos1, pos2);
212  return true;
213  }
214 
215  // get the sign
216  double s = ((pos2 - pos1) > 0.0) - ((pos2 - pos1) < 0.0);
217 
218  if (s * vel1 <= 0)
219  {
220  // TODO initial velocity is in opposite direction of start-end vector
221  return false;
222  }
223 
224  start_pos_ = pos1;
225  end_pos_ = pos2;
226  start_vel_ = vel1;
227 
228  // minimum brake distance
229  double min_brake_dis = 0.5 * vel1 * vel1 / max_dec_;
230  // minimum distance to reach the maximum velocity
231  double min_dis_max_vel =
232  0.5 * (max_vel_ - start_vel_) * (max_vel_ + start_vel_) / max_acc_ + 0.5 * max_vel_ * max_vel_ / max_dec_;
233  double dis = fabs(end_pos_ - start_pos_);
234 
235  // brake, acceleration in opposite direction, deceleration
236  if (dis <= min_brake_dis)
237  {
238  // brake to zero velocity
239  t_a_ = fabs(start_vel_ / max_dec_);
240  a1_ = start_pos_;
241  a2_ = start_vel_;
242  a3_ = -0.5 * s * max_dec_;
243 
244  // compute the velocity in opposite direction
245  double new_vel = -s * sqrt(2.0 * fabs(min_brake_dis - dis) * max_acc_ * max_dec_ / (max_acc_ + max_dec_));
246 
247  // acceleration in opposite direction
248  t_b_ = fabs(new_vel / max_acc_);
249  b1_ = a1_ + a2_ * t_a_ + a3_ * t_a_ * t_a_;
250  b2_ = 0;
251  b3_ = -s * 0.5 * max_acc_;
252 
253  // deceleration to zero
254  t_c_ = fabs(new_vel / max_dec_);
255  c1_ = b1_ + b2_ * t_b_ + b3_ * t_b_ * t_b_;
256  c2_ = new_vel;
257  c3_ = 0.5 * s * max_dec_;
258  }
259  else if (dis <= min_dis_max_vel)
260  {
261  // compute the reached velocity
262  double new_vel =
263  s * sqrt((dis + 0.5 * start_vel_ * start_vel_ / max_acc_) * 2.0 * max_acc_ * max_dec_ / (max_acc_ + max_dec_));
264 
265  // acceleration to new velocity
266  t_a_ = fabs(new_vel - start_vel_) / max_acc_;
267  a1_ = start_pos_;
268  a2_ = start_vel_;
269  a3_ = 0.5 * s * max_acc_;
270 
271  // no constant velocity phase
272  t_b_ = 0;
273  b1_ = a1_ + a2_ * t_a_ + a3_ * t_a_ * t_a_;
274  b2_ = 0;
275  b3_ = 0;
276 
277  // deceleration to zero velocity
278  t_c_ = fabs(new_vel / max_dec_);
279  c1_ = b1_;
280  c2_ = new_vel;
281  c3_ = -0.5 * s * max_dec_;
282  }
283  else
284  {
285  // acceleration to max velocity
286  t_a_ = fabs(max_vel_ - start_vel_) / max_acc_;
287  a1_ = start_pos_;
288  a2_ = start_vel_;
289  a3_ = 0.5 * s * max_acc_;
290 
291  // constant velocity
292  t_b_ = (dis - min_dis_max_vel) / max_vel_;
293  b1_ = a1_ + a2_ * t_a_ + a3_ * t_a_ * t_a_;
294  b2_ = max_vel_;
295  b3_ = 0;
296 
297  // deceleration to zero velocity
298  t_c_ = max_vel_ / max_dec_;
299  c1_ = b1_ + b2_ * t_b_ + b3_ * t_b_ * t_b_;
300  c2_ = max_vel_;
301  c3_ = -0.5 * s * max_dec_;
302  }
303 
304  return true;
305 }
306 
308 {
309  return t_a_ + t_b_ + t_c_;
310 }
311 
312 double VelocityProfileATrap::Pos(double time) const
313 {
314  if (time < 0)
315  {
316  return start_pos_;
317  }
318  else if (time < t_a_)
319  {
320  return a1_ + time * (a2_ + a3_ * time);
321  }
322  else if (time < (t_a_ + t_b_))
323  {
324  return b1_ + (time - t_a_) * (b2_ + b3_ * (time - t_a_));
325  }
326  else if (time <= (t_a_ + t_b_ + t_c_))
327  {
328  return c1_ + (time - t_a_ - t_b_) * (c2_ + c3_ * (time - t_a_ - t_b_));
329  }
330  else
331  {
332  return end_pos_;
333  }
334 }
335 
336 double VelocityProfileATrap::Vel(double time) const
337 {
338  if (time < 0)
339  {
340  return start_vel_;
341  }
342  else if (time < t_a_)
343  {
344  return a2_ + 2 * a3_ * time;
345  }
346  else if (time < (t_a_ + t_b_))
347  {
348  return b2_ + 2 * b3_ * (time - t_a_);
349  }
350  else if (time <= (t_a_ + t_b_ + t_c_))
351  {
352  return c2_ + 2 * c3_ * (time - t_a_ - t_b_);
353  }
354  else
355  {
356  return 0;
357  }
358 }
359 
360 double VelocityProfileATrap::Acc(double time) const
361 {
362  if (time <= 0)
363  {
364  return 0;
365  }
366  else if (time <= t_a_)
367  {
368  return 2 * a3_;
369  }
370  else if (time <= (t_a_ + t_b_))
371  {
372  return 2 * b3_;
373  }
374  else if (time <= (t_a_ + t_b_ + t_c_))
375  {
376  return 2 * c3_;
377  }
378  else
379  {
380  return 0;
381  }
382 }
383 
384 KDL::VelocityProfile* VelocityProfileATrap::Clone() const
385 {
386  VelocityProfileATrap* trap = new VelocityProfileATrap(max_vel_, max_acc_, max_dec_);
387  trap->setProfileAllDurations(start_pos_, end_pos_, t_a_, t_b_, t_c_);
388  return trap;
389 }
390 
391 // LCOV_EXCL_START // No tests for the print function
392 void VelocityProfileATrap::Write(std::ostream& os) const
393 {
394  os << *this;
395 }
396 
397 std::ostream& operator<<(std::ostream& os, const VelocityProfileATrap& p)
398 {
399  os << "Asymmetric Trapezoid " << '\n'
400  << "maximal velocity: " << p.max_vel_ << '\n'
401  << "maximal acceleration: " << p.max_acc_ << '\n'
402  << "maximal deceleration: " << p.max_dec_ << '\n'
403  << "start position: " << p.start_pos_ << '\n'
404  << "end position: " << p.end_pos_ << '\n'
405  << "start velocity: " << p.start_vel_ << '\n'
406  << "a1: " << p.a1_ << '\n'
407  << "a2: " << p.a2_ << '\n'
408  << "a3: " << p.a3_ << '\n'
409  << "b1: " << p.b1_ << '\n'
410  << "b2: " << p.b2_ << '\n'
411  << "b3: " << p.b3_ << '\n'
412  << "c1: " << p.c1_ << '\n'
413  << "c2: " << p.c2_ << '\n'
414  << "c3: " << p.c3_ << '\n'
415  << "firstPhaseDuration " << p.firstPhaseDuration() << '\n'
416  << "secondPhaseDuration " << p.secondPhaseDuration() << '\n'
417  << "thirdPhaseDuration " << p.thirdPhaseDuration() << '\n';
418  return os;
419 }
420 // LCOV_EXCL_STOP
421 
423 {
424  return (max_vel_ == other.max_vel_ && max_acc_ == other.max_acc_ && max_dec_ == other.max_dec_ &&
425  start_pos_ == other.start_pos_ && end_pos_ == other.end_pos_ && start_vel_ == other.start_vel_ &&
426  a1_ == other.a1_ && a2_ == other.a2_ && a3_ == other.a3_ && b1_ == other.b1_ && b2_ == other.b2_ &&
427  b3_ == other.b3_ && c1_ == other.c1_ && c2_ == other.c2_ && c3_ == other.c3_ && t_a_ == other.t_a_ &&
428  t_b_ == other.t_b_ && t_c_ == other.t_c_);
429 }
430 
432 {
433 }
434 
435 void VelocityProfileATrap::setEmptyProfile()
436 {
437  a1_ = end_pos_;
438  a2_ = 0;
439  a3_ = 0;
440  b1_ = end_pos_;
441  b2_ = 0;
442  c1_ = end_pos_;
443  c2_ = 0;
444  c3_ = 0;
445 
446  t_a_ = 0;
447  t_b_ = 0;
448  t_c_ = 0;
449 }
450 
451 } // namespace pilz_industrial_motion_planner
A PTP Trajectory Generator of Asymmetric Trapezoidal Velocity Profile. Differences to VelocityProfile...
double Vel(double time) const override
Get velocity at given time.
double Pos(double time) const override
Get position at given time.
double thirdPhaseDuration() const
get the time of third phase
VelocityProfileATrap(double max_vel=0, double max_acc=0, double max_dec=0)
Constructor.
void SetProfile(double pos1, double pos2) override
compute the fastest profile Algorithm:
bool setProfileStartVelocity(double pos1, double pos2, double vel1)
Profile with start velocity Note: This function is not general and is currently only used for live co...
double secondPhaseDuration() const
get the time of second phase
void SetProfileDuration(double pos1, double pos2, double duration) override
Profile scaled by the total duration.
KDL::VelocityProfile * Clone() const override
returns copy of current VelocityProfile object
bool setProfileAllDurations(double pos1, double pos2, double duration1, double duration2, double duration3)
Profile with given acceleration/constant/deceleration durations. Each duration must obey the maximal ...
bool operator==(const VelocityProfileATrap &other) const
Compares two Asymmetric Trapezoidal Velocity Profiles.
void Write(std::ostream &os) const override
Write basic information.
double Acc(double time) const override
Get given acceleration/deceleration at given time.
double firstPhaseDuration() const
get the time of first phase
std::ostream & operator<<(std::ostream &os, const VelocityProfileATrap &p)