added obsacles, things dont always work, time to do some more tuning …

…with last commit

Project.toml

@@ -4,6 +4,7 @@ authors = ["Brian Jackson <[email protected]>"]
 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"

examples/quickstart.jl → 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]

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]

src/Altro.jl

@@ -18,6 +18,7 @@ using TimerOutputs
 using ForwardDiff
 using FiniteDiff
 import Octavian
+using Accessors
 
 const TO = TrajectoryOptimization
 const RD = RobotDynamics

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)

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,14 +48,15 @@ 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
         # waypoints
         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)