diff --git a/Project.toml b/Project.toml index 7ef4af9..ef31f8f 100644 --- a/Project.toml +++ b/Project.toml @@ -4,6 +4,7 @@ authors = ["Brian Jackson "] version = "0.5.0" [deps] +Accessors = "7d9f7c33-5ae7-4f3b-8dc6-eff91059b697" BenchmarkTools = "6e4b80f9-dd63-53aa-95a3-0cdb28fa8baf" Colors = "5ae59095-9a9b-59fe-a467-6f913c188581" ControlSystems = "a6e380b2-a6ca-5380-bf3e-84a91bcd477e" diff --git a/control_inputs.png b/control_inputs.png index 6484faa..981e0d5 100644 Binary files a/control_inputs.png and b/control_inputs.png differ diff --git a/examples/quickstart.jl b/quickstart.jl similarity index 89% rename from examples/quickstart.jl rename to quickstart.jl index 1bc09c7..74c3345 100644 --- a/examples/quickstart.jl +++ b/quickstart.jl @@ -1,4 +1,5 @@ # Set up problem using TrajectoryOptimization.jl and RobotZoo.jl +import Pkg; Pkg.activate(@__DIR__); Pkg.instantiate() using TrajectoryOptimization using Altro import RobotZoo.Cartpole @@ -33,6 +34,16 @@ goal = GoalConstraint(xf) add_constraint!(conSet, bnd, 1:N-1) add_constraint!(conSet, goal, N) + +# Linear Constraint +p = 5 +A = @SMatrix rand(p,n+m) +b = @SVector rand(p) +lin = LinearConstraint(n,m,A,b, Inequality()) + +add_constraint!(conSet, lin, 3) +add_constraint!(conSet, lin, 20) + # Initialization u0 = @SVector fill(0.01,m) U0 = [u0 for k = 1:N-1] diff --git a/refined_path.png b/refined_path.png index 83a6c1b..e137d1d 100644 Binary files a/refined_path.png and b/refined_path.png differ diff --git a/refined_vel.png b/refined_vel.png index 9b6ffb3..23c2f5b 100644 Binary files a/refined_vel.png and b/refined_vel.png differ diff --git a/ros_test.jl b/ros_test.jl index f362c39..f359bc9 100644 --- a/ros_test.jl +++ b/ros_test.jl @@ -38,6 +38,8 @@ using Altro: ALTROSolver using Rotations using Plots + + include("./yifei/quadrotor_kr.jl") obstacles = []# Ref{PolyhedronArray}() # Store it into a `Ref` which it's mutable but has a constant type @@ -98,7 +100,7 @@ function path_callback(msg::TrajectoryDiscretized, pub_obj::Publisher{Marker}) U_hover = [MM * v for v in FM] # end of debug - solver = ALTROSolver(Quadrotor_kr(traj_ref = wpts, vel_ref = vel, FM_ref = FM, obstacles = obstacles, t_vec = msg.t)..., verbose=0) + solver = ALTROSolver(Quadrotor_kr(traj_ref = wpts, vel_ref = vel, FM_ref = FM, obstacles = [], t_vec = msg.t)..., verbose=0) Z0 = deepcopy(get_trajectory(solver)) TO.initial_trajectory!(solver,Z0) solve!(solver) @@ -109,7 +111,7 @@ function path_callback(msg::TrajectoryDiscretized, pub_obj::Publisher{Marker}) K = ilqr.K # feedback gain matrices d = ilqr.d # feedforward gains. Should be small. - plotting_bool = false + plotting_bool = true if plotting_bool x_plot = [v[1] for v in X] y_plot = [v[2] for v in X] diff --git a/src/Altro.jl b/src/Altro.jl index d59d6e0..f0d8ab2 100644 --- a/src/Altro.jl +++ b/src/Altro.jl @@ -18,6 +18,7 @@ using TimerOutputs using ForwardDiff using FiniteDiff import Octavian +using Accessors const TO = TrajectoryOptimization const RD = RobotDynamics diff --git a/src/augmented_lagrangian/al_solve.jl b/src/augmented_lagrangian/al_solve.jl index d64fbe4..2c6d8ff 100644 --- a/src/augmented_lagrangian/al_solve.jl +++ b/src/augmented_lagrangian/al_solve.jl @@ -56,6 +56,7 @@ function solve!(solver::ALSolver) # Outer loop updates dualupdate!(conset) + # lin_cons_update!(conset,Z̄) penaltyupdate!(conset) # Reset iLQR solver @@ -69,6 +70,108 @@ function solve!(solver::ALSolver) return solver end +function lin_cons_update!(conset::ALConstraintSet, Z̄::SampledTrajectory) + #need to get all polytopes as A,b matrices + println("going through cons") + #save the linear constraints into another array + lincons_A = [] + lincons_b = [] + large_λ_arr = Vector{Bool}() + idx_needs_change = Vector{Int}() + + for alcon in conset.constraints + if isa(alcon.con, TO.LinearConstraint{}) + # println("linear constraint") + # check if constraint unique, if yes, add to lincons + if !any([A == alcon.con.A for A in lincons_A]) + # display(alcon.con.A) + push!(lincons_A, alcon.con.A) + push!(lincons_b, alcon.con.b) + end + end + end + # splice!(conset.constraints, 2) + for alcon_idx in 1:length(conset.constraints) + alcon = conset.constraints[alcon_idx] + if isa(alcon.con, TO.LinearConstraint{}) + max_λ = maximum(alcon.λ[1]) + println("Max lamda = ",max_λ) + if max_λ > 1e2 + for i in 1:length(lincons_A) + println(i) + cur_state = state(Z̄[alcon.inds[1]]) + if minimum(lincons_A[i]*cur_state[1:3] .- lincons_b[i]) > 0.0 + @set alcon.con = TO.LinearConstraint(13,4,lincons_A[i], lincons_b[i],Inequality(),1:3) + # @set alcon.con.b = lincons_b[i] + # replace_con_ineq = + # replace_con = ALConstraint{Float64}(Z̄, replace_con_ineq, alcon.inds) + # println("switching constraint") + # # simply remove this constraint and add a new one in the same indexed position + # #!!!![FATAL] [1687809539.259629]: Error: setfield! immutable struct of type ALConstraint cannot be changed + + # # alcon.con.A = lincons_A[i] + # # alcon.con.b = lincons_b[i] + # splice!(conset.constraints, alcon_idx, replace_con) + break + end + end + end + end + end + + + + # max_λ = maximum(alcon.λ[1]) + # idx_needs_change = push!(idx_needs_change, alcon.inds[1]) + # if max_λ > 1e5 + # push!(large_λ_arr, true) + # else + # push!(large_λ_arr, false) + # end + # end + # push!(lincons, alcon.con) + # max_λ = maximum(alcon.λ[1]) + # idx_needs_change = push!(idx_needs_change, alcon.inds[1]) + # if max_λ > 1e5 + # push!(large_λ_arr, true) + # else + # push!(large_λ_arr, false) + # end + # end + # end + # for + # RD.evaluate(con::LinearConstraint, z::AbstractKnotPoint) = con.A*RD.getdata(z)[con.inds] .- con.b + # check if trajectory current position is in any other polytope + + #if yes, then switch the constraint to that polytope + # println(typeof(alcon)) + # println(alcon.inds) + # display(alcon.λ) + + + + + # for i in eachindex(alcon.inds) + # λ, μ, c = alcon.λ[i], alcon.μ[i], alcon.vals[i] + # conval = alcon.con[i] + # println(typeof(conval)) + # if isa(conval, LinearConstraint{}) + # println("Linear constraint max value is: ", maximum(conval.λ)) + # println(typeof(conval)) + # end + # end + # c = conval.vals + # λ = conval.λ + # μ = conval.μ + # λ_max = conval.params.λ_max + # cone = TO.sense(conval.con) + # λ_min = TO.sense(conval.con) == Equality() ? -λ_max : zero(λ_max) + # for i in eachindex(conval.inds) + # λ[i] .= dual_update(cone, SVector(λ[i]), SVector(c[i]), SVector(μ[i]), λ_max) + # end + +end + function record_iteration!(solver::ALSolver, J, c_max, μ_max) stats = solver.stats record_iteration!(stats, c_max=c_max, penalty_max=μ_max, is_outer=true) diff --git a/yifei/quadrotor_kr.jl b/yifei/quadrotor_kr.jl index 98f97e7..f09d12a 100644 --- a/yifei/quadrotor_kr.jl +++ b/yifei/quadrotor_kr.jl @@ -19,7 +19,7 @@ function Quadrotor_kr(Rot=UnitQuaternion{Float64}; traj_ref, vel_ref, FM_ref, ob # discretization N = length(traj_ref) # number of knot points - u0 = @SVector fill(0.5*9.81/4, m) #TODO: Change to actual vehicle mass + # u0 = @SVector fill(0.5*9.81/4, m) #TODO: Change to actual vehicle mass tf = convert(Float64, t_vec[end]) # Assigned from SplineTrajectory segment 0 total_time # what is a reasonable longest final time!? @@ -30,12 +30,12 @@ function Quadrotor_kr(Rot=UnitQuaternion{Float64}; traj_ref, vel_ref, FM_ref, ob x0 = RobotDynamics.build_state(model, x0_pos, UnitQuaternion(I), vel_ref[1], zeros(3)) # cost - costfun == :QuatLQR ? sq = 0 : sq = 1 - rm_quat = @SVector [1,2,3,4,5,6,8,9,10,11,12,13] + # costfun == :QuatLQR ? sq = 0 : sq = 1 + # rm_quat = @SVector [1,2,3,4,5,6,8,9,10,11,12,13] Q_diag = Dynamics.fill_state(model, 1e-5, 0.0, 0.0, 0.0) # x q:1e-5*sq v 1e-3 w - Q = Diagonal(Q_diag) - R = Diagonal(@SVector fill(1e-2,m)) + # Q = Diagonal(Q_diag) + R = Diagonal(@SVector fill(1e-2,m)) q_nom = UnitQuaternion(I) v_nom, ω_nom = zeros(3), zeros(3) # x_nom = Dynamics.build_state(model, zeros(3), q_nom, v_nom, ω_nom) @@ -48,6 +48,7 @@ function Quadrotor_kr(Rot=UnitQuaternion{Float64}; traj_ref, vel_ref, FM_ref, ob # else # cost_nom = LQRCost(Q*dt, R*dt, x_nom, u0) # end + U_hover = [MM * v for v in FM_ref] # inital reference input control traj = traj_ref #about 50/7 = 7 points # how many waypoints do you leave behind @@ -55,7 +56,7 @@ function Quadrotor_kr(Rot=UnitQuaternion{Float64}; traj_ref, vel_ref, FM_ref, ob times = 1:N #round.(Int, range(1, stop=101, length=length(traj))) println("length of traj is $(length(traj))") # times = [33, 66, 101] - Qw_diag = Dynamics.fill_state(model, 10, 0.0,0.0,0.0) #no waypoint cost since only the final point matters + Qw_diag = Dynamics.fill_state(model, 0, 0.0,0,0.0) #no waypoint cost since only the final point matters # x q:1*sq v:1 w:1 Qf_diag = Dynamics.fill_state(model, 10., 0.0, 10, 0.0) xf = Dynamics.build_state(model, traj[end], UnitQuaternion(I), vel_ref[end], zeros(3)) @@ -64,19 +65,19 @@ function Quadrotor_kr(Rot=UnitQuaternion{Float64}; traj_ref, vel_ref, FM_ref, ob xg = Dynamics.build_state(model, traj[i], q_nom, vel_ref[i], ω_nom) if times[i] == N Q = Diagonal(Qf_diag) - w = 4.0 - else - Q = Diagonal(1e-3*Qw_diag) * dt - w = 0.1 - end - if costfun == :QuatLQR - QuatLQRCost(Q, R*dt, xg, w=w) - elseif costfun == :ErrorQuad - Qd = diag(Q) - ErrorQuadratic(model, Diagonal(Qd[rm_quat]), R, xg) + # w = 4.0 else - LQRCost(Q, R, xg, u0) + Q = Diagonal(Qw_diag) * dt + # w = 0.1 end + # if costfun == :QuatLQR + # QuatLQRCost(Q, R*dt, xg, w=w) + # elseif costfun == :ErrorQuad + # Qd = diag(Q) + # ErrorQuadratic(model, Diagonal(Qd[rm_quat]), R, xg) + # else + LQRCost(Q, R, xg, U_hover[i]) + # end end # println(times) costs_all = map(1:N) do k @@ -96,10 +97,10 @@ function Quadrotor_kr(Rot=UnitQuaternion{Float64}; traj_ref, vel_ref, FM_ref, ob # println(typeof(costs[1])) # println(typeof(cost_nom)) - obj = Objective(costs_all) + obj = Objective(costs) # Initialization - U_hover = [MM * v for v in FM_ref] # inital reference input control + # U_hover = [copy(u0) for k = 1:N-1] # initial hovering control trajectory @@ -123,10 +124,11 @@ function Quadrotor_kr(Rot=UnitQuaternion{Float64}; traj_ref, vel_ref, FM_ref, ob #if 1 in poly, 2 in poly, 3 not in poly, then assign poly constraint to 1 and 2 polytope_counter = 1 traj_counter = 1 - result_mat = zeros(Int8, length(traj), length(obstacles)) + #make a double for loop to check if each point is in each polytope debug_flag = false if debug_flag + result_mat = zeros(Int8, length(traj), length(obstacles)) for i = 1:length(traj) for j = 1:length(obstacles) result_mat[i,j] = all(obstacles[j][1]*traj[i]-obstacles[j][2] .<= 0.0) @@ -135,19 +137,33 @@ function Quadrotor_kr(Rot=UnitQuaternion{Float64}; traj_ref, vel_ref, FM_ref, ob display(result_mat) exit() end - while traj_counter <= length(traj) && polytope_counter <= length(obstacles) + # while traj_counter <= length(traj) && polytope_counter <= length(obstacles) + # poly = obstacles[polytope_counter] + # if all(poly[1]*traj[traj_counter]-poly[2] .<= 0.0) + # println("""Adding constraint, point $traj_counter is in polytope $polytope_counter""") + # add_constraint!(conSet, LinearConstraint(n,m,-poly[1], -poly[2],Inequality(),1:3), traj_counter) + # traj_counter += 1 + # else + # polytope_counter += 1 + # end + # end + # if traj_counter < length(traj) + # println("Warning: not all traj initialized points are in polytopes") + # end + + polytope_counter = length(obstacles) + traj_counter = length(traj) + while traj_counter >= 1 && polytope_counter >= 1 poly = obstacles[polytope_counter] if all(poly[1]*traj[traj_counter]-poly[2] .<= 0.0) println("""Adding constraint, point $traj_counter is in polytope $polytope_counter""") - add_constraint!(conSet, LinearConstraint(n,m,poly[1], poly[2],Inequality(),1:3), traj_counter) - traj_counter += 1 + add_constraint!(conSet, LinearConstraint(n,m,-poly[1], -poly[2],Inequality(),1:3), traj_counter) + traj_counter -= 1 else - polytope_counter += 1 + polytope_counter -= 1 end end - if traj_counter < length(traj) - println("Warning: not all traj initialized points are in polytopes") - end + # Problem prob = Problem(model, obj, x0, tf, xf=xf, constraints=conSet) diff --git a/yifei/quadrotor_kr_min_time.jl b/yifei/quadrotor_kr_min_time.jl new file mode 100644 index 0000000..f2bfe9b --- /dev/null +++ b/yifei/quadrotor_kr_min_time.jl @@ -0,0 +1,117 @@ +function Quadrotor_kr_min_time(Rot=UnitQuaternion{Float64}; traj_ref, obstacles, time_total, + costfun=:Quadratic, normcon=false) + model = RobotZoo.Quadrotor{Rot}() + n,m = RD.dims(model) + + opts = SolverOptions( + penalty_scaling=100., + penalty_initial=1.0, + projected_newton=false, + ) + + # discretization + N = 101 # number of knot points + u0 = @SVector fill(0.5*9.81/4, m) #TODO: Change to actual vehicle mass + + tf = convert(Float64, time_total) # Assigned from SplineTrajectory segment 0 total_time + # what is a reasonable longest final time!? + dt = tf/(N-1) # total time + + # Initial condition + x0_pos = traj_ref[1] #this is planning in local coordinates + x0 = RobotDynamics.build_state(model, x0_pos, UnitQuaternion(I), zeros(3), zeros(3)) + + # cost + costfun == :QuatLQR ? sq = 0 : sq = 1 + rm_quat = @SVector [1,2,3,4,5,6,8,9,10,11,12,13] + Q_diag = Dynamics.fill_state(model, 1e-5, 0.0, 0.0, 0.0) + # x q:1e-5*sq v 1e-3 w + Q = Diagonal(Q_diag) + R = Diagonal(@SVector fill(1e-2,m)) + q_nom = UnitQuaternion(I) + v_nom, ω_nom = zeros(3), zeros(3) + x_nom = Dynamics.build_state(model, zeros(3), q_nom, v_nom, ω_nom) + #why is the state zero? should it follow referecne trajectory? + + if costfun == :QuatLQR + cost_nom = QuatLQRCost(Q*dt, R*dt, x_nom, w=0.0) + elseif costfun == :ErrorQuad + cost_nom = ErrorQuadratic(model, Diagonal(Q_diag[rm_quat])*dt, R*dt, x_nom) + else + cost_nom = LQRCost(Q*dt, R*dt, x_nom, u0) + end + + traj = traj_ref[1:7:end] #about 50/7 = 7 points + # how many waypoints do you leave behind + # waypoints + times = round.(Int, range(1, stop=101, length=length(traj))) + println("length of traj is $(length(traj))") + # times = [33, 66, 101] + Qw_diag = Dynamics.fill_state(model, 1e3, 0.0,0.0,0.0) + # x q:1*sq v:1 w:1 + Qf_diag = Dynamics.fill_state(model, 10., 100*sq, 10, 10) + xf = Dynamics.build_state(model, traj[end], UnitQuaternion(I), zeros(3), zeros(3)) + + costs = map(1:length(traj)) do i + r = traj[i] + xg = Dynamics.build_state(model, r, q_nom, v_nom, ω_nom) + if times[i] == N + Q = Diagonal(Qf_diag) + w = 40.0 + else + Q = Diagonal(1e-3*Qw_diag) * dt + w = 0.1 + end + if costfun == :QuatLQR + QuatLQRCost(Q, R*dt, xg, w=w) + elseif costfun == :ErrorQuad + Qd = diag(Q) + ErrorQuadratic(model, Diagonal(Qd[rm_quat]), R, xg) + else + LQRCost(Q, R, xg, u0) + end + end + # println(times) + costs_all = map(1:N) do k + # println("k is $k") + i = findfirst(x->(x ≥ k), times) + # println(i) + if k ∈ times + costs[i] + # println("Using goodcost") + else + cost_nom + + end + end + + # println(typeof(costs_all)) + # println(typeof(costs_all[1])) + # println(typeof(costs[1])) + # println(typeof(cost_nom)) + + obj = Objective(costs_all) + + # Initialization + U_hover = [copy(u0) for k = 1:N-1] # initial hovering control trajectory + + # Constraints + conSet = ConstraintList(n,m,N) + if normcon + if use_rot == :slack + add_constraint!(conSet, QuatSlackConstraint(), 1:N-1) + else + add_constraint!(conSet, QuatNormConstraint(), 1:N-1) + u0 = [u0; (@SVector [1.])] + end + end + bnd = BoundConstraint(n,m, u_min=0.0, u_max=12.0) + add_constraint!(conSet, bnd, 1:N-1) + + # Problem + prob = Problem(model, obj, x0, tf, xf=xf, constraints=conSet) + initial_controls!(prob, U_hover) + rollout!(prob) + + return prob, opts +end diff --git a/yifei/quadrotor_with_time_state.jl b/yifei/quadrotor_with_time_state.jl new file mode 100644 index 0000000..f1103af --- /dev/null +++ b/yifei/quadrotor_with_time_state.jl @@ -0,0 +1,137 @@ + +""" + Quadrotor{R} + +A standard quadrotor model, with simple aerodynamic forces. The orientation is represent by +a general rotation `R`. The body z-axis point is vertical, so positive controls cause acceleration +in the positive z direction. + +# Constructor + Quadrotor(; kwargs...) + Quadrotor{R}(; kwargs...) + +where `R <: Rotation{3}` and defaults to `UnitQuaternion{Float64}` if omitted. The keyword arguments are +* `mass` - mass of the quadrotor, in kg (default = 0.5) +* `J` - inertia of the quadrotor, in kg⋅m² (default = `Diagonal([0.0023, 0.0023, 0.004])`) +* `gravity` - gravity vector, in kg/m² (default = [0,0,-9.81]) +* `motor_dist` - distane between the motors, in m (default = 0.1750) +* `km` - motor torque constant (default = 0.0245) +* `kf` - motor force constant (default = 1.0) +""" +struct Quadrotor_time <: LieGroupModel + n::Int + m::Int + mass::Float64 + J::Diagonal{Float64,SVector{3,Float64}} + Jinv::Diagonal{Float64,SVector{3,Float64}} + gravity::SVector{3,Float64} + motor_dist::Float64 + kf::Float64 + km::Float64 + bodyframe::Bool # velocity in body frame? + info::Dict{Symbol,Any} +end +RobotDynamics.control_dim(::Quadrotor) = 4 + +function Quadrotor(; + mass=0.5, + J=Diagonal(@SVector [0.0023, 0.0023, 0.004]), + gravity=SVector(0,0,-9.81), + motor_dist=0.1750, + kf=1.0, + km=0.0245, + bodyframe=false, + info=Dict{Symbol,Any}()) where R + Quadrotor(13,4,mass,J,inv(J),gravity,motor_dist,kf,km,bodyframe,info) +end + +(::Type{Quadrotor})(;kwargs...) = Quadrotor{UnitQuaternion{Float64}}(;kwargs...) + +@inline RobotDynamics.velocity_frame(model::Quadrotor) = model.bodyframe ? :body : :world + +function trim_controls(model::Quadrotor) + @SVector fill(-model.gravity[3]*model.mass/4.0, size(model)[2]) +end + +function RobotDynamics.forces(model::Quadrotor, x, u) + q = orientation(model, x) + kf = model.kf + g = model.gravity + m = model.mass + + w1 = u[1] + w2 = u[2] + w3 = u[3] + w4 = u[4] + + F1 = max(0,kf*w1); + F2 = max(0,kf*w2); + F3 = max(0,kf*w3); + F4 = max(0,kf*w4); + F = @SVector [0., 0., F1+F2+F3+F4] #total rotor force in body frame + + m*g + q*F # forces in world frame +end + +function RobotDynamics.moments(model::Quadrotor, x, u) + + kf, km = model.kf, model.km + L = model.motor_dist + + w1 = u[1] + w2 = u[2] + w3 = u[3] + w4 = u[4] + + F1 = max(0,kf*w1); + F2 = max(0,kf*w2); + F3 = max(0,kf*w3); + F4 = max(0,kf*w4); + + M1 = km*w1; + M2 = km*w2; + M3 = km*w3; + M4 = km*w4; + tau = @SVector [L*(F2-F4), L*(F3-F1), (M1-M2+M3-M4)] #total rotor torque in body frame +end + +function RobotDynamics.wrenches(model::Quadrotor, x::SVector, u::SVector) + F = RobotDynamics.forces(model, x, u) + M = RobotDynamics.moments(model, x, u) + return [F; M] + + q = orientation(model, x) + C = forceMatrix(model) + mass, g = model.mass, model.gravity + + # Calculate force and moments + w = max.(u, 0) # keep forces positive + fM = forceMatrix(model)*w + f = fM[1] + M = @SVector [fM[2], fM[3], fM[4]] + e3 = @SVector [0,0,1] + F = mass*g - q*(f*e3) + return F,M +end + +function forceMatrix(model::Quadrotor) + kf, km = model.kf, model.km + L = model.motor_dist + @SMatrix [ + kf kf kf kf; + 0 L*kf 0 -L*kf; + -L*kf 0 L*kf 0; + km -km km -km; + ] +end + + +RobotDynamics.inertia(model::Quadrotor) = model.J +RobotDynamics.inertia_inv(model::Quadrotor) = model.Jinv +RobotDynamics.mass(model::Quadrotor) = model.mass + +function Base.zeros(model::Quadrotor{R}) where R + x = RobotDynamics.build_state(model, zero(RBState)) + u = @SVector fill(-model.mass*model.gravity[end]/4, 4) + return x,u +end