On Fast Jerk–, Acceleration– and Velocity–Restricted Motion Functions for Online Trajectory Generation

robotics

Article On Fast Jerk–, Acceleration– and Velocity–Restricted Motion Functions for Online Trajectory Generation

Burkhard Alpers

Department of Mechanical Engineering, Aalen University, D-73430 Aalen, Germany; [email protected]

Abstract: Finding fast motion functions to get from an initial state (distance, velocity, acceleration) to a final one has been of interest for decades. For a solution to be practically relevant, restrictions on jerk, acceleration and velocity have to be taken into account. Such solutions use optimization algorithms or try to directly construct a motion function allowing online trajectory generation. In this contribution, we follow the latter strategy and present an approach which first deals with the situation where initial and final accelerations are 0, and then relates the general case as much as possible to this situation. This leads to a classification with just four major cases. A continuity argument guarantees full coverage of all situations which is not the case or is not clear for other available algorithms. We present several examples that show the variety of different situations and, thus, the complexity of the task. We also describe an implementation in MATLAB® and results from a huge number of test runs regarding accuracy and efficiency, thus demonstrating that the algorithm is suitable for online trajectory generation.

Keywords: online trajectory generation; fast motion functions; seven segment proﬁle; jerk restriction

1. Introduction Citation: Alpers, B. On Fast Jerk–, Acceleration– and Velocity–Restricted For achieving high throughput in handling machines, one of the most basic tasks Motion Functions for Online consists of quickly moving from one position to the next one, taking into account machine Trajectory Generation. Robotics 2021, restrictions regarding velocity, acceleration and jerk. This task is generally known as 10, 25. https://doi.org/10.3390/ “path and trajectory planning” for robots and other automatic machines [1,2], and beside robotics10010025 short execution times, other goals such as low energy consumption or low jerk have been considered in [3]. In this contribution, we restrict ourselves to a one-dimensional case Received: 7 January 2021 where a certain distance ∆s is given and a motion function s(t) is to be found, such that Accepted: 26 January 2021 symmetric restrictions regarding velocity v(t), acceleration a(t) and jerk j(t) apply, i.e., Published: 1 February 2021 −jmax ≤ j(t) ≤ jmax, −amax ≤ a(t) ≤ amax and −vmax ≤ v(t) ≤ vmax. The limit to the jerk function implicitly requires the acceleration function to be contin- Publisher’s Note: MDPI stays neutral uous, whereas the jerk function is allowed to have discontinuities. This task can be sub- with regard to jurisdictional claims in divided into subtasks regarding the given initial and final states (velocity and acceleration). published maps and institutional affil- In the classification given in the respective guideline of the German Association of Engi- iations. neers [4], 16 combinations are stated depending on whether the initial state (v(t0), a(t0)) is (0, 0) (called “rest”: R), (6= 0, 0) (G), (0, 6= 0) (U) or (6= 0, 6= 0) (B) (German abbreviations). The most well-known task is RR (“rest-to-rest”), which, in robotics, is usually called “point- to-point motion”. Kröger and Wahl [5] provide a different classification that also includes Copyright: © 2021 by the author. the types of restrictions, but for our purposes, the VDI classification suffices. Licensee MDPI, Basel, Switzerland. The task of finding a fast (sometimes also called “optimal”) motion function has This article is an open access article been investigated for several decades. Roughly, one can distinguish between two kinds distributed under the terms and of approaches: In the first approach, the task is formulated as a restricted optimization conditions of the Creative Commons problem and then well-known or self-developed algorithms from the fields of parameter Attribution (CC BY) license (https:// optimization or optimal control are used [6–10]. The class of functions considered are creativecommons.org/licenses/by/ often spline functions [6,7,10]. Lin et al. [8] use a specific class of piecewise-defined 4.0/).

Robotics 2021, 10, 25. https://doi.org/10.3390/robotics10010025 https://www.mdpi.com/journal/robotics Robotics 2021, 10, 25 2 of 26

functions (“seven segments”) which are optimized. Such approaches have the advantage that optimization can take into account further aspects by modifying the objective (goal) function, e.g., energy consumption or deviation from a certain path, and they can also handle variable restrictions on velocity, acceleration and jerk. On the other hand, they are more time-consuming, such that they are mainly used for so-called off-line trajectory construction taking place in advance as opposed to on-line trajectory planning where the trajectory is changed “on the fly” during a controller cycle due to unforeseen events [11,12]. The second approach constructs the motion function directly using piecewise-defined functions [1,13–22]. We first consider the most basic task, RR. Choosing a piecewise constant jerk function leads to the so-called seven-segment profile (depicted in Figure1 ) with continuous acceleration but discontinuous jerk, which has been fully specified in [1] (pp. 90–93) and can be found in industrial implementations [13] (Section 11.2). The approach has been extended to a 15-segment profile with piecewise constant snap in [1] (pp. 107–114), making the jerk function continuous. Considerable improvements have been achieved by using higher-order polynomials or the sigmoid function [14,15]. They provide fast motion functions with continuity in higher-order derivatives (or even all derivatives [15]) and are, hence, very suitable for vibration reduction. Smoothing can also be achieved by applying moving averages, as is shown in [16]. It is open whether and how the use of such advanced functions can be extended to motion tasks other than RR. When non-zero values are prescribed for initial and/or final velocities and/or acceleration, direct construction seems to be much harder. Nearly all contributions known to the author use the relatively simple seven-segment profile since this already leads to complex computations. The subtask GG (zero acceleration at initial and final states) has been dealt with for seven- segment profiles by [17] and [1], but both solutions are incomplete (for [17], cf. Kröger [10] (Section 4.1.2)) or can be improved (in [1], absolute values of the occurring minimal and maximal acceleration are assumed to be equal even if they are lower than the limit value, cf. Example 3.11, p. 84). Using, also, the seven-segment profile, Haschke et al. [18] developed an algorithm for the subtask BR (arbitrary initial state, rest), and Kröger and Wahl [5] and Kröger [12] provided a solution for the subtask BG, going through all possible branches in their decision tree. Their work shows the complexity of the task since they could provide only a small portion of their decision trees in their publications (cf. [5] (p. 106) for information on the number of nodes in these trees). For the general task BB (arbitrary values for initial and final velocities and accelerations), Ezair et al. [19] developed a recursive algorithm which even enables them to handle a case where initial and final values as well as restrictions are given for an arbitrary number of derivatives and higher orders of continuity can be achieved, but only for order 2 (i.e., continuous acceleration); the algorithm is fast enough for online trajectory generation. In this case, it provides a seven-segment profile but not necessarily the fastest one. As we will show, there might be several seven-segment profiles solving the problem with quite different execution times. Broquère [20] and Broquère et al. [21] present a complex algorithm for the seven-segment profile but, as observed by Kröger [12] (Section 4.1.2), their work does not provide a solution in all cases, and we will give examples in Section4 for configurations which are easily overseen. Recently, Sidobre and Desormeaux [22] modified the work in [20,21] and developed an algorithm where they claim to be able to reduce the general situation to just four simple sub-cases, but there is no proof, and the claim is not supported by extensive test runs with random values for all input data. It is the goal of this contribution to provide an algorithm for constructing a seven- segment profile for the general BB task which provenly covers all possible situations, has a clear systematic structure with manageable complexity and is efficient enough for being suitable for online trajectory generation. Note that our results have some overlap with those of Sidobre and Desormeaux [22], but we apply a different approach by reducing the general BB task as much as possible to the GG subtask using intermediate velocities. We do not claim to provide an optimal solution (although this might be the case) since we do not have mathematical proof for this. Therefore, we just label our solution as “fast” since, Robotics 2021, 10, x FOR PEER REVIEW 3 of 27

general BB task as much as possible to the GG subtask using intermediate velocities. We Robotics 2021, 10, 25 3 of 26 do not claim to provide an optimal solution (although this might be the case) since we do not have mathematical proof for this. Therefore, we just label our solution as “fast” since, at any time, at least one restriction is exactly fulfilled, but we also show that this does not atimply any time, global at leastoptimality. one restriction is exactly fulfilled, but we also show that this does not imply globalThe paper optimality. is structured as follows: In the next section, we present the seven-segment motionThe paperprofile is with structured modifications as follows: and Inrecall the nextthe “acceleration–velocity section, we present the plane” seven-segment introduced motionby Broquère profile et with al. modifications[20] and Broquère and recall[21]. Section the “acceleration–velocity 3 models the subtask plane” GG introducedwhere initial byand Broqu finalè revelocities et al. [20 are] and 0 and, Broqu basedère [on21 ].this, Section Section3 models 4 deals the with subtask the general GG where task. Section initial 5 anddiscusses final velocities implementation are 0 and, issues based and on this,describe Sections the4 deals results with of the the validation general task. in MATLAB Section5 ®. discussesSection 6 implementation summarizes the issues results and and describes discusses the further results work. of the validation in MATLAB®. Section6 summarizes the results and discusses further work. 2. Basic Models, Quantities and Visualizations 2. Basic Models, Quantities and Visualizations The task we want to solve in this contribution consists of finding a fast motion func- tionThe when task the we following want to solve eight in values this contribution are given: consists of finding a fast motion function when the following eight values are given: 𝑗 , 𝑎 , 𝑣 (each >0), such that −𝑗 ≤𝑗(𝑡)≤𝑗 , −𝑎 ≤𝑎(𝑡)≤𝑎 jmax, amax, vmax (each > 0), such that −jmax ≤ j(t) ≤ jmax, −amax ≤ a(t) ≤ amax and and −𝑣 ≤𝑣(𝑡)≤𝑣; initial conditions: 𝑣, 𝑎; end conditions: 𝑣, 𝑎; distance: Δs −(arbitraryvmax ≤ v (realt) ≤ numbers).vmax; initial conditions: vA, aA; end conditions: vE, aE; distance: ∆s (arbitraryNote real that numbers). the distance is meant to be the difference between the final and the initial Note that the distance is meant to be the difference between the final and the initial value of the path scale (∆𝑠 = 𝑠 −𝑠), not the “travelled distance”, which might be much s = s − s valuelonger of thewhen path the scale motion (∆ goesE backA ),first not and the then “travelled forward distance”, again. which might be much longer when the motion goes back first and then forward again. As in [20–22], we work with a so-called 7-segment motion profile, shown in Figure 1. As in [20–22], we work with a so-called 7-segment motion profile, shown in Figure1 . It is based on a piecewise constant jerk function together with initial conditions on velocity It is based on a piecewise constant jerk function together with initial conditions on velocity and acceleration. In this profile, the motion function has, at most, 4 segments where the and acceleration. In this profile, the motion function has, at most, 4 segments where the jerk is ±𝑗 and, at most, 3 segments with zero jerk. In the classical RR situation (“rest- jerk is ±j and, at most, 3 segments with zero jerk. In the classical RR situation (“rest- to-rest”,max see Figure 1a), the sign pattern of the jerk segments is (+,−,−,+), but we allow to-rest”, see Figure1a), the sign pattern of the jerk segments is (+, −, −, +), but we allow arbitrary patterns such as (+, −, +, −) to occur in order to have full coverage of all possi- arbitrary patterns such as (+, −, +, −) to occur in order to have full coverage of all possible ble configurations as will be seen later. Formally stated, we have: configurations as will be seen later. Formally stated, we have: 𝑗(𝑡) =±𝑗 in [𝑡,𝑡], [𝑡,𝑡], [𝑡,𝑡]and [𝑡,𝑡]; 𝑗(𝑡) =0 in [𝑡,𝑡], [𝑡,𝑡] and [𝑡 ,𝑡 ]. j(t) = ±jmax in [t0, t1], [t2, t3], [t4, t5] and [t6, t7]; j(t) = 0 in [t1, t2], [t3, t4] and [t5, t6]. Note that all of these intervals might have zero length. In the example displayed in FigureNote 1a, that the all maximal of these values intervals for mightvelocity have and zero acceleration length. In are the reached, example which displayed need innot Figurealways1a, be the the maximal case. Figure values 1b forprovides velocity an and example acceleration for the general are reached, situation which where need 𝑣 not, 𝑎, always𝑣 and be 𝑎 the are case. non-zero. Figure1 Inb providesthat example, an example only 𝑎 for the is reached general and situation two of where the potentiallyvA, aA, vEsevenand asegmentsE are non-zero. have length In that zero. example, only amax is reached and two of the potentially seven segments have length zero.

(a) (b)

FigureFigure 1. 1.Seven-segment Seven-segment proﬁle profile (a )(a for) for RR; RR; (b ()b for) for BB—two BB—two segments segments with with length length 0. 0.

InIn order order to to visualize visualize the the change change of of velocity velocity and and acceleration acceleration in in such such seven-segment seven-segment motionmotion functions, functions, Broqu Broquèreère [20 [20,21],21] introduced introduced the the velocity–acceleration velocity–acceleration plane, plane, shown shown in in Figure2. In this plane, motion functions are displayed mathematically as parametric curves (v(t), a(t)), t ∈ [t0, t7]. When one assumes that j(t) = ±jmax or 0, then the curves “move” 1 2 along square root curves or horizontally (e.g., from a(t) = jmaxt and v(t) = 2 jmaxt one Robotics 2021, 10, x FOR PEER REVIEW 4 of 27

Figure 2. In this plane, motion functions are displayed mathematically as parametric curves 𝑣(𝑡),𝑎(𝑡), 𝑡 ∈ [𝑡 ,𝑡 ]. When one assumes that 𝑗(𝑡) =±𝑗 or 0, then the curves Robotics 2021, 10, 25 4 of 26 “move” along square root curves or horizontally (e.g., from 𝑎(𝑡) =𝑗𝑡 and 𝑣(𝑡) = 𝑗 𝑡 one obtains 𝑎(𝑣) = 2𝑗 𝑣). The square root curves can be of the kind 𝑎= ±𝑐(𝑣 − 𝑐 ) (open to pthe right) or 𝑎=±𝑐(𝑐 −𝑣) (open to the left) with constantsp obtains a(v) = 2jmaxv). The square root curves can be of the kind a = ± c1(v − c2) 𝑐,𝑐. We assume that the horizontalp motion only occurs when 𝑎(𝑡) =±𝑎 since otherwise, (openfaster tomotion the right) is possible. or a = ± Motionc1(c2 −alonv)g(open the square to the left)root with curves constants is onlyc 1possible, c2. We assume clockwisethat since the horizontalwhen the accelera motion onlytion is occurs positive when (resp.a(t) nega= ±ative),max since the velocity otherwise, must faster in- motion crease (resp.is possible. decrease). Motion The only along points the square where roota fast curves motion is function only possible might clockwisestay for a time since when the acceleration is positive (resp. negative), the velocity must increase (resp. decrease). of length >0 are the points (±𝑣,0). This is the case when there is a segment with zero jerk andThe acceleration. only points Figure where 2a ashows fast motion a repres functionentation mightof the classical stay for aRR time situation of length where >0 are the points (±vmax,0). This is the case when there is a segment with zero jerk and acceleration. 𝑣 and 𝑎 are reached. Note that only points within the area bounded by the left Figure2a shows a representation of the classical RR situation where vmax and amax are and right square root curves and the horizontal lines at 𝑎 and −𝑎 are admissible reached. Note that only points within the area bounded by the left and right square root since only those points can be reached from (0,0) and one can go back from there to (0,0) curves and the horizontal lines at a and −a are admissible since only those points without violating the restrictions (a larger openingmax of maxthe square root curve corresponds can be reached from (0,0) and one can go back from there to (0,0) without violating the to a higher value of 𝑗 ). The velocity–acceleration plane has one particular “pitfall” restrictions (a larger opening of the square root curve corresponds to a higher value of j ). where misunderstandings can easily occur, so special care is called for: It does not show max The velocity–acceleration plane has one particular “pitfall” where misunderstandings can the distance ∆s, and this distance heavily depends on where in the velocity–acceleration easily occur, so special care is called for: It does not show the distance ∆s, and this distance plane a curve is placed. The length of the curve has nothing to do with the distance ∆s. heavily depends on where in the velocity–acceleration plane a curve is placed. The length The blue curve in Figure 2b depicts the motion function of Figure 1b (with positive ∆s) in of the curve has nothing to do with the distance ∆s. The blue curve in Figure2b depicts the velocity–acceleration plane. The green curve has the same initial and end conditions the motion function of Figure1b (with positive ∆s) in the velocity–acceleration plane. The but a negativegreen curve value has for the∆s. same initial and end conditions but a negative value for ∆s.

(a) (b)

Figure 2.Figure Velocity–acceleration 2. Velocity–acceleration curve for curve the motion for the function motion function(a) in Figure (a) in 1a; Figure (b) in1a; Figure ( b) in 1b. Figure 1b.

3. The Subtask3. The Subtask GG: Zero GG: Acceleration Zero Acceleration Initially Initially and Finally and Finally In this section, we consider a motion with zero acceleration initially and finally. Let In this section, we consider a motion with zero acceleration initially and finally. Let vA 𝑣 resp. 𝑣 be the initial resp. final velocity. We first determine the time 𝑇(𝑣 ,𝑣 ) neces- resp. vE be the initial resp. final velocity. We first determine the timeT(vA, vE) necessary sary to getto get from from the the initial initial to the to the final final state state by byapplying applying first first maximal maximal (minimal) (minimal) jerk jerk and and then then minimalminimal (maximal) (maximal) jerkjerk where where in between in between the jerk the might jerk might be zero be and zero the and acceleration the acceleration equal toequal ±𝑎 to. ±Examplesamax. Examples are shown are shownin Figure in 3 Figure in the3 velocity–accelerationin the velocity–acceleration plane. Type plane. Type |𝑣 −𝑣 | 1 depicts1 depictsthe situation the situation where where |v isE −sov smallA| is sothat small maximal/minimal that maximal/minimal acceleration acceleration is not reached,is not whereas reached, in whereastype 2, the in latter type is 2, the the case. latter When is the ±𝑎 case. is Whenjust reached,±amax weis justhave reached, 2 2 amax amax |𝑣 −𝑣we| = have ,| vsoE −typevA |1 =occurs ,when so type |𝑣 1−𝑣 occurs| < when and|vE −typevA |2

Robotics 2021, 10, x FOR PEER REVIEW 5 of 27 Robotics 2021, 10, 25 5 of 26

(a) (b)

Figure 3.Figure Switching 3. Switching between betweenvelocities: velocities: (a) positive (a) positiveacceleration; acceleration; (b) negative (b) negativeacceleration. acceleration.

T In type 1,In the type maximally 1, the maximally reached reachedacceleration acceleration is 𝑎 =𝑗 is am ∙=, jwheremax· , T where is the Toverallis the overall 2 2 q T T |vE−vA| time. Hence, |vE − vA|= am· = jmax· , and thus, T|=2| . The distance travelled time. Hence, |𝑣 −𝑣| =𝑎 ∙ =𝑗2 ∙ , and 4thus, 𝑇=2 . Thejmax distance travelled q vA+vE |vE−vA| in this case is S(vA, vE) = ·T(vA, vE) = (vA + vE)· . In type 2, we have ( ) (2 ) ( ) || jmax in this case is 𝑆 𝑣,𝑣 = ∙𝑇 𝑣,𝑣 = 𝑣 +𝑣| − ∙ | . In type 2, we have |v − v | = T − amax ·a , and hence, T = vE vA +amax . The distance travelled in this E A jmax max | | amax jmax |𝑣 −𝑣 | =𝑇− ∙𝑎 , and hence, 𝑇= + . The distance travelled in this vA+vE vA+vE |vE−vA| amax case is againS(vA, vE) = ·T(vA,vE) = · + . We summarize 2 |2| amax jmax case is again 𝑆(𝑣,𝑣) = ∙𝑇(𝑣,𝑣) = ∙ + . We summarize the the result as follows: result as follows:  q | − | 2  2 vE vA i f |v − v | < amax  jmax E A jmax T(vA, vE) = (1) |𝑣 |−𝑣− | | 𝑎 2 ⎧  vE vA + amax i f |v −v | ≥ amax ⎪ 2 amax 𝑖𝑓 |𝑣jmax −𝑣| vE, vm2 < vA, vA ≤ vm3 ≤ vE), where the third situation is not the fastest as we ⎩ 2 𝑎 𝑗 𝑗 will see. Figure 3 shows examples for the GG situation, each of which works just for one specific given distance. For other distances, the curve goes from 𝑣 to an intermediate velocity 𝑣 and then further on to the final velocity 𝑣, as is shown in Figure 4 for three situations (𝑣 >𝑣, 𝑣 <𝑣, 𝑣 ≤𝑣 ≤𝑣), where the third situation is not the fastest as we will see.

(a) (b)

Figure 4.Figure Examples 4. Examples for getting for from getting 𝑣 fromto 𝑣v: A(ato) velocity–accelerationvE:(a) velocity–acceleration plane; (b plane;) velocity (b) velocityover time. over time.

From (1) and (2), we can easily compute the overall time and distance when going via an intermediate velocity 𝑣:

𝑇(𝑣,𝑣,𝑣) =𝑇(𝑣,𝑣) +𝑇(𝑣,𝑣) , 𝑆(𝑣,𝑣,𝑣) =𝑆(𝑣,𝑣) +𝑆(𝑣,𝑣) (3)

In order to visualize how the distance changes when 𝑣 and 𝑣 are fixed and 𝑣 runs from 𝑣 to 𝑣, one can plot 𝑆(𝑣,𝑣,𝑣) over 𝑣, as shown in Figure 5a. For seeing in one plot how 𝑇(𝑣,𝑣,𝑣) varies simultaneously, a parametric curve (𝑆(𝑣,𝑣,𝑣),𝑇(𝑣,𝑣,𝑣),𝑡 ∈[−𝑣 , 𝑣]) is adequate, as shown in Figure 5b. Both parts provide interesting information which will also be of use for the general situation (Section 4). If 𝑣 =𝑣 (resp. 𝑣 =𝑣) and remains constant for a certain time interval, the distance increases (resp. decreases) linearly with time. Therefore, in Figure 5b, the graph could be extended linearly to the left and to the right. We further observe that the distance varies continuously with 𝑣, which follows mathematically from (2), (3); therefore, for any given distance, a solution can be found.

(a) (b)

Figure 5. For 𝑣 >0 and 𝑣 >0: (a) S over 𝑣; (b) T over S.

The behavior shown in Figure 5a might be astonishing at first sight; the distance increases first when 𝑣 is below 𝑣 before decreasing monotonously. We explain this using Figure 4b. Here, there are two controversial effects: decreasing 𝑣 makes the velocity curve lie “lower”, leading to a smaller integral (less distance); on the other hand, the time needed to get to 𝑣 becomes larger, leading to a longer integration interval (larger distance). Obviously, in the long run, the first effect becomes dominant, but in the short term, the second one is dominant, leading to an increase in distance. This gives, already, an

Robotics 2021, 10, x FOR PEER REVIEW 6 of 27

(a) (b) Robotics 2021, 10, 25 6 of 26 Figure 4. Examples for getting from 𝑣 to 𝑣: (a) velocity–acceleration plane; (b) velocity over time.

From (1) and (2), we can easily compute the overall time and distance when going via an intermediate Fromvelocity (1) and𝑣: (2), we can easily compute the overall time and distance when going via an intermediate velocity vm:

𝑇(𝑣,𝑣,𝑣) =𝑇(𝑣,𝑣) +𝑇(𝑣,𝑣) , 𝑆(𝑣,𝑣,𝑣) =𝑆(𝑣,𝑣) +𝑆(𝑣,𝑣) (3) T(vA, vE, vm) = T(vA, vm) + T(vm, vE) , S(vA, vE, vm) = S(vA, vm) + S(vm, vE) (3)

In order to visualize how the distance changes when 𝑣 and 𝑣 are fixed and 𝑣 In order to visualize how the distance changes when vA and vE are ﬁxed and vm runs runs from 𝑣 to 𝑣, one can plot 𝑆(𝑣,𝑣,𝑣) over 𝑣, as shown in Figure 5a. For from vmin to vmax, one can plot S(vA, vE, vm) over vm, as shown in Figure5a. For seeing seeing in one plot how 𝑇(𝑣,𝑣,𝑣) varies simultaneously, a parametric curve in one plot how T(vA, vE, vm) varies simultaneously, a parametric curve (S(vA, vE, vm), (𝑆(𝑣,𝑣,𝑣),𝑇(𝑣,𝑣,𝑣),𝑡 ∈[−𝑣 , 𝑣]) is adequate, as shown in Figure 5b. Both T(v , v , vm), t ∈ [−vmax , vmax]) is adequate, as shown in Figure5b. Both parts provide parts provide interestingA E information which will also be of use for the general situation interesting information which will also be of use for the general situation (Section4). If (Section 4). If 𝑣 =𝑣 (resp. 𝑣 =𝑣) and remains constant for a certain time inter- vm = vmax (resp. vm = vmin) and remains constant for a certain time interval, the distance val, the distanceincreases increases (resp. (resp. decreases) decreases) linearly linearly with with time. time. Therefore, Therefore, in in Figure Figure5 b,5b, the the graph could be graph could beextended extended linearlylinearly toto thethe leftleft andand toto thethe right.right. WeWe further further observeobserve thatthat thethe distance varies distance varies continuously with 𝑣, which follows mathematically from (2), (3); there- continuously with vm, which follows mathematically from (2) and (3); therefore, for any fore, for any givengiven distance, distance, a solution a solution can can bebe found. found.

(a) (b) 𝑣 >0 𝑣 >0 𝑣 Figure 5. For Figure and 5. For:v (Aa) >S over0 and vE; (>b)0: T (overa) S overS. vm;(b) T over S.

The behavior shownThe behavior in Figure shown 5a might in Figurebe astonishing5a might at be first astonishing sight; the atdistance first sight; in- the distance creases first whenincreases 𝑣 is firstbelow when 𝑣 v beforem is below decreasingvA before monoto decreasingnously. monotonously. We explain this We us- explain this using ing Figure 4b. Here,Figure there4b. are Here, two there controversial are two controversial effects: decreasing effects: 𝑣 decreasing makes the velocityvm makes the velocity curve lie “lower”,curve leading lie “lower”, to a smaller leading integral to a (less smaller distance); integral on (lessthe other distance); hand, onthe thetime other hand, the needed to get totime 𝑣 neededbecomes to larger, get to vleadingE becomes to a larger,longer leadingintegration to a interval longer integration (larger dis- interval (larger tance). Obviously,distance). in the long Obviously, run, the in first the effect long becomes run, the firstdominant, effect becomesbut in the dominant, short term, but in the short the second oneterm, is dominant, the second leading one is dominant,to an increase leading in distance. to an increase This ingives, distance. already, This an gives, already, an impression of the complexity of the situation, which is easily overlooked, leading to false assumptions. Figure5a shows that depending on the given distance, there are up to five solutions to

the motion problem using an intermediate velocity vm. Five solutions occur when the given distance is slightly above S(vA, vE). This also shows that the fact that, at any time, one of the restrictions is taken does not guarantee optimality. Figure5b shows which solution is the fastest one; for a given distance, the lowest intersection of the vertical line at that distance with the curve provides the best solution. One can also observe that the best time is discontinuous over the given distance. In [22], Sidobre and Desormeaux also provide examples for this phenomenon. They also use a parametric curve of distance over time needed, but they use a different parameter (time). For ﬁnding the best solution within the class of seven-segment functions, one has to solve the equation S(vA, vE, vm) = ∆s. We will explain how to do this efﬁciently in Section5. Robotics 2021, 10, 25 7 of 26

4. The General Task: Arbitrary Velocity and Acceleration, Initially and Finally In this section, we consider the general case where initial and final velocities and accelerations can be arbitrary. We start with introducing some auxiliary functions that will be helpful in the later exposition. For an arbitrary admissible point (v0, a0) in the velocity–acceleration plane, vb(v0, a0) is defined to be the velocity for which the following holds: If you start at the point (vb(v0, a0), 0) and apply minimum (resp. maximum) jerk if a0 < 0 (resp. a0 ≥ 0), then you reach (v0, a0). The index b here stands for “backward”, i.e., going backward from (v0, a0). Figure6 illustrates the definitions. Analogously, v f (v0, a0) is defined to be the velocity for which the following holds: If you start at the point (v0, a0) and apply maximum (resp. minimum) jerk if a0 < 0 (resp. a0 ≥ 0), then you reach (v f (v0, a0), 0). The index f here stands for “forward”. An easy computation yields:

1 a2 1 a2 vb(v, a) = v − sgn(a)· , v f (v, a) = v + sgn(a)· . (4) 2 jmax 2 jmax

Moreover, let Sb(v0, a0) be the additional distance when one goes from (vb(v0, a0), 0) to (v0, a0), applying minimum (resp. maximum) jerk if a0 < 0 (resp. a0 ≥ 0). Analo- gously, S f (v0, a0) is deﬁned to be the distance when one goes from (v0, a0) to (v f (v0, a0), 0), applying maximum (resp. minimum) jerk if a0 < 0 (resp. a0 ≥ 0). Again, an easy computation shows:

|a| 1 a3 |a| 1 a3 ( ) = ( ) + · ( ) = ( ) − · Sb v, a vb v, a 2 , S f v, a v f v, a 2 (5) jmax 6 jmax jmax 6 jmax

Finally, let Ta(a0) be the time needed to go from a point (v, a0) (v arbitrary) to v f (v, a0) (or, equivalently, to go from vb(v, a0) to (v, a0)), applying maximum resp. minimum jerk. Robotics 2021, 10, x FOR PEER REVIEWWe have: 8 of 27 |a| Ta(a) = (6) jmax

Figure 6. IllustrationIllustration of au auxiliaryxiliary functions.

If one changes the positive direction of thethe distance, then the original problem is transformed into a a problem problem with with data data −Δ−∆s,s ,−𝑣−vA, ,−𝑎−aA, ,−𝑣−vE andand −𝑎−aE which which hashas the same solution regarding time time intervals, intervals, only only the the sign sign factors factors for for 𝑗jmax have have toto bebe changedchanged (see Broquère,Broquère, 2011). Therefore, we can restri restrictct ourselves to two main situations: a𝑎A ≤0≤ 0 and 𝑎aE ≥≥00, or or a𝑎A ≥≥00 andand a𝑎E≥≥00.. The The other other combinationscombinations cancan bebe transformedtransformed intointo thethe former ones by changing the positive direction of distance.distance. These two main cases are further split up into four sub-cases which depend additionallyadditionally ononv 𝑣b((𝑣vA,,𝑎aA), v𝑣b((𝑣vE,,𝑎aE)),, v𝑣f((𝑣vA,𝑎, aA)) and 𝑣vf(𝑣(vE,𝑎, aE));; see see Table Table1 1..

Table 1. Basic cases in the BB situation.

Case Conditions on 𝒂𝑨, 𝒂𝑬 Conditions on 𝒗𝒃,𝒗𝒇

Case A 𝑎 ≤0 and 𝑎 ≥0 𝑣(𝑣,𝑎)≤𝑣(𝑣,𝑎) Case B 𝑎 ≤0 and 𝑎 ≥0 𝑣(𝑣,𝑎) >𝑣(𝑣,𝑎) Case C 𝑎 ≥0 and 𝑎 ≥0 𝑣(𝑣,𝑎)≤𝑣(𝑣,𝑎) and 𝑣(𝑣,𝑎)≤𝑣(𝑣,𝑎) Case D 𝑎 ≥0 and 𝑎 ≥0 𝑣(𝑣,𝑎) >𝑣(𝑣,𝑎) or 𝑣(𝑣,𝑎) >𝑣(𝑣,𝑎) From Table 1, it is logically clear that this is a complete coverage of all cases. We will see that only in case C do we need a further division into two sub-cases. In the sequel, we use the following abbreviations: • 𝐴=(𝑣,𝑎),𝐸 =(𝑣,𝑎); • 𝑣 =𝑣(𝑣,𝑎), 𝑣 =𝑣(𝑣,𝑎), 𝑣 =𝑣(𝑣,𝑎) , 𝑣 =𝑣(𝑣,𝑎); • 𝑆 =𝑆(𝑣,𝑎), 𝑆 =𝑆(𝑣,𝑎), 𝑆 =𝑆(𝑣,𝑎) , 𝑆 =𝑆(𝑣,𝑎); • 𝑇 =𝑇(𝑎), 𝑇 =𝑇(𝑎). Our general procedure in the following treatment of the cases is based on the principle that we try to relate the general case BB as much as possible to the special case GG by either embedding the general case in a GG situation or extending a GG situation to the general case BB. We discuss case A in detail, elaborating on the lines of argumentation, and will then treat cases B–D shorter. Case A: 𝑎 ≤0 and 𝑎 ≥0, 𝑣 ≤𝑣 In this case, E lies “right of” A in the sense that E lies on a square root curve (open to the right) right of the square root curve (open to the right) on which the point A lies or the curves are identical. For a certain subset of all distances Δs, we can embed the task in a GG task for going from (𝑣,0) to (𝑣,0) via a 𝑣 ≤𝑣 as is shown in Figure 7a. Making 𝑣 smaller and inserting a phase with constant velocity 𝑣 enables us to realize arbitrarily small values for Δs (mathematically spoken: Δs goes to −∞ when the length of the phase with constant 𝑣 goes to ∞). We get the corresponding distances by computing:

Robotics 2021, 10, 25 8 of 26

Table 1. Basic cases in the BB situation.

Case Conditions on aA, aE Conditions on vb,vf

Case A aA ≤ 0 and aE ≥ 0 v f (vA, aA) ≤ vb(vE, aE) Case B aA ≤ 0 and aE ≥ 0 v f (vA, aA) > vb(vE, aE) Case C aA ≥ 0 and aE ≥ 0 vb(vA, aA) ≤ vb(vE, aE) and v f (vA, aA) ≤ v f (vE, aE) Case D aA ≥ 0 and aE ≥ 0 vb(vA, aA) > vb(vE, aE) or v f (vA, aA) > v f (vE, aE)

From Table1, it is logically clear that this is a complete coverage of all cases. We will see that only in case C do we need a further division into two sub-cases. In the sequel, we use the following abbreviations:

• A = (vA, aA), E = (vE, aE); • vbA = vb(vA, aA), v f A = v f (vA, aA), vbE = vb(vE, aE), v f E = v f (vE, aE); • SbA = Sb(vA, aA), S f A = S f (vA, aA), SbE = Sb(vE, aE), S f E = S f (vE, aE); • TA = Ta(aA), TE = Ta(aE). Our general procedure in the following treatment of the cases is based on the principle that we try to relate the general case BB as much as possible to the special case GG by either embedding the general case in a GG situation or extending a GG situation to the general case BB. We discuss case A in detail, elaborating on the lines of argumentation, and will then treat cases B–D shorter.

Case A: aA ≤ 0 and aE ≥ 0, v f A ≤ vbE In this case, E lies “right of” A in the sense that E lies on a square root curve (open to the right) right of the square root curve (open to the right) on which the point A lies or the curves are identical. For a certain subset of all distances ∆s, we can embed the task in a GG task for going from (vbA, 0) to (v f E, 0) via a vm ≤ v f A as is shown in Figure7a. Making vm smaller and inserting a phase with constant velocity vmin enables us to realize arbitrarily small values for ∆s (mathematically spoken: ∆s goes to −∞ when the length of the phase with constant vmin goes to ∞). We get the corresponding distances by computing:

S(vm) = S(vbA, v f E, vm) − SbA − S f E for vm ≤ v f A (Part I) (7)

Note that this holds for vmin ≤ vm, but we will disregard, ﬁrst, this restriction on vm and allow vm to be arbitrarily small and correct that later. We can similarly cover arbitrarily large distances by extending the GG task going from (v f A, 0) to (vbE, 0) via a vm ≥ vbE, as is shown in Figure7b. When vm reaches vmax, one can insert a phase with constant velocity vmax. Again, we will disregard this restriction on vm ﬁrst and correct that later. We obtain the corresponding distances by computing:

S(vm) = S(v f A, vbE, vm) + S f A + SbE for vm ≥ vbE (Part III). (8)

As we will see in examples, there might be a gap between the interval of distances covered in Part I and that covered in Part III. Distances in this gap can be achieved in two ways. We again extend the GG task going from (v f A, 0) to (vbE, 0) via a vm with vbE < vm < v f E, where we let vm run from right to left (!), i.e., from v f E to vbE, and then “cut off” the part going from vbE to vm and back to vbE (shown in Figure7c as a dashed pink curve). This continuously connects the distance covered in Part I when using vm = v f A with the distance covered in Part III when using vm = vbE. Therefore, we have a full coverage of all possible real distances. We obtain the corresponding distances by computing:

S(vm) = S(v f A, vbE, vm) + S f A + SbE − 2·S(vbE, vm) for vbE < vm < v f E (Part IIa). (9) Robotics 2021, 10, 25 9 of 26

Another way to close the gap is a bit more complicated and is shown in Figure7d. The ﬁrst part up to point C is the same as in Part IIa, but then we have the sequence −jmax, jmax, −jmax instead of jmax, −jmax, jmax in Part IIa. We can embed part of the curve in a GG task going from (vbE, 0) to (v f E, 0) via a vm with v f A < vm < vbE, as is shown in Figure7d . n 2 o 2 Actually, v can be replaced by max v , v − amax since at v − amax , the minimal f A f A bE jmax bE jmax acceleration −amax is reached and going on to the left with vm does not further change the distance ∆s since the parts added are cut off again later, as can be seen by drawing a ﬁgure which is omitted here. We can compute the corresponding distances in the following way:

S(vm) = S(vbE, v f E, vm) + S(v f A, vbE) + S f A − S f E − 2·S(vbE, vm) for

2 amax max v f A, vbE − < vm < vbE (Part IIb). (10) jmax In our experiments (see the examples below), we have encountered a situation where a solution using Part IIb provides a shorter time than a solution using Part IIa, but then the distances were already covered by Part I or III, which provided even shorter times. However, since we have no mathematical proof that this is always the case, we simply include both possibilities. As we will see in the implementation section, this adds only the effort of ﬁnding the roots of three additional polynomials. Note that in case of v f A = vbE or aE = 0, parts IIa/b do not occur. For each of the parts, the times needed can be easily computed:

T(vm) = T(vbA, v f E, vm) − TA − TE for vm ≤ v f A (Part I). (11)

T(vm) = T(v f A, vbE, vm) + TA + TE for vm ≥ vbE (Part III). (12)

T(vm) = T(v f A, vbE, vm) + TA + TE − 2·T(vbE, vm) for vbE < vm < v f E (Part IIa). (13)

T(vm) = T(vbE, v f E, vm) + T(v f A, vbE) + TA − TE − 2·T(vbE, vm) for 2 amax max v f A, vbE − < vm < vbE (Part IIb) (14) jmax

As in the GG task, we can illustrate different situations by plotting (S(vm), T(vm)) as a parametric curve for parts I–III. We present a few examples showing interesting situations and behavior. In the examples, all distances have the unit m, velocities m/s, acceleration m/s2 and jerk m/s3. Units are omitted for the sake of brevity. As restrictions, we have chosen values similar as the ones used in [1]: jmax = 30, amax = 10, vmax = 20. Example 1: No gap between Parts I and III, but Parts IIa or IIb are better. Initial and ﬁnal conditions: (vA, aA) = (10, −5), (vE, aE) = (18, 9). Note that the colors used in the following ﬁgures have nothing to do with the colors used in Figure7. The corresponding curve is shown in Figure8a, where the green part (Part I) goes on to the left, covering arbitrarily small (negative) distances, and the black part (Part III) goes on to the right, covering arbitrarily large distances. One can see that Parts I and III cover all distances but Parts IIa and IIb provide solutions needing less time in the interval of distances where they are valid. There is no huge difference between Parts IIa and IIb, but when zooming in (Figure8b), one can see that, in this example, the red part (Part IIa) is better. Figure8 also shows that for a given distance, there might be several solutions in different parts or in the same part, as we have already recognized in the GG situation in Section3. For example, for the distance ∆s = 18, there are two solutions in Part I and one solution each in Parts IIa and IIb. Robotics 2021, 10, x FOR PEER REVIEW 9 of 27

Robotics 2021, 10, 25 𝑆(𝑣)=𝑆(𝑣,𝑣,𝑣)−𝑆 −𝑆 for 𝑣 ≤𝑣 (Part I) (7)10 of 26

(a) (b)

Robotics 2021, 10, x FOR PEER REVIEW 11 of 27

Section 3. For(c )example, for the distance Δs = 18, there are two solutions(d) in Part I and one solutionFigure eachFigure 7. Case in 7.Parts A.Case (a) IIaPart A. and (a I;) Part( bIIb.) Part I; (b III;) Part (c) III;Part (c IIa;) Part (d) IIa; Part (d IIb.) Part IIb.

Note that this holds for 𝑣 ≤𝑣, but we will disregard, first, this restriction on 𝑣 and allow 𝑣 to be arbitrarily small and correct that later. We can similarly cover arbitrarily large distances by extending the GG task going from (𝑣,0) to (𝑣,0) via a 𝑣 ≥𝑣 , as is shown in Figure 7b. When 𝑣 reaches 𝑣, one can insert a phase with constant velocity 𝑣. Again, we will disregard this restriction on 𝑣 first and correct that later. We obtain the corresponding distances by computing:

𝑆(𝑣)=𝑆(𝑣,𝑣,𝑣)+𝑆 +𝑆 for 𝑣 ≥𝑣 (Part III). (8)

As we will see in examples, there might be a gap between the interval of distances covered in Part I and that covered in Part III. Distances in this gap can be achieved in two ways. (a) (b) We again extend the GG task going from (𝑣,0) to (𝑣,0) via a 𝑣 with 𝑣 < Figure 8. (a) No gap between Parts I and III, but Parts IIa or IIb are better; (b) zoom into area. Figure𝑣 8.(a<𝑣) No gap, where between we Partslet 𝑣 I and run III, from but Partsright IIa to orleft IIb (!), are i.e., better; from (b) zoom𝑣 to into 𝑣 area., and then “cut off” the part going from 𝑣 to 𝑣 and back to 𝑣 (shown in Figure 7c as a dashed pink ExampleExample 2: No gap 2: Nobetween gap between Parts I and Parts III, I Parts and III, IIa Parts and IIb IIa are and not IIb needed. are notneeded. curve). This continuously connects the distance covered in Part I when using 𝑣 =𝑣 Initial andInitial final and conditions: ﬁnal conditions: (𝑣 ,𝑎 ()v=,(a−18,) = −5(−),18,(𝑣 −,𝑎5)), =(v (−10,9), a ) =. (We−10, use 9). Wethe use the with the distance covered in Part III when A usingA 𝑣 =𝑣 . Therefore, E E we have a full cov- same accelerationssame accelerations as in Example as in Example 1 but shift 1 but the shift velocities the velocities to the left to by the 28. left In by this 28. way, In this way, erage of all possible real distances. We obtain the corresponding distances by computing: variationsvariations are also areproduced also produced in [22]. in [22]. The correspondingThe corresponding curve is curveshown is in shown Figure in 9a. Figure Again,9a. Again,the green the part green (Part part I) (Partgoes I) goes on to theon left, to thecovering left, covering arbitrarily arbitrarily small (neg smallative) (negative) distances, distances, and the andblack the part black (Part part III) (Part III) goes on goesto the on right, to the covering right, covering arbitrarily arbitrarily large distances. large distances. Parts I and Parts IIII cover and III all cover distances all distances and oneand of them one of provides them provides the best the solution, best solution, so Parts so IIa Parts and IIa IIb and are IIb not are needed. not needed. There There is is no no hugehuge difference difference between between Parts Parts IIa and IIa andIIb, but IIb, when but when zooming zooming in (Figure in (Figure 9b),9 oneb), one can can see see that, in this example, the blue part (Part IIb) is better. Part IIb provides a solution where the jerk changes sign three times (see Figure 7d), whereas in the solution of Part IIa, the jerk changes sign only two times. Therefore, the conjecture that solutions with less changes are better, which might look plausible at first sight, is wrong. What might be true is that the best solution (here from Part I) always has the lowest number of sign changes, but we do not have mathematical proof for this conjecture.

(a) (b) Figure 9. (a) No gap between Parts I and III, Parts IIa or IIb are not needed; (b) zoom into area.

Example 3: Gap between Parts I and III, Parts IIa or IIb are needed. Initial and final conditions: (𝑣,𝑎) = (−1.5, −5), (𝑣,𝑎) = (6.5,9). We use the same accelerations as in Example 1 but shift the velocities by −11.5. For Parts I and III, the re- marks given in Examples 1 and 2 are still valid; see Figure 10. However, here, in both parts, the distance changes monotonously. This example shows that without using the

Robotics 2021, 10, x FOR PEER REVIEW 11 of 27

Section 3. For example, for the distance Δs = 18, there are two solutions in Part I and one solution each in Parts IIa and IIb.

(a) (b) Figure 8. (a) No gap between Parts I and III, but Parts IIa or IIb are better; (b) zoom into area.

Example 2: No gap between Parts I and III, Parts IIa and IIb are not needed. Initial and final conditions: (𝑣,𝑎) = (−18, −5), (𝑣,𝑎) = (−10,9) . We use the same accelerations as in Example 1 but shift the velocities to the left by 28. In this way, variations are also produced in [22]. The corresponding curve is shown in Figure 9a. Again, the green part (Part I) goes Robotics 2021, 10, 25 on to the left, covering arbitrarily small (negative) distances, and the black part (Part III) 11 of 26 goes on to the right, covering arbitrarily large distances. Parts I and III cover all distances and one of them provides the best solution, so Parts IIa and IIb are not needed. There is no huge difference between Parts IIa and IIb, but when zooming in (Figure 9b), one can see that, that,in this in example, this example, the blue the part blue (Part part IIb (Part) is better. IIb) is better.Part IIb Part provides IIb provides a solution a solution where where the jerk thechanges jerk changessign three sign times three (see times Figure (see 7d), Figure whereas7d), whereas in the solution in the solution of Part IIa, of Part the IIa, the jerk changesjerk changes sign only sign two only times. two times. Therefore, Therefore, the theconjecture conjecture that that solutions solutions with with lessless changes changes areare better,better, whichwhich might look plausible at at first ﬁrst sight, sight, is is wrong. wrong. What What might might be be true true is that is that thethe best best solution solution (here (here from from Part Part I) I) always always has has the the lowest lowest number number of of sign sign changes, changes, but we but we dodo not not have have mathematical mathematical proof proof for for this this conjecture. conjecture.

(a) (b)

Figure 9. Figure(a) No 9.gap (a) betweenNo gap Parts between I and Parts III, IParts and III,IIa or Parts IIb IIaareor not IIb needed; are not ( needed;b) zoom( intob) zoom area. into area.

ExampleExample 3: Gap between 3: Gap betweenParts I and Parts III, I Parts and III, IIa Partsor IIb IIa are or needed. IIb are needed. Initial and ﬁnal conditions: (v , a ) = (−1.5, −5), (v , a ) = (6.5, 9). We use the Robotics 2021, 10, x FOR PEER REVIEW Initial and final conditions: (𝑣,𝑎) = (−1.5,A A −5), (𝑣,𝑎) = (6.5,9)E .E We use the12 same of 27 accelerationssame as accelerations in Example as 1 inbut Example shift the 1 butvelocities shift the by velocities −11.5. For by Parts−11.5. I and For III, Parts the I andre- III, the marks givenremarks in Examples given in Examples1 and 2 are 1 andstill 2valid; are still see valid; Figure see 10. Figure However, 10. However, here, in here,both in both parts, theparts, distance the distance changes changesmonotonously. monotonously. This example This example shows that shows without that withoutusing the using the constructionsconstructions of Parts ofIIa Parts or IIb, IIa a orcertain IIb, a range certain of range distances of distances (here, roughly (here, roughlyfrom 1.4 from to 3) 1.4 to 3) might notmight be covered. not be covered. In the example, In the example, Part IIa Partprovides IIa provides the better the solution better solution and we andhave we have not encountered an example of case A where Part IIb is best, but we do not have proof not encountered an example of case A where Part IIb is best, but we do not have proof that that thisthis is always is always so. so.

(a) (b)

Figure 10.Figure (a) Gap 10. between(a) Gap betweenParts I and Parts III, IParts and III,IIa Partsor IIb IIaare or needed; IIb are ( needed;b) zoom ( binto) zoom area. into area.

ExampleExample 4: Best solution 4: Best solutionis in Part is IIa in where Part IIa the where curve the is curve“bulging”. is “bulging”. Initial andInitial final and conditions: final conditions: (𝑣,𝑎) =(v(A−7.2,, aA) −5=),((−𝑣7.2,,𝑎−)5=), (0.8,9)(vE, aE. )We= use(0.8, the 9) .same We use the accelerationssame as accelerations in Example as 1 but in Example shift the 1velocities but shift theto the velocities left by − to17.2. the left by −17.2. In this example,In this example, zooming zoomingshows that shows the red that Part the IIa red curve Part “bulges” IIa curve to “bulges”the right (Fig- to the right ure 11b)(Figure (cf. similar 11b) (cf.examples similar in examples [22]). Theref in [22ore,]). Therefore,there is a theresmall isinterval a small of interval distances of distances to to the rightthe by rightabout by −5.34 about where−5.34 Part where IIa provides Part IIa provides the best thesolution, best solution, ending at ending the S-value at the S-valueof of the utmostthe utmostright point right of point the red of the curve. red curve. This example This example demonstrates demonstrates that thatfor finding for finding the the best best solution,solution, it is, it is,in ingeneral, general, not not sufficient sufficient to to just just consider consider the interval ofof distancesdistances between be- the tween the “end points” of the parts. If one considers the interval of S-values between the end points of the red curve, then on that interval, the Part I curve (green) is always better. Therefore, an approach working with “critical distances” as in [20] has its limits.

(a) (b) Figure 11. (a) Best solution is in Part IIa where the curve is “bulging”; (b) zoom into area.

The examples given above show that a visualization using a parametric (𝑆, 𝑇) curve allows to easily create a variety of examples for finding counter-examples to conjectures one might have in mind. This has also been presented in [22] using a different parameterization. Using 𝑣 allows us to easily recognize in which part the best solution occurs. Moreover, the parameter 𝑣 has a direct meaning in the velocity–acceleration plane. One might be inclined to look for further patterns to see in advance which part provides the best solution, but even this small set of examples shows the variability. We will make no further distinctions but just compute the solution(s) provided by each part and take the

Robotics 2021, 10, x FOR PEER REVIEW 12 of 27

constructions of Parts IIa or IIb, a certain range of distances (here, roughly from 1.4 to 3) might not be covered. In the example, Part IIa provides the better solution and we have not encountered an example of case A where Part IIb is best, but we do not have proof that this is always so.

(a) (b) Figure 10. (a) Gap between Parts I and III, Parts IIa or IIb are needed; (b) zoom into area.

Example 4: Best solution is in Part IIa where the curve is “bulging”. Initial and final conditions: (𝑣,𝑎) = (−7.2, −5), (𝑣,𝑎) = (0.8,9). We use the same accelerations as in Example 1 but shift the velocities to the left by −17.2. In this example, zooming shows that the red Part IIa curve “bulges” to the right (Fig- Robotics 2021, 10, 25 ure 11b) (cf. similar examples in [22]). Therefore, there is a small interval of distances to 12 of 26 the right by about −5.34 where Part IIa provides the best solution, ending at the S-value of the utmost right point of the red curve. This example demonstrates that for finding the best solution, it is, in general, not sufficient to just consider the interval of distances between the“end “end points” points” of theof the parts. parts. If oneIf one considers considers the the interval interval of S-valuesof S-values between between the the end points end pointsof theof the red red curve, curve, then then on on that that interval, interval, the the Part Part I curve I curve (green) (green) is alwaysis always better. better. Therefore, Therefore,an an approach approach working working with with “critical “critical distances” distances” as inas [in20 [20]] has has its limits.its limits.

(a) (b)

Figure 11.Figure (a) Best 11. solution(a) Best is solution in Part isIIa in where Part IIa the where curve the is “bulging”; curve is “bulging”; (b) zoom ( binto) zoom area. into area.

The examplesThe examples given above given show above that show a visualization that a visualization using a parametric using a parametric (𝑆, 𝑇) curve(S, T ) curve allows toallows easily to create easily a create variety a variety of examples of examples for finding for ﬁnding counter-examples counter-examples to conjectures to conjectures one one mightmight have have in mind. in mind. This This has hasalso also been been presented presented in [22] in [22 using] using a different a different parameter- parameterization. ization. Using v𝑣m allowsallows us us to to easily easily recognize recognize in in which which part part the the best best solution solution occurs. occurs. Moreover, Moreover,the the parameter parametervm 𝑣has has a directa direct meaning meaning in in the the velocity–accelerationvelocity–acceleration plane. One One might might bebe inclined inclined to to look look for for further further pattern patternss to tosee see in inadvance advance which which part part provides provides the the best best solution,solution, but buteven even this this small small set setof ofexamples examples shows shows the the variability. variability. We We will will make make no no further further distinctionsdistinctions but but just compute the solution(s)solution(s) provided byby eacheach partpart andand taketakethe the best one (this approach is also used in [22]). We consider this as a sound compromise between complexity (comprehensibility) on the one hand and computational effort on the other hand. We ﬁnd the best solution within the solutions provided by Parts I–III by inserting

the given distance in Equations (7)–(10) and computing vm. As we will see in the next section, this requires ﬁnding the roots of polynomials. For each solution candidate, it is checked whether vm is admissible (real and within the allowed range). For the admissible ones, one computes the times and chooses the solution with lowest time. If vm < vmin or vm > vmax, one sets vm = vmin resp. vm = vmax and inserts a phase with constant (minimum or maximum) velocity.

Case B: aA ≤ 0 and aE ≥ 0, v f A > vbE In this case, E lies “left of” A in the sense that E lies on a square root curve (open to the right) left of the square root curve (open to the right) that the point A lies on. We can proceed in the same way as we did in case A and have, again, four parts, as shown in Figure 12. Robotics 2021, 10, x FOR PEER REVIEW 13 of 27

best one (this approach is also used in [22]). We consider this as a sound compromise between complexity (comprehensibility) on the one hand and computational effort on the other hand. We find the best solution within the solutions provided by Parts I–III by inserting the given distance in Equations (7)–(10) and computing 𝑣. As we will see in the next section, this requires finding the roots of polynomials. For each solution candidate, it is checked whether 𝑣 is admissible (real and within the allowed range). For the admissible ones, one computes the times and chooses the solution with lowest time. If 𝑣 < 𝑣 or 𝑣 >𝑣, one sets 𝑣 =𝑣 resp. 𝑣 =𝑣 and inserts a phase with constant (minimum or maximum) velocity. Case B: 𝑎 ≤0 and 𝑎 ≥0, 𝑣 >𝑣 In this case, E lies “left of” A in the sense that E lies on a square root curve (open to the right) left of the square root curve (open to the right) that the point A lies on. We can Robotics 2021, 10, 25 proceed in the same way as we did in case A and have, again, four parts, as shown in 13 of 26 Figure 12.

(a) (b)

Figure Figure12. Case 12. B.Case (a) Part B. ( aI;) ( Partb) Part I; ( bIII;) Part (c) Part III; ( cIIa;) Part (d) IIa;Part ( dIIb.) Part IIb.

Again,Again, with Part with I resp. Part IIII, resp. we III,can we realize can realizearbitrarily arbitrarily small resp. small large resp. distances large distances going going via intermediatevia intermediate velocities velocities 𝑣. Arguingvm. Arguing as in case as in A, case we A,obtain we obtain for Parts for I Parts and III I and the IIIsame the same formulaeformulae as in (7) as and in (7) (8) and resp. (8) (11) resp. and (11) (12), and but (12), the butranges the rangesfor 𝑣 for arev differentm are different for Part for I Part I ≤ ≥ 𝑣 ≤𝑣vm andvbE forand Part for III Part 𝑣 III≥𝑣vm . Thev f Aformulae. The formulae for Parts for IIa Parts and IIa IIb and are IIb different are different and and providedprovided below. below. S(v ) = S(v , v , v ) + S + S − 2·S v , v for v < v < v (Part IIa). (15) 𝑆(𝑣)=𝑆(𝑣m ,𝑣f A,𝑣bE)+𝑆m +𝑆f A −2∙𝑆(𝑣bE ,𝑣f) A form 𝑣 <𝑣f A <𝑣m (PartbA IIa). (15) S(vm) = S(v , v , vm) + S + S − 2·S v , vm for v < vm < v (Part IIa) for 𝑆(𝑣)=𝑆(𝑣,𝑣f A,𝑣bE)+𝑆 +𝑆f A −2∙𝑆(𝑣bE ,𝑣f) A for 𝑣 <𝑣f A <𝑣 (PartbA IIa) for 2 amax max vbE, v f A − < vm < v f A (Part IIb). (16) jmax T(vm) = T(v f A, vbE, vm) + TA + TE − 2·T v f A, vm for v f A < vm < vbA (Part IIa). (17) T(vm) = T(vbA, v f A, vm) + T(v f A, vbE) + TE − TA − 2·T v f A, vm for

2 amax max vbE, v f A − < vm < v f A (Part IIb). (18) jmax We omit examples for this case since they provide no new insights. The procedure for computing the best solution follows the same lines as the one described in case A.

Case C: aA ≥ 0 and aE ≥ 0, vbA ≤ vbE and v f A ≤ v f E In this case, E lies “right of” A in the sense that E lies in the area right of or on the square root curve through A open to the right, and right of or on the square root curve through A open to the left (Figure 13). Robotics 2021, 10, x FOR PEER REVIEW 14 of 27 Robotics 2021, 10, x FOR PEER REVIEW 14 of 27

max{𝑣 ,𝑣 − }<𝑣 <𝑣 (Part IIb). max{𝑣 ,𝑣 − }<𝑣 <𝑣 (Part IIb).

𝑇(𝑣)=𝑇(𝑣,𝑣,𝑣)+𝑇 +𝑇 −2∙𝑇(𝑣,𝑣) for 𝑣 <𝑣 <𝑣 (Part IIa). (17) 𝑇(𝑣)=𝑇(𝑣,𝑣,𝑣)+𝑇 +𝑇 −2∙𝑇(𝑣,𝑣) for 𝑣 <𝑣 <𝑣 (Part IIa). (17)

𝑇(𝑣)=𝑇(𝑣,𝑣,𝑣)+𝑇(𝑣,𝑣)+𝑇 −𝑇 −2∙𝑇(𝑣,𝑣) for 𝑇(𝑣)=𝑇(𝑣,𝑣,𝑣)+𝑇(𝑣,𝑣)+𝑇 −𝑇 −2∙𝑇(𝑣,𝑣) for max{𝑣 ,𝑣 − }<𝑣 <𝑣 (Part IIb). (18) max{𝑣 ,𝑣 − }<𝑣 <𝑣 (Part IIb). (18)

We omitWe examples omit examples for this for case this since case they since provide they provide no new no insights. new insights. The procedure The procedure for computingfor computing the best the solution best solution follows follows the same the lines same as lines the oneas the described one described in case inA. case A. Case C: 𝑎 ≥0 and 𝑎 ≥0, 𝑣 ≤𝑣 and 𝑣 ≤𝑣 Case C: 𝑎 ≥0 and 𝑎 ≥0, 𝑣 ≤𝑣 and 𝑣 ≤𝑣 In this Incase, this E case, lies “rightE lies of”“right A inof” the A sensein the thatsense E liesthat inE thelies areain the right area of right or on of theor on the Robotics 2021, 10, 25 square squareroot curve root throughcurve through A open A to open the right,to the andright, right and of right or on of theor onsquare the squareroot curve root 14curve of 26 throughthrough A open A to open the leftto the (Figure left (Figure 13). 13).

Figure 13. Positioning of A and E in cases C and D. Figure 13. Positioning of A and E in cases C and D.

We split this case up into two sub-cases (𝑣 ≤𝑣 and 𝑣 >𝑣) that need partially We split this case up into two sub-cases (v𝑣bE ≤≤𝑣vf A and v𝑣bE>>𝑣v f A) that need partially differentdifferent treatments. treatments. The sub-cases The sub-cases are shown are shownin Figure in Figure14. In the1414.. first In the sub-case ﬁrstfirst sub-case (left), we (left), we have a haveclosed a closedcurved curved quadrilateral quadrilateral ABED, ABED, whereas whereas in the secondin the second one (right), one (right), we have we a have a curvedcurved pentagon, pentagon, ABCED. ABCED. Note that Note in that the fi in rst the sub-case, fi ﬁrstrst sub-case, there might there also might occur also a occur curved a curved pentagon when 𝑎 is reached, but this does not require a further distinction. pentagon when a𝑎maxis is reached, reached, but but this this does does not no requiret require a furthera further distinction. distinction.

(a) (a) (b) (b) Figure 14.Figure (a) Sub-case 14. (a) Sub-case Ca; (b) sub-case Ca; ((b)) sub-case Cb. Cb.Cb.

Sub-Case Ca: 𝑣 ≤𝑣 Sub-CaseSub-Case Ca: v Ca:bE ≤ 𝑣v f A≤𝑣 As in caseAs inA, case we splitA, we up split the upconstruction the construction of solutions of solutions for all distancesfor all distances into different into different parts, as shownAs in in case Figure A, we 15. split With up Part the constructionI resp. III, we of can solutions realizefor arbitrarily all distances small into resp. different parts, asas shownshown in in Figure Figure 15 15.. With With Part Part I resp. I resp. III, weIII, canwe realizecan realize arbitrarily arbitrarily small small resp. largeresp. distances going via intermediate velocities vm ≤ vbE resp. vm ≥ v f A. If equality holds, then Part I and Part III provide the same point and, hence, the same distance ∆s. Therefore, Parts I and III already cover all distances but do not necessarily provide the shortest time. Some distances can be realized better by going directly within the curved quadrilateral ABED using the sequence jmax, −jmax, jmax in Part IIa or the sequence −jmax, jmax, −jmax in Part IIb (see Figure 15c,d). We will give an example for this situation below. Robotics 2021, 10, x FOR PEER REVIEW 15 of 27

large distances going via intermediate velocities 𝑣 ≤𝑣 resp. 𝑣 ≥𝑣 . If equality holds, then Part I and Part III provide the same point and, hence, the same distance Δs. Therefore, Parts I and III already cover all distances but do not necessarily provide the shortest time. Some distances can be realized better by going directly within the curved quadrilateral ABED using the sequence 𝑗 , −𝑗 , 𝑗 in Part IIa or the sequence Robotics 2021, 10, 25 15 of 26 −𝑗, 𝑗, −𝑗 in Part IIb (see Figure 15c,d). We will give an example for this situation below.

(a) (b)

Figure 15.Figure Case 15.Ca.Case (a) Part Ca. I; (a ()b Part) Part I; III; (b) ( Partc) Part III; IIa; (c) Part(d) Part IIa; (IIb.d) Part IIb.

ArguingArguing as in case as A, in casewe obtain A, we the obtain following the following formulae formulae for distances for distances and times: and times:

S(v ) = S(v , v , v ) + S − S for v ≤ v (Part I). (19) 𝑆(𝑣)=𝑆(𝑣m ,𝑣f,𝑣 A )+𝑆f E m −𝑆f A for 𝑣f E ≤𝑣m (PartbE I). (19)

S(vm) = S(vbA, vbE, vm) − SbA + SbE for vm ≥ v f A (Part III). (20) 𝑆(𝑣)=𝑆(𝑣,𝑣,𝑣)−𝑆 +𝑆 for 𝑣 ≥𝑣 (Part III). (20) S(vm) = S(vbA, vbE, vm) − SbA + SbE − 2·S(vbE, vm) for v f A < vm < v f E (Part IIa). (21) (21) S(vm)𝑆(𝑣= S()=𝑆(𝑣v f A, v f E, v,𝑣m) +,𝑣S)−𝑆f A −S f+𝑆 E −2·S−2∙𝑆(𝑣v f A, vm,𝑣for) vforbA <𝑣vm<𝑣<vbE<𝑣(Part (Part IIb) .IIa). (22)

T(vm) = T(v f A, v f E, vm) + TA − TE for vm ≤ vbE (Part I). (23) 𝑆(𝑣)=𝑆(𝑣,𝑣,𝑣)+𝑆 −𝑆 −2∙𝑆(𝑣,𝑣) for 𝑣 <𝑣 <𝑣 (Part IIb). (22) T(vm) = T(vbA, vbE, vm) − TA + TE for vm ≥ v f A (Part III). (24)

T(vm) = 𝑇(𝑣T(vbA)=𝑇(𝑣, vbE, vm),𝑣−T,𝑣A+)+𝑇TE − −𝑇2·T( vforbE ,𝑣vm)≤𝑣forv (Partf A < vI).m < v f E (Part(23) IIa) (25) T(vm) = T(v f A, v f E, vm) + TA − TE − 2·T v f A, vm for vbA < vm < vbE (Part IIb). (26) 𝑇(𝑣)=𝑇(𝑣,𝑣,𝑣)−𝑇 +𝑇 for 𝑣 ≥𝑣 (Part III). (24) We provide some examples that give an insight into the variability of situations. 𝑇(𝑣We)=𝑇(𝑣 use the,𝑣 same,𝑣)−𝑇 values +𝑇 for −2∙𝑇(𝑣 the restrictions,𝑣) for as in𝑣 the<𝑣 examples <𝑣 (Part given IIa) for Case(25) A, i.e., jmax = 30, amax = 10, vmax = 20. We have, again, one set of initial and ﬁnal conditions and shift this in the v-direction. The values used for producing Figure 16 are: (vA, aA) = (−3.3, 8), (vE, aE) = (−0.3, 11.8). Robotics 2021, 10, x FOR PEER REVIEW 16 of 27

𝑇(𝑣)=𝑇(𝑣,𝑣,𝑣)+𝑇 −𝑇 −2∙𝑇(𝑣,𝑣) for 𝑣 <𝑣 <𝑣 (Part IIb). (26)

We provide some examples that give an insight into the variability of situations. We use the same values for the restrictions as in the examples given for Case A, i.e., 𝑗 = 30, 𝑎 =10, 𝑣 =20. We have, again, one set of initial and final conditions and shift Robotics 2021, 10, 25 16 of 26 this in the v-direction. The values used for producing Figure 16 are: (𝑣,𝑎) = (−3.3,8), (𝑣,𝑎) = (−0.3,11.8).

(a) (b)

Figure 16.Figure (a) Example 16. (a) Example 1; (b) example 1; (b) example 2; (c): example 2; (c): example 3; (d) example 3; (d) example 3 with zoom. 3 with zoom.

ExampleExample 1: Shift 1:= 0, Shift i.e., =we 0, i.e.,use weexactly use exactlythe data the given data above. given above. ExampleExample 2: Shift 2:= 4, Shift i.e., =we 4, i.e.,use: we(𝑣 use:,𝑎)(=vA(,0.7,8aA) =), ((𝑣0.7,,𝑎 8),=(v (3.7,11.8)E, aE) = (. 3.7, 11.8). We recognizeWe recognize that the that parts the are parts no longer are no longerconnected, connected, but there but are there two are “connectivity two “connectivity components”,components”, Parts I/III Parts and I/III IIa/b. and It IIa/b.can also It canbe seen also bethat seen Parts that I and Parts III I provide and III provide solutions solutions for all distances,for all distances, but for the but distances for the distances for which for Parts which IIa and Parts IIb IIa are and applicable, IIb are applicable,they pro- they vide shorterprovide times. shorter In example times. In 1, examplePart IIb is 1, be Parttter IIb than isbetter Part IIa, than and Part in IIa,example and in 2, exampleit is the 2, it is other waythe around. other way around. Example 3: Shift = 2.2, i.e., we use: (v , a ) = (−1.1, 8), (v , a ) = (1.9, 11.8). Example 3: Shift = 2.2, i.e., we use: (𝑣,𝑎) =A(−1.1,8A ), (𝑣,𝑎) = (1.9,11.8)E E . In this example,In this example, we also we recognize also recognize the disc theonnectedness. disconnectedness. In addition, In addition, we see wethat see both that both the red thecurve red (Part curve IIa) (Part and IIa) the and blue the curve blue (Part curve IIb) (Part “bulge”. IIb) “bulge”. Part IIb Part provides IIb provides the shortest the shortest solutionsolution within withina range a of range distances of distances from about from 0.068 about to 0.068 about to 0.074, about whereas 0.074, whereas for Part for IIa, Part IIa, this holdsthis for holds a range for a from range about from 0.074 about to 0.074 about to 0.116. about This 0.116. again This shows again that shows an thatapproach an approach consideringconsidering different different cases only cases between only between the distances the distances reached reached at the boundary at the boundary points of points of the partsthe cannot parts cannotalways alwaysprovide provide the best the solution. best solution. Sub-CaseSub-Case Cb: Cb: 𝑣vbE>𝑣>v f A This case is depicted in Figure 14b. Regarding Part I resp. III, the same holds as was stated for sub-case Ca, i.e., we can realize arbitrarily small resp. large distances going via intermediate velocities, but here, vm ≤ v f A resp. vm ≥ vbE (see Figure 17a,b). Moreover, if equality holds, then Part I and Part III do not provide the same point and, hence, the same distance ∆s. Robotics 2021, 10, x FOR PEER REVIEW 17 of 27

This case is depicted in Figure 14b. Regarding Part I resp. III, the same holds as was stated for sub-case Ca, i.e., we can realize arbitrarily small resp. large distances going via Robotics 2021, 10, 25 intermediate velocities, but here, 𝑣 ≤𝑣 resp. 𝑣 ≥𝑣 (see Figure 17a,b). Moreover, 17 of 26 if equality holds, then Part I and Part III do not provide the same point and, hence, the same distance Δs.

(a) (b)

FigureFigure 17. Case 17. Case Cb. ( Cb.a) Part (a) I; Part (b) Part I; (b )III; Part (c) III;Part (c IIa;) Part (d) IIa; Part (d IIb.) Part IIb.

In orderIn order to guarantee to guarantee the continuity the continuity of covered of distances, covered distances,we again include we again two other include two partsother called parts Part called IIa and Part IIb, IIa using and the IIb, sequence using the 𝑗 sequence, −𝑗j, max𝑗, − injmax Part, jmax IIa inand Part the IIase- and the quencesequence −𝑗,− 𝑗jmax,, −𝑗jmax, − injmax Partin IIb Part (see IIb Figure (see Figure 17c,d). 17 Inc,d). Part In IIa, Part the IIa, motion the motioncurve curve startingstarting at Aat moves A moves up in up the in direction the direction of D beyond of D beyond the point the F, point then F,moves then movesdown to down a to a pointpoint on the on curve the curve CE and CE then and thenup to upE (see to E Figure (see Figure 17c). This 17c). is Thismuch is the much same the as same shown as shown in Figurein Figure 15c for15c thefor previous the previous sub-case sub-case Ca, bu Ca,t note but that note now, that the now, motion the motion curve must curve go must go beyondbeyond F (otherwise F (otherwise one onewould would need need a sequence a sequence with with three three changes changes in injerk jerk instead instead of of two). two).In In Part Part IIb, IIb, the the motion motion curve curve starting starting at at A A moves moves downdown inin thethe directiondirection of B, then moves movesup up to ato point a point on on the the curve curve DE DE (which (which lieslies above the the point point G) G) and and then then down down to E to E (see (seeFigure Figure 17 17d).d). WeWe checkedchecked that all all possible possible distances distances are are covered covered by by using using continuity: continuity: If in If in Part I we choose 𝑣 =𝑣, then we obtain the motion curve ABGE. This is also the Part I we choose vm = v f A, then we obtain the motion curve ABGE. This is also the motion motioncurve curve one one obtains obtains in Partin Part IIb IIb when when one one goes goes down down to to the the point point B. B. At At the the other other “end” of “end”Part of IIb,Part one IIb, obtains one obtains the motion the motion curve curve ADE ADE (i.e., (i.e., going going down down by 0). by This 0). This is also is also the motion the motion curve one obtains in Part IIa when one goes up from A to D. At the other “end” curve one obtains in Part IIa when one goes up from A to D. At the other “end” of Part IIa, of Part IIa, one obtains the motion curve AFCE. We also obtains this motion curve when one obtains the motion curve AFCE. We also obtains this motion curve when we choose we choose 𝑣 =𝑣 in Part III. Therefore, Parts IIa and b fulfill the purpose to continu- v = v in Part III. Therefore, Parts IIa and b fulﬁll the purpose to continuously connect ouslym connectbE Parts I and III. As we will see in the examples, in this sub-case, the special Parts I and III. As we will see in the examples, in this sub-case, the special situation might situation might occur that we need both Parts IIa and IIb to have a full coverage of all occur that we need both Parts IIa and IIb to have a full coverage of all distances, whereas in the other cases considered so far, only one of the parts was needed for this purpose. The formulae for distances and time are the same as in the previous sub-case (i.e., Equa-

tions (19)–(26)) but the ranges for vm are different. Therefore, we just provide these ranges:

vm ≤ v f A (Part I), vm ≥ vbE (Part III) , vbE < vm < v f E (Part IIa), vbA < vm < v f A (Part IIb). (27) We provide some examples that give an insight into the variability of situations and the speciﬁcs of this sub-case. We use the same values for the restrictions as in the examples Robotics 2021, 10, x FOR PEER REVIEW 18 of 27

distances, whereas in the other cases considered so far, only one of the parts was needed for this purpose. The formulae for distances and time are the same as in the previous sub-case (i.e., Equations (19–26)) but the ranges for 𝑣 are different. Therefore, we just provide these ranges:

𝑣 ≤𝑣 (Part I), 𝑣 ≥𝑣 (Part III) , 𝑣 <𝑣 <𝑣 (Part IIa), 𝑣 <𝑣 <𝑣 (Part IIb). (27) We provide some examples that give an insight into the variability of situations and the specifics of this sub-case. We use the same values for the restrictions as in the examples given for case A, i.e., 𝑗 =30, 𝑎 =10, 𝑣 =20. We have, again, one set of initial and final conditions and shift this in the v-direction: (𝑣,𝑎) = (0.5,4.9), (𝑣,𝑎) = Robotics 2021, 10, 25 (2.76,7.9). 18 of 26 Example 1: Shift = 0, i.e., we use exactly the data given above (Figure 18a). In this example, there is a gap between Parts I and III. Only Part IIa is needed to fill the gap. It also provides the shortest time where it is valid. One can see how one goes continuouslygiven for from case Part A, i.e., I viajmax IIb =and30 IIa, amax to III= as10 described, vmax = 20 above.. We have, again, one set of initial and ﬁnal conditions and shift this in the v-direction: (v , a ) = (0.5, 4.9), (v , a ) = (2.76, 7.9). Example 2: Shift = −1.3, i.e., we use: (𝑣,𝑎) = (−0.8,4.9A A), (𝑣,𝑎) = (1.46,7.9)E E (Fig- ure 18b). Example 1: Shift = 0, i.e., we use exactly the data given above (Figure 18a).

(a) (b)

FigureFigure 18. (a) 18. Example(a) Example 1; (b) example 1; (b) example 2; (c) example 2; (c) example 3; (d) example 3; (d) example 3 with zoom. 3 with zoom.

Here,In both this parts example, IIa and there IIb are is a needed gap between to cover Parts all possible I and III. distances. Only Part For IIa a iscertain needed to ﬁll distance,the gap.there Itis alsoonly providesone part that the shortestcovers that time distance where and it is is, valid. hence, One automatically can see how the one goes best one.continuously from Part I via IIb and IIa to III as described above. Example 3: Shift =−1.05, i.e., we use: (𝑣 ,𝑎 ) = (−0.55,4.9), (𝑣 ,𝑎 ) = (1.71,7.9) (Fig- Example 2: Shift = −1.3, i.e., we use: (vA, aA) = (− 0.8, 4.9), (vE, aE) = (1.46, 7.9) ure 18c).(Figure 18b). Here, both parts IIa and IIb are needed to cover all possible distances. For a certain distance, there is only one part that covers that distance and is, hence, automatically the best one. Example 3: Shift =−1.05, i.e., we use: (vA, aA) = (−0.55, 4.9), (vE, aE) = (1.71, 7.9) (Figure 18c). This example looks, at ﬁrst, similar to example 2, but when one zooms into the interesting area, one can recognize that for some distances covered by Part IIb (blue), there is also a coverage by Part IIa with a shorter time. Moreover, for some distances (e.g., 0.132), there are even three solutions within Part IIb, one of them having the shortest time. This again shows the variability of situations.

Case D: aA ≥ 0 and aE ≥ 0, vbA > vbE or v f A > v f E This case is illustrated in Figure 19. Case D is the easiest one since it can be completely reduced to the GG situation by a combination of embedding and extension and we just need Parts I and III. Figure 19 shows situations when both of the two velocity conditions are fulﬁlled, but it is easy to check that nothing changes when only one of them holds.

Consider the GG task for going from (v f A, 0) to (v f E, 0) via a vmI ≤ vbE. If one extends Robotics 2021, 10, x FOR PEER REVIEW 19 of 27

This example looks, at first, similar to example 2, but when one zooms into the interesting area, one can recognize that for some distances covered by Part IIb (blue), there is also a coverage by Part IIa with a shorter time. Moreover, for some distances (e.g., 0.132), there are even three solutions within Part IIb, one of them having the shortest time. This again shows the variability of situations. Case D: 𝑎 ≥0 and 𝑎 ≥0, 𝑣 >𝑣 or 𝑣 >𝑣 This case is illustrated in Figure 19. Case D is the easiest one since it can be completely reduced to the GG situation by a combination of embedding and extension and we just need Parts I and III. Figure 19 shows situations when both of the two velocity conditions Robotics 2021, 10, 25 19 of 26 are fulfilled, but it is easy to check that nothing changes when only one of them holds.

Consider the GG task for going from (𝑣,0) to (𝑣,0) via a 𝑣 ≤𝑣. If one extends this by going from A to (𝑣,0) and cuts off the part going from E to (𝑣,0), one obtains

athis motion by going curve from from A toA to(v fE A ,with 0) and a certain cuts off distance. the part Making going from 𝑣 E smaller to (v f E ,and 0), one inserting obtains a

phasea motion with curve velocity from 𝑣 A to enables E with aus certain to realize distance. arbitrarily Making smallvm Ivaluessmaller for and Δs. inserting Similarly, a thephase red with motion velocity curvev mincanenables be constructed us to realize from arbitrarily the GG task small going values from for (𝑣∆s.,0) Similarly, to (𝑣 the,0) red motion curve can be constructed from the GG task going from (v , 0) to (v , 0) via a via a 𝑣 ≥𝑣. Again, making 𝑣 larger and inserting a phase withbA velocitybE 𝑣 en- vm ≥ v . Again, making vm larger and inserting a phase with velocity vmax enables us ablesII us fto A realize arbitrarily largeII values for Δs. Since for 𝑣 =𝑣 and 𝑣 =𝑣 we = = obtainto realize the arbitrarily same motion large curve, values we for continuously∆s. Since for covervmI allv bEdistances.and vmII v f A we obtain the same motion curve, we continuously cover all distances.

Figure 19. Case D.

We obtain the the same same formulae formulae for for distances distances an andd times times as asfor for Parts Parts I and I and III in III Case in Case Ca, i.e.,Ca, i.e.,Equations Equations (19), (19), (20), (20), (23), (23), (24) (24) also also hold hold inin this this case. case. Since Since there there are are no no remarkable special situations occurring in this case, we give no examples.

5. Implementation Implementation and Validation with MATLAB®® In both both the the GG GG and and the the BB BB situations, situations, beside beside splitting splitting into several cases, which is easy to implement, thethe mainmain tasktask consists consists of of solving solving an an equation. equation. In In GG GG and and in in Parts Parts I and I and III III of ofthe the BB BB cases, cases, the the equation equation is ofis theof the type: type:

w = S(vA, vE, vm) = S(vA, vm) + S(vm, vE) (28) 𝑤=𝑆(𝑣,𝑣,𝑣) =𝑆(𝑣,𝑣) +𝑆(𝑣,𝑣) (28) where v is unknown and the distance w and the velocities v , v are given. The equations where 𝑣m is unknown and the distance 𝑤 and the velocitiesA 𝑣E,𝑣 are given. The equa- for Parts IIa and IIb in the BB situation (9), (10), (15), (16), (21) and (22) can be rewritten in tions for Parts IIa and IIb in the BB situation (9), (10), (15), (16), (21) and (22) can be rewrit- the form: ten in the form: w = S(vA, vm) − S(vm, vE). (29)

We abbreviate the difference𝑤=𝑆 on the(𝑣 right,𝑣) hand−𝑆(𝑣 side,𝑣 as). Sneg(vA, vE, vm). (29) We split up Equation (28) into sub-cases depending on whether the maximum acceler- We abbreviate the difference on the right hand side as 𝑆(𝑣,𝑣,𝑣). ation is reached going from vA to vm resp. from vm to vE (cf. Equations (1) and (2)): s s |vm − vA| |vE − vm| w = (vA + vm)· + (vm + vE)· (30) jmax jmax

An analogous set of equations can be produced for (29) using minus instead of plus between the two parts. Moreover, a closer investigation of the interval where vm lies in Parts IIa and IIb shows that sub-case (33) does not occur, so when considering Equation (29), we can restrict ourselves to (30)–(32). Available numerical equation solvers for (28) and (29) normally find, at most, one solution and then stop. Therefore, we transform the equations symbolically into problems where all zeros of polynomials are to be found. For doing this, we first remove the absolute values by making a further distinction between the cases (α) vm ≤ vA, vE and (β) vm ≥ vA, vE (the overlap at equality does not matter). For example, in case (α), we write vA − vm for |vm − vA|. Using a Computer Algebra System (here, Maple®), one can then reduce the task of solving the equations (30)–(32) to find the roots of polynomials of degree 4 and one obtains even a symbolic solution for (33). We first consider the equation w = S(vA, vE, vm). The polynomials for Equations (30)–(32) are given in Table A1 (AppendixA) in form of lists of coefficients (starting from the largest power) and the solutions for Equation (33) are given directly. If we consider instead the equation w = Sneg(vA, vE, vm), we obtain only polynomials for the modified Equations (30)–(32), as shown in Table A2 (Equation (33) does not occur as stated above). Once all zeros z of the polynomials for the (modified) Equations (30)–(32) have been found using a polynomial root solver (here: MATLAB®), the solutions for Equations (30)–(32) for both equations w = S(vA, vE, vm) and w = Sneg(vA, vE, vm) can be found according to Table A3 (provided by the Computer Algebra System). All solution candidates are then further filtered: • Only the real solutions (not the complex ones) are taken. • It is tested whether the solutions fulfill the case conditions in (α) resp. (β). • It is tested whether the solutions fulfill the requirements of Equations (30)–(33) regarding reaching the maximum/minimum acceleration. • Not all zeros of the polynomial might solve the original equation (only the implication that all solutions of the equation can be found using zeros of the polynomials and the computations given in Table A3 holds in general; there is no equivalence relationship). Therefore, all zeros are tested (by insertion) and only those ones fulfilling the equation are kept. Only those solutions that pass the tests are then used for further processing. This algorithm is described semi-formally in pseudo-code in Algorithm 1.

Algorithm 1. Solving w = S(vA, vE, vm) (resp. w = Sneg(vA, vE, vm)).

Input: w, vA vE, amax, jmax, case (α or β)

Output: Candidates for vm 1. If case = α Then 2. Compute the zeros of polynomial (30) in Table A1 (resp. A2) listed under (α) 3. From the zeros compute candidates for vm according to Table A3 listed under (α) 4. For each candidate vm do 5. If vm is not real Then omit vm End If 6. If not (vm ≤ vA, vE ) Then omit vm End If % Not case α 2 2 amax amax 7. If not |vm − vA| < j and |vm − vE| < j ) Then omit vm End If % necessary condition for (30) max max 8. If not (w = S(vA, vE, vm) ) resp. not w = Sneg(vA, vE, vm)) Then omit vm End If % equation not solved 9. End For 10. Do the same (lines 2–9) for polynomials (31) and (32) adapting the inequalities in line 7 11. Compute candidates for vm directly from (33) in Table A1 (not applicable for w = Sneg(vA, vE, vm)) 12. If applicable Then Do the same as in lines 4–9, adapting the inequalities in line 7 End if 13. else if case = β Then 14. Do the same as in case α now using the polynomials/expressions listed under case β in Table A1 (resp. A2) 15. End If 16. Return the remaining candidates for vm % might be the empty set Robotics 2021, 10, 25 21 of 26

If we have the BB task, we ﬁrst determine which of the cases, A to D, is applicable (or rewrite the problem by changing the direction of distance). In the respective case, we determine the solutions for an equation of type w = S(vA, vE, vm) for Parts I and III and the solutions for an equation of type w = Sneg(vA, vE, vm) for Parts IIa and IIb as described above. Note that even for Parts I and III where we can relate the BB task to a GG task by embedding or extension, we cannot simply use an algorithm for the GG task since we have to take into account the restrictions on the interval where vm must lie. We have to determine the zeros of, at most, 12 polynomials of degree 4 (four parts with three polynomials each at most). Then, we additionally check whether the solutions for vm are within the given range for the respective part (the ranges are given in the equations above) and drop those who are not. If vm < vmin or vm > vmax, we insert a phase with constant velocity. We then compute the lengths for the different time intervals in the seven-segment proﬁle and sum these up. The candidate with the lowest sum is then the best one within the set of all candidates considered. The algorithm is described semi-formally in Algorithm 2.

Algorithm 2. Computing the time intervals and signs for the 7-segment proﬁle.

Input: ∆s, vA, aA, vE, aE, jmax, amax, vmax

Output: Time vector [t1, t2, t3, t4, t5, t6, t7], sign vector for jmax in the intervals where j = ±jmax 1: If not case ∈ {A, B, Ca, Cb, D} Then 2: Set ∆s − ∆s, vA = −vA, aA = −aA, vE = −vE, aE = −aE 3: End If 4: If case = A Then 5: Call Algorithm 1 according to Equation (7) 6: For each vm returned 7: If condition on vm in Equation (7) is not fulﬁlled Then 8: omit vm 9: Else 10: If vm < −vmax Then Set vm = −vmax End If % Take into account restriction on v 11: Compute time vector and sign vector 12: End if 13: End For 14: Call Algorithm 1 according to Equation (8) and perform lines 6–13 15: Call Algorithm 1 with w = Sneg(vA, vE, vm) according to Equation (9) and perform lines 6–13 16: Call Algorithm 1 with w = Sneg(vA, vE, vm) according to Equation (10) and perform lines 6–13 17: Else If case = B 18: Execute lines 5–16 using the equations of case B 19: Else If case = Ca 20: Execute lines 5–16 using the equations of case Ca 21: Else If case = Cb 22: Execute lines 5–16 using the equations of case Cb 23: Else If case = D 24: Execute lines 5–16 using the equations of case D 25: End If 26: Return Time vector [t1, t2, t3, t4, t5, t6, t7] and sign vector for jmax where t7 = min

If we have a GG task, we can determine in advance which one of the Equations (30)– (33) is applicable and determine whether we have case (α) or (β) (we omit the details). Therefore, we would have to ﬁnd the roots of at most one polynomial. Regarding validation, we have to state ﬁrst what we intend to validate. Since the presented algorithm logically covers all possible situations by the continuity argument we explained in the previous section, theoretically, there is no necessity for validation otherwise than checking for a mistake in the argumentation that might come up during test runs. What certainly needs validation is the implementation of the algorithm, particularly regarding numerical issues such as cancellation effects (accuracy). Moreover, for checking the algorithm’s suitability for online trajectory planning, it is also important to investigate Robotics 2021, 10, 25 22 of 26

efficiency, i.e., the time necessary for finding the solution. Our accuracy and efficiency check was performed by having a huge number of test runs with randomly generated data covering all the cases and parts described in Section4. We used the following scheme:

• We first prescribe upper boundary values for jmax, amax, vmax, called jmaxb, amaxb, vmaxb, 3 and the largest distance ∆smax is considered. We choose 100 for each (units: m/s , m/s2, m/s, m) for our test runs to allow for a larger range but this is a bit arbitrary. • Within the interval [0.0001·jmaxb, jmaxb], we generate a random value for jmax and proceed analogously for amax, vmax. Moreover, for ∆s ∈ [−∆smax, ∆smax], we do so randomly. • Within the area in the velocity–acceleration plane that is bounded by the horizontal lines at ±amax and the left and right square root curves shown in Figure3, we choose values for (vA, aA) and (vE, aE) randomly. • Once the configuration [jmax, amax, vmax, ∆s, vA, aA, vE, aE] has been generated randomly this way, a test run for computing the best function is performed and it is checked whether the solution really gives the prescribed final velocity and acceleration and the distance ∆s within a certain margin of error. If the error is larger than 10–7, the configuration producing it is recorded for further investigation. Given this scheme, we made several runs with 107 random configurations which took about 40 min each (depending on whether the profiler in MATLAB was turned on or off). During the first test runs, we encountered, a few times, the situation that no solution was found, which is in contradiction with the logical argumentation in Section4. The reasons were, as already assumed, of numerical origin and of two different kinds. In the first type of situation, the solutions were at border points between different parts. Consider, for example, case B: If there is a solution very close to the border for vm between Part IIa and Part IIb (i.e., vm = v f A), then it might not be accepted in Part IIa because it is required there that vm > v f A. By simply allowing equality here (which in theory means one has an overlap between Parts IIa and III), one can avoid this problem. One could also allow a small overlapping ε-interval here. The second type of situation where no solution was found had its origin in the error margin for the distance in the computation of candidates (roots of the polynomials). By enlarging the error margin, this type of problem could be avoided in the subsequent runs. Since, in the final solution, the distance is again checked, this does not lead to the acceptance of solutions with errors above a specified value. Actually, since distances are computed using Equations (30)–(33), stated above in this section, we might have typical cancellation effects when vA and vm or vE and vm have nearly the same absolute value. We performed a test run with 108 configurations, which provided a solution for all configurations, so the numerical remedies reported above seem to work. Table2 gives information on errors. The maximum error in ∆s was 6.57 × 10−6 and there were only 15 configurations with errors in ∆s of more than 1 × 10−7. Therefore, we conclude that the implementation provides acceptable results for realistic situations regarding the values for jmax, amax and vmax.

Table 2. Largest errors in a test run with 108 conﬁgurations.

Run with Lower Bounds Max. error ∆s 6.57 × 10−6 (m) Max. error vE 4.67 × 10−12 (m/s) Max. error aE 7.11 × 10−14 (m/s2) Number of Errors in ∆s > = 10−7 15

Table3 provides information on the case and part coverage. It shows that all cases and parts were sufﬁciently covered except for Part IIb in cases A and B (in case D, Parts IIa and IIb do not occur; see Section4). In earlier test runs with less conﬁgurations, it was nearly always the case that there was no example for Part IIb in cases A and B, and we Robotics 2021, 10, 25 23 of 26

conjectured already in Section4 that Part IIb might be unnecessary. A closer look at the conﬁgurations that led to the 12 entries for Part IIb in cases A and B revealed that, again, this is a numerical effect. As is shown in Figure 10, if the solution point is very close to the “meeting point” of the curves for Parts IIa, IIb and III, it might happen that because of round-off errors, IIb gives a lower time value than IIa, although zooming in shows that Part IIb is above Part IIa. Practically, this does not really matter since the solutions provide nearly the same time. Therefore, we still stick to our conjecture that in cases A and B, Part IIb never contains the best solution.

Table 3. Coverage of cases and parts.

Case A Case B Case Ca Case Cb Case D Part I 10,561,715 10,563,034 1,085,981 9,209,731 13,996,250 Part IIa 727,205 728316 17,555 691,269 0 Part IIb 3 9 17,756 692,692 0 Part III 13,710,379 13,713,522 1,087,589 9,206,664 13,990,330 Sum 24,999,302 25,004,881 2,208,881 19,800,356 27,986,580

Regarding efficiency, we wanted to investigate whether the computation of the best function can be performed within a controller cycle which might be 1 ms (cf. [12]) or less, and for very fast controllers, even 100 µs. In our computations, we used a desktop PC with a Xeon E5-1630v4 processor with 3.7 GHz. It is not straightforward to generate valid efficiency data when running MATLAB functions. As stated in Mathwork’s own performance white paper [23], there is much “noise” to be taken into account, which might lead to considerable variation of times. This noise includes scheduling of processes and threads by the operating system and use of different memory caches. Moreover, the just-in-time compiler of MATLAB compiles a function once and uses then the compiled version. The variation can be made visible by running the best function with a fixed configuration several times, such that the computational work is identical. This gave values between a few milliseconds and a few hundred microseconds. We took this into account by repeating the computation for the same configuration several times and then taking the lowest value. We measured the times for the best function finder for 100,000 configurations, each repeated 1000 times, which provides a good coverage of all cases and parts. The maximum time was 303 µs. The result already shows that a run of the best function finder can be performed with a cycle time of 1 ms without any problem. However, there is even considerable potential for improvement. One of the main tasks in the best function finders is the computation of roots of the 12 polynomials of degree 4. In a test run with 1,000,000 configurations with 20 repetitions each, we measured an average time of 156 µs for computing the roots of the 12 polynomials. MATLAB’s roots function is a general root-finding function for polynomials of an arbitrary degree. For polynomials of degree 4 (also called “quartics”), there are many more efficient special solvers available. For solving a quartic accurately and efficiently, Orellana and De Michele [24] needed less than 0.5 µs on a dual-core i7 processor with 3.3 GHz. This means that by using such a specific solver, we can reduce the effort for root finding to about 6 µs (12 polynomials). That would cut the “worst case” time for computing the best function of about 303 µs by half. Moreover, our current MATLAB code contains overhead for providing additional information, and the code is split up into several small functions for making it better readable. Modern compilers contain optimizers that try to reduce the overhead when actually producing the executable code, e.g., by so-called “inlining”, where calls to “small functions” are substituted by inserting the code of the functions into the calling function. Orellana and De Michele [24] showed that by using a certain optimizer option, they could reduce the time needed by about half. This indicates that the time needed for computing the best function could even be brought well below 100 µs. This is comparable with the time reported in [12] for the worst case (540 µs for six axes) and in [19] as average (74 µs for one axis). Sidobre and Desormeaux [22] even achieved 2.5 µs, but the three-variable Robotics 2021, 10, 25 24 of 26

Newton method they applied after ﬁrst obtaining an approximation did not always provide the optimal solution. The most important criterion for online trajectory generation is not the absolute time as long as the computation can be performed within one controller cycle.

6. Conclusions In this contribution, we investigate how a fast motion function can be found when restrictions for jerk, acceleration and velocity, initial and final values for velocity and acceleration and the distance are given. Our approach applies a seven-segment motion profile with piecewise constant jerk, which was already considered by other authors. We developed an algorithm for computing such a profile which considers only four major cases and, hence, has a manageable complexity. We investigate these cases systematically in the velocity–acceleration plane, which provides an easily understandable visual explanation. The investigation showed that there are often several seven-segment profiles fulfilling the condition differing considerably in execution time. We take the best one, whereas the algorithm by Ezair et al. [19] leads to just one of them. Since (like Sidobre/Desormeaux [22]) we do not have mathematical proof that there are no other seven-segment profiles with shorter times, we only claim to have a “fast” solution, not necessarily an “optimal” one. Moreover, a continuity argument proved that we cover all possible distances, whereas in the work by Sidobre and Desormeaux [22] treating the same problem, only a brief explanation but no proof is given. We implemented our algorithm in MATLAB, where we reduced the computation essentially to the finding of roots of up to 12 polynomials of degree 4, which can easily be re-implemented. A large test run with 108 configurations showed that a solution was always found and all cases as well as all parts were covered satisfactorily. This also supports our claim of full coverage of all possible situations. Moreover, our timing experiments showed that our algorithm is suitable for online trajectory generation. Although finding the zeros of up to 12 polynomials makes our algorithm a bit slower than algorithms for special situations (such as [12]), the additional cost is small since very fast solvers for quartics are available. There are also clear limitations to the approach using a seven-segment profile. Since the jerk function is piecewise constant, the discontinuity might lead to unwanted vibrations. As stated in the literature review, for special situations such as RR (“rest-to-rest”), smoother functions have been designed. This deficit of the seven-segment approach might be reduced by working with a 15-segment profile with continuous jerk. A feasible way for finding such a profile in the general BB task might be to generalize the approach developed in this contribution. A further limitation of the seven-segment approach is that it does not handle variable constraints but only constant ones, and the constraints are assumed to be symmetric, although it would be possible to modify our approach in order to include asymmetric constraints. We have not tested the applicability of seven-segment profiles experimentally since this has already been performed by other authors (e.g., [12,22]), on whom we rely. In real applications such as robotics, the situation we investigated in this article is only one problem among many. Further considerations are necessary when there are several drives that need coordination or when restrictions change and one has to turn the initial state into an admissible one first. One can find strategies for this in [12,19,20,25].

Funding: This research received no external funding. Institutional Review Board Statement: Not applicable. Informed Consent Statement: Not applicable. Data Availability Statement: Not applicable. Conﬂicts of Interest: The author declares no conﬂict of interest. Robotics 2021, 10, 25 25 of 26

Appendix A

Table A1. Polynomials and solutions for w = S(vA, vE, vm).

List of Coefﬁcients for (30)–(32) Resp. Solutions for (33) Case (α) 2 2 2 2 2 2 3 2 2 3 (30) jmaxvA − jmaxvE, 2jmaxw, jmaxvA − 2jmaxvAvE + jmaxvE, −4jmaxw·vE, jmaxw − vA − vAvE + vAvE + vE 3 2 2 2 2 2 2 2 (31) jmax, 2amax jmax, amax jmax − 2jmaxvA, −4amax jmaxvA, −amaxvA − amaxvE + 2amax jmaxw + jmaxvA − jmaxvE 3 2 2 2 2 2 2 2 (32) jmax, 2amax jmax, amax jmax − 2jmaxvE, −4amax jmaxvE, −amaxvA − amaxvE + 2amax jmaxw − jmaxvA + jmaxvE √ a2 ± a4 +2a2 j v +2a2 j v −4a j2 w+2j2 v2 +2j2 v2 (33) max max max max A max max E max max max A max E 2jmax Case (β) 2 2 2 2 2 2 3 2 2 3 (30) jmaxvA − jmaxvE, 2jmaxw, −jmaxvA + 2jmaxvAvE − jmaxvE, 4jmaxw·vE, −jmaxw − vA − vAvE + vAvE + vE 3 2 2 2 2 2 2 2 (31) jmax, 2amax jmax, amax jmax + 2jmaxvA, 4amax jmaxvA, amaxvA + amaxvE − 2amax jmaxw + jmaxvA − jmaxvE 3 2 2 2 2 2 2 2 (32) jmax, 2amax jmax, amax jmax + 2jmaxvE, 4amax jmaxvE, amaxvA + amaxvE − 2amax jmaxw − jmaxvA + jmaxvE √ −a2 ± a4 −2a2 j v −2a2 j v +4a j2 w+2j2 v2 +2j2 v2 (33) max max max max A max max E max max max A max E 2jmax

Table A2. Polynomials and solutions for w = Sneg(vA, vE, vm).

List of Coefﬁcients for Modiﬁed (30)–(32) Case (α) 2 2 2 2 2 2 3 2 2 3 (30) jmaxvA − jmaxvE, −2jmaxw, jmaxvA − 2jmaxvAvE + jmaxvE, 4jmaxw·vE, jmaxw − vA − vAvE + vAvE + vE 3 2 2 2 2 2 2 2 (31) jmax, −2amax jmax, amax jmax − 2jmaxvA, 4amax jmaxvA, −amaxvA − amaxvE − 2amax jmaxw + jmaxvA − jmaxvE 3 2 2 2 2 2 2 2 (32) jmax, −2amax jmax, amax jmax − 2jmaxvE, 4amax jmaxvE, −amaxvA − amaxvE + 2amax jmaxw − jmaxvA + jmaxvE

Case (β) 2 2 2 2 2 2 3 2 2 3 (30) jmaxvA − jmaxvE, −2jmaxw, −jmaxvA + 2jmaxvAvE − jmaxvE, −4jmaxw·vE, −jmaxw − vA − vAvE + vAvE + vE 3 2 2 2 2 2 2 2 (31) jmax, −2amax jmax, amax jmax + 2jmaxvA, −4amax jmaxvA, amaxvA + amaxvE + 2amax jmaxw + jmaxvA − jmaxvE 3 2 2 2 2 2 2 2 (32) jmax, −2amax jmax, amax jmax + 2jmaxvE, −4amax jmaxvE, amaxvA + amaxvE − 2amax jmaxw − jmaxvA + jmaxvE

Table A3. Solution candidates for vm.

Case (α) 2 (30) −z jmax + vE 2 (31) −z jmax + vA 2 (32) −z jmax + vE Case (β) 2 (30) z jmax + vE 2 (31) z jmax + vA 2 (32) z jmax + vE

References 1. Biagiotti, L.; Melchiorri, C. Trajectory Planning for Automatic Machines and Robots; Springer: Berlin/Heidelberg, Germany, 2008. 2. Carbone, G.; Gomez-Bravo, F. (Eds.) Motion and Operation Planning of Robotic Systems; Mechanisms and Machine Science 29; Springer: Berlin/Heidelberg, Germany, 2015. 3. Gasparetto, A.; Boscariol, P.; Lanzutti, A.; Vidoni, R. Path Planning and Trajectory Planning Algorithms: A General Overview. In Motion and Operation Planning of Robotic Systems; Carbone, G., Gomez-Bravo, F., Eds.; Springer: Berlin/Heidelberg, Germany, 2015; pp. 3–27. Robotics 2021, 10, 25 26 of 26

4. VDI (Ed.) Guideline 2143: Motion Rules for Cam Mechanisms; Theoretical Fundamentals (German); Beuth Verlag: Berlin, Germany, 1980. 5. Kröger, T.; Wahl, F.M. Online Trajectory Generation: Basic Concepts for Instantaneous Reactions to Unforseen Events. IEEE Trans. Robot. 2010, 26, 94–111. 6. Macfarlane, S.; Croft, E.A. Jerk-bounded manipulator trajectory planning: Design for real-time applications. IEEE Trans. Robot. Autom. 2003, 19, 42–52. [CrossRef] 7. Liu, H.; Liu, X.; Wu, W. Time-optimal and jerk-continuous trajectory planning for robot manipulators with kinematic constraints. Robot. Comput. Integr. Manuf. 2013, 29, 309–317. [CrossRef] 8. Lin, J.; Somani, N.; Hu, B.; Rickert, M.; Knoll, A. An Efficient and Time-Optimal Trajectory Generation Approach for Waypoints under Kinematic Constraints and Error Bounds. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Madrid, Spain, 1–5 October 2018. 9. Shen, P.; Zhang, X.; Fang, Y. Complete and time-optimal path-constrained trajectory planning with torque and velocity constraints: Theory and applications. IEEE/ASME Trans. Mechatron. 2018, 23, 735–746. [CrossRef] 10. Penin, B.; Giordana, P.R.; Chaumette, F. Minimum-time trajectory planning under intermittent measurements. IEEE Robot. Autom. Lett. 2019, 4, 153–160. [CrossRef] 11. Shiller, Z. Off-line and On-Line Trajectory Planning. In Motion and Operation Planning of Robotic Systems; Carbone, G., Gomez-Bravo, F., Eds.; Springer: Berlin/Heidelberg, Germany, 2015; pp. 29–62. 12. Kröger, T. On-Line Trajectory Generation in Robotic Systems; Springer: Berlin/Heidelberg, Germany, 2010. 13. SEW Eurodrive. Manual Program. Module MultiMotion; SEW: Bruchsal, Germany, 2010. 14. Wang, H.; Wang, H.; Huang, J.; Zhao, B.; Quan, L. Smooth point-to-point trajectory planning for industrial robots with kinematical constraints based on high-order polynomial curve. Mech. Mach. Theory 2019, 139, 284–293. [CrossRef] 15. Fang, Y.; Hu, J.; Liu, W.; Shao, Q.; Qi, J.; Peng, Y. Smooth and time-optimal S-curve trajectory planning for automated robots and machines. Mech. Mach. Theory 2019, 137, 127–153. [CrossRef] 16. Trigatti, G.; Boscariol, P.; Scalera, L.; Pillan, D.; Gasparetto, A. A look-ahead trajectory planning algorithm for spray painting robots with non-spherical wrists. In Proceedings of the 4th IFToMM Symposium on Mechanism Design for Robotics, Udine, Italy, 11–13 September 2018; pp. 235–242. 17. Liu, S. An On-line Reference-Trajectory Generator for Smooth Motion of Impulse-Controlled Industrial Manipulators. In Proceedings of the 7th International Workshop on Advanced Motion Control, Maribor, Slovenia, 3–5 July 2002; pp. 365–370. 18. Haschke, R.; Weitnauer, E.; Ritter, H. On-line planning of time-optimal, jerk-limited trajectories. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, Nice, France, 27–31 October 2008; pp. 3248–3253. 19. Ezair, B.; Tassa, T.; Shiller, Z. Planning high order trajectories with general initial and final conditions and asymmetric bounds. Int. J. Robot. Res. 2014, 33, 898–916. [CrossRef] 20. Broquère, X. Planification de Trajectoire Pour la Manipulation D’objets et L’interaction Homme-Robot. Ph.D. Thesis, Université Paul Sabatier Toulouse III, Toulouse, France, 2011. 21. Broquère, X.; Sidobre, D.; Herrera-Aguilar, I. Soft motion trajectory planner for service manipulator robot. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, Nice, France, 22–26 September 2008; pp. 2808–2813. 22. Sidobre, D.; Desormeaux, K. Smooth cubic polynomial trajectories for human-robot interactions. J. Intell. Robot. Syst. 2019, 95, 851–869. [CrossRef] 23. McKeeman, B. MATLAB Performance Measurement; Mathworks: Natick, MA, USA, 2020. 24. Orellana, A.G.; De Michele, C. Algorithm 1010: Boosting Efficiency in Solving Quartic Equations with No Compromise in Accuracy. ACM Trans. Math. Softw. 2020, 46, 20:1–20:28. [CrossRef] 25. Desormeaux, K.; Sidobre, D. Online Trajectory Generation: Reactive Control with Return Inside an Admissible Kinematic Domain. In Proceedings of the International Conference on Intelligent Robots and Systems (IROS 2019), Macau, China, 4–8 November 2019.