Open lucpaoli opened 1 year ago
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)
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!