Manual RK4 method for solving IVP (data formatting problem)

Question

Currently I'm attempting to solve 4 coupled ODE's to stabilize an inverted pendulum on a cart. I have no problem doing it with ODEINT from Scipy, however, I can't make it work with a manual implementation. Most likely this is due to some weird data formatting done in the 'model' function in the code.

I have tried multiple things to no avail, thus I won't post my error codes, since they range from the size not fitting when adding all the calculated steps in the RK4 method.

My current code with ODEINT working is down below. What I'm asking is whether someone can help me, so that the function 'model' is properly made, so that I can implement the RK4 solver (which I can do for other ODE's without any problem).

import numpy as np
from scipy.integrate import solve_ivp
from scipy import signal
g = 9.82
l = 0.281 
mc = 6.28
alpha = 0.4
mp = 0.175
t_start = 0.
t_end = 12.
tol = 10**(-1)
# Define A and B and the poles we want
A = np.array([[0., 1., 0., 0.], [(mc+mp)*g/(l*mc), 0., 0., (-alpha)/(l*mc)], [0., 0., 0., 1.], [(g*mp)/mc, 0., 0., (-alpha)/mc]])
B = np.array([[0.], [1./(l*mc)], [0.], [1./mc]])
Poles = np.array([complex(-1.,2.), complex(-1.,-2.), complex(-2.,1.), complex(-2.,-1.)])

# Determine K
signal = signal.place_poles(A, B, Poles)
K = signal.gain_matrix
# print(signal.computed_poles) # To verify if the computes poles are correct

# Define the model
def model(t,x):
    x1, x2, x3, x4 = x
    u = -np.matmul(K,x)
    dx1dt = x2
    dx2dt = (np.cos(x1.astype(float))*(u-alpha*x4-mp*l*x2**2*np.sin(x1.astype(float)))+(mc+mp)*g*np.sin(x1.astype(float)))/(l*(mc+mp*(1-np.cos(x1.astype(float))**2)))
    dx3dt = x4
    dx4dt = (u-alpha*x4-mp*l*x2**2*np.sin(x1.astype(float))+mp*g*np.sin(x1.astype(float))*np.cos(x1.astype(float)))/(mc+mp*(1-np.cos(x1.astype(float))**2))
    return np.array([dx1dt, dx2dt, dx3dt, dx4dt])

# Solve the system
N = 10000 # Number of steps
t = np.linspace(t_start, t_end, N)
t_span = (t_start, t_end)
x0 = np.array([0.2, 0., 0., 0.])
sol = solve_ivp(model,t_span,x0, t_eval=t, method='RK45')
index = np.argmin(sol.y[2,:]) # Max displacement from the origin
print(f' The biggest deviation from the origin is: {abs(sol.y[2, index])} meters.')

#This doesn't work
def RK4(fcn,a ,b ,y0 ,N):
    h = (b-a)/N
    x = a + np.arange(N+1)*h
    y = np.zeros((x.size,y0.size))
    y[0,:] = y0
    for k in range(N):
        k1 = fcn(x[k], y[k,:])
        k2 = fcn(x[k] + h/2, y[k,:] + h*k1/2)
        k3 = fcn(x[k] + h/2, y[k,:] + h*k2/2)
        k4 = fcn(x[k] + h, y[k,:] + h*k3)
        y[k+1,:] = y[k,:] + h/6*(k1 + 2*(k2 + k3) + k4)
    
    return x,y

a,b = RK4(model, 0, 12, x0, 1000)

Which yields the following error:

runcell(0, 'C:/Users/Nikolai Lund Kühne/OneDrive - Aalborg Universitet/Uni/3. semester/P3 - Dynamiske Systemer/manualRK4.py')
 The biggest deviation from the origin is: 0.48256054833140316 meters.
Traceback (most recent call last):

  File "C:\Users\Nikolai Lund Kühne\OneDrive - Aalborg Universitet\Uni\3. semester\P3 - Dynamiske Systemer\manualRK4.py", line 57, in <module>
    a,b = RK4(model, 0, 12, x0, 1000)

  File "C:\Users\Nikolai Lund Kühne\OneDrive - Aalborg Universitet\Uni\3. semester\P3 - Dynamiske Systemer\manualRK4.py", line 53, in RK4
    y[k+1,:] = y[k,:] + h/6*(k1 + 2*(k2 + k3) + k4)

ValueError: could not broadcast input array from shape (4,4,4) into shape (4)

Edit 2: Attempt to implement RK4 manually results in some weird errors.

Edit 1: Based on a comment the code is now implemented with solve_ivp.

You do not need the second line in the `model` function, as the `x` passed as argument from the solver is already a numpy array. On the other hand, formatting the return tuple as numpy array is required if you want to apply this in a hand-crafted RK4 routine in the easy way. // Try to change from odeint to solve_ivp and use the option without t_eval to get the used step-sizes in the returned sequence. The RK4 step sizes need to be smaller (5th order method vs. 4th order). — Lutz Lehmann, Nov 30 '21 at 15:21
@LutzLehmann Yeah I can see that the line serves no purpose, thank you, I updated the code. Changing from odeint to solve_ivp without doing anything else to the code yields a `TypeError: unhashable type: 'numpy.ndarray'`. The documentation for solve_ivp says that the calling signature is `fun(t, x)` but changing this yields the same error. — Nikolai, Nov 30 '21 at 20:22
@LutzLehmann It would appear that I was too quick, I can make solve_ivp work flawlessly. Now to make a manual implementation with the code. — Nikolai, Nov 30 '21 at 20:31
For testing purposes you could also add a `print(t)` statement into the model function. Then you can directly see what the time sequence for the actual evaluations is. I think in general there are 2 evaluations per time step (odeint uses a linear multi-step method, or a dynamically changing collection of them). — Lutz Lehmann, Nov 30 '21 at 20:48
@LutzLehmann Lehmann I tried to implement the RK4 model and I edited my post so the error code is displayed. I tried printing k1,k2,k3 and k4 and they all looked very strange, any suggestions? — Nikolai, Dec 02 '21 at 09:25
What is the reason for the `.astype(float)`? Somewhere broadcasting gets involved and produces the 3rd order tensor. There always strange effects if matrices and arrays get mixed, check the type and format of the `K` matrix and the matrix-array product. Use flatten or ravel if necessary. — Lutz Lehmann, Dec 02 '21 at 09:40
@LutzLehmann Else I get the following error: ```AttributeError: 'float' object has no attribute 'cos' The above exception was the direct cause of the following exception: Traceback (most recent call last): ... line 57, in a,b = RK4(model, 0, 12, x0, 1000) ... line 51, in RK4 k3 = fcn(x[k] + h/2, y[k,:] + h*k2/2) ... line 27, in model dx2dt = (np.cos(x1)*(u-alpha*x4-mp*l*x2**2*np.sin(x1))+(mc+mp)*g*np.sin(x1))/(l*(mc+mp*(1-np.cos(x1)**2))) TypeError: loop of ufunc does not support argument 0 of type float which has no callable cos method``` — Nikolai, Dec 02 '21 at 09:46
@LutzLehmann It works! K.flatten() did it! I did as you said and printed all matrices and arrays and checked, that their sizes and such made sence! Thanks a lot. If you compile all the comments in an answer I'll be happy to mark this question as answered. I'll update the code immediately — Nikolai, Dec 02 '21 at 09:54

score 1 · Accepted Answer · answered Dec 02 '21 at 10:11

I did not completely debug this, and you could also reduce the data to a state where the expected happens. So some speculation.

Numpy is halping in the style of Matlab. The constructed format of K is an array in the shape of a row vector, [[K1,K2,K3,K4]]. Now the matrix-vector multiplication in any form, K@x, has a one-dimensional result. Mathematically, one would expect either a scalar or a 1x1 matrix [[u1]]. Following the Matlab philosophy it is neither, it is a simple array u=[u1]. Any further scalar operation that has u inside will also result in 1-element arrays. Putting the derivatives together, this has the effect of producing a column vector. Now further operations with arrays have the potential to broadcast that to a 4x4 matrix-shaped array. How the 4x4x4 shaped tensor occurs I did not follow-up on, but it seems quite possible.

Manual RK4 method for solving IVP (data formatting problem)

1 Answers1