From 20f726ebf168107305b36c7c551029e031ba24d3 Mon Sep 17 00:00:00 2001 From: Zac Manchester Date: Sat, 11 Jul 2020 17:47:09 -0700 Subject: [PATCH 01/16] fixed quickstart --- examples/quickstart.jl | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/examples/quickstart.jl b/examples/quickstart.jl index ba17906f..531f9490 100644 --- a/examples/quickstart.jl +++ b/examples/quickstart.jl @@ -1,13 +1,16 @@ using TrajectoryOptimization +using Altro +using RobotDynamics using StaticArrays using LinearAlgebra const TO = TrajectoryOptimization +const RD = RobotDynamics -struct DoubleIntegrator{T} <: AbstractModel +struct DoubleIntegrator{T} <: TO.AbstractModel mass::T end -function TO.dynamics(model::DoubleIntegrator, x, u) +function RD.dynamics(model::DoubleIntegrator, x, u) SA[x[2], u[1] / model.mass] end @@ -28,7 +31,7 @@ R = Diagonal(@SVector ones(m)) obj = LQRObjective(Q, R, N*Q, xf, N) # Constraints -cons = TO.ConstraintSet(n,m,N) +cons = ConstraintList(n,m,N) add_constraint!(cons, GoalConstraint(xf), N:N) add_constraint!(cons, BoundConstraint(n,m, u_min=-10, u_max=10), 1:N-1) From 870180b7756d07a62a64984a7cd468d5b479d4ff Mon Sep 17 00:00:00 2001 From: Brian Jackson Date: Tue, 4 Aug 2020 16:39:09 -0400 Subject: [PATCH 02/16] Updates examples --- examples/Cartpole.ipynb | 4 +- examples/Project.toml | 2 + examples/cartpole.jl | 119 ---------------------------------------- examples/quickstart.jl | 8 ++- 4 files changed, 9 insertions(+), 124 deletions(-) create mode 100644 examples/Project.toml delete mode 100644 examples/cartpole.jl diff --git a/examples/Cartpole.ipynb b/examples/Cartpole.ipynb index 83ccc6ad..49782268 100644 --- a/examples/Cartpole.ipynb +++ b/examples/Cartpole.ipynb @@ -194,7 +194,7 @@ "metadata": {}, "source": [ "## Solving with ALTRO\n", - "Now that the problem is defined, we can use any of the supported solvers to solve the problem. Here we use [`ALTRO.jl`](https://github.com/RoboticExplorationLab/ALTRO.jl), a very fast specialized solver that uses iterative LQR (iLQR) within an augmented Lagrangrian framework to handle the constraints." + "Now that the problem is defined, we can use any of the supported solvers to solve the problem. Here we use [`Altro.jl`](https://github.com/RoboticExplorationLab/ALTRO.jl), a very fast specialized solver that uses iterative LQR (iLQR) within an augmented Lagrangrian framework to handle the constraints." ] }, { @@ -218,7 +218,7 @@ } ], "source": [ - "using ALTRO\n", + "using Altro\n", "opts = SolverOptions(\n", " cost_tolerance_intermediate=1e-2,\n", " penalty_scaling=10.,\n", diff --git a/examples/Project.toml b/examples/Project.toml new file mode 100644 index 00000000..cd262b58 --- /dev/null +++ b/examples/Project.toml @@ -0,0 +1,2 @@ +[deps] +RobotZoo = "74be38bb-dcc2-4b9e-baf3-d6373cd95f10" diff --git a/examples/cartpole.jl b/examples/cartpole.jl deleted file mode 100644 index bcc5bb24..00000000 --- a/examples/cartpole.jl +++ /dev/null @@ -1,119 +0,0 @@ -#--- Set up problem using TrajectoryOptimization.jl and RobotZoo.jl -using TrajectoryOptimization -using RobotDynamics -using ALTRO -import RobotZoo.Cartpole -using StaticArrays, LinearAlgebra - -# Use the Cartpole model from RobotZoo -model = Cartpole() -n,m = size(model) - -# Define model discretization -N = 101 -tf = 5. -dt = tf/(N-1) - -# Define initial and final conditions -x0 = @SVector 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) - -# Add constraints -conSet = ConstraintList(n,m,N) -u_bnd = 3.0 -bnd = BoundConstraint(n,m, u_min=-u_bnd, u_max=u_bnd) -goal = GoalConstraint(xf) -add_constraint!(conSet, bnd, 1:N-1) -add_constraint!(conSet, goal, N) - -# Initialization -u0 = @SVector fill(0.01,m) -U0 = [u0 for k = 1:N-1] - -# Define problem -prob = Problem(model, obj, xf, tf, x0=x0, constraints=conSet) -initial_controls!(prob, U0) -rollout!(prob) - -#--- Solve with ALTRO -opts = SolverOptions( - cost_tolerance_intermediate=1e-2, - penalty_scaling=10., - penalty_initial=1.0 -) -altro = ALTROSolver(prob, opts) -solve!(altro) - -# Get some info on the solve -max_violation(altro) # 3.42e-9 -cost(altro) # 1.55 -iterations(altro) # 40 - -# Extract the solution -X = states(altro) -U = controls(altro) -Z_altro = copy(get_trajectory(altro)) - -#--- Solve with iLQR (ignores constraints) -ilqr = iLQRSolver(prob, opts) -initial_controls!(ilqr, U0) # reset controls to initial guess, since they are modified by the previous solve -solve!(ilqr) -cost(ilqr) # 1.45 -iterations(ilqr) # 84 -Z_ilqr = copy(get_trajectory(ilqr)) - -#--- Solve using Ipopt -using Ipopt -using MathOptInterface -const MOI = MathOptInterface -prob_nlp = copy(prob) -TrajectoryOptimization.add_dynamics_constraints!(prob_nlp) -initial_controls!(prob_nlp, U0) -rollout!(prob_nlp) -prob_nlp.constraints[2] -nlp = TrajOptNLP(prob_nlp, remove_bounds=true, jac_type=:vector) -Z0 = copy(TrajectoryOptimization.get_trajectory(nlp).Z) - -optimizer = Ipopt.Optimizer(print_level=0) -TrajectoryOptimization.build_MOI!(nlp, optimizer) -Z = MOI.VariableIndex.(1:length(Z0)) -MOI.optimize!(optimizer) -MOI.set(optimizer, MOI.VariablePrimalStart(), Z, Z0) -MOI.get(optimizer, MOI.TerminationStatus()) - -Z_ipopt = copy(get_trajectory(nlp)) - - -#--- Visualize -using TrajOptPlots -using MeshCat -using Plots - -vis = Visualizer() -open(vis) - -TrajOptPlots.set_mesh!(vis, model) -visualize!(vis, altro) -visualize!(vis, nlp) - -visualize!(vis, model, get_trajectory(nlp)) -X1 = states(Z_altro) -X2 = X1 .+ [SA[0,-pi,0,0] for k in X1] -visualize!(vis, model, prob.tf, states(Z_altro), states(Z_ipopt)) -visualize!(vis, model, Z_altro, Z_ipopt) -visualize!(vis, altro, nlp) - -plot() -plot!(controls(Z_altro)) -plot!(controls(Z_ilqr)) -plot!(controls(Z_ipopt)) - -plot(states(Z_altro),2:2) -plot!(states(Z_ilqr),2:2) -plot!(states(Z_ipopt),2:2) diff --git a/examples/quickstart.jl b/examples/quickstart.jl index ba17906f..f49a2f6f 100644 --- a/examples/quickstart.jl +++ b/examples/quickstart.jl @@ -1,13 +1,15 @@ using TrajectoryOptimization using StaticArrays using LinearAlgebra +using RobotDynamics +using Altro const TO = TrajectoryOptimization struct DoubleIntegrator{T} <: AbstractModel mass::T end -function TO.dynamics(model::DoubleIntegrator, x, u) +function RobotDynamics.dynamics(model::DoubleIntegrator, x, u) SA[x[2], u[1] / model.mass] end @@ -28,8 +30,8 @@ R = Diagonal(@SVector ones(m)) obj = LQRObjective(Q, R, N*Q, xf, N) # Constraints -cons = TO.ConstraintSet(n,m,N) -add_constraint!(cons, GoalConstraint(xf), N:N) +cons = ConstraintList(n,m,N) +add_constraint!(cons, GoalConstraint(xf), N) add_constraint!(cons, BoundConstraint(n,m, u_min=-10, u_max=10), 1:N-1) # Create and solve problem From 1ca58aebd8c26932cff0277ee2ef6f74b199e0b9 Mon Sep 17 00:00:00 2001 From: Brian Jackson Date: Tue, 4 Aug 2020 16:39:28 -0400 Subject: [PATCH 03/16] Change ALTRO to Altro --- docs/src/index.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/src/index.md b/docs/src/index.md index 94d66d76..7ef82bc7 100644 --- a/docs/src/index.md +++ b/docs/src/index.md @@ -17,7 +17,7 @@ Importantly, this package should be considered more of a modeling framework than optimization solver, similar to [Convex.jl](https://github.com/JuliaOpt/Convex.jl). While general trajectory optimization problems are nonconvex, primarily due to the presence of nonlinear equality constraints imposed by the dynamics, they exhibit a unique -structure that allows purpose-built solvers such as [ALTRO.jl](https://github.com/bjack205/ALTRO.jl) +structure that allows purpose-built solvers such as [Altro.jl](https://github.com/bjack205/ALTRO.jl) to gain significant computational savings over the use of more generalized NLP solvers such as SNOPT and Ipopt. From 7fb9ea9412e6bd7ed4e7dfa0ec029e0dd5865105 Mon Sep 17 00:00:00 2001 From: Brian Jackson Date: Tue, 4 Aug 2020 16:41:39 -0400 Subject: [PATCH 04/16] Make init and goal states mutable for MPC --- src/constraints.jl | 15 ++++++++++++--- src/costfunctions.jl | 22 ++++++++++++++++++++-- src/problem.jl | 17 +++++++++++++++++ 3 files changed, 49 insertions(+), 5 deletions(-) diff --git a/src/constraints.jl b/src/constraints.jl index b380dfe1..fc25bb1d 100644 --- a/src/constraints.jl +++ b/src/constraints.jl @@ -32,20 +32,20 @@ only `xf[inds]` will be used. """ struct GoalConstraint{P,T} <: StateConstraint n::Int - xf::SVector{P,T} + xf::MVector{P,T} inds::SVector{P,Int} end function GoalConstraint(xf::AbstractVector, inds=1:length(xf)) n = length(xf) p = length(inds) - xf = SVector{n}(xf) + xf = MVector{n}(xf) inds = SVector{p}(inds) GoalConstraint(xf, inds) end function GoalConstraint(xf::SVector{n}, inds::SVector{p,Int}) where {n,p} - GoalConstraint(length(xf), SVector{p}(xf[inds]), inds) + GoalConstraint(length(xf), MVector{p}(xf[inds]), inds) end @inline sense(::GoalConstraint) = Equality() @@ -75,6 +75,15 @@ function change_dimension(con::GoalConstraint, n::Int, m::Int, xi=1:n, ui=1:m) GoalConstraint(n, con.xf, xi[con.inds]) end +function set_goal_state!(con::GoalConstraint, xf::AbstractVector) + if length(xf) != length(con.xf) + for (i,j) in enumerate(con.inds) + con.xf[i] = xf[j] + end + else + con.xf .= xf + end +end ############################################################################################ # LINEAR CONSTRAINTS # diff --git a/src/costfunctions.jl b/src/costfunctions.jl index d0e50b42..f80786d5 100644 --- a/src/costfunctions.jl +++ b/src/costfunctions.jl @@ -127,6 +127,24 @@ function Base.copy(c::QC) where QC<:QuadraticCostFunction end # Other methods +""" + set_LQR_goal!(cost::QuadraticCostFunction, xf) + set_LQR_goal!(cost::QuadraticCostFunction, xf, uf) + +Change the reference state and control for an LQR tracking cost. +Only changes `q` and `r`, and not the constant term `c`. If `uf` is +not passed in, it isn't changed. +""" +function set_LQR_goal!(cost::QuadraticCostFunction, xf) + cost.q .= -cost.Q * xf + return nothing +end + +function set_LQR_goal!(cost::QuadraticCostFunction, xf, uf) + set_LQR_goal!(cost, xf) + cost.r .= -cost.R * uf + return nothing +end function +(c1::QuadraticCostFunction, c2::QuadraticCostFunction) @assert state_dim(c1) == state_dim(c2) @@ -197,8 +215,8 @@ Any optional or omitted values will be set to zero(s). The keyword arguments are struct DiagonalCost{n,m,T} <: QuadraticCostFunction{n,m,T} Q::Diagonal{T,SVector{n,T}} R::Diagonal{T,SVector{m,T}} - q::SVector{n,T} - r::SVector{m,T} + q::MVector{n,T} + r::MVector{m,T} c::T terminal::Bool function DiagonalCost(Qd::StaticVector{n}, Rd::StaticVector{m}, diff --git a/src/problem.jl b/src/problem.jl index c1111b8a..f11a2b07 100644 --- a/src/problem.jl +++ b/src/problem.jl @@ -149,6 +149,23 @@ function set_initial_state!(prob::Problem, x0::AbstractVector) prob.x0 .= x0 end +function set_goal_state!(prob::Problem, xf::AbstractVector; objective=true, constraint=true) + if objective + obj = get_objective(prob) + for k in eachindex(obj.cost) + set_LQR_goal!(obj[k], xf) + end + end + if constraint + for con in get_constraints(prob) + if con isa GoalConstraint + set_goal_state!(con, xf) + end + end + end + return nothing +end + """ initial_controls!(::Problem, U0::Vector{<:AbstractVector}) initial_controls!(::Problem, U0::AbstractMatrx) From 1017318ad6ac31146394f0e1e0e772e6cfd2deab Mon Sep 17 00:00:00 2001 From: Brian Jackson Date: Tue, 4 Aug 2020 16:42:04 -0400 Subject: [PATCH 05/16] Clean up spacing --- test/moi_test.jl | 1 - test/nlp_tests.jl | 1 - 2 files changed, 2 deletions(-) diff --git a/test/moi_test.jl b/test/moi_test.jl index 6bbe34e7..bb77dbb2 100644 --- a/test/moi_test.jl +++ b/test/moi_test.jl @@ -1,5 +1,4 @@ using TrajectoryOptimization -# using ALTRO using Ipopt using MathOptInterface const MOI = MathOptInterface diff --git a/test/nlp_tests.jl b/test/nlp_tests.jl index a3a40809..3945044e 100644 --- a/test/nlp_tests.jl +++ b/test/nlp_tests.jl @@ -1,5 +1,4 @@ using TrajectoryOptimization -# using ALTRO using RobotDynamics using BenchmarkTools using ForwardDiff From 120bacfe64b7410b82862ee007d524cac061a9b1 Mon Sep 17 00:00:00 2001 From: Brian Jackson Date: Thu, 6 Aug 2020 15:50:45 -0400 Subject: [PATCH 06/16] Add initial state update --- src/problem.jl | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/src/problem.jl b/src/problem.jl index f11a2b07..27025747 100644 --- a/src/problem.jl +++ b/src/problem.jl @@ -149,6 +149,21 @@ function set_initial_state!(prob::Problem, x0::AbstractVector) prob.x0 .= x0 end +""" + set_initial_time!(prob, t0) + +Set the initial time of the optimization problem, shifting the time of all points in the trajectory. +Returns the updated final time. +""" +function set_initial_time!(prob::Problem, t0::Real) + Z = get_trajectory(prob) + Δt = t0 - Z[1].t + for k in eachindex(Z) + Z[k].t += Δt + end + return Z[end].t +end + function set_goal_state!(prob::Problem, xf::AbstractVector; objective=true, constraint=true) if objective obj = get_objective(prob) From 7c5ff35c7ffe6f6ed37dc57ca1f026ff2bcfd32d Mon Sep 17 00:00:00 2001 From: Brian Jackson Date: Fri, 7 Aug 2020 15:53:12 -0400 Subject: [PATCH 07/16] Fixes to eliminate allocs --- src/constraints.jl | 12 +++++------- src/costfunctions.jl | 10 ++++++---- test/constraint_tests.jl | 6 +++--- test/cost_tests.jl | 15 +++++++-------- test/runtests.jl | 1 + 5 files changed, 22 insertions(+), 22 deletions(-) diff --git a/src/constraints.jl b/src/constraints.jl index fc25bb1d..4427dcea 100644 --- a/src/constraints.jl +++ b/src/constraints.jl @@ -34,19 +34,17 @@ struct GoalConstraint{P,T} <: StateConstraint n::Int xf::MVector{P,T} inds::SVector{P,Int} + function GoalConstraint(xf::AbstractVector{T}, inds::SVector{p,Int}) where {p,T} + new{p,T}(length(xf), xf[inds], inds) + end end function GoalConstraint(xf::AbstractVector, inds=1:length(xf)) - n = length(xf) p = length(inds) - xf = MVector{n}(xf) inds = SVector{p}(inds) GoalConstraint(xf, inds) end -function GoalConstraint(xf::SVector{n}, inds::SVector{p,Int}) where {n,p} - GoalConstraint(length(xf), MVector{p}(xf[inds]), inds) -end @inline sense(::GoalConstraint) = Equality() @inline Base.length(con::GoalConstraint{P}) where P = P @@ -60,7 +58,7 @@ function primal_bounds!(zL,zU,con::GoalConstraint) return true end -evaluate(con::GoalConstraint, x::SVector) = x[con.inds] - con.xf +evaluate(con::GoalConstraint, x::StaticVector) = x[con.inds] - con.xf function jacobian!(∇c, con::GoalConstraint, z::KnotPoint) T = eltype(∇c) for (i,j) in enumerate(con.inds) @@ -72,7 +70,7 @@ end ∇jacobian!(G, con::GoalConstraint, z::AbstractKnotPoint, λ::AbstractVector) = true # zeros function change_dimension(con::GoalConstraint, n::Int, m::Int, xi=1:n, ui=1:m) - GoalConstraint(n, con.xf, xi[con.inds]) + GoalConstraint(con.xf, xi[con.inds]) end function set_goal_state!(con::GoalConstraint, xf::AbstractVector) diff --git a/src/costfunctions.jl b/src/costfunctions.jl index f80786d5..4fd1c7a9 100644 --- a/src/costfunctions.jl +++ b/src/costfunctions.jl @@ -57,7 +57,7 @@ Calculate the scalar cost using `costfun` given state `x` and control `u`. If on state is provided, it is assumed it is a terminal cost. """ function stage_cost(cost::QuadraticCostFunction, x::AbstractVector, u::AbstractVector) - J = 0.5*u'cost.R*u + cost.r'u + stage_cost(cost, x) + J = 0.5*u'cost.R*u + dot(cost.r,u) + stage_cost(cost, x) if !is_blockdiag(cost) J += u'cost.H*x end @@ -65,7 +65,7 @@ function stage_cost(cost::QuadraticCostFunction, x::AbstractVector, u::AbstractV end function stage_cost(cost::QuadraticCostFunction, x::AbstractVector{T}) where T - 0.5*x'cost.Q*x .+ cost.q'*x .+ cost.c + 0.5*x'cost.Q*x + dot(cost.q,x) + cost.c end """ @@ -230,7 +230,7 @@ struct DiagonalCost{n,m,T} <: QuadraticCostFunction{n,m,T} @warn "R needs to be positive definite." end end - new{n,m,T}(Diagonal(SVector(Qd)), Diagonal(SVector(Rd)), SVector(q), SVector(r), T(c), terminal) + new{n,m,T}(Diagonal(SVector(Qd)), Diagonal(SVector(Rd)), q, r, T(c), terminal) end end @@ -246,9 +246,11 @@ function DiagonalCost(Q::AbstractArray, R::AbstractArray, n,m = length(q), length(r) Qd = SVector{n}(diag(Q)) Rd = SVector{m}(diag(R)) - DiagonalCost(Qd, Rd, SVector{n}(q), SVector{m}(r), c; kwargs...) + DiagonalCost(Qd, Rd, convert(MVector{n},q), convert(MVector{m},r), c; kwargs...) end + +# Pass in vectors for Q and R function DiagonalCost(Q::AbstractVector, R::AbstractVector, q::AbstractVector, r::AbstractVector, c::Real; kwargs...) DiagonalCost(Diagonal(Q), Diagonal(R), q, r, c; kwargs...) diff --git a/test/constraint_tests.jl b/test/constraint_tests.jl index ee68d02c..6010f947 100644 --- a/test/constraint_tests.jl +++ b/test/constraint_tests.jl @@ -1,8 +1,8 @@ #--- Setup function alloc_con(con,z) ∇c = TO.gen_jacobian(con) - allocs = @allocated TO.evaluate(con, z) - allocs += @allocated TO.jacobian!(∇c, con,z) + allocs = @ballocated TO.evaluate($con, $z) samples=1 evals=1 + allocs += @ballocated TO.jacobian!($∇c, $con, $z) samples=1 evals=1 end model = Cartpole() @@ -31,7 +31,7 @@ z = KnotPoint(x,u,0.1) @test TO.widths(goal, n+1, m) == (n+1,) @test state_dim(goal) == n - @test GoalConstraint(Vector(xf)).xf isa SVector{n} + @test GoalConstraint(Vector(xf)).xf isa MVector{n} end diff --git a/test/cost_tests.jl b/test/cost_tests.jl index 0e755c74..030b8a86 100644 --- a/test/cost_tests.jl +++ b/test/cost_tests.jl @@ -4,13 +4,12 @@ Q = Diagonal(@SVector fill(0.1, n)) R = Diagonal(@SVector fill(0.01, m)) H = rand(m, n) - q = @SVector rand(n) - r = @SVector rand(m) + q = @MVector rand(n) + r = @MVector rand(m) c = rand() Qf = Diagonal(@SVector fill(10.0, n)) xf = @SVector ones(n) - @testset "Constructors" begin qcost = QuadraticCost(Q, R) @test qcost.Q == Q @@ -81,25 +80,25 @@ dcost = DiagonalCost(Q, R) @test dcost.Q === Q @test dcost.R === R - @test dcost.r === zero(r) - @test dcost.q === zero(q) + @test dcost.r == zero(r) + @test dcost.q == zero(q) dcost = DiagonalCost(Q, R, q = q) @test dcost.Q === Q @test dcost.R === R - @test dcost.r === zero(r) + @test dcost.r == zero(r) @test dcost.q === q dcost = DiagonalCost(Q.diag, R.diag, q = q) @test dcost.Q === Q @test dcost.R === R - @test dcost.r === zero(r) + @test dcost.r == zero(r) @test dcost.q === q dcost = DiagonalCost(Vector(Q.diag), Vector(R.diag), q = q) @test dcost.Q === Q @test dcost.R === R - @test dcost.r === zero(r) + @test dcost.r == zero(r) @test dcost.q === q @test dcost.Q isa Diagonal{Float64,<:SVector} diff --git a/test/runtests.jl b/test/runtests.jl index 0ec19c0d..3c0c8ce8 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -6,6 +6,7 @@ using StaticArrays using SparseArrays using ForwardDiff using RobotDynamics +using BenchmarkTools const TO = TrajectoryOptimization include("test_models.jl") From fc59addd63e4ca2d840ec6c767a15d43f8640ff7 Mon Sep 17 00:00:00 2001 From: Brian Jackson Date: Mon, 10 Aug 2020 13:44:11 -0400 Subject: [PATCH 08/16] Update copy methods for constraints --- src/constraint_list.jl | 14 +++++++++----- src/constraints.jl | 13 +++++++++++++ test/constraint_sets.jl | 5 +++++ 3 files changed, 27 insertions(+), 5 deletions(-) diff --git a/src/constraint_list.jl b/src/constraint_list.jl index 1605767f..d218b83a 100644 --- a/src/constraint_list.jl +++ b/src/constraint_list.jl @@ -136,14 +136,18 @@ Base.zip(cons::ConstraintList) = zip(cons.inds, cons.constraints) @inline Base.getindex(cons::ConstraintList, i::Int) = cons.constraints[i] -function Base.copy(cons::ConstraintList) - cons2 = ConstraintList(cons.n, cons.m, length(cons.p)) - for i in eachindex(cons.constraints) - add_constraint!(cons2, cons.constraints[i], copy(cons.inds[i])) +for method in (:deepcopy, :copy) + @eval function Base.$method(cons::ConstraintList) + cons2 = ConstraintList(cons.n, cons.m, length(cons.p)) + for i in eachindex(cons.constraints) + con_ = $(method == :deepcopy ? :(copy(cons.constraints[i])) : :(cons.constraints[i])) + add_constraint!(cons2, con_, copy(cons.inds[i])) + end + return cons2 end - return cons2 end + """ num_constraints(::ConstraintList) num_constraints(::Problem) diff --git a/src/constraints.jl b/src/constraints.jl index 4427dcea..75248cba 100644 --- a/src/constraints.jl +++ b/src/constraints.jl @@ -11,6 +11,7 @@ import RobotDynamics: state_dim, control_dim +Base.copy(con::AbstractConstraint) = con ############################################################################################ # GOAL CONSTRAINTS # @@ -45,6 +46,7 @@ function GoalConstraint(xf::AbstractVector, inds=1:length(xf)) GoalConstraint(xf, inds) end +Base.copy(con::GoalConstraint) = GoalConstraint(copy(con.xf), con.inds) @inline sense(::GoalConstraint) = Equality() @inline Base.length(con::GoalConstraint{P}) where P = P @@ -121,6 +123,8 @@ function LinearConstraint(n::Int, m::Int, A::AbstractMatrix, b::AbstractVector, LinearConstraint(n,m, A, b, sense, inds) end +Base.copy(con::LinearConstraint{S}) where S = + LinearConstraint(con.n, con.m, copy(con.A), copy(con.b), S(), con.inds) @inline sense(con::LinearConstraint) = con.sense @inline Base.length(con::LinearConstraint{<:Any,P}) where P = P @@ -444,6 +448,10 @@ struct BoundConstraint{P,NM,T} <: StageConstraint inds::SVector{P,Int} end +Base.copy(bnd::BoundConstraint{P,nm,T}) where {P,nm,T} = + BoundConstraint(bnd.n, bnd.m, bnd.z_max, bnd.z_min, + copy(bnd.i_max), copy(bnd.i_min), bnd.inds) + function BoundConstraint(n, m; x_max=Inf*(@SVector ones(n)), x_min=-Inf*(@SVector ones(n)), u_max=Inf*(@SVector ones(m)), u_min=-Inf*(@SVector ones(m))) nm = n+m @@ -674,6 +682,11 @@ end @inline Base.length(con::IndexedConstraint) = length(con.con) @inline sense(con::IndexedConstraint) = sense(con.con) +function Base.copy(c::IndexedConstraint{C,n0,m0}) where {C,n0,m0} + IndexedConstraint{C,n0,m0,}(c.n, c.m, c.n0, c.m0, copy(c.con), c.ix, c.iu, + copy(∇c),copy(A),copy(B)) +end + function IndexedConstraint(n,m,con::AbstractConstraint, ix::UnitRange{Int}, iu::UnitRange{Int}) p = length(con) diff --git a/test/constraint_sets.jl b/test/constraint_sets.jl index feff18be..d669b550 100644 --- a/test/constraint_sets.jl +++ b/test/constraint_sets.jl @@ -7,6 +7,7 @@ x,u = rand(model) dt = 0.1 z = KnotPoint(x,u,dt) + #--- Generate some constraints # Circle Constraint xc = SA[1,1,1] @@ -40,9 +41,13 @@ add_constraint!(cons, cir, 1:N) add_constraint!(cons, goal, N) add_constraint!(cons, lin, 2:N-1) cons2 = copy(cons) +cons2_ = deepcopy(cons) add_constraint!(cons, bnd, 1:N-1) add_constraint!(cons, dyn, 1:N-1) @test length(cons) == 5 +@test length(cons2) == 3 +@test cons[2] === cons2[2] +@test cons[2] !== cons2_[2] #--- Create an ALConstraintSet conset = TO.ALConstraintSet(cons, model) From 6bcbdb9fbaea74fad579244b24194a40fe295c11 Mon Sep 17 00:00:00 2001 From: Brian Jackson Date: Mon, 10 Aug 2020 13:45:12 -0400 Subject: [PATCH 09/16] Update objective copy methods and problem tests --- src/costfunctions.jl | 4 ++++ src/objective.jl | 2 +- src/problem.jl | 1 + test/problems_tests.jl | 42 ++++++++++++++++++++++++++++++++++++++++++ 4 files changed, 48 insertions(+), 1 deletion(-) diff --git a/src/costfunctions.jl b/src/costfunctions.jl index 4fd1c7a9..3a8384cc 100644 --- a/src/costfunctions.jl +++ b/src/costfunctions.jl @@ -234,6 +234,10 @@ struct DiagonalCost{n,m,T} <: QuadraticCostFunction{n,m,T} end end +Base.copy(c::DiagonalCost) = + DiagonalCost(c.Q.diag, c.R.diag, copy(c.q), copy(c.r), c.c, + checks=false, terminal=c.terminal) + # strip away the H @inline function (::Type{<:DiagonalCost})(Q::AbstractArray, R::AbstractArray, H::AbstractArray, q::AbstractVector, r::AbstractVector, c; kwargs...) diff --git a/src/objective.jl b/src/objective.jl index 73d7507d..f1a9b096 100644 --- a/src/objective.jl +++ b/src/objective.jl @@ -57,7 +57,7 @@ end "Get the vector of costs at each knot point. `sum(get_J(obj))` is equal to the cost" get_J(obj::Objective) = obj.J -Base.copy(obj::Objective) = Objective(copy(obj.cost)) +Base.copy(obj::Objective) = Objective(copy.(obj.cost)) Base.getindex(obj::Objective,i::Int) = obj.cost[i] diff --git a/src/problem.jl b/src/problem.jl index 27025747..e28199eb 100644 --- a/src/problem.jl +++ b/src/problem.jl @@ -178,6 +178,7 @@ function set_goal_state!(prob::Problem, xf::AbstractVector; objective=true, cons end end end + copyto!(prob.xf, xf) return nothing end diff --git a/test/problems_tests.jl b/test/problems_tests.jl index 34c5cf21..d50f8613 100644 --- a/test/problems_tests.jl +++ b/test/problems_tests.jl @@ -119,3 +119,45 @@ x0_ = rand(n) TO.set_initial_state!(prob, x0_) @test prob.x0 ≈ x0_ @test_throws DimensionMismatch TO.set_initial_state!(prob, rand(2n)) + +## Change initial and goal states +prob = Problem(model, copy(obj), xf, tf, x0=x0, constraints=deepcopy(conSet)) +x0_new = @SVector rand(n) +TO.set_initial_state!(prob, x0_new) +@test TO.get_initial_state(prob) == x0_new +TO.set_initial_state!(prob, Vector(2*x0_new)) +@test TO.get_initial_state(prob) ≈ 2*x0_new + +# goal state, changing both objective and terminal constraint +@test conSet[2].xf ≈ xf +xf_new = @SVector rand(n) +TO.set_goal_state!(prob, xf_new) +@test prob.xf ≈ xf_new +@test prob.obj[1].q ≈ -Q*xf_new +@test prob.constraints[2].xf ≈ xf_new + +# make sure it doesn't modify the orignal since they're copied +@test obj[1].q ≈ -Q*xf +@test conSet[2].xf ≈ xf + +# don't modify the terminal constraint +prob = Problem(model, copy(obj), xf, tf, x0=x0, constraints=copy(conSet)) +TO.set_goal_state!(prob, xf_new, constraint=false) +@test prob.xf ≈ xf_new +@test prob.obj[1].q ≈ -Q*xf_new +@test prob.obj[end].q ≈ -Qf*xf_new +@test prob.constraints[2].xf ≈ xf + +# don't modify the objective, and leave off constraints +prob = Problem(model, copy(obj), xf, tf, x0=x0) +TO.set_goal_state!(prob, xf_new, objective=false) +@test prob.xf ≈ xf_new +@test prob.obj[1].q ≈ -Q*xf +@test prob.obj[end].q ≈ -Qf*xf + +# check that it modifies the orignal objective and constraint list if not copied +prob = Problem(model, obj, xf, tf, x0=x0, constraints=copy(conSet)) +TO.set_goal_state!(prob, xf_new) +@test obj[1].q ≈ -Q*xf_new +@test obj[end].q ≈ -Qf*xf_new +@test conSet[2].xf ≈ xf_new \ No newline at end of file From 2dc23c1f2915bac2bd5d8f933e134d0105394589 Mon Sep 17 00:00:00 2001 From: Brian Jackson Date: Mon, 10 Aug 2020 15:21:46 -0400 Subject: [PATCH 10/16] Add quickstart example to tests --- test/Project.toml | 3 +++ test/moi_test.jl | 2 +- test/nlp_tests.jl | 4 ++-- test/runtests.jl | 7 +++++++ test/test_models.jl | 49 ++------------------------------------------- 5 files changed, 15 insertions(+), 50 deletions(-) diff --git a/test/Project.toml b/test/Project.toml index cbd75746..da23a1d2 100644 --- a/test/Project.toml +++ b/test/Project.toml @@ -1,11 +1,14 @@ [deps] +Altro = "5dcf52e5-e2fb-48e0-b826-96f46d2e3e73" BenchmarkTools = "6e4b80f9-dd63-53aa-95a3-0cdb28fa8baf" ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210" Ipopt = "b6b21f68-93f8-5de0-b562-5493be1d77c9" LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" MathOptInterface = "b8f27783-ece8-5eb3-8dc8-9495eed66fee" +NBInclude = "0db19996-df87-5ea3-a455-e3a50d440464" 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" diff --git a/test/moi_test.jl b/test/moi_test.jl index bb77dbb2..a4c82a0c 100644 --- a/test/moi_test.jl +++ b/test/moi_test.jl @@ -9,7 +9,7 @@ if !isdefined(Main,:TEST_TIME) end # Parallel Park -prob = DubinsCar(:parallel_park) +prob = DubinsCarProblem(:parallel_park) TO.add_dynamics_constraints!(prob) nlp = TO.TrajOptNLP(prob, remove_bounds=true, jac_type=:vector) diff --git a/test/nlp_tests.jl b/test/nlp_tests.jl index 3945044e..4a78bfec 100644 --- a/test/nlp_tests.jl +++ b/test/nlp_tests.jl @@ -28,7 +28,7 @@ Z_ = TO.NLPTraj(Z,Zdata) @test eltype(Z_) == StaticKnotPoint{n,m,Float64,n+m} # Test with problem -prob = DubinsCar(:parallel_park) +prob = DubinsCarProblem(:parallel_park) prob.constraints TO.add_dynamics_constraints!(prob) n,m,N = size(prob) @@ -281,7 +281,7 @@ TO.primal_bounds!(zL, zU, cons2, true) @test cons2[1] isa TO.DynamicsConstraint @test length(cons) == 5 -prob = DubinsCar(:parallel_park) +prob = DubinsCarProblem(:parallel_park) TO.add_dynamics_constraints!(prob) n,m,N = size(prob) cons = prob.constraints diff --git a/test/runtests.jl b/test/runtests.jl index 3c0c8ce8..0262af7a 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -7,6 +7,7 @@ using SparseArrays using ForwardDiff using RobotDynamics using BenchmarkTools +using NBInclude const TO = TrajectoryOptimization include("test_models.jl") @@ -35,3 +36,9 @@ end include("nlp_tests.jl") include("moi_test.jl") end + +@testset "Examples" begin + @test_nowarn include(joinpath(@__DIR__, "..", "examples", "quickstart.jl")) + # @nbinclude(joinpath(@__DIR__, "..", "examples", "Cartpole.ipynb"); softscope=true) + # @nbinclude(joinpath(@__DIR__, "..", "examples", "Quadrotor.ipynb"); softscope=true) +end \ No newline at end of file diff --git a/test/test_models.jl b/test/test_models.jl index eaa29d89..a02583d0 100644 --- a/test/test_models.jl +++ b/test/test_models.jl @@ -1,36 +1,5 @@ +import RobotZoo: Cartpole, DubinsCar -struct Cartpole{T} <: AbstractModel - mc::T - mp::T - l::T - g::T -end - -Cartpole() = Cartpole(1.0, 0.2, 0.5, 9.81) - -function RobotDynamics.dynamics(model::Cartpole, x, u) - mc = model.mc # mass of the cart in kg (10) - mp = model.mp # mass of the pole (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[@SVector [1, 2]] - qd = x[@SVector [3, 4]] - - s = sin(q[2]) - c = cos(q[2]) - - H = @SMatrix [mc + mp mp * l * c; mp * l * c mp * l^2] - C = @SMatrix [0 -mp * qd[2] * l * s; 0 0] - G = @SVector [0, mp * g * l * s] - B = @SVector [1, 0] - - qdd = -H \ (C * qd + G - B * u[1]) - return [qd; qdd] -end - -RobotDynamics.state_dim(::Cartpole) = 4 -RobotDynamics.control_dim(::Cartpole) = 1 function CartpoleProblem() model = Cartpole() @@ -63,21 +32,7 @@ function CartpoleProblem() end -struct DubinsCar <: AbstractModel end - -RobotDynamics.state_dim(::DubinsCar) = 3 -RobotDynamics.control_dim(::DubinsCar) = 2 - -function RobotDynamics.dynamics(::DubinsCar,x,u) - ẋ = @SVector [u[1]*cos(x[3]), - u[1]*sin(x[3]), - u[2]] -end - -Base.position(::DubinsCar, x::SVector) = @SVector [x[1], x[2], 0.0] -orientation(::DubinsCar, x::SVector) = expm(x[3]*@SVector [0,0,1.]) - -function DubinsCar(scenario; N=101) +function DubinsCarProblem(scenario; N=101) if scenario == :three_obstacles # Car w/ obstacles From ee3ac5e3dd64a7eec27f54979791bffb641107c8 Mon Sep 17 00:00:00 2001 From: Brian Jackson Date: Tue, 11 Aug 2020 14:25:06 -0400 Subject: [PATCH 11/16] Move trajectories to RobotDynamics --- Project.toml | 2 +- src/TrajectoryOptimization.jl | 6 +- src/nlp.jl | 4 +- src/problem.jl | 6 +- src/trajectories.jl | 180 ---------------------------------- test/problems_tests.jl | 4 +- test/runtests.jl | 1 - test/trajectories.jl | 113 --------------------- 8 files changed, 11 insertions(+), 305 deletions(-) delete mode 100644 src/trajectories.jl delete mode 100644 test/trajectories.jl diff --git a/Project.toml b/Project.toml index 3b28aa1e..3e402a2f 100644 --- a/Project.toml +++ b/Project.toml @@ -16,7 +16,7 @@ UnsafeArrays = "c4a57d5a-5b31-53a6-b365-19f8c011fbd6" DocStringExtensions = "0.8" ForwardDiff = "0.10" MathOptInterface = "0.9" -RobotDynamics = "0.1.2" +RobotDynamics = "0.2" StaticArrays = "0.12" UnsafeArrays = "1" julia = "1" diff --git a/src/TrajectoryOptimization.jl b/src/TrajectoryOptimization.jl index af5f9f1a..7424e8fc 100644 --- a/src/TrajectoryOptimization.jl +++ b/src/TrajectoryOptimization.jl @@ -20,9 +20,10 @@ using RobotDynamics: AbstractModel, LieGroupModel, KnotPoint, StaticKnotPoint, AbstractKnotPoint, QuadratureRule, Implicit, Explicit, DEFAULT_Q, HermiteSimpson, is_terminal, state_diff, state_diff_jacobian!, state_diff_jacobian, - state, control + state, control, states, controls, get_times, Traj, AbstractTrajectory -import RobotDynamics: jacobian!, state_dim, control_dim, state_diff_jacobian! +import RobotDynamics: jacobian!, state_dim, control_dim, states, controls, + state_diff_jacobian!, rollout! # API export # types @@ -62,7 +63,6 @@ export NormConstraint, add_constraint! -include("trajectories.jl") include("expansions.jl") include("costfunctions.jl") include("objective.jl") diff --git a/src/nlp.jl b/src/nlp.jl index 433de79e..ba797d04 100644 --- a/src/nlp.jl +++ b/src/nlp.jl @@ -386,14 +386,14 @@ end @inline Base.firstindex(Z::NLPTraj) = 1 @inline Base.lastindex(Z::NLPTraj) = length(Z) -function set_states!(Z::NLPTraj, X0) +function RobotDynamics.set_states!(Z::NLPTraj, X0) xinds = Z.Zdata.xinds for k in eachindex(X0) Z.Z[xinds[k]] = X0[k] end end -function set_controls!(Z::NLPTraj, U0) +function RobotDynamics.set_controls!(Z::NLPTraj, U0) uinds = Z.Zdata.uinds for k in eachindex(U0) Z.Z[uinds[k]] = U0[k] diff --git a/src/problem.jl b/src/problem.jl index e28199eb..6915024f 100644 --- a/src/problem.jl +++ b/src/problem.jl @@ -116,7 +116,7 @@ states(prob::Problem) = states(prob.Z) Get the times for all the knot points in the problem. """ -@inline get_times(prob::Problem) = get_times(get_trajectory(prob)) +@inline RobotDynamics.get_times(prob::Problem) = get_times(get_trajectory(prob)) """ @@ -137,7 +137,7 @@ end Copy the state trajectory """ -@inline initial_states!(prob, X0) = set_states!(get_trajectory(prob), X0) +@inline initial_states!(prob, X0) = RobotDynamics.set_states!(get_trajectory(prob), X0) """ @@ -188,7 +188,7 @@ end Copy the control trajectory """ -@inline initial_controls!(prob, U0) = set_controls!(get_trajectory(prob), U0) +@inline initial_controls!(prob, U0) = RobotDynamics.set_controls!(get_trajectory(prob), U0) "```julia cost(::Problem) diff --git a/src/trajectories.jl b/src/trajectories.jl deleted file mode 100644 index 4ee03ef7..00000000 --- a/src/trajectories.jl +++ /dev/null @@ -1,180 +0,0 @@ -abstract type AbstractTrajectory{n,m,T} <: AbstractVector{T} end - -terminal_control(Z::AbstractTrajectory) = !RobotDynamics.is_terminal(Z[end]) -traj_size(Z::AbstractTrajectory{n,m}) where {n,m} = n,m,length(Z) -num_vars(Z::AbstractTrajectory) = num_vars(traj_size(Z)..., terminal_control(Z)) -eachcontrol(Z::AbstractTrajectory) = terminal_control(Z) ? Base.OneTo(length(Z)) : Base.OneTo(length(Z)-1) - -function Base.copyto!(Z1::AbstractTrajectory, Z2::AbstractTrajectory) - @assert traj_size(Z1) == traj_size(Z2) - for k = 1:length(Z1) - Z1[k] = Z2[k] - end - return Z1 -end - -@inline states(Z::AbstractTrajectory) = state.(Z) -function controls(Z::AbstractTrajectory) - return [control(Z[k]) for k in eachcontrol(Z) ] -end - -states(x) = states(get_trajectory(x)) -controls(x) = controls(get_trajectory(x)) - -""" - Traj{n,m,T,KP} - -A vector of `AbstractKnotPoint`s of type `KP` with state dimension `n`, -control dimension `m`, and value type `T` - -Supports iteration and indexing. - -# Constructors - Traj(n, m, dt, N, equal=false) - Traj(x, u, dt, N, equal=false) - Traj(X, U, dt, t) - Traj(X, U, dt) -""" -struct Traj{n,m,T,KP} <: AbstractTrajectory{n,m,T} - data::Vector{KP} - function Traj(Z::Vector{<:AbstractKnotPoint{T,n,m}}) where {n,m,T} - new{n,m,T,eltype(Z)}(Z) - end -end - -# AbstractArray interface -@inline Base.iterate(Z::Traj, k::Int) = iterate(Z.data, k) -Base.IteratorSize(Z::Traj) = Base.HasLength() -Base.IteratorEltype(Z::Traj) = IteratorEltype(Z.data) -@inline Base.eltype(Z::Traj) = eltype(Z.data) -@inline Base.length(Z::Traj) = length(Z.data) -@inline Base.size(Z::Traj) = size(Z.data) -@inline Base.getindex(Z::Traj, i) = Z.data[i] -@inline Base.setindex!(Z::Traj, v, i) = Z.data[i] = v -@inline Base.firstindex(Z::Traj) = 1 -@inline Base.lastindex(Z::Traj) = lastindex(Z.data) -Base.IndexStyle(::Traj) = IndexLinear() - -Traj(Z::Traj) = Z - -function Base.copy(Z::AbstractTrajectory) where {T,N,M} - Traj([KnotPoint(copy(z.z), copy(z._x), copy(z._u), z.dt, z.t) for z in Z]) -end - -function Traj(n::Int, m::Int, dt::AbstractFloat, N::Int; equal=false) - x = NaN*@SVector ones(n) - u = @SVector zeros(m) - Traj(x,u,dt,N; equal=equal) -end - -function Traj(x::SVector, u::SVector, dt::AbstractFloat, N::Int; equal=false) - equal ? uN = N : uN = N-1 - Z = [KnotPoint(x,u,dt,(k-1)*dt) for k = 1:uN] - if !equal - m = length(u) - push!(Z, KnotPoint(x,m,(N-1)*dt)) - end - return Traj(Z) -end - -function Traj(X::Vector, U::Vector, dt::Vector, t=cumsum(dt) .- dt[1]) - Z = [KnotPoint(X[k], U[k], dt[k], t[k]) for k = 1:length(U)] - if length(U) == length(X)-1 - push!(Z, KnotPoint(X[end],length(U[1]),t[end])) - end - return Traj(Z) -end - - -states(Z::Traj, i::Int) = [state(z)[i] for z in Z] - -function set_states!(Z::Traj, X) - for k in eachindex(Z) - RobotDynamics.set_state!(Z[k], X[k]) - end -end - -function set_states!(Z::Traj, X::AbstractMatrix) - for k in eachindex(Z) - RobotDynamics.set_state!(Z[k], X[:,k]) - end -end - -function set_controls!(Z::AbstractTrajectory, U) - for k in 1:length(Z)-1 - RobotDynamics.set_control!(Z[k], U[k]) - end -end - -function set_controls!(Z::AbstractTrajectory, U::AbstractMatrix) - for k in 1:length(Z)-1 - RobotDynamics.set_control!(Z[k], U[:,k]) - end -end - -function set_controls!(Z::AbstractTrajectory, u::SVector) - for k in 1:length(Z)-1 - RobotDynamics.set_control!(Z[k], u) - end -end - -function set_times!(Z::AbstractTrajectory, ts) - for k in eachindex(ts) - Z[k].t = ts[k] - end -end - -function get_times(Z::Traj) - [z.t for z in Z] -end - -function shift_fill!(Z::Traj) - N = length(Z) - for k in eachindex(Z) - Z[k].t += Z[k].dt - if k < N - Z[k].z = Z[k+1].z - else - Z[k].t += Z[k-1].dt - end - end -end - -function Base.copyto!(Z::Traj, Z0::Traj) - @assert length(Z) == length(Z0) - for k in eachindex(Z) - copyto!(Z[k].z, Z0[k].z) - end -end - -function Base.copyto!(Z::Union{Vector{<:KnotPoint},Traj{<:Any,<:Any,<:Any,<:KnotPoint}}, Z0::Traj) - @assert length(Z) == length(Z0) - for k in eachindex(Z) - Z[k].z = Z0[k].z - end -end - -#~~~~~~~~~~~~~~~~~~~~~~~~~~ FUNCTIONS ON TRAJECTORIES ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~# - -"Evaluate the discrete dynamics for all knot points" -function discrete_dynamics!(::Type{Q}, f, model, Z::Traj) where Q - for k in eachindex(Z) - f[k] = RobotDynamics.discrete_dynamics(Q, model, Z[k]) - end -end - - -@inline state_diff_jacobian!(G, model::AbstractModel, Z::Traj) = nothing -function state_diff_jacobian!(G, model::LieGroupModel, Z::Traj) - for k in eachindex(Z) - G[k] .= 0 - state_diff_jacobian!(G[k], model, Z[k]) - end -end - -function rollout!(model::AbstractModel, Z::Traj, x0) - Z[1].z = [x0; control(Z[1])] - for k = 2:length(Z) - RobotDynamics.propagate_dynamics(DEFAULT_Q, model, Z[k], Z[k-1]) - end -end diff --git a/test/problems_tests.jl b/test/problems_tests.jl index d50f8613..81d75295 100644 --- a/test/problems_tests.jl +++ b/test/problems_tests.jl @@ -71,7 +71,7 @@ prob = Problem(model, obj, xf, tf) @test controls(prob) == [zeros(m) for k = 1:N-1] @test isempty(prob.constraints) @test TO.integration(prob) == RK3 -@test TO.get_times(prob) ≈ range(0, tf; step=dt) +@test RobotDynamics.get_times(prob) ≈ range(0, tf; step=dt) # Set initial trajectories initial_states!(prob, 2 .* X0) @@ -99,7 +99,7 @@ prob = Problem(model, obj, xf, tf, X0=X0_mat, U0=U0_mat) dts = rand(N-1) dts = dts / sum(dts) * tf prob = Problem(model, obj, xf, tf, dt=dts) -times = TO.get_times(prob) +times = RobotDynamics.get_times(prob) @test times[end] ≈ tf @test times[1] ≈ 0 @test times[2] ≈ dts[1] diff --git a/test/runtests.jl b/test/runtests.jl index 0262af7a..cd071e19 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -29,7 +29,6 @@ end end @testset "Utils" begin - include("trajectories.jl") end @testset "NLP" begin diff --git a/test/trajectories.jl b/test/trajectories.jl deleted file mode 100644 index 4da4a547..00000000 --- a/test/trajectories.jl +++ /dev/null @@ -1,113 +0,0 @@ -@testset "Trajectories" begin -n,m,N = 5,3,11 -dt = 0.1 -x = @SVector rand(n) -u = @SVector rand(m) -z = KnotPoint(x,u,dt) - -#--- Empty contructor -Z = Traj(n,m,dt,N) -@test all(isnan, state(Z[rand(1:N)])) -@test control(Z[rand(1:N-1)]) == zeros(m) -@test length(controls(Z)) == N-1 -@test length(states(Z)) == N -@test eltype(states(Z)) <: SVector{n} -@test eltype(controls(Z)) <: SVector{m} -@test Z[1].dt ≈ dt -@test Z[end].dt ≈ 0 -@test Z[1].t ≈ 0 -@test Z[N].t ≈ (N-1)*dt -@test TO.is_terminal(Z[end]) == true - -Z = Traj(n,m,dt,N, equal=true) -@test all(isnan, state(Z[rand(1:N)])) -@test control(Z[rand(1:N)]) == zeros(m) -@test length(controls(Z)) == N -@test length(states(Z)) == N -@test eltype(states(Z)) <: SVector{n} -@test eltype(controls(Z)) <: SVector{m} -@test Z[1].dt ≈ dt -@test Z[end].dt ≈ dt -@test Z[1].t ≈ 0 -@test Z[N].t ≈ (N-1)*dt -@test TO.is_terminal(Z[end]) == false - -#--- Copy single constructor -Z = Traj(x, u, dt, N) -@test length(Z) == N -@test eltype(Z) <: KnotPoint{Float64,n,m} -@test state(Z[1]) == x -Z[1].z = 2*Z[1].z -@test state(Z[1]) ≈ 2x -@test state(Z[2]) ≈ x -@test control(Z[1]) ≈ 2u -@test control(Z[2]) ≈ u -@test Z[1].dt ≈ dt -@test Z[end].dt ≈ 0 -@test Z[1].t ≈ 0 -@test Z[N].t ≈ (N-1)*dt - -#--- Vector constructor -X = [@SVector rand(n) for k = 1:N] -U = [@SVector rand(m) for k = 1:N-1] -Z = Traj(X,U,fill(dt,N)) -@test states(Z) ≈ X -@test controls(Z) ≈ U -@test TO.get_times(Z) ≈ range(0, length=N, step=dt) - -Z2 = copy(Z) -RobotDynamics.set_state!(Z[1], x) -@test !(state(Z[1]) ≈ state(Z2[1])) -@test state(Z[2]) ≈ state(Z2[2]) -X = 2 .* X -U = 3 .* U -X[1] = x -TO.set_states!(Z2, X) -@test state(Z[1]) ≈ state(Z2[1]) -@test !(control(Z2[2]) ≈ U[2]) -TO.set_controls!(Z2, U) -@test control(Z2[2]) ≈ U[2] - -@test TO.is_terminal(Z[end]) == true -push!(U, 2*U[end]) -Z = Traj(X,U,fill(dt,N)) -@test length(controls(Z)) == N -@test TO.is_terminal(Z[end]) == false - -#--- Test copyto! -Z0 = [KnotPoint(3*X[k], 2*U[k], dt, dt*(k-1)) for k = 1:N] -@test state.(Z0) ≈ 3 .* states(Z) -@test control.(Z0) ≈ 2 .* controls(Z) -copyto!(Z0, Z) -@test state.(Z0) ≈ states(Z) -@test control.(Z0) ≈ controls(Z) - -Z2 = Traj(rand() .* X, rand() .* U,fill(dt,N)) -@test !(states(Z) ≈ states(Z2)) -@test !(controls(Z) ≈ controls(Z2)) -copyto!(Z2, Z) -@test states(Z) ≈ states(Z2) -@test controls(Z) ≈ controls(Z2) - -#--- Test functions on trajectories -model = Cartpole() -n,m = size(model) -fVal = [@SVector zeros(n) for k = 1:N] -X = [@SVector rand(n) for k = 1:N] -U = [@SVector rand(m) for k = 1:N-1] -Z = Traj(X,U,fill(dt,N)) - -# Test dynamics evaluation -TO.discrete_dynamics!(RK3, fVal, model, Z) -@test fVal[1] ≈ discrete_dynamics(RK3, model, X[1], U[1], 0.0, dt) -dyn = TO.DynamicsConstraint{RK3}(model, N) -conval = TO.ConVal(n,m, dyn, 1:N-1) -TO.evaluate!(conval, Z) -@test conval.vals ≈ fVal[1:N-1] .- X[2:N] -@test !(conval.vals ≈ [zeros(n) for k = 1:N-1]) - -# Test rollout -rollout!(model, Z, X[1]) -TO.evaluate!(conval, Z) -@test conval.vals ≈ [zeros(n) for k = 1:N-1] -end From 313a6d8cc21884e50031535384794e03b07a49b4 Mon Sep 17 00:00:00 2001 From: Brian Jackson Date: Tue, 11 Aug 2020 14:25:33 -0400 Subject: [PATCH 12/16] Update examples --- examples/Cartpole.ipynb | 209 +++++++++++++++------------------------ examples/Quadrotor.ipynb | 59 +++++++---- 2 files changed, 118 insertions(+), 150 deletions(-) diff --git a/examples/Cartpole.ipynb b/examples/Cartpole.ipynb index 49782268..65432e66 100644 --- a/examples/Cartpole.ipynb +++ b/examples/Cartpole.ipynb @@ -17,20 +17,11 @@ "cell_type": "code", "execution_count": 1, "metadata": {}, - "outputs": [ - { - "name": "stderr", - "output_type": "stream", - "text": [ - "┌ Info: Precompiling TrajectoryOptimization [c79d492b-0548-5874-b488-5a62c1d9d0ca]\n", - "└ @ Base loading.jl:1260\n", - "┌ Info: Precompiling RobotZoo [74be38bb-dcc2-4b9e-baf3-d6373cd95f10]\n", - "└ @ Base loading.jl:1260\n" - ] - } - ], + "outputs": [], "source": [ + "# import Pkg; Pkg.activate(@__DIR__); Pkg.instantiate();\n", "using TrajectoryOptimization\n", + "using RobotDynamics\n", "import RobotZoo.Cartpole\n", "using StaticArrays, LinearAlgebra" ] @@ -201,22 +192,7 @@ "cell_type": "code", "execution_count": 9, "metadata": {}, - "outputs": [ - { - "name": "stderr", - "output_type": "stream", - "text": [ - "┌ Info: Precompiling ALTRO [5dcf52e5-e2fb-48e0-b826-96f46d2e3e73]\n", - "└ @ Base loading.jl:1260\n", - "┌ Warning: Package ALTRO does not have Statistics in its dependencies:\n", - "│ - If you have ALTRO checked out for development and have\n", - "│ added Statistics as a dependency but haven't updated your primary\n", - "│ environment's manifest file, try `Pkg.resolve()`.\n", - "│ - Otherwise you may need to report an issue with ALTRO\n", - "└ Loading Statistics into ALTRO from project dependency, future warnings for ALTRO are suppressed.\n" - ] - } - ], + "outputs": [], "source": [ "using Altro\n", "opts = SolverOptions(\n", @@ -245,8 +221,8 @@ "name": "stdout", "output_type": "stream", "text": [ - "max_violation: 3.4246685487460127e-9\n", - "cost: 1.5525585358618206\n", + "max_violation: 3.4246698810136422e-9\n", + "cost: 1.5525585360226632\n", "iterations: 40\n" ] } @@ -273,31 +249,31 @@ "data": { "text/plain": [ "100-element Array{SArray{Tuple{1},Float64,1,1},1}:\n", - " [-0.05339476134137313]\n", - " [0.6325107750343213]\n", - " [1.2322342041211627]\n", - " [1.7121543655024503]\n", - " [2.0447173714460316]\n", - " [2.20961999978949]\n", - " [2.1956276826724075]\n", - " [2.003215905773719]\n", - " [1.6467765356488377]\n", - " [1.1541681797671546]\n", - " [0.5622707965893421]\n", - " [-0.09019073798340234]\n", - " [-0.7689872082185228]\n", + " [-0.05339476749902891]\n", + " [0.6325107702753207]\n", + " [1.232234201032278]\n", + " [1.7121543642574573]\n", + " [2.044717372110078]\n", + " [2.2096200023123016]\n", + " [2.195627686886445]\n", + " [2.003215911401063]\n", + " [1.6467765423214773]\n", + " [1.1541681870613645]\n", + " [0.5622708040671263]\n", + " [-0.0901907307383951]\n", + " [-0.76898720158141]\n", " ⋮\n", - " [2.3242572958222927]\n", - " [2.1076972052335985]\n", - " [1.8037587023746806]\n", - " [1.4556318685493417]\n", - " [1.0817771024271379]\n", - " [0.6836845285775933]\n", - " [0.2516400247153723]\n", - " [-0.23177959530502554]\n", - " [-0.7901251451548023]\n", - " [-1.4532874227659318]\n", - " [-2.2592466905576227]\n", + " [2.3242572964604316]\n", + " [2.107697206027066]\n", + " [1.8037587034265141]\n", + " [1.4556318697947677]\n", + " [1.081777103677683]\n", + " [0.683684529542819]\n", + " [0.25164002500560456]\n", + " [-0.2317795961921558]\n", + " [-0.7901251478630485]\n", + " [-1.4532874281258075]\n", + " [-2.2592466996525067]\n", " [-3.000000000027138]" ] }, @@ -321,20 +297,20 @@ }, { "cell_type": "code", - "execution_count": 14, + "execution_count": 12, "metadata": {}, "outputs": [ { "data": { "text/plain": [ "4×101 Array{Float64,2}:\n", - " -3.26608e-11 -6.67435e-5 0.00059117 … -0.00375642 9.13895e-11\n", - " 1.02032e-10 0.000133487 -0.00118996 3.13402 3.14159\n", - " -6.21867e-11 -0.00266537 0.0289344 0.150251 -2.23344e-10\n", - " -2.96702e-11 0.0052871 -0.0576546 0.303027 7.11421e-10" + " -3.26606e-11 -6.67435e-5 0.00059117 … -0.00375642 9.13898e-11\n", + " 1.0203e-10 0.000133487 -0.00118996 3.13402 3.14159\n", + " -6.21863e-11 -0.00266537 0.0289344 0.150251 -2.23344e-10\n", + " -2.96701e-11 0.0052871 -0.0576546 0.303027 7.11422e-10" ] }, - "execution_count": 14, + "execution_count": 12, "metadata": {}, "output_type": "execute_result" } @@ -353,7 +329,7 @@ }, { "cell_type": "code", - "execution_count": 15, + "execution_count": 13, "metadata": {}, "outputs": [ { @@ -387,7 +363,7 @@ }, { "cell_type": "code", - "execution_count": 16, + "execution_count": 14, "metadata": {}, "outputs": [], "source": [ @@ -418,7 +394,7 @@ }, { "cell_type": "code", - "execution_count": 17, + "execution_count": 15, "metadata": { "scrolled": true }, @@ -599,8 +575,8 @@ "Number of equality constraint Jacobian evaluations = 118\n", "Number of inequality constraint Jacobian evaluations = 0\n", "Number of Lagrangian Hessian evaluations = 0\n", - "Total CPU secs in IPOPT (w/o function evaluations) = 2.274\n", - "Total CPU secs in NLP function evaluations = 0.344\n", + "Total CPU secs in IPOPT (w/o function evaluations) = 2.210\n", + "Total CPU secs in NLP function evaluations = 0.402\n", "\n", "EXIT: Optimal Solution Found.\n" ] @@ -611,7 +587,7 @@ "LOCALLY_SOLVED::TerminationStatusCode = 4" ] }, - "execution_count": 17, + "execution_count": 15, "metadata": {}, "output_type": "execute_result" } @@ -632,7 +608,7 @@ }, { "cell_type": "code", - "execution_count": 18, + "execution_count": 16, "metadata": {}, "outputs": [ { @@ -659,7 +635,7 @@ }, { "cell_type": "code", - "execution_count": 19, + "execution_count": 17, "metadata": {}, "outputs": [ { @@ -669,16 +645,16 @@ " memory estimate: 1.52 MiB\n", " allocs estimate: 1581\n", " --------------\n", - " minimum time: 6.169 ms (0.00% GC)\n", - " median time: 7.061 ms (0.00% GC)\n", - " mean time: 7.549 ms (1.56% GC)\n", - " maximum time: 9.973 ms (0.00% GC)\n", + " minimum time: 6.547 ms (0.00% GC)\n", + " median time: 7.132 ms (0.00% GC)\n", + " mean time: 7.095 ms (1.28% GC)\n", + " maximum time: 7.588 ms (4.16% GC)\n", " --------------\n", " samples: 10\n", " evals/sample: 10" ] }, - "execution_count": 19, + "execution_count": 17, "metadata": {}, "output_type": "execute_result" } @@ -693,7 +669,7 @@ }, { "cell_type": "code", - "execution_count": 20, + "execution_count": 18, "metadata": {}, "outputs": [ { @@ -703,16 +679,16 @@ " memory estimate: 7.79 MiB\n", " allocs estimate: 22123\n", " --------------\n", - " minimum time: 616.781 ms (0.00% GC)\n", - " median time: 631.013 ms (0.00% GC)\n", - " mean time: 635.028 ms (0.07% GC)\n", - " maximum time: 666.010 ms (0.00% GC)\n", + " minimum time: 619.169 ms (0.00% GC)\n", + " median time: 693.174 ms (0.00% GC)\n", + " mean time: 752.114 ms (0.08% GC)\n", + " maximum time: 1.220 s (0.00% GC)\n", " --------------\n", - " samples: 8\n", + " samples: 7\n", " evals/sample: 1" ] }, - "execution_count": 20, + "execution_count": 18, "metadata": {}, "output_type": "execute_result" } @@ -736,7 +712,7 @@ }, { "cell_type": "code", - "execution_count": 21, + "execution_count": 19, "metadata": {}, "outputs": [ { @@ -744,21 +720,21 @@ "output_type": "stream", "text": [ "cost(nlp) = 1.4958739082433778\n", - "cost(altro) = 1.5525585358618206\n", + "cost(altro) = 1.5525585360226632\n", "max_violation(nlp) = 1.2434497875801753e-13\n", - "max_violation(altro) = 3.4246685487460127e-9\n", - "Speed improvement: 89.0x\n" + "max_violation(altro) = 3.4246698810136422e-9\n", + "Speed improvement: 97.0x\n" ] }, { "data": { "text/plain": [ "BenchmarkTools.TrialJudgement: \n", - " time: -98.88% => \u001b[32mimprovement\u001b[39m (5.00% tolerance)\n", + " time: -98.97% => \u001b[32mimprovement\u001b[39m (5.00% tolerance)\n", " memory: -80.46% => \u001b[32mimprovement\u001b[39m (1.00% tolerance)\n" ] }, - "execution_count": 21, + "execution_count": 19, "metadata": {}, "output_type": "execute_result" } @@ -783,17 +759,15 @@ }, { "cell_type": "code", - "execution_count": 22, + "execution_count": 20, "metadata": {}, "outputs": [ { "name": "stderr", "output_type": "stream", "text": [ - "┌ Info: Precompiling TrajOptPlots [7770976a-8dee-4930-bf39-a1782fd21ce6]\n", - "└ @ Base loading.jl:1260\n", "┌ Info: MeshCat server started. You can open the visualizer by visiting the following URL in your browser:\n", - "│ http://localhost:8700\n", + "│ http://localhost:8702\n", "└ @ MeshCat /home/bjack205/.julia/packages/MeshCat/ECbzr/src/visualizer.jl:73\n" ] }, @@ -801,14 +775,14 @@ "data": { "text/html": [ "
\n", - " \n", + " \n", "
\n" ], "text/plain": [ - "MeshCat.DisplayedVisualizer(MeshCat.CoreVisualizer(MeshCat.SceneTrees.SceneNode(nothing, nothing, Dict{String,Array{UInt8,1}}(), nothing, Dict{String,MeshCat.SceneTrees.SceneNode}()), Set(Any[]), ip\"127.0.0.1\", 8700))" + "MeshCat.DisplayedVisualizer(MeshCat.CoreVisualizer(MeshCat.SceneTrees.SceneNode(nothing, nothing, Dict{String,Array{UInt8,1}}(), nothing, Dict{String,MeshCat.SceneTrees.SceneNode}()), Set(Any[]), ip\"127.0.0.1\", 8702))" ] }, - "execution_count": 22, + "execution_count": 20, "metadata": {}, "output_type": "execute_result" } @@ -831,16 +805,16 @@ }, { "cell_type": "code", - "execution_count": 24, + "execution_count": 21, "metadata": {}, "outputs": [ { "data": { "text/plain": [ - "MeshCat Visualizer with path /meshcat/robot/cart/pole at http://localhost:8700" + "MeshCat Visualizer with path /meshcat/robot/cart/pole at http://localhost:8702" ] }, - "execution_count": 24, + "execution_count": 21, "metadata": {}, "output_type": "execute_result" } @@ -858,7 +832,7 @@ }, { "cell_type": "code", - "execution_count": 41, + "execution_count": 22, "metadata": {}, "outputs": [], "source": [ @@ -867,7 +841,7 @@ }, { "cell_type": "code", - "execution_count": 28, + "execution_count": 23, "metadata": {}, "outputs": [], "source": [ @@ -883,17 +857,9 @@ }, { "cell_type": "code", - "execution_count": 37, + "execution_count": 24, "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "typeof(Xs) = Tuple{Array{SArray{Tuple{4},Float64,1,4},1},Array{SArray{Tuple{4},Float64,1,4},1}}\n" - ] - } - ], + "outputs": [], "source": [ "visualize!(vis, altro, nlp);" ] @@ -907,17 +873,9 @@ }, { "cell_type": "code", - "execution_count": 39, + "execution_count": 25, "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "typeof(Xs) = Tuple{Array{SArray{Tuple{4},Float64,1,4},1},Array{SArray{Tuple{4},Float64,1,4},1}}\n" - ] - } - ], + "outputs": [], "source": [ "visualize!(vis, model, get_trajectory(altro), get_trajectory(nlp));" ] @@ -931,20 +889,9 @@ }, { "cell_type": "code", - "execution_count": 40, + "execution_count": 26, "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "MeshCat Visualizer with path /meshcat/robot_copies at http://localhost:8700" - ] - }, - "execution_count": 40, - "metadata": {}, - "output_type": "execute_result" - } - ], + "outputs": [], "source": [ "delete!(vis[\"robot_copies\"]);" ] diff --git a/examples/Quadrotor.ipynb b/examples/Quadrotor.ipynb index 0e4b2783..dd455304 100644 --- a/examples/Quadrotor.ipynb +++ b/examples/Quadrotor.ipynb @@ -11,11 +11,39 @@ "To define the quadrotor model, we import `RobotDynamics` and `Rotations`, and use `TrajectoryOptimization` to define the problem. We load in `StaticArrays` and `LinearAlgebra` to help with the setup." ] }, + { + "cell_type": "code", + "execution_count": 14, + "metadata": {}, + "outputs": [ + { + "name": "stderr", + "output_type": "stream", + "text": [ + "\u001b[32m\u001b[1m Activating\u001b[22m\u001b[39m environment at `~/.julia/dev/TrajectoryOptimization/examples/Project.toml`\n" + ] + } + ], + "source": [ + "# import Pkg; Pkg.activate(@__DIR__); Pkg.instantiate();" + ] + }, { "cell_type": "code", "execution_count": 1, "metadata": {}, - "outputs": [], + "outputs": [ + { + "name": "stderr", + "output_type": "stream", + "text": [ + "┌ Info: Precompiling RobotDynamics [38ceca67-d8d3-44e8-9852-78a5596522e1]\n", + "└ @ Base loading.jl:1260\n", + "┌ Info: Precompiling TrajectoryOptimization [c79d492b-0548-5874-b488-5a62c1d9d0ca]\n", + "└ @ Base loading.jl:1260\n" + ] + } + ], "source": [ "using RobotDynamics, Rotations\n", "using TrajectoryOptimization\n", @@ -51,7 +79,6 @@ " bodyframe::Bool # velocity in body frame?\n", " info::Dict{Symbol,Any}\n", "end\n", - "control_dim(::Quadrotor) = 4\n", "\n", "function Quadrotor{R}(;\n", " mass=0.5,\n", @@ -329,35 +356,29 @@ }, { "cell_type": "code", - "execution_count": 11, + "execution_count": 12, "metadata": {}, "outputs": [ { "name": "stderr", "output_type": "stream", "text": [ - "┌ Info: Precompiling ALTRO [5dcf52e5-e2fb-48e0-b826-96f46d2e3e73]\n", - "└ @ Base loading.jl:1260\n", - "┌ Warning: Package ALTRO does not have Statistics in its dependencies:\n", - "│ - If you have ALTRO checked out for development and have\n", - "│ added Statistics as a dependency but haven't updated your primary\n", - "│ environment's manifest file, try `Pkg.resolve()`.\n", - "│ - Otherwise you may need to report an issue with ALTRO\n", - "└ Loading Statistics into ALTRO from project dependency, future warnings for ALTRO are suppressed.\n" + "┌ Info: Precompiling Altro [5dcf52e5-e2fb-48e0-b826-96f46d2e3e73]\n", + "└ @ Base loading.jl:1260\n" ] }, { "name": "stdout", "output_type": "stream", "text": [ - "Cost: 0.2992834848479653\n", - "Constraint violation: 7.598400797448335e-10\n", + "Cost: 0.2992834848449584\n", + "Constraint violation: 7.598522921981044e-10\n", "Iterations: 90\n" ] } ], "source": [ - "using ALTRO\n", + "using Altro\n", "opts = SolverOptions(\n", " penalty_scaling=100.,\n", " penalty_initial=0.1,\n", @@ -380,7 +401,7 @@ }, { "cell_type": "code", - "execution_count": 12, + "execution_count": 15, "metadata": {}, "outputs": [ { @@ -405,7 +426,7 @@ "MeshCat.DisplayedVisualizer(MeshCat.CoreVisualizer(MeshCat.SceneTrees.SceneNode(nothing, nothing, Dict{String,Array{UInt8,1}}(), nothing, Dict{String,MeshCat.SceneTrees.SceneNode}()), Set(Any[]), ip\"127.0.0.1\", 8701))" ] }, - "execution_count": 12, + "execution_count": 15, "metadata": {}, "output_type": "execute_result" } @@ -428,7 +449,7 @@ }, { "cell_type": "code", - "execution_count": 15, + "execution_count": 16, "metadata": {}, "outputs": [ { @@ -437,7 +458,7 @@ "MeshCat Visualizer with path /meshcat/robot/geom at http://localhost:8701" ] }, - "execution_count": 15, + "execution_count": 16, "metadata": {}, "output_type": "execute_result" } @@ -457,7 +478,7 @@ }, { "cell_type": "code", - "execution_count": 18, + "execution_count": 17, "metadata": {}, "outputs": [], "source": [ From 2ab22856357393ca700de3e95240e10bba66296b Mon Sep 17 00:00:00 2001 From: Brian Jackson Date: Tue, 11 Aug 2020 14:26:07 -0400 Subject: [PATCH 13/16] Delete old tests --- test/old/Project.toml | 11 -- test/old/attitude_costs.jl | 192 ----------------------- test/old/car_tests.jl | 46 ------ test/old/constraint_tests.jl | 172 --------------------- test/old/controllers_test.jl | 114 -------------- test/old/copymodel_test.jl | 177 --------------------- test/old/ddl.jl | 125 --------------- test/old/dynamics_constraints.jl | 44 ------ test/old/dynamics_tests.jl | 86 ----------- test/old/infeasible_start.jl | 119 -------------- test/old/knotpoint_tests.jl | 23 --- test/old/logger_tests.jl | 100 ------------ test/old/models_test.jl | 89 ----------- test/old/quadrotor_tests.jl | 151 ------------------ test/old/retraction_maps.jl | 89 ----------- test/old/rigidbody_tests.jl | 104 ------------- test/old/rotations_tests.jl | 257 ------------------------------- test/old/satellite_test.jl | 224 --------------------------- test/old/simple_rotation_test.jl | 219 -------------------------- test/old/solver_comparision.jl | 152 ------------------ test/old/solver_options.jl | 6 - test/old/sqrt_bp_tests.jl | 85 ---------- test/old/state_diff_tests.jl | 22 --- test/old/static_constraints.jl | 248 ----------------------------- test/old/test_noisyrb.jl | 13 -- test/old/yak_tests.jl | 43 ------ 26 files changed, 2911 deletions(-) delete mode 100644 test/old/Project.toml delete mode 100644 test/old/attitude_costs.jl delete mode 100644 test/old/car_tests.jl delete mode 100644 test/old/constraint_tests.jl delete mode 100644 test/old/controllers_test.jl delete mode 100644 test/old/copymodel_test.jl delete mode 100644 test/old/ddl.jl delete mode 100644 test/old/dynamics_constraints.jl delete mode 100644 test/old/dynamics_tests.jl delete mode 100644 test/old/infeasible_start.jl delete mode 100644 test/old/knotpoint_tests.jl delete mode 100644 test/old/logger_tests.jl delete mode 100644 test/old/models_test.jl delete mode 100644 test/old/quadrotor_tests.jl delete mode 100644 test/old/retraction_maps.jl delete mode 100644 test/old/rigidbody_tests.jl delete mode 100644 test/old/rotations_tests.jl delete mode 100644 test/old/satellite_test.jl delete mode 100644 test/old/simple_rotation_test.jl delete mode 100644 test/old/solver_comparision.jl delete mode 100644 test/old/solver_options.jl delete mode 100644 test/old/sqrt_bp_tests.jl delete mode 100644 test/old/state_diff_tests.jl delete mode 100644 test/old/static_constraints.jl delete mode 100644 test/old/test_noisyrb.jl delete mode 100644 test/old/yak_tests.jl diff --git a/test/old/Project.toml b/test/old/Project.toml deleted file mode 100644 index a957956d..00000000 --- a/test/old/Project.toml +++ /dev/null @@ -1,11 +0,0 @@ -[deps] -BenchmarkTools = "6e4b80f9-dd63-53aa-95a3-0cdb28fa8baf" -Distributions = "31c24e10-a181-5473-b8eb-7969acd0382f" -ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210" -Ipopt = "b6b21f68-93f8-5de0-b562-5493be1d77c9" -LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" -Logging = "56ddb016-857b-54e1-b83d-db4d58db5568" -Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c" -SparseArrays = "2f01184e-e22b-5df5-ae63-d93ebab69eaf" -StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" -Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40" diff --git a/test/old/attitude_costs.jl b/test/old/attitude_costs.jl deleted file mode 100644 index 6c1af43b..00000000 --- a/test/old/attitude_costs.jl +++ /dev/null @@ -1,192 +0,0 @@ -using StaticArrays -using LinearAlgebra -using Test -using Random -using ForwardDiff -const TO = TrajectoryOptimization - -# Unit Quaternions -q = rand(UnitQuaternion) -q0 = rand(UnitQuaternion) -qval = SVector(q) -ϕ = @SVector zeros(3) - -function mycost(q::SVector{4}) - return 1 - SVector(q0)'q -end -function mycostdiff(v::SVector{3}) - mycost(SVector(q*VectorPart(v))) -end - -grad_q = ForwardDiff.gradient(mycost, qval) -hess_q = ForwardDiff.hessian(mycost, qval) -G = TO.∇differential(q) - -@test ForwardDiff.gradient(mycostdiff, ϕ) ≈ G'grad_q -@test ForwardDiff.hessian(mycostdiff, ϕ) ≈ G'hess_q*G + TO.∇²differential(q, grad_q) -dq = SVector(q0)'SVector(q) -@test ForwardDiff.gradient(mycostdiff, ϕ) ≈ -G'SVector(q0) -@test ForwardDiff.gradient(mycostdiff, ϕ) ≈ -G'SVector(q0) -G'SVector(q0) ≈ Vmat()*Lmult(q)'SVector(q0) -@test ForwardDiff.hessian(mycostdiff, ϕ) ≈ I(3)*dq - -# MRPs -g = rand(MRP) -gval = SVector(g) -g0 = rand(MRP) -dg = @SVector zeros(3) -function mycost(g::SVector{3}) - g = MRP(g) - dg = g ⊖ g0 - return norm(dg) -end -mycostdiff(dg::SVector{3}) = mycost(SVector(g*MRP(dg))) -mycost(gval) -mycostdiff(dg) -G = TO.∇differential(g) -dfdp = ForwardDiff.gradient(mycost,gval) -dfdp2 = ForwardDiff.hessian(mycost,gval) - -ForwardDiff.gradient(mycostdiff,dg) ≈ G'dfdp -ForwardDiff.hessian(mycostdiff,dg) ≈ G'dfdp2*G + TO.∇²differential(g,dfdp) - - -# State diff cost (Quaternion) -Random.seed!(1) -q = rand(UnitQuaternion) -q0 = rand(UnitQuaternion) -qval = SVector(q) -ϕ = @SVector zeros(3) - -RMAP = CayleyMap -function mycost(q::SVector{4}) - q = UnitQuaternion{RMAP}(q) - err = RMAP(q0\q) - 0.5*err'err -end -function mycostdiff(ϕ::SVector{3}) - mycost(SVector(q*RMAP(ϕ))) -end - -mycostdiff(ϕ) ≈ mycost(qval) -grad_q = ForwardDiff.gradient(mycost, qval) -hess_q = ForwardDiff.hessian(mycost, qval) -dq = q0\q -err = CayleyMap(dq) -G = TO.∇differential(dq) -dmap = jacobian(CayleyMap,dq) -∇jac = TO.∇jacobian(CayleyMap, dq, err) -TO.∇²differential(q, grad_q) - -@test grad_q ≈ Lmult(q0)*dmap'err -@test ForwardDiff.gradient(mycostdiff,ϕ) ≈ (dmap*G)'err -@test ForwardDiff.hessian(mycostdiff,ϕ) ≈ G'dmap'dmap*G + G'∇jac*G + TO.∇²differential(q,grad_q) - -RMAP = MRPMap -@test mycostdiff(ϕ) ≈ mycost(qval) -grad_q = ForwardDiff.gradient(mycost, qval) -hess_q = ForwardDiff.hessian(mycost, qval) -dq = q0\q -err = RMAP(dq) -G = TO.∇differential(dq) -dmap = jacobian(RMAP,dq) -∇jac = TO.∇jacobian(RMAP, dq, err) - -@test grad_q ≈ Lmult(q0)*dmap'err -@test ForwardDiff.gradient(mycostdiff,ϕ) ≈ (dmap*G)'err -@test ForwardDiff.hessian(mycostdiff,ϕ) ≈ G'dmap'dmap*G + G'∇jac*G + TO.∇²differential(q,grad_q) - -RMAP = VectorPart -@test mycostdiff(ϕ) ≈ mycost(qval) -grad_q = ForwardDiff.gradient(mycost, qval) -hess_q = ForwardDiff.hessian(mycost, qval) -dq = q0\q -err = RMAP(dq) -G = TO.∇differential(dq) -dmap = jacobian(RMAP,dq) -∇jac = TO.∇jacobian(RMAP, dq, err) - -@test grad_q ≈ Lmult(q0)*dmap'err -@test ForwardDiff.gradient(mycostdiff,ϕ) ≈ (dmap*G)'err -@test ForwardDiff.hessian(mycostdiff,ϕ) ≈ G'dmap'dmap*G + G'∇jac*G + TO.∇²differential(q,grad_q) - -RMAP = ExponentialMap -@test mycostdiff(ϕ) ≈ mycost(qval) -grad_q = ForwardDiff.gradient(mycost, qval) -hess_q = ForwardDiff.hessian(mycost, qval) -dq = q0\q -err = RMAP(dq) -G = TO.∇differential(dq) -dmap = jacobian(RMAP,dq) -∇jac = TO.∇jacobian(RMAP, dq, err) - -@test grad_q ≈ Lmult(q0)*dmap'err -ForwardDiff.gradient(mycostdiff,ϕ) -(dmap*G)'err -@test ForwardDiff.hessian(mycostdiff,ϕ) ≈ G'dmap'dmap*G + G'∇jac*G + TO.∇²differential(q,grad_q) - -jacobian(ExponentialMap, q) -b = @SVector rand(3) -q0 = ExponentialMap(1e-7*@SVector ones(3)) -TO.vecnorm(q0) -jacobian(ExponentialMap, q)'b ≈ TO.∇jacobian(ExponentialMap, q, b) -jacobian(ExponentialMap, q)'b -ForwardDiff.jacobian(x->jacobian(ExponentialMap, UnitQuaternion(x))'b, SVector(q0)) ≈ TO.∇jacobian(ExponentialMap, q0, b) -ForwardDiff.jacobian(x->jacobian(ExponentialMap, UnitQuaternion(x))'b, SVector(q)) ≈ - TO.∇jacobian(ExponentialMap, q, b) -@btime TO.∇jacobian(ExponentialMap, $q, $b) - - -# MRPs -p = rand(MRP) -p0 = rand(MRP) -pval = SVector(p) -ϕ = @SVector zeros(3) -Q = Diagonal(@SVector rand(3)) - -function mycost(p::SVector{3}) - p = MRP(p) - err = SVector(p0\p) - 0.5*err'Q*err -end -function mycostdiff(ϕ::SVector{3}) - mycost(SVector(p*MRP(ϕ))) -end - -grad_p = ForwardDiff.gradient(mycost, pval) -hess_p = ForwardDiff.hessian(mycost, pval) -err = SVector(p0\p) - -grad_p ≈ TO.∇err(p0,p)'Q*err -hess_p ≈ TO.∇²err(p0,p,Q*err) + TO.∇err(p0,p)'Q*TO.∇err(p0,p) - -grad_d = ForwardDiff.gradient(mycostdiff, ϕ) -hess_d = ForwardDiff.hessian(mycostdiff, ϕ) -@test grad_d ≈ TO.∇differential(p0\p)'Q*err -@test hess_d ≈ TO.∇differential(p0\p)'Q*TO.∇differential(p0\p) + TO.∇²differential(p0\p,Q*err) - - -# RPs -g = rand(RodriguesParam) -g0 = rand(RodriguesParam) -gz = RodriguesParam(0.,0.,0.) -gval = SVector(g) -ϕ = @SVector zeros(3) - -Q = Diagonal(@SVector rand(3)) - -function mycost(g::SVector{3}) - g = RodriguesParam(g) - err = SVector(g0\g) - 0.5*err'Q*err -end -function mycostdiff(ϕ::SVector{3}) - mycost(SVector(g*RodriguesParam(ϕ))) -end - - -grad_d = ForwardDiff.gradient(mycostdiff, ϕ) -hess_d = ForwardDiff.hessian(mycostdiff, ϕ) -err = g ⊖ g0 -@test grad_d ≈ TO.∇differential(g0\g)'Q*err -@test hess_d ≈ TO.∇differential(g0\g)'Q*TO.∇differential(g0\g) + TO.∇²differential(g0\g,Q*err) diff --git a/test/old/car_tests.jl b/test/old/car_tests.jl deleted file mode 100644 index 16c4a6d9..00000000 --- a/test/old/car_tests.jl +++ /dev/null @@ -1,46 +0,0 @@ -using Test -prob = Problems.DubinsCar(:three_obstacles)[1] -Z0 = copy(prob.Z) -X0 = deepcopy(states(Z0)) -U0 = deepcopy(controls(Z0)) - -# Solve with iLQR -ilqr = iLQRSolver(Problems.DubinsCar(:three_obstacles)...) -Z0 = copy(ilqr.Z) -X0 = deepcopy(states(Z0)) -U0 = deepcopy(controls(Z0)) -solve!(ilqr) -@test cost(ilqr) < 8.47 -initial_controls!(ilqr, U0) -@test (@allocated solve!(ilqr)) == 0 - -# Solve with AL-iLQR -al = AugmentedLagrangianSolver(Problems.DubinsCar(:three_obstacles)...) -initial_controls!(al, U0) -solve!(al) -@test max_violation(al) < al.opts.constraint_tolerance -@test cost(al) < 10 -initial_controls!(al, U0) -@test (@allocated solve!(al)) == 0 - -# Solve with ALTRO -altro = ALTROSolver(Problems.DubinsCar(:three_obstacles)...) -altro.opts.opts_pn.verbose = false -initial_controls!(altro, U0) -solve!(altro) -Z_sol = copy(get_trajectory(altro)) -X_sol = deepcopy(states(altro)) -@test max_violation(altro) < 1e-8 -@test cost(altro) < 10 -altro.opts.projected_newton = false -initial_controls!(altro, U0) -solve!(altro) -initial_controls!(altro, U0) -@test (@allocated solve!(altro)) == 0 - -# Ipopt -ipopt = DIRCOLSolver(Problems.DubinsCar(:three_obstacles)..., integration=HermiteSimpson) -ipopt.opts.verbose = false -solve!(ipopt) -@test max_violation(ipopt) < 2e-8 -@test cost(ipopt) < 12 diff --git a/test/old/constraint_tests.jl b/test/old/constraint_tests.jl deleted file mode 100644 index 9aeed5f7..00000000 --- a/test/old/constraint_tests.jl +++ /dev/null @@ -1,172 +0,0 @@ -using Test - -function alloc_con(con,z) - p,w = length(con), TO.width(con) - ∇c = zeros(p,w) - allocs = @allocated evaluate(con,z) - allocs += @allocated jacobian!(∇c, con,z) -end - -model = Dynamics.DubinsCar() -x,u = rand(model) -z = KnotPoint(x,u,0.1) -n,m = size(model) - -# All static -A = @SMatrix rand(4,3) -b = @SVector rand(4) -∇c = zeros(4,3) -con = LinearConstraint{Inequality,State}(n,m,A,b) -evaluate(con, z) -@test length(con) == 4 -@test TO.width(con) == 3 -@test evaluate(con, z) == A*x - b -jacobian!(∇c, con, x) -@test ∇c == A -@test alloc_con(con,z) == 0 -@test state_dim(con) == n -@test_throws MethodError control_dim(con) - -# both dynamic -con = LinearConstraint{Inequality,State}(n,m,Matrix(A),Vector(b)) -@test evaluate(con, z) == A*x - b -jacobian!(∇c, con, x) -@test ∇c == A -@test alloc_con(con,z) == 0 - -# mixed -con = LinearConstraint{Inequality,State}(n,m,A,Vector(b)) -@test evaluate(con, z) == A*x - b -jacobian!(∇c, con, x) -@test ∇c == A -@test alloc_con(con,z) == 0 - -# wrong input size -@test_throws AssertionError LinearConstraint{Inequality,State}(m,m,A,b) -@test_throws AssertionError LinearConstraint{Inequality,Control}(n,m,A,b) -con = LinearConstraint{Inequality,Control}(n,n,A,b) -@test evaluate(con, x) == A*x - b -jacobian!(∇c, con, x) -@test ∇c == A - - -model = Dynamics.Quadrotor2{UnitQuaternion{Float64,VectorPart}}() -x,u = rand(model) -n,m = size(model) - -# Goal Constraint -xf = @SVector rand(n) -con = GoalConstraint(xf) -size(con) == (13,13) -∇c = zeros(size(con)...) -@test evaluate(con, x) == x-xf -@test jacobian!(∇c, con, x) == I(n) - -con = GoalConstraint(xf, 1:3) -∇c = zeros(size(con)) -@test evaluate(con, x) == (x-xf)[1:3] -@test jacobian!(∇c, con, x) == Matrix(I,3,n) - -noquat = collect(1:n) -noquat = deleteat!(noquat, 4:7) -noquat = SVector{9}(noquat) -con = GoalConstraint(xf, noquat) -∇c = zeros(size(con)) -@test evaluate(con, x) == (x-xf)[noquat] -@test jacobian!(∇c, con, x) == I(n)[noquat,:] - -# Sphere Constraint -xc = @SVector rand(4) -yc = @SVector rand(4) -zc = @SVector rand(4) -r = @SVector fill(0.1,4) -con = SphereConstraint(n, xc, yc, zc, r) -∇c = zeros(size(con)) -@test state_dim(con) == n -@test evaluate(con, x) isa SVector{4} -@test norm(jacobian!(∇c, con, x)[:,4:end],Inf) == 0 - - -# Norm Constraint -con = TO.NormConstraint{Equality,Control}(m,4.0) -@test TO.sense(con) == Equality -@test TO.contype(con) == Control -@test evaluate(con, u)[1] == norm(u)^2 - 4 -@test_throws MethodError state_dim(con) -@test control_dim(con) == m -@test TO.check_dims(con,n,m) - -con = TO.NormConstraint{Inequality,State}(n, 2.0) -@test TO.sense(con) == Inequality -@test TO.contype(con) == State -@test evaluate(con, x)[1] == norm(x)^2 - 2 -@test_throws MethodError control_dim(con) -@test state_dim(con) == n -@test TO.check_dims(con,n,m) - - -# Bounds Constraint -x_max = @SVector fill(+1,n) -x_min = @SVector fill(-1,n) -u_max = @SVector fill(+2,m) -u_min = @SVector fill(-2,m) - -bnd = BoundConstraint(n,m, x_max=x_max, x_min=x_min) -@test_throws ArgumentError BoundConstraint(n,m, x_max=x_min, x_min=x_max) -@test_nowarn BoundConstraint(n,m, x_max=1, x_min=x_min) -@test_nowarn BoundConstraint(n,m, x_max=x_max, x_min=-1.) -@test TO.is_bound(bnd) -@test TO.upper_bound(bnd) == [x_max; u_max*Inf] -@test TO.lower_bound(bnd) == [x_min; u_min*Inf] - - -# Variable Bound Constraint -N = 101 -X_max = [copy(x_max) for k = 1:N] -bnd = VariableBoundConstraint(n,m,N, x_max=X_max) -@test state_dim(bnd) == n -@test control_dim(bnd) == m -vals = [@SVector zeros(n) for k = 1:N] -Z = Traj(x,u,0.1,N) -@test_nowarn evaluate!(vals, bnd, Z) -@test jacobian(bnd, Z[1]) == Matrix(I,n,n+m) - - -# Indexed Constraint -bnd = BoundConstraint(n,m, x_max=x_max, x_min=x_min) -con = TO.IndexedConstraint(2n,2m, bnd) -∇c = zeros(size(con)) -x2 = repeat(x,2) -u2 = repeat(u,2) -z = KnotPoint(x,u,0.1) -z2 = KnotPoint(x2,u2,0.1) -@test evaluate(bnd, z) ≈ evaluate(con, z2) -∇c0 = zeros(size(bnd)) -jacobian!(∇c0, bnd, z) -jacobian!(∇c, con, z2) -@test ∇c ≈ [∇c0 zeros(26, 17)] - -@test TO.width(con) == 2(n+m) -@test TO.width(bnd) == n+m - -con = TO.NormConstraint{Equality,Control}(m,4.0) -∇c0 = zeros(size(con)) -ix = n .+ @SVector [i for i in 1:n] -iu = m .+ @SVector [i for i in 1:m] -idx = TO.IndexedConstraint(2n,2m, con, ix, iu) -∇c = zeros(size(idx)) -@test TO.contype(idx) == Control -@test TO.sense(idx) == Equality -@test evaluate(idx, z2) == evaluate(con, z) -jacobian!(∇c0, con, z) -jacobian!(∇c, idx, z2) -@test ∇c == [zeros(1,m) ∇c0] - -con = TO.NormConstraint{Equality,State}(n,4.0) -idx = TO.IndexedConstraint(2n,2m, con) -@test evaluate(idx, z2) == evaluate(con, z) -∇c0 = zeros(size(con)) -∇c = zeros(size(idx)) -jacobian!(∇c0, con, z) -jacobian!(∇c, idx, z2) -@test ∇c == [∇c0 zeros(1,n)] diff --git a/test/old/controllers_test.jl b/test/old/controllers_test.jl deleted file mode 100644 index e9957d94..00000000 --- a/test/old/controllers_test.jl +++ /dev/null @@ -1,114 +0,0 @@ -using TrajectoryOptimization -using StaticArrays -using LinearAlgebra -using Random -using Test -using Distributions -const TO = TrajectoryOptimization - -# Solve Quadrotor zig-zag problem -prob,opts = Problems.Quadrotor(:zigzag, costfun=:Quadratic) -solver = iLQRSolver(prob,opts) -solve!(solver) -iterations(solver) -@test norm(state(solver.Z[end])[1:3] - solver.xf[1:3]) < 0.1 - -# Convert trajectory to general RBState -Xref = Controllers.RBState(solver.model, solver.Z) -Uref = controls(solver) -push!(Uref, Dynamics.trim_controls(solver.model)) -dt_ref = solver.Z[1].dt - -# Track with different model -Rot = MRP{Float64} -model = Dynamics.Quadrotor2{Rot}(use_rot=true, mass=.5) -dt = 1e-4 -Nsim = Int(solver.tf/dt) + 1 -inds = Int.(range(1,Nsim,length=solver.N)) -Q = Diagonal(Dynamics.fill_error_state(model, 200, 200, 50, 50)) -R = Diagonal(@SVector fill(1.0, 4)) - -mlqr = Controllers.TVLQR(model, Q, R, Xref, Uref, dt_ref) - -noise = Distributions.MvNormal(Diagonal(fill(5.,6))) -model_ = Dynamics.NoisyRB(model, noise) -X = Controllers.simulate(model_, mlqr, Xref[1], solver.tf, w=0.0) -res_lqr = [Controllers.RBState(model, x) for x in X[inds]] -e1 = norm(res_lqr .⊖ Xref) - - -# Track with SE3 controller - -# get reference trajectories -b_ = map(Xref) do x - # prefer pointing torwards the goal - x.r - Xref[end].r + 0.01*@SVector rand(3) -end -X_ = map(Xref) do x - Dynamics.build_state(model, x) -end -Xd_ = map(1:solver.N) do k - dynamics(model, X_[k], Uref[k]) -end -t = collect(0:dt_ref:solver.tf) - -# cntrl = SE3Tracking(model, X_, Xd_, b_, t) -kx = 2.71 + 10.0 -kv = 1.01 + 3.0 -kR = 3.0*Diagonal(@SVector [1,1,1.]) -kO = 0.5*Diagonal(@SVector [1,1,1.]) -hfca = Controllers.HFCA(model, X_, Xd_, b_, t, kx=kx, kv=kv, kR=kR, kO=kO) - -X2 = Controllers.simulate(model, hfca, Xref[1], solver.tf, w=0.0) -res_so3 = [Controllers.RBState(model, x) for x in X2[inds]] -e2 = norm(res_so3 .⊖ Xref) -@test e1 < e2 - - -# Test MLQR -model = Dynamics.Quadrotor2() -Q_ = (200.,200,50,50) -R = Diagonal(@SVector fill(1.0,4)) -xref = zeros(model)[1] -uref = Dynamics.trim_controls(model) -dt_ctrl = 0.01 -x0 = Controllers.RBState([0,0,0.2], expm(deg2rad(80)*@SVector [1,0,0]), [0,0,1.], [0.5,0.5,0]) -tf = 10.0 - -Q = Diagonal(Dynamics.fill_error_state(model, Q_...)) -cntrl = Controllers.MLQR(model, dt_ctrl, Q, R, xref, uref) -xinit = Dynamics.build_state(model, x0) -res = Controllers.simulate(model, cntrl, xinit, tf, dt=dt, w=0.0) -err = TrajectoryOptimization.state_diff(model, res[end], xref) -@test norm(err) < 0.01 - -# Test LQR -model = Dynamics.Quadrotor2{RodriguesParam{Float64}}(use_rot=false) -Q = Diagonal(Dynamics.fill_error_state(model, Q_...)) -xref = zeros(model)[1] -cntrl = Controllers.LQR(model, dt_ctrl, Q, R, xref, uref) -xinit = Dynamics.build_state(model, x0) -res = Controllers.simulate(model, cntrl, xinit, tf, dt=dt, w=0.0) - -# Test HFCA -Rot = UnitQuaternion{Float64,IdentityMap} -model = Dynamics.Quadrotor2{Rot}(use_rot=false) - -times = range(0,tf,step=dt) -xref = zeros(model)[1] -Xref = [copy(xref) for k = 1:length(times)] -bref = [@SVector [1,0,0.] for k = 1:length(times)] -Xdref = [copy(xref) for k = 1:length(times)] - -cntrl = Controllers.HFCA(model, Xref, Xdref, bref, collect(times)) -xinit = Dynamics.build_state(model, x0) -res = Controllers.simulate(model, cntrl, xinit, tf, dt=dt, w=0.0) -err = TrajectoryOptimization.state_diff(model, res[end], xref) -@test norm(err) < 0.01 - -# Test SE3 -cntrl = Controllers.SE3Tracking(model, Xref, Xdref, bref, collect(times)) -xinit = Dynamics.build_state(model, x0) -res = Controllers.simulate(model, cntrl, xinit, tf, dt=dt, w=0.0) -err = TrajectoryOptimization.state_diff(model, res[end], xref) -@test norm(err) < 0.01 diff --git a/test/old/copymodel_test.jl b/test/old/copymodel_test.jl deleted file mode 100644 index 90db808c..00000000 --- a/test/old/copymodel_test.jl +++ /dev/null @@ -1,177 +0,0 @@ -using TrajectoryOptimization -using BenchmarkTools -using ForwardDiff -using LinearAlgebra -using StaticArrays -using Test -using TrajOptPlots -using MeshCat -const TO = TrajectoryOptimization - - -model = Dynamics.DubinsCar() -if !isdefined(Main,:vis) - vis = Visualizer(); open(vis); - set_mesh!(vis, model) -end - -models = Dynamics.CopyModel(model, 2) -size(models) == (6,4) -n,m = size(models) -zeros(models) - -x,u = rand(models) -x1,u1 = x[1:3], u[1:2] -x2,u2 = x[4:6], u[3:4] -@test !(states(models, x, 1) ≈ states(models, x, 2)) -@test !(states(models, x, 1) ≈ states(models, x, 2)) - -@test dynamics(models, x, u) == [dynamics(model, x1,u1); dynamics(model,x2,u2)] -z = KnotPoint(x,u,0.1) -z1 = KnotPoint(x1,u1,0.1) -z2 = KnotPoint(x2,u2,0.1) -@test discrete_dynamics(RK3, models, z) == - [discrete_dynamics(model, z1);discrete_dynamics(model, z2)] - -function my_jacobian(z::SVector) - n,m = size(models) - ix,iu = 1:n, n .+ (1:m) - f_aug(z) = dynamics(models, view(z,ix), view(z,iu)) - ForwardDiff.jacobian(f_aug, z) -end -@test jacobian(models, z) ≈ my_jacobian(z.z) - -function my_discrete_jacobian(s::SVector{NM1}) where NM1 - t = 0.0 - ix = @SVector [1,2,3,4,5,6] - iu = @SVector [7,8,9,10] - idt = NM1 - fd_aug(s) = discrete_dynamics(RK3, models, s[ix], s[iu], t, s[idt]) - ForwardDiff.jacobian(fd_aug, s) -end -s = [z.z; @SVector [z.dt]] -@test my_discrete_jacobian(s) ≈ discrete_jacobian(RK3, models, z) - -xf = rand(models)[1] -@test TO.state_diff(models, x, xf) == x - xf -@test TO.state_diff_jacobian(models, x) == I -@test TO.∇²differential(models, x, @SVector rand(6)) == I*0 - - -# Test solve -N = 101 -tf = 5.0 - -x0_1 = @SVector [ 1,0,pi/2] -x0_2 = @SVector [-1,0,pi/2] -x0 = Dynamics.build_state(models, x0_1, x0_2) - -xf_1 = @SVector [-1,2,pi/2] -xf_2 = @SVector [ 1,2,pi/2] -xf = Dynamics.build_state(models, xf_1, xf_2) - -Q = Diagonal(@SVector fill(1e-2,n)) -Rd = @SVector [0.1, 1e-2] -R = Diagonal( [Rd; Rd] ) -obj = LQRObjective(Q,R,Q*100,xf,N) - -conSet = ConstraintSet(n,m,N) -con = TO.CollisionConstraint(n, (@SVector [1,2]), (@SVector [4,5]), 0.5) -add_constraint!(conSet, con, 1:N-1) - -prob = Problem(models, obj, xf, tf, x0=x0, constraints=conSet) -# solver = iLQRSolver(prob) -solver = AugmentedLagrangianSolver(prob) -@test size(solver) == (6,4,N) -solve!(solver) -@test iterations(solver) < 20 # should be 16 -@test max_violation(solver) < 1e-10 # should be 0 - -prob = Problem(models, obj, xf, tf, x0=x0, constraints=conSet) -solver = iLQRSolver2(prob) -solve!(solver) -visualize!(vis, solver) - -# Test with rigid bodies -Rot = UnitQuaternion{Float64,CayleyMap} -model = Dynamics.FreeBody{Rot,Float64}() -models = Dynamics.CopyModel(model, 2) - -x,u = zeros(models) -x = repeat(zeros(model)[1],2) -u = repeat(zeros(model)[2],2) -x,u = rand(models) -x1,u1 = SVector{13}(x[1:13]), SVector{6}(u[1:6]) -x2,u2 = SVector{13}(x[14:26]), SVector{6}(u[7:12]) -@test !(states(models, x, 1) ≈ states(models, x, 2)) -@test !(states(models, x, 1) ≈ states(models, x, 2)) - -@test dynamics(models, x, u) == [dynamics(model, x1,u1); dynamics(model,x2,u2)] -z = KnotPoint(x,u,0.1) -z1 = KnotPoint(x1,u1,0.1) -z2 = KnotPoint(x2,u2,0.1) -@test discrete_dynamics(RK3, models, z) == - [discrete_dynamics(model, z1);discrete_dynamics(model, z2)] - -function my_jacobian(z::SVector) - n,m = size(models) - ix,iu = 1:n, n .+ (1:m) - f_aug(z) = dynamics(models, view(z,ix), view(z,iu)) - ForwardDiff.jacobian(f_aug, z) -end -@test jacobian(models, z) ≈ my_jacobian(z.z) - -function my_discrete_jacobian(s::SVector{NM1}) where NM1 - t = 0.0 - ix = z._x - iu = z._u - idt = NM1 - fd_aug(s) = discrete_dynamics(RK3, models, s[ix], s[iu], t, s[idt]) - ForwardDiff.jacobian(fd_aug, s) -end -s = [z.z; @SVector [z.dt]] -@test my_discrete_jacobian(s) ≈ discrete_jacobian(RK3, models, z) - -xf = rand(models)[1] -x1f = SVector{13}(xf[1:13]) -x2f = SVector{13}(xf[14:26]) -@test TO.state_diff(models, x, xf) == - [TO.state_diff(model, x1, x1f); TO.state_diff(model, x2, x2f)] -G1 = TO.state_diff_jacobian(model, x1) -G2 = TO.state_diff_jacobian(model, x2) -@test cat(G1,G2,dims=[1,2]) ≈ TO.state_diff_jacobian(models, x) -b1 = @SVector rand(12) -b2 = @SVector rand(12) -∇G1 = TO.∇²differential(model, x1, b1) -∇G2 = TO.∇²differential(model, x2, b2) -@test cat(∇G1, ∇G2, dims=[1,2]) ≈ TO.∇²differential(models, x, [b1;b2]) - - -# Test solve -N = 101 -tf = 5.0 - -x01 = Dynamics.build_state(model, [0, 1,0], UnitQuaternion(I), zeros(3), zeros(3)) -x02 = Dynamics.build_state(model, [0,-1,0], UnitQuaternion(I), zeros(3), zeros(3)) -x0 = [x01; x02] - -xf1 = Dynamics.build_state(model, [ 1,0,0], expm(deg2rad( 45)*@SVector [1,0,0.]), zeros(3), zeros(3)) -xf2 = Dynamics.build_state(model, [-1,0,0], expm(deg2rad(-45)*@SVector [1,0,0.]), zeros(3), zeros(3)) -xf = [xf1; xf2] - -Qd = Dynamics.fill_state(model, 1e-1, 1e-1, 1e-2, 1e-2) -Q = Diagonal([Qd; Qd]) -Rd = @SVector fill(1e-2, 6) -R = Diagonal([Rd; Rd]) -obj = LQRObjective(Q,R,Q*10,xf,N) - -prob = Problem(models, obj, xf, tf, x0=x0) -solver = iLQRSolver(prob) -solver.opts.verbose = true -solve!(solver) - -if !isdefined(Main,:vis) - vis = Visualizer(); open(vis); - set_mesh!(vis, model) -end -visualize!(vis, models, get_trajectory(solver)) diff --git a/test/old/ddl.jl b/test/old/ddl.jl deleted file mode 100644 index 090c23a3..00000000 --- a/test/old/ddl.jl +++ /dev/null @@ -1,125 +0,0 @@ -using Parameters, StaticArrays, BenchmarkTools -import TrajectoryOptimization: dynamics, discrete_dynamics, RK3, AbstractModel, KnotPoint, - jacobian, discrete_jacobian - -@with_kw mutable struct BicycleCar{T} <: AbstractModel - k::T = 0.1 - a::T = 1. # dist to front axle - b::T = 1. # dist to rear axle - h::T = 1. - mass::T = 1000. # mass - Iz::T = 10. - Cf::T = 1. - Cr::T = 1. - μ::T = 0.1 - Cd0::T = 0.1 - Cd1_Npmps::T = 0.1 - g::T = 9.81 -end - -Base.size(::BicycleCar) = 8,2 - -function dynamics(car::BicycleCar, x, u) - δ_dot = u[1] - fx_dot = u[2] - δ = x[1] - fx = x[2] - r = x[3] - Uy = x[4] - Ux = x[5] - Δψ = x[6] - e = x[7] - t = x[8] - - # Drag Force - Fx_drag = 0.0 - - # Spatial derivative - s_dot = (Ux * cos(Δψ) - Uy * sin(Δψ)) / (1 - car.k*e) - - # Get tire forces - Fxf, Fxr, Fyf, Fyr = logit_lateral_force_model(car, x) - - # State Derivatives - r_dot = (car.a * (Fxf * cos(δ) + Fxf*sin(δ)) - car.b * Fyr) / car.Iz - Ux_dot = r * Uy + (Fxf * cos(δ) - Fyf * sin(δ) + Fxr - Fx_drag) / car.mass - Uy_dot = -r * Ux + (Fyf * cos(δ) + Fxf * sin(δ)) / car.mass - Δψ_dot = r - car.k * s_dot - e_dot = Ux * sin(Δψ) + Uy * cos(Δψ) - t_dot = 1 / s_dot - - return @SVector [δ_dot, fx_dot, r_dot, Uy_dot, Ux_dot, Δψ_dot, e_dot, t_dot] -end - -function FWD_force_model(car::BicycleCar, fx) - l = car.a + car.b - Fxf = max(fx * car.b / l, fx) - Fxr = min(fx * car.a / l, 0.0) - Fzf = car.mass * car.g * car.b / l - Fzr = car.mass * car.g * car.a / l - return Fxf, Fxr, Fzf, Fzr -end - -function logit_lateral_force_model(car::BicycleCar, x) - fx = x[2] - r = x[3] - Uy = x[4] - Ux = x[5] - Cα = 1.0 - - α_f = atan(Uy + car.a * r, Ux) - α_r = atan(Uy - car.b * r, Ux) - - Fxf, Fxr, Fzf, Fzr = FWD_force_model(car, fx) - Fyf = lateral_tire_model(car.μ, Cα, α_f, Fxf, Fzf) - Fyr = lateral_tire_model(car.μ, Cα, α_r, Fxr, Fzr) - return Fxf, Fxr, Fyf, Fyr -end - -function lateral_tire_model(μ, Cα, α, Fx, Fz) - Fy_max = sqrt((μ*Fz)^2 - Fx^2) - slope = 2*Cα / Fy_max - Fy = Fy_max * (1-2*exp(slope*α)) / (1 + exp(slope*α)) - return Fy -end - - -struct ContingencyCar{T} <: AbstractModel - cars::Vector{BicycleCar{T}} - ix::Vector{SVector{8,Int}} - iu::Vector{SVector{2,Int}} -end - -function ContingencyCar(num_cars=2) - cars = [BicycleCar() for i = 1:num_cars] - n,m = 8,2 - ix = [SVector{n}((1:n) .+ (i-1)*n) for i = 1:num_cars] - iu = [SVector{m}((1:m) .+ (i-1)*m) for i = 1:num_cars] - ContingencyCar(cars, ix, iu) -end - - -Base.size(::ContingencyCar) = 16,4 - -function dynamics(car::ContingencyCar, x, u) - ix, iu = car.ix, car.iu - cars = car.cars - x1,u1 = x[ix[1]], u[iu[1]] - ẋ1 = dynamics(cars[1], x1, u1) - x2,u2 = x[ix[2]], u[iu[2]] - ẋ2 = dynamics(cars[2], x1, u1) - return [ẋ1; ẋ2] -end - - -car = ContingencyCar() -n,m = size(car) -x = @SVector rand(n) -u = @SVector rand(m) -z = KnotPoint(x,u,0.1) - -@btime dynamics($car, $x, $u) - -@btime discrete_dynamics(RK3, $car, $x, $u, 0.1) - -@btime discrete_jacobian(RK3, $car, $z) diff --git a/test/old/dynamics_constraints.jl b/test/old/dynamics_constraints.jl deleted file mode 100644 index 812ea4a9..00000000 --- a/test/old/dynamics_constraints.jl +++ /dev/null @@ -1,44 +0,0 @@ -model = Dynamics.Cartpole() -prob = Problems.Cartpole()[1] -bnd = prob.constraints.constraints[1] -n,m = size(prob) - -N = prob.N -rollout!(prob) -Z = prob.Z -vals = [@SVector zeros(model.n) for k = 1:N-1] - -∇c = [SizedMatrix{n,2n+m}(zeros(n,2n+m)) for k = 1:N-1] -dyn_con = DynamicsConstraint{RK3}(model, N) -@test TO.width(dyn_con) == 2n+m -evaluate!(vals, dyn_con, Z) -jacobian!(∇c, dyn_con, Z) -@test (@allocated evaluate!(vals, dyn_con, Z)) == 0 -@test (@allocated jacobian!(∇c, dyn_con, Z)) == 0 - -con_rk3 = ConstraintVals(dyn_con, 1:N-1) -evaluate!(con_rk3, Z) -jacobian!(con_rk3, Z) -TO.max_violation!(con_rk3) -maximum(con_rk3.c_max) -@test (@allocated evaluate!(con_rk3, Z)) == 0 -@test (@allocated jacobian!(con_rk3, Z)) == 0 - -∇c = [zeros(SizedMatrix{n,2n+2m}) for k = 1:N-1] -dyn_con = DynamicsConstraint{HermiteSimpson}(model, N) -@test TO.width(dyn_con) == 2(n+m) -evaluate!(vals, dyn_con, Z) -jacobian!(∇c, dyn_con, Z) -@test (@allocated evaluate!(vals, dyn_con, Z)) == 0 -@test (@allocated jacobian!(∇c, dyn_con, Z)) == 0 - -con_hs = ConstraintVals(dyn_con, 1:N-1) -evaluate!(con_hs, Z) -jacobian!(con_hs, Z) -@test (@allocated evaluate!(con_hs, Z)) == 0 -@test (@allocated jacobian!(con_hs, Z)) == 0 - - -# Test default -dyn_con = DynamicsConstraint(model, N) -@test integration(dyn_con) == RK3 == TO.DEFAULT_Q diff --git a/test/old/dynamics_tests.jl b/test/old/dynamics_tests.jl deleted file mode 100644 index e4815d98..00000000 --- a/test/old/dynamics_tests.jl +++ /dev/null @@ -1,86 +0,0 @@ -using StaticArrays, LinearAlgebra, BenchmarkTools -import TrajectoryOptimization: dynamics, jacobian, discrete_dynamics, discrete_jacobian -import TrajectoryOptimization: RigidBody - -# Get Quadrotor Model -model = Dynamics.Quadrotor2{UnitQuaternion{Float64,VectorPart}}() -n,m = size(model) -N = 101 # knot points - -# Build trajectory -X = [@SVector rand(n) for k = 1:N] -U = [@SVector rand(m) for k = 1:N-1] -dt = fill(0.1, N) -Z = Traj(X,U,dt) # vector of KnotPoints - -# Allocate storage arrays -fVal = [@SVector zeros(n) for k = 1:N-1] -∇f = [@SMatrix zeros(n,n+m) for k = 1:N-1] -∇fd = [@SMatrix zeros(n,n+m+1) for k = 1:N-1] # discrte Jacobian also takes Jacobian wrt time - - -# Continuous Dynamics -function dynamics(fVal, model::AbstractModel, Z::Traj) - @inbounds for k in eachindex(fVal) - # Most straightforward, passes through 2 "conversion functions": - # dynamics(model, z::KnotPoint) = dynamics(model, state(z), control(z), z.t) - # dynamics(model, x, u, t) = dynamics(model, x, u) - # The first extracts the state, control, and time from KnotPoint - # The second will drop dependence on time, by default - fVal[k] = dynamics(model, Z[k]) - - # ALTERNATE: call the dynamics function directly, by-passing the above pipeline - # dynamics(model, state(Z[k]), control(Z[k])) - # - # This function is defined in the Dynamics module - end -end - -# Continuous Jacobian -function jacobian(∇f, model::AbstractModel, Z::Traj) - @inbounds for k in eachindex(∇f) - # This function is defined in static_model.jl::93 - # It's best to pass in the KnotPoint directly, since having the state and control - # stacked together avoids some allocations when using ForwardDiff - ∇f[k] = jacobian(model, Z[k]) - end -end - -# Discrete Dynamics -function discrete_dynamics(fVal, model::AbstractModel, Z::Traj) - @inbounds for k in eachindex(fVal) - # Most straightforward, passes through 1 "conversion function": - # discrete_dynamics(::Type{Q}, model, z::KnotPoint) = discrete_dynamics(Q, model, state(z), control(z), z.t, z.dt) - fVal[k] = discrete_dynamics(RK3, model, Z[k]) - - # ALTERNATE: bypass the extra function by making the call directly to the RK3 function - # defined in integration.jl::5 - # discrete_dynamics(RK3, model, state(z), control(z), z.t, z.dt) - end - end -end - -# Discrete Dynamics jacobian -function discrete_jacobian(∇fd, model::AbstractModel, Z::Traj) - @inbounds for k in eachindex(∇fd) - # This function is defined in static_model.jl::138 - ∇fd[k] = discrete_jacobian(RK3, model, Z[k]) - end -end - -# Test functions -dynamics(fVal, model, Z) -model -get_orientation(model::RigidBody{UnitQuaternion{T,D}}, x::SVector{N,T2}) where {T,D,N,T2} = - UnitQuaternion{T2,D}(x[4],x[5],x[6],x[7]) -x,u = rand(model) -normalize(get_orientation(model,x)) -jacobian(∇f, model, Z) -discrete_dynamics(fVal, model, Z) -discrete_jacobian(∇fd, model, Z) - -# Benchmark functions (should all have 0 allocations) -@btime dynamics($fVal, $model, $Z) -@btime jacobian($∇f, $model, $Z) -@btime discrete_dynamics($fVal, $model, $Z) -@btime discrete_jacobian($∇fd, $model, $Z) diff --git a/test/old/infeasible_start.jl b/test/old/infeasible_start.jl deleted file mode 100644 index cfa8cd51..00000000 --- a/test/old/infeasible_start.jl +++ /dev/null @@ -1,119 +0,0 @@ -prob = copy(Problems.car_3obs_static) -n,m,N = size(prob) -U0 = deepcopy(controls(prob)) -dt = prob.Z[1].dt - -# Generate an initial state trajectory -N = prob.N -x0 = prob.x0 -xf = prob.xf -x_guess = SVector{N}(range(x0[1], stop=xf[1], length=N)) -y_guess = SVector{N}(range(x0[2], stop=xf[2], length=N)) -X0 = [SVector(x_guess[k], y_guess[k], x0[3]) for k = 1:N] -Z0 = Traj(X0,U0,dt*ones(N)) - -# Create infeasible model -inf_model = InfeasibleModel(prob.model) -inf_con = InfeasibleConstraint(inf_model) - -Z = infeasible_trajectory(inf_model, Z0) -@test states(Z) ≈ states(Z0) -rollout!(inf_model, Z, x0) -@test states(Z) ≈ states(Z0) - -# Test constraints -model = prob.model -x,u0 = rand(model) -ui = @SVector rand(n) -u = [u0; ui] - -z0 = KnotPoint(x,u0,dt) -z = KnotPoint(x,u,dt) - -function test_con_allocs(con,z) - allocs = @allocated evaluate(con, z) - allocs += @allocated jacobian(con, z) -end - - -con = prob.constraints[1].con -idx_con = IndexedConstraint(n,n+m,con) -@test evaluate(idx_con, z) == evaluate(con, z0) -@test jacobian(idx_con, z) == jacobian(con, z0) -@test test_con_allocs(idx_con, z) == 0 - -bnd = BoundConstraint(n,m, u_min=[0,-2], u_max=[Inf,2]) -idx_bnd = IndexedConstraint(n,n+m,bnd) - -@test test_con_allocs(idx_bnd, z) == 0 - -evaluate(bnd, z0) -evaluate(idx_bnd, z) -@test evaluate(bnd, z0) == evaluate(idx_bnd, z) -@test [jacobian(bnd, z0) zeros(3,3)] == jacobian(idx_bnd, z) - -conSet0 = copy(get_constraints(prob)) -add_constraint!(conSet0, bnd, 1:N-1, 1) -conSet = change_dimension(conSet0, n, n+m) -evaluate!(conSet, Z) -jacobian!(conSet, Z) -@test size(conSet[1].∇c[1]) == (3,8) -@test size(conSet0[1].∇c[1]) == (3,5) - - -# Cost functions -cost0 = prob.obj[1] -ix = 1:n -iu = 1:m -idx_cost = IndexedCost(cost0,ix,iu) -@test stage_cost(cost0, x, u0) ≈ stage_cost(idx_cost, x, u) -@test stage_cost(cost0, x) ≈ stage_cost(idx_cost, x) -Qx0, Qu0 = gradient(cost0, x, u0) -Qx, Qu = gradient(idx_cost, x, u) -@test Qx0 == Qx -@test [Qu0; zeros(n)] == Qu - -Qxx0, Quu0, Qux0 = hessian(cost0, x, u0) -Qxx, Quu, Qux = hessian(idx_cost, x, u) -@test Qxx0 == Qxx -@test Qux == zeros(n+m,n) -@test Quu == Diagonal([diag(Quu0); zeros(n)]) - -costfun = change_dimension(cost0, n, n+m) -@test stage_cost(costfun, x, u) == stage_cost(idx_cost, x, u) -@test gradient(costfun, x, u) == gradient(idx_cost, x, u) -@test hessian(costfun, x, u) == hessian(idx_cost, x, u) -@test costfun.Q isa Diagonal{Float64,<:SVector} -@test costfun.R isa Diagonal{Float64,<:SVector} -@test costfun.H isa SMatrix -@test costfun.q isa SVector -@test costfun.r isa SVector - -costs = infeasible_objective(prob.obj, 2.3) -@test costs[1].Q == cost0.Q -@test costs[1].R == Diagonal([diag(cost0.R); fill(2.3,n)]) -@test costs[1].q == cost0.q -@test costs[1].r == [cost0.r; zeros(n)] -@test sum(costs[1].H) == sum(cost0.H) - - -# Test the solve -sprob = copy(Problems.car_3obs_static) -sal = AugmentedLagrangianSolver(sprob) -solve!(sal) -max_violation(sal) - - -# Test Infeasible solve -sprob = copy(Problems.car_3obs_static) -initial_trajectory!(sprob, Z0) -sprob_inf = InfeasibleProblem(sprob, Z0, 0.01/sprob.Z[1].dt) -sal = AugmentedLagrangianSolver(sprob_inf) -Z_init = copy(sprob_inf.Z) - -initial_trajectory!(sal, Z_init) -solve!(sal) -max_violation(sal) - -initial_trajectory!(sal, Z_init) -@test (@allocated solve!(sal)) == 0 diff --git a/test/old/knotpoint_tests.jl b/test/old/knotpoint_tests.jl deleted file mode 100644 index ec8b9e21..00000000 --- a/test/old/knotpoint_tests.jl +++ /dev/null @@ -1,23 +0,0 @@ -using Test -using StaticArrays - -model = Dynamics.DubinsCar() -x,u = rand(model) -n,m = size(model) -N = 101 -dt = 0.1 -tf = 10.0 - -X = [@SVector rand(n) for k = 1:N] -U = [@SVector rand(m) for k = 1:N-1] -Z = Traj(x,u,dt,N) -@test Z[1].dt == dt -@test Z[1].t == 0 -@test Z[end].dt == 0 -@test Z[end].t == tf - -Z = Traj(X,U,fill(dt,N)) -@test Z[1].dt == dt -@test Z[1].t == 0 -@test Z[end].dt == 0 -@test Z[end].t ≈ tf diff --git a/test/old/logger_tests.jl b/test/old/logger_tests.jl deleted file mode 100644 index b55531d0..00000000 --- a/test/old/logger_tests.jl +++ /dev/null @@ -1,100 +0,0 @@ -import TrajectoryOptimization: LogData, cache_size, add_col!, create_header, - create_row, cache_data!, clear! - -# Default log data -ld = LogData() -@test isempty(ld.cols) -@test isempty(ld.data) -@test ld.color == :default - -cols = [:iter,:cost] -width = [8,10,10] -ld = LogData(cols,width) -@test ld.cols == cols -@test !(ld.cols === cols) -@test !(ld.widths === width) -@test cache_size(ld) == 0 -add_col!(ld,:c_max, 15, vartype=Float64) -@test ld.cols == [:iter,:cost,:c_max] -@test ld.widths[3] == 15 -add_col!(ld,:outer, 15, 2, vartype=Float64, do_print=false) -@test ld.cols == [:iter,:outer,:cost,:c_max] -@test ld.print[2] == false -add_col!(ld,:cost) -@test length(ld.cols) == 4 - -strhead = create_header(ld) -@test occursin("iter",strhead) -@test !occursin("outer",strhead) -@test ld.freq == 10 - -# Add entry -ld.data[:iter] = 1 -ld.data[:outer] = 10 -ld.data[:cost] = 2.5 -strrow = create_row(ld) -@test occursin("1",strrow) -@test occursin("2.5",strrow) -@test !occursin("10",strrow) - -# Cache row -cache_data!(ld) -@test ld.cache[:iter][1] == 1 -@test ld.cache[:outer][1] == 10 -@test isnan(ld.cache[:c_max][1]) -@test ld.data[:iter] == 1 - -# Clear data -clear!(ld) -@test ld.data[:iter] == "" - - -# Logger Tests -import TrajectoryOptimization: SolverLogger, add_level!, InnerLoop, OuterLoop, - print_header, print_row, clear_cache! - -logger = SolverLogger(InnerLoop, default_width=15) -add_level!(logger, InnerLoop, cols, width; print_color=:green, indent=4) -@test logger.leveldata[InnerLoop].indent == 4 -add_level!(logger, InnerLoop, cols, width; print_color=:green, indent=5) -@test logger.leveldata[InnerLoop].indent == 5 -@test length(logger.leveldata) == 1 -@test :iter in logger[InnerLoop].cols - -# Add some data -with_logger(logger) do - global_logger(logger) - @logmsg InnerLoop :iter value=5 - @test logger[InnerLoop].data[:iter] == 5 - @test logger[InnerLoop].data[:cost] == "" - @logmsg InnerLoop :cost value=10.5 - @test logger[InnerLoop].data[:cost] == 10.5 - - # Printing clears and caches the values - print_row(logger,InnerLoop) - @test logger[InnerLoop].data[:iter] == "" - @test logger[InnerLoop].data[:cost] == "" - @test logger[InnerLoop].cache[:iter][1] == 5 - @test logger[InnerLoop].cache[:cost][1] == 10.5 - @test cache_size(logger[InnerLoop]) == 1 - - # Add column - @logmsg InnerLoop :c_max value=3.2 - @test logger[InnerLoop].cols[3] == :c_max - @logmsg InnerLoop :iter value=2 - @test logger[InnerLoop].data[:iter] == 2 - @test logger[InnerLoop].data[:c_max] == 3.2 - - # Add Info column - @logmsg InnerLoop "Something happened" - @test logger[InnerLoop][:info][1] == "Something happened" - @logmsg InnerLoop "Awesome" - @test logger[InnerLoop][:info][2] == "Awesome" - strrow = create_row(logger[InnerLoop]) - @test occursin("Something happened. Awesome", strrow) - - # Try a level not in the logger - @test_logs (:warn,:iter) @warn :iter # Should log - @test_logs @logmsg LogLevel(-500) :hi # No logs - clear_cache!(logger[InnerLoop]) -end diff --git a/test/old/models_test.jl b/test/old/models_test.jl deleted file mode 100644 index b39b561e..00000000 --- a/test/old/models_test.jl +++ /dev/null @@ -1,89 +0,0 @@ -using Test, BenchmarkTools - -function num_allocs(model) - dt = 0.1 - x, u = rand(model) - n,m = size(model) - z = KnotPoint(x, u, dt) - ∇c = zeros(n,n+m) - ∇cd = zeros(n,n+m+1) - dynamics(model, x, u) - jacobian!(∇c, model, z) - discrete_dynamics(RK3, model, x, u, z.t, dt) - discrete_jacobian!(RK3, ∇cd, model, z) - allocs = @allocated dynamics(model, x, u) - allocs += @allocated jacobian!(∇c, model, z) - allocs += @allocated discrete_dynamics(RK3, model, x, u, z.t, dt) - allocs += @allocated discrete_jacobian!(RK3, ∇cd, model, z) -end - -# Double Integrator -dim = 3 -di = Dynamics.DoubleIntegrator(dim) -n,m = size(di) -@test (n,m) == (6,3) -x,u = rand(di) -@test num_allocs(di) == 0 - -# Pendulum -pend = Dynamics.Pendulum() -@test size(pend) == (2,1) -@test num_allocs(pend) == 0 - -# Car -car = Dynamics.DubinsCar() -@test size(car) == (3,2) -@test num_allocs(car) == 0 - -# Cartpole -cartpole = Dynamics.Cartpole() -@test size(cartpole) == (4,1) -@test num_allocs(cartpole) == 0 - -# Quadrotor -quad = Dynamics.Quadrotor2() -@test size(quad) == (13,4) -@test num_allocs(quad) == 0 - - -# Infeasible -model = Dynamics.DubinsCar() -inf = InfeasibleModel(model) -@test inf._u == 1:2 -@test inf._ui == 3:5 -@test size(inf) == (3,5) -x = rand(inf)[1] -u0 = @SVector rand(2) -ui = @SVector rand(3) -u = [u0; ui] -dt = 0.1 -@test length(rand(inf)[2]) == 5 -@test_throws ErrorException dynamics(inf, x, u) -@test discrete_dynamics(RK3, inf, KnotPoint(x, u, dt)) == - (discrete_dynamics(RK3, model, x, u0, 0.0, dt) + ui) - -function inf_allocs(inf) - x,u = rand(inf) - n,m = size(inf) - dt = 0.1 - z = KnotPoint(x,u,0.1) - # allocs = @allocated discrete_dynamics(RK3, inf, x, u, dt) - ∇c = zeros(n,n+m+1) - allocs = @allocated discrete_dynamics(RK3, inf, z) - allocs += @allocated discrete_jacobian!(RK3, ∇c, inf, z) -end -@test inf_allocs(inf) == 0 - - -# Test other functions -car = Dynamics.DubinsCar() -n,m = size(car) -@test zeros(car) == (zeros(n), zeros(m)) -@test zeros(Int,car)[1] isa SVector{n,Int} -@test fill(car,0.1) == (fill(0.1,n), fill(0.1,m)) -@test ones(Float32,car)[2] isa SVector{m,Float32} - -# Test default integrator -x,u = rand(car) -z = KnotPoint(x,u,dt) -@test discrete_dynamics(car, z) == discrete_dynamics(RK3, car, z) diff --git a/test/old/quadrotor_tests.jl b/test/old/quadrotor_tests.jl deleted file mode 100644 index 7b90cbf8..00000000 --- a/test/old/quadrotor_tests.jl +++ /dev/null @@ -1,151 +0,0 @@ -using Random -using StaticArrays -using LinearAlgebra -using MeshCat -using TrajOptPlots - -vis = Visualizer(); open(vis); - -max_con_viol = 1.0e-8 -T = Float64 -verbose = true - -opts_ilqr = iLQRSolverOptions{T}(verbose=verbose, - cost_tolerance=1e-4, - iterations=300) - -opts_al = AugmentedLagrangianSolverOptions{T}(verbose=verbose, - opts_uncon=opts_ilqr, - iterations=40, - cost_tolerance=1.0e-5, - cost_tolerance_intermediate=1.0e-4, - constraint_tolerance=max_con_viol, - penalty_scaling=10., - penalty_initial=1.) - - - -# Step through the solve -model = Dynamics.Quadrotor() -n,m = 13,4 - -# discretization -N = 101 # number of knot points -tf = 5.0 -dt = tf/(N-1) # total time - -q0 = @SVector [1,0,0,0] - -x0_pos = @SVector [0., -10., 1.] -x0 = [x0_pos; q0; @SVector zeros(6)] - -xf = zero(x0) -xf_pos = @SVector [0., 50., 1.] -xf = [xf_pos; q0; @SVector zeros(6)] - -u0 = @SVector fill(0.5*9.81/4, m) -U_hover = [copy(u0) for k = 1:N-1] # initial hovering control trajectory - -# cost -Qdiag = fill(1e-3,n) -Qdiag[4:7] .= 1e-2 -Q = Diagonal(SVector{13}(Qdiag)) -R = (1.0e-4)*Diagonal(@SVector ones(m)) -Qf_diag = 1000*Qdiag; -Qf = Diagonal(SVector{13}(Qf_diag)) -obj = LQRObjective(Q,R,Qf,xf,N, uf=u0) - -conSet = ConstraintSet(n,m,N) -bnd = BoundConstraint(n,m, u_min=0) -add_constraint!(conSet, bnd, 1:N-1) - -# Test entire solves -model = Dynamics.Quadrotor(use_quat=false) -prob = Problem(model, obj, xf, tf, x0=x0, constraints=conSet) -# prob = copy(Problems.quadrotor_static) -initial_controls!(prob, U_hover) -solver = iLQRSolver(prob, opts_ilqr) -solver = AugmentedLagrangianSolver(prob, opts_al) -solve!(solver) -solver.stats.iterations_total -solver.stats.iterations -cost(solver) -visualize!(vis, model, get_trajectory(solver)) -plot(controls(solver)) - -model = Dynamics.Quadrotor(use_quat=true) -prob = Problem(model, obj, xf, tf, x0=x0, constraints=conSet) -# prob = Problem(copy(Problems.quadrotor_static), model=model) -initial_controls!(prob, U_hover) -solver = iLQRSolver(prob, opts_ilqr) -solver = AugmentedLagrangianSolver(prob, opts_al) -solve!(solver) -solver.stats.iterations_total -solver.stats.iterations -cost(solver) -visualize!(vis, model, get_trajectory(solver)) - -model = Dynamics.Quadrotor(use_quat=false) -prob = Problem(model, obj, xf, tf, x0=x0, constraints=conSet) -solver = iLQRSolver(prob, opts_ilqr) -solver = AugmentedLagrangianSolver(prob, opts_al) -solver.opts.verbose = false -@btime begin - initial_controls!($solver, $U_hover) - solve!($solver) -end - -model = Dynamics.Quadrotor(use_quat=true) -prob = Problem(model, obj, xf, tf, x0=x0, constraints=conSet) -solver = iLQRSolver(prob, opts_ilqr) -solver = AugmentedLagrangianSolver(prob, opts_al) -solver.opts.verbose = false -@btime begin - initial_controls!($solver, $U_hover) - solve!($solver) -end - -plot(controls(solver)) - -# Solver -solver = iLQRSolver(prob, opts_ilqr) -size(solver.K[1]) == (4,13) -solver.G[1] isa LinearAlgebra.UniformScaling - -J = cost(solver) -Z = solver.Z -state_diff_jacobian!(solver.G, solver.model, Z) -discrete_jacobian!(solver.∇F, solver.model, Z) -cost_expansion!(solver.Q, solver.obj, solver.Z) -solver.∇F[50] -solver.Q.x[60] -ΔV = backwardpass!(solver) -forwardpass!(solver, ΔV, J) -for k = 1:N - solver.Z[k].z = solver.Z̄[k].z -end - - -# With Quaternion -model = Dynamics.Quadrotor(use_quat=true) -prob = Problem(model, obj, xf, tf, x0=x0) -initial_controls!(prob, U_hover) -rollout!(prob) -cost(prob) - -solver = iLQRSolver(prob, opts_ilqr) -size(solver.K[1]) == (4,12) -solver.G[1] isa SMatrix - -J = cost(solver) -Z = solver.Z -state_diff_jacobian!(solver.G, solver.model, Z) -discrete_jacobian!(solver.∇F, solver.model, Z) -cost_expansion!(solver.Q, solver.obj, solver.Z) -solver.∇F[50] -solver.Q.x[60] -ΔV = backwardpass!(solver) -forwardpass!(solver, ΔV, J) -for k = 1:N - solver.Z[k].z = solver.Z̄[k].z -end diff --git a/test/old/retraction_maps.jl b/test/old/retraction_maps.jl deleted file mode 100644 index c4720c0f..00000000 --- a/test/old/retraction_maps.jl +++ /dev/null @@ -1,89 +0,0 @@ -# Exponential -ϕ = @SVector rand(3) -@test ForwardDiff.jacobian(x->SVector(ExponentialMap(x)),ϕ) ≈ jacobian(ExponentialMap,ϕ) -@test retraction_map(ExponentialMap(ϕ)) == ExponentialMap - -ϕ = 1e-6*@SVector rand(3) -@test ForwardDiff.jacobian(x->SVector(ExponentialMap(x)),ϕ) ≈ jacobian(ExponentialMap,ϕ) -@test retraction_map(ExponentialMap(ϕ)) == ExponentialMap - -# Vector Part -v = 0.1*@SVector rand(3) -@test ForwardDiff.jacobian(x->SVector(VectorPart(x)),v) ≈ - jacobian(VectorPart, v) -@test retraction_map(VectorPart(v)) == VectorPart - -# Gibbs Vectors -g = @SVector rand(3) -@test ForwardDiff.jacobian(x->SVector(CayleyMap(x)),g) ≈ jacobian(CayleyMap, g) -@test retraction_map(CayleyMap(g)) == CayleyMap - -# MRPs -p = SVector(rand(MRP)) -@test ForwardDiff.jacobian(x->SVector(MRPMap(x)),p) ≈ - jacobian(MRPMap, p) -@test retraction_map(MRPMap(p)) == MRPMap - - -μ0 = 1/TO.scaling(VectorPart) -jac_eye = [@SMatrix zeros(1,3); μ0*Diagonal(@SVector ones(3))]; -@test jacobian(ExponentialMap, p*1e-10) ≈ jac_eye -@test jacobian(MRPMap, p*1e-10) ≈ jac_eye -@test jacobian(CayleyMap, p*1e-10) ≈ jac_eye -@test jacobian(VectorPart, p*1e-10) ≈ jac_eye - - -############################################################################################ -# INVERSE RETRACTION MAPS -############################################################################################ - -# Exponential Map -Random.seed!(1); -q = rand(UnitQuaternion) -q = UnitQuaternion{ExponentialMap}(q) -qval = SVector(q) -@test ExponentialMap(q) == TO.scaling(ExponentialMap)*logm(q) -@test ExponentialMap(ExponentialMap(q)) ≈ q -@test ExponentialMap(ExponentialMap(ϕ)) ≈ ϕ - -function invmap(q) - μ = TO.scaling(ExponentialMap) - v = @SVector [q[2], q[3], q[4]] - s = q[1] - θ = norm(v) - M = μ*2atan(θ, s)/θ - return M*v -end -@test invmap(qval) ≈ TO.scaling(ExponentialMap)*logm(q) - -qI = VectorPart(v*1e-5) -@test ForwardDiff.jacobian(invmap, qval) ≈ jacobian(ExponentialMap, q) -@test ForwardDiff.jacobian(invmap, SVector(qI)) ≈ jacobian(ExponentialMap, qI) - -# Vector Part -@test VectorPart(q) == TO.scaling(VectorPart)*qval[2:4] -@test VectorPart(VectorPart(q)) ≈ q -@test VectorPart(VectorPart(v)) ≈ v - -# Cayley -invmap(q) = 1/q[1] * @SVector [q[2], q[3], q[4]] -@test CayleyMap(q) ≈ TO.scaling(CayleyMap)*invmap(qval) -@test ForwardDiff.jacobian(invmap, qval) ≈ jacobian(CayleyMap, q) -@test CayleyMap(CayleyMap(q)) ≈ q -@test CayleyMap(CayleyMap(g)) ≈ g - -# MRP -invmap(q) = TO.scaling(MRPMap)/(1+q[1]) * @SVector [q[2], q[3], q[4]] -@test MRPMap(q) ≈ invmap(qval) -@test ForwardDiff.jacobian(invmap, qval) ≈ jacobian(MRPMap, q) -@test MRPMap(MRPMap(q)) ≈ q -@test MRPMap(MRPMap(p)) ≈ p - - -# Test near origin -μ0 = TO.scaling(VectorPart) -jacT_eye = [@SMatrix zeros(1,3); μ0*Diagonal(@SVector ones(3))]'; -@test isapprox(jacobian(ExponentialMap,qI), jacT_eye, atol=1e-5) -@test isapprox(jacobian(VectorPart,qI), jacT_eye, atol=1e-5) -@test isapprox(jacobian(CayleyMap,qI), jacT_eye, atol=1e-5) -@test isapprox(jacobian(MRPMap,qI), jacT_eye, atol=1e-5) diff --git a/test/old/rigidbody_tests.jl b/test/old/rigidbody_tests.jl deleted file mode 100644 index 9ae20f82..00000000 --- a/test/old/rigidbody_tests.jl +++ /dev/null @@ -1,104 +0,0 @@ -using Test -using StaticArrays -using LinearAlgebra -using TrajOptPlots -using MeshCat -using GeometryTypes -const TO = TrajectoryOptimization - -model_qv = Dynamics.FreeBody{UnitQuaternion{Float64,VectorPart},Float64}() -model_qe = Dynamics.FreeBody{UnitQuaternion{Float64,ExponentialMap},Float64}() -model_qp = Dynamics.FreeBody{UnitQuaternion{Float64,MRPMap},Float64}() -model_qg = Dynamics.FreeBody{UnitQuaternion{Float64,CayleyMap},Float64}() -model_p = Dynamics.FreeBody{MRP{Float64},Float64}() -model_e = Dynamics.FreeBody{RPY{Float64},Float64}() - -x,u = rand(model_p) - -model_p isa Dynamics.FreeBody{<:Rotation} -size(model_e) == (12,6) -size(model_qv) == (13,6) - - -# Set up Trajectory Optimization Problem -model = model_qv - -# Params -N = 101 -tf = 10. -x0 = zeros(model)[1] - -r0 = @SVector zeros(3) -q0 = zero(UnitQuaternion) -v0 = @SVector zeros(3) -ω0 = @SVector zeros(3) -x0 = Dynamics.build_state(model, r0, q0, v0, ω0) -@test x0[4] == 1 - -# Objective -Q = Diagonal(@SVector fill(0.1, 12)) -R = Diagonal(@SVector fill(0.05, 6)) -Qf = Diagonal(@SVector fill(10.0, 12)) - -rf = 0*@SVector [10,10,10] -qf = UnitQuaternion(RPY(pi/4, 0, 0)) -xf = Dynamics.build_state(model, rf, qf, v0, ω0) -Gf = Dynamics.state_diff_jacobian(model, xf) - -Q = Gf*Q*Gf' -Qf = Gf*Qf*Gf' -obj = LQRObjective(Q, R, Qf, xf, N, checks=false) - -# Build Problem -prob = Problem(model, obj, xf, tf, x0=x0) - -# Solve the problem -ilqr = iLQRSolver(prob) -ilqr.opts.verbose = true -U0 = [@SVector zeros(6) for k = 1:N-1] -initial_controls!(ilqr, U0) -solve!(ilqr) -q = states(ilqr)[N][4:7] -q'xf[4:7] - -vis = Visualizer(); open(vis); -dims = [1,1,3] -robot = HyperRectangle(Vec((-1 .* dims ./2)...), Vec(dims...)); setobject!(vis["robot"], robot); -visualize!(vis, model, get_trajectory(ilqr)) - - -Z = get_trajectory(ilqr) -states(ilqr)[end] -TO.stage_cost(obj[N], Z[end]) -controls(ilqr) -ilqr.∇F -ilqr.K - -# Step through the solve -solver = ilqr -initial_controls!(solver, U0) -rollout!(solver) -cost(solver) -Z = solver.Z -TO.state_diff_jacobian!(solver.G, solver.model, Z) - -solver.G -controls(ilqr) -states(ilqr)[end] - - -# Visualizer -using MeshCat, GeometryTypes, CoordinateTransformations -using TrajOptPlots -vis = Visualizer() -open(vis) -robot = HyperRectangle(Vec((-1 .* dims ./2)...), Vec(dims...)) -setobject!(vis["robot"], robot) - - -visualize!(vis, model, get_trajectory(ilqr)) -Z = get_trajectory(ilqr) -X = states(Z) -states(ilqr)[2] -q = orientation(model, X[end]) -settransform!(vis["robot"], LinearMap(Quat(q))) diff --git a/test/old/rotations_tests.jl b/test/old/rotations_tests.jl deleted file mode 100644 index 5287adc3..00000000 --- a/test/old/rotations_tests.jl +++ /dev/null @@ -1,257 +0,0 @@ -using BenchmarkTools -using ForwardDiff -using Test -using Random -using StaticArrays -using LinearAlgebra -import TrajectoryOptimization: kinematics, ∇rotate, ∇composition1, ∇composition2, vector -const TO = TrajectoryOptimization - -q1 = rand(UnitQuaternion) -q2 = rand(UnitQuaternion) -r = @SVector rand(3) -ω = @SVector rand(3) - - -ϕ = 0.1*@SVector [1,0,0] -@test logm(expm(ϕ)) ≈ ϕ -@test expm(logm(q1)) ≈ q1 -@test angle(expm(ϕ)) ≈ 0.1 -# @btime expm($ϕ) -# @btime logm($u1) -@test norm(q1 ⊕ ϕ) ≈ 1 -@test q1 ⊖ q2 isa SVector{3} -@test (q1 ⊕ ϕ) ⊖ q1 ≈ ϕ - -# Test inverses -q3 = q2*q1 -@test q2\q3 ≈ q1 -@test q3/q1 ≈ q2 -@test inv(q1)*r ≈ q1\r -@test r ≈ q3\(q2*q1*r) - - - -q = q1 -rhat = UnitQuaternion(r) -@test q*r ≈ Vmat()*Lmult(q)*Rmult(q)'*Vmat()'r -@test q*r ≈ Vmat()*Lmult(q)*Lmult(rhat)*Tmat()*SVector(q) -@test q*r ≈ Vmat()*Rmult(q)'*Rmult(rhat)*SVector(q) - -@test ForwardDiff.jacobian(q->UnitQuaternion{VectorPart}(q)*r,SVector(q)) ≈ ∇rotate(q,r) -# @btime ForwardDiff.jacobian(q->UnitQuaternion(q)*$r,SVector($q)) -# @btime ∇rotate($q,$r) - -@test ForwardDiff.jacobian(q->SVector(q2*UnitQuaternion{VectorPart}(q)),SVector(q1)) ≈ ∇composition1(q2,q1) -@test ForwardDiff.jacobian(q->SVector(UnitQuaternion{VectorPart}(q)*q1),SVector(q2)) ≈ ∇composition2(q2,q1) -# @btime ForwardDiff.jacobian(q->SVector($u2*UnitQuaternion(q)),SVector($u1)) -# @btime ∇composition1($u2,$u1) -# @btime ForwardDiff.jacobian(q->SVector(UnitQuaternion(q)*$u1),SVector($u2)) -# @btime ∇composition2($u2,$u1) - -@test Lmult(q) ≈ ∇composition1(q,q2) - -ϕ = @SVector zeros(3) -@test TO.∇differential(q) ≈ Lmult(q)*jacobian(VectorPart,ϕ) -@test TO.∇differential(q) ≈ Lmult(q)*jacobian(ExponentialMap,ϕ) -@test TO.∇differential(q) ≈ Lmult(q)*jacobian(CayleyMap,ϕ) -@test TO.∇differential(q) ≈ Lmult(q)*jacobian(MRPMap,ϕ) - - -############################################################################################ -# ROLL, PITCH, YAW EULER ANGLES -############################################################################################ - -e0 = @SVector [deg2rad(45), deg2rad(60), deg2rad(20)] -e1 = RPY(e0...) -# e2 = RotXYZ(e0...) - -# @test rotmat(e1) == RotX(e0[1])*RotY(e0[2])*RotZ(e0[3]) -# @test rotmat(e1) ≈ e2 - -# @test e1*r ≈ e2*r -# @btime $e2*$r -# @btime $e1*$r - -R = rotmat(e1) -e1_ = TO.from_rotmat(rotmat(e1)) -@test e1_.θ ≈ e1.θ -@test e1_.ψ ≈ e1.ψ -@test e1_.ϕ ≈ e1.ϕ - -# @test rotmat(e1*e1) ≈ RotXYZ(e2*e2) - -e1 = RPY(rand(3)...) -e2 = RPY(rand(3)...) -R = rotmat(e2*e1) - -# Test inverses -e1,e2 = rand(RPY), rand(RPY) -e3 = e2*e1 -@test e2\e3 ≈ e1 -@test e3/e1 ≈ e2 -@test r ≈ e3\(e2*e1*r) - -@test ∇rotate(e1,r) isa SMatrix{3,3} -@test ∇composition1(e2,e1) isa SMatrix{3,3} -@test ∇composition2(e2,e1) isa SMatrix{3,3} - -# sϕ1,cϕ1 = sincos(e1.ϕ) -# sθ1,cθ1 = sincos(e1.θ) -# sψ1,cψ1 = sincos(e1.ψ) -# -# sϕ2,cϕ2 = sincos(e2.ϕ) -# sθ2,cθ2 = sincos(e2.θ) -# sψ2,cψ2 = sincos(e2.ψ) -# -# R11 = cθ2*cψ2*(cθ1*cψ1) - cθ2*sψ2*(sϕ1*sθ1*cψ1+cϕ1*sψ1) + sθ2*(-cϕ1*sθ1*cψ1+sϕ1*sψ1) -# R11 ≈ R[1,1] -# R12 = cθ2*cψ2*(-cθ1*sψ1) - cθ2*sψ2*(-sϕ1*sθ1*sψ1+cϕ1*cψ1) + sθ2*(cϕ1*sθ1*sψ1+sϕ1*cψ1) -# R12 ≈ R[1,2] -# -# dxϕ1 = -cθ2*sψ2*(cϕ1*sθ1*cψ1-sϕ1*sψ1) + sθ2*( sϕ1*sθ1*cψ1+cϕ1*sψ1) -# dxθ1 = -cθ2*cψ2*(sθ1*cψ1) - cθ2*sψ2*(sϕ1*cθ1*cψ1) + sθ2*(-cϕ1*cθ1*cψ1) -# dxψ1 = -cθ2*cψ2*(cθ1*sψ1) - cθ2*sψ2*(-sϕ1*sθ1*sψ1+cϕ1*cψ1) + sθ2*( cϕ1*sθ1*sψ1+sϕ1*cψ1) -# -# dyϕ1 = -cθ2*sψ2*(-sϕ1*sθ1*sψ1+cϕ1*cψ1) + sθ2*(cϕ1*sθ1*sψ1+sϕ1*cψ1) -# -# dψdϕ1 = -R11/(R11^2 + R12^2)*dϕ1 + (R12)/(R11^2 + R12^2)*dϕ1 -# -# vals1 = @SVector [e1.ϕ, e1.θ, e1.ψ] -# R2 = rotmat(e2) -# f(e) = rotmat_to_rpy(R2*rotmat(e...)) -# f(vals1) -# ForwardDiff.jacobian(f, vals1) - -# @btime ∇rotation1($e2, $e1) -# @btime ∇rotation2($e2, $e1) - - -############################################################################################ -# MODIFIED RODRIGUES PARAMETERS -############################################################################################ -p1 = MRP(q1) -p2 = MRP(q2) -# @btime $p2*$p1 -# @btime $p2*$r - -# Test rotations and composition -@test p2*r ≈ q2*r -@test p2*p1*r ≈ q2*q1*r -@test rotmat(p2)*r ≈ p2*r -p3 = p2*p1 -@test p2\p3 ≈ p1 -@test p3/p1 ≈ p2 - -@test r ≈ p3\(p2*p1*r) - -r = @SVector rand(3) -R = rotmat(q1) -@test R*r ≈ q1*r - -p = MRP(p1) -R1 = rotmat(p) -@test R1*r ≈ R*r -@test p*r ≈ R*r - -@test UnitQuaternion(p) ≈ q1 - - -# Test composition jacobians -@test ForwardDiff.jacobian(x->SVector(p2*MRP(x)),SVector(p1)) ≈ ∇composition1(p2,p1) -@test ForwardDiff.jacobian(x->SVector(MRP(x)*p1),SVector(p2)) ≈ ∇composition2(p2,p1) -p0 = MRP(0,0,0) -@test TO.∇differential(p2) ≈ ∇composition1(p2,p0) - -############################################################################################ -# RODRIGUES PARAMETERS -############################################################################################ - -g1 = RodriguesParam(q1) -g2 = RodriguesParam(q2) -@test q2 ≈ -UnitQuaternion(g2) -@test g2 ≈ RodriguesParam(UnitQuaternion(g2)) - -# Test compostion and rotation -@test g1*r ≈ q1*r -@test g2*r ≈ q2*r -@test g2*g1*r ≈ q2*q1*r -@test rotmat(g2)*r ≈ g2*r - -g3 = g2*g1 -@test g2\g3 ≈ g1 -@test g3/g1 ≈ g2 -@test r ≈ g3\(g2*g1*r) -@test r ≈ (g3\g2*g1)*r - -# Test Jacobians -@test ForwardDiff.jacobian(g->RodriguesParam(g)*r, SVector(g1)) ≈ ∇rotate(g1, r) - -function compose(g2,g1) - N = (g2+g1 + g2 × g1) - D = 1/(1-g2'g1) - return D*N -end -@test ForwardDiff.jacobian(g->compose(SVector(g2),g), SVector(g1)) ≈ ∇composition1(g2,g1) -@test ForwardDiff.jacobian(g->compose(g,SVector(g1)), SVector(g2)) ≈ ∇composition2(g2,g1) - -g0 = RodriguesParam{Float64}(0,0,0) -@test ∇composition1(g2, g0) ≈ TO.∇differential(g2) - - -# Conversions -import TrajectoryOptimization: rotmat_to_quat -Random.seed!(1) # i = 3 -q = rand(UnitQuaternion) -@test rotmat_to_quat(rotmat(q)) ≈ q - -Random.seed!(2) # i = 4 -q = rand(UnitQuaternion) -@test rotmat_to_quat(rotmat(q)) ≈ q - -Random.seed!(3) # i = 2 -q = rand(UnitQuaternion) -@test rotmat_to_quat(rotmat(q)) ≈ q - -Random.seed!(5) # i = 1 -q = rand(UnitQuaternion) -@test rotmat_to_quat(rotmat(q)) ≈ q - -# -# # Quadrotors -# quad1 = Dynamics.Quadrotor() -# quad2 = Dynamics.Quadrotor2{Quat{VectorPart}}() -# x,u = rand(quad2) -# dynamics(quad1,x,u) ≈ dynamics(quad2,x,u) -# # @btime dynamics($quad1,$x,$u) -# # @btime dynamics($quad2,$x,$u) -# # -# # @btime state_diff($quad1,$x,$(1.1x)) -# # @btime state_diff($quad2,$x,$(1.1x)) -# # @btime state_diff_jacobian($quad1, $x) -# # @btime state_diff_jacobian($quad2, $x) -# -# a = @SVector rand(3) -# b = @SVector rand(3) -# @test skew(a)*b == cross(a,b) -# skew(b)*a == -skew(a)*b -# -# p = a -# r = b -# f(x) = MRP_to_DCM(x)*r -# @test ForwardDiff.jacobian(f,p) ≈ MRP_rotate_jacobian(p,r) -# -# # Test MRP compositio -# p1 = @SVector rand(3) -# p2 = @SVector rand(3) -# r = @SVector rand(3) -# R1 = MRP_to_DCM(p1) -# R2 = MRP_to_DCM(p2) -# R3 = R2*R1 -# p3 = MRP_composition(p2,p1) -# @test MRP_rotate_vec(p1,r) ≈ R1*r -# @test MRP_rotate_vec(p2,r) ≈ R2*r -# @test MRP_rotate_vec(p3,r) ≈ R3*r -# -# ForwardDiff.jacobian(x->MRP_composition(x,p1), p2) ≈ MRP_composition_jacobian_p2(p2,p1) -# ForwardDiff.jacobian(x->MRP_composition(p2,x), p1) ≈ MRP_composition_jacobian_p1(p2,p1) diff --git a/test/old/satellite_test.jl b/test/old/satellite_test.jl deleted file mode 100644 index d53a5eed..00000000 --- a/test/old/satellite_test.jl +++ /dev/null @@ -1,224 +0,0 @@ -using TrajectoryOptimization -import TrajectoryOptimization: stage_cost, cost_expansion -using TrajOptPlots -const TO = TrajectoryOptimization - -# using PlanningWithAttitude -# import PlanningWithAttitude: cost_gradient, stage_cost, cost_hessian -# include("visualization.jl") -using StaticArrays -using LinearAlgebra -using BenchmarkTools -using Plots -using ForwardDiff -using Random -using MeshCat - - - -# Dynamics -model = Dynamics.Satellite() -N = 101 -tf = 5.0 - -if !isdefined(Main,:vis) - vis = Visualizer(); open(vis); - set_mesh!(vis, model) -end - -# Objective -struct SatCost <: CostFunction - Q::Diagonal{Float64,SVector{3,Float64}} - R::Diagonal{Float64,SVector{3,Float64}} - q::SVector{3,Float64} - q0::SVector{4,Float64} - Qq::Float64 - c::Float64 -end - -function SatCost(Q::Diagonal,R::Diagonal,q0::SVector, Qq=0.0, ω0=@SVector zeros(3)) - q = -Q*ω0 - c = 0.5*ω0'Q*ω0 - SatCost(Q,R,q,q0,Qq,c) -end - -function stage_cost(cost::SatCost, x::SVector, u::SVector=(@SVector zeros(3))) - ω = @SVector [x[1], x[2], x[3]] - q = @SVector [x[4], x[5], x[6], x[7]] - J = 0.5*(ω'cost.Q*ω + u'cost.R*u) + cost.q'ω + cost.c - d = cost.q0'q - if d ≥ 0 - J += cost.Qq*(1-d) - else - J += cost.Qq*(1+d) - end - return J -end - - -function cost_expansion(cost::SatCost, model::Dynamics.Satellite, z::KnotPoint{T,N,M}, G) where {T,N,M} - Q = cost.Q - q0 = cost.q0 - u = control(z) - ω = @SVector [z.z[1], z.z[2], z.z[3]] - q = @SVector [z.z[4], z.z[5], z.z[6], z.z[7]] - d = cost.q0'q - Qω = Q*ω + cost.q - if d ≥ 0 - Qq = -cost.Qq*cost.q0'Lmult(q)*Vmat()' - else - Qq = cost.Qq*cost.q0'Lmult(q)*Vmat()' - end - Qx = [Qω; Qq'] - Qu = cost.R*u - - Qxx = Diagonal(@SVector [Q[1,1], Q[2,2], Q[3,3], 0,0,0]) - d = q0'q*cost.Qq - if d ≤ 0 - d *= -1 - end - Qqq = I(3)*d - Qxx = @SMatrix [ - Q[1,1] 0 0 0 0 0; - 0 Q[2,2] 0 0 0 0; - 0 0 Q[3,3] 0 0 0; - 0 0 0 Qqq[1] Qqq[4] Qqq[7]; - 0 0 0 Qqq[2] Qqq[5] Qqq[8]; - 0 0 0 Qqq[3] Qqq[6] Qqq[9]; - ] - Quu = cost.R - Qux = @SMatrix zeros(M,N-1) - return Qxx, Quu, Qux, Qx, Qu -end - -Q_diag = @SVector [1e-3,1e-3,1e-3, 1e-2,1e-2,1e-2,1e-2] -R_diag = @SVector fill(1e-4,3) -θs = range(0,2pi,length=N) -u = normalize([1,0,0]) -iω = @SVector [1,2,3] -iq = @SVector [4,5,6,7] - -Xref = map(1:N) do k - θ = θs[k] #+ deg2rad(185) - ωf = [2pi/5,0,0] - qf = @SVector [cos(θ/2), u[1]*sin(θ/2), u[2]*sin(θ/2), u[3]*sin(θ/2)] - xf = @SVector [ωf[1], ωf[2], ωf[3], qf[1], qf[2], qf[3], qf[4]] -end - -costs = map(1:N) do k - ωf = Xref[k][iω] - qf = Xref[k][iq] - k < N ? s = 1 : s = 1 - # LQRCost(Q_diag*s, R_diag, xf) - SatCost(Diagonal(Q_diag[iω]), Diagonal(R_diag), qf, 1.0, ωf) - # xf = Xref[k] - # QuatLQRCost(Diagonal(Q_diag), Diagonal(R_diag), xf) -end -obj = Objective(costs) - -# Initial Condition -x0 = @SVector [0,0,0, 1,0,0,0.] -u0 = @SVector [0.3, 0, 0] -Random.seed!(1) -U0 = [u0 + randn(3)*4e-1 for k = 1:N-1] - -# Final Condition -θ = θs[end] -xf = @SVector [0,0,0, cos(θ/2), u[1]*sin(θ/2), u[2]*sin(θ/2), u[3]*sin(θ/2)] - -# Solver -prob = Problem(model, obj, xf, tf, x0=x0) -solver = iLQRSolver(prob) -initial_controls!(solver, U0) -rollout!(solver) -visualize!(vis, model, solver.Z) -cost(obj, solver.Z) - -solver.opts.verbose = true -initial_controls!(solver, U0) - -cost(solver) -TO.state_diff_jacobian!(solver.G, solver.model, solver.Z) -TO.discrete_jacobian!(solver.∇F, solver.model, solver.Z) -TO.cost_expansion!(solver.Q, solver.G, solver.obj, solver.model, solver.Z) -ΔV = TO.backwardpass!(solver) -TO.forwardpass!(solver, ΔV, cost(solver)) -for k = 1:N - solver.Z[k].z = solver.Z̄[k].z -end - - -initial_controls!(solver, U0) -solve!(solver) -solver.stats.iterations -states(solver)[end][4:7]'xf[4:7] -visualize!(vis, model, solver.Z) -Z = solver.Z - -solver.opts.verbose = false -@btime begin - initial_controls!($solver, $U0) - solve!($solver) -end - - - -############################################################################################ -# Intermediate Keyframes -############################################################################################ - -Q_nom = @SVector [1e-3,1e-3,1e-3, 1e-4,1e-4,1e-4,1e-4] -R_nom = @SVector fill(1e-4,3) - -Q_key = @SVector [1e-3,1e-3,1e-3, 1e-2,1e-2,1e-2,1e-2] -R_key = @SVector fill(1e-4,3) - -keyframes = Int.(range(1,N, length=3)) -xf = @SVector [0,0,0, cos(θ/2), u[1]*sin(θ/2), u[2]*sin(θ/2), u[3]*sin(θ/2)] - -costs = map(1:N) do k - if k ∈ keyframes - ωf = Xref[k][iω] - qf = Xref[k][iq] - SatCost(Diagonal(Q_key[iω]), Diagonal(R_key), qf, 1.0, ωf) - else - ωf = xf[iω] - qf = xf[iq] - SatCost(Diagonal(Q_nom[iω]), Diagonal(R_nom), qf, 0.0, ωf) - end -end -obj = Objective(costs) - -# Initial Condition -x0 = @SVector [0,0,0, 1,0,0,0.] -u0 = @SVector [0.3, 0, 0] -Random.seed!(1) -U0 = [u0 + randn(3)*4e-1 for k = 1:N-1] - -# Solver -prob = Problem(model, obj, xf, tf, x0=x0) -solver = iLQRSolver(prob) -initial_controls!(solver, U0) -rollout!(solver) -visualize!(vis, model, solver.Z) -cost(obj, solver.Z) - -initial_controls!(solver, U0) -# initial_controls!(solver, controls(Z_sol)) -solver.opts.verbose = true -solve!(solver) -solver.stats.iterations -states(solver)[end][4:7]'xf[4:7] -visualize!(vis, model, solver.Z) -cost(solver) -Z = solver.Z - -Zsol = deepcopy(Z) - -iq = @SVector [4,5,6,7] -n = 13 -function to_diag(iq,::Val{N}) where N - D = @MVector zeros(N) -end -@btime to_diag($iq, Val($n)) -@btime @MVector zeros(3) diff --git a/test/old/simple_rotation_test.jl b/test/old/simple_rotation_test.jl deleted file mode 100644 index 6755bf35..00000000 --- a/test/old/simple_rotation_test.jl +++ /dev/null @@ -1,219 +0,0 @@ -using Parameters -using StaticArrays -using LinearAlgebra -using Rotations -using BenchmarkTools -using MeshCat -using CoordinateTransformations -using GeometryTypes -using TrajOptPlots -const TO = TrajectoryOptimization -import TrajectoryOptimization: Rotation - -# Set up visualizer -vis = Visualizer(); open(vis); -dims = [1,1,3]*0.5 -robot = HyperRectangle(Vec((-1 .* dims ./2)...), Vec(dims...)); -setobject!(vis["robot"], robot); - -# Define Spinner Model -@with_kw struct Spinner{R<:Rotation} <: AbstractModel - mass::Float64 = 1.0 - J::Diagonal{Float64, SVector{3, Float64}} = Diagonal(@SVector ones(3)) -end -Base.size(::Spinner{<:UnitQuaternion}) = (7,3) -Base.size(::Spinner) = (7,3) -Dynamics.orientation(::Spinner{UnitQuaternion{T,D}}, x::SVector{N,T2}) where {T,T2,N,D} = - UnitQuaternion{T2,D}(x[4], x[5], x[6], x[7]) -Dynamics.orientation(::Spinner, x::SVector) = R(x[4], x[5], x[6]) - -function TrajectoryOptimization.dynamics(model::Spinner, x::SVector, u::SVector) - J = model.J - ω = @SVector [x[1], x[2], x[3]] - q = Dynamics.orientation(model, x) - ωdot = J\(u - ω × (J*ω)) - qdot = kinematics(q,ω) - return [ωdot; qdot] -end - -function TrajectoryOptimization.state_diff_jacobian!(G, model::Spinner, Z::Traj) - for k in eachindex(Z) - G[k] = TrajectoryOptimization.state_diff_jacobian(model, state(Z[k])) - end -end - -function TrajectoryOptimization.state_diff_jacobian(::Spinner{<:UnitQuaternion}, x::SVector) - q = UnitQuaternion(x[4], x[5], x[6], x[7]) - G = TrajectoryOptimization.∇differential(q) - @SMatrix [1 0 0 0 0 0; - 0 1 0 0 0 0; - 0 0 1 0 0 0; - 0 0 0 G[1] G[5] G[ 9]; - 0 0 0 G[2] G[6] G[10]; - 0 0 0 G[3] G[7] G[11]; - 0 0 0 G[4] G[8] G[12]] -end - -function TrajectoryOptimization.state_diff_jacobian( - ::Spinner{UnitQuaternion{T,IdentityMap}}, x::SVector) where T - return I -end - -function TrajectoryOptimization.state_diff(model::Spinner, x, x0) - δω = @SVector [x[1]-x0[1], x[2]-x0[2], x[3]-x0[3]] - q = Dynamics.orientation(model, x) - q0 = Dynamics.orientation(model, x0) - inv(q0)*q - δq = VectorPart(inv(q0)*q) - return @SVector [δω[1], δω[2], δω[3], δq[1], δq[2], δq[3]] -end - -TrajectoryOptimization.state_diff(::Spinner{UnitQuaternion{T,IdentityMap}}, x, x0) where T = x - x0 - -TrajectoryOptimization.state_diff_size(model::Spinner) = 6 -TrajectoryOptimization.state_diff_size(model::Spinner{UnitQuaternion{T,IdentityMap}}) where T = 7 - -function TrajOptPlots.visualize!(vis, model::Spinner, Z::Traj) - X = states(Z) - fps = Int(round(length(Z)/Z[end].t)) - anim = MeshCat.Animation(fps) - for k in eachindex(Z) - atframe(anim, k) do - x = @SVector zeros(3) - q = UnitQuaternion(Dynamics.orientation(model, X[k])) - settransform!(vis["robot"], compose(Translation(x), LinearMap(Quat(q)))) - end - end - setanimation!(vis, anim) - return anim -end - -model = Spinner{UnitQuaternion{Float64,MRPMap}}() -Dynamics.rotation_type(::Spinner{R}) where R = R - -# Params -N = 101 -tf = 5. - -# Initial and Final conditions -x0 = @SVector [0,0,0, 1,0,0,0.] -xf = @SVector [0,0,0, 1,0,0,0.] - - -# Intermediate frames -qs = [UnitQuaternion(Quat(RotZ(ang))) for ang in range(0,2pi,length=N)] -X_guess = map(1:N) do k - om = @SVector [x0[1], x0[2], x0[3]] - if k > 76 - q = -qs[k] - else - q = qs[k] - end - [om; SVector(q)] -end - -# Build Objective -Q_diag = [(@SVector fill(1e-3,3)); (@SVector fill(1e-1,4))] -R_diag = @SVector fill(1e-4, 3) -no_quat = SVector{6}(deleteat!(collect(1:7), 4)) - -costs = map(1:N) do k - x = X_guess[k] - if Dynamics.rotation_type(model) <: UnitQuaternion{T,IdentityMap} where T - Q = Diagonal(Q_diag) - else - Q = Diagonal(Q_diag[no_quat]) - Gf = TO.state_diff_jacobian(model, x) - Q = Gf*Q*Gf' - # Q = Diagonal(Q_diag) - end - R = Diagonal(R_diag) - - if k == N - Q *= 100 - end - - LQRCost(Q,R,x, checks=false) - # QuadraticCost(Q,R, checks=false) - # Q = Diagonal(Q_diag[no_quat]) - # RBCost(model, Q, R, x) - # QuatLQRCost(Q,R,x) -end -obj = Objective(costs) -obj[51].Q - -# Problem -model = Spinner{UnitQuaternion{Float64,MRPMap}}() -model = Spinner{UnitQuaternion{Float64,IdentityMap}}() -prob = Problem(model, obj, xf, tf, x0=x0) - -# Solve -ilqr = iLQRSolver(prob) -ilqr.opts.verbose = true -U0 = [@SVector zeros(3) for k = 1:N-1] -initial_controls!(ilqr, U0) -solve!(ilqr) -cost(ilqr) -ilqr.K[1] - -q = states(ilqr)[N][4:7] -q'xf[4:7] -visualize!(vis, model, ilqr.Z) - -# With Quaternion -model_quat = Spinner{UnitQuaternion{VectorPart}}() -Gf = TO.state_diff_jacobian(model_quat, xf) -Q_quat = Gf*Diagonal(diag(Q)[no_quat])*Gf' -no_quat = @SVector [1,2,3,5,6,7] -Qf_quat = Gf*Diagonal(diag(Qf)[no_quat])*Gf' -costfun = QuadraticCost(Q_quat,R, checks=false) -costfun_term = QuadraticCost(Qf_quat, R*0, checks=false) - -obj_quat = Objective(costfun, costfun_term, N) - -prob_quat = Problem(model_quat, obj_quat, xf, tf, x0=x0) -# Solve -ilqr_quat = iLQRSolver(prob_quat) -ilqr_quat.opts.verbose = true -initial_controls!(ilqr_quat, U0) -solve!(ilqr_quat) -cost(ilqr_quat) -xf[4:7]'states(ilqr_quat)[end][4:7] -visualize!(vis, model, ilqr.Z) - - -ilqr_quat.opts.verbose = false -@btime begin - initial_controls!($ilqr_quat, $U0) - solve!($ilqr_quat) -end - -ilqr.opts.verbose = false -@btime begin - initial_controls!($ilqr, $U0) - solve!($ilqr) -end - - -# Step through the solve -solver = iLQRSolver(prob_quat) -initial_controls!(solver, U0) -rollout!(solver) -states(solver)[end] -cost(solver) - -Z = get_trajectory(solver) -TO.state_diff_jacobian!(solver.G, solver.model, Z) -TO.discrete_jacobian!(solver.∇F, solver.model, Z) -TO.cost_expansion!(solver.Q, solver.obj, solver.Z) -solver.∇F[50] -solver.Q.x[50] -solver.Q.u[50] -solver.G[50] -ΔV = backwardpass!(solver) -forwardpass!(solver, ΔV, cost(solver)) - -for k = 1:N - solver.Z[k].z = solver.Z̄[k].z -end -states(solver)[end] diff --git a/test/old/solver_comparision.jl b/test/old/solver_comparision.jl deleted file mode 100644 index 096a7fff..00000000 --- a/test/old/solver_comparision.jl +++ /dev/null @@ -1,152 +0,0 @@ -using TrajectoryOptimization -using StaticArrays -using LinearAlgebra -using Statistics -using BenchmarkTools -using ForwardDiff -using Test -using SparseArrays -const TO = TrajectoryOptimization - -function make_mutable(conSet::ConstraintSet) - n,m,P = size(conSet) - N = length(conSet.p) - conSet_ = ConstraintSet(n,m,N) - for con in conSet - add_constraint!(conSet_, con.con, con.inds, mutable=true) - end - return conSet_ -end -function make_mutable(prob::Problem) - return Problem(prob, constraints = make_mutable(prob.constraints)) -end - -# Test entire solve -prob,opts = Problems.DubinsCar(:escape) -solver0 = iLQRSolver(copy(prob), opts) -solver1 = iLQRSolver2(copy(make_mutable(prob)), opts) -solver0 = ALTROSolver(copy(prob), opts, infeasible=true, R_inf=0.1) -solver1 = ALTROSolver(copy(make_mutable(prob)), opts, solver_uncon=iLQRSolver2, infeasible=true, R_inf=0.1) -# solve!(solver0) -# solve!(solver1) -# solver0.opts.projected_newton = false -# solver1.opts.projected_newton = false -b1 = benchmark_solve!(solver0) -b2 = benchmark_solve!(solver1) -judge(median(b2),median(b1)) -@show median(b2).allocs -iterations(solver0) -iterations(solver1) -@show cost(solver0) ≈ cost(solver1) - -model = get_model(solver1) -n,m = size(model) -x,u = rand(model) -z = KnotPoint(x,u,0.1) -∇f = zeros(n,n+m+1) -@btime TO.discrete_jacobian!($RK3, $∇f, $model, $z) -∇f = SizedMatrix{n,n+m+1}(∇f) -@btime TO.discrete_jacobian!($RK3, $∇f, $model, $z) - -altro0 = ALTROSolver(copy(prob), opts)#, infeasible=true, R_inf=0.1) -altro1 = ALTROSolver(copy(make_mutable(prob)), opts, solver_uncon=iLQRSolver2)#, infeasible=true, R_inf=0.1) -al0 = altro0.solver_al -al1 = altro1.solver_al -initialize!(al0) -initialize!(al1) -solver0 = al0.solver_uncon -solver1 = al1.solver_uncon -solve!(solver0) -solve!(solver1) -iterations(solver0) -iterations(solver1) - -# Step through -altro0 = ALTROSolver(copy(prob), opts)#, infeasible=true, R_inf=0.1) -altro1 = ALTROSolver(copy(make_mutable(prob)), opts, solver_uncon=iLQRSolver2)#, infeasible=true, R_inf=0.1) -al0 = altro0.solver_al -al1 = altro1.solver_al -solver0 = al0.solver_uncon -solver1 = al1.solver_uncon -rollout!(solver0) -rollout!(solver1) -states(solver0) ≈ states(solver1) -cost(solver0) ≈ cost(solver1) -TO.state_diff_jacobian!(solver0.G, solver0.model, solver0.Z) -TO.state_diff_jacobian!(solver1.G, solver1.model, solver1.Z) -# solver0.G ≈ solver1.G -TO.cost_expansion!(solver0.Q, solver0.G, solver0.obj, solver0.model, solver0.Z) -TO.cost_expansion!(solver1.Q, solver1.obj, solver1.Z) -TO.error_expansion!(solver1.Q, solver1.model, solver1.Z, solver1.G) - -Q = [TO.error_expansion(Q, solver1.model) for Q in solver1.Q] -Qxx = [Q.xx for Q in Q] -Quu = [Q.uu for Q in Q] -Qux = [Q.ux for Q in Q] -Qx = [Q.x for Q in Q] -Qu = [Q.u for Q in Q] - -Qxx ≈ solver0.Q.xx -Quu ≈ solver0.Q.uu -Qux ≈ solver0.Q.ux -Qx ≈ solver0.Q.x -Qu ≈ solver0.Q.u -TO.discrete_jacobian!(solver0.∇F, solver0.model, solver0.Z) -TO.dynamics_expansion!(solver1.D, solver1.model, solver1.Z) -∇F = [D.∇f for D in solver1.D] -∇F ≈ solver0.∇F - -# step through backward pass -ΔV0 = backwardpass!(solver0) -ΔV1 = backwardpass!(solver1) -ΔV2 = TO.static_backwardpass!(solver1) -ΔV0 ≈ ΔV1 ≈ ΔV2 -solver0.K ≈ solver1.K -solver0.d ≈ solver1.d - -J0 = TO.forwardpass!(solver0, ΔV0, cost(solver0)) -J1 = forwardpass!(solver1, ΔV1, cost(solver1)) -J0 ≈ J1 -states(solver0.Z̄) ≈ states(solver1.Z̄) - -TO.copy_trajectories!(solver0) -TO.copy_trajectories!(solver1) - - -# Compare timing -al0 = AugmentedLagrangianSolver(copy(prob), opts) -al1 = AugmentedLagrangianSolver(copy(make_mutable(prob)), opts, solver_uncon=iLQRSolver2) -solver0 = al0.solver_uncon -solver1 = al1.solver_uncon -@btime rollout!($solver0) -@btime rollout!($solver1) -@btime TO.cost!($solver0.obj, $solver0.Z) -@btime TO.cost!($solver1.obj, $solver1.Z) -@btime TO.state_diff_jacobian!($solver0.G, $solver0.model, $solver0.Z) -@btime TO.state_diff_jacobian!($solver1.G, $solver1.model, $solver1.Z) -@btime TO.cost_expansion!($solver0.Q, $solver0.G, $solver0.obj, $solver0.model, $solver0.Z) -@btime TO.cost_expansion!($solver1.Q, $solver1.obj, $solver1.Z) -@btime TO.error_expansion!($solver1.Q, $solver1.model, $solver1.Z, $solver1.G) -@btime TO.discrete_jacobian!($solver0.∇F, $solver0.model, $solver0.Z) -@btime TO.dynamics_expansion!($solver1.D, $solver1.model, $solver1.Z) -@btime TO.error_expansion!($solver1.D, $solver1.model, $solver1.G) - -@btime ΔV0 = backwardpass!($solver0) -@btime ΔV1 = backwardpass!($solver1) -@btime ΔV1 = TO.static_backwardpass!($solver1) - -@btime TO.forwardpass!($solver0, $ΔV0, cost($solver0)) -@btime TO.forwardpass!($solver1, $ΔV1, cost($solver1)) -@btime rollout!($solver0, 1.0) -@btime rollout!($solver1, 1.0) - -E1 = TO.GeneralExpansion{Float64}(SizedArray, 4,4,1) -E2 = TO.GeneralExpansion{Float64}(SizedArray, 4,4,1) -E1 = TO.SizedExpansion{Float64}(4,4,1) -E2 = TO.SizedExpansion{Float64}(4,4,1) - -@btime TO._error_expansion!($E1,$E2,$G) -@btime TO.error_expansion!($solver.E, $solver.Q[1], $solver.model, $solver.Z[1], $(solver.G[1])) - - -struct MyType{T} diff --git a/test/old/solver_options.jl b/test/old/solver_options.jl deleted file mode 100644 index 94ead87d..00000000 --- a/test/old/solver_options.jl +++ /dev/null @@ -1,6 +0,0 @@ - -# Make sure all solver options can be created without specifying float type -@test_nowarn ilqr = iLQRSolverOptions() -@test_nowarn al = AugmentedLagrangianSolverOptions() -@test_nowarn altro = ALTROSolverOptions() -@test_nowarn pn = ProjectedNewtonSolverOptions() diff --git a/test/old/sqrt_bp_tests.jl b/test/old/sqrt_bp_tests.jl deleted file mode 100644 index 1e4d0bde..00000000 --- a/test/old/sqrt_bp_tests.jl +++ /dev/null @@ -1,85 +0,0 @@ -## Pendulum -T = Float64 - -# model -model = TrajectoryOptimization.Dynamics.car -n = model.n; m = model.m -model_d = TO.discretize_model(model,:rk4) - -# cost -Q = Array(1e-3*Diagonal(I,n)) -R = Array(1e-3*Diagonal(I,m)) -Qf = Array(Diagonal(I,n)*100.0) -x0 = zeros(n) -xf = [0.0;1.0;0.0] - -# problem -N = 31 -U = [ones(m) for k = 1:N-1] -obj = TrajectoryOptimization.LQRObjective(Q,R,Qf,xf,N) -dt = 0.15 -prob = Problem(model_d,obj,U,dt=dt,x0=x0) -rollout!(prob) - -## unconstrained -#bp -opts_ilqr = iLQRSolverOptions{T}() -ilqr_solver = iLQRSolver(prob,opts_ilqr) -jacobian!(prob,ilqr_solver) -TO.cost_expansion!(prob,ilqr_solver) -ΔV = TO._backwardpass!(prob,ilqr_solver) - -#sqrt bp -opts_ilqr_sqrt = iLQRSolverOptions{T}(square_root=true) -ilqr_solver_sqrt = iLQRSolver(prob,opts_ilqr_sqrt) -jacobian!(prob,ilqr_solver_sqrt) -TO.cost_expansion!(prob,ilqr_solver_sqrt) -ΔV_sqrt = TO._backwardpass_sqrt!(prob,ilqr_solver_sqrt) -@test isapprox(ΔV_sqrt,ΔV) -@test all(isapprox(ilqr_solver.K,ilqr_solver_sqrt.K)) -@test all(isapprox(ilqr_solver.d,ilqr_solver_sqrt.d)) -for k = 1:N - @test isapprox(ilqr_solver.S[k].xx,ilqr_solver_sqrt.S[k].xx'*ilqr_solver_sqrt.S[k].xx) - @test isapprox(ilqr_solver.S[k].x,ilqr_solver_sqrt.S[k].x) -end - -## constrained -opts_al = AugmentedLagrangianSolverOptions{T}(opts_uncon=opts_ilqr) -opts_al_sqrt = AugmentedLagrangianSolverOptions{T}(opts_uncon=opts_ilqr_sqrt) - -# constraints -u_bnd = 5. -bnd = BoundConstraint(n,m,u_min=-u_bnd,u_max=u_bnd,trim=true) -bnd -goal_con = goal_constraint(xf) -con = [bnd, goal_con] -prob = update_problem(prob,constraints=Constraints(con,N)) -rollout!(prob) - -#bp -solver_ilqr = iLQRSolver(prob,opts_ilqr) -solver_al = AugmentedLagrangianSolver(prob,opts_al) -prob_al = TO.AugmentedLagrangianProblem(prob,solver_al) -TO.update_constraints!(prob_al.obj.C,prob_al.obj.constraints, prob.X, prob.U) -TO.update_active_set!(prob_al.obj) -jacobian!(prob_al,solver_ilqr) -TO.cost_expansion!(prob_al,solver_ilqr) -ΔV = TO._backwardpass!(prob_al,solver_ilqr) - -#bp sqrt -solver_al_sqrt = AugmentedLagrangianSolver(prob,opts_al_sqrt) -solver_ilqr_sqrt = solver_al_sqrt.solver_uncon -prob_al_sqrt = TO.AugmentedLagrangianProblem(prob,solver_al_sqrt) -TO.update_constraints!(prob_al_sqrt.obj.C,prob_al_sqrt.obj.constraints, prob_al_sqrt.X, prob_al_sqrt.U) -TO.update_active_set!(prob_al_sqrt.obj) -jacobian!(prob_al_sqrt,solver_ilqr_sqrt) -TO.cost_expansion!(prob_al_sqrt,solver_ilqr_sqrt) -ΔV_sqrt = TO._backwardpass_sqrt!(prob_al_sqrt,solver_ilqr_sqrt) - -@test isapprox(ΔV_sqrt,ΔV) -@test all(isapprox(solver_ilqr.K,solver_ilqr_sqrt.K)) -@test all(isapprox(solver_ilqr.d,solver_ilqr_sqrt.d)) -for k = 1:N - @test isapprox(solver_ilqr.S[k].xx,solver_ilqr_sqrt.S[k].xx'*solver_ilqr_sqrt.S[k].xx) - @test isapprox(solver_ilqr.S[k].x,solver_ilqr_sqrt.S[k].x) -end diff --git a/test/old/state_diff_tests.jl b/test/old/state_diff_tests.jl deleted file mode 100644 index 0e96a138..00000000 --- a/test/old/state_diff_tests.jl +++ /dev/null @@ -1,22 +0,0 @@ -q = normalize(@SVector rand(4)) -q0 = normalize(@SVector rand(4)) -dq(q) = quat_diff(q,q0) -@test ForwardDiff.jacobian(dq,q) ≈ quat_diff_jacobian(q0) - - -quad = Dynamics.Quadrotor() -x0 = Problems.quadrotor_static.x0 -xf = Problems.quadrotor_static.xf -state_diff(quad, xf, x0) -@test let quad=quad, xf=xf, x0=x0 - allocs = (@allocated state_diff(quad, xf, x0)) - allocs += (@allocated state_diff_jacobian(quad, x0)) -end == 0 -state_diff_jacobian(quad, x0) -@btime state_diff($quad, $xf, $x0) -@btime state_diff_jacobian($quad, $x0) -sd(x) = let model=quad, x0=x0 - state_diff(model, x, x0) -end -state_diff(quad, xf, x0) == sd(xf) -ForwardDiff.jacobian(sd, xf) ≈ state_diff_jacobian(quad, x0) diff --git a/test/old/static_constraints.jl b/test/old/static_constraints.jl deleted file mode 100644 index 70900fc8..00000000 --- a/test/old/static_constraints.jl +++ /dev/null @@ -1,248 +0,0 @@ -using StaticArrays, ForwardDiff, BenchmarkTools, LinearAlgebra - -n,m,N = 3,2,11 -dt = 0.1 -x = @SVector [1.,2,3,] -u = @SVector [-1.,1] -z = [x;u] -Z = [KnotPoint(x,u,dt) for k = 1:N] - - -xc = @SVector [0,1,2,3.] -yc = @SVector [0,2,2,0.] -r = 0.5*@SVector ones(length(xc)) -vals = [@SVector zeros(length(xc)) for k = 1:N] -circlecon = CircleConstraint(n,m,xc,yc,r) -generate_jacobian(circlecon) - -vals2 = [@SVector zeros(1) for k = 1:N] -normcon = NormConstraint(n,m,1.0) -generate_jacobian(normcon) - -@btime evaluate($circlecon, $x, $u) -@btime evaluate($normcon, $x, $u) - -@btime jacobian($circlecon, $z) -@btime jacobian($normcon, $z) - -∇c = [@SMatrix zeros(length(circlecon), n+m) for k = 1:N] -@btime jacobian!($∇c, $circlecon, $Z) -∇c = [@SMatrix zeros(length(normcon), n+m) for k = 1:N] -@btime jacobian!($∇c, $normcon, $Z) - -con1 = ConstraintVals(circlecon, 1:N) -con2 = ConstraintVals(normcon, 1:4) - -@btime evaluate!($con1.vals, $circlecon, $Z) -@btime evaluate!($con1, $Z) -con2.∇c - -jacobian!(con2.∇c, normcon, Z) - -@btime evaluate!($con1, $Z) -@btime jacobian!($con1, $Z) -@btime jacobian!($con2, $Z) - -@btime update_active_set!($con1, $(Val(0.01))) -@btime duals($con1, 3) - -@btime max_violation!($con1) -@btime max_violation!($con2) - -constraints = [con1, con2] -conSet = ConstraintSet(constraints, N) -jacobian!(con1, Z) -jacobian!(con2, Z) -for con in conSet.constraints - jacobian!(con, Z) -end - - -@btime jacobian!($conSet, $Z) - -evaluate!(conSet, Z) -jacobian!(conSet, Z) - -max_violation!(conSet) -@btime max_violation!($conSet) - - - -# AL Objective -xf = @SVector zeros(n) -Q = Diagonal(@SVector ones(n)) -R = Diagonal(@SVector ones(m)) -Qf = Diagonal(@SVector ones(n)) -obj = LQRObjective(Q,R,Qf,xf,N) - -alobj = ALObjective3(obj, conSet) -E = CostExpansion(n,m,N) -@btime cost_expansion($E, $obj, $Z) -cost_expansion(E,alobj,Z) -@btime cost_expansion($Q,$alobj,$Z) - -cost_expansion(E, con1, Z) -@btime cost_expansion($E, $con1, $Z) -J = obj.J -@btime cost!($J, $con1, $Z) -@btime cost!($alobj, $Z) - - - - - - -# Custom constraints - -struct CustomConstraint{C} <: AbstractConstraint{C} - n::Int - m::Int - p::Int - c::Function - ∇c::Function -end - -function CustomConstraint{C}(n::Int, m::Int, p::Int, c::Function) where C - generate_jacobian(c, n, m) - ∇c = eval(Symbol("∇" * string(c))) - CustomConstraint{C}(n,m,p,c,∇c) -end - -evaluate(con::CustomConstraint, x, u) = con.c(x,u) -evaluate(con::CustomConstraint, z) = con.c(z) -jacobian(con::CustomConstraint, x, u) = con.∇c(x, u) -jacobian(con::CustomConstraint, z) = con.∇c(z) - - - -# Test constraints -n,m = 3,2 -p1 = 3 - -x = @SVector [1.,2,3] -u = @SVector [-5.,5] -z = [x;u] - -x_max = @SVector [3,2,Inf] -x_min = @SVector [-3,-2,-Inf] -u_max = @SVector [Inf, 0.5] -u_min = @SVector [0.0, -0.5] - -bnd = BoundConstraint(n,m, x_max=x_max, x_min=x_min, u_max=u_max, u_min=u_min) - -@btime evaluate($bnd, $xs, $us) -@btime evaluate($bnd, $xs) - -function mycon1(x, u) - @SVector [x[1]^2 + x[2]^2 - 5, u[1] - 1, u[2] - 1] -end -jacob_eq1(x,u) = [2x[1] 2x[2] 0 0 0; - 0 0 0 1 0; - 0 0 0 0 1]; -eq2 = CustomConstraint{Equality}(n,m,3,mycon1) - -evaluate(eq2, x, u) == [0,-6,4] - -@btime evaluate($eq2,$x,$u) -@btime jacobian($eq2, $z) - - -mycon2(x,u) = @SVector [sin(x[1]), sin(x[3])] -ineq2 = CustomConstraint{Inequality}(n,m,2,mycon2) -@btime evaluate($ineq2, $x, $u) -@btime jacobian($ineq2, $z) - - - -constraints = [bnd, eq2, ineq2] -constraints = [eq2, eq2] - -out = [@SVector zeros(3) for k = 1:2] -function eval_constraints!(out, con, x, u) - out[2] = evaluate(con, x, u) -end -constraints = (eq2, eq2) -@btime eval_constraints!($out, $eq2, $x, $u) -@btime evaluate($eq2, $x, $u) -@btime evaluate($constraints[2], $x, $u) - -constraints = @SVector [eq2, eq2] -@btime evaluate($bnd, $x, $u) - - - - -# Quadrotor constraint -n,m = 13,4 -dt = 0.1 -xs,us = (@SVector rand(n)), (@SVector rand(m)) -x,u = Array(xs), Array(us) -zs = [xs;us] -z = [x;u] -Z = KnotPoint(x,u,dt) - - -struct CircleConstraint{T} <: AbstractConstraint{Inequality} - n::Int - m::Int - p::Int - r0::SVector{3,T} - radius::T -end - -Base.size(con::CircleConstraint) = (con.n, con.m, con.p) - -function evaluate(con::CircleConstraint, x, u) - p = con.r0 - r = con.radius - return @SVector [-((x[1] - p[1])^2 + (x[2]-p[2])^2 - r^2)] -end - -function evaluate(con::CircleConstraint, z) - p = con.r0 - r = con.radius - return @SVector [-((z[1] - p[1])^2 + (z[2]-p[2])^2 - r^2)] -end - -r0 = @SVector [0.5, 0.5, 0] -radius = 0.1 -con = CircleConstraint(n, m, 1, r0, radius) -@btime evaluate($con, $xs, $us) - -generate_jacobian(con) -jacobian(con, zs) -jacobian(con, xs, us) -@btime jacobian($con, $zs) - - -x_max = @SVector ones(n) -x_min = -@SVector ones(n) -u_max = 2@SVector ones(m) -u_min = -2@SVector ones(m) - -bnd = BoundConstraint(n,m, x_max=x_max, x_min=x_min, u_max=u_max, u_min=u_min) - -constraints = (con, con, bnd) - -Z = [KnotPoint(x,u,dt) for k = 1:10] - -function eval_constraints!(cval, constraint, Z::Traj) - for k in eachindex(cval) - cval[k] = evaluate(constraint, state(Z[k]), control(Z[k])) - end -end - - -function eval_all_constraints!(cvals, constraints, Z::Traj) - eval_constraints!(cvals[1], constraints[1], Z) - # eval_constraints!(cvals[2], constraints[2], Z) - # eval_constraints!(cvals[3], constraints[3], Z) - # for i in eachindex(cvals) - # eval_constraints!(cvals[i], constraints[i], Z) - # end -end - -@btime eval_constraints!($(cvals[1]), $(constraints[1]), $Z) -constraints = (bnd, bnd, con) -cvals = [[@SVector zeros(size(con)[3]) for k = 1:10] for con in constraints] -@btime eval_all_constraints!($cvals, $constraints, $Z) diff --git a/test/old/test_noisyrb.jl b/test/old/test_noisyrb.jl deleted file mode 100644 index 442086dc..00000000 --- a/test/old/test_noisyrb.jl +++ /dev/null @@ -1,13 +0,0 @@ -using Distributions -using Test -using LinearAlgebra -model = Dynamics.Quadrotor2() -x,u = rand(model) - -w0 = MvNormal(Diagonal(ones(6))*0) -model0 = Dynamics.NoisyRB(model, w0) -@test dynamics(model, x, u) ≈ dynamics(model0,x,u) - -w1 = MvNormal(Diagonal(ones(6))*1) -model1 = Dynamics.NoisyRB(model, w1) -@test !(dynamics(model, x, u) ≈ dynamics(model1,x,u)) diff --git a/test/old/yak_tests.jl b/test/old/yak_tests.jl deleted file mode 100644 index 0953425d..00000000 --- a/test/old/yak_tests.jl +++ /dev/null @@ -1,43 +0,0 @@ -using TrajectoryOptimization -const TO = TrajectoryOptimization -using BenchmarkTools -using TrajOptPlots -using LinearAlgebra -using StaticArrays -using FileIO -using MeshCat -using GeometryTypes -using CoordinateTransformations - -# Start visualizer -Rot = MRP{Float64} -model = Dynamics.YakPlane(Rot) -if !isdefined(Main,:vis) - vis = Visualizer(); open(vis); - set_mesh!(vis, model) -end - -# Generate problem -prob = Problems.YakProblems(UnitQuaternion{Float64,IdentityMap}) -U0 = deepcopy(controls(prob)) - -# Solve -solver = iLQRSolver(prob) - -initial_controls!(solver, U0) -# benchmark_solve!(solver) -rollout!(solver) -TO.state_diff_size(solver.model) - -solver.opts.verbose = true -cost(solver) -solve!(solver) -iterations(solver) -cost(solver) -visualize!(vis, solver) -X = states(solver) -findfirst(isnan.(X)) -X[15] - - -solver.obj.J From 0336b7428e8b93c2d09b3b88eddfd5d25cb26163 Mon Sep 17 00:00:00 2001 From: Brian Jackson Date: Tue, 11 Aug 2020 15:29:03 -0400 Subject: [PATCH 14/16] Fix missing methods from RobotDynamics --- src/TrajectoryOptimization.jl | 3 ++- src/nlp.jl | 4 ++-- src/problem.jl | 2 ++ src/utils.jl | 2 +- test/nlp_tests.jl | 5 +---- test/runtests.jl | 3 ++- 6 files changed, 10 insertions(+), 9 deletions(-) diff --git a/src/TrajectoryOptimization.jl b/src/TrajectoryOptimization.jl index 7424e8fc..68795aee 100644 --- a/src/TrajectoryOptimization.jl +++ b/src/TrajectoryOptimization.jl @@ -20,7 +20,8 @@ using RobotDynamics: AbstractModel, LieGroupModel, KnotPoint, StaticKnotPoint, AbstractKnotPoint, QuadratureRule, Implicit, Explicit, DEFAULT_Q, HermiteSimpson, is_terminal, state_diff, state_diff_jacobian!, state_diff_jacobian, - state, control, states, controls, get_times, Traj, AbstractTrajectory + state, control, states, controls, get_times, Traj, AbstractTrajectory, + num_vars import RobotDynamics: jacobian!, state_dim, control_dim, states, controls, state_diff_jacobian!, rollout! diff --git a/src/nlp.jl b/src/nlp.jl index ba797d04..419e8bf8 100644 --- a/src/nlp.jl +++ b/src/nlp.jl @@ -371,7 +371,7 @@ end @inline Base.getindex(Z::NLPTraj, k::Int) = StaticKnotPoint(Z.Z, Z.Zdata, k) function Base.setindex!(Z::NLPTraj, z::AbstractKnotPoint, k::Int) Z.Z[Z.Zdata.xinds[k]] = state(z) - if k < length(Z) || terminal_control(Z) + if k < length(Z) || RobotDynamics.terminal_control(Z) Z.Z[Z.Zdata.uinds[k]] = control(z) end return z @@ -490,7 +490,7 @@ function TrajOptNLP(prob::Problem; remove_bounds::Bool=false, jac_type=:sparse) end @inline num_knotpoints(nlp::TrajOptNLP) = length(nlp.zinds) -@inline num_vars(nlp::TrajOptNLP) = length(nlp.data.g) +@inline RobotDynamics.num_vars(nlp::TrajOptNLP) = length(nlp.data.g) @inline num_constraints(nlp::TrajOptNLP) = length(nlp.data.d) @inline get_primals(nlp::TrajOptNLP) = nlp.Z.Z diff --git a/src/problem.jl b/src/problem.jl index 6915024f..724fdd86 100644 --- a/src/problem.jl +++ b/src/problem.jl @@ -220,6 +220,8 @@ num_constraints(prob::Problem) = get_constraints(prob).p @inline is_constrained(prob) = isempty(get_constraints(prob)) @inline get_initial_state(prob::Problem) = prob.x0 +states(x) = states(get_trajectory(x)) +controls(x) = controls(get_trajectory(x)) "```julia change_integration(prob::Problem, Q<:QuadratureRule) diff --git a/src/utils.jl b/src/utils.jl index 2e200ae8..551f3da0 100644 --- a/src/utils.jl +++ b/src/utils.jl @@ -19,7 +19,7 @@ end Base.showerror(io::IO, ex::NotImplemented) = print(io, "Not Implemented Error: ", ex.fun, " not implemented for type ", ex.type) -function num_vars(n::Int, m::Int, N::Int, isequal::Bool=false) +function RobotDynamics.num_vars(n::Int, m::Int, N::Int, isequal::Bool=false) n*N + (N-1)*m + isequal*m end diff --git a/test/nlp_tests.jl b/test/nlp_tests.jl index 4a78bfec..a1933662 100644 --- a/test/nlp_tests.jl +++ b/test/nlp_tests.jl @@ -7,13 +7,10 @@ using SparseArrays using LinearAlgebra const TO = TrajectoryOptimization -# using TrajectoryOptimization: StaticKnotPoint -# using TrajectoryOptimization: num_vars, TrajData, NLPTraj, TrajOptNLP, NLPConstraintSet, -# JacobianStructure, NLPData # Test NLPTraj iteration n,m,N = 3,2,101 -NN = TO.num_vars(n,m,N) +NN = RobotDynamics.num_vars(n,m,N) @test NN == N*n + (N-1)*m Z0 = Traj(n,m,0.1,N) Zdata = TO.TrajData(Z0) diff --git a/test/runtests.jl b/test/runtests.jl index cd071e19..5692062b 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -7,7 +7,6 @@ using SparseArrays using ForwardDiff using RobotDynamics using BenchmarkTools -using NBInclude const TO = TrajectoryOptimization include("test_models.jl") @@ -36,6 +35,8 @@ end include("moi_test.jl") end +using NBInclude +include(joinpath(@__DIR__, "..", "examples", "quickstart.jl")) @testset "Examples" begin @test_nowarn include(joinpath(@__DIR__, "..", "examples", "quickstart.jl")) # @nbinclude(joinpath(@__DIR__, "..", "examples", "Cartpole.ipynb"); softscope=true) From 919fc24510e375359630a36589c946ccffd4a8f3 Mon Sep 17 00:00:00 2001 From: Brian Jackson Date: Tue, 11 Aug 2020 15:41:33 -0400 Subject: [PATCH 15/16] Don't test example, for now --- test/Project.toml | 1 - test/runtests.jl | 3 +-- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/test/Project.toml b/test/Project.toml index da23a1d2..8c8e36e6 100644 --- a/test/Project.toml +++ b/test/Project.toml @@ -1,5 +1,4 @@ [deps] -Altro = "5dcf52e5-e2fb-48e0-b826-96f46d2e3e73" BenchmarkTools = "6e4b80f9-dd63-53aa-95a3-0cdb28fa8baf" ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210" Ipopt = "b6b21f68-93f8-5de0-b562-5493be1d77c9" diff --git a/test/runtests.jl b/test/runtests.jl index 5692062b..26a0caa1 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -36,9 +36,8 @@ end end using NBInclude -include(joinpath(@__DIR__, "..", "examples", "quickstart.jl")) @testset "Examples" begin - @test_nowarn include(joinpath(@__DIR__, "..", "examples", "quickstart.jl")) + # @test_nowarn include(joinpath(@__DIR__, "..", "examples", "quickstart.jl")) # @nbinclude(joinpath(@__DIR__, "..", "examples", "Cartpole.ipynb"); softscope=true) # @nbinclude(joinpath(@__DIR__, "..", "examples", "Quadrotor.ipynb"); softscope=true) end \ No newline at end of file From 340a552c49b739c02a480b817aac8fa62c067d6d Mon Sep 17 00:00:00 2001 From: Brian Jackson Date: Tue, 11 Aug 2020 15:42:05 -0400 Subject: [PATCH 16/16] Update to 0.3.1 --- Project.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Project.toml b/Project.toml index 3e402a2f..760d2781 100644 --- a/Project.toml +++ b/Project.toml @@ -1,6 +1,6 @@ name = "TrajectoryOptimization" uuid = "c79d492b-0548-5874-b488-5a62c1d9d0ca" -version = "0.3.0" +version = "0.3.1" [deps] DocStringExtensions = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae" pFad - Phonifier reborn

Pfad - The Proxy pFad of © 2024 Garber Painting. All rights reserved.

Note: This service is not intended for secure transactions such as banking, social media, email, or purchasing. Use at your own risk. We assume no liability whatsoever for broken pages.


Alternative Proxies:

Alternative Proxy

pFad Proxy

pFad v3 Proxy

pFad v4 Proxy