FiniteDiff Error when calling DiscretizedDynamics

[yixjia]

Hello,
I'm trying to discretize the continuous model of the quadrotor and I got this error:

ERROR: MethodError: Cannot `convert` an object of type 
  FiniteDiff.JacobianCache{Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}, UnitRange{Int64}, Nothing, Val{:forward}(), Float64} to an object of type 
  FiniteDiff.JacobianCache{Vector{Float64}, Vector{Float64}, Vector{Float64}, UnitRange{Int64}, Nothing, Val{:forward}(), Float64}
Closest candidates are:
  convert(::Type{T}, ::T) where T at ~/julia/share/julia/base/essentials.jl:218

Here's the code that can reproduce this error:

using TrajectoryOptimization
using RobotDynamics
using StaticArrays, LinearAlgebra
using ForwardDiff
using FiniteDiff

struct One_QuadRotor <: RobotDynamics.ContinuousDynamics
    mass::Float64
    J::Diagonal{Float64,SVector{3,Float64}}
    Jinv::Diagonal{Float64,SVector{3,Float64}}
    gravity::SVector{3,Float64}
    motor_dist::Float64
    kf::Float64
    km::Float64
end

mass=0.5
J=Diagonal(@SVector [0.0023, 0.0023, 0.004])
Jinv = inv(J)
gravity=@SVector[0,0,-9.81]
motor_dist=0.1750
kf=1.0
km=0.0245

One_QuadRotor() = One_QuadRotor(mass,J,Jinv,gravity,motor_dist,kf,km)


RobotDynamics.state_dim(::One_QuadRotor) = 13
RobotDynamics.control_dim(::One_QuadRotor) = 4

function RobotDynamics.dynamics(model::One_QuadRotor, x, u)
    m = model.mass
    J = model.J
    Jinv = model.Jinv
    g = model.gravity
    L = model.motor_dist
    kf = model.kf
    km = model.km

    # Extract motor speeds
    w1_1 = u[1]
    w2_1 = u[2]
    w3_1 = u[3]
    w4_1 = u[4]

    # Calculate motor forces
    F1_1 = max(0,kf*w1_1);
    F2_1 = max(0,kf*w2_1);
    F3_1 = max(0,kf*w3_1);
    F4_1 = max(0,kf*w4_1);
    
    F_1_b = @SVector [0., 0., F1_1+F2_1+F3_1+F4_1] #total rotor force in body frame

    # Calculate motor torques
    M1_1 = km*w1_1;
    M2_1 = km*w2_1;
    M3_1 = km*w3_1;
    M4_1 = km*w4_1;
    
    tau_1 = @SVector [L*(F2_1-F4_1), L*(F3_1-F1_1), (M1_1-M2_1+M3_1-M4_1)] #total rotor torque in body frame
    
    r1 = x[1:3]
    q1 = UnitQuaternion(x[4],x[5],x[6],x[7], false)
    v1 = x[8:10]
    w1 = x[11:13]
    
    F1 = m*g + q1*F_1_b
    
    q1dot = Rotations.kinematics(q1,w1)
    
    r1dot = v1
    v1dot = F1 ./m
    
    w1dot = Jinv*(tau_1 - w1 × (J*w1))
    
    return [r1dot; q1dot; v1dot; w1dot]
end

function RobotDynamics.dynamics!(model::One_QuadRotor, xdot, x, u)
    xstatic = SA[x[1], x[2], x[3], x[4], x[5], x[6], x[7], x[8], x[9], x[10], x[11], x[12], x[13]]
    ustatic = SA[u[1], u[2], u[3], u[4]]
    xdot .= RobotDynamics.dynamics(model, xstatic, ustatic)
    return nothing
end


rk4 = RobotDynamics.RK4(13, 4)
baseline_model = RobotDynamics.DiscretizedDynamics(One_QuadRotor(), rk4)

I also tried to follow the tutorial and add @autodiff before struct One_QuadRotor, but then this same error occurs when calling
One_QuadRotor() = One_QuadRotor(mass,J,Jinv,gravity,motor_dist,kf,km)

[Song921012]

This may happen because version of FiniteDiff updates from 2.12 to 2.13 (now is 2.14). For this time, to normally use TrajectoryOptimization.jl, you can try ]add FiniteDiff@2.12 and then ]pin FiniteDiff. For future use, it needs maintainers to fix the issue.

[yixjia]

This may happen because version of FiniteDiff updates from 2.12 to 2.13 (now is 2.14). For this time, to normally use TrajectoryOptimization.jl, you can try ]add FiniteDiff@2.12 and then ]pin FiniteDiff. For future use, it needs maintainers to fix the issue.

Thanks!! This indeed solves my problem.