From 46f5d531c51943c996d68283b564427d79ed16d7 Mon Sep 17 00:00:00 2001 From: Brian Jackson Date: Thu, 4 Aug 2022 08:10:21 -0400 Subject: [PATCH 1/2] Fix finite diff error --- .vscode/settings.json | 3 ++ Project.toml | 1 + src/RobotDynamics.jl | 1 + src/jacobian_gen.jl | 13 +++-- src/utils.jl | 6 +++ test/function_base_explicit_test.jl | 82 ++++++++++++++++++++++++++++ test/function_base_test.jl | 83 ++--------------------------- 7 files changed, 106 insertions(+), 83 deletions(-) create mode 100644 .vscode/settings.json create mode 100644 test/function_base_explicit_test.jl diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..6b873e5 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,3 @@ +{ + "julia.environmentPath": "/home/brian/.julia/dev/RobotDynamics" +} \ No newline at end of file diff --git a/Project.toml b/Project.toml index 0ab53de..647cffe 100644 --- a/Project.toml +++ b/Project.toml @@ -7,6 +7,7 @@ version = "0.4.6" 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 index b450f55..d08e6d9 100644 --- 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 index 0fbf6f4..ef3c3e9 100644 --- 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 index 94e214a..72cc385 100644 --- 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 \ No newline at end of file diff --git a/test/function_base_explicit_test.jl b/test/function_base_explicit_test.jl new file mode 100644 index 0000000..cadc121 --- /dev/null +++ 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) \ No newline at end of file diff --git a/test/function_base_test.jl b/test/function_base_test.jl index 03c36e7..860b856 100644 --- 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 ############################## From 40a447607a0719384ba02fb81b778512b7087c26 Mon Sep 17 00:00:00 2001 From: Brian Jackson Date: Thu, 4 Aug 2022 08:56:08 -0400 Subject: [PATCH 2/2] Up minor version --- Project.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Project.toml b/Project.toml index 647cffe..8f8ac80 100644 --- a/Project.toml +++ b/Project.toml @@ -1,7 +1,7 @@ name = "RobotDynamics" uuid = "38ceca67-d8d3-44e8-9852-78a5596522e1" authors = ["Brian Jackson "] -version = "0.4.6" +version = "0.4.7" [deps] FiniteDiff = "6a86dc24-6348-571c-b903-95158fe2bd41"