change objective and constraint interface

Project.toml

@@ -1,7 +1,7 @@
 name = "DirectTrajectoryOptimization"
 uuid = "a9f42406-efe7-414c-8b71-df971cc98041"
 authors = ["thowell <[email protected]>"]
-version = "0.1.1"
+version = "0.1.2"
 
 [deps]
 IfElse = "615f187c-cbe4-4ef1-ba3b-2fcf58d6d173"
@@ -19,5 +19,5 @@ Ipopt = "0.8"
 MathOptInterface = "0.10"
 Parameters = "0.12"
 Scratch = "1.0"
-Symbolics = "0.1.29 - 0.1.29"
+Symbolics = "0.1.29 - 0.1.32"
 julia = "1.6"

examples/acrobot/acrobot.jl

@@ -82,7 +82,6 @@ end
 # ## model
 dt = Dynamics(midpoint_implicit, nx, nx, nu, nw=nw)
 dyn = [dt for t = 1:T-1] 
-model = DynamicsModel(dyn)
 
 # ## initialization
 x1 = [0.0; 0.0; 0.0; 0.0] 
@@ -91,40 +90,39 @@ xT = [0.0; π; 0.0; 0.0]
 # ## objective 
 ot = (x, u, w) -> 0.1 * dot(x[3:4], x[3:4]) + 0.1 * dot(u, u)
 oT = (x, u, w) -> 0.1 * dot(x[3:4], x[3:4])
-ct = Cost(ot, nx, nu, nw, [t for t = 1:T-1])
-cT = Cost(oT, nx, 0, nw, [T])
-obj = [ct, cT]
+ct = Cost(ot, nx, nu, nw)
+cT = Cost(oT, nx, 0, nw)
+obj = [[ct for t = 1:T-1]..., cT]
 
 # ## constraints
-x_init = Bound(nx, nu, [1], xl=x1, xu=x1)
-x_goal = Bound(nx, 0, [T], xl=xT, xu=xT)
-cons = ConstraintSet([x_init, x_goal], [StageConstraint()])
+bnd1 = Bound(nx, nu, xl=x1, xu=x1)
+bndt = Bound(nx, nu)
+bndT = Bound(nx, 0, xl=xT, xu=xT)
+bnds = [bnd1, [bndt for t = 2:T-1]..., bndT]
+
+cons = [Constraint() for t = 1:T]
 
 # ## problem 
-trajopt = TrajectoryOptimizationProblem(obj, model, cons)
-s = Solver(trajopt, options=Options())
+p = ProblemData(obj, dyn, cons, bnds, options=Options())
 
 # ## initialize
 x_interpolation = linear_interpolation(x1, xT, T)
 u_guess = [1.0 * randn(nu) for t = 1:T-1]
-z0 = zeros(s.p.num_var)
-for (t, idx) in enumerate(s.p.trajopt.model.idx.x)
-    z0[idx] = x_interpolation[t]
-end
-for (t, idx) in enumerate(s.p.trajopt.model.idx.u)
-    z0[idx] = u_guess[t]
-end
-initialize!(s, z0)
+
+initialize_states!(p, x_interpolation)
+initialize_controls!(p, u_guess)
 
 # ## solve
-@time solve!(s)
+@time solve!(p)
 
 # ## solution
-@show trajopt.x[1]
-@show trajopt.x[T]
+x_sol, u_sol = get_trajectory(p)
+
+@show x_sol[1]
+@show x_sol[T]
 
 # ## state
-plot(hcat(trajopt.x...)')
+plot(hcat(x_sol...)')
 
 # ## control
-plot(hcat(trajopt.u[1:end-1]..., trajopt.u[end-1])', linetype = :steppost)
+plot(hcat(u_sol[1:end-1]..., u_sol[end-1])', linetype = :steppost)

examples/car/car.jl

@@ -28,7 +28,6 @@ end
 # ## model
 dt = Dynamics(midpoint_implicit, nx, nx, nu, nw=nw)
 dyn = [dt for t = 1:T-1] 
-model = DynamicsModel(dyn)
 
 # ## initialization
 x1 = [0.0; 0.0; 0.0] 
@@ -37,55 +36,53 @@ xT = [1.0; 1.0; 0.0]
 # ## objective 
 ot = (x, u, w) -> 0.0 * dot(x - xT, x - xT) + 1.0 * dot(u, u)
 oT = (x, u, w) -> 0.0 * dot(x - xT, x - xT)
-ct = Cost(ot, nx, nu, nw, [t for t = 1:T-1])
-cT = Cost(oT, nx, 0, nw, [T])
-obj = [ct, cT]
+ct = Cost(ot, nx, nu, nw)
+cT = Cost(oT, nx, 0, nw)
+obj = [[ct for t = 1:T-1]..., cT]
 
 # ## constraints
 ul = -0.5 * ones(nu) 
 uu = 0.5 * ones(nu)
-bnd1 = Bound(nx, nu, [1], xl=x1, xu=x1, ul=ul, uu=uu)
-bndt = Bound(nx, nu, [t for t = 2:T-1], ul=ul, uu=uu)
-bndT = Bound(nx, 0, [T], xl=xT, xu=xT)
+bnd1 = Bound(nx, nu, xl=x1, xu=x1, ul=ul, uu=uu)
+bndt = Bound(nx, nu, ul=ul, uu=uu)
+bndT = Bound(nx, 0, xl=xT, xu=xT)
+bnds = [bnd1, [bndt for t = 2:T-1]..., bndT]
 
 p_obs = [0.5; 0.5] 
 r_obs = 0.1
 function obs(x, u, w) 
     e = x[1:2] - p_obs
     return [r_obs^2.0 - dot(e, e)]
 end
-cont = StageConstraint(obs, nx, nu, nw, [t for t = 1:T-1], :inequality)
-conT = StageConstraint(obs, nx, 0, nw, [T], :inequality)
-cons = ConstraintSet([bnd1, bndt, bndT], [cont, conT])
+
+cont = Constraint(obs, nx, nu, nw, idx_ineq=collect(1:1))
+conT = Constraint(obs, nx, 0, nw, idx_ineq=collect(1:1))
+cons = [[cont for t = 1:T-1]..., conT]
 
 # ## problem 
-trajopt = TrajectoryOptimizationProblem(obj, model, cons)
-s = Solver(trajopt, options=Options())
+p = ProblemData(obj, dyn, cons, bnds, options=Options())
 
 # ## initialize
 x_interpolation = linear_interpolation(x1, xT, T)
 u_guess = [0.001 * randn(nu) for t = 1:T-1]
-z0 = zeros(s.p.num_var)
-for (t, idx) in enumerate(s.p.trajopt.model.idx.x)
-    z0[idx] = x_interpolation[t]
-end
-for (t, idx) in enumerate(s.p.trajopt.model.idx.u)
-    z0[idx] = u_guess[t]
-end
-initialize!(s, z0)
+
+initialize_states!(p, x_interpolation)
+initialize_controls!(p, u_guess)
 
 # ## solve
-@time solve!(s)
+solve!(p)
 
 # ## solution
-@show trajopt.x[1]
-@show trajopt.x[T]
+x_sol, u_sol = get_trajectory(p)
+
+@show x_sol[1]
+@show x_sol[T]
 
 # ## state
-plot(hcat(trajopt.x...)[1, :], hcat(trajopt.x...)[2, :], label = "", color = :orange, width=2.0)
+plot(hcat(x_sol...)[1, :], hcat(x_sol...)[2, :], label = "", color = :orange, width=2.0)
 pts = Plots.partialcircle(0.0, 2.0 * π, 100, r_obs)
 cx, cy = Plots.unzip(pts)
 plot!(Shape(cx .+ p_obs[1], cy .+ p_obs[2]), color = :black, label = "", linecolor = :black)
 
 # ## control
-plot(hcat(trajopt.u[1:end-1]..., trajopt.u[end-1])', linetype = :steppost)
+plot(hcat(u_sol[1:end-1]..., u_sol[end-1])', linetype = :steppost)

src/DirectTrajectoryOptimization.jl

@@ -10,26 +10,27 @@ const MOI = MathOptInterface
 
 include("objective.jl")
 include("constraints.jl")
+include("bounds.jl")
 include("dynamics.jl")
-include("problem.jl")
-include("moi.jl")
 include("solver.jl")
+include("data.jl")
+include("moi.jl")
 include("utils.jl")
 
 # objective 
 export Cost
 
 # constraints 
-export Bound, StageConstraint, ConstraintSet
+export Bound, Bounds, Constraint, Constraints
 
 # dynamics 
-export Dynamics, DynamicsModel
+export Dynamics
 
 # problem 
-export TrajectoryOptimizationProblem
+export ProblemData
 
 # solver 
-export Solver, Options, initialize!, solve!
+export Solver, Options, initialize_states!, initialize_controls!, solve!, get_trajectory
 
 # utils 
 export linear_interpolation

src/bounds.jl

@@ -0,0 +1,13 @@
+struct Bound{T}
+    xl::Vector{T} 
+    xu::Vector{T}
+    ul::Vector{T}
+    uu::Vector{T}
+end
+
+function Bound(nx::Int=0, nu::Int=0; 
+    xl=-Inf * ones(nx), xu=Inf * ones(nx), ul=-Inf * ones(nu), uu=Inf * ones(nu)) 
+    return Bound(xl, xu, ul, uu)
+end
+
+const Bounds{T} = Vector{Bound{T}}