Merge pull request #27 from RoboticExplorationLab/fix-finitediff

Fix finite diff error
RoboticExplorationLab · Aug 4, 2022 · de615b6 · de615b6 · bjack205 · Aug 4, 2022
2 parents 6ea2482 + 40a4476
commit de615b6
Show file tree

Hide file tree

Showing 7 changed files with 107 additions and 84 deletions.
diff --git a/.vscode/settings.json b/.vscode/settings.json
@@ -0,0 +1,3 @@
+{
+    "julia.environmentPath": "/home/brian/.julia/dev/RobotDynamics"
+}
diff --git a/Project.toml b/Project.toml
@@ -1,12 +1,13 @@
 name = "RobotDynamics"
 uuid = "38ceca67-d8d3-44e8-9852-78a5596522e1"
 authors = ["Brian Jackson <[email protected]>"]
-version = "0.4.6"
+version = "0.4.7"
 
 [deps]
 FiniteDiff = "6a86dc24-6348-571c-b903-95158fe2bd41"
 ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210"
 LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
+Pkg = "44cfe95a-1eb2-52ea-b672-e2afdf69b78f"
 RecipesBase = "3cdcf5f2-1ef4-517c-9805-6587b60abb01"
 Rotations = "6038ab10-8711-5258-84ad-4b1120ba62dc"
 SparseArrays = "2f01184e-e22b-5df5-ae63-d93ebab69eaf"

diff --git a/src/RobotDynamics.jl b/src/RobotDynamics.jl
@@ -7,6 +7,7 @@ using ForwardDiff
 using FiniteDiff
 using RecipesBase
 using SparseArrays
+using Pkg
 
 using Rotations: skew
 using StaticArrays: SUnitRange

diff --git a/src/jacobian_gen.jl b/src/jacobian_gen.jl
@@ -777,9 +777,16 @@ function modify_struct_def(::FiniteDifference, struct_expr::Expr, mod, is_scalar
             :(hesscache::FiniteDiff.HessianCache{Vector{Float64}, Val{:hcentral}(), Val{true}()})
         ]
     else
-        newfield = [
-            :(cache::FiniteDiff.JacobianCache{Vector{$type_param}, Vector{$type_param}, Vector{$type_param}, UnitRange{Int64}, Nothing, Val{:forward}(), $type_param})
-        ]
+        finitediff_version = get_dependency_version("FiniteDiff")
+        newfield = if finitediff_version >= v"2.13"
+            [
+                :(cache::FiniteDiff.JacobianCache{Vector{$type_param}, Vector{$type_param}, Vector{$type_param}, Vector{$type_param}, UnitRange{Int64}, Nothing, Val{:forward}(), $type_param})
+            ]
+        else
+            [
+                :(cache::FiniteDiff.JacobianCache{Vector{$type_param}, Vector{$type_param}, Vector{$type_param}, UnitRange{Int64}, Nothing, Val{:forward}(), $type_param})
+            ]
+        end
     end
 
     struct_expr = copy(struct_expr)

diff --git a/src/utils.jl b/src/utils.jl
@@ -52,3 +52,9 @@ function LinearAlgebra.lu!(A::StridedMatrix{T}, ipiv::AbstractVector{BlasInt}; c
     check && LinearAlgebra.checknonsingular(lpt[3])
     return LU{T,typeof(A)}(lpt[1], lpt[2], lpt[3])
 end
+
+function get_dependency_version(pkg_name)
+    m = Pkg.dependencies()
+    v = m[findfirst(v -> v.name == pkg_name, m)].version
+    v
+end
diff --git a/test/function_base_explicit_test.jl b/test/function_base_explicit_test.jl
@@ -0,0 +1,82 @@
+
+struct TestFun0{CH} <: RobotDynamics.AbstractFunction
+    cfg::ForwardDiff.JacobianConfig{Nothing, Float64, CH, Tuple{Vector{ForwardDiff.Dual{Nothing, Float64, CH}}, Vector{ForwardDiff.Dual{Nothing, Float64, CH}}}}
+    cache::FiniteDiff.JacobianCache{Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}, UnitRange{Int64}, Nothing, Val{:forward}(), Float64}
+    function TestFun0()
+        n,m,p = 2,2,3
+        cfg = ForwardDiff.JacobianConfig(nothing, zeros(p), zeros(n+m))
+        cache = FiniteDiff.JacobianCache(zeros(n+m), zeros(p))
+        new{length(cfg.seeds)}(cfg, cache)
+    end
+end
+function RobotDynamics.evaluate(::TestFun0, x, u, p)
+    return SA[cos(x[1]) * u[1], sin(x[2]^2 * x[1]) * u[2], exp(x[2] + x[1]/10)] * p[1]
+end
+function RobotDynamics.evaluate!(::TestFun0, y, x, u, p)
+    y[1] = cos(x[1])  * u[1]
+    y[2] = sin(x[2]^2 * x[1]) * u[2]
+    y[3] = exp(x[2] + x[1]/10)
+    y .*= p[1]
+    return nothing
+end
+RobotDynamics.state_dim(::TestFun0) = 2
+RobotDynamics.control_dim(::TestFun0) = 2
+RobotDynamics.output_dim(::TestFun0) = 3
+
+function RobotDynamics.jacobian!(::TestFun0, J, y, x, u, p)
+    J .= 0
+    J[1,1] = -sin(x[1]) * u[1]
+    J[1,3] = cos(x[1])
+    J[2,1] = x[2]^2 * cos(x[2]^2 * x[1]) * u[2]
+    J[2,2] = 2 * x[1] * x[2] * cos(x[2]^2 * x[1]) * u[2]
+    J[2,4] = sin(x[2]^2 * x[1])
+    J[3,1] = exp(x[2] + x[1]/10) / 10
+    J[3,2] = exp(x[2] + x[1]/10)
+    J .*= p[1]
+    return nothing
+end
+
+function RobotDynamics.jacobian!(::StaticReturn, ::ForwardAD, fun::TestFun0, J, y, z)
+    f(_z) = RobotDynamics.evaluate(fun, getstate(z, _z), getcontrol(z, _z), getparams(z))
+    J .= ForwardDiff.jacobian(f, getdata(z))
+    return nothing
+end
+
+function RobotDynamics.jacobian!(::InPlace, ::ForwardAD, fun::TestFun0, J, y, z)
+    f!(_y,_z) = RobotDynamics.evaluate!(fun, _y, getstate(z, _z), getcontrol(z, _z), getparams(z))
+    ForwardDiff.jacobian!(J, f!, y, getdata(z), fun.cfg)
+    return nothing
+end
+
+function RobotDynamics.jacobian!(::StaticReturn, ::FiniteDifference, fun::TestFun0, J, y, z)
+    f!(_y,_z) = _y .= RobotDynamics.evaluate(fun, getstate(z, _z), getcontrol(z, _z), getparams(z))
+    FiniteDiff.finite_difference_jacobian!(J, f!, getdata(z), fun.cache)
+    return nothing
+end
+
+function RobotDynamics.jacobian!(::InPlace, ::FiniteDifference, fun::TestFun0, J, y, z)
+    f!(_y,_z) = RobotDynamics.evaluate!(fun, _y, getstate(z, _z), getcontrol(z, _z), getparams(z))
+    FiniteDiff.finite_difference_jacobian!(J, f!, getdata(z), fun.cache)
+    return nothing
+end
+
+fun = TestFun0()
+n,m,p = RD.dims(fun)
+x = @SVector randn(n)
+u = @SVector randn(m)
+t = 1.2
+dt = 0.1
+
+zs = KnotPoint{n,m}([x;u],t,dt) 
+z = KnotPoint{n,m}(Vector([x;u]),t,dt)
+z_ = copy(z.z)
+@test getstate(zs, z_) isa SVector{n}
+@test getcontrol(zs, z_) isa SVector{m}
+@test getstate(z, z_) isa SubArray 
+@test getcontrol(z, z_) isa SubArray 
+
+RD.evaluate(fun, zs)
+y = zeros(3)
+RD.evaluate!(fun, y, z)
+
+test_fun(fun)
diff --git a/test/function_base_test.jl b/test/function_base_test.jl
@@ -78,88 +78,11 @@ end
 
 
 ## 
-struct TestFun0{CH} <: RobotDynamics.AbstractFunction
-    cfg::ForwardDiff.JacobianConfig{Nothing, Float64, CH, Tuple{Vector{ForwardDiff.Dual{Nothing, Float64, CH}}, Vector{ForwardDiff.Dual{Nothing, Float64, CH}}}}
-    cache::FiniteDiff.JacobianCache{Vector{Float64}, Vector{Float64}, Vector{Float64}, UnitRange{Int64}, Nothing, Val{:forward}(), Float64}
-    function TestFun0()
-        n,m,p = 2,2,3
-        cfg = ForwardDiff.JacobianConfig(nothing, zeros(p), zeros(n+m))
-        cache = FiniteDiff.JacobianCache(zeros(n+m), zeros(p))
-        new{length(cfg.seeds)}(cfg, cache)
-    end
-end
-function RobotDynamics.evaluate(::TestFun0, x, u, p)
-    return SA[cos(x[1]) * u[1], sin(x[2]^2 * x[1]) * u[2], exp(x[2] + x[1]/10)] * p[1]
-end
-function RobotDynamics.evaluate!(::TestFun0, y, x, u, p)
-    y[1] = cos(x[1])  * u[1]
-    y[2] = sin(x[2]^2 * x[1]) * u[2]
-    y[3] = exp(x[2] + x[1]/10)
-    y .*= p[1]
-    return nothing
-end
-RobotDynamics.state_dim(::TestFun0) = 2
-RobotDynamics.control_dim(::TestFun0) = 2
-RobotDynamics.output_dim(::TestFun0) = 3
-
-function RobotDynamics.jacobian!(::TestFun0, J, y, x, u, p)
-    J .= 0
-    J[1,1] = -sin(x[1]) * u[1]
-    J[1,3] = cos(x[1])
-    J[2,1] = x[2]^2 * cos(x[2]^2 * x[1]) * u[2]
-    J[2,2] = 2 * x[1] * x[2] * cos(x[2]^2 * x[1]) * u[2]
-    J[2,4] = sin(x[2]^2 * x[1])
-    J[3,1] = exp(x[2] + x[1]/10) / 10
-    J[3,2] = exp(x[2] + x[1]/10)
-    J .*= p[1]
-    return nothing
-end
-
-function RobotDynamics.jacobian!(::StaticReturn, ::ForwardAD, fun::TestFun0, J, y, z)
-    f(_z) = RobotDynamics.evaluate(fun, getstate(z, _z), getcontrol(z, _z), getparams(z))
-    J .= ForwardDiff.jacobian(f, getdata(z))
-    return nothing
-end
-
-function RobotDynamics.jacobian!(::InPlace, ::ForwardAD, fun::TestFun0, J, y, z)
-    f!(_y,_z) = RobotDynamics.evaluate!(fun, _y, getstate(z, _z), getcontrol(z, _z), getparams(z))
-    ForwardDiff.jacobian!(J, f!, y, getdata(z), fun.cfg)
-    return nothing
-end
-
-function RobotDynamics.jacobian!(::StaticReturn, ::FiniteDifference, fun::TestFun0, J, y, z)
-    f!(_y,_z) = _y .= RobotDynamics.evaluate(fun, getstate(z, _z), getcontrol(z, _z), getparams(z))
-    FiniteDiff.finite_difference_jacobian!(J, f!, getdata(z), fun.cache)
-    return nothing
-end
-
-function RobotDynamics.jacobian!(::InPlace, ::FiniteDifference, fun::TestFun0, J, y, z)
-    f!(_y,_z) = RobotDynamics.evaluate!(fun, _y, getstate(z, _z), getcontrol(z, _z), getparams(z))
-    FiniteDiff.finite_difference_jacobian!(J, f!, getdata(z), fun.cache)
-    return nothing
+const finitediff_version = RobotDynamics.get_dependency_version("FiniteDiff")
+@static if finitediff_version >= v"2.13"
+    include("function_base_explicit_test.jl")
 end
 
-fun = TestFun0()
-n,m,p = RD.dims(fun)
-x = @SVector randn(n)
-u = @SVector randn(m)
-t = 1.2
-dt = 0.1
-
-zs = KnotPoint{n,m}([x;u],t,dt) 
-z = KnotPoint{n,m}(Vector([x;u]),t,dt)
-z_ = copy(z.z)
-@test getstate(zs, z_) isa SVector{n}
-@test getcontrol(zs, z_) isa SVector{m}
-@test getstate(z, z_) isa SubArray 
-@test getcontrol(z, z_) isa SubArray 
-
-RD.evaluate(fun, zs)
-y = zeros(3)
-RD.evaluate!(fun, y, z)
-
-test_fun(fun)
-
 ##############################
 # Autogen With Inner Constructor 
 ##############################