fix constraint_violation run_ci

Author: thowell

examples/test.jl

@@ -0,0 +1,48 @@
+using ForwardDiff 
+using Symbolics
+
+
+# function 
+my_func(x) = cos(x[1])
+
+# variables
+@variables x[1:1]
+
+# symbolic functions
+f = my_func(x)
+df = Symbolics.gradient(f, x)
+
+# julia functions 
+f_func = eval(Symbolics.build_function(f, x))
+df_func = eval(Symbolics.build_function(df, x)[1])
+
+# eval functions
+f_func([0.5 * π])
+df_func([0.5 * π])
+
+# diff gradient function (autodiff)
+ForwardDiff.jacobian(df_func, [0.5 * π])
+
+# diff gradient function (symbolics)
+dfs = df_func(x)
+ddfs = Symbolics.gradient(dfs[1], x)
+
+
+## 
+using Symbolics
+using LinearAlgebra
+
+function midpoint(f,x,u,dt)
+    return x + dt*f(x + 0.5*dt*f(x,u),u)
+end
+
+function car(x, u)
+    [u[1] * cos(x[3]); u[1] * sin(x[3]); u[2]]
+end
+
+@variables x[1:3] u[1:2]
+dt = .1
+val = midpoint(car,x,u,dt)
+@show val
+jac = Symbolics.jacobian(val,[x;u])
+@show jac

src/constraints.jl

@@ -98,9 +98,9 @@ function constraint_violation(c_data::ConstraintsData; norm_type=Inf)
     c_max = 0.0
     for t = 1:T
         nc = cons[t].nc 
+        ineq = cons[t].idx_ineq
         for i = 1:nc 
-            cti = c_data.c[t][i]
-            (i in cons[t].idx_ineq) && (cti = max(0.0, cti))
+            cti = (i in ineq) ? max(0.0, c_data.c[t][i]) : abs(c_data.c[t][i])
             c_max = max(c_max, cti)
         end
     end

src/dynamics.jl

@@ -34,13 +34,6 @@ function step!(d::Dynamics, x, u, w)
     return d.val_cache
 end
 
-# function eval_con!(c, cons::Model, x, u, w) 
-#     for (t, con) in enumerate(cons)
-#         step!(con, x[t], u[t], w[t])
-#         @views c[t] .= con.val_cache
-#     end
-# end
-
 function eval_con_jac!(jx, ju, cons::Model, x, u, w)
     for (t, con) in enumerate(cons) 
         con.jacx(con.jacx_cache, x[t], u[t], w[t])