Using problem based optimization to control a system

As a learning excercise I'm trying to get the optimal control force for a spring mass system using the optimization toolbox. The solutions that I'm getting seems very wrong. The validaton script that runs the open control loop seems to verify there is an error but I'm not sure what my error is, though I'm pretty sure it has to do with some constraints somewhere.

Here is the code:

clc
clear
close all

p0 = 2;
pf = 1;
v0 = 0;

k = 1;
m = 1;

T = 5;                                                                    % Total time
N = 50;                                                                   % Number of discretization points
init.F = ones(N,1);

[massModel, p, t] = springMassSystem(N, T,p0,pf,v0);
sol = solve(massModel, init);

Solving problem using fmincon.

Local minimum possible. Constraints satisfied.

fmincon stopped because the size of the current step is less than
the value of the step size tolerance and constraints are 
satisfied to within the value of the constraint tolerance.

plotMotionAndForce(sol,p,p0,pf,t,T)

springMassVerify(m,k,p0,v0,sol.F,t,T)

function [mass, p, t] = springMassSystem(N, T, p0, pf, v0)
    t = T/N;                                                              % discretized time step size
    Fmax = 10;                                                            % Max force
    a_max = 3;
    s_max = 5;
    k = 1;
    m = 1;

    F = optimvar('F',N,1,'LowerBound',-Fmax, 'UpperBound',Fmax);          % Our controlling force
    p = optimexpr(N,1);                                                   % position variable - controlled
    accel_x = (-k*p + F)/m; % ma = -kx + F                                % accelleration of spring mass damper system

    a_cons = optimconstr(N);
    for i = 1:N-1
        a_cons(i) = accel_x(i) <= a_max;                              % accel constraint - magnitude of a should be less than a_max
    end

    v = cumsum([v0; t*accel_x(1:N-1)]);                                   % v equals the cumulative sum of v0 + accel*time - vel at each time instance
    p = cumsum([p0; t*v(1:N-1) + 1/2*t*t*(accel_x(1:N-1))]);              % use pos equation with pos and accel instead of avg velocity 

    accel_xCon = accel_x(N,:) == 0;
    vEndCon = v(N,:) == 0;                                                % set end point velocity as a constraint
    pEndCon = p(N,:) == pf;                                               % set end point position as a constraint

    mass = optimproblem('ObjectiveSense','min');                          % model
    s = norm(p);
    mass.Objective = sum(s);                                              % we want to minimize the total distance traveled
        
%     mass.Constraints.accel_xCon = accel_xCon;
    mass.Constraints.aCon = a_cons;
    mass.Constraints.vEndCon = vEndCon;
    mass.Constraints.pEndCon = pEndCon;
end

%verification for the spring mass system
function springMassVerify(m,k,p0,v0,F,t,T)
    positions = [];
    positions(1) = p0;
    velocities = [];
    velocities(1) = v0;
    
    N = length(F);
    for i = 1:N
        [positions(i+1), velocities(i+1)] = springMassUpdate(m,k,positions(i), velocities(i),F(i),t);
    end
    plotPos(positions,t,T)
end

% updated the positin and velocity of the spring mass system
function [posNew, velNew]= springMassUpdate(m,k,x,v,F,t)
    accel = (-k*x + F)/m;
    velNew = v + t*accel;
    posNew = x + t*v + 1/2*t*t*accel;
end

% plotting for validation
function plotPos(positions,t,T)
figure
plot(t:t:T, positions(1:end-1),'rx')
xlabel("Time step")
ylabel("position/acceleration")
end

% main plotting
function plotMotionAndForce(sol,p,p0,pF,t,T)
figure
t_vec = t:t:T;

psol = evaluate(p, sol);
plot(t_vec,psol(1:end,1),'rx')
hold on
plot(t,p0(1),'ks')
plot(T,pF(1),'bo')

fsol = sol.F;
plot(t_vec(:), fsol,"mx")
xlabel("Time step")
ylabel("position/acceleration")
legend("Steps","Initial Point","Final Point","accel")
end

Answer 1

[Hiro Yoshino]

It seems that it simply found a local optimum. If this is not the one that you expected, you should change the intial values. Try something similar to the solution to see if it converges the solution.

By the way, why is "init" structure?

[2Lindsay2]

The init is a structure because I was getting an error message saying the initialization needed to be a structure.

I'm a little skeptical that this is a valid solution though. The verification function shows that the system is doing something else.

Answer 2

[Sam Chak]

Hi @2Lindsay2,

I'm unsure of what your expectation is. However, if the final position is changed to the origin, then the convergence is observed. Thus, you should try modifying the objective function for a non-zero final state. Can also try optimizing according to the Integral Time Absolute Error (ITAE) criterion.

p0 = 1;     % initial position  <--
pf = 0;     % final position    <--
v0 = 0;     % initial velocity

k  = 1;     % stiffness
m  = 1;     % mass

T  = 5;     % Total time
N  = 50;    % Number of discretization points
init.F = ones(N,1);

[massModel, p, t] = springMassSystem(N, T, p0, pf, v0);
sol = solve(massModel, init);

Solving problem using fmincon.

Local minimum possible. Constraints satisfied.

fmincon stopped because the size of the current step is less than
the value of the step size tolerance and constraints are 
satisfied to within the value of the constraint tolerance.

plotMotionAndForce(sol, p, p0, pf, t, T)

springMassVerify(m, k, p0, v0, sol.F, t, T)

function [mass, p, t] = springMassSystem(N, T, p0, pf, v0)
    t     = T/N;    % discretized time step size
    Fmax  = 10;     % Max force
    a_max = 3;
    s_max = 5;
    k     = 1;
    m     = 1;

    F = optimvar('F', N, 1, 'LowerBound', -Fmax, 'UpperBound', Fmax);   % Our controlling force
    p = optimexpr(N, 1);                                                % position variable - controlled
    accel_x = (F - k*p)/m;  % ma = -kx + F                              % accelleration of spring mass damper system

    a_cons = optimconstr(N);
    for i = 1:N-1
        a_cons(i) = accel_x(i) <= a_max;                        % accel constraint - magnitude of a should be less than a_max
    end

    v = cumsum([v0; t*accel_x(1:N-1)]);                         % v equals the cumulative sum of v0 + accel*time - vel at each time instance
    p = cumsum([p0; t*v(1:N-1) + 1/2*t*t*(accel_x(1:N-1))]);    % use pos equation with pos and accel instead of avg velocity 

    accel_xCon = accel_x(N,:) == 0;
    vEndCon = v(N,:) == 0;                                      % set end point velocity as a constraint
    pEndCon = p(N,:) == pf;                                     % set end point position as a constraint

    mass           = optimproblem('ObjectiveSense','min');      % model
    s              = norm(p);
    mass.Objective = sum(s);                                    % we want to minimize the total distance traveled
        
%     mass.Constraints.accel_xCon = accel_xCon;
    mass.Constraints.aCon    = a_cons;
    mass.Constraints.vEndCon = vEndCon;
    mass.Constraints.pEndCon = pEndCon;
end

%verification for the spring mass system
function springMassVerify(m, k, p0, v0, F, t, T)
    positions     = [];
    positions(1)  = p0;
    velocities    = [];
    velocities(1) = v0;
    
    N = length(F);
    for i = 1:N
        [positions(i+1), velocities(i+1)] = springMassUpdate(m, k, positions(i), velocities(i), F(i), t);
    end
    plotPos(positions, t, T)
end

% updated the positin and velocity of the spring mass system
function [posNew, velNew]= springMassUpdate(m, k, x, v, F, t)
    accel = (- k*x + F)/m;
    velNew = v + t*accel;
    posNew = x + t*v + 1/2*t*t*accel;
end

% plotting for validation
function plotPos(positions, t, T)
figure
plot(t:t:T, positions(1:end-1), 'rx')
xlabel("Time step")
ylabel("position/acceleration")
end

% main plotting
function plotMotionAndForce(sol, p, p0, pF, t, T)
    figure
    t_vec = t:t:T;
    
    psol  = evaluate(p, sol);
    plot(t_vec, psol(1:end,1), 'rx'), grid on
    xlabel("Time step")
    ylabel("Position")
    % hold on
    % plot(t, p0(1), 'ks')
    % plot(T, pF(1), 'bo')
    % 
    % fsol = sol.F;
    % plot(t_vec(:), fsol, "mx")
    % xlabel("Time step")
    % ylabel("position/acceleration")
    % legend("Steps", "Initial Point", "Final Point", "accel", 'location', 'best')
end