From a635d9258de9e30c5cfb6f0b2b7446842cc3db98 Mon Sep 17 00:00:00 2001
From: Twan Koolen <koolen.twan@gmail.com>
Date: Mon, 30 Jul 2018 12:03:55 -0400
Subject: [PATCH 1/3] Work around (fix?) #68.

Using `u_modified!(integrator, true)` instead of `false`. See
https://github.com/JuliaRobotics/RigidBodySim.jl/pull/72#issuecomment-408911804.
---
 src/control.jl   | 4 ++--
 test/runtests.jl | 4 ++--
 2 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/control.jl b/src/control.jl
index 937582d..550cfec 100644
--- a/src/control.jl
+++ b/src/control.jl
@@ -77,7 +77,7 @@ julia> τ = zeros(velocity(state)); Δt = 1 / 200
 
 julia> problem = ODEProblem(Dynamics(mechanism, PeriodicController(τ, Δt, pdcontrol!)), state, (0., 5.));
 
-julia> sol = solve(problem, Vern7());
+julia> sol = solve(problem, Tsit5());
 
 julia> sol.u[end]
 4-element Array{Float64,1}:
@@ -119,7 +119,7 @@ function DiffEqCallbacks.PeriodicCallback(controller::PeriodicController)
     f = let controller = controller
         function (integrator)
             controller.docontrol[] = true
-            u_modified!(integrator, false)
+            u_modified!(integrator, true) # see https://github.com/JuliaRobotics/RigidBodySim.jl/pull/72#issuecomment-408911804
         end
     end
     PeriodicCallback(f, controller.Δt; initialize = periodic_initialize, save_positions = controller.save_positions)
diff --git a/test/runtests.jl b/test/runtests.jl
index 9d87fb0..9c79c87 100644
--- a/test/runtests.jl
+++ b/test/runtests.jl
@@ -232,10 +232,10 @@ end
     problem = ODEProblem(Dynamics(mechanism, controller), state, (0., final_time))
 
     # ensure that controller gets called at appropriate times:
-    sol = solve(problem, Vern7(), abs_tol = 1e-10, dt = 0.05)
+    sol = solve(problem, Tsit5(), abs_tol = 1e-10, dt = 0.05)
     @test controltimes == collect(0. : Δt : final_time - rem(final_time, Δt))
 
-    # ensure that we can solve the same problem again without errors
+    # ensure that we can solve the same problem again without errors and with a different integrator
     empty!(controltimes)
     sol = solve(problem, Vern7(), abs_tol = 1e-10, dt = 0.05)
     @test controltimes == collect(0. : Δt : final_time - rem(final_time, Δt))

From e131bcba8b2ebdb52b81901290165f6c92a25e7e Mon Sep 17 00:00:00 2001
From: Twan Koolen <koolen.twan@gmail.com>
Date: Mon, 30 Jul 2018 13:29:04 -0400
Subject: [PATCH 2/3] Fix floating point issue.

---
 src/control.jl | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/src/control.jl b/src/control.jl
index 550cfec..3d271a5 100644
--- a/src/control.jl
+++ b/src/control.jl
@@ -152,8 +152,7 @@ function (controller::PeriodicController)(τ::AbstractVector, t, state)
         controller.last_control_time[] = t
     end
     Compat.copyto!(τ, controller.τ)
-    time_since_last_control = t - controller.last_control_time[]
-    if time_since_last_control > controller.Δt || time_since_last_control < zero(time_since_last_control)
+    if t > controller.last_control_time[] + controller.Δt || t < controller.last_control_time[]
         throw(PeriodicControlFailure(controller.Δt, t, controller.last_control_time[]))
     end
     τ

From 7ad4e0ca9facc761cbbda2380fde9f7936843f07 Mon Sep 17 00:00:00 2001
From: Twan Koolen <koolen.twan@gmail.com>
Date: Mon, 30 Jul 2018 14:08:56 -0400
Subject: [PATCH 3/3] Fix doctest.

---
 src/control.jl | 7 -------
 1 file changed, 7 deletions(-)

diff --git a/src/control.jl b/src/control.jl
index 3d271a5..ad74dea 100644
--- a/src/control.jl
+++ b/src/control.jl
@@ -79,13 +79,6 @@ julia> problem = ODEProblem(Dynamics(mechanism, PeriodicController(τ, Δt, pdco
 
 julia> sol = solve(problem, Tsit5());
 
-julia> sol.u[end]
-4-element Array{Float64,1}:
- -3.25923e-5
- -1.67942e-5
-  8.16715e-7
-  1.55292e-8
-
 julia> @assert all(x -> isapprox(x, 0, atol = 1e-4), sol.u[end]) # ensure state converges to zero
 
 julia> controlcalls[]