From 61e7985af79bb168b1b97495c8ffab90a9b59b32 Mon Sep 17 00:00:00 2001 From: Brian Jackson Date: Wed, 20 Mar 2024 20:17:50 -0400 Subject: [PATCH 1/4] Update rotations dep --- Project.toml | 2 +- problems/airplane.jl | 78 +++++----- problems/quadrotor.jl | 54 +++---- problems/quadrotor_maze.jl | 264 +++++++++++++++++----------------- test/Project.toml | 2 - test/ilqr_test.jl | 18 +-- test/projected_newton_test.jl | 55 +++---- test/runtests.jl | 6 +- 8 files changed, 242 insertions(+), 237 deletions(-) diff --git a/Project.toml b/Project.toml index 3b173d7..c4313d8 100644 --- a/Project.toml +++ b/Project.toml @@ -34,7 +34,7 @@ Interpolations = "0.12,0.13" Octavian = "0.3" RobotDynamics = "0.4" RobotZoo = "0.3" -Rotations = "~1" +Rotations = "1" SolverLogging = "0.2" StaticArrays = "1" TimerOutputs = "0.5" diff --git a/problems/airplane.jl b/problems/airplane.jl index 0395376..44dd598 100644 --- a/problems/airplane.jl +++ b/problems/airplane.jl @@ -2,57 +2,57 @@ const RD = RobotDynamics using TrajectoryOptimization: QuatLQRCost, ErrorQuadratic, QuatVecEq function YakProblems(; - integration=RD.RK4, - N = 101, - vecstate=false, - scenario=:barrellroll, - costfun=:Quadratic, - termcon=:goal, - quatnorm=:none, - kwargs... - ) - model = RobotZoo.YakPlane(UnitQuaternion) + integration=RD.RK4, + N=101, + vecstate=false, + scenario=:barrellroll, + costfun=:Quadratic, + termcon=:goal, + quatnorm=:none, + kwargs... +) + model = RobotZoo.YakPlane(QuatRotation) opts = SolverOptions( - cost_tolerance_intermediate = 1e-1, - penalty_scaling = 10., - penalty_initial = 10.; + cost_tolerance_intermediate=1e-1, + penalty_scaling=10.0, + penalty_initial=10.0; kwargs... ) s = RD.LieState(model) - n,m = RD.dims(model) + n, m = RD.dims(model) rsize = RD.state_dim(model) - 9 - vinds = SA[1,2,3,8,9,10,11,12,13] + vinds = SA[1, 2, 3, 8, 9, 10, 11, 12, 13] # Discretization tf = 1.25 - dt = tf/(N-1) + dt = tf / (N - 1) if scenario == :barrellroll - ey = @SVector [0,1,0.] + ey = @SVector [0, 1, 0.0] # Initial and final condition - p0 = MRP(0.997156, 0., 0.075366) # initial orientation - pf = MRP(0., -0.0366076, 0.) # final orientation (upside down) + p0 = MRP(0.997156, 0.0, 0.075366) # initial orientation + pf = MRP(0.0, -0.0366076, 0.0) # final orientation (upside down) - x0 = RD.build_state(model, [-3,0,1.5], p0, [5,0,0], [0,0,0]) - utrim = @SVector [41.6666, 106, 74.6519, 106] - xf = RD.build_state(model, [3,0,1.5], pf, [5,0,0], [0,0,0]) + x0 = RD.build_state(model, [-3, 0, 1.5], p0, [5, 0, 0], [0, 0, 0]) + utrim = @SVector [41.6666, 106, 74.6519, 106] + xf = RD.build_state(model, [3, 0, 1.5], pf, [5, 0, 0], [0, 0, 0]) # Xref trajectory x̄0 = RBState(model, x0) x̄f = RBState(model, xf) Xref = map(1:N) do k - x̄0 + (x̄f - x̄0)*((k-1)/(N-1)) + x̄0 + (x̄f - x̄0) * ((k - 1) / (N - 1)) end # Objective - Qf_diag = RD.fill_state(model, 100, 500, 100, 100.) + Qf_diag = RD.fill_state(model, 100, 500, 100, 100.0) Q_diag = RD.fill_state(model, 0.1, 0.1, 0.1, 0.1) Qf = Diagonal(Qf_diag) Q = Diagonal(Q_diag) - R = Diagonal(@SVector fill(1e-3,4)) + R = Diagonal(@SVector fill(1e-3, 4)) if quatnorm == :slack m += 1 R = Diagonal(push(R.diag, 1e-6)) @@ -60,42 +60,42 @@ function YakProblems(; end if costfun == :Quadratic costfuns = map(Xref) do xref - LQRCost(Q*dt, R*dt, xf, utrim) + LQRCost(Q * dt, R * dt, xf, utrim) end - costfun = LQRCost(Q*dt, R*dt, xf, utrim) + costfun = LQRCost(Q * dt, R * dt, xf, utrim) costterm = LQRCost(Qf, R, xf, utrim) costfuns[end] = costterm elseif costfun == :QuatLQR costfuns = map(Xref) do xref - QuatLQRCost(Q*dt, R*dt, xf, utrim, w=0.1) + QuatLQRCost(Q * dt, R * dt, xf, utrim, w=0.1) end - costfun = QuatLQRCost(Q*dt, R*dt, xf, utrim; w=0.1) + costfun = QuatLQRCost(Q * dt, R * dt, xf, utrim; w=0.1) costterm = QuatLQRCost(Qf, R, xf, utrim; w=200.0) costfuns[end] = costterm elseif costfun == :LieLQR - costfun = LieLQR(s, Q*dt, R*dt, xf, utrim) - costterm = LieLQR(s, Qf, R*dt, xf, utrim) + costfun = LieLQR(s, Q * dt, R * dt, xf, utrim) + costterm = LieLQR(s, Qf, R * dt, xf, utrim) elseif costfun == :ErrorQuadratic costfuns = map(Xref) do xref - ErrorQuadratic(model, Q*dt, R*dt, xref, utrim) + ErrorQuadratic(model, Q * dt, R * dt, xref, utrim) end - costfun = ErrorQuadratic(model, Q*dt, R*dt, xf, utrim) - costterm = ErrorQuadratic(model, Qf*10, R, xf, utrim) + costfun = ErrorQuadratic(model, Q * dt, R * dt, xf, utrim) + costterm = ErrorQuadratic(model, Qf * 10, R, xf, utrim) costfuns[end] = costterm end obj = Objective(costfuns) # Constraints - conSet = ConstraintList(n,m,N) - vecgoal = GoalConstraint(xf, vinds) + conSet = ConstraintList(n, m, N) + vecgoal = GoalConstraint(xf, vinds) rot_diffmethod = RD.UserDefined() if termcon == :goal - rotgoal = GoalConstraint(xf, SA[4,5,6,7]) + rotgoal = GoalConstraint(xf, SA[4, 5, 6, 7]) elseif termcon == :quatvec rotgoal = QuatVecEq(n, m, pf) rot_diffmethod = RD.ForwardAD() elseif termcon == :quaterr - rotgoal = QuatErr(n, UnitQuaternion(pf), SA[4,5,6,7]) + rotgoal = QuatErr(n, QuatRotation(pf), SA[4, 5, 6, 7]) else throw(ArgumentError("$termcon is not a known option for termcon. Options are :goal, :quatvec, :quaterr")) end @@ -110,7 +110,7 @@ function YakProblems(; U0 = [copy(utrim) for k = 1:N-1] # Use a standard model (no special handling of rotation states) - if quatnorm == :renorm + if quatnorm == :renorm model = QuatRenorm(model) elseif quatnorm == :slack model = QuatSlackModel(model) diff --git a/problems/quadrotor.jl b/problems/quadrotor.jl index 068feb4..7decb29 100644 --- a/problems/quadrotor.jl +++ b/problems/quadrotor.jl @@ -1,49 +1,49 @@ -function Quadrotor(scenario=:zigzag, Rot=UnitQuaternion{Float64}; - costfun=:Quadratic, normcon=false) +function Quadrotor(scenario=:zigzag, Rot=QuatRotation{Float64}; + costfun=:Quadratic, normcon=false) if scenario == :zigzag model = RobotZoo.Quadrotor{Rot}() - n,m = RD.dims(model) + n, m = RD.dims(model) opts = SolverOptions( - penalty_scaling=100., + penalty_scaling=100.0, penalty_initial=0.1, ) # discretization N = 101 # number of knot points tf = 5.0 - dt = tf/(N-1) # total time + dt = tf / (N - 1) # total time # Initial condition - x0_pos = @SVector [0., -10., 1.] - x0 = RobotDynamics.build_state(model, x0_pos, UnitQuaternion(I), zeros(3), zeros(3)) + x0_pos = @SVector [0.0, -10.0, 1.0] + x0 = RobotDynamics.build_state(model, x0_pos, QuatRotation(I), zeros(3), zeros(3)) # cost costfun == :QuatLQR ? sq = 0 : sq = 1 - rm_quat = @SVector [1,2,3,4,5,6,8,9,10,11,12,13] - Q_diag = Dynamics.fill_state(model, 1e-5, 1e-5*sq, 1e-3, 1e-3) + rm_quat = @SVector [1, 2, 3, 4, 5, 6, 8, 9, 10, 11, 12, 13] + Q_diag = Dynamics.fill_state(model, 1e-5, 1e-5 * sq, 1e-3, 1e-3) Q = Diagonal(Q_diag) - R = Diagonal(@SVector fill(1e-4,m)) - q_nom = UnitQuaternion(I) + R = Diagonal(@SVector fill(1e-4, m)) + q_nom = QuatRotation(I) v_nom, ω_nom = zeros(3), zeros(3) x_nom = Dynamics.build_state(model, zeros(3), q_nom, v_nom, ω_nom) if costfun == :QuatLQR - cost_nom = QuatLQRCost(Q*dt, R*dt, x_nom, w=0.0) + cost_nom = QuatLQRCost(Q * dt, R * dt, x_nom, w=0.0) elseif costfun == :ErrorQuad - cost_nom = ErrorQuadratic(model, Diagonal(Q_diag[rm_quat])*dt, R*dt, x_nom) + cost_nom = ErrorQuadratic(model, Diagonal(Q_diag[rm_quat]) * dt, R * dt, x_nom) else - cost_nom = LQRCost(Q*dt, R*dt, x_nom) + cost_nom = LQRCost(Q * dt, R * dt, x_nom) end # waypoints - wpts = [(@SVector [10,0,1.]), - (@SVector [-10,0,1.]), - (@SVector [0,10,1.])] + wpts = [(@SVector [10, 0, 1.0]), + (@SVector [-10, 0, 1.0]), + (@SVector [0, 10, 1.0])] times = [33, 66, 101] - Qw_diag = Dynamics.fill_state(model, 1e3,1*sq,1,1) - Qf_diag = Dynamics.fill_state(model, 10., 100*sq, 10, 10) - xf = Dynamics.build_state(model, wpts[end], UnitQuaternion(I), zeros(3), zeros(3)) + Qw_diag = Dynamics.fill_state(model, 1e3, 1 * sq, 1, 1) + Qf_diag = Dynamics.fill_state(model, 10.0, 100 * sq, 10, 10) + xf = Dynamics.build_state(model, wpts[end], QuatRotation(I), zeros(3), zeros(3)) costs = map(1:length(wpts)) do i r = wpts[i] @@ -52,11 +52,11 @@ function Quadrotor(scenario=:zigzag, Rot=UnitQuaternion{Float64}; Q = Diagonal(Qf_diag) w = 40.0 else - Q = Diagonal(1e-3*Qw_diag) * dt + Q = Diagonal(1e-3 * Qw_diag) * dt w = 0.1 end if costfun == :QuatLQR - QuatLQRCost(Q, R*dt, xg, w=w) + QuatLQRCost(Q, R * dt, xg, w=w) elseif costfun == :ErrorQuad Qd = diag(Q) ErrorQuadratic(model, Diagonal(Qd[rm_quat]), R, xg) @@ -66,7 +66,7 @@ function Quadrotor(scenario=:zigzag, Rot=UnitQuaternion{Float64}; end costs_all = map(1:N) do k - i = findfirst(x->(x ≥ k), times) + i = findfirst(x -> (x ≥ k), times) if k ∈ times costs[i] else @@ -77,20 +77,20 @@ function Quadrotor(scenario=:zigzag, Rot=UnitQuaternion{Float64}; obj = Objective(costs_all) # Initialization - u0 = @SVector fill(0.5*9.81/4, m) + u0 = @SVector fill(0.5 * 9.81 / 4, m) U_hover = [copy(u0) for k = 1:N-1] # initial hovering control trajectory # Constraints - conSet = ConstraintList(n,m,N) + conSet = ConstraintList(n, m, N) if normcon if use_rot == :slack add_constraint!(conSet, QuatSlackConstraint(), 1:N-1) else add_constraint!(conSet, QuatNormConstraint(), 1:N-1) - u0 = [u0; (@SVector [1.])] + u0 = [u0; (@SVector [1.0])] end end - bnd = BoundConstraint(n,m, u_min=0.0, u_max=12.0) + bnd = BoundConstraint(n, m, u_min=0.0, u_max=12.0) add_constraint!(conSet, bnd, 1:N-1) # Problem diff --git a/problems/quadrotor_maze.jl b/problems/quadrotor_maze.jl index 2257f67..739b49d 100644 --- a/problems/quadrotor_maze.jl +++ b/problems/quadrotor_maze.jl @@ -1,162 +1,162 @@ # Quadrotor in Maze function QuadrotorMaze(::Type{Rot}; use_rot=true, costfun=:Quadratic, - normcon=false) where Rot<:Rotation + normcon=false) where {Rot<:Rotation} -# model -model = Dynamics.Quadrotor2{Rot}(use_rot=use_rot) -n,m = size(model) + # model + model = Dynamics.Quadrotor2{Rot}(use_rot=use_rot) + n, m = size(model) -N = 101 # number of knot points -tf = 5.0 -dt = tf/(N-1) # total time + N = 101 # number of knot points + tf = 5.0 + dt = tf / (N - 1) # total time -x0 = Dynamics.build_state(model, [0,-15,1], I(UnitQuaternion), zeros(3), zeros(3)) -xf = Dynamics.build_state(model, [0, 15,1], I(UnitQuaternion), zeros(3), zeros(3)) -utrim = Dynamics.trim_controls(model) + x0 = Dynamics.build_state(model, [0, -15, 1], I(QuatRotation), zeros(3), zeros(3)) + xf = Dynamics.build_state(model, [0, 15, 1], I(QuatRotation), zeros(3), zeros(3)) + utrim = Dynamics.trim_controls(model) -# cost -if n == 13 - rm_quat = @SVector [1,2,3,4,5,6,8,9,10,11,12,13] -else - rm_quat = @SVector [1,2,3,4,5,6,7,8,9,10,11,12] -end -costfun == :QuatLQR ? sq = 0 : sq = 1 -Q_diag = Dynamics.fill_state(model, 1e-3, 1e-2*sq, 1e-3, 1e-3) -R_diag = @SVector fill(1e-4,m) -Q = Diagonal(Q_diag) -R = Diagonal(R_diag) -Qf = Diagonal(@SVector fill(1e3,n)) - -if costfun == :Quadratic - cost = LQRCost(Q*dt, R*dt, xf) - obj = Objective(cost, N) - # obj = LQRObjective(Q, R, Qf, xf, N) # objective with same stagewise costs -elseif costfun == :QuatLQR - cost = QuatLQRCost(Q*dt, R*dt, xf, w=1e-1) - cost_term = QuatLQRCost(Qf, R, xf, w=1.0) - obj = Objective(cost, cost_term, N) -elseif costfun == :ErrorQuad - cost = ErrorQuadratic(model, Diagonal(Q_diag[rm_quat])*dt, R*dt, xf) - cost_term = ErrorQuadratic(model, Diagonal(diag(Qf)[rm_quat]), R, xf) - obj = Objective(cost, cost_term, N) -end - -# constraints -r_quad_maze = 1.0 -r_cylinder_maze = 1.2 -maze_cylinders = [] -zh = 3 -l1 = 5 -l2 = 4 -l3 = 7 -l4 = 10 - -d = 3 # 0.5 door width -w = 10 # y location of wall -mid = 3 # 0.5 middle width - -x_enter=-10 -x_mid=0 -x_exit=10 - -for i = range(-w,stop=-d,length=l1) # enter wall - push!(maze_cylinders,(i, x_enter,r_cylinder_maze)) -end - -for i = range(d,stop=w,length=l1) # enter wall - push!(maze_cylinders,(i, x_enter, r_cylinder_maze)) -end - -for i = range(-mid,stop=mid,length=l3) # middle obstacle - push!(maze_cylinders,(i, x_mid, r_cylinder_maze)) -end - -for i = range(-w,stop=-d,length=l1) # exit wall - push!(maze_cylinders,(i, x_exit, r_cylinder_maze)) -end + # cost + if n == 13 + rm_quat = @SVector [1, 2, 3, 4, 5, 6, 8, 9, 10, 11, 12, 13] + else + rm_quat = @SVector [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12] + end + costfun == :QuatLQR ? sq = 0 : sq = 1 + Q_diag = Dynamics.fill_state(model, 1e-3, 1e-2 * sq, 1e-3, 1e-3) + R_diag = @SVector fill(1e-4, m) + Q = Diagonal(Q_diag) + R = Diagonal(R_diag) + Qf = Diagonal(@SVector fill(1e3, n)) + + if costfun == :Quadratic + cost = LQRCost(Q * dt, R * dt, xf) + obj = Objective(cost, N) + # obj = LQRObjective(Q, R, Qf, xf, N) # objective with same stagewise costs + elseif costfun == :QuatLQR + cost = QuatLQRCost(Q * dt, R * dt, xf, w=1e-1) + cost_term = QuatLQRCost(Qf, R, xf, w=1.0) + obj = Objective(cost, cost_term, N) + elseif costfun == :ErrorQuad + cost = ErrorQuadratic(model, Diagonal(Q_diag[rm_quat]) * dt, R * dt, xf) + cost_term = ErrorQuadratic(model, Diagonal(diag(Qf)[rm_quat]), R, xf) + obj = Objective(cost, cost_term, N) + end -for i = range(d,stop=w,length=l1) # exit wall - push!(maze_cylinders,(i, x_exit, r_cylinder_maze)) -end + # constraints + r_quad_maze = 1.0 + r_cylinder_maze = 1.2 + maze_cylinders = [] + zh = 3 + l1 = 5 + l2 = 4 + l3 = 7 + l4 = 10 + + d = 3 # 0.5 door width + w = 10 # y location of wall + mid = 3 # 0.5 middle width + + x_enter = -10 + x_mid = 0 + x_exit = 10 + + for i = range(-w, stop=-d, length=l1) # enter wall + push!(maze_cylinders, (i, x_enter, r_cylinder_maze)) + end -# Top and bottom walls -for i = range(x_enter+2*r_cylinder_maze,stop=x_exit-2*r_cylinder_maze,length=l4) - push!(maze_cylinders,(-w, i, r_cylinder_maze)) -end + for i = range(d, stop=w, length=l1) # enter wall + push!(maze_cylinders, (i, x_enter, r_cylinder_maze)) + end -for i = range(x_enter+2*r_cylinder_maze,stop=x_exit-2*r_cylinder_maze,length=l4) - push!(maze_cylinders,(w, i, r_cylinder_maze)) -end + for i = range(-mid, stop=mid, length=l3) # middle obstacle + push!(maze_cylinders, (i, x_mid, r_cylinder_maze)) + end -n_maze_cylinders = length(maze_cylinders) -maze_xyr = collect(zip(maze_cylinders...)) + for i = range(-w, stop=-d, length=l1) # exit wall + push!(maze_cylinders, (i, x_exit, r_cylinder_maze)) + end -cx = SVector{n_maze_cylinders}(maze_xyr[1]) -cy = SVector{n_maze_cylinders}(maze_xyr[2]) -cr = SVector{n_maze_cylinders}(maze_xyr[3]) + for i = range(d, stop=w, length=l1) # exit wall + push!(maze_cylinders, (i, x_exit, r_cylinder_maze)) + end -obs = CircleConstraint(n, cx, cy, cr .+ r_quad_maze) + # Top and bottom walls + for i = range(x_enter + 2 * r_cylinder_maze, stop=x_exit - 2 * r_cylinder_maze, length=l4) + push!(maze_cylinders, (-w, i, r_cylinder_maze)) + end -u_min = 0. -u_max = 50. -x_max = Inf*ones(n) -x_min = -Inf*ones(n) + for i = range(x_enter + 2 * r_cylinder_maze, stop=x_exit - 2 * r_cylinder_maze, length=l4) + push!(maze_cylinders, (w, i, r_cylinder_maze)) + end + n_maze_cylinders = length(maze_cylinders) + maze_xyr = collect(zip(maze_cylinders...)) -x_max[1:3] = [25.0; Inf; 20] -x_min[1:3] = [-25.0; -Inf; 0.] + cx = SVector{n_maze_cylinders}(maze_xyr[1]) + cy = SVector{n_maze_cylinders}(maze_xyr[2]) + cr = SVector{n_maze_cylinders}(maze_xyr[3]) -if n == 13 - noquat = @SVector [1,2,3,8,9,10,11,12,13] - x_max[8:10] .= 30. -else - noquat = @SVector [1,2,3,7,8,9,10,11,12] - x_max[7:9] .= 30. -end + obs = CircleConstraint(n, cx, cy, cr .+ r_quad_maze) -bnd1 = BoundConstraint(n,m,u_min=u_min,u_max=u_max) -bnd2 = BoundConstraint(n,m,u_min=u_min,u_max=u_max,x_min=x_min,x_max=x_max) -goal = GoalConstraint(xf, noquat) + u_min = 0.0 + u_max = 50.0 + x_max = Inf * ones(n) + x_min = -Inf * ones(n) -u0 = @SVector fill(0.5*9.81/4.0, m) -U_hover = [copy(u0) for k = 1:N-1] # initial hovering control trajectory -conSet = ConstraintSet(n,m,N) -add_constraint!(conSet, obs, 1:N-1) -add_constraint!(conSet, bnd2, 2:N-1) -add_constraint!(conSet, goal, N:N) + x_max[1:3] = [25.0; Inf; 20] + x_min[1:3] = [-25.0; -Inf; 0.0] -# Quaternion norm constraint -if normcon - if use_rot == :slack - add_constraint!(conSet, QuatSlackConstraint(), 1:N-1) + if n == 13 + noquat = @SVector [1, 2, 3, 8, 9, 10, 11, 12, 13] + x_max[8:10] .= 30.0 else - add_constraint!(conSet, QuatNormConstraint(), 1:N-1) - u0 = [u0; (@SVector [1.])] + noquat = @SVector [1, 2, 3, 7, 8, 9, 10, 11, 12] + x_max[7:9] .= 30.0 end -end -quadrotor_maze = Problem(model, obj, x0, tf, xf=xf, constraints=conSet) -initial_controls!(quadrotor_maze,U_hover); # initialize problem with controls + bnd1 = BoundConstraint(n, m, u_min=u_min, u_max=u_max) + bnd2 = BoundConstraint(n, m, u_min=u_min, u_max=u_max, x_min=x_min, x_max=x_max) + goal = GoalConstraint(xf, noquat) + + u0 = @SVector fill(0.5 * 9.81 / 4.0, m) + U_hover = [copy(u0) for k = 1:N-1] # initial hovering control trajectory + + conSet = ConstraintSet(n, m, N) + add_constraint!(conSet, obs, 1:N-1) + add_constraint!(conSet, bnd2, 2:N-1) + add_constraint!(conSet, goal, N:N) + + # Quaternion norm constraint + if normcon + if use_rot == :slack + add_constraint!(conSet, QuatSlackConstraint(), 1:N-1) + else + add_constraint!(conSet, QuatNormConstraint(), 1:N-1) + u0 = [u0; (@SVector [1.0])] + end + end -X_guess = zeros(n,7) -X_guess[:,1] = x0 -X_guess[:,7] = xf -wpts = [2 -7 1; + quadrotor_maze = Problem(model, obj, x0, tf, xf=xf, constraints=conSet) + initial_controls!(quadrotor_maze, U_hover) # initialize problem with controls + + X_guess = zeros(n, 7) + X_guess[:, 1] = x0 + X_guess[:, 7] = xf + wpts = [2 -7 1; 5 -5 1; - 7 0 1; - 5 5 1; - 2 7 1;] -X_guess[1:3,2:6] .= wpts' + 7 0 1; + 5 5 1; + 2 7 1] + X_guess[1:3, 2:6] .= wpts' -if n == 13 - X_guess[4:7,:] .= q0 -end -X0 = interp_rows(N,tf,X_guess); -initial_states!(quadrotor_maze, X0) + if n == 13 + X_guess[4:7, :] .= q0 + end + X0 = interp_rows(N, tf, X_guess) + initial_states!(quadrotor_maze, X0) -quadrotor_maze_objects = (cx,cy,cr) + quadrotor_maze_objects = (cx, cy, cr) -return quadrotor_maze + return quadrotor_maze end diff --git a/test/Project.toml b/test/Project.toml index ce89229..e724691 100644 --- a/test/Project.toml +++ b/test/Project.toml @@ -7,9 +7,7 @@ JLD2 = "033835bb-8acc-5ee8-8aae-3f567f8a3819" LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" Octavian = "6fd5a793-0b7e-452c-907f-f8bfe9c57db4" Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c" -RobotDynamics = "38ceca67-d8d3-44e8-9852-78a5596522e1" RobotZoo = "74be38bb-dcc2-4b9e-baf3-d6373cd95f10" SparseArrays = "2f01184e-e22b-5df5-ae63-d93ebab69eaf" StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40" -TrajectoryOptimization = "c79d492b-0548-5874-b488-5a62c1d9d0ca" diff --git a/test/ilqr_test.jl b/test/ilqr_test.jl index 176985e..6c02797 100644 --- a/test/ilqr_test.jl +++ b/test/ilqr_test.jl @@ -22,18 +22,20 @@ function ilqrallocs(solver) grad = Altro.gradient!(solver) # Record the iteration - Altro.record_iteration!(solver, Jnew, dJ, grad) + Altro.record_iteration!(solver, Jnew, dJ, grad) # Check convergence exit = Altro.evaluate_convergence(solver) return allocs end -@testset "iLQR Solver Allocations" begin -solver = Altro.iLQRSolver(Problems.Cartpole()..., use_static=Val(true)) -ilqrallocs(solver) -@test ilqrallocs(solver) == 0 -solver = Altro.iLQRSolver(Problems.Cartpole()..., use_static=Val(false)) -ilqrallocs(solver) -@test ilqrallocs(solver) == 0 +if TEST_ALLOCS + @testset "iLQR Solver Allocations" begin + solver = Altro.iLQRSolver(Problems.Cartpole()..., use_static=Val(true)) + ilqrallocs(solver) + @test ilqrallocs(solver) == 0 + solver = Altro.iLQRSolver(Problems.Cartpole()..., use_static=Val(false)) + ilqrallocs(solver) + @test ilqrallocs(solver) == 0 + end end \ No newline at end of file diff --git a/test/projected_newton_test.jl b/test/projected_newton_test.jl index ab1420b..51a90e6 100644 --- a/test/projected_newton_test.jl +++ b/test/projected_newton_test.jl @@ -5,7 +5,7 @@ using Test using BenchmarkTools using SparseArrays using LinearAlgebra -using RobotDynamics: KnotPoint, SampledTrajectory +using RobotDynamics: KnotPoint, SampledTrajectory using SparseArrays using Altro.Cqdldl const TO = TrajectoryOptimization @@ -33,23 +33,23 @@ copyto!(pn.Z̄data, pn.Zdata) @test pn.Z̄ ≈ Zsol # Check sizes -nx,nu = RD.dims(prob) -N = TO.horizonlength(prob) +nx, nu = RD.dims(prob) +N = TO.horizonlength(prob) Np = Altro.num_primals(pn) @test Np == sum(nx) + sum(nu) -@test size(pn.hess[end]) == (nx[end]+nu[end], nx[end]+nu[end]) -@test size(pn.grad[end]) == (nx[end]+nu[end],) +@test size(pn.hess[end]) == (nx[end] + nu[end], nx[end] + nu[end]) +@test size(pn.grad[end]) == (nx[end] + nu[end],) # Check Hessian and regularization Altro.cost_hessian!(pn) iz = pn.iz -@test pn.Atop[iz[end], iz[end]] ≈ cat(prob.obj[end].Q, prob.obj[end].R*0, dims=(1,2)) -pn.Atop[iz[end],iz[end]] +@test pn.Atop[iz[end], iz[end]] ≈ cat(prob.obj[end].Q, prob.obj[end].R * 0, dims=(1, 2)) +pn.Atop[iz[end], iz[end]] Altro.primalregularization!(pn) ρ_primal = opts.ρ_primal -@test pn.Atop[iz[end], iz[end]] ≈ - cat(prob.obj[end].Q, prob.obj[end].R*0, dims=(1,2)) + I * ρ_primal +@test pn.Atop[iz[end], iz[end]] ≈ + cat(prob.obj[end].Q, prob.obj[end].R * 0, dims=(1, 2)) + I * ρ_primal # Compute the factorization Altro.evaluate_constraints!(pn) @@ -57,7 +57,7 @@ Altro.constraint_jacobians!(pn) Altro.update_active_set!(pn) A = Altro.getKKTMatrix(pn) -resize!(pn.qdldl, size(A,1)) +resize!(pn.qdldl, size(A, 1)) Cqdldl.eliminationtree!(pn.qdldl, A) Cqdldl.factor!(pn.qdldl) F = Cqdldl.QDLDLFactorization(pn.qdldl) @@ -104,36 +104,39 @@ viol = solve!(pn) bpn = @benchmark let pn = $pn copyto!(pn.Zdata, $Z0data) solve!(pn) -end samples=1 evals=1 -@test bpn.allocs == 0 +end samples = 1 evals = 1 + +if TEST_ALLOCS + @test bpn.allocs == 0 +end ################################# ## Basic tests ################################# # Create a trajectory with views into a single array -nx,nu = RD.dims(prob) +nx, nu = RD.dims(prob) N = horizonlength(prob) h = 0.1 -NN = N*nx[1] + (N-1)*nu[1] +NN = N * nx[1] + (N - 1) * nu[1] Zdata = zeros(NN + nu[1]) -n,m = nx[1], nu[1] -ix = [(1:n) .+ (k-1)*(n+m) for k = 1:N] -iu = [n .+ (1:m) .+ (k-1)*(n+m) for k = 1:N-1] -iz = [(1:n+m) .+ (k-1)*(n+m) for k = 1:N] -Z = SampledTrajectory([KnotPoint{n,m}(view(Zdata, iz[k]), (k-1)*h, h) for k = 1:N]) +n, m = nx[1], nu[1] +ix = [(1:n) .+ (k - 1) * (n + m) for k = 1:N] +iu = [n .+ (1:m) .+ (k - 1) * (n + m) for k = 1:N-1] +iz = [(1:n+m) .+ (k - 1) * (n + m) for k = 1:N] +Z = SampledTrajectory([KnotPoint{n,m}(view(Zdata, iz[k]), (k - 1) * h, h) for k = 1:N]) Z[end].dt = 0.0 RD.state(Z[2]) .= 1 RD.control(Z[3]) .= 2.1 -@test Zdata[ix[2]] ≈ fill(1,n) -@test Zdata[iu[3]] ≈ fill(2.1,m) +@test Zdata[ix[2]] ≈ fill(1, n) +@test Zdata[iu[3]] ≈ fill(2.1, m) Nc = sum(TO.num_constraints(prob)) # number of stage constraints -Np = N*n + (N-1)*m # number of primals -Nd = N*n + Nc # number of duals (no constraints) +Np = N * n + (N - 1) * m # number of primals +Nd = N * n + Nc # number of duals (no constraints) ## Create PNConstraintSet A = spzeros(Np + Nd, Np + Nd) -b = zeros(Np + Nd) +b = zeros(Np + Nd) a = trues(Nd) # Add Hessian blocks @@ -165,13 +168,13 @@ end Altro.constraint_jacobians!(pnconset, Z) Altro.constraint_jacobians!(alconset, prob.Z) -for i = 1:length(alconset) - 1 +for i = 1:length(alconset)-1 @test pnconset[i].jac ≈ alconset[i].jac end # Create a PN Solver al = Altro.ALSolver(prob) -ilqr = al.ilqr +ilqr = al.ilqr pn = Altro.ProjectedNewtonSolver(prob) Np = Altro.num_primals(pn) diff --git a/test/runtests.jl b/test/runtests.jl index dbd381a..0e56b88 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -16,6 +16,8 @@ const TO = TrajectoryOptimization TEST_TIME = false Random.seed!(1) +TEST_ALLOCS = false + ## # @testset "Initialization" begin # include("large_init.jl") @@ -33,7 +35,7 @@ end include("quadrotor.jl") include("escape_solve.jl") include("rocket_test.jl") - include(joinpath(@__DIR__,"..","examples","quickstart.jl")) + include(joinpath(@__DIR__, "..", "examples", "quickstart.jl")) end @testset "Solvers" begin @@ -57,7 +59,7 @@ end @testset "Quickstart" begin include(joinpath(@__DIR__, "..", "examples", "quickstart.jl")) - @test max_violation(altro) ≈ 5.8962e-7 rtol=1e-4 + @test max_violation(altro) ≈ 5.8962e-7 rtol = 1e-4 @test cost(altro) ≈ 1.539 rtol = 1e-4 @test iterations(altro) == 44 end From 16aa3c3cbd0064453ebe521198706d2eb5b77bf2 Mon Sep 17 00:00:00 2001 From: Brian Jackson Date: Tue, 4 Mar 2025 10:03:36 -0500 Subject: [PATCH 2/4] Allow latest Interpolations --- Project.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Project.toml b/Project.toml index c4313d8..da47ec3 100644 --- a/Project.toml +++ b/Project.toml @@ -30,7 +30,7 @@ Crayons = "4" FiniteDiff = "2" Formatting = "0.4" ForwardDiff = "0.10" -Interpolations = "0.12,0.13" +Interpolations = "0.12,0.13,0.14" Octavian = "0.3" RobotDynamics = "0.4" RobotZoo = "0.3" From 4063b644bee469c1fc5e7c4876820ee2c4303a69 Mon Sep 17 00:00:00 2001 From: Brian Jackson Date: Tue, 4 Mar 2025 10:05:13 -0500 Subject: [PATCH 3/4] Up version to 0.5.1 --- Project.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Project.toml b/Project.toml index da47ec3..c080d8a 100644 --- a/Project.toml +++ b/Project.toml @@ -1,7 +1,7 @@ name = "Altro" uuid = "5dcf52e5-e2fb-48e0-b826-96f46d2e3e73" authors = ["Brian Jackson "] -version = "0.5.0" +version = "0.5.1" [deps] BenchmarkTools = "6e4b80f9-dd63-53aa-95a3-0cdb28fa8baf" From 1f95627a9a9e20178a5ba845c49b6da596db5fc4 Mon Sep 17 00:00:00 2001 From: Brian Jackson Date: Thu, 6 Mar 2025 13:04:21 -0500 Subject: [PATCH 4/4] Up versions of TO and RD --- Project.toml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Project.toml b/Project.toml index c080d8a..3133298 100644 --- a/Project.toml +++ b/Project.toml @@ -32,11 +32,11 @@ Formatting = "0.4" ForwardDiff = "0.10" Interpolations = "0.12,0.13,0.14" Octavian = "0.3" -RobotDynamics = "0.4" +RobotDynamics = "0.4.8" RobotZoo = "0.3" Rotations = "1" SolverLogging = "0.2" StaticArrays = "1" TimerOutputs = "0.5" -TrajectoryOptimization = "0.7" +TrajectoryOptimization = "0.7.1" julia = "1"