CODEWITHSUNDEEP
optimizationinputconstraintsdrake
Taylor Baum
Taylor Baum

Reputation: 37

Constraint on Ability to Switch Input Values for Trajectory Optimization in Model Predictive Control Framework

I am trying to add a constraint which specifies that, in the optimization, the solver must pick a value of u for a set duration of time and can only switch after that set amount of time. For instance, say I have a mechanical device which can only switch its input value every 10 seconds. Then, I want the optimizer to account for that. I'll just attach the code here:

        for it_i in range(0, N-1, equivalence_samples):

            print("N: {}".format(N))

            for it_j in range(0, equivalence_samples - 1):

                if (it_i + it_j + 1) > N-1:
                    print("Breaking")
                    break
                else:
                    constraint_u0 = prog.AddConstraint(u[0, it_i + it_j] == u[0, it_i + it_j + 1]) # add equivalence constraints
                    constraint_u1 = prog.AddConstraint(u[1, it_i + it_j] == u[1, it_i + it_j + 1]) # add equivalence constraints
                    print('Constraint_u_PE: {}'.format(constraint_u0))
                    print('Constraint_u_NI: {}'.format(constraint_u1))

I have implemented this in, what I expect to be a working solution. Sometimes it seems like it is working and other times, it does not.

I will show some photos of the output constraints from this and then a not working example.

example constraints part 1

example constraints part 2

Then, here are the plots that come out which clearly show there is some delineation between the switching times, but the values are not equivalent. I am attaching the code which generates this plot as well.

almost equal inputs!!

        u_sol = result.GetSolution(u)
        u_time = np.linspace(0, N-1, num = N)
        # u_sol_trajectory = PiecewisePolynomial.ZeroOrderHold(u_time, u_sol)

        plt.figure()
        plt.plot(u_time, u_sol[0, :], 'o')
        plt.plot(u_time, u_sol[1, :], 'o')
        plt.xlabel('time steps')
        plt.ylabel('u [mcg/min]')
        plt.legend(['u_PE', 'u_NI'])

Upvotes: 0

Views: 57

Answers (1)

Taylor Baum
Taylor Baum

Reputation: 37

The particular solver that was being used in this case is the OSQP solver. Although I, in an ideal solver world, specified the correct constraints in the above code (that input 1 == input 2, input 2 == input 3, etc.), I did not account for the fact that solvers have an accuracy with which they try to uphold the constraints.

I can fix this problem by either updating the accuracy of the solver (as recommended by https://osqp.discourse.group/t/constraints-violated/143) or inputing more explicit constraints. I solved this with the second option. Now, I am specifying not just constraints like the following pattern:

input 1 == input 2, input 2 == input 3, etc.

but I am also including constraints like the following pattern:

input 1 == input 3, input 1 == input 4, input 1 == input 5

input 2 == input 4, input 2 == input 5 etc.

By being more explicit, my solver is now doing what I asked with small deviations from the constraint. The small deviations are acceptable for my application, however! It is a bit slower, but this isn't a problem for what I am using this for at the moment. Here is my updated code:


        for it_i in range(0, N-1, equivalence_samples):
            
            for it_j in range(0, equivalence_samples - 1):

                for it_f in range(1, equivalence_samples - it_j):

                    if (it_i + it_j + it_f) > N-1:
                        print("Breaking")
                        break
                    else:
                        con_0 = eq(u[:, it_i + it_j], u[:, it_i + it_j + it_f])
                        constraint_u = prog.AddConstraint(con_0) # add equivalence constraints
                        print('Constraint_u: {}'.format(constraint_u))

Not the prettiest code in the world, but it works.

Upvotes: 1

Related Questions