A few bug fixes

RoboticExplorationLab · Mar 2, 2022 · 103d4f6 · 103d4f6 · bjack205 · Mar 2, 2022
1 parent 85811e5
commit 103d4f6
Show file tree

Hide file tree

Showing 3 changed files with 12 additions and 5 deletions.
diff --git a/src/deprecate.jl b/src/deprecate.jl
@@ -1,7 +1,7 @@
 @deprecate state_diff_jacobian!(args...) errstate_jacobian!(args...)
 @deprecate ∇²differential!(args...) ∇errstate_jacobian!(args...)
-@deprecate SampledTrajectory(X,U,dt::Vector{<:Real}) Sampled(X,U,dt=dt)
-@deprecate Sampled(x::AbstractVector{<:Real}, u::AbstractVector{<:Real}, dt::AbstractVector{<:Real}) Traj([copy(x) for k = 1:length(dt)], [copy(u) for k = 1:length(dt)-1], dt=dt)
+@deprecate SampledTrajectory(X,U,dt::Vector{<:Real}) SampledTrajectory(X,U,dt=dt)
+@deprecate SampledTrajectory(x::AbstractVector{<:Real}, u::AbstractVector{<:Real}, dt::AbstractVector{<:Real}) Traj([copy(x) for k = 1:length(dt)], [copy(u) for k = 1:length(dt)-1], dt=dt)
 @deprecate get_data(x::SampledTrajectory) getdata(x)
 Base.Base.@deprecate_binding Traj SampledTrajectory true
 

diff --git a/src/statevectortype.jl b/src/statevectortype.jl
@@ -144,5 +144,12 @@ the state vector space. The vector ``\\bar{x}`` is some element of the state spa
 errstate_dim(::EuclideanState, fun::AbstractFunction) = state_dim(fun)
 state_diff!(::EuclideanState, fun::AbstractFunction, dx, x, x0) = dx .= x .- x0
 state_diff(::EuclideanState, fun::AbstractFunction, x, x0) = x - x0
-errstate_jacobian!(::EuclideanState, fun::AbstractFunction, J, x) = J .= I(state_dim(fun))
-∇errstate_jacobian!(::EuclideanState, fun::AbstractFunction, ∇G, x, dx) = ∇G .= 0
+∇errstate_jacobian!(::EuclideanState, fun::AbstractFunction, ∇G, x, dx) = ∇G .= 0
+
+function errstate_jacobian!(::EuclideanState, fun::AbstractFunction, J, x)
+    J .= 0
+    for i = 1:size(J,1)
+        J[i,i] = one(eltype(J))
+    end
+    return nothing
+end
diff --git a/src/utils.jl b/src/utils.jl
@@ -5,4 +5,4 @@ end
 function Base.showerror(io::IO, err::NotImplementedError)
     print(io, "NotImplementedError: ")
     print(io, err.msg)
-end
+end