Avoid floating point error accumulation in PeriodicController

[ChrisRackauckas]

This is the classic problem of 0.1 + 0.1 + 0.1 != 0.3, but the periodic callback checker that was there assumes it's going to be exact, and checks exactly above and below. However, we can fix this by instead using start + steps*dt for calculating the current location. This is the change that happened internally to DifferentialEquations and thus it was more correct, but sadly that made it different from the calculation here which exposed the floating point accumulation problem. However, by doing the same thing start + steps * dt here, we can avoid accumulating floating point error and thus keep the accurate check. Another way to fix this is by things like +eps(t), but this is probably nicer because it directly fixes the problem of accumulation in the first place.

Fixes https://github.com/JuliaRobotics/RigidBodySim.jl/issues/113

[ChrisRackauckas]

Fixes the legendary issue https://github.com/JuliaRobotics/RigidBodySim.jl/pull/72 :

using OrdinaryDiffEq
using DiffEqCallbacks

lc1 = -0.5
l1 = -1.
m1 = 1.
I1 = 0.333
lc2 = -1.
l2 = -2.
m2 = 1.
I2 = 1.33
g = -9.81

tau = zeros(2)
last_control_time = Ref(NaN)
next_control_time = Ref(NaN)
controltimes = Float64[]

function dynamics(u, p, t)
    @show t

    if (t == next_control_time[] && t != last_control_time[]) || isnan(last_control_time[])
        println("control")
        tau[1] = sin(t)
        tau[2] = cos(t)
        push!(controltimes, t)
        last_control_time[] = t
    end

    # double pendulum
    q = u[1 : 2]
    v = u[3 : 4]

    c1 = cos(q[1])
    c2 = cos(q[2])
    s1 = sin(q[1])
    s2 = sin(q[2])
    s12 = sin(q[1] + q[2])

    M11 = I1 + I2 + m2 * l1^2 + 2 * m2 * l1 * lc2 * c2
    M12 = I2 + m2 * l1 * lc2 * c2
    M22 = I2
    M = [M11 M12; M12 M22]

    C11 = -2 * m2 * l1 * lc2 * s2 * v[2]
    C12 = -m2 * l1 * lc2 * s2 * v[2]
    C21 = m2 * l1 * lc2 * s2 * v[1]
    C22 = 0
    C = [C11 C12; C21 C22]

    G = [m1 * g * lc1 * s1 + m2 * g * (l1 * s1 + lc2 * s12); m2 * g * lc2 * s12]

    vd = M \ (tau - C * v - G)
    [v; vd]
end

Δt = 0.25
f = function (integrator)
    next_control_time[] = integrator.t + Δt
    u_modified!(integrator, false)
end
initialize = (c, u, t, integrator) -> (empty!(controltimes); last_control_time[] = NaN)
periodic = PeriodicCallback(f, Δt; initialize = initialize, save_positions = (false, false))
u0 = zeros(4)
final_time = 25.3
problem = ODEProblem(dynamics, u0, (0., final_time), callback = periodic)
solve(problem, Tsit5(), abs_tol = 1e-10, dt = 0.05);
@assert controltimes == collect(0. : Δt : final_time - rem(final_time, Δt))

which is the oldest thing on my todo list (October 30th, 2018)

[ferrolho]

which is the oldest thing on my todo list (October 30th, 2018)

Cheers for this one! :beers: And many thanks for the PR!

[ChrisRackauckas]

        tau[1] = sin(t)
        tau[2] = cos(t)

then

vd = M \ (tau - C * v - G)

That's why u_modified needs to be true. I guess it's not the best named, because it means that u and du are going to be the same, otherwise the FSAL point needs to be re-evaluated.

[ChrisRackauckas]

The floating point fixes can only exist past v1.1, and most things have a pretty hard break at v1.3 due to parallelism, so I set the floor to Julia v1.3 and set Travis to roam at 1.x.

[ChrisRackauckas]

I'm not sure what to do about the visualizer but the simulation seems to be good now.

[ferrolho]

@tkoolen, can you have a look at this? :slightly_smiling_face:

[phelipe]

This will be merged? #113 still happens.