Smooth transitions between flow commands

[nerdoug]

The original flow implementation made abrupt changes in the servo movements between one flow command and the following one. For example, one flow might be moving the knee servo +3 degrees per frame, and the following one might require -5 degrees per frame. At the transition between frames, the would be a reversal and change of 8 degrees in one frame. It's desirable to smooth out this change for a couple of reasons:

• it's unrealistic. In the real world, inertia and momentum resist sudden large changes
• it's stressful on the leg hardware
• it may not be possible to fit a large change into the time available for one frame. Better to spread it out so the servos can keep up
• it may cause a surge on the current required by the servos, possibly resulting in the move not being completed, or in the worst case, in a fuse blowing or damage to circuitry

[nerdoug]

We need to do 50 frames per second for smooth leg motion, and if we assume that each flow is at least 200 Msec long, we will have at least 10 frames per flow. Thus we can assume that we have 5 frames at the beginning of the flow, an 5 at the end of the flow than can be modified for smoothing purposes. This is equivalent to smoothing the transition over a 200 msec period.

It is important that the smoothing process maintain the total amount of servo rotation, otherwise the motion of the robot would be affected.

Let's use the example above for a knee servo to illustrate. Assume we have a flow with 20 frames identified as f01... f20 with a frame delta servo movement of +3 degrees. It's followed by another flow with 20 frames, identified as g01, g02... with a frame delta servo movement of -5 degrees. This might be represented by a timeline of servo changes like this:

f14  f15  f16  f17  f18  f19  f20 : g01  g02  g03  g04  g05  g06  g07
----------------------------------:-----------------------------------
 +3   +3   +3   +3   +3   +3   +3 :  -5   -5   -5   -5   -5   -5   -5

The big change happens between the last frame of the first flow, f20, and the first frame of the next flow, where servo rotation changes from +5 degrees to -5 degrees. To smooth this, we'll modify the last five frames of the first flow, and the first five frames of the second flow, highlighted here:

f14  f15  f16  f17  f18  f19  f20 : g01  g02  g03  g04  g05  g06  g07
----------------------------------:-----------------------------------
 +3   +3   +3   +3   +3   +3   +3 :  -5   -5   -5   -5   -5   -5   -5
           ^^^^^^^^^^^^^^^^^^^^^^   ^^^^^^^^^^^^^^^^^^^^^^

We want our smoothed movement to have the same result as the original, i.e. leaving the servo with a -10 degree displacement ( = 3 + 3 + 3 + 3 + 3 - 5 - 5 - 5 - 5 - 5 = 15 -25 = -10), and at the delta value of the second flow, -5.

We'll build a set of linear steps from the starting value ( +3 at f15 in our example) to the end value ( -5 at g06 ). In our example, this is a delta of -5 - 3 = -8, in 11 steps (including one at each end), for what I call the average delta of -8/11 = .83 . This leads to frame values like this:

f14   f15    f16     f17    f18    f19     f20 :   g01   g02    g03    g04    g05    g06  
-----------------------------------------------:-----------------------------------
 +3   +3    +2.27   +1.55  +0.82  +0.09  -0.64 : -1.36  -2.09  -2.82  -3.55  -4.27  -5.00
            ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^   ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^

If we sum these values, we see that they add up to the 10 degree displacement of the original abrupt change.

to be added:

• interpretation of angular velocity and angular acceleration, and jerk
• graphs of V,A & J for original, and smoothed cases.
• implication that frames are pre-calculated so values for smoothing are known in previous flow
• need for abort mechanism if collision sensor, or navigation control detects need for quick change
• similar approach to starting from a standstill - ramp up speed over 5 frames

[nerdoug]

The angle changes above for each frame represent the angular velocity. Velocity is the rate of change of location, and in our case, the location is the rotation angle of the servo. If each frame has a change of 3 degrees every 20 msec, the angular velocity is 3 degrees / .020 sec = 150 degrees per second (not a very realistic example)

Acceleration is the rate of change of velocity. Within one flow, the velocity is the same for all frames, so the acceleration is zero. However, at the flow boundary we go from 3 degrees per frame to -5 degrees per frame, which is a sudden strong acceleration (think whiplash). In that one frame, the acceleration would be (change in velocity) / (change in time) = (-8 degrees per second) / (.020 sec) = -400 degrees per second per second, or -400 degrees per second squared)

For reason outlined at the beginning of this issue, smooth acceleration is better that abrupt acceleration. A limo driver accelerates gradually, to minimize discomfort for his passengers. a teenager pops the clutch to get maximum acceleration ("G force") at the cost of the discomfort of being pushed into his seat.

As a skydiver, I know that parachute openings are intentionally slowed down so the rapid deceleration is spread over more time, decrease the maximum deceleration (and pain) experienced by the skydiver.

There is actually another measurement called jerk, which is the rate of change of acceleration over time. The limo driver, and our smoothed flow transition have a relatively low jerk, whereas the original sudden frame delta, and the clutch popper have a large jerk. Some graphs are in order. (pause while Doug builds a spreadsheet).

[nerdoug]

charts and graphs illustrating the reduced acceleration when flow transitions are smoothed can be found in .../docs/math/smoothed-flows.*

Implementation notes:

• first, clean up dispatch of flow processing from loop()
• benchmark flow processing cpu usage
• restructure loop() code so maximum of one routine runs per loop
• benchmark flow processing cpu usage again
• decouple leg movement from calculation of frame positions. frame positions need to be pre-calculated to have the data available that's need for smoothing at the end of the previous flow
• benchmark flow processing cpu usage, ideally for both leg movement code, and frame precalculation code
• optimize coding of flow processing general. Need a separate issue for that, which I'll enter
• benchmark flow processing after optimization efforts

[nerdoug]

re-opening after uploading charts/graphs. Still need to do implementation.