diff --git a/Project.toml b/Project.toml
index 96f1ad6dc..f7a43228b 100644
--- a/Project.toml
+++ b/Project.toml
@@ -30,18 +30,20 @@ Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"
 
 [compat]
 Colors = "<0.12.8, 0.12.8"
+CoordinateTransformations = "<0.6.2, 0.6.2"
 DocStringExtensions = "<0.8.6, 0.8.6"
-FFMPEG = "0.4.1"
-GeometryBasics = "<0.4.0, 0.4.0"
+FFMPEG = "0.4.0, 0.4.1"
+FiniteDiff = "2.0, 2.11"
+GeometryBasics = "<0.4.2, 0.4.2"
 Graphs = "1.5, 1.6"
 JLD2 = "<0.4.22, 0.4.22"
-LaTeXStrings = "1.3"
+LaTeXStrings = "1.3, 1.3"
 LightXML = "<0.9.0, 0.9.0"
-MeshCat = "<0.14.0, 0.14.0"
+MeshCat = "<0.14.2, 0.14.2"
 Meshing = "<0.5.7, 0.5.7"
-Parameters = "<0.12.0, 0.12.0"
-Polyhedra = "<0.7.1, 0.7.1"
-Quaternions = "0.5.2"
-Scratch = "1.1"
-StaticArrays = "1.4"
-julia = "1.7"
+Parameters = "<0.12.3, 0.12.3"
+Polyhedra = "<0.7.3, 0.7.3"
+Quaternions = "0.5.2, 0.5.3"
+Scratch = "1.1, 1.1"
+StaticArrays = "1.2, 1.4"
+julia = "1.6, 1.7"
diff --git a/src/integrators/integrator.jl b/src/integrators/integrator.jl
index 70400b528..581b0c3fc 100644
--- a/src/integrators/integrator.jl
+++ b/src/integrators/integrator.jl
@@ -11,8 +11,8 @@ current_configuration_velocity(state::State) = (current_position(state), state.v
 initial_configuration_velocity(state::State) = (current_position(state), state.v15, current_orientation(state), state.ϕ15)
 
 # next
-next_position(x2::SVector{3,T}, v25::SVector{3,T}, timestep::T) where T = x2 + v25 * timestep
-next_orientation(q2::Quaternion{T}, ϕ25::SVector{3,T}, timestep::T) where T = q2 * quaternion_map(ϕ25, timestep) * timestep / 2
+next_position(x2::SVector{3}, v25::SVector{3}, timestep::Real) = x2 + v25 * timestep
+next_orientation(q2::Quaternion, ϕ25::SVector{3}, timestep::Real) = q2 * quaternion_map(ϕ25, timestep) * timestep / 2
 next_position(state::State, timestep) = next_position(state.x2, state.vsol[2], timestep)
 next_orientation(state::State, timestep) = next_orientation(state.q2, state.ϕsol[2], timestep)
 next_configuration(state::State, timestep) = (next_position(state, timestep), next_orientation(state, timestep))
@@ -32,13 +32,13 @@ function ∂angular_velocity∂q2(q1::Quaternion, q2::Quaternion, timestep)
 end
 
 # Jacobians
-function integrator_jacobian_velocity(x2::AbstractVector{T}, v25::AbstractVector{T}, q2::Quaternion{T}, ϕ25::SVector{3,T}, timestep::T) where T
+function integrator_jacobian_velocity(x2::AbstractVector, v25::AbstractVector, q2::Quaternion, ϕ25::SVector{3}, timestep::T) where T
     V = [linear_integrator_jacobian_velocity(x2, v25, timestep) szeros(T,3,3)]
     Ω = [szeros(T,4,3) rotational_integrator_jacobian_velocity(q2, ϕ25, timestep)]
     return [V; Ω] # 7x6
 end
 
-function integrator_jacobian_configuration(x2::AbstractVector{T}, v25::AbstractVector{T}, q2::Quaternion{T}, ϕ25::SVector{3,T}, timestep::T; 
+function integrator_jacobian_configuration(x2::AbstractVector, v25::AbstractVector, q2::Quaternion, ϕ25::SVector{3}, timestep::T; 
     attjac::Bool=true) where T
 
     Z = attjac ? szeros(T,3,3) : szeros(T,3,4)
@@ -47,22 +47,22 @@ function integrator_jacobian_configuration(x2::AbstractVector{T}, v25::AbstractV
     return [X; Q] # 7x6 or 7x7
 end
 
-function linear_integrator_jacobian_position(x2::AbstractVector{T}, v2::AbstractVector{T}, timestep::T) where T
+function linear_integrator_jacobian_position(x2::AbstractVector, v2::AbstractVector, timestep::T) where T
     return SMatrix{3,3,T,9}(Diagonal(sones(T, 3)))
 end
 
-function linear_integrator_jacobian_velocity(x2::AbstractVector{T}, v2::AbstractVector{T}, timestep::T) where T
+function linear_integrator_jacobian_velocity(x2::AbstractVector, v2::AbstractVector, timestep::T) where T
     return timestep * SMatrix{3,3,T,9}(Diagonal(sones(T,3)))
 end
 
-function rotational_integrator_jacobian_orientation(q2::Quaternion{T}, ϕ25::SVector{3,T}, timestep::T; 
-    attjac::Bool = true) where T
+function rotational_integrator_jacobian_orientation(q2::Quaternion, ϕ25::SVector{3}, timestep::Real; 
+    attjac::Bool = true)
     M = Rmat(quaternion_map(ϕ25, timestep) * timestep / 2)
     attjac && (M *= LVᵀmat(q2))
     return M
 end
 
-function rotational_integrator_jacobian_velocity(q2::Quaternion{T}, ϕ25::SVector{3,T}, timestep::T) where T
+function rotational_integrator_jacobian_velocity(q2::Quaternion, ϕ25::SVector{3}, timestep::Real)
     return Lmat(q2) * quaternion_map_jacobian(ϕ25, timestep) * timestep / 2
 end
 
diff --git a/src/joints/rotational/minimal.jl b/src/joints/rotational/minimal.jl
index 0502ea56a..eadd24fbb 100644
--- a/src/joints/rotational/minimal.jl
+++ b/src/joints/rotational/minimal.jl
@@ -10,9 +10,9 @@ function displacement(joint::Rotational,
     vmat ? (return Vmat(q)) : (return q)
 end
 
-function displacement_jacobian_configuration(relative::Symbol, joint::Rotational,
-        xa::AbstractVector{T}, qa::Quaternion,
-        xb::AbstractVector{T}, qb::Quaternion;
+function displacement_jacobian_configuration(relative::Symbol, joint::Rotational{T},
+        xa::AbstractVector, qa::Quaternion,
+        xb::AbstractVector, qb::Quaternion;
         attjac::Bool=true, vmat=true) where T
     X = szeros(T, 3, 3)
     if relative == :parent
diff --git a/src/mechanism/state.jl b/src/mechanism/state.jl
index 17fd474c2..198381559 100644
--- a/src/mechanism/state.jl
+++ b/src/mechanism/state.jl
@@ -6,7 +6,7 @@
 	mechanism: Mechanism 
 	y: minimal state
 """
-function minimal_to_maximal(mechanism::Mechanism{T,Nn,Ne,Nb,Ni}, y::AbstractVector{Tx}) where {T,Nn,Ne,Nb,Ni,Tx}
+function minimal_to_maximal(mechanism::Mechanism{T,Nn,Ne,Nb,Ni}, y::AbstractVector) where {T,Nn,Ne,Nb,Ni}
 	off = 0
 	for id in mechanism.root_to_leaves
 		(id > Ne) && continue # only treat joints
diff --git a/test/Manifest.toml b/test/Manifest.toml
deleted file mode 100644
index 69cd3c6e8..000000000
--- a/test/Manifest.toml
+++ /dev/null
@@ -1,317 +0,0 @@
-# This file is machine-generated - editing it directly is not advised
-
-julia_version = "1.7.2"
-manifest_format = "2.0"
-
-[[deps.ArgTools]]
-uuid = "0dad84c5-d112-42e6-8d28-ef12dabb789f"
-
-[[deps.ArrayInterface]]
-deps = ["Compat", "IfElse", "LinearAlgebra", "Requires", "SparseArrays", "Static"]
-git-tree-sha1 = "6e8fada11bb015ecf9263f64b156f98b546918c7"
-uuid = "4fba245c-0d91-5ea0-9b3e-6abc04ee57a9"
-version = "5.0.5"
-
-[[deps.Artifacts]]
-uuid = "56f22d72-fd6d-98f1-02f0-08ddc0907c33"
-
-[[deps.Base64]]
-uuid = "2a0f44e3-6c83-55bd-87e4-b1978d98bd5f"
-
-[[deps.BenchmarkTools]]
-deps = ["JSON", "Logging", "Printf", "Profile", "Statistics", "UUIDs"]
-git-tree-sha1 = "4c10eee4af024676200bc7752e536f858c6b8f93"
-uuid = "6e4b80f9-dd63-53aa-95a3-0cdb28fa8baf"
-version = "1.3.1"
-
-[[deps.Calculus]]
-deps = ["LinearAlgebra"]
-git-tree-sha1 = "f641eb0a4f00c343bbc32346e1217b86f3ce9dad"
-uuid = "49dc2e85-a5d0-5ad3-a950-438e2897f1b9"
-version = "0.5.1"
-
-[[deps.ChainRulesCore]]
-deps = ["Compat", "LinearAlgebra", "SparseArrays"]
-git-tree-sha1 = "c9a6160317d1abe9c44b3beb367fd448117679ca"
-uuid = "d360d2e6-b24c-11e9-a2a3-2a2ae2dbcce4"
-version = "1.13.0"
-
-[[deps.ChangesOfVariables]]
-deps = ["ChainRulesCore", "LinearAlgebra", "Test"]
-git-tree-sha1 = "bf98fa45a0a4cee295de98d4c1462be26345b9a1"
-uuid = "9e997f8a-9a97-42d5-a9f1-ce6bfc15e2c0"
-version = "0.1.2"
-
-[[deps.Compat]]
-deps = ["Base64", "Dates", "DelimitedFiles", "Distributed", "InteractiveUtils", "LibGit2", "Libdl", "LinearAlgebra", "Markdown", "Mmap", "Pkg", "Printf", "REPL", "Random", "SHA", "Serialization", "SharedArrays", "Sockets", "SparseArrays", "Statistics", "Test", "UUIDs", "Unicode"]
-git-tree-sha1 = "96b0bc6c52df76506efc8a441c6cf1adcb1babc4"
-uuid = "34da2185-b29b-5c13-b0c7-acf172513d20"
-version = "3.42.0"
-
-[[deps.CompilerSupportLibraries_jll]]
-deps = ["Artifacts", "Libdl"]
-uuid = "e66e0078-7015-5450-92f7-15fbd957f2ae"
-
-[[deps.Dates]]
-deps = ["Printf"]
-uuid = "ade2ca70-3891-5945-98fb-dc099432e06a"
-
-[[deps.DelimitedFiles]]
-deps = ["Mmap"]
-uuid = "8bb1440f-4735-579b-a4ab-409b98df4dab"
-
-[[deps.Distributed]]
-deps = ["Random", "Serialization", "Sockets"]
-uuid = "8ba89e20-285c-5b6f-9357-94700520ee1b"
-
-[[deps.DocStringExtensions]]
-deps = ["LibGit2"]
-git-tree-sha1 = "b19534d1895d702889b219c382a6e18010797f0b"
-uuid = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae"
-version = "0.8.6"
-
-[[deps.Downloads]]
-deps = ["ArgTools", "LibCURL", "NetworkOptions"]
-uuid = "f43a241f-c20a-4ad4-852c-f6b1247861c6"
-
-[[deps.DualNumbers]]
-deps = ["Calculus", "NaNMath", "SpecialFunctions"]
-git-tree-sha1 = "90b158083179a6ccbce2c7eb1446d5bf9d7ae571"
-uuid = "fa6b7ba4-c1ee-5f82-b5fc-ecf0adba8f74"
-version = "0.6.7"
-
-[[deps.FiniteDiff]]
-deps = ["ArrayInterface", "LinearAlgebra", "Requires", "SparseArrays", "StaticArrays"]
-git-tree-sha1 = "56956d1e4c1221000b7781104c58c34019792951"
-uuid = "6a86dc24-6348-571c-b903-95158fe2bd41"
-version = "2.11.0"
-
-[[deps.IfElse]]
-git-tree-sha1 = "debdd00ffef04665ccbb3e150747a77560e8fad1"
-uuid = "615f187c-cbe4-4ef1-ba3b-2fcf58d6d173"
-version = "0.1.1"
-
-[[deps.InteractiveUtils]]
-deps = ["Markdown"]
-uuid = "b77e0a4c-d291-57a0-90e8-8db25a27a240"
-
-[[deps.InverseFunctions]]
-deps = ["Test"]
-git-tree-sha1 = "91b5dcf362c5add98049e6c29ee756910b03051d"
-uuid = "3587e190-3f89-42d0-90ee-14403ec27112"
-version = "0.1.3"
-
-[[deps.IrrationalConstants]]
-git-tree-sha1 = "7fd44fd4ff43fc60815f8e764c0f352b83c49151"
-uuid = "92d709cd-6900-40b7-9082-c6be49f344b6"
-version = "0.1.1"
-
-[[deps.JLLWrappers]]
-deps = ["Preferences"]
-git-tree-sha1 = "abc9885a7ca2052a736a600f7fa66209f96506e1"
-uuid = "692b3bcd-3c85-4b1f-b108-f13ce0eb3210"
-version = "1.4.1"
-
-[[deps.JSON]]
-deps = ["Dates", "Mmap", "Parsers", "Unicode"]
-git-tree-sha1 = "3c837543ddb02250ef42f4738347454f95079d4e"
-uuid = "682c06a0-de6a-54ab-a142-c8b1cf79cde6"
-version = "0.21.3"
-
-[[deps.LibCURL]]
-deps = ["LibCURL_jll", "MozillaCACerts_jll"]
-uuid = "b27032c2-a3e7-50c8-80cd-2d36dbcbfd21"
-
-[[deps.LibCURL_jll]]
-deps = ["Artifacts", "LibSSH2_jll", "Libdl", "MbedTLS_jll", "Zlib_jll", "nghttp2_jll"]
-uuid = "deac9b47-8bc7-5906-a0fe-35ac56dc84c0"
-
-[[deps.LibGit2]]
-deps = ["Base64", "NetworkOptions", "Printf", "SHA"]
-uuid = "76f85450-5226-5b5a-8eaa-529ad045b433"
-
-[[deps.LibSSH2_jll]]
-deps = ["Artifacts", "Libdl", "MbedTLS_jll"]
-uuid = "29816b5a-b9ab-546f-933c-edad1886dfa8"
-
-[[deps.Libdl]]
-uuid = "8f399da3-3557-5675-b5ff-fb832c97cbdb"
-
-[[deps.LinearAlgebra]]
-deps = ["Libdl", "libblastrampoline_jll"]
-uuid = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
-
-[[deps.LogExpFunctions]]
-deps = ["ChainRulesCore", "ChangesOfVariables", "DocStringExtensions", "InverseFunctions", "IrrationalConstants", "LinearAlgebra"]
-git-tree-sha1 = "58f25e56b706f95125dcb796f39e1fb01d913a71"
-uuid = "2ab3a3ac-af41-5b50-aa03-7779005ae688"
-version = "0.3.10"
-
-[[deps.Logging]]
-uuid = "56ddb016-857b-54e1-b83d-db4d58db5568"
-
-[[deps.Markdown]]
-deps = ["Base64"]
-uuid = "d6f4376e-aef5-505a-96c1-9c027394607a"
-
-[[deps.MbedTLS_jll]]
-deps = ["Artifacts", "Libdl"]
-uuid = "c8ffd9c3-330d-5841-b78e-0817d7145fa1"
-
-[[deps.Mmap]]
-uuid = "a63ad114-7e13-5084-954f-fe012c677804"
-
-[[deps.MozillaCACerts_jll]]
-uuid = "14a3606d-f60d-562e-9121-12d972cd8159"
-
-[[deps.NaNMath]]
-git-tree-sha1 = "737a5957f387b17e74d4ad2f440eb330b39a62c5"
-uuid = "77ba4419-2d1f-58cd-9bb1-8ffee604a2e3"
-version = "1.0.0"
-
-[[deps.NetworkOptions]]
-uuid = "ca575930-c2e3-43a9-ace4-1e988b2c1908"
-
-[[deps.OpenBLAS_jll]]
-deps = ["Artifacts", "CompilerSupportLibraries_jll", "Libdl"]
-uuid = "4536629a-c528-5b80-bd46-f80d51c5b363"
-
-[[deps.OpenLibm_jll]]
-deps = ["Artifacts", "Libdl"]
-uuid = "05823500-19ac-5b8b-9628-191a04bc5112"
-
-[[deps.OpenSpecFun_jll]]
-deps = ["Artifacts", "CompilerSupportLibraries_jll", "JLLWrappers", "Libdl", "Pkg"]
-git-tree-sha1 = "13652491f6856acfd2db29360e1bbcd4565d04f1"
-uuid = "efe28fd5-8261-553b-a9e1-b2916fc3738e"
-version = "0.5.5+0"
-
-[[deps.Parsers]]
-deps = ["Dates"]
-git-tree-sha1 = "85b5da0fa43588c75bb1ff986493443f821c70b7"
-uuid = "69de0a69-1ddd-5017-9359-2bf0b02dc9f0"
-version = "2.2.3"
-
-[[deps.Pkg]]
-deps = ["Artifacts", "Dates", "Downloads", "LibGit2", "Libdl", "Logging", "Markdown", "Printf", "REPL", "Random", "SHA", "Serialization", "TOML", "Tar", "UUIDs", "p7zip_jll"]
-uuid = "44cfe95a-1eb2-52ea-b672-e2afdf69b78f"
-
-[[deps.Preferences]]
-deps = ["TOML"]
-git-tree-sha1 = "d3538e7f8a790dc8903519090857ef8e1283eecd"
-uuid = "21216c6a-2e73-6563-6e65-726566657250"
-version = "1.2.5"
-
-[[deps.Printf]]
-deps = ["Unicode"]
-uuid = "de0858da-6303-5e67-8744-51eddeeeb8d7"
-
-[[deps.Profile]]
-deps = ["Printf"]
-uuid = "9abbd945-dff8-562f-b5e8-e1ebf5ef1b79"
-
-[[deps.Quaternions]]
-deps = ["DualNumbers", "LinearAlgebra", "Random"]
-git-tree-sha1 = "522770af103809e8346aefa4b25c31fbec377ccf"
-uuid = "94ee1d12-ae83-5a48-8b1c-48b8ff168ae0"
-version = "0.5.3"
-
-[[deps.REPL]]
-deps = ["InteractiveUtils", "Markdown", "Sockets", "Unicode"]
-uuid = "3fa0cd96-eef1-5676-8a61-b3b8758bbffb"
-
-[[deps.Random]]
-deps = ["SHA", "Serialization"]
-uuid = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c"
-
-[[deps.Requires]]
-deps = ["UUIDs"]
-git-tree-sha1 = "838a3a4188e2ded87a4f9f184b4b0d78a1e91cb7"
-uuid = "ae029012-a4dd-5104-9daa-d747884805df"
-version = "1.3.0"
-
-[[deps.Rotations]]
-deps = ["LinearAlgebra", "Quaternions", "Random", "StaticArrays", "Statistics"]
-git-tree-sha1 = "a167638e2cbd8ac41f9cd57282cab9b042fa26e6"
-uuid = "6038ab10-8711-5258-84ad-4b1120ba62dc"
-version = "1.3.0"
-
-[[deps.SHA]]
-uuid = "ea8e919c-243c-51af-8825-aaa63cd721ce"
-
-[[deps.SafeTestsets]]
-deps = ["Test"]
-git-tree-sha1 = "36ebc5622c82eb9324005cc75e7e2cc51181d181"
-uuid = "1bc83da4-3b8d-516f-aca4-4fe02f6d838f"
-version = "0.0.1"
-
-[[deps.Serialization]]
-uuid = "9e88b42a-f829-5b0c-bbe9-9e923198166b"
-
-[[deps.SharedArrays]]
-deps = ["Distributed", "Mmap", "Random", "Serialization"]
-uuid = "1a1011a3-84de-559e-8e89-a11a2f7dc383"
-
-[[deps.Sockets]]
-uuid = "6462fe0b-24de-5631-8697-dd941f90decc"
-
-[[deps.SparseArrays]]
-deps = ["LinearAlgebra", "Random"]
-uuid = "2f01184e-e22b-5df5-ae63-d93ebab69eaf"
-
-[[deps.SpecialFunctions]]
-deps = ["ChainRulesCore", "IrrationalConstants", "LogExpFunctions", "OpenLibm_jll", "OpenSpecFun_jll"]
-git-tree-sha1 = "5ba658aeecaaf96923dce0da9e703bd1fe7666f9"
-uuid = "276daf66-3868-5448-9aa4-cd146d93841b"
-version = "2.1.4"
-
-[[deps.Static]]
-deps = ["IfElse"]
-git-tree-sha1 = "87e9954dfa33fd145694e42337bdd3d5b07021a6"
-uuid = "aedffcd0-7271-4cad-89d0-dc628f76c6d3"
-version = "0.6.0"
-
-[[deps.StaticArrays]]
-deps = ["LinearAlgebra", "Random", "Statistics"]
-git-tree-sha1 = "74fb527333e72ada2dd9ef77d98e4991fb185f04"
-uuid = "90137ffa-7385-5640-81b9-e52037218182"
-version = "1.4.1"
-
-[[deps.Statistics]]
-deps = ["LinearAlgebra", "SparseArrays"]
-uuid = "10745b16-79ce-11e8-11f9-7d13ad32a3b2"
-
-[[deps.TOML]]
-deps = ["Dates"]
-uuid = "fa267f1f-6049-4f14-aa54-33bafae1ed76"
-
-[[deps.Tar]]
-deps = ["ArgTools", "SHA"]
-uuid = "a4e569a6-e804-4fa4-b0f3-eef7a1d5b13e"
-
-[[deps.Test]]
-deps = ["InteractiveUtils", "Logging", "Random", "Serialization"]
-uuid = "8dfed614-e22c-5e08-85e1-65c5234f0b40"
-
-[[deps.UUIDs]]
-deps = ["Random", "SHA"]
-uuid = "cf7118a7-6976-5b1a-9a39-7adc72f591a4"
-
-[[deps.Unicode]]
-uuid = "4ec0a83e-493e-50e2-b9ac-8f72acf5a8f5"
-
-[[deps.Zlib_jll]]
-deps = ["Libdl"]
-uuid = "83775a58-1f1d-513f-b197-d71354ab007a"
-
-[[deps.libblastrampoline_jll]]
-deps = ["Artifacts", "Libdl", "OpenBLAS_jll"]
-uuid = "8e850b90-86db-534c-a0d3-1478176c7d93"
-
-[[deps.nghttp2_jll]]
-deps = ["Artifacts", "Libdl"]
-uuid = "8e850ede-7688-5339-a07c-302acd2aaf8d"
-
-[[deps.p7zip_jll]]
-deps = ["Artifacts", "Libdl"]
-uuid = "3f19e933-33d8-53b3-aaab-bd5110c3b7a0"
diff --git a/test/Project.toml b/test/Project.toml
index 3234f34aa..823ae93b4 100644
--- a/test/Project.toml
+++ b/test/Project.toml
@@ -1,6 +1,7 @@
 [deps]
 BenchmarkTools = "6e4b80f9-dd63-53aa-95a3-0cdb28fa8baf"
 FiniteDiff = "6a86dc24-6348-571c-b903-95158fe2bd41"
+ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210"
 LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
 Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c"
 Rotations = "6038ab10-8711-5258-84ad-4b1120ba62dc"
@@ -10,8 +11,9 @@ StaticArrays = "90137ffa-7385-5640-81b9-e52037218182"
 Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"
 
 [compat]
-BenchmarkTools = "1.2.0"
-FiniteDiff = "2.8.1"
-Rotations = "1.3.0"
-SafeTestsets = "0.0.1"
-StaticArrays = "0.8.0 - 1.4.1"
+BenchmarkTools = "<1.3.1, 1.3.1"
+FiniteDiff = "2.0, 2.11"
+ForwardDiff = "0.10.19, 0.10.25"
+Rotations = "1.3, 1.3"
+SafeTestsets = "0.0.1, 0.0.1"
+StaticArrays = "1.2, 1.4"
\ No newline at end of file
diff --git a/test/collisions.jl b/test/collisions.jl
index 88bd3019c..11b24bdd6 100644
--- a/test/collisions.jl
+++ b/test/collisions.jl
@@ -58,8 +58,7 @@
         return mechanism
     end
 
-    function test_jacobians(mechanism;
-        tolerance=1.0e-5)
+    function test_jacobians(mechanism)
 
         # unpack
         collision = mechanism.contacts[1].model.collision
@@ -74,57 +73,57 @@
 
             X = Dojo.∂contact_normal_transpose∂x(jacobian, collision, xp, qp, xc, qc)
             if jacobian == :parent
-                FD = FiniteDiff.finite_difference_jacobian(x -> Dojo.contact_normal(collision, x, qp, xc, qc)', xp)
+                FD = ForwardDiff.jacobian(x -> Dojo.contact_normal(collision, x, qp, xc, qc)', xp)
             elseif jacobian == :child
-                FD = FiniteDiff.finite_difference_jacobian(x -> Dojo.contact_normal(collision, xp, qp, x, qc)', xc)
+                FD = ForwardDiff.jacobian(x -> Dojo.contact_normal(collision, xp, qp, x, qc)', xc)
             end
 
-            @test norm((dis >= 0.0 ? 1.0 : -1.0) * X - FD, Inf) < tolerance
+            @test norm((dis >= 0.0 ? 1.0 : -1.0) * X - FD, Inf) < 1.0e-8
 
             Q = Dojo.∂contact_normal_transpose∂q(jacobian, collision, xp, qp, xc, qc)
             if jacobian == :parent
-                FD = FiniteDiff.finite_difference_jacobian(q -> Dojo.contact_normal(collision, xp, Quaternion(q..., false), xc, qc)', Dojo.vector(qp))
+                FD = ForwardDiff.jacobian(q -> Dojo.contact_normal(collision, xp, Quaternion(q..., false), xc, qc)', Dojo.vector(qp))
             elseif jacobian == :child
-                FD = FiniteDiff.finite_difference_jacobian(q -> Dojo.contact_normal(collision, xp, qp, xc, Quaternion(q..., false))', Dojo.vector(qc))
+                FD = ForwardDiff.jacobian(q -> Dojo.contact_normal(collision, xp, qp, xc, Quaternion(q..., false))', Dojo.vector(qc))
             end
 
-            @test norm((dis >= 0.0 ? 1.0 : -1.0) * Q - FD, Inf) < tolerance
+            @test norm((dis >= 0.0 ? 1.0 : -1.0) * Q - FD, Inf) < 1.0e-8
 
             X = Dojo.∂contact_tangent_one_transpose∂x(jacobian, collision, xp, qp, xc, qc)
             if jacobian == :parent
-                FD = FiniteDiff.finite_difference_jacobian(x -> Dojo.contact_tangent(collision, x, qp, xc, qc)[1, :]', xp)
+                FD = ForwardDiff.jacobian(x -> Dojo.contact_tangent(collision, x, qp, xc, qc)[1, :]', xp)
             elseif jacobian == :child
-                FD = FiniteDiff.finite_difference_jacobian(x -> Dojo.contact_tangent(collision, xp, qp, x, qc)[1, :]', xc)
+                FD = ForwardDiff.jacobian(x -> Dojo.contact_tangent(collision, xp, qp, x, qc)[1, :]', xc)
             end
 
-            @test norm(FD - X, Inf) < tolerance
+            @test norm(FD - X, Inf) < 1.0e-8
 
             X = Dojo.∂contact_tangent_two_transpose∂x(jacobian, collision, xp, qp, xc, qc)
             if jacobian == :parent
-                FD = FiniteDiff.finite_difference_jacobian(x -> Dojo.contact_tangent(collision, x, qp, xc, qc)[2, :]', xp)
+                FD = ForwardDiff.jacobian(x -> Dojo.contact_tangent(collision, x, qp, xc, qc)[2, :]', xp)
             elseif jacobian == :child
-                FD = FiniteDiff.finite_difference_jacobian(x -> Dojo.contact_tangent(collision, xp, qp, x, qc)[2, :]', xc)
+                FD = ForwardDiff.jacobian(x -> Dojo.contact_tangent(collision, xp, qp, x, qc)[2, :]', xc)
             end
 
-            @test norm(FD - X, Inf) < tolerance
+            @test norm(FD - X, Inf) < 1.0e-8
 
             Q = Dojo.∂contact_tangent_one_transpose∂q(jacobian, collision, xp, qp, xc, qc)
             if jacobian == :parent
-                FD = FiniteDiff.finite_difference_jacobian(q -> Dojo.contact_tangent(collision, xp, Quaternion(q..., false), xc, qc)[1, :]', Dojo.vector(qp))
+                FD = ForwardDiff.jacobian(q -> Dojo.contact_tangent(collision, xp, Quaternion(q..., false), xc, qc)[1, :]', Dojo.vector(qp))
             elseif jacobian == :child
-                FD = FiniteDiff.finite_difference_jacobian(q -> Dojo.contact_tangent(collision, xp, qp, xc, Quaternion(q..., false))[1, :]', Dojo.vector(qc))
+                FD = ForwardDiff.jacobian(q -> Dojo.contact_tangent(collision, xp, qp, xc, Quaternion(q..., false))[1, :]', Dojo.vector(qc))
             end
 
-            @test norm(FD - Q, Inf) < tolerance
+            @test norm(FD - Q, Inf) < 1.0e-8
 
             Q = Dojo.∂contact_tangent_two_transpose∂q(jacobian, collision, xp, qp, xc, qc)
             if jacobian == :parent
-                FD = FiniteDiff.finite_difference_jacobian(q -> Dojo.contact_tangent(collision, xp, Quaternion(q..., false), xc, qc)[2, :]', Dojo.vector(qp))
+                FD = ForwardDiff.jacobian(q -> Dojo.contact_tangent(collision, xp, Quaternion(q..., false), xc, qc)[2, :]', Dojo.vector(qp))
             elseif jacobian == :child
-                FD = FiniteDiff.finite_difference_jacobian(q -> Dojo.contact_tangent(collision, xp, qp, xc, Quaternion(q..., false))[2, :]', Dojo.vector(qc))
+                FD = ForwardDiff.jacobian(q -> Dojo.contact_tangent(collision, xp, qp, xc, Quaternion(q..., false))[2, :]', Dojo.vector(qc))
             end
 
-            @test norm(FD - Q, Inf) < tolerance
+            @test norm(FD - Q, Inf) < 1.0e-8
 
             # gradients
             gradient = jacobian
@@ -136,7 +135,7 @@
                 FD = FiniteDiff.finite_difference_jacobian(x -> Dojo.distance(collision, xp, qp, x, qc), xc)
             end
 
-            @test norm(D - FD, Inf) < tolerance
+            @test norm(D - FD, Inf) < 1.0e-5
 
             Q = Dojo.∂distance∂q(gradient, collision, xp, qp, xc, qc)
             if gradient == :parent
@@ -145,28 +144,28 @@
                 FD = FiniteDiff.finite_difference_jacobian(q -> Dojo.distance(collision, xp, qp, xc, Quaternion(q..., false)), Dojo.vector(qc))
             end
 
-            @test norm(Q - FD, Inf) < tolerance
+            @test norm(Q - FD, Inf) < 1.0e-5
 
             for relative in [:parent, :child]
                 X = Dojo.∂contact_point∂x(relative, jacobian, collision, xp, qp, xc, qc)
 
                 if jacobian == :parent
-                    FD =  FiniteDiff.finite_difference_jacobian(x -> Dojo.contact_point(relative, collision, x, qp, xc, qc), xp)
+                    FD =  ForwardDiff.jacobian(x -> Dojo.contact_point(relative, collision, x, qp, xc, qc), xp)
                 elseif jacobian == :child
-                    FD = FiniteDiff.finite_difference_jacobian(x -> Dojo.contact_point(relative, collision, xp, qp, x, qc), xc)
+                    FD = ForwardDiff.jacobian(x -> Dojo.contact_point(relative, collision, xp, qp, x, qc), xc)
                 end
 
-                @test norm(X - FD, Inf) < tolerance
+                @test norm(X - FD, Inf) < 1.0e-8
 
                 Q = Dojo.∂contact_point∂q(relative, jacobian, collision, xp, qp, xc, qc)
 
                 if jacobian == :parent
-                    FD = FiniteDiff.finite_difference_jacobian(q -> Dojo.contact_point(relative, collision, xp, Quaternion(q..., false), xc, qc), Dojo.vector(qp))
+                    FD = ForwardDiff.jacobian(q -> Dojo.contact_point(relative, collision, xp, Quaternion(q..., false), xc, qc), Dojo.vector(qp))
                 elseif jacobian == :child
-                    FD = FiniteDiff.finite_difference_jacobian(q -> Dojo.contact_point(relative, collision, xp, qp, xc, Quaternion(q..., false)), Dojo.vector(qc))
+                    FD = ForwardDiff.jacobian(q -> Dojo.contact_point(relative, collision, xp, qp, xc, Quaternion(q..., false)), Dojo.vector(qc))
                 end
 
-                @test norm(Q - FD, Inf) < tolerance
+                @test norm(Q - FD, Inf) < 1.0e-8
             end
         end
     end
@@ -229,7 +228,7 @@
         test_jacobians(mech)
 
         # test no interpentration
-        @test norm(storage.x[2][end] - [0.0; 0.0; 1.0], Inf) < 1.0e-3
+        @test norm(storage.x[2][end] - [0.0; 0.0; 1.0], Inf) < 1.0e-4
 
         ## z velocity (no gravity)
         mech = get_two_body(;
diff --git a/test/damper.jl b/test/damper.jl
index 777c8129d..edf78a64c 100644
--- a/test/damper.jl
+++ b/test/damper.jl
@@ -35,68 +35,68 @@
 
         # Configuration
         J0 = Dojo.damper_jacobian_configuration(:parent, :parent, rot0, pbody0, cbody0, timestep0)
-        J1 = FiniteDiff.finite_difference_jacobian(
+        J1 = ForwardDiff.jacobian(
             xq -> timestep0 * Dojo.damper_force(:parent, rot0,
                 Dojo.Quaternion(xq[4:7]...,true), ϕa0, qb0, ϕb0, timestep0; rotate=true, unitary=false),
             [xa0; Dojo.vector(qa0)]) * Dojo.cat(I(3), Dojo.LVᵀmat(qa0), dims=(1,2))
         norm(J0 - J1, Inf)
-        @test norm(J0 - J1, Inf) < 1e-6
+        @test norm(J0 - J1, Inf) < 1.0e-8
 
         J0 = Dojo.damper_jacobian_configuration(:parent, :child, rot0, pbody0, cbody0, timestep0)
-        J1 = FiniteDiff.finite_difference_jacobian(
+        J1 = ForwardDiff.jacobian(
             xq -> timestep0 * Dojo.damper_force(:parent, rot0, qa0, ϕa0,
                 Dojo.Quaternion(xq[4:7]...,true), ϕb0, timestep0; rotate=true, unitary=false),
             [xb0; Dojo.vector(qb0)]) * Dojo.cat(I(3), Dojo.LVᵀmat(qb0), dims=(1,2))
         norm(J0 - J1, Inf)
-        @test norm(J0 - J1, Inf) < 1e-6
+        @test norm(J0 - J1, Inf) < 1.0e-8
 
         J0 = Dojo.damper_jacobian_configuration(:child, :parent, rot0, pbody0, cbody0, timestep0)
-        J1 = FiniteDiff.finite_difference_jacobian(
+        J1 = ForwardDiff.jacobian(
             xq -> timestep0 * Dojo.damper_force(:child, rot0,
                 Dojo.Quaternion(xq[4:7]...,true), ϕa0, qb0, ϕb0, timestep0; rotate=true, unitary=false),
             [xa0; Dojo.vector(qa0)]) * Dojo.cat(I(3), Dojo.LVᵀmat(qa0), dims=(1,2))
         norm(J0 - J1, Inf)
-        @test norm(J0 - J1, Inf) < 1e-6
+        @test norm(J0 - J1, Inf) < 1.0e-8
 
         J0 = Dojo.damper_jacobian_configuration(:child, :child, rot0, pbody0, cbody0, timestep0)
-        J1 = FiniteDiff.finite_difference_jacobian(
+        J1 = ForwardDiff.jacobian(
             xq -> timestep0 * Dojo.damper_force(:child, rot0, qa0, ϕa0,
                 Dojo.Quaternion(xq[4:7]...,true), ϕb0, timestep0; rotate=true, unitary=false),
             [xb0; Dojo.vector(qb0)]) * Dojo.cat(I(3), Dojo.LVᵀmat(qb0), dims=(1,2))
         norm(J0 - J1, Inf)
-        @test norm(J0 - J1, Inf) < 1e-6
+        @test norm(J0 - J1, Inf) < 1.0e-8
 
         # Velocity
         J0 = Dojo.damper_jacobian_velocity(:parent, :parent, rot0, pbody0, cbody0, timestep0)
-        J1 = FiniteDiff.finite_difference_jacobian(
+        J1 = ForwardDiff.jacobian(
             vϕ -> timestep0 * Dojo.damper_force(:parent, rot0, qa0,
                 vϕ[Dojo.SUnitRange(4,6)], qb0, ϕb0, timestep0; rotate=true, unitary=false),
             [va0; ϕa0])
         norm(J0 - J1, Inf)
-        @test norm(J0 - J1, Inf) < 1e-6
+        @test norm(J0 - J1, Inf) < 1.0e-8
 
         J0 = Dojo.damper_jacobian_velocity(:parent, :child, rot0, pbody0, cbody0, timestep0)
-        J1 = FiniteDiff.finite_difference_jacobian(
+        J1 = ForwardDiff.jacobian(
             vϕ -> timestep0 * Dojo.damper_force(:parent, rot0, qa0, ϕa0, qb0,
                 vϕ[Dojo.SUnitRange(4,6)], timestep0; rotate=true, unitary=false),
             [vb0; ϕb0])
         norm(J0 - J1, Inf)
-        @test norm(J0 - J1, Inf) < 1e-6
+        @test norm(J0 - J1, Inf) < 1.0e-8
 
         J0 = Dojo.damper_jacobian_velocity(:child, :parent, rot0, pbody0, cbody0, timestep0)
-        J1 = FiniteDiff.finite_difference_jacobian(
+        J1 = ForwardDiff.jacobian(
             vϕ -> timestep0 * Dojo.damper_force(:child, rot0, qa0,
                 vϕ[Dojo.SUnitRange(4,6)], qb0, ϕb0, timestep0; rotate=true, unitary=false),
             [va0; ϕa0])
         norm(J0 - J1, Inf)
-        @test norm(J0 - J1, Inf) < 1e-6
+        @test norm(J0 - J1, Inf) < 1.0e-8
 
         J0 = Dojo.damper_jacobian_velocity(:child, :child, rot0, pbody0, cbody0, timestep0)
-        J1 = FiniteDiff.finite_difference_jacobian(
+        J1 = ForwardDiff.jacobian(
             vϕ -> timestep0 * Dojo.damper_force(:child, rot0, qa0, ϕa0, qb0,
                 vϕ[Dojo.SUnitRange(4,6)], timestep0; rotate=true, unitary=false),
             [vb0; ϕb0])
         norm(J0 - J1, Inf)
-        @test norm(J0 - J1, Inf) < 1e-6
+        @test norm(J0 - J1, Inf) < 1.0e-8
     end
 end
diff --git a/test/impulse_map.jl b/test/impulse_map.jl
index 5384c94b8..cff8a01a4 100644
--- a/test/impulse_map.jl
+++ b/test/impulse_map.jl
@@ -18,22 +18,22 @@
             attjac=true)
         J0 = [X0 Q0]
         attjac = cat(I(3),Dojo.LVᵀmat(qa), dims=(1,2))
-        J1 = FiniteDiff.finite_difference_jacobian(
+        J1 = ForwardDiff.jacobian(
             z -> Dojo.displacement(rot0, z[1:3], Quaternion(z[4:7]...), xb, qb),
             [xa; Dojo.vector(qa)]
             ) * attjac
-        @test norm(J0 - J1, Inf) < 1.0e-7
+        @test norm(J0 - J1, Inf) < 1.0e-8
 
         # displacement_jacobian_configuration
         X0, Q0 = Dojo.displacement_jacobian_configuration(:child, rot0, xa, qa, xb, qb, 
             attjac=true)
         J0 = [X0 Q0]
         attjac = cat(I(3),Dojo.LVᵀmat(qb), dims=(1,2))
-        J1 = FiniteDiff.finite_difference_jacobian(
+        J1 = ForwardDiff.jacobian(
             z -> Dojo.displacement(rot0, xa, qa, z[1:3], Quaternion(z[4:7]...)),
             [xb; Dojo.vector(qb)]
             ) * attjac
-        @test norm(J0 - J1, Inf) < 1.0e-7
+        @test norm(J0 - J1, Inf) < 1.0e-8
     end
 
     @testset "Translational" begin
@@ -51,22 +51,22 @@
             attjac=true)
         J0 = [X0 Q0]
         attjac = cat(I(3),Dojo.LVᵀmat(qa), dims=(1,2))
-        J1 = FiniteDiff.finite_difference_jacobian(
+        J1 = ForwardDiff.jacobian(
             z -> Dojo.displacement(tra0, z[1:3], Quaternion(z[4:7]...), xb, qb),
             [xa; Dojo.vector(qa)]
             ) * attjac
-        @test norm(J0 - J1, Inf) < 1.0e-7
+        @test norm(J0 - J1, Inf) < 1.0e-8
 
         # displacement_jacobian_configuration
         X0, Q0 = Dojo.displacement_jacobian_configuration(:child, tra0, xa, qa, xb, qb, 
             attjac=true)
         J0 = [X0 Q0]
         attjac = cat(I(3),Dojo.LVᵀmat(qb), dims=(1,2))
-        J1 = FiniteDiff.finite_difference_jacobian(
+        J1 = ForwardDiff.jacobian(
             z -> Dojo.displacement(tra0, xa, qa, z[1:3], Quaternion(z[4:7]...)),
             [xb; Dojo.vector(qb)]
             ) * attjac
-        @test norm(J0 - J1, Inf) < 1.0e-7
+        @test norm(J0 - J1, Inf) < 1.0e-8
     end
 end
 
@@ -88,35 +88,35 @@ end
         p0 = rand(3)
         J0 = Dojo.impulse_transform_jacobian(:parent, :parent, rot0, xa, qa, xb, qb, p0)
         attjac = cat(I(3),Dojo.LVᵀmat(qa), dims=(1,2))
-        J1 = FiniteDiff.finite_difference_jacobian(
+        J1 = ForwardDiff.jacobian(
             z -> Dojo.impulse_transform(:parent, rot0, z[1:3], Quaternion(z[4:7]...), xb, qb) * p0,
             [xa; Dojo.vector(qa)]
             ) * attjac
-        @test norm(J0 - J1, Inf) < 1.0e-7
+        @test norm(J0 - J1, Inf) < 1.0e-8
 
         J0 = Dojo.impulse_transform_jacobian(:parent, :child, rot0, xa, qa, xb, qb, p0)
         attjac = cat(I(3),Dojo.LVᵀmat(qb), dims=(1,2))
-        J1 = FiniteDiff.finite_difference_jacobian(
+        J1 = ForwardDiff.jacobian(
             z -> Dojo.impulse_transform(:parent, rot0, xa, qa, z[1:3], Quaternion(z[4:7]...)) * p0,
             [xb; Dojo.vector(qb)]
             ) * attjac
-        @test norm(J0 - J1, Inf) < 1.0e-7
+        @test norm(J0 - J1, Inf) < 1.0e-8
 
         J0 = Dojo.impulse_transform_jacobian(:child, :parent, rot0, xa, qa, xb, qb, p0)
         attjac = cat(I(3),Dojo.LVᵀmat(qa), dims=(1,2))
-        J1 = FiniteDiff.finite_difference_jacobian(
+        J1 = ForwardDiff.jacobian(
             z -> Dojo.impulse_transform(:child, rot0, z[1:3], Quaternion(z[4:7]...), xb, qb) * p0,
             [xa; Dojo.vector(qa)]
             ) * attjac
-        @test norm(J0 - J1, Inf) < 1.0e-7
+        @test norm(J0 - J1, Inf) < 1.0e-8
 
         J0 = Dojo.impulse_transform_jacobian(:child, :child, rot0, xa, qa, xb, qb, p0)
         attjac = cat(I(3),Dojo.LVᵀmat(qb), dims=(1,2))
-        J1 = FiniteDiff.finite_difference_jacobian(
+        J1 = ForwardDiff.jacobian(
             z -> Dojo.impulse_transform(:child, rot0, xa, qa, z[1:3], Quaternion(z[4:7]...)) * p0,
             [xb; Dojo.vector(qb)]
             ) * attjac
-        @test norm(J0 - J1, Inf) < 1.0e-7
+        @test norm(J0 - J1, Inf) < 1.0e-8
     end
 
     @testset "Translational" begin
@@ -132,35 +132,35 @@ end
         p0 = rand(3)
         J0 = Dojo.impulse_transform_jacobian(:parent, :parent, tra0, xa, qa, xb, qb, p0)
         attjac = cat(I(3),Dojo.LVᵀmat(qa), dims=(1,2))
-        J1 = FiniteDiff.finite_difference_jacobian(
+        J1 = ForwardDiff.jacobian(
             z -> Dojo.impulse_transform(:parent, tra0, z[1:3], Quaternion(z[4:7]...), xb, qb) * p0,
             [xa; Dojo.vector(qa)]
             ) * attjac
-        @test norm(J0 - J1, Inf) < 1.0e-7
+        @test norm(J0 - J1, Inf) < 1.0e-8
 
         J0 = Dojo.impulse_transform_jacobian(:parent, :child, tra0, xa, qa, xb, qb, p0)
         attjac = cat(I(3), Dojo.LVᵀmat(qb), dims=(1,2))
-        J1 = FiniteDiff.finite_difference_jacobian(
+        J1 = ForwardDiff.jacobian(
             z -> Dojo.impulse_transform(:parent, tra0, xa, qa, z[1:3], Quaternion(z[4:7]...)) * p0,
             [xb; Dojo.vector(qb)]
             ) * attjac
-        @test norm(J0 - J1, Inf) < 1.0e-7
+        @test norm(J0 - J1, Inf) < 1.0e-8
 
         J0 = Dojo.impulse_transform_jacobian(:child, :parent, tra0, xa, qa, xb, qb, p0)
         attjac = cat(I(3), Dojo.LVᵀmat(qa), dims=(1,2))
-        J1 = FiniteDiff.finite_difference_jacobian(
+        J1 = ForwardDiff.jacobian(
             z -> Dojo.impulse_transform(:child, tra0, z[1:3], Quaternion(z[4:7]...), xb, qb) * p0,
             [xa; Dojo.vector(qa)]
             ) * attjac
-        @test norm(J0 - J1, Inf) < 1.0e-7
+        @test norm(J0 - J1, Inf) < 1.0e-8
 
         J0 = Dojo.impulse_transform_jacobian(:child, :child, tra0, xa, qa, xb, qb, p0)
         attjac = cat(I(3),Dojo.LVᵀmat(qb), dims=(1,2))
-        J1 = FiniteDiff.finite_difference_jacobian(
+        J1 = ForwardDiff.jacobian(
             z -> Dojo.impulse_transform(:child, tra0, xa, qa, z[1:3], Quaternion(z[4:7]...)) * p0,
             [xb; Dojo.vector(qb)]
             ) * attjac
-        @test norm(J0 - J1, Inf) < 1.0e-7
+        @test norm(J0 - J1, Inf) < 1.0e-8
     end
 end
 
@@ -196,38 +196,38 @@ end
         # impulse_map_parent_jacobian_parent
         J0 = Dojo.impulse_map_jacobian(:parent, :parent, rot0, pbody, cbody, λ)
         attjac = cat(I(3),Dojo.LVᵀmat(qa), dims=(1,2))
-        J1 = FiniteDiff.finite_difference_jacobian(
+        J1 = ForwardDiff.jacobian(
             z -> Dojo.impulse_map(:parent, rot0, z[1:3], Quaternion(z[4:7]...), xb, qb, 0) * λ,
             [xa; Dojo.vector(qa)]
             ) * attjac
-        @test norm(J0 - J1, Inf) < 1.0e-7
+        @test norm(J0 - J1, Inf) < 1.0e-8
 
         # impulse_map_parent_jacobian_child
         J0 = Dojo.impulse_map_jacobian(:parent, :child, rot0, pbody, cbody, λ)
         attjac = cat(I(3), Dojo.LVᵀmat(qb), dims=(1,2))
-        J1 = FiniteDiff.finite_difference_jacobian(
+        J1 = ForwardDiff.jacobian(
             z -> Dojo.impulse_map(:parent, rot0, xa, qa, z[1:3], Quaternion(z[4:7]...), 0) * λ,
             [xb; Dojo.vector(qb)]
             ) * attjac
-        @test norm(J0 - J1, Inf) < 1.0e-7
+        @test norm(J0 - J1, Inf) < 1.0e-8
 
         # impulse_map_child_jacobian_parent
         J0 = Dojo.impulse_map_jacobian(:child, :parent, rot0, pbody, cbody, λ)
         attjac = cat(I(3), Dojo.LVᵀmat(qa), dims=(1,2))
-        J1 = FiniteDiff.finite_difference_jacobian(
+        J1 = ForwardDiff.jacobian(
             z -> Dojo.impulse_map(:child, rot0, z[1:3], Quaternion(z[4:7]...), xb, qb, 0) * λ,
             [xa; Dojo.vector(qa)]
             ) * attjac
-        @test norm(J0 - J1, Inf) < 1.0e-7
+        @test norm(J0 - J1, Inf) < 1.0e-8
 
         # impulse_map_child_jacobian_child
         J0 = Dojo.impulse_map_jacobian(:child, :child, rot0, pbody, cbody, λ)
         attjac = cat(I(3), Dojo.LVᵀmat(qb), dims=(1,2))
-        J1 = FiniteDiff.finite_difference_jacobian(
+        J1 = ForwardDiff.jacobian(
             z -> Dojo.impulse_map(:child, rot0, xa, qa, z[1:3], Quaternion(z[4:7]...), 0) * λ,
             [xb; Dojo.vector(qb)]
             ) * attjac
-        @test norm(J0 - J1, Inf) < 1.0e-7
+        @test norm(J0 - J1, Inf) < 1.0e-8
     end
 
     @testset "Translational" begin
@@ -256,37 +256,37 @@ end
         J0 = Dojo.impulse_map_jacobian(:parent, :parent, tra0, pbody, cbody, λ)
 
         attjac = cat(I(3),Dojo.LVᵀmat(qa), dims=(1,2))
-        J1 = FiniteDiff.finite_difference_jacobian(
+        J1 = ForwardDiff.jacobian(
             z -> Dojo.impulse_map(:parent, tra0, z[1:3], Quaternion(z[4:7]...), xb, qb, 0) * λ,
             [xa; Dojo.vector(qa)]
             ) * attjac
-        @test norm(J0 - J1, Inf) < 1.0e-7
+        @test norm(J0 - J1, Inf) < 1.0e-8
 
         # impulse_map_parent_jacobian_child
         J0 = Dojo.impulse_map_jacobian(:parent, :child, tra0, pbody, cbody, λ)
         attjac = cat(I(3), Dojo.LVᵀmat(qb), dims=(1,2))
-        J1 = FiniteDiff.finite_difference_jacobian(
+        J1 = ForwardDiff.jacobian(
             z -> Dojo.impulse_map(:parent, tra0, xa, qa, z[1:3], Quaternion(z[4:7]...), 0) * λ,
             [xb; Dojo.vector(qb)]
             ) * attjac
-        @test norm(J0 - J1, Inf) < 1.0e-7
+        @test norm(J0 - J1, Inf) < 1.0e-8
 
         # impulse_map_child_jacobian_parent
         J0 = Dojo.impulse_map_jacobian(:child, :parent, tra0, pbody, cbody, λ)
         attjac = cat(I(3), Dojo.LVᵀmat(qa), dims=(1,2))
-        J1 = FiniteDiff.finite_difference_jacobian(
+        J1 = ForwardDiff.jacobian(
             z -> Dojo.impulse_map(:child, tra0, z[1:3], Quaternion(z[4:7]...), xb, qb, 0) * λ,
             [xa; Dojo.vector(qa)]
             ) * attjac
-        @test norm(J0 - J1, Inf) < 1.0e-7
+        @test norm(J0 - J1, Inf) < 1.0e-8
 
         # impulse_map_child_jacobian_child
         J0 = Dojo.impulse_map_jacobian(:child, :child, tra0, pbody, cbody, λ)
         attjac = cat(I(3), Dojo.LVᵀmat(qb), dims=(1,2))
-        J1 = FiniteDiff.finite_difference_jacobian(
+        J1 = ForwardDiff.jacobian(
             z -> Dojo.impulse_map(:child, tra0, xa, qa, z[1:3], Quaternion(z[4:7]...), 0) * λ,
             [xb; Dojo.vector(qb)]
             ) * attjac
-        @test norm(J0 - J1, Inf) < 1.0e-7
+        @test norm(J0 - J1, Inf) < 1.0e-8
     end
 end
diff --git a/test/integrator.jl b/test/integrator.jl
index 6deb4584b..3451379d4 100644
--- a/test/integrator.jl
+++ b/test/integrator.jl
@@ -1,4 +1,4 @@
-@testset "FiniteDiff comparison" begin
+@testset "ForwardDiff comparison" begin
     Random.seed!(100)
     x0 = srand(3)
     v0 = srand(3)
@@ -9,33 +9,33 @@
     q1 = Dojo.next_orientation(q0, ϕ0, timestep0)
 
     # ∇v
-    FD∂i∂v = FiniteDiff.finite_difference_jacobian(v0 -> Dojo.next_position(x0, v0, timestep0), v0)
-    @test norm(FD∂i∂v - Dojo.linear_integrator_jacobian_velocity(x0, v0, timestep0), Inf) < 1.0e-6
+    FD∂i∂v = ForwardDiff.jacobian(v0 -> Dojo.next_position(x0, v0, timestep0), v0)
+    @test norm(FD∂i∂v - Dojo.linear_integrator_jacobian_velocity(x0, v0, timestep0), Inf) < 1.0e-8
     # ∇ϕ
-    FD∂i∂ϕ = FiniteDiff.finite_difference_jacobian(ϕ0 -> Dojo.vector(Dojo.next_orientation(q0, ϕ0, timestep0)), ϕ0)
-    @test norm(FD∂i∂ϕ - Dojo.rotational_integrator_jacobian_velocity(q0, ϕ0, timestep0), Inf) < 1.0e-6
+    FD∂i∂ϕ = ForwardDiff.jacobian(ϕ0 -> Dojo.vector(Dojo.next_orientation(q0, ϕ0, timestep0)), ϕ0)
+    @test norm(FD∂i∂ϕ - Dojo.rotational_integrator_jacobian_velocity(q0, ϕ0, timestep0), Inf) < 1.0e-8
 
     # ∇x
-    FD∂i∂x = FiniteDiff.finite_difference_jacobian(x0 -> Dojo.next_position(x0, v0, timestep0), x0)
-    @test norm(FD∂i∂x - Dojo.linear_integrator_jacobian_position(x0, v0, timestep0), Inf) < 1.0e-6
+    FD∂i∂x = ForwardDiff.jacobian(x0 -> Dojo.next_position(x0, v0, timestep0), x0)
+    @test norm(FD∂i∂x - Dojo.linear_integrator_jacobian_position(x0, v0, timestep0), Inf) < 1.0e-8
     # ∇q
-    FD∂i∂q = FiniteDiff.finite_difference_jacobian(q0 ->
+    FD∂i∂q = ForwardDiff.jacobian(q0 ->
         Dojo.vector(Dojo.next_orientation(Quaternion(q0...), ϕ0, timestep0)),
         Dojo.vector(q0))
-    @test norm(FD∂i∂q - Dojo.rotational_integrator_jacobian_orientation(q0, ϕ0, timestep0, attjac=false), Inf) < 1.0e-6
+    @test norm(FD∂i∂q - Dojo.rotational_integrator_jacobian_orientation(q0, ϕ0, timestep0, attjac=false), Inf) < 1.0e-8
 
     # ∇vϕ
     vel0 = [v0; ϕ0]
-    FDvel = FiniteDiff.finite_difference_jacobian(vel0 ->
+    FDvel = ForwardDiff.jacobian(vel0 ->
         [Dojo.next_position(x0, SVector{3}(vel0[1:3]), timestep0);
         Dojo.vector(Dojo.next_orientation(q0, SVector{3}(vel0[4:6]), timestep0))], vel0)
-    @test norm(FDvel - Dojo.integrator_jacobian_velocity(x0, v0, q0, ϕ0, timestep0), Inf) < 1.0e-6
+    @test norm(FDvel - Dojo.integrator_jacobian_velocity(x0, v0, q0, ϕ0, timestep0), Inf) < 1.0e-8
 
     # ∇xq
     con0 = [x0; Dojo.vector(q0)]
-    FDcon = FiniteDiff.finite_difference_jacobian(con0 ->
+    FDcon = ForwardDiff.jacobian(con0 ->
         [Dojo.next_position(SVector{3}(con0[1:3]), v0, timestep0);
         Dojo.vector(Dojo.next_orientation(Quaternion(con0[4:7]...), ϕ0, timestep0))], con0)
-    @test norm(FDcon - Dojo.integrator_jacobian_configuration(x0, v0, q0, ϕ0, timestep0, attjac=false), Inf) < 1e-8
-    @test norm(FDcon * cat(I(3), Dojo.LVᵀmat(q0), dims=(1,2)) - Dojo.integrator_jacobian_configuration(x0, v0, q0, ϕ0, timestep0, attjac=true), Inf) < 1e-8
+    @test norm(FDcon - Dojo.integrator_jacobian_configuration(x0, v0, q0, ϕ0, timestep0, attjac=false), Inf) < 1.0e-8
+    @test norm(FDcon * cat(I(3), Dojo.LVᵀmat(q0), dims=(1,2)) - Dojo.integrator_jacobian_configuration(x0, v0, q0, ϕ0, timestep0, attjac=true), Inf) < 1.0e-8
 end
diff --git a/test/minimal.jl b/test/minimal.jl
index 508471ba0..63681f15b 100644
--- a/test/minimal.jl
+++ b/test/minimal.jl
@@ -20,6 +20,25 @@ joint_types = [
     :PlanarFree
     ]
 
+# TODO this is necessary, because some conversions do not return a vector for scalar values
+# See TODO below for removing finite diff
+function force_to_jacobian_finite_diff(f,x)
+	if typeof(f(x)) <: AbstractVector
+		return FiniteDiff.finite_difference_jacobian(f,x)
+	else
+		return FiniteDiff.finite_difference_jacobian(x -> [f(x)],x)
+	end
+end
+
+
+function force_to_jacobian_forward_diff(f,x)
+	if typeof(f(x)) <: AbstractVector
+		return ForwardDiff.jacobian(f,x)
+	else
+		return ForwardDiff.jacobian(x -> [f(x)],x)
+	end
+end
+
 ################################################################################
 # Test get and set position and velocities
 ################################################################################
@@ -80,20 +99,20 @@ joint_types = [
 				Dojo.set_minimal_coordinates!(rot0, pnodes0[i], cnodes0[i],  timestep,
 					Δθ=Δθ)
 				Δθ0 = Dojo.minimal_coordinates(rot0, pnodes0[i], cnodes0[i])
-				@test norm(Δθ0 - Δθ, Inf) < 1.0e-7
+				@test norm(Δθ0 - Δθ, Inf) < 1.0e-8
 
 				Dojo.set_minimal_coordinates!(tra0, pnodes0[i], cnodes0[i], timestep,
 					Δx=Δx)
 				Δx0 = Dojo.minimal_coordinates(tra0, pnodes0[i], cnodes0[i])
-				@test norm(Δx0 - Δx, Inf) < 1.0e-7
+				@test norm(Δx0 - Δx, Inf) < 1.0e-8
 
 				Dojo.set_minimal_velocities!(joint0, pnodes0[i], cnodes0[i], timestep,
 					Δv=Δv,
 					Δϕ=Δϕ)
 				Δϕ0 = Dojo.minimal_velocities(rot0, pnodes0[i], cnodes0[i], timestep)
 				Δv0 = Dojo.minimal_velocities(tra0, pnodes0[i], cnodes0[i], timestep)
-				@test norm(Δϕ0 - Δϕ, Inf) < 1.0e-7
-				@test norm(Δv0 - Δv, Inf) < 1.0e-7
+				@test norm(Δϕ0 - Δϕ, Inf) < 1.0e-8
+				@test norm(Δv0 - Δv, Inf) < 1.0e-8
 			end
 		end
 	end
@@ -259,7 +278,7 @@ end
 end
 
 ################################################################################
-# Test minimal coordinates and velocities Jacobians
+#Test minimal coordinates and velocities Jacobians
 ################################################################################
 @testset "Jacobians" begin
 	@testset "Minimal velocity Jacobian" begin
@@ -296,28 +315,28 @@ end
 
 				# Jacobians
 				∇0 = Dojo.minimal_velocities_jacobian_configuration(:parent, joint, xa, va, qa, ωa, xb, vb, qb, ωb, timestep)
-				∇1 = FiniteDiff.finite_difference_jacobian(
+				∇1 = force_to_jacobian_forward_diff(
 					xq -> Dojo.minimal_velocities(joint, xq[Dojo.SUnitRange(1,3)], va, Quaternion(xq[4:7]...), ωa, xb, vb, qb, ωb, timestep),
 					[xa; Dojo.vector(qa)]) * cat(I(3), Dojo.LVᵀmat(qa), dims=(1,2))
-				@test norm(∇0 - ∇1, Inf) < 1.0e-5
+				@test norm(∇0 - ∇1, Inf) < 1.0e-8
 
 				∇0 = Dojo.minimal_velocities_jacobian_configuration(:child, joint, xa, va, qa, ωa, xb, vb, qb, ωb, timestep)
-				∇1 = FiniteDiff.finite_difference_jacobian(
+				∇1 = force_to_jacobian_forward_diff(
 					xq -> Dojo.minimal_velocities(joint, xa, va, qa, ωa, xq[Dojo.SUnitRange(1,3)], vb, Quaternion(xq[4:7]...), ωb, timestep),
 					[xb; Dojo.vector(qb)]) * cat(I(3), Dojo.LVᵀmat(qb), dims=(1,2))
-				@test norm(∇0 - ∇1, Inf) < 1.0e-5
+				@test norm(∇0 - ∇1, Inf) < 1.0e-8
 
 				∇0 = Dojo.minimal_velocities_jacobian_velocity(:parent, joint, xa, va, qa, ωa, xb, vb, qb, ωb, timestep)
-				∇1 = FiniteDiff.finite_difference_jacobian(
+				∇1 = force_to_jacobian_forward_diff(
 					vϕ -> Dojo.minimal_velocities(joint, xa, vϕ[Dojo.SUnitRange(1,3)], qa, vϕ[Dojo.SUnitRange(4,6)], xb, vb, qb, ωb, timestep),
 					[va; ωa])
-				@test norm(∇0 - ∇1, Inf) < 1.0e-5
+				@test norm(∇0 - ∇1, Inf) < 1.0e-8
 
 				∇0 = Dojo.minimal_velocities_jacobian_velocity(:child, joint, xa, va, qa, ωa, xb, vb, qb, ωb, timestep)
-				∇1 = FiniteDiff.finite_difference_jacobian(
+				∇1 = force_to_jacobian_forward_diff(
 					vϕ -> Dojo.minimal_velocities(joint, xa, va, qa, ωa, xb, vϕ[Dojo.SUnitRange(1,3)], qb, vϕ[Dojo.SUnitRange(4,6)], timestep),
 					[vb; ωb])
-				@test norm(∇0 - ∇1, Inf) < 1.0e-5
+				@test norm(∇0 - ∇1, Inf) < 1.0e-8
 			end
 		end
 	end
@@ -354,29 +373,37 @@ end
 				Dojo.minimal_coordinates(joint, xa, qa, xb, qb)
 
 				∇0 = Dojo.minimal_coordinates_jacobian_configuration(:parent, joint, xa, qa, xb, qb)
-				∇1 = FiniteDiff.finite_difference_jacobian(
+				∇1 = force_to_jacobian_forward_diff(
 					xq -> Dojo.minimal_coordinates(joint, xq[1:3], Quaternion(xq[4:7]...), xb, qb),
 					[xa; Dojo.vector(qa)]) * cat(I(3), Dojo.LVᵀmat(qa), dims=(1,2))
-				@test norm(∇0 - ∇1, Inf) < 1.0e-6
+				@test norm(∇0 - ∇1, Inf) < 1.0e-8
 
 				∇0 = Dojo.minimal_coordinates_jacobian_configuration(:child, joint, xa, qa, xb, qb)
-				∇1 = FiniteDiff.finite_difference_jacobian(
+				∇1 = force_to_jacobian_forward_diff(
 					xq -> Dojo.minimal_coordinates(joint, xa, qa, xq[1:3], Quaternion(xq[4:7]...)),
 					[xb; Dojo.vector(qb)]) * cat(I(3), Dojo.LVᵀmat(qb), dims=(1,2))
-				@test norm(∇0 - ∇1, Inf) < 1.0e-6
+				@test norm(∇0 - ∇1, Inf) < 1.0e-8
 			end
 		end
 	end
 
 	@testset "Minimal to maximal Jacobian" begin
 		function maximal_to_minimal_jacobian_fd(mechanism::Mechanism, z)
-			J = FiniteDiff.finite_difference_jacobian(y -> Dojo.maximal_to_minimal(mechanism, y), z)
+			J = force_to_jacobian_forward_diff(y -> Dojo.maximal_to_minimal(mechanism, y), z)
 			G = attitude_jacobian(z, length(mechanism.bodies))
 			return J * G
 		end
 
+		# TODO switch to ForwardDiff once it works
+		function maximal_to_minimal_jacobian_fd_finite_diff(mechanism::Mechanism, z)
+			J = force_to_jacobian_finite_diff(y -> Dojo.maximal_to_minimal(mechanism, y), z)
+			G = attitude_jacobian(z, length(mechanism.bodies))
+			return J * G
+		end
+
+		# TODO switch to ForwardDiff once it works
 		function minimal_to_maximal_jacobian_fd(mechanism::Mechanism, x)
-			J = FiniteDiff.finite_difference_jacobian(y -> Dojo.minimal_to_maximal(mechanism, y), x)
+			J = force_to_jacobian_finite_diff(y -> Dojo.minimal_to_maximal(mechanism, y), x)
 			z = minimal_to_maximal(mechanism, x)
 			G = attitude_jacobian(z, length(mechanism.bodies))
 			return G' * J
@@ -404,19 +431,19 @@ end
 		u = zeros(Dojo.input_dimension(mechanism))
 
 		@test norm(minimal_to_maximal(mechanism, x) - z) < 1.0e-5
-		@test norm(Dojo.maximal_to_minimal(mechanism, z) - x) < 1.0e-5
+		@test norm(Dojo.maximal_to_minimal(mechanism, z) - x) < 1.0e-8
 
 		M_fd = maximal_to_minimal_jacobian_fd(mechanism, z)
 		M_a = Dojo.maximal_to_minimal_jacobian(mechanism, z)
 		@test size(M_fd) == size(M_a)
-		@test norm(M_fd - M_a, Inf) < 1.0e-5
+		@test norm(M_fd - M_a, Inf) < 1.0e-8
 
 		N_fd = minimal_to_maximal_jacobian_fd(mechanism, Dojo.maximal_to_minimal(mechanism, z))
 		N_a = Dojo.minimal_to_maximal_jacobian(mechanism, Dojo.maximal_to_minimal(mechanism, z))
 		@test size(N_fd) == size(N_a)
 		@test norm(N_fd - N_a, Inf) < 1.0e-5
-		@test norm(diag(M_fd * N_fd) .- 1.0, Inf) < 5.0e-5
-		@test norm(diag(M_a * N_a) .- 1.0, Inf) < 5.0e-5
+		@test norm(diag(M_fd * N_fd) .- 1.0, Inf) < 1.0e-5
+		@test norm(diag(M_a * N_a) .- 1.0, Inf) < 1.0e-8
 
 		# n-pendulum
 		mechanism = Dojo.get_mechanism(:npendulum,
@@ -435,19 +462,19 @@ end
 		u = zeros(Dojo.input_dimension(mechanism))
 
 		@test norm(minimal_to_maximal(mechanism, x) - z) < 1.0e-5
-		@test norm(Dojo.maximal_to_minimal(mechanism, z) - x) < 1.0e-5
+		@test norm(Dojo.maximal_to_minimal(mechanism, z) - x) < 1.0e-8
 
 		M_fd = maximal_to_minimal_jacobian_fd(mechanism, z)
 		M_a = Dojo.maximal_to_minimal_jacobian(mechanism, z)
 		@test size(M_fd) == size(M_a)
-		@test norm(M_fd - M_a, Inf) < 1.0e-5
+		@test norm(M_fd - M_a, Inf) < 1.0e-8
 
 		N_fd = minimal_to_maximal_jacobian_fd(mechanism, Dojo.maximal_to_minimal(mechanism, z))
 		N_a = Dojo.minimal_to_maximal_jacobian(mechanism, Dojo.maximal_to_minimal(mechanism, z))
 		@test size(N_fd) == size(N_a)
 		@test norm(N_fd - N_a, Inf) < 1.0e-5
-		@test norm(diag(M_fd * N_fd) .- 1.0, Inf) < 5.0e-5
-		@test norm(diag(M_a * N_a) .- 1.0, Inf) < 5.0e-5
+		@test norm(diag(M_fd * N_fd) .- 1.0, Inf) < 1.0e-5
+		@test norm(diag(M_a * N_a) .- 1.0, Inf) < 1.0e-5
 
 		# sphere
 		mechanism = Dojo.get_mechanism(:sphere,
@@ -462,10 +489,10 @@ end
 		x = Dojo.get_minimal_state(mechanism)
 		u = zeros(Dojo.input_dimension(mechanism))
 
-		@test norm(minimal_to_maximal(mechanism, x) - z) < 1.0e-5
-		@test norm(Dojo.maximal_to_minimal(mechanism, z) - x) < 1.0e-5
+		@test norm(minimal_to_maximal(mechanism, x) - z) < 1.0e-8
+		@test norm(Dojo.maximal_to_minimal(mechanism, z) - x) < 1.0e-8
 
-		M_fd = maximal_to_minimal_jacobian_fd(mechanism, z)
+		M_fd = maximal_to_minimal_jacobian_fd_finite_diff(mechanism, z)
 		M_a = Dojo.maximal_to_minimal_jacobian(mechanism, z)
 		@test size(M_fd) == size(M_a)
 		@test norm(M_fd - M_a, Inf) < 1.0e-5
@@ -473,9 +500,9 @@ end
 		N_fd = minimal_to_maximal_jacobian_fd(mechanism, Dojo.maximal_to_minimal(mechanism, z))
 		N_a = Dojo.minimal_to_maximal_jacobian(mechanism, Dojo.maximal_to_minimal(mechanism, z))
 		@test size(N_fd) == size(N_a)
-		@test norm(N_fd - N_a, Inf) < 1.0e-6
-		@test norm(diag(M_fd * N_fd) .- 1.0, Inf) < 5.0e-5
-		@test norm(diag(M_a * N_a) .- 1.0, Inf) < 5.0e-5
+		@test norm(N_fd - N_a, Inf) < 1.0e-5
+		@test norm(diag(M_fd * N_fd) .- 1.0, Inf) < 1.0e-5
+		@test norm(diag(M_a * N_a) .- 1.0, Inf) < 1.0e-8
 
 		# half cheetah
 		mechanism = Dojo.get_mechanism(:halfcheetah,
@@ -491,21 +518,21 @@ end
 		u = zeros(Dojo.input_dimension(mechanism))
 
 		@test norm(minimal_to_maximal(mechanism, x) - z) < 1.0e-5
-		@test norm(Dojo.maximal_to_minimal(mechanism, z) - x) < 1.0e-5
+		@test norm(Dojo.maximal_to_minimal(mechanism, z) - x) < 1.0e-8
 
 		M_fd = maximal_to_minimal_jacobian_fd(mechanism, z)
 		M_a = Dojo.maximal_to_minimal_jacobian(mechanism, z)
 		@test size(M_fd) == size(M_a)
-		@test norm(M_fd - M_a, Inf) < 1.0e-5
+		@test norm(M_fd - M_a, Inf) < 1.0e-8
 
 		N_fd = minimal_to_maximal_jacobian_fd(mechanism, Dojo.maximal_to_minimal(mechanism, z))
 		N_a = Dojo.minimal_to_maximal_jacobian(mechanism, Dojo.maximal_to_minimal(mechanism, z))
 		@test size(N_fd) == size(N_a)
 		@test norm(N_fd - N_a, Inf) < 1.0e-5
 
-		@test norm(diag(M_fd * N_fd) .- 1.0, Inf) < 5.0e-5
-		@test norm(diag(M_a * N_a) .- 1.0, Inf) < 5.0e-5
-		@test norm(diag(M_a * N_fd) .- 1.0, Inf) < 5.0e-5
+		@test norm(diag(M_fd * N_fd) .- 1.0, Inf) < 1.0e-5
+		@test norm(diag(M_a * N_a) .- 1.0, Inf) < 1.0e-5
+		@test norm(diag(M_a * N_fd) .- 1.0, Inf) < 1.0e-5
 
 		# atlas
 		mechanism = Dojo.get_mechanism(:atlas,
@@ -527,25 +554,32 @@ end
 		u = zeros(Dojo.input_dimension(mechanism))
 
 		@test norm(minimal_to_maximal(mechanism, x) - z) < 1.0e-5
-		@test norm(Dojo.maximal_to_minimal(mechanism, z) - x) < 1.0e-5
+		@test norm(Dojo.maximal_to_minimal(mechanism, z) - x) < 1.0e-8
 
 		M_fd = maximal_to_minimal_jacobian_fd(mechanism, z)
 		M_a = Dojo.maximal_to_minimal_jacobian(mechanism, z)
 		@test size(M_fd) == size(M_a)
-		@test norm(M_fd - M_a, Inf) < 1.0e-5
+		@test norm(M_fd - M_a, Inf) < 1.0e-8
 
 		N_fd = minimal_to_maximal_jacobian_fd(mechanism, Dojo.maximal_to_minimal(mechanism, z))
 		N_a = Dojo.minimal_to_maximal_jacobian(mechanism, Dojo.maximal_to_minimal(mechanism, z))
 		@test size(N_fd) == size(N_a)
 		@test norm(N_fd - N_a, Inf) < 5.0e-5
 
-		@test norm(diag(M_fd * N_fd) .- 1.0, Inf) < 5.0e-5
-		@test norm(diag(M_a * N_a) .- 1.0, Inf) < 5.0e-5
+		@test norm(diag(M_fd * N_fd) .- 1.0, Inf) < 1.0e-5
+		@test norm(diag(M_a * N_a) .- 1.0, Inf) < 1.0e-5
 	end
 
 	@testset "Maximal to minimal Jacobian" begin
+
+		function maximal_to_minimal_jacobian_fd_finite_diff(mechanism::Mechanism, z)
+			J = force_to_jacobian_finite_diff(y -> maximal_to_minimal(mechanism, y), z)
+			G = attitude_jacobian(z, length(mechanism.bodies))
+			return J * G
+		end
+
 		function maximal_to_minimal_jacobian_fd(mechanism::Mechanism, z)
-			J = FiniteDiff.finite_difference_jacobian(y -> maximal_to_minimal(mechanism, y), z)
+			J = force_to_jacobian_forward_diff(y -> maximal_to_minimal(mechanism, y), z)
 			G = attitude_jacobian(z, length(mechanism.bodies))
 			return J * G
 		end
@@ -572,7 +606,7 @@ end
 		M_fd = maximal_to_minimal_jacobian_fd(mech, z)
 		M_a = Dojo.maximal_to_minimal_jacobian(mech, z)
 		@test size(M_fd) == size(M_a)
-		@test norm(M_fd - M_a, Inf) < 1.0e-6
+		@test norm(M_fd - M_a, Inf) < 1.0e-8
 
 		# # sphere
 		mech = Dojo.get_mechanism(:sphere,
@@ -588,7 +622,7 @@ end
 		z = Dojo.get_maximal_state(mech)
 
 		attjac = Dojo.attitude_jacobian(z, length(mech.bodies))
-		M_fd = maximal_to_minimal_jacobian_fd(mech, z)
+		M_fd = maximal_to_minimal_jacobian_fd_finite_diff(mech, z)
 		M_a = Dojo.maximal_to_minimal_jacobian(mech, z)
 
 		@test size(M_fd) == size(M_a)
diff --git a/test/momentum.jl b/test/momentum.jl
index ed7414cbe..09419619f 100644
--- a/test/momentum.jl
+++ b/test/momentum.jl
@@ -64,8 +64,8 @@ joint_types = [
     mang0= [Vector(m - m0[1])[4:6] for m in m0]
     # plot([(i-1) * timestep0 for i in 1:length(m0)], hcat(mlin0...)')
     # plot([(i-1) * timestep0 for i in 1:length(m0)], hcat(mang0...)')
-    @test all(norm.(mlin0, Inf) .< 1.0e-7)
-    @test all(norm.(mang0, Inf) .< 1.0e-7)
+    @test all(norm.(mlin0, Inf) .< 1.0e-8)
+    @test all(norm.(mang0, Inf) .< 1.0e-8)
 end
 
 ################################################################################
@@ -101,7 +101,7 @@ end
     # plot([(i-1) * timestep0 for i in 1:length(m0)], hcat(mlin0...)')
     # plot([(i-1) * timestep0 for i in 1:length(m0)], hcat(mang0...)')
     # @test all(norm.(mlin0, Inf) .< 1e-11)
-    @test all(norm.(mang0, Inf) .< 1.0e-7)
+    @test all(norm.(mang0, Inf) .< 1.0e-8)
 end
 
 ################################################################################
diff --git a/test/mrp.jl b/test/mrp.jl
index fbd454a55..a761523f5 100644
--- a/test/mrp.jl
+++ b/test/mrp.jl
@@ -2,14 +2,15 @@
     q = [1,2,3,4.0]
     q = Quaternion(q ./ norm(q)...,true)
     @test norm(Dojo.dmrpdq(Dojo.vector(q)) -
-        FiniteDiff.finite_difference_jacobian(Dojo.mrp, Dojo.vector(q)), Inf) < 1.0e-5
+        ForwardDiff.jacobian(Dojo.mrp, Dojo.vector(q)), Inf) < 1.0e-8
     @test norm(Dojo.daxisdq(Dojo.vector(q)) -
-        FiniteDiff.finite_difference_jacobian(Dojo.axis, Dojo.vector(q))) < 1.0e-5
+        ForwardDiff.jacobian(Dojo.axis, Dojo.vector(q))) < 1.0e-8
     @test norm(Dojo.drotation_vectordq(Dojo.vector(q)) -
-        FiniteDiff.finite_difference_jacobian(Dojo.rotation_vector, Dojo.vector(q))) < 1.0e-5
+        ForwardDiff.jacobian(Dojo.rotation_vector, Dojo.vector(q))) < 1.0e-8
     q = one(Quaternion{Float64})
     @test norm(Dojo.dmrpdq(Dojo.vector(q)) -
-        FiniteDiff.finite_difference_jacobian(Dojo.mrp, Dojo.vector(q)), Inf) < 1.0e-5
+        ForwardDiff.jacobian(Dojo.mrp, Dojo.vector(q)), Inf) < 1.0e-8
+    # TODO For zero rotation, ForwardDiff runs into a numeric singularity
     @test norm(Dojo.drotation_vectordq(Dojo.vector(q)) -
         FiniteDiff.finite_difference_jacobian(Dojo.rotation_vector, Dojo.vector(q))) < 1.0e-5
 end
@@ -19,13 +20,13 @@ end
     n1 = [0,0,0.0]
     n2 = [0,0,0.0]
     q = Dojo.axes_pair_to_quaternion(n1, n2)
-    @test norm(n2 - Dojo.vector_rotate(n1, q), Inf) < 1e-10
+    @test norm(n2 - Dojo.vector_rotate(n1, q), Inf) < 1e-8
 
     n1 = rand(3)
     n1 /= norm(n1)
     n2 = n1
     q = Dojo.axes_pair_to_quaternion(n1, n2)
-    @test norm(n2 - Dojo.vector_rotate(n1, q), Inf) < 1e-10
+    @test norm(n2 - Dojo.vector_rotate(n1, q), Inf) < 1e-8
 
     n1 = rand(3)
     n1 /= norm(n1)
@@ -38,5 +39,5 @@ end
     n2 = rand(3)
     n2 /= norm(n2)
     q = Dojo.axes_pair_to_quaternion(n1, n2)
-    @test norm(n2 - Dojo.vector_rotate(n1, q), Inf) < 1e-10
+    @test norm(n2 - Dojo.vector_rotate(n1, q), Inf) < 1e-8
 end
diff --git a/test/runtests.jl b/test/runtests.jl
index 055584681..1c36289e2 100644
--- a/test/runtests.jl
+++ b/test/runtests.jl
@@ -6,6 +6,7 @@ using Random
 using SparseArrays
 using StaticArrays
 using FiniteDiff
+using ForwardDiff
 using Rotations
 
 using Dojo