Gradient not defined for custom cost functions using RD.@autodiff

[lucpaoli]

Based off of the tutorial code here:

http://roboticexplorationlab.org/TrajectoryOptimization.jl/stable/costfunction_interface.html

when I attempt to optimise cartpole controls with ALTRO, I find that the solver throws an error:

NotImplementedError: Gradient not implemented for scalar function CartpoleCost{5}

Is this intended behaviour? The documentation seems to suggest that the RD.@autodiff macro is supposed to automatically define these functions.

The code to produce this error is below, or otherwise I'm happy to share the Jupyter notebook and Project file I have been working with.

Thank you!

using TrajectoryOptimization
using RobotDynamics
import RobotZoo.Cartpole
using StaticArrays, LinearAlgebra
using ForwardDiff, FiniteDiff
using Altro
using Plots

RobotDynamics.@autodiff struct CartpoleCost <: TrajectoryOptimization.CostFunction
    Q
    R
    CartpoleCost() = CartpoleCost(1.0e-2*Diagonal(@SVector ones(n)), 1.0e-1*Diagonal(@SVector ones(m)))
end

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

function RobotDynamics.evaluate(cost::CartpoleCost, x, u)
    y = x[1]
    θ = x[2]
    ydot = x[3]
    θdot = x[4]
    J = cost.Q[2] * cos(θ/2)
    J += 0.5* (cost.Q[1] * y^2 + cost.Q[3] * ydot^2 + cost.Q[4] * θdot^2)
    if !isempty(u)
        J += 0.5 * cost.R[1] * u[1]^2
    end
    return J
end

model = Cartpole()
n,m = size(model);

N = 101
tf = 5.
dt = tf/(N-1)

x0 = @SVector [0, pi/2, 0, 0];#zeros(n)
xf = @SVector [0, pi, 0, 0];  # i.e. swing up

# Set up
# Q = 1.0e-2*Diagonal(@SVector ones(n))
# Qf = 100.0*Diagonal(@SVector ones(n))
# R = 1.0e-1*Diagonal(@SVector ones(m))
# obj = LQRObjective(Q,R,Qf,xf,N);

costfun = CartpoleCost()
obj = Objective(costfun, N)

# Create Empty ConstraintList
conSet = ConstraintList(n,m,N)

# Control Bounds
u_bnd = 3.0
bnd = BoundConstraint(n,m, u_min=-u_bnd, u_max=u_bnd)
add_constraint!(conSet, bnd, 1:N-1)

# Goal Constraint
goal = GoalConstraint(xf)
add_constraint!(conSet, goal, N)

prob = Problem(model, obj, x0, tf, constraints=conSet, xf=xf);
rollout!(prob)
solver = ALTROSolver(prob)
solve!(solver)