Develop Reference

Solving Receding Horizon Control in GEKKO

I'm trying to implement a receding horizon control (RHC) scheme using GEKKO in Python, and I'd like to check my formulation. The goal is to solve the OCP over some horizon from t=tk to t=tk+H-1, apply the control solution at tk, and discard the remaining values (u_k+1 to u_k+H-1). The following code appears to give the correct solution, but I want to verify I've used the correct functions in GEKKO, namely when "resetting" the states for the next horizon. I had a few issues trying to use the .VALUE function to reset x1 and x2, e.g. TypeError: 'float' object is not subscriptable.
import numpy as np
import matplotlib.pylab as plt
from gekko import GEKKO
if __name__ == '__main__':
# Instantiate GEKKO
m = GEKKO()
# Constants
nRHC = 21
tRHC = 2
m.time = np.linspace(0, tRHC, nRHC)
# Control
u = m.MV(value=0.0,fixed_initial=False)
u.STATUS = 1
u.DCOST = 0
# Vars
t = m.SV(value=0)
x1 = m.SV(value=1)
x2 = m.SV(value=0)
# Equations
m.Equation(t.dt() == 1)
m.Equation(x1.dt() == x2)
m.Equation(x2.dt() == (1 - x2*x2)*x1 - x2 + u)
# Objective Function
m.Minimize(10*x1**2 + 10*x2**2 + u**2)
# Solve RHC
m.options.IMODE = 6
m.options.NODES = 11
m.options.MV_TYPE = 2
m.options.SOLVER = 3
nTotal = 101
tTotal = np.linspace(0, 10, nTotal)
uStore = np.zeros((1,nTotal))
xStore = np.zeros((2,nTotal))
xStore[:,0] = [1, 0]
for i in range(nTotal):
print('Solving Step: ', i+1, ' of ', nTotal-1)
if i == nTotal-1:
break
# Solve MPC over horizon
m.solve(disp=False)
# Update States
t.VALUE = t[1]
x1.MEAS = x1[1]
x2.MEAS = x2[1]
# Store
uStore[:,i] = u.NEWVAL
xStore[:,i+1] = np.array([x1[1], x2[1]])
# Plot States
f1, axs = plt.subplots(2)
axs[0].plot(tTotal, xStore[0,:])
axs[0].set_ylabel('x')
axs[0].grid()
axs[1].plot(tTotal, xStore[1,:])
axs[1].set_ylabel('x_dot')
axs[1].set_xlabel('time')
axs[1].grid()
# Show Plots
plt.show()
Thank you!

There is no need to update the states because Gekko does this automatically.
# Update States
t.VALUE = t[1]
x1.MEAS = x1[1]
x2.MEAS = x2[1]
The state values are stored in run directory files (see m.path or open with m.open_folder()). The file is ctl.t0. At the next command m.solve(), that file is imported and time shifted to make the values at the next time step the initial conditions. The time shift is adjusted with m.options.TIME_SHIFT=1 (1 is the default). If you do want to override the initial condition, use x1.MEAS=x1.value[1] or x1.value=x1.value[1].

Changing an ODE inside the function with ODEINT and python

This is my first question here on the forum, so I hope I'm doing evrything right. My problem is, I am supposed to solve an ODE with python. The ODE describes the movement of an electron inside of a "bubble" (a bubble is something that occurs in Plasmaphysics when you shoot with a laser at plasma but thats not that important for the question im asking). So my task is to build a programm that can plot the trajectory of such an electron. For that I have to basically solve 2 different ODE's since the electron inside the bubble is moving according to an ODE and if it exits the bubble it is in a field-free-area where it should move according to the classical equations of motion.
In the following I will show you the code if only 1 ODE is solved:
import numpy as np
from scipy.integrate import odeint
import matplotlib.pyplot as plt
from math import sqrt,pi
def f(r,t):
px = r[0]
py = r[1]
x = r[2]
y = r[3]
gamma = sqrt(1+px**2+py**2)
dpx_dt = -(1+V)*(x-V*t)/4 + py*y/(4*gamma)
dpy_dt = -(1+px/gamma)*y/4
dx_dt = px/gamma
dy_dt = py/gamma
return [dpx_dt, dpy_dt, dx_dt, dy_dt]
def Plot_Aufgabe5(t,p_x,p_y,xi,y):
#Plotten der Trajektorie des Elektrons
plt.plot(xi,y)
return
#Konstanten
r_b = 10
m = 9.109*10**-31
c = 2.99*10**8
roh = 9
#Anfangsbedingungen
gamma_0 = 4
V = np.sqrt(((gamma_0**2) - 1)/(gamma_0**2))
t_end = 300
N = 1000
t = np.linspace(0,t_end,N)
y = [0, 0, np.sqrt(r_b**2 - roh**2), roh]
#Array mit äquidistanten Werten erstellen
Äqui_array = np.linspace(0,2*np.pi,10)
#Array mit verschd. Anfangsimpulsen
Impuls_array = [-1,-0.5,0.5,1]
for j in Impuls_array:
plt.figure()
#Plotten eines Kreises
theta = np.linspace(0,2*np.pi,t_end)
Punkt_x = r_b*np.cos(theta)
Punkt_y = r_b*np.sin(theta)
plt.plot(Punkt_x,Punkt_y)
for i in range(10):
#Neue Anfangsbedingungen aufstellen
y = [j,j,r_b*np.cos(Äqui_array[i]),r_b*np.sin(Äqui_array[i])]
#Ausführen des Solvers
sol = odeint(f,y,t)
p_x = sol[:,0]
p_y = sol[:,1]
x = sol[:,2]
xi = x - V*t
y = sol[:,3]
#Plot
Plot_Aufgabe5(t,p_x,p_y,xi,y)
plt.axis([-15,15,-10,10])
plt.xlabel(r'$xi_{num}$ = $k_p$$\xi$')
plt.ylabel(r'$y_{num}$ = $k_p$y')
plt.title('Trajektorie des Elekrons in einer Bubble mit Impuls p = ' + str(j) + '*mc')
plt.show()
Plots for the code for 1 ODE
As you can see in the plot, the electron goes outside the bubble but it comes back in which should not happen. (Also the plot is for 10 different electrons which start on the edge of the bubble)
Therefore I made an approach to change between 2 ODE's according to the position of the electron however that did not quite work:
def f(r,t):
px = r[0] #boundary conditions for the electron
py = r[1]
x = r[2]
y = r[3]
gamma = sqrt(1+px**2+py**2)
if ((x**2 + y**2) <= r_b**2): #if electron is inside bubble
dpx_dt = -(1+V)*(x-V*t)/4 + py*y/(4*gamma)
dpy_dt = -(1+px/gamma)*y/4
dx_dt = px/gamma
dy_dt = py/gamma
else: #if it is outside of the bubble
dpx_dt = 0
dpy_dt = 0
dx_dt = px/gamma
dy_dt = py/gamma
result = [dpx_dt, dpy_dt, dx_dt, dy_dt]
return result
My Idea here was that with the if-statement i check, whether the electron is inside the bubble or not and change the ODE according to that. However it does not really work out and im not sure why, underneath you can find the plots that come with that function (also the rest of the code stayed the same as above):
Plots for the Function to change between 2 ODE's
I hope my problem gets clear and that maybe someone has an idea that could help me! Good day to evryone :)

How to save value of variable from one function call and use it to the next function call?

I am implementing a cartesian control of a robotic manipulator and below you can see the function which produces the joint velocities in order to perform the control:
def cartesian_control(joint_transforms, b_T_ee_current, b_T_ee_desired,
red_control, q_current, q0_desired):
num_joints = len(joint_transforms)
dq = numpy.zeros(num_joints)
ee_current_T_b = tf.transformations.inverse_matrix(b_T_ee_current)
current_T_desired = numpy.dot(ee_current_T_b, b_T_ee_desired)
current_P_desired = tf.transformations.translation_from_matrix(current_T_desired)
current_R_desired = current_T_desired[:3,:3]
angle, axis = rotation_from_matrix(current_T_desired)
rot_movement = numpy.dot(angle,axis)
desired_R_current = tf.transformations.inverse_matrix(current_R_desired)
delta_X = numpy.append(current_P_desired, rot_movement)
# Proportional - Derivative Controller
Kp = 5 # Proportional Gain
Kd = 1 # Derivative Gain
delta_delta_X = (delta_X - delta_X_prev)/0.01
x_dot = Kp*delta_X + Kd*delta_delta_X
# Scale translational and angular velocities for smoothness
for i in range(3):
if x_dot[i] > 1:
x_dot[i] = 1
for i in range(3,1,5):
if x_dot[i] > 0.1:
x_dot[i] = 0.1
J = numpy.empty((6,0))
V = numpy.zeros((6,6))
for i in range(num_joints):
b_T_joint = joint_transforms[i]
ee_T_joint = numpy.dot(ee_current_T_b, b_T_joint)
joint_T_ee = tf.transformations.inverse_matrix(ee_T_joint)
ee_R_joint = rotation_matrix(ee_T_joint)
joint_P_ee = tf.transformations.translation_from_matrix(joint_T_ee)
joint_skew_matrix = S_matrix(joint_P_ee)
V[:3,:3] = ee_R_joint
V[3:6,3:6] = ee_R_joint
V[:3, 3:6] = numpy.dot(-ee_R_joint, joint_skew_matrix)
J = numpy.column_stack((J, V[:,5]))
J_pseudoinv = numpy.linalg.pinv(J, rcond=0.01)
dq = numpy.dot(J_pseudoinv, x_dot)
# Scale joint velocities
for i in range(len(dq)):
if dq[i] > 0.1:
dq[i] = 0.1
return dq
The above function works perfectly in case I implement just a proportional controller, which means that in case of x_dot = Kp*delta_X + Kd*delta_delta_X1 I have only x_dot = Kp*delta_X. However, I would like also to implement a proportional derivative controller but my problem is how to obtain the delta_X_prev variable. This means that I need to find a way of saving the value of the variable delta_X of one function call and use it at the function call. Did some search and I found that the multiprocessing python module would help but really haven't understood the whole concept of it. Is there any other simpler way it order to do that?

Large angle pendulum plot did not show as expected

I am trying to plot the relationship between period and amplitude for an undamped and undriven pendulum for when small angle approximation breaks down, however, my code did not do what I expected...
I think I should be expecting a strictly increasing graph as shown in this video: https://www.youtube.com/watch?v=34zcw_nNFGU
Here is my code, I used zero crossing method to calculate period:
import numpy as np
import matplotlib.pyplot as plt
from scipy.integrate import solve_ivp
from itertools import chain
# Second order differential equation to be solved:
# d^2 theta/dt^2 = - (g/l)*sin(theta) - q* (d theta/dt) + F*sin(omega*t)
# set g = l and omega = 2/3 rad per second
# Let y[0] = theta, y[1] = d(theta)/dt
def derivatives(t,y,q,F):
return [y[1], -np.sin(y[0])-q*y[1]+F*np.sin((2/3)*t)]
t = np.linspace(0.0, 100, 10000)
#initial conditions:theta0, omega0
theta0 = np.linspace(0.0,np.pi,100)
q = 0.0 #alpha / (mass*g), resistive term
F = 0.0 #G*np.sin(2*t/3)
value = []
amp = []
period = []
for i in range (len(theta0)):
sol = solve_ivp(derivatives, (0.0,100.0), (theta0[i], 0.0), method = 'RK45', t_eval = t,args = (q,F))
velocity = sol.y[1]
time = sol.t
zero_cross = 0
for k in range (len(velocity)-1):
if (velocity[k+1]*velocity[k]) < 0:
zero_cross += 1
value.append(k)
else:
zero_cross += 0
if zero_cross != 0:
amp.append(theta0[i])
# period calculated using the time evolved between the first and last zero-crossing detected
period.append((2*(time[value[zero_cross - 1]] - time[value[0]])) / (zero_cross -1))
plt.plot(amp,period)
plt.title('Period of oscillation of an undamped, undriven pendulum \nwith varying initial angular displacemnet')
plt.xlabel('Initial Displacement')
plt.ylabel('Period/s')
plt.show()
enter image description here

You can use the event mechanism of solve_ivp for such tasks, it is designed for such "simple" situations
def halfperiod(t,y): return y[1]
halfperiod.terminal=True # stop when root found
halfperiod.direction=1 # find sign changes from negative to positive
for i in range (1,len(theta0)): # amp==0 gives no useful result
sol = solve_ivp(derivatives, (0.0,100.0), (theta0[i], 0.0), method = 'RK45', events =(halfperiod,) )
if sol.success and len(sol.t_events[-1])>0:
period.append(2*sol.t_events[-1][0]) # the full period is twice the event time
amp.append(theta0[i])
This results in the plot

Solving ODE numerically with Python

I am solving an ODE for an harmonic oscillator numerically with Python. When I add a driving force it makes no difference, so I'm guessing something is wrong with the code. Can anyone see the problem? The (h/m)*f0*np.cos(wd*i) part is the driving force.
import numpy as np
import matplotlib.pyplot as plt
# This code solves the ODE mx'' + bx' + kx = F0*cos(Wd*t)
# m is the mass of the object in kg, b is the damping constant in Ns/m
# k is the spring constant in N/m, F0 is the driving force in N,
# Wd is the frequency of the driving force and x is the position
# Setting up
timeFinal= 16.0 # This is how far the graph will go in seconds
steps = 10000 # Number of steps
dT = timeFinal/steps # Step length
time = np.linspace(0, timeFinal, steps+1)
# Creates an array with steps+1 values from 0 to timeFinal
# Allocating arrays for velocity and position
vel = np.zeros(steps+1)
pos = np.zeros(steps+1)
# Setting constants and initial values for vel. and pos.
k = 0.1
m = 0.01
vel0 = 0.05
pos0 = 0.01
freqNatural = 10.0**0.5
b = 0.0
F0 = 0.01
Wd = 7.0
vel[0] = vel0 #Sets the initial velocity
pos[0] = pos0 #Sets the initial position
# Numerical solution using Euler's
# Splitting the ODE into two first order ones
# v'(t) = -(k/m)*x(t) - (b/m)*v(t) + (F0/m)*cos(Wd*t)
# x'(t) = v(t)
# Using the definition of the derivative we get
# (v(t+dT) - v(t))/dT on the left side of the first equation
# (x(t+dT) - x(t))/dT on the left side of the second
# In the for loop t and dT will be replaced by i and 1
for i in range(0, steps):
vel[i+1] = (-k/m)*dT*pos[i] + vel[i]*(1-dT*b/m) + (dT/m)*F0*np.cos(Wd*i)
pos[i+1] = dT*vel[i] + pos[i]
# Ploting
#----------------
# With no damping
plt.plot(time, pos, 'g-', label='Undampened')
# Damping set to 10% of critical damping
b = (freqNatural/50)*0.1
# Using Euler's again to compute new values for new damping
for i in range(0, steps):
vel[i+1] = (-k/m)*dT*pos[i] + vel[i]*(1-(dT*(b/m))) + (F0*dT/m)*np.cos(Wd*i)
pos[i+1] = dT*vel[i] + pos[i]
plt.plot(time, pos, 'b-', label = '10% of crit. damping')
plt.plot(time, 0*time, 'k-') # This plots the x-axis
plt.legend(loc = 'upper right')
#---------------
plt.show()

The problem here is with the term np.cos(Wd*i). It should be np.cos(Wd*i*dT), that is note that dT has been added into the correct equation, since t = i*dT.
If this correction is made, the simulation looks reasonable. Here's a version with F0=0.001. Note that the driving force is clear in the continued oscillations in the damped condition.
The problem with the original equation is that np.cos(Wd*i) just jumps randomly around the circle, rather than smoothly moving around the circle, causing no net effect in the end. This can be best seen by plotting it directly, but the easiest thing to do is run the original form with F0 very large. Below is F0 = 10 (ie, 10000x the value used in the correct equation), but using the incorrect form of the equation, and it's clear that the driving force here just adds noise as it randomly moves around the circle.

Note that your ODE is well behaved and has an analytical solution. So you could utilize sympy for an alternate approach:
import sympy as sy
sy.init_printing() # Pretty printer for IPython
t,k,m,b,F0,Wd = sy.symbols('t,k,m,b,F0,Wd', real=True) # constants
consts = {k: 0.1, # values
m: 0.01,
b: 0.0,
F0: 0.01,
Wd: 7.0}
x = sy.Function('x')(t) # declare variables
dx = sy.Derivative(x, t)
d2x = sy.Derivative(x, t, 2)
# the ODE:
ode1 = sy.Eq(m*d2x + b*dx + k*x, F0*sy.cos(Wd*t))
sl1 = sy.dsolve(ode1, x) # solve ODE
xs1 = sy.simplify(sl1.subs(consts)).rhs # substitute constants
# Examining the solution, we note C3 and C4 are superfluous
xs2 = xs1.subs({'C3':0, 'C4':0})
dxs2 = xs2.diff(t)
print("Solution x(t) = ")
print(xs2)
print("Solution x'(t) = ")
print(dxs2)
gives
Solution x(t) =
C1*sin(3.16227766016838*t) + C2*cos(3.16227766016838*t) - 0.0256410256410256*cos(7.0*t)
Solution x'(t) =
3.16227766016838*C1*cos(3.16227766016838*t) - 3.16227766016838*C2*sin(3.16227766016838*t) + 0.179487179487179*sin(7.0*t)
The constants C1,C2 can be determined by evaluating x(0),x'(0) for the initial conditions.