Avoid floating point error accumulation in PeriodicController

.travis.yml

@@ -3,8 +3,7 @@ os:
   - linux
   - osx
 julia:
-  - 1.0
-  - 1.2
+  - 1
   - nightly
 matrix:
   allow_failures:

Project.toml

@@ -28,7 +28,7 @@ Observables = "≥ 0.1.2"
 OrdinaryDiffEq = "≥ 3.0.1"
 RigidBodyDynamics = "≥ 0.6.0"
 WebIO = "≥ 0.2.6"
-julia = "≥ 0.7.0"
+julia = "1.3"
 
 [extras]
 GR = "28b8d3ca-fb5f-59d9-8090-bfdbd6d07a71"

src/control.jl

@@ -92,27 +92,28 @@ struct PeriodicController{Tau<:AbstractVector, T<:Number, C, I}
     initialize::I
     save_positions::Tuple{Bool, Bool}
     docontrol::Base.RefValue{Bool}
-    last_control_time::Base.RefValue{T} # only used for checking that PeriodicCallback is correctly set up
-
+    start_time::Base.RefValue{T} # only used for checking that PeriodicCallback is correctly set up
+    index::Base.RefValue{Int}
     function PeriodicController(τ::Tau, Δt::T, control!::C;
             initialize::I = DiffEqBase.INITIALIZE_DEFAULT,
             save_positions = (false, false)) where {Tau<:AbstractVector, T<:Number, C, I}
-        new{Tau, T, C, I}(τ, Δt, control!, initialize, save_positions, Ref(true), Ref(T(NaN)))
+        new{Tau, T, C, I}(τ, Δt, control!, initialize, save_positions, Ref(true), Ref(T(NaN)), Ref(-1))
     end
 end
 
 function DiffEqCallbacks.PeriodicCallback(controller::PeriodicController)
     periodic_initialize = let controller = controller
         function (c, u, t, integrator)
             controller.docontrol[] = true
-            controller.last_control_time[] = NaN
+            controller.start_time[] = t
             controller.initialize(c, u, t, integrator)
         end
     end
     f = let controller = controller
         function (integrator)
             controller.docontrol[] = true
-            u_modified!(integrator, true) # see https://github.com/JuliaRobotics/RigidBodySim.jl/pull/72#issuecomment-408911804
+            controller.index[] += 1
+            u_modified!(integrator, true) # see https://github.com/JuliaRobotics/RigidBodySim.jl/pull/126
         end
     end
     PeriodicCallback(f, controller.Δt; initialize = periodic_initialize, save_positions = controller.save_positions)
@@ -142,11 +143,12 @@ function (controller::PeriodicController)(τ::AbstractVector, t, state)
     if controller.docontrol[]
         controller.control!(controller.τ, t, state)
         controller.docontrol[] = false
-        controller.last_control_time[] = t
     end
     copyto!(τ, controller.τ)
-    if t > controller.last_control_time[] + controller.Δt || t < controller.last_control_time[]
-        throw(PeriodicControlFailure(controller.Δt, t, controller.last_control_time[]))
+    last_control_time = controller.start_time[] + controller.index[] * controller.Δt
+    next_control_time = controller.start_time[] + (controller.index[]+1) * controller.Δt
+    if t > next_control_time || t < last_control_time
+        throw(PeriodicControlFailure(controller.Δt, t, last_control_time))
     end
     τ
 end

test/runtests.jl

@@ -110,16 +110,18 @@ end
 
     # ensure that we can solve the same problem again without errors and with a different integrator
     empty!(controltimes)
+    controller.index[] = -1
     sol = solve(problem, Vern7(), abs_tol = 1e-10, dt = 0.05)
     @test controltimes == collect(0. : Δt : final_time - rem(final_time, Δt))
 
     # issue #60
     empty!(controltimes)
+    controller.index[] = -1
     problem60 = ODEProblem(Dynamics(mechanism, (τ, t, state) -> controller(τ, t, state)), state, (0., final_time))
     @test_throws RigidBodySim.Control.PeriodicControlFailure solve(problem60, Vern7(), abs_tol = 1e-10, dt = 0.05)
     controller = controller = make_controller()
     problem60 = ODEProblem(Dynamics(mechanism, (τ, t, state) -> controller(τ, t, state)), state, (0., final_time))
-    @test_throws RigidBodySim.Control.PeriodicControlFailure solve(problem60, Vern7(), abs_tol = 1e-10, dt = 0.05)
+    @test 0.25 ∉ solve(problem60, Vern7(), abs_tol = 1e-10, dt = 0.05).t
     problem60_fixed = ODEProblem(Dynamics(mechanism, (τ, t, state) -> controller(τ, t, state)), state, (0., final_time),
         callback = PeriodicCallback(controller))
     sol60 = solve(problem60_fixed, Vern7(), abs_tol = 1e-10, dt = 0.05)