added duration and symmetry consraints

src/constraints/linear_constraints.jl

@@ -7,6 +7,8 @@ export TimeStepsAllEqualConstraint
 export L1SlackConstraint
 export TotalConstraint
 export DurationConstraint
+export SymmetryConstraint
+export SymmetricControlConstraint
 ### 
 ### EqualityConstraint
 ###
@@ -262,4 +264,41 @@ function DurationConstraint(
     @assert traj.timestep isa Symbol
     indices = [index(k, traj.components[traj.timestep][1], traj.dim) for k ∈ 1:traj.T]
     return TotalConstraint(indices, value ,label)
+end
+
+
+struct SymmetryConstraint <: AbstractLinearConstraint
+    even_index_pairs::Vector{Tuple{Int64,Int64}}
+    odd_index_pairs::Vector{Tuple{Int64,Int64}}
+    label::String 
+end
+
+function SymmetricControlConstraint(
+    traj::NamedTrajectory,
+    name::Symbol,
+    idx::Vector{Int64};
+    even = true,
+    label = "Symmetry Constraint on $name"
+)
+    even_pairs = Vector{Tuple{Int64,Int64}}()
+    odd_pairs = Vector{Tuple{Int64,Int64}}()
+
+    component_indicies = [slice(t, traj.components[name], traj.dim)[idx] for t ∈ 1:traj.T]
+    if(even)
+        even_pairs = vcat(even_pairs,reduce(vcat,[collect(zip(component_indicies[[idx,traj.T - idx+1]]...)) for idx in 1:traj.T ÷ 2]))
+    else 
+        odd_pairs = vcat(odd_pairs,reduce(vcat,[collect(zip(component_indicies[[idx,traj.T - idx+1]]...)) for idx in 1:traj.T ÷ 2]))
+    end 
+
+    if traj.timestep isa Symbol
+        time_indices = [index(k, traj.components[traj.timestep][1], traj.dim) for k ∈ 1:traj.T]
+        even_pairs = vcat(even_pairs,[(time_indices[idx],time_indices[traj.T + 1 - idx]) for idx ∈ 1:traj.T÷2]) 
+    end 
+
+    return SymmetryConstraint(
+        even_pairs,
+        odd_pairs,
+        label
+    )
+
 end

src/solvers/ipopt_solver/constraints.jl

@@ -94,4 +94,25 @@ MOI.add_constraints(
     MOI.EqualTo(con.value)
 )
 
+end
+
+function (con::SymmetryConstraint)(
+    opt::Ipopt.Optimizer,
+    vars::Vector{MOI.VariableIndex}
+)
+
+    for (i1,i2) in con.even_index_pairs
+        MOI.add_constraints(
+                    opt,
+                    MOI.ScalarAffineFunction([MOI.ScalarAffineTerm(1.0, vars[i1]) , MOI.ScalarAffineTerm(-1.0, vars[i2])], 0.0),
+                    MOI.EqualTo(0.0)
+                )
+    end
+    for (i1,i2) in con.odd_index_pairs
+        MOI.add_constraints(
+                    opt,
+                    MOI.ScalarAffineFunction([MOI.ScalarAffineTerm(1.0, vars[i1]) , MOI.ScalarAffineTerm(1.0, vars[i2])], 0.0),
+                    MOI.EqualTo(0.0)
+                )
+    end
 end