Arc planning only considers tangential acceleration

[Wallacoloo]

Currently, an arc is implemented such that its tangential velocity at any time while under constant cartesian acceleration is v=a*t.

But because of circular motion, there is an acceleration normal to the direction of motion (An = v^2/r for uniform r). Thus, the total cartesian acceleration experienced at any time is sqrt(An^2 + At^2), where At is the tangential acceleration. This value should be kept constant if using a constant acceleration profile.

Furthermore, since there is a limit upon the maximum cartesian acceleration, and An=v^2/r for constant v, then there is also a limit to the ratio of v^2/r - we need to ensure that smaller arcs are taken at a slower velocity.

This page has a decent explanation of non-uniform circular motion.

[Wallacoloo]

For circular motion with angular acceleration, let P = (rcos(p), rcos(p)), where p is some function of t. Then v = P' = (-rsin(p)p', rcos(p)p')
a = v' = (-rsin(p)p'' + -rcos(p)(p')^2, rcos(p)p'' + -rsin(p)(p')^2)
|a|^2 = r^2sin^2(p)(p'')^2 + 2r^2sin(p)cos(p)(p'')(p')^2 + r^2cos^2(p)(p')^4 + r^2cos^(p)(p'')^2 - 2r^2sin(p)cos(p)(p'')(p')^2 + r^2sin^2(p)(p'')^2
|a|^2 = r^2*(p'')^2 + r^2*(p')^4

a is the quantity we wish to fix. The above can be rewritten as (p'')^2 + (p')^4 = |a/r|^2. The solution to this is supposedly p'(t) = +/-sqrt(a/r)*sn(sqrt(a/r)*t+c1, -1)

When integrated, the (positive) phase function becomes:
p(t) = -((I Log[-I JacobiCN[c + Sqrt[a/r] t, -1] + JacobiDN[c + Sqrt[a/r] t, -1]])/Sqrt[(a/r)])

This is very nasty, and note that the above only covers movement in the first quadrant. We can consider the other solution, -p(t), but that only covers the fourth quadrant. Below is a plot of the phase function and the resulting movement for a=r=1 and c1=0. The main issue appears to be that the phase function is not monotonic.

As can be seen by the plot below, the magnitude of the cartesian acceleration is indeed kept constant by this solution.

[Wallacoloo]

Actually, the movements do not necessarily cover only the first or fourth quadrant - that's only when starting from a velocity of 0. The c1 parameter can be used to control the initial angular frequency by sqrt(a/r)*sn(sqrt(a/r)*0+c1, -1) = w0. And once c1 has a non-zero value, the phase function will have a different range.

[Wallacoloo]

It may be appropriate to use the solved phase function and then switch to uniform circular motion at the first time where p'(t) is maximized. This assumes that p''(t)=0 at that point, otherwise transitioning to uniform circular motion would result in a lower cartesian acceleration value. This assumption is true for the phase function graphed further up; maximum occurs at t=1.31103 at which point p' = 1, p'' = 0.

This also assumes that any valid initial conditions produces a monotonic phase function up until the time at which p'(t) is first maximized. Intuitively, this looks to be the case, provided that our c1 is chosen to be the lowest non-negative value for which p'(0) = +/-sqrt(a/r)*sn(c1, -1) = w0.

[Wallacoloo]

Another (possibly easier) option is to choose p(t) = {nt^2+mt+p for t < t1 else n*t1^2+mt+p} such that the cartesian acceleration along the path for 0 < t < t1 never exceeds a limit.

Along this interval, |a|^2 = r^2*p'(t)^4 + r^2*p''(t)^2, or, |a/r|^2 = (2nt+m)^4 + (2n)^2. We would want p''(t1) = 0 so that the cartesian acceleration is the same at t1- as t1+. If it isn't, then that means that the cartesian acceleration during the portion of the movement with angular acceleration is greater than that in uniform circular motion, which means we either exceeded our acceleration limit, or we aren't running at full sleep during the uniform circular motion bit. p''(t) != 0 unless n = 0, so we need a 3rd degree polynomial phase function.

Then p(t) = {nt^3+mt^2+pt+q for t < t1 else n*t1^3+mt1^2+pt+q}. |a/r|^2 = (3nt^2+2mt+p)^4 + (6nt+2m)^2.
We know that p''(t1) = 0, so 6nt1+2m = 0.
The acceleration at t1 should be the predefined maximum, |amax/r|^2 = (3n*t1^2+2m*t1+p)^4, or sqrt(amax/r) = 3n*t1^2+2m*t1+p.
p is given as the initial angular velocity and q is known via the initial phase.
Still need one more constraint to determine t1.

[Wallacoloo]

For either approach, motion is first calculated at a constant velocity (or constant angular velocity for these arcs) and then the time axis is transformed in order to introduce acceleration.

Another way to think of this, for arcs, is that we first determine the phase as a function of the step number, i.e. p_s for all integer s, our acceleration function solves for p(t), and then we need to use that to determine the time, t(s) at which a motor should be at step s. We can set p_s = p(t), and then apply the inverse of p to each side to solve for t: t = p^-1(p_s). And so what we actually want to solve for is p^-1(t), not p(t).

Note: when we solve the second order differential directly in Mathematica, rather than reducing it to a first order and then integrating, we get a solution of a different form:

p = py /. DSolve[py''[t]^4 + py'[t]^2 == k, py, t]

{{py -> Function[{t}, 
    C[2] + 2/
      3 (k - InverseFunction[(
           Hypergeometric2F1[1/4, 1/2, 3/2, #1^2/k] #1 ((k - #1^2)/
             k)^(1/4))/(k - #1^2)^(1/4) &][-t + C[1]]^2)^(
      3/4)]}, {py -> 
   Function[{t}, 
    C[2] - 2/
      3 I (k - 
        InverseFunction[(
           Hypergeometric2F1[1/4, 1/2, 3/2, #1^2/k] #1 ((k - #1^2)/
             k)^(1/4))/(k - #1^2)^(1/4) &][-I t + C[1]]^2)^(
      3/4)]}, {py -> 
   Function[{t}, 
    C[2] + 2/
      3 I (k - 
        InverseFunction[(
           Hypergeometric2F1[1/4, 1/2, 3/2, #1^2/k] #1 ((k - #1^2)/
             k)^(1/4))/(k - #1^2)^(1/4) &][I t + C[1]]^2)^(
      3/4)]}, {py -> 
   Function[{t}, 
    C[2] - 2/
      3 (k - InverseFunction[(
           Hypergeometric2F1[1/4, 1/2, 3/2, #1^2/k] #1 ((k - #1^2)/
             k)^(1/4))/(k - #1^2)^(1/4) &][t + C[1]]^2)^(3/4)]}}

The four solutions are all pretty similar; I'm going to look at just the first one for now. To get p^-1(x), we take InverseFunction[ p[[1]] ]. This gives (note that even though the parameter is t, it should be treated as a phase):

Function[{t}, 
 C[1] - (Sqrt[
     4 k - 3 2^(2/3) 3^(1/3) t (t - C[2])^(1/3) + 
      3 2^(2/3) 3^(1/3) (t - C[2])^(1/3) C[2]] (1/
       k(k + 1/4 (-4 k + 3 2^(2/3) 3^(1/3) t (t - C[2])^(1/3) - 
           3 2^(2/3) 3^(1/3) (t - C[2])^(1/3) C[2])))^(1/4)
      Hypergeometric2F1[1/4, 1/2, 3/2, (
      4 k - 3 2^(2/3) 3^(1/3) t (t - C[2])^(1/3) + 
       3 2^(2/3) 3^(1/3) (t - C[2])^(1/3) C[2])/(
      4 k)])/(2 (k + 
       1/4 (-4 k + 3 2^(2/3) 3^(1/3) t (t - C[2])^(1/3) - 
          3 2^(2/3) 3^(1/3) (t - C[2])^(1/3) C[2]))^(1/4))]

If we look at the plot for the Hypergeometric2F1[1/4, 1/2, 3/2, m] function, we see that it's only valid for m < 1:

But under most circumstances, it's valid for all t > 0:

We only need to evaluate the Hypergeometric2F1[1/4, 1/2, 3/2, m(t)] function over the domain t in (0, pi), as the arc segments we traverse to round joints will always be at most pi radians in angle.

As k=(a/r)^2->0, the range of Hypergeometric2F1[1/4, 1/2, 3/2, m(t)] increases (m(t) spans a larger domain).

We can try to approximate Hypergeometric2F1[1/4, 1/2, 3/2, m] with a polynomial, but the error is not the best (upwards of 5% error):

MiniMaxApproximation[
 Hypergeometric2F1[1/4, 1/2, 3/2, m], {m, {-100, 1}, 7, 0}]
{{-100., -95.349, -82.2654, -63.195, -41.704, -21.8059, -7.16397, 
-0.0601049, 
  1.}, {1.04753 + 0.084571 m + 0.00856223 m^2 + 0.000424479 m^3 + 
   0.0000108773 m^4 + 1.48456*10^-7 m^5 + 1.0252*10^-9 m^6 + 
   2.81622*10^-12 m^7, 0.0476093}}

Note that the same error offered by a 7th degree polynomial can be matched by a rational function of degree 1 on top and degree 1 on bottom. The ratio of 2 degree-8 polynomials offers .001% error, and the ratio of 2 degree-4 polynomials offers a good error bound of < 0.1%:

MiniMaxApproximation[
 Hypergeometric2F1[1/4, 1/2, 3/2, m], {m, {-100, 1}, 4, 4}, 
 Brake -> {50, 50}]
{{-100., -70.2094, -28.8598, -8.71881, -1.8, 0.270868, 0.83571, 
  0.971243, 0.997355, 
  1.}, {(0.999516 - 1.73948 m + 0.823448 m^2 - 0.0772332 m^3 + 
     0.000510473 m^4)/(1 - 1.8227 m + 0.942005 m^2 - 0.115077 m^3 + 
     0.00141515 m^4), -0.000640665}}

Below is a plot of the error function:

Surprisingly, it doesn't diverge very rapidly for m < -100, reaching 1% error only around m=-220 and 10% error around m=-2000.

[Wallacoloo]

So how about the initial conditions:

1. A phase of 0 maps to a time of 0, i.e. p^-1(0) = 0, and p(0) = 0
2. We have a given initial angular velocity: p'(0) = w0

Start by defining p and p^-1 ("timeFromP"):

p := Function[{t}, 
 C[2] + 2/3 (k - 
     InverseFunction[(
        Hypergeometric2F1[1/4, 1/2, 3/2, #1^2/k] #1 ((k - #1^2)/k)^(
         1/4))/(k - #1^2)^(1/4) &][-t + C[1]]^2)^(3/4)]
timeFromP := InverseFunction[p]

For 2:

C1 = Solve[p'[0] == w0, C[1]]
{{C[1] -> (
   w0 ((k - w0^2)/k)^(1/4)
     Hypergeometric2F1[1/4, 1/2, 3/2, w0^2/k])/(k - w0^2)^(1/4)}}

For 1 (p^-1(0) = 0), Solve[(timeFromP[0] /. C1) == 0, C[2]] fails. But,

FullSimplify[(timeFromP[0] /. C1) == 0]

(1/k)C[2] (-((
     Sqrt[4 k - 3 2^(2/3) 3^(1/3) (-C[2])^(4/3)] ((-C[2])^(4/3))^(3/4)
       Hypergeometric2F1[1/4, 1/2, 3/2, 
       1 - (3 (3/2)^(1/3) (-C[2])^(4/3))/(2 k)])/((-C[2])^(4/3)/k)^(
     3/4)) + 2 w0 (k - w0^2)^(3/4)
      Hypergeometric2F1[1, 5/4, 3/2, w0^2/k]) == 0

Or, taking p(0) =0, we can do:

Solve[(p[0] /. C1) == 0, C[2]]

{{C[2] -> -(2/
     3) (k - InverseFunction[(
         Hypergeometric2F1[1/4, 1/2, 3/2, #1^2/k] #1 ((k - #1^2)/k)^(
          1/4))/(k - #1^2)^(1/4) &][(
       w0 ((k - w0^2)/k)^(1/4)
         Hypergeometric2F1[1/4, 1/2, 3/2, w0^2/k])/(k - w0^2)^(
       1/4)]^2)^(3/4)}}

which is really just

{{C[2] -> -(2/
     3) (k - InverseFunction[(
         Hypergeometric2F1[1/4, 1/2, 3/2, #1^2/k] #1 ((k - #1^2)/k)^(
          1/4))/(k - #1^2)^(1/4) &][C[1]]^2)^(3/4)}}

It still requires solving the inverse function, unfortunately.

[Wallacoloo]

Something is wrong in the solution that involves Hypergeometric2F1. Cartesian acceleration should be constant, but it isn't:

[Wallacoloo]

Plotting a(t)^2 for k=1, C[1]=0, C[2]=0 (using the 4th solution of p) gives the following result, which is also incorrect:

In the above case, a(t)^2 should be a constant 1.

[Wallacoloo]

So disregard all the above posts about the Hypergeometric2F1 solution; that was derived in error via p = py /. DSolve[py''[t]^4 + py'[t]^2 == k, py, t], instead of p = py /. DSolve[py''[t]^2 + py'[t]^4 == k, py, t] (exponents were swapped).

Back to the Jacobi elliptical functions. We can derive the solution for phase[t] and solve its initial conditions fairly easily:

There were two solutions to the differential equation. Picking the first one gives a phase function like this:

Whereas the second one provides this phase function:

I will first solve for counter-clockwise motion, and so the second solution is the one I will be dealing with. As before, we now know phase(t) and p_s (phase at any step). By setting them equal, and applying the inverse phase function, we get t = phase^-1(p_s), which is the time we wish to schedule the step, s.
Mathematica fails to find a closed-form solution for t, and InverseFunction[phase] is unable to be simplified:

[Wallacoloo]

The first thing to notice is that
phase[t] = C[2] - I Log[-I JacobiCN[k^(1/4) (t + C[1]), -1] + JacobiDN[k^(1/4) (t + C[1]), -1]]
is just
psimple[t] = -I Log[-I JacobiCN[t, -1] + JacobiDN[t, -1] ]
shifted C[2] up, compressed horizontally by k^(1/4), and then shifted C[1] to the left.

That is, phase[t] = C[2] + psimple[k^(1/4)(t + C[1])]. If we can solve for psimple^-1, then:

phase[t] - C[2] = psimple[k^(1/4)(t + C[1])]
psimple^-1[phase[t] - C[2]] = k^(1/4)(t + C[1])
psimple^-1[phase[t] - C[2]]*k^(-1/4) - C[1]=t

To make things clear, in this case, phase[t] = p_s is the phase for the desired step, and t becomes the time at which that step should occur.

Mathematica cannot find an inverse for psimple[t], but perhaps we can approximate the inverse with a polynomial. It appears that for a real argument, the imaginary part of psimple[t] is always -0.346574i (which is -Im[ I Log[1 - I] ]). Even when restricted to the real portion of psimple, MiniMaxApproximation is having a seriously hard time solving this.

I found a way to get MiniMaxApproximation to deal with this function:
Note that psimple[0] = -I Log[1 - I]. Then we want MiniMaxApproximation to approximate psimple[t] - -I Log[1 - I] and then add Re[ psimple[0] ] manually.

  psimple[t] - -I Log[1 - I]
=psimple[t] + -I Log[1/(1 - I)]
=-I Log[1/(1 - I) * (-I JacobiCN[t, -1] + JacobiDN[t, -1])]
=Re[-I Log[1/(1 - I) * (-I JacobiCN[t, -1] + JacobiDN[t, -1])]]
=Arg[-I JacobiCN[t, -1] + JacobiDN[t, -1] + JacobiCN[t, -1] + I JacobiDN[t, -1]]
=Arg[I (-JacobiCN[t, -1] + JacobiDN[t, -1]) + (JacobiCN[t, -1] + JacobiDN[t, -1])]
=ArcTan[JacobiCN[t, -1] + JacobiDN[t, -1], -JacobiCN[t, -1] + JacobiDN[t, -1]]

And so we have MiniMaxApproximation model ArcTan, since that maps reals to reals and has real derivatives.

MiniMaxApproximation[
 ArcTan[JacobiCN[t, -1] + JacobiDN[t, -1], -JacobiCN[t, -1] + 
   JacobiDN[t, -1]], {t, {0.01, Pi - 0.01}, 3, 3}, 
 WorkingPrecision -> 40, Brake -> {100, 100}]

gives psimple[t]-psimple[0] approximately equal to:

(-0.001031981576912337009860396084357273797144 + 
   0.02656793327771770373589789103896668830631 t + 
   0.3771824345317184022964928850400782106848 t^2 - 
   0.09063794108378029289036600533713127391886 \
t^3)/(1.000000000000000000000000000000000000000 - 
   0.5838344480310404550653756078342174178262 t + 
   0.2616200502516551770723808352668579236782 t^2 - 
   0.03403047408035478316539290470506712182309 t^3)

Below is psimple[t]-psimple[0] followed by the rational approximation, followed by the difference between them over 0 < t < Pi

Note: we actually want to approximate psimple^-1[t], not psimple[t]...

[Wallacoloo]

It turns out that MiniMaxApproximation accepts a Derivatives argument, which may help with it erroring on certain inputs. From the Documentation: "Derivatives - {func, D[func, x], D[func, x, 2]}. This option may be left Automatic if Mathematica can find the derivatives itself. However, it may be more efficient to specify a function that returns the required list of values for any x in the interval."