Skip to content

Commit

Permalink
Clean out src
Browse files Browse the repository at this point in the history
  • Loading branch information
@bjack205
bjack205 committed Apr 9, 2022
1 parent 6319c4a commit 6cbc3c9
Show file tree
Hide file tree
Showing 15 changed files with 67 additions and 2,408 deletions.
6 changes: 0 additions & 6 deletions src/TrajectoryOptimization.jl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -70,7 +70,6 @@ export
NormConstraint,
add_constraint!

# include("expansions.jl")
include("costfunctions.jl")
include("quadratic_costs.jl")
include("lie_costs.jl")
Expand All @@ -79,19 +78,14 @@ include("objective.jl")
include("cones.jl")
include("abstract_constraint.jl")
include("constraints.jl")
# include("dynamics_constraints.jl")
include("constraint_list.jl")
# include("integration.jl")

include("cost.jl")
# include("convals.jl")

include("problem.jl")
# include("conset.jl")


include("utils.jl")
# include("deprecated.jl")

import Base.length
@deprecate length(con::AbstractConstraint) RobotDynamics.output_dim(con)
Expand Down
308 changes: 30 additions & 278 deletions src/abstract_constraint.jl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,42 +50,23 @@ abstract type StageConstraint <: AbstractConstraint end
abstract type StateConstraint <: StageConstraint end
"Only a function of controls at a single knotpoint"
abstract type ControlConstraint <: StageConstraint end
# "Only a function of states and controls at two adjacent knotpoints"
# abstract type CoupledConstraint <: AbstractConstraint end
# "Only a function of states at adjacent knotpoints"
# abstract type CoupledStateConstraint <: CoupledConstraint end
# "Only a function of controls at adjacent knotpoints"
# abstract type CoupledControlConstraint <: CoupledConstraint end

# const StateConstraints =
# Union{StageConstraint,StateConstraint,CoupledConstraint,CoupledStateConstraint}
# const ControlConstraints =
# Union{StageConstraint,ControlConstraint,CoupledConstraint,CoupledControlConstraint}

"Get constraint sense (Inequality vs Equality)"
sense(::C) where {C<:AbstractConstraint} = throw(NotImplemented(:sense, Symbol(C)))

"Dimension of the state vector"
RobotDynamics.state_dim(::C) where {C<:StateConstraint} =
throw(NotImplemented(:state_dim, Symbol(C)))
# "Dimension of the state vector"
# RobotDynamics.state_dim(::C) where {C<:StateConstraint} =
# throw(NotImplemented(:state_dim, Symbol(C)))

"Dimension of the control vector"
RobotDynamics.control_dim(::C) where {C<:ControlConstraint} =
throw(NotImplemented(:control_dim, Symbol(C)))
# "Dimension of the control vector"
# RobotDynamics.control_dim(::C) where {C<:ControlConstraint} =
# throw(NotImplemented(:control_dim, Symbol(C)))

"Return the constraint value"
evaluate(::C) where {C<:AbstractConstraint} = throw(NotImplemented(:evaluate, Symbol(C)))
# "Return the constraint value"
# evaluate(::C) where {C<:AbstractConstraint} = throw(NotImplemented(:evaluate, Symbol(C)))

"Length of constraint vector (deprecated for RD.output_dim)"
# Base.length(con::C) where {C<:AbstractConstraint} = RD.output_dim(con)
RD.output_dim(::C) where {C<:AbstractConstraint} = throw(NotImplemented(:output_dim, Symbol(C)))

# widths(con::StageConstraint, n=state_dim(con), m=control_dim(con)) = (n+m,)
# widths(con::StateConstraint, n=state_dim(con), m=0) = (n,)
# widths(con::ControlConstraint, n=0, m=control_dim(con)) = (m,)
# widths(con::CoupledConstraint, n=state_dim(con), m=control_dim(con)) = (n+m, n+m)
# widths(con::CoupledStateConstraint, n=state_dim(con), m=0) = (n,n)
# widths(con::CoupledControlConstraint, n=0, m=control_dim(con)) = (m,m)
# "Length of constraint vector (deprecated for RD.output_dim)"
# RD.output_dim(::C) where {C<:AbstractConstraint} = throw(NotImplemented(:output_dim, Symbol(C)))

"Upper bound of the constraint, as a vector, which is 0 for all constraints
(except bound constraints)"
Expand All @@ -101,15 +82,6 @@ RD.output_dim(::C) where {C<:AbstractConstraint} = throw(NotImplemented(:output_
@inline lower_bound(::Inequality) = -Inf
@inline lower_bound(::Equality) = 0.0

# """
# primal_bounds!(zL, zU, con::AbstractConstraint)

# Set the lower `zL` and upper `zU` bounds on the primal variables imposed by the constraint
# `con`. Return whether or not the vectors `zL` or `zU` could be modified by `con`
# (i.e. if the constraint `con` is a bound constraint).
# """
# primal_bounds!(zL, zU, con::AbstractConstraint) = false

"Is the constraint a bound constraint or not"
@inline is_bound(con::AbstractConstraint) = false

Expand All @@ -119,41 +91,28 @@ RD.output_dim(::C) where {C<:AbstractConstraint} = throw(NotImplemented(:output_
@inline check_dims(con::AbstractConstraint, n, m) =
state_dim(con) == n && control_dim(con) == m

# get_dims(con::Union{StateConstraint,CoupledStateConstraint}, nm::Int) =
# state_dim(con), nm - state_dim(con)
# get_dims(con::Union{ControlConstraint,CoupledControlConstraint}, nm::Int) =
# nm - control_dim(con), control_dim(con)
# get_dims(con::AbstractConstraint, nm::Int) = state_dim(con), control_dim(con)

con_label(::AbstractConstraint, i::Int) = "index $i"

# """
# get_inds(con::AbstractConstraint)

# Get the indices of the joint state-control vector that are used to calculate the constraint.
# If the constraint depends on more than one time step, the indices start from the beginning
# of the first one.
# """
# get_inds(con::StateConstraint, n, m) = (1:n,)
# get_inds(con::ControlConstraint, n, m) = (n .+ (1:m),)
# get_inds(con::StageConstraint, n, m) = (1:n+m,)
# get_inds(con::CoupledConstraint, n, m) = (1:n+m, n+m+1:2n+2m)

# """
# widths(::AbstractConstraint)
# widths(::AbstractConstraint, n, m)

# Return a tuple of the widths of the Jacobians for a constraint. If `n` and `m` are not passed
# in, they are assumed to be consistent with those returned by `state_dim` and `control_dim`.
# """
# @inline widths(con::AbstractConstraint, n, m) = length.(get_inds(con, n, m))
# @inline widths(con::StageConstraint) = (state_dim(con) + control_dim(con),)
# @inline widths(con::StateConstraint) = (state_dim(con),)
# @inline widths(con::ControlConstraint) = (control_dim(con),)
# @inline widths(con::CoupledConstraint) =
# (state_dim(con) + control_dim(con), state_dim(con) + control_dim(con))
# @inline widths(con::CoupledStateConstraint) = (state_dim(con), state_dim(con))
# @inline widths(con::CoupledControlConstraint) = (control_dim(con), control_dim(con))
"""
constraintlabel(con, i)
Return a string describing the `i`th index of `con`. Default is simply `"index \$i"`.
"""
constraintlabel(::AbstractConstraint, i::Integer) = "index $i"


"""
gen_jacobian(con)
Generate a Jacobian of the correct size for constraint `con`.
"""
gen_jacobian(con::AbstractConstraint) = gen_jacobian(Float64, con)
function gen_jacobian(::Type{T}, con::AbstractConstraint) where T
nm = RD.input_dim(con)
p = RD.output_dim(con)
zeros(T, p, nm)
end


############################################################################################
# EVALUATION METHODS #
Expand Down Expand Up @@ -209,28 +168,6 @@ function evaluate_constraints!(
end
end

# function evaluate!(
# vals::Vector{<:AbstractVector},
# con::StageConstraint,
# Z::SampledTrajectory,
# inds = 1:length(Z),
# )
# for (i, k) in enumerate(inds)
# vals[i] .= evaluate(con, Z[k])
# end
# end

# function evaluate!(
# vals::Vector{<:AbstractVector},
# con::CoupledConstraint,
# Z::SampledTrajectory,
# inds = 1:length(Z)-1,
# )
# for (i, k) in enumerate(inds)
# vals[i] .= evaluate(con, Z[k], Z[k+1])
# end
# end

"""
constraint_jacobians!(∇c, con::AbstractConstraint, Z, [inds, is_const])
Expand Down Expand Up @@ -261,30 +198,6 @@ function constraint_jacobians!(
end
end

# function jacobian!(
# ∇c::VecOrMat{<:AbstractMatrix},
# con::StageConstraint,
# Z::SampledTrajectory,
# inds = 1:length(Z),
# is_const = BitArray(undef, size(∇c))
# )
# for (i, k) in enumerate(inds)
# is_const[i] = jacobian!(∇c[i], con, Z[k])
# end
# end

# function jacobian!(
# ∇c::VecOrMat{<:AbstractMatrix},
# con::CoupledConstraint,
# Z::SampledTrajectory,
# inds = 1:size(∇c, 1),
# is_const = BitArray(undef, size(∇c))
# )
# for (i, k) in enumerate(inds)
# is_const[i,1] = jacobian!(∇c[i, 1], con, Z[k], Z[k+1], 1)
# is_const[i,2] = jacobian!(∇c[i, 2], con, Z[k], Z[k+1], 2)
# end
# end

"""
∇constraint_jacobians!(G, con::AbstractConstraint, Z, λ, inds, is_const, init)
Expand Down Expand Up @@ -339,164 +252,3 @@ function error_expansion!(jac, jac0, con::StageConstraint, model::DiscreteDynami
end
end
end

# function ∇jacobian!(
# G::VecOrMat{<:AbstractMatrix},
# con::StageConstraint,
# Z::SampledTrajectory,
# λ::Vector{<:AbstractVector},
# inds = 1:length(Z),
# is_const = ones(Bool, length(inds)),
# init::Bool = false,
# )
# for (i, k) in enumerate(inds)
# if init || !is_const[i]
# is_const[i] = ∇jacobian!(G[i], con, Z[k], λ[i])
# end
# end
# end

# function ∇jacobian!(
# G::VecOrMat{<:AbstractMatrix},
# con::CoupledConstraint,
# Z::SampledTrajectory,
# λ::Vector{<:AbstractVector},
# inds = 1:length(Z),
# is_const = ones(Bool, length(inds)),
# init::Bool = false,
# )
# for (i, k) in enumerate(inds)
# if init || !is_const[i]
# is_const[i] = ∇jacobian!(G[i, 1], con, Z[k], Z[k+1], λ[i], 1)
# is_const[i] = ∇jacobian!(G[i, 2], con, Z[k], Z[k+1], λ[i], 2)
# end
# end
# end

# # Default methods for converting KnotPoints to states and controls for StageConstraints
# """
# evaluate(con::AbstractConstraint, z) # stage constraint
# evaluate(con::AbstractConstraint, z1, z2) # coupled constraint

# Evaluate the constraint `con` at knot point `z`. By default, this method will attempt to call

# evaluate(con, x)

# if `con` is a `StateConstraint`,

# evaluate(con, u)

# if `con` is a `ControlConstraint`, or

# evaluate(con, x, u)

# if `con` is a `StageConstraint`. If `con` is a `CoupledConstraint` the constraint should
# define

# evaluate(con, z1, z2)

# """
# @inline evaluate(con::StateConstraint, z::AbstractKnotPoint) = evaluate(con, state(z))
# @inline evaluate(con::ControlConstraint, z::AbstractKnotPoint) = evaluate(con, control(z))
# @inline evaluate(con::StageConstraint, z::AbstractKnotPoint) =
# evaluate(con, state(z), control(z))


# """
# jacobian!(∇c, con::AbstractConstraint, z, i=1) # stage constraint
# jacobian!(∇c, con::AbstractConstraint, z1, z2, i=1) # coupled constraint

# Evaluate the constraint `con` at knot point `z`. By default, this method will attempt to call

# jacobian!(∇c, con, x)

# if `con` is a `StateConstraint`,

# jacobian!(∇c, con, u)

# if `con` is a `ControlConstraint`, or

# jacobian!(∇c, con, x, u)

# if `con` is a `StageConstraint`. If `con` is a `CoupledConstraint` the constraint should
# define

# jacobian!(∇c, con, z, i)

# where `i` determines which Jacobian should be evaluated. E.g. if `i = 1`, the Jacobian
# with respect to the first knot point's stage and controls is calculated.

# # Automatic Differentiation
# If `con` is a `StateConstraint` or `ControlConstraint` then this method is automatically
# defined using ForwardDiff.
# """
# jacobian!(∇c, con::StateConstraint, z::AbstractKnotPoint, i = 1) =
# jacobian!(∇c, con, state(z))
# jacobian!(∇c, con::ControlConstraint, z::AbstractKnotPoint, i = 1) =
# jacobian!(∇c, con, control(z))
# jacobian!(∇c, con::StageConstraint, z::AbstractKnotPoint, i = 1) =
# jacobian!(∇c, con, state(z), control(z))

# # ForwardDiff jacobians that are of only state or control
# function jacobian!(∇c, con::StageConstraint, x::StaticVector)
# eval_c(x) = evaluate(con, x)
# ∇c .= ForwardDiff.jacobian(eval_c, x)
# return false
# end

# @inline ∇jacobian!(G, con::StateConstraint, z::AbstractKnotPoint, λ, i = 1) =
# ∇jacobian!(G, con, state(z), λ)
# @inline ∇jacobian!(G, con::ControlConstraint, z::AbstractKnotPoint, λ, i = 1) =
# ∇jacobian!(G, con, control(z), λ)
# @inline ∇jacobian!(G, con::StageConstraint, z::AbstractKnotPoint, λ, i = 1) =
# ∇jacobian!(G, con, state(z), control(z), λ)

# function ∇jacobian!(G, con::StageConstraint, x::StaticVector, λ)
# eval_c(x) = evaluate(con, x)'λ
# G_ = ForwardDiff.hessian(eval_c, x)
# G .+= G_
# return false
# end

# function ∇jacobian!(
# G,
# con::StageConstraint,
# x::StaticVector{n},
# u::StaticVector{m},
# λ,
# ) where {n,m}
# ix = SVector{n}(1:n)
# iu = SVector{m}(n .+ (1:m))
# eval_c(z) = evaluate(con, z[ix], z[iu])'λ
# G .+= ForwardDiff.hessian(eval_c, [x; u])
# return false
# end


gen_jacobian(con::AbstractConstraint) = gen_jacobian(Float64, con)
function gen_jacobian(::Type{T}, con::AbstractConstraint) where T
nm = RD.input_dim(con)
p = RD.output_dim(con)
zeros(T, p, nm)
end

# function gen_views(∇c::AbstractMatrix, con::StateConstraint, n = state_dim(con), m = 0)
# view(∇c, :, 1:n), view(∇c, :, n:n-1)
# end

# function gen_views(∇c::AbstractMatrix, con::ControlConstraint, n = 0, m = control_dim(con))
# view(∇c, :, 1:0), view(∇c, :, 1:m)
# end

# function gen_views(
# ∇c::AbstractMatrix,
# con::AbstractConstraint,
# n = state_dim(con),
# m = control_dim(con),
# )
# if size(∇c, 2) < n + m
# view(∇c, :, 1:n), view(∇c, :, n:n-1)
# else
# view(∇c, :, 1:n), view(∇c, :, n .+ (1:m))
# end
# end
Loading

0 comments on commit 6cbc3c9

Please sign in to comment.