Ejemplo n.º 1
0
    def _cap_top(self, idx, cap_max):
        if cap_max is None:
            return

        # x_out = min(x, cap_max) or equivalently x_out = -max(-x, -cap_max)
        # x_out = -xs1, xs1 = max(-x, -cap_max) or equivalently
        # x_out = -xs1, xs1 = max(xs2, xs3), xs2 = -x, xs3 = -cap_max
        x = self.model_output_vars[idx]
        xs1 = getNewVariable("xs")
        xs2 = getNewVariable("xs")
        xs3 = getNewVariable("xs")
        x_out = getNewVariable("xs")

        constraint1 = Constraint(
            "EQUALITY",
            monomials=[Monomial(1.0, x_out),
                       Monomial(1.0, xs1)],
            scalar=0)  # x_out = -xs1
        constraint2 = MaxConstraint(varsin=[xs2, xs3],
                                    varout=xs1)  # xs1 = max(xs2, xs3)
        constraint3 = Constraint(
            "EQUALITY",
            monomials=[Monomial(1.0, xs2),
                       Monomial(1.0, x)],
            scalar=0)  # set xs2 = -x
        constraint4 = Constraint("EQUALITY",
                                 monomials=[Monomial(1.0, xs3)],
                                 scalar=-cap_max)  # set xs1 = -cap_max
        self.constraints += [
            constraint1, constraint2, constraint3, constraint4
        ]
        self.model_output_vars[idx] = x_out
Ejemplo n.º 2
0
def constraint_variable_to_interval(variable, LB, UB):
    p1 = Constraint(ConstraintType('GREATER'))
    p1.monomials = [Monomial(1, variable)]
    p1.scalar = LB  # 0 #
    #
    p2 = Constraint(ConstraintType('LESS'))
    p2.monomials = [Monomial(1, variable)]
    p2.scalar = UB
    return [p1, p2]
Ejemplo n.º 3
0
 def setup_euler_constraint(self, dx_vec, dt):
     for x, dx, next_x in zip(self.states.reshape(-1), dx_vec,
                              self.next_states.reshape(-1)):
         c = Constraint(ConstraintType('EQUALITY'))
         c.monomials = [
             Monomial(1, x),
             Monomial(dt, dx),
             Monomial(-1, next_x)
         ]
         self.euler_constraints.append(c)
Ejemplo n.º 4
0
 def setup_euler_constraints(self):
     for x, dx, next_x in zip(self.states.reshape(-1), self.dx,
                              self.next_states.reshape(-1)):
         # next_x = x + dx*dt
         c = Constraint(ConstraintType('EQUALITY'))
         c.monomials = [
             Monomial(1, x),
             Monomial(self.dt, dx),
             Monomial(-1, next_x)
         ]
         self.constraints.append(c)
Ejemplo n.º 5
0
def ex1():
    print("~~~~~ test 1 ~~~~~")
    m = [Monomial(5, 'x'), Monomial(6, 'y')]
    c = Constraint('LESS', monomials=m, scalar=5)
    # 5x 6y < 5
    print(c)
    print(c.complement())
    #
    p = ConstraintProperty([c])
    print(p)
    print(p.complement())
Ejemplo n.º 6
0
def ex3():
    print("~~~~~ test 3 ~~~~~")
    m = [Monomial(-5, 'x'), Monomial(-6, 'y')]
    c = Constraint('GREATER', monomials=m, scalar=-5)
    # -5x -6y < -5
    print(c)
    print(c.complement())
    #
    p = ConstraintProperty([c])
    print(p)
    print(p.complement())
Ejemplo n.º 7
0
 def assert_init(self, init_set):
     """
     assert that states are in init set
     """
     for k in init_set.keys():
         self.input_vars.append(k)
         LB = init_set[k][0]
         UB = init_set[k][1]
         cLB = Constraint('GREATER_EQ',
                          monomials=[Monomial(1., k)],
                          scalar=LB)
         cUB = Constraint('LESS_EQ', monomials=[Monomial(1., k)], scalar=UB)
         self.constraints += [cLB, cUB]
Ejemplo n.º 8
0
def ex4():
    print("~~~~~ test 4 ~~~~~")
    m1 = [Monomial(-5, 'x'), Monomial(-6, 'y')]
    c1 = Constraint('GREATER', monomials=m1, scalar=-5)
    # -5x -6y < -5
    #
    m2 = [Monomial(-4, 'x'), Monomial(-5, 'y')]
    c2 = Constraint('GREATER', monomials=m2, scalar=-4)
    # -4x -5y < -4
    #
    p = ConstraintProperty([c1, c2])
    print(p)
    print(p.complement())
Ejemplo n.º 9
0
def equality_constraint(varsin, varsout):
    """
    If you need a list of scalar constraints instead of a single matrix constraint.
    """
    assert (len(varsin) == len(varsout))
    if len(varsin) > 1:
        mc = matrix_equality_constraint(varsin, varsout)
        return matrix_to_scalar(mc)
    else:
        mono1 = Monomial(1, varsin[0][0])
        mono2 = Monomial(-1, varsout[0][0])
        return Constraint(ConstraintType('EQUALITY'),
                          monomials=[mono1, mono2],
                          scalar=0)
Ejemplo n.º 10
0
    def read_inequalities(self):
        for i in range(self.n_ineq):
            left_var = self.f['/ineq/varleft%d' % (i + 1)][()][0]
            rite_var = self.f['/ineq/varright%d' % (i + 1)][()][0]
            for v in [left_var, rite_var]:
                if v not in self.var_dict.keys():
                    self.var_dict[v] = getNewVariable('xd')

            # add lvar <= rvar
            monomial_list = [
                Monomial(1, self.var_dict[left_var]),
                Monomial(-1, self.var_dict[rite_var])
            ]
            self.ineq_list.append(
                Constraint(ConstraintType('LESS_EQ'), monomial_list, 0))
Ejemplo n.º 11
0
def infeasible_problem_relu():
    c1 = ReluConstraint(varin="x", varout="y")
    c2 = Constraint("LESS_EQ", monomials=[Monomial(1., "y")], scalar=-5)
    solver = GurobiPyWrapper()
    solver.assert_constraints([c1, c2])
    result, vals, stats = solver.check_sat()
    print("result is:", result)
Ejemplo n.º 12
0
def ex2():
    print("~~~~~ test 2 ~~~~~")
    m1 = [Monomial(5, 'x'), Monomial(6, 'y')]
    c1 = Constraint('LESS', monomials=m1, scalar=5)
    # 5x 6y < 5
    print(c1)
    print(c1.complement())
    #
    m2 = [Monomial(4, 'x'), Monomial(5, 'y')]
    c2 = Constraint('LESS', monomials=m2, scalar=4)
    # 4x 5y < 4
    print(c2)
    print(c2.complement())
    #
    p = ConstraintProperty([c1, c2])
    print(p)
    print(p.complement())
Ejemplo n.º 13
0
 def __init__(self):
     self.control_inputs = ['T']
     self.states = np.array(['theta', 'theta_dot'], dtype=object)
     self.dx = ['theta_dot', 'theta_double_dot']
     # the constraints that define the dx variables.
     # "theta_double_dot = T + sin(theta) - 0.2*theta_dot"
     self.dx_constraints = [
         NLConstraint('EQUALITY', out='v1', fun="sin", indep_var="theta"),
         Constraint('EQUALITY',
                    monomials=[
                        Monomial(1.0, "T"),
                        Monomial(1.0, "v1"),
                        Monomial(-0.2, "theta_dot"),
                        Monomial(-1.0, "theta_double_dot")
                    ],
                    scalar=0)
     ]
Ejemplo n.º 14
0
 def popluate_graph_mateq(self, G, eq):
     A = eq.A
     x = eq.x
     for i in range(A.shape[0]):
         new_eq = Constraint("EQUALITY", monomials=[])
         for j in range(A.shape[1]):
             Aij = A[i][j]
             if Aij != 0.:
                 new_eq.monomials.append(Monomial(coeff=Aij, var=x[j]))
         G = self.populate_graph_eq(G, new_eq)
     return G
Ejemplo n.º 15
0
def feasible_problem():
    c1 = Constraint('EQUALITY',
                    monomials=[Monomial(1., "theta@0"),
                               Monomial(-2., "x@1")],
                    scalar=1.3)
    c2 = MatrixConstraint('LESS_EQ',
                          A=np.random.rand(3, 3),
                          x=["x", "theta", "x2"],
                          b=np.random.rand(3, ))
    # NOTE: FOR GUROBI INTERFACE, variable stringnames in x must be in a list NOT a numpy array, inside MatrixConstraint
    c3 = MaxConstraint(varsin=['t', 'b'], varout="theta")
    c4 = ReluConstraint(varin="bob", varout="alice")

    solver = GurobiPyWrapper()

    solver.assert_init({'theta': [7, 9], 'bob': [-10, 1]})

    solver.assert_constraints([c1, c2, c3, c4])

    result, vals, stats = solver.check_sat()
Ejemplo n.º 16
0
 def _turn_max_to_relu(self):
     new_constraints = []
     for c in self.constraints:
         if isinstance(c, MaxConstraint):
             # varnew1 - varin1 + varin2 = 0
             # varnew2 = relu(varnew1)
             # varout - varnew2 - varin2 = 0
             var1in = c.var1in
             var2in = c.var2in
             varout = c.varout
             varnew1 = getNewVariable("max2relu")
             varnew2 = getNewVariable("max2relu")
             cnew1 = Constraint("EQUALITY")
             cnew1.monomials = [
                 Monomial(1.0, varnew1),
                 Monomial(-1.0, var1in),
                 Monomial(1.0, var2in)
             ]
             cnew2 = ReluConstraint(varin=varnew1, varout=varnew2)
             cnew3 = Constraint("EQUALITY")
             cnew3.monomials = [
                 Monomial(1.0, varout),
                 Monomial(-1.0, var2in),
                 Monomial(-1.0, varnew2)
             ]
             new_constraints.extend([cnew1, cnew2, cnew3])
         else:
             new_constraints.append(c)
     self.constraints = new_constraints
Ejemplo n.º 17
0
    def read_equations(self):
        for i in range(self.n_eq):
            vars = self.f['/eq/vars%d' % (i + 1)][()]
            for v in vars:
                if v not in self.var_dict.keys():
                    self.var_dict[v] = getNewVariable('xd')

            coeffs = self.f['/eq/coeffs%d' % (i + 1)][()].astype(np.float)
            b = self.f['/eq/scalar%d' % (i + 1)][()].astype(np.float)[0]
            monomial_list = [
                Monomial(c, self.var_dict[v]) for (c, v) in zip(coeffs, vars)
            ]
            self.eq_list.append(
                Constraint(ConstraintType('EQUALITY'), monomial_list, b))
Ejemplo n.º 18
0
    def _cap_bottom(self, idx, cap_min):
        if cap_min is None:
            return

        # x_out =  max(x, cap_min)
        x = self.model_output_vars[idx]
        xs1 = getNewVariable("xs")
        x_out = getNewVariable("xs")
        constraint1 = Constraint("EQUALITY",
                                 monomials=[Monomial(1.0, xs1)],
                                 scalar=cap_min)  # set xs1 = cap_min
        constraint2 = MaxConstraint(varsin=[x, xs1],
                                    varout=x_out)  # x_out = max(x, xs1)
        self.constraints += [constraint1, constraint2]
        self.model_output_vars[idx] = x_out
Ejemplo n.º 19
0
    def cnf_conversion_helper(self, constraint):
        """
        Take a constraint of the form 
        5x + 6y -5 R 0 and turns it into

        Y == 5x + 6y - 5
        (for later to assert: Y >=0 or max(Y, somethingelse) >= 0)
        """
        # turn into >= inequality: -5x -6y <= -5  -->  5x + 6y >= 5
        geq_comp = constraint.get_geq()
        # define new var: Y = 5x + 6y - 5
        new_var_constraint = copy.deepcopy(geq_comp)
        new_var_constraint.type = ConstraintType('EQUALITY')
        Y = self.get_new_var()
        new_var_constraint.monomials += [Monomial(
            -1, Y)]  # -a + 5x + 6y == 5  ->  5x + 6y -5 == a
        return [new_var_constraint, Y]
Ejemplo n.º 20
0
def ex5():
    print("~~~~~ test 5 ~~~~~")
    m1 = [Monomial(-5, 'x'), Monomial(-6, 'y')]
    c1 = Constraint('GREATER', monomials=m1, scalar=-5)
    # -5x -6y < -5
    #
    m2 = [Monomial(-4, 'x'), Monomial(-5, 'y')]
    c2 = Constraint('GREATER', monomials=m2, scalar=-4)
    # -4x -5y < -4
    m3 = [Monomial(-3, 'x'), Monomial(-4, 'y')]
    c3 = Constraint('GREATER', monomials=m3, scalar=-3)
    # -3x -4y < -3
    #
    p = ConstraintProperty([c1, c2, c3])
    print(p)
    print(p.complement())
Ejemplo n.º 21
0
def linear_plant_test():
    """
    linear plant test
    # phi
    y = x  
    # phi hat
    OA: x - 1 <= y <= x + 1
    """
    print("~~~~~~~~~~~lin plant test~~~~~~~~~~~~~~~\n")
    # phi
    x = 'x'
    y = 'y'
    phi = [Constraint("EQUALITY",
                      [Monomial(1, x), Monomial(-1, y)], 0)]  # x - y == 0
    # phi hat
    c1 = Constraint("LESS_EQ",
                    [Monomial(1, x), Monomial(-1, y)], 1)  # x - y <= 1
    c2 = Constraint("LESS_EQ",
                    [Monomial(-1, x), Monomial(1, y)], 1)  # y - x <= 1
    phi_hat = [c1, c2]
    # check!
    oav = OverapproxVerifier(phi, phi_hat)
    oav.create_smtlib_script(
    )  # should write to file that can be checked with: https://cvc4.github.io/app/
Ejemplo n.º 22
0
def infeasible_problem():
    c1 = Constraint('LESS_EQ', monomials=[Monomial(1., "x")], scalar=5)
    c2 = Constraint('GREATER_EQ', monomials=[Monomial(1., "x")], scalar=10)
    solver = GurobiPyWrapper()
    solver.assert_constraints([c1, c2])
    result, vals, stats = solver.check_sat()
Ejemplo n.º 23
0
def test_marabou_interface(alpha,
                           prop_desc,
                           n_invar,
                           with_relu=False,
                           with_max=False):
    # create controller object, this is just a place holder. I will modify the object later.
    model = load_model(
        "../OverApprox/models/single_pend_nn_controller_lqr_data.h5")
    controller = KerasController(keras_model=model)

    # rewrite to make a simple controller that is always equal to alpha*x
    controller.control_outputs = [['c']]
    controller.state_inputs = [['xc']]
    fake_constraint = []
    if with_relu:
        alpha_times_x = 'var1'
        monomial_list = [
            Monomial(alpha, controller.state_inputs[0][0]),
            Monomial(-1, alpha_times_x)
        ]
        fake_constraint.append(
            Constraint(ConstraintType('EQUALITY'), monomial_list, 0.0))
        relu_constraint = [
            ReluConstraint(varin=alpha_times_x,
                           varout=controller.control_outputs[0][0])
        ]
        controller.constraints = relu_constraint + fake_constraint
        controller.relus = relu_constraint
    elif with_max:
        alpha_times_x = 'var1'
        monomial_list = [
            Monomial(alpha, controller.state_inputs[0][0]),
            Monomial(-1, alpha_times_x)
        ]
        fake_constraint.append(
            Constraint(ConstraintType('EQUALITY'), monomial_list, 0.0))
        max_second_arg = 'var2'
        fake_constraint.append(
            Constraint(ConstraintType('EQUALITY'),
                       [Monomial(1, max_second_arg)], -1 / 2))
        max_constraint = [
            MaxConstraint(varsin=[alpha_times_x, max_second_arg],
                          varout=controller.control_outputs[0][0])
        ]
        controller.constraints = max_constraint + fake_constraint
        controller.relus = []
    else:
        monomial_list = [
            Monomial(-1, controller.control_outputs[0][0]),
            Monomial(alpha, controller.state_inputs[0][0])
        ]
        fake_constraint = [
            Constraint(ConstraintType('EQUALITY'), monomial_list, 0.0)
        ]
        controller.constraints = fake_constraint
        controller.relus = []

    # create overt dynamics objects. this is just a place holder. I will modify the object later.
    overt_obj = OvertConstraint(
        "../OverApprox/models/single_pend_acceleration_overt.h5")

    # rewrite to make a simple controller that is always equal to x
    overt_obj.control_vars = [['cd']]
    overt_obj.state_vars = [['x']]
    overt_obj.output_vars = [['dx']]
    monomial_list2 = [
        Monomial(1, overt_obj.control_vars[0][0]),
        Monomial(-1, overt_obj.output_vars[0][0])
    ]
    fake_constraint2 = [
        Constraint(ConstraintType('EQUALITY'), monomial_list2, 0.5)
    ]
    overt_obj.constraints = fake_constraint2

    simple_dynamics = Dynamics(np.array(['x']), np.array(['cd']))
    next_states = simple_dynamics.next_states.reshape(1, )

    # x_next = x + dt*dx
    dt = 1
    c1 = Constraint(ConstraintType('EQUALITY'))
    c1.monomials = [
        Monomial(1, overt_obj.state_vars[0][0]),
        Monomial(dt, overt_obj.output_vars[0][0]),
        Monomial(-1, next_states[0])
    ]

    simple_dynamics.constraints = [c1] + overt_obj.constraints

    print(len(simple_dynamics.constraints))
    print(len(controller.constraints))

    # create transition relation using controller and dynamics
    tr = TFControlledTransitionRelation(dynamics_obj=simple_dynamics,
                                        controller_obj=controller)

    # initial set
    init_set = {overt_obj.state_vars[0][0]: (0., 1.)}

    # build the transition system as an (S, I(S), TR) tuple
    ts = TransitionSystem(states=tr.states,
                          initial_set=init_set,
                          transition_relation=tr)

    # property x< 0.105, x' < 0.2
    p = Constraint(ConstraintType(prop_desc["type"]))
    p.monomials = [Monomial(1, overt_obj.state_vars[0][0])]
    p.scalar = prop_desc["scalar"]  #
    prop = ConstraintProperty([p], [overt_obj.state_vars[0][0]])

    # solver
    solver = MarabouWrapper()
    algo = BMC(ts=ts, prop=prop, solver=solver)
    result, vals, stats = algo.check_invariant_until(n_invar)
    return result.name
Ejemplo n.º 24
0
    output = tf.nn.relu(tf.matmul(W1,x) + b1)
    W2 =  np.random.rand(1,2)
    b2 =  np.random.rand(1,1)
    output = tf.nn.relu(tf.matmul(W2,output) + b2)
    sess.run(tf.global_variables_initializer()) # actually sets Variable values to values specified

# smoosh all tf.Variables to tf.Constants, put into new graph
new_graph = smoosh_to_const(sess, output.op.name)
# create controller object with network
controller = TFController(tf_sess=tf.Session(graph=new_graph), inputNames=[x.op.name], outputName=output.op.name)

# create a super simple plant directly using constraint objects
dynamics = Dynamics(states=np.array([["x"], ["y"]]), controls=["u"], fun=np.sin)
# x' = relu(x + u)   ->   x + u - z = 0 , x' = relu(z)
c1 = Constraint(ConstraintType('EQUALITY'))
c1.monomials = [Monomial(1, "x"), Monomial(1,"u"), Monomial(-1,"z")]
c3 = ReluConstraint(varin="z", varout="x'")
# y' = y  ->  y - y' = 0
c2 = Constraint(ConstraintType('EQUALITY'))
c2.monomials = [Monomial(1,"y"), Monomial(-1, "y'")]
dynamics.constraints = [c1,c2,c3]

# create transition relation using controller and dynamics
tr = TFControlledTransitionRelation(dynamics_obj=dynamics, 
                                        controller_obj=controller)

# initial set
init_set = {"x": (1.1,2), "y": (-1,1)}

# build the transition system as an (S, I(S), TR) tuple
ts = TransitionSystem(states=tr.states, initial_set=init_set, transition_relation=tr)
Ejemplo n.º 25
0
import numpy as np
from dreal_interface import FormulaConverter, OverapproxVerifier
from MC_constraints import Constraint, Monomial, MaxConstraint, ReluConstraint, MatrixConstraint, NLConstraint

f = FormulaConverter()

print(f.prefix_notate("+", ["A", "B"]))

print(f.declare_const("A", "Bool"))

print(f.define_atom("A", "(< y 5)"))

print(f.negate("(< y 5)"))

c1 = Constraint('LESS_EQ', [Monomial(-6, "x"), Monomial(5, "y")], -2)
print(f.convert_Constraint(c1))

c2 = MaxConstraint(['v1', 'v2'], 'v3')
print(f.convert_MaxConstraint(c2))

c3 = ReluConstraint('p', 'q')
print(f.convert_ReluConstraint(c3))

c4 = MatrixConstraint('EQUALITY',
                      A=np.random.rand(2, 2),
                      x=np.array([['x'], ['y']], dtype='object'),
                      b=np.zeros((2, 1)))
print(f.convert_MatrixConstraint(c4))

c5 = NLConstraint('EQUALITY', "v1", "sin", "x")
print(f.convert_NLConstraint(c5))
Ejemplo n.º 26
0
# initial set
x1_init_set = (0.5, 1)
x2_init_set = (-0.5, 0.5)
init_set = {states[0]: x1_init_set, states[1]: x2_init_set}

# build the transition system as an (S, I(S), TR) tuple
ts = TransitionSystem(states=tr.states,
                      initial_set=init_set,
                      transition_relation=tr)

# solver
solver = GurobiPyWrapper()  #MarabouWrapper()

prop_list = []
p1 = Constraint(ConstraintType('GREATER'))
p1.monomials = [Monomial(1, states[0])]
p1.scalar = 0.3
prop_list.append(p1)

p2 = Constraint(ConstraintType('LESS'))
p2.monomials = [Monomial(1, states[0])]
p2.scalar = 1.15
prop_list.append(p2)

# p3 = Constraint(ConstraintType('GREATER'))
# p3.monomials = [Monomial(1, states[1])]
# p3.scalar = -1.1
# prop_list.append(p3)
# #
# p4 = Constraint(ConstraintType('LESS'))
# p4.monomials = [Monomial(1, states[1])]
Ejemplo n.º 27
0
    def convert_to_CNF(self, DNF_complements):
        """
        Converts complements of constraints in DNF to CNF
        using Max.
        Populates self.constraint_complements.
        """
        CNF_complements = []
        n_clauses = len(DNF_complements)
        if n_clauses == 0:
            pass

        elif n_clauses == 1:
            # define new var: Y = 5x + 6y - 5
            Y_definition, Y = self.cnf_conversion_helper(DNF_complements[0])
            # we want Y >= 0
            Y_ineq = Constraint(ConstraintType('GREATER_EQ'),
                                monomials=[Monomial(1, Y)],
                                scalar=0)
            CNF_complements.extend([Y_definition, Y_ineq])

        else:  # nclauses > 1
            # handle first ineq
            Y_def, Y = self.cnf_conversion_helper(DNF_complements[0])
            CNF_complements.append(Y_def)
            # then disjunct all the complements using MaxConstraint
            for c in DNF_complements[1:]:
                # take complement, and turn into >= inequality
                Z_def, Z = self.cnf_conversion_helper(c)
                # begin disjunct train max(Z,Y) >= 0 ...
                Q = self.get_new_var()
                ###########################################################
                # changing max to be represented with Relu
                YmZ = self.get_new_var()
                YmZdef = Constraint('EQUALITY',
                                    monomials=[
                                        Monomial(1, Y),
                                        Monomial(-1, Z),
                                        Monomial(-1, YmZ)
                                    ],
                                    scalar=0)
                RYmZ = self.get_new_var()
                RYmZdef = ReluConstraint(varin=YmZ, varout=RYmZ)
                # Q = relu(Y-Z) + Z
                max_constraint = Constraint('EQUALITY',
                                            monomials=[
                                                Monomial(1, RYmZ),
                                                Monomial(1, Z),
                                                Monomial(-1, Q)
                                            ],
                                            scalar=0)
                ###########################################################
                # max_constraint = MaxConstraint((Y,Z), Q) # version with max
                # CNF_complements.extend([Z_def, max_constraint]) # version with max
                ############################################################
                CNF_complements.extend(
                    [Z_def, YmZdef, RYmZdef, max_constraint])
                Y = Q
            # Q >= 0
            geq0 = Constraint(ConstraintType('GREATER_EQ'),
                              monomials=[Monomial(1, Q)],
                              scalar=0)
            CNF_complements.append(geq0)

        self.constraint_complements = CNF_complements
        return CNF_complements
Ejemplo n.º 28
0

controls = overt_obj_1.control_vars
acceleration_1 = overt_obj_1.output_vars[0]
acceleration_2 = overt_obj_2.output_vars[0]

double_pendulum_dynamics = Dynamics(np.array(states).reshape(-1, 1), np.array(controls).reshape(-1, 1))
next_states = double_pendulum_dynamics.next_states.reshape(4,)

print(states, controls, acceleration_1, acceleration_2, next_states)

dt = 0.01

# x1_next = x1 + dt*u1
c1 = Constraint(ConstraintType('EQUALITY'))
c1.monomials = [Monomial(1, theta1), Monomial(dt, theta1d), Monomial(-1, next_states[0])]
print(c1.monomials)


# x2_next = x2 + dt*u2
c2 = Constraint(ConstraintType('EQUALITY'))
c2.monomials = [Monomial(1, theta2), Monomial(dt, theta2d), Monomial(-1, next_states[1])]
print(c2.monomials)

# u1_next = u1 + dt*a1
c3 = Constraint(ConstraintType('EQUALITY'))
c3.monomials = [Monomial(1, theta1d), Monomial(dt, acceleration_1), Monomial(-1, next_states[2])]
print(c3.monomials)

# u2_next = u2 + dt*a2
c4 = Constraint(ConstraintType('EQUALITY'))