Using discrete dynamics throws error

[JTaets]

Hi, thanks for making his package available!

When trying to use the solvers with discrete dynamics, an error is thrown. Complaining it doesn't have RobotDynamics.discrete_dynamics(model::Cartpole,x,u,t) in the line altro = ALTROSolver(prob, opts).

Kind regards.

using TrajectoryOptimization
using RobotDynamics
using StaticArrays
using Altro
using LinearAlgebra

## Setting up the dynamics model
struct Cartpole{T} <: TrajectoryOptimization.AbstractModel
    mc::T
    mp::T
    l::T
    g::T
end

Cartpole() = Cartpole(1.0, 0.2, 0.5, 9.81)

function RobotDynamics.discrete_dynamics(::Type{PassThrough}, model::Cartpole,
        x::StaticVector, u::StaticVector, t, dt)

    mc = model.mc   # mass of the cart in kg (10)
    mp = model.mp   # mass of the pole j(point mass at the end) in kg
    l = model.l     # length of the pole in m
    g = model.g     # gravity m/s^2

    q  = x[SA[1,2]]
    qd = x[SA[3,4]]

    s = sin(q[2])
    c = cos(q[2])

    H = SA[mc+mp mp*l*c; mp*l*c mp*l^2]
    C = SA[0 -mp*qd[2]*l*s; 0 0]
    G = SA[0, mp*g*l*s]
    B = SA[1, 0]

    qdd = -H\(C*qd + G - B*u[1])
    xdot = [qd; qdd]
    return x + xdot * dt  # simple Euler integration
end

RobotDynamics.state_dim(::Cartpole) = 4
RobotDynamics.control_dim(::Cartpole) = 1

## Setting up the objective function
model = Cartpole()
n,m = size(model)
N = 51   # number of knot points
tf = 5.0  # final time
x0 = @SVector [0, 0, 0, 0.]  # initial state
xf = @SVector [0, π, 0, 0.]  # goal state (i.e. swing up)

Q = Diagonal(@SVector fill(0.1,n))
R = Diagonal(@SVector fill(0.1,m))
Qf = Diagonal(@SVector fill(1000.,n))
obj = LQRObjective(Q, R, Qf, xf, N)

## Adding constraints
cons = ConstraintList(n,m,N)
goalcon = GoalConstraint(xf)
add_constraint!(cons, goalcon, N)  # add to the last time step
ubnd = 3
bnd = BoundConstraint(n,m, u_min=-ubnd, u_max=ubnd)
add_constraint!(cons, bnd, 1:N-1)  # add to all but the last time step

## Setting up the problem
prob = Problem(model, obj, xf, tf, constraints=cons, x0=x0, integration=PassThrough)

## Solving the problem
opts = SolverOptions(
    penalty_scaling=10.,
    penalty_initial=1.0
)
altro = ALTROSolver(prob, opts)
@time Altro.solve!(altro)

[JTaets]

I was able to fix this, by removing some of the DEFAULT_Q in the code and passing the correct integrator(prob)

More specifically:

1. in ilqr.jl: iLQRSolver( prob::Problem{QUAD,T}, opts::SolverOptions=SolverOptions(), stats::SolverStats=SolverStats(parent=solvername(iLQRSolver)); kwarg_opts... ) where {QUAD,T}. Change cache = FiniteDiff.JacobianCache(prob.model) to cache = FiniteDiff.JacobianCache(prob.model, sparsity = RobotDynamics.detect_sparsity(QUAD, prob.model))

2. Similarly in constraints.jl: DynamicsConstraint{Q}(model::L, N) where {Q,L} . Change cache = FiniteDiff.JacobianCache(model) to cache = FiniteDiff.JacobianCache(model, sparsity = RobotDynamics.detect_sparsity(Q, model))