Simulating a Rotary Inverted Pendulum in Python

[Remusco]

Homework Statement

I need to simulate a rotary inverted pendulum in Python for a school project. After about a week of struggling with this, I need help.

Relevant Equations

# Compute accelerations using the equations of motion
alphaddot = tau_m / J_p # Motor arm angular acceleration
thetaddot = ((m_p * g * l / 2) * np.sin(theta[k-1]) + tau_p) / J_p #pendulum acceleration

[Mentor Note: Two threads on the same project have been merged into one]

I need serious help with this. I'm totally stuck as to why my results are mediocre. It would mean a ton if someone could at least point me in the right direction.
Below is my attempt:

import matplotlib.pyplot as plt
import numpy as np
from scipy.integrate import solve_ivp
# System Parameters
m_p = 0.5 # Mass of the pendulum (kg)
l = 0.4 # Length of the pendulum (m)
r_a = 0.2 # Length of the motor-driven arm (m)
g = 9.81 # Gravity (m/s^2)
J_p = (1/3) * m_p * l**2 # Moment of inertia of the pendulum (rod pivoted at one end)
# Motor Parameters
V_supply = 18 # Motor supply voltage (V)
R_motor = 9 # Motor resistance (Ohms)
k_e = .1 # Back-EMF constant (V/(rad/s))
k_t = .1 # Motor torque constant (Nm/A)
# PID controller parameters for pendulum
Kp_pend, Ki_pend, Kd_pend = 10, 0, 0
# PID controller parameters for motor arm
Kp_arm, Ki_arm, Kd_arm = 5, 0, 0
# Desired angles
theta_desired = 0 # Upright pendulum position
alpha_desired = np.radians(135) # Desired arm angle
# Simulation Parameters
t_sim = np.linspace(0, 20,800) # Time vector
dt = t_sim[1] - t_sim[0]
# Initialize State Variables
theta = np.zeros(len(t_sim)) # Pendulum angle
thetadot = np.zeros(len(t_sim)) # Pendulum angular velocity
alpha = np.zeros(len(t_sim)) # Motor arm angle
alphadot = np.zeros(len(t_sim)) # Motor arm angular velocity
u_control_signal = np.zeros(len(t_sim)) # Motor control signal PWM
# Define initial conditions
theta[0], alpha[0] = np.radians(2), np.radians(270)
thetadot[0], alphadot[0] = 0, 0
# Initialize the PID Control Variables at 0
integral_term_pend, derivative_term_pend = 0, 0
integral_term_arm, derivative_term_arm = 0, 0
# Previous error initialization
previous_theta_error = 0
previous_alpha_error = 0
# Simulation loop
for k in range(1, len(t_sim)):
# Compute errors (desired - actual)
theta_error = (theta_desired - theta[k-1])
alpha_error = (alpha_desired - alpha[k-1])
# PID control calculations for pendulum
integral_term_pend += theta_error * dt
derivative_term_pend = (theta_error - previous_theta_error) / dt
u_pend = Kp_pend * theta_error + Ki_pend * integral_term_pend + Kd_pend * derivative_term_pend
# PID control calculations for motor arm
integral_term_arm += alpha_error * dt
derivative_term_arm = (alpha_error - previous_alpha_error) / dt
u_arm = Kp_arm * alpha_error + Ki_arm * integral_term_arm + Kd_arm * derivative_term_arm
# Total control signal
u_control_signal[k] = np.clip(u_pend + u_arm,-255,255)
# Convert control signal (PWM) to motor voltage
V_motor = (u_control_signal[k] / 255) * V_supply
# Estimate motor current
I_motor = (V_motor - k_e * alphadot[k-1]) / R_motor
# Compute motor torque
tau_m = k_t * I_motor
# Compute pendulum torque from motor torque
tau_p = (tau_m * l) / r_a
# Compute accelerations using the equations of motion
alphaddot = tau_m / J_p # Motor arm angular acceleration
thetaddot = ((m_p * g * l / 2) * np.sin(theta[k-1]) + tau_p) / J_p # Pendulum angular acceleration
# Update velocities and positions using Euler integration
alphadot[k] = alphadot[k-1] + alphaddot * dt
thetadot[k] = thetadot[k-1] + thetaddot * dt
alpha[k] = alpha[k-1] + alphadot[k] * dt
theta[k] = theta[k-1] + thetadot[k] * dt
# Update previous errors
previous_theta_error = theta_error
previous_alpha_error = alpha_error
# Plot Results
plt.figure(figsize=(12, 6))
# Plot Pendulum Angle Response
plt.subplot(2, 1, 1)
plt.plot(t_sim, theta, label=r'$\theta$ (Pendulum Angle)')
plt.plot(t_sim, alpha, label=r'$\alpha$ (Motor Arm Angle)')
plt.axhline(y=0, color='gray', linestyle='dotted', label=r'$\theta = 0$ (Upright)')
plt.xlabel('Time (s)')
plt.ylabel('Angle (rad)')
plt.title('PID-Controlled Pendulum Response')
plt.legend()
plt.grid(True)
# Plot Control Signal (u)
plt.subplot(2, 1, 2)
plt.plot(t_sim, u_control_signal, label=r'$u$ (Motor Control Signal)', color='red')
plt.xlabel('Time (s)')
plt.ylabel('PWM Signal (u)')
plt.title('Control Signal Output from PID Controller')
plt.legend()
plt.grid(True)
plt.tight_layout()
plt.show()

 

[pbuk]

Don't use icode tags for blocks of code, use code=Python like this.

alphaddot = tau_m / J_p # Motor arm angular acceleration
thetaddot = ((m_p * g * l / 2) * np.sin(theta[k-1]) + tau_p) / J_p #pendulum acceleration

import matplotlib.pyplot as plt
import numpy as np
from scipy.integrate import solve_ivp
# System Parameters
m_p = 0.5  # Mass of the pendulum (kg)
l = 0.4  # Length of the pendulum (m)
r_a = 0.2 # Length of the motor-driven arm (m)
g = 9.81  # Gravity (m/s^2)
J_p = (1/3) * m_p * l**2  # Moment of inertia of the pendulum (rod pivoted at one end)
# Motor Parameters
V_supply = 18  # Motor supply voltage (V)
R_motor = 9  # Motor resistance (Ohms)
k_e = .1  # Back-EMF constant (V/(rad/s))
k_t = .1  # Motor torque constant (Nm/A)
# PID controller parameters for pendulum
Kp_pend, Ki_pend, Kd_pend = 10, 0, 0
# PID controller parameters for motor arm
Kp_arm, Ki_arm, Kd_arm = 5, 0, 0
# Desired angles
theta_desired = 0  # Upright pendulum position
alpha_desired = np.radians(135)  # Desired arm angle
# Simulation Parameters
t_sim = np.linspace(0, 20,800)  # Time vector
dt = t_sim[1] - t_sim[0]
# Initialize State Variables
theta = np.zeros(len(t_sim))  # Pendulum angle
thetadot = np.zeros(len(t_sim))  # Pendulum angular velocity
alpha = np.zeros(len(t_sim))  # Motor arm angle
alphadot = np.zeros(len(t_sim))  # Motor arm angular velocity
u_control_signal = np.zeros(len(t_sim))  # Motor control signal PWM
# Define initial conditions
theta[0], alpha[0] = np.radians(2), np.radians(270)
thetadot[0], alphadot[0] = 0, 0
# Initialize the PID Control Variables at 0
integral_term_pend, derivative_term_pend = 0, 0
integral_term_arm, derivative_term_arm = 0, 0
# Previous error initialization
previous_theta_error = 0
previous_alpha_error = 0
# Simulation loop
for k in range(1, len(t_sim)):
    # Compute errors (desired - actual)
    theta_error = (theta_desired - theta[k-1])
    alpha_error = (alpha_desired - alpha[k-1])
    # PID control calculations for pendulum
    integral_term_pend += theta_error * dt
    derivative_term_pend = (theta_error - previous_theta_error) / dt
    u_pend = Kp_pend * theta_error + Ki_pend * integral_term_pend + Kd_pend * derivative_term_pend
    # PID control calculations for motor arm
    integral_term_arm += alpha_error * dt
    derivative_term_arm = (alpha_error - previous_alpha_error) / dt
    u_arm = Kp_arm * alpha_error + Ki_arm * integral_term_arm + Kd_arm * derivative_term_arm
    # Total control signal
    u_control_signal[k] = np.clip(u_pend + u_arm,-255,255)
    # Convert control signal (PWM) to motor voltage
    V_motor = (u_control_signal[k] / 255) * V_supply
    # Estimate motor current
    I_motor = (V_motor - k_e * alphadot[k-1]) / R_motor
    # Compute motor torque
    tau_m = k_t * I_motor
    # Compute pendulum torque from motor torque
    tau_p = (tau_m * l) / r_a
    # Compute accelerations using the equations of motion
    alphaddot = tau_m / J_p  # Motor arm angular acceleration
    thetaddot = ((m_p * g * l / 2) * np.sin(theta[k-1]) + tau_p) / J_p  # Pendulum angular acceleration
    # Update velocities and positions using Euler integration
    alphadot[k] = alphadot[k-1] + alphaddot * dt
    thetadot[k] = thetadot[k-1] + thetaddot * dt
    alpha[k] = alpha[k-1] + alphadot[k] * dt
    theta[k] = theta[k-1] + thetadot[k] * dt
    # Update previous errors
    previous_theta_error = theta_error
    previous_alpha_error = alpha_error
# Plot Results
plt.figure(figsize=(12, 6))
# Plot Pendulum Angle Response
plt.subplot(2, 1, 1)
plt.plot(t_sim, theta, label=r'$\theta$ (Pendulum Angle)')
plt.plot(t_sim, alpha, label=r'$\alpha$ (Motor Arm Angle)')
plt.axhline(y=0, color='gray', linestyle='dotted', label=r'$\theta = 0$ (Upright)')
plt.xlabel('Time (s)')
plt.ylabel('Angle (rad)')
plt.title('PID-Controlled Pendulum Response')
plt.legend()
plt.grid(True)
# Plot Control Signal (u)
plt.subplot(2, 1, 2)
plt.plot(t_sim, u_control_signal, label=r'$u$ (Motor Control Signal)', color='red')
plt.xlabel('Time (s)')
plt.ylabel('PWM Signal (u)')
plt.title('Control Signal Output from PID Controller')
plt.legend()
plt.grid(True)
plt.tight_layout()
plt.show()

 

[pbuk]

I'm totally stuck as to why my results are mediocre.

You import scipy.integrate.solve_ivp but you are not using it and have hand-rolled Euler integration instead. Why?

Have you tried altering the parameters of e.g. the moment of inertia of the rod (dramatically, like a factor of 2)? How does this affect the result?

 

[Remusco]

You import scipy.integrate.solve_ivp but you are not using it and have hand-rolled Euler integration instead. Why?

Have you tried altering the parameters of e.g. the moment of inertia of the rod (dramatically, like a factor of 2)? How does this affect the result?

I tried both methods and both produced the same results. I used Euler integration for simplicity and because I know how it works.
Yes I played with the parameters for hours. I feel like my physics model/underlying equations may not be accurate. The thing that I'm most confused about is that the motor arm does not oscillate with the pendulum. Instead I get results like:

I would expect the motor arm angle to change as it tries to balance the pendulum. Instead I get a flat curve with no high frequency component.

 

[pbuk]

Have you drawn a diagram showing all the forces/torques?

Have you made a physical model of similar dimensions? How well does a motor of similar size work with a pendulum that heavy?

 

[Remusco]

Have you drawn a diagram showing all the forces/torques?

Have you made a physical model of similar dimensions? How well does a motor of similar size work with a pendulum that heavy?

 

[Remusco]

Have you drawn a diagram showing all the forces/torques?

Have you made a physical model of similar dimensions? How well does a motor of similar size work with a pendulum that heavy?

Do you think my calculation for the inertia seen by the motor shaft is correct? Isn't this just J_p or should this be J_p + m_p*r_a**2?

 

[pbuk]

Do you think my calculation for the inertia seen by the motor shaft is correct?

So you haven't worked through the equations of motion in class, or as an assignment that has already been verified? I'm guessing not because the analyses of this set-up I have seen use the angles the other way round: $\theta$ for the motor arm and $\alpha$ for the inverted pendulum.

What course are you studying and at what level? In particular are you comfortable with Lagrangian mechanics? It might be possible to tackle this with Newtonian mechanics, but you will have to reinvent a few wheels (pun intended).

Were you given the physical parameters? In particular the mass of 0.5 kg looks a bit challenging to me and will exacerbate the non-linearity of the system (once that baby starts to topple, it's going to go down hard). Have you looked at any devices that are on the market for study? If you have difficulty with this, add the word "Quanser" to your search. You should also find some materials that give you the (Langrangian) equations of motion.

 

[Filip Larsen]

The thing that I'm most confused about is that the motor arm does not oscillate with the pendulum.

For what its worth, if I try run (using python-fiddle.com) the code pbuk has been so nice to put in code tag, I get the plot below which does not look like your plot (but perhaps you or pbuk has tinkered with the parameters since your plot was made). A quick test running with half dt seems to yield same plot so its likely this particular solution (if it is correct) is not outside the stability region for the Euler method as applied to your field.

 

[Remusco]

I need help calculating inertias for a rotary inverted pendulum system (Furuta pendulum). This is for a school project.
The pendulum is a unform rod, and the motor arm is a unform rod.
The equations of motion are:

Where theta is the angle of the pendulum (0 degrees is in vertical position). alpha is the angle of the motor arm.

The question is, what do I use for J in these equations?

chatGPT gives me the following for the inertias, which I doubt are correct. I'm totally stuck on how to derive these myself.

I feel J_eff should be dependent on the angular velocity of the pendulum, since I can see torque being applied to the arm if the pendulum is rotating very quickly.

 

[Remusco]

So you haven't worked through the equations of motion in class, or as an assignment that has already been verified? I'm guessing not because the analyses of this set-up I have seen use the angles the other way round: $\theta$ for the motor arm and $\alpha$ for the inverted pendulum.

What course are you studying and at what level? In particular are you comfortable with Lagrangian mechanics? It might be possible to tackle this with Newtonian mechanics, but you will have to reinvent a few wheels (pun intended).

Were you given the physical parameters? In particular the mass of 0.5 kg looks a bit challenging to me and will exacerbate the non-linearity of the system (once that baby starts to topple, it's going to go down hard). Have you looked at any devices that are on the market for study? If you have difficulty with this, add the word "Quanser" to your search. You should also find some materials that give you the (Langrangian) equations of motion.

 

[Remusco]

I am getting really good results now with this code:

import numpy as np
import matplotlib.pyplot as plt

# System Parameters
m_p = 0.1   # Mass of pendulum (kg)
m_c = 0.5   # Mass of arm (kg)
l_p = 0.2   # Length of pendulum (m)
l_a = 0.15  # Length of arm (m)
g = 9.81    # Gravity (m/s^2)
b = 0.00    # Damping coefficient
I_a = (1/3) * m_c * l_a**2  # Arm moment of inertia
I_p = (1/3) * m_p * l_p**2  # Pendulum moment of inertia

# Motor Parameters
V_supply = 28  # Motor supply voltage (V)
R_motor = 1.59  # Motor resistance (Ohms)
k_e = .0384  # Back-EMF constant (V/(rad/s))
k_t = .0384  # Motor torque constant (Nm/A)

# Setpoints
arm_setpoint = np.radians(20)  # Desired arm angle (rad)
pendulum_setpoint = np.radians(0)  # Desired pendulum angle (rad)

# Control switch (turn control on or off)
control_switch = 1

if control_switch == 1:
    Kp_arm, Ki_arm, Kd_arm = -20, 0, -30
    Kp_pendulum, Ki_pendulum, Kd_pendulum = 500, 0, 20
else:
    Kp_arm, Ki_arm, Kd_arm = 0, 0, 0
    Kp_pendulum, Ki_pendulum, Kd_pendulum = 0, 0, 0

# Force switch (to turn disturbance forces on or off)
force_switch = 0

# Define times for disturbances
disturbance_times = [3, 6, 9, 12]  # Apply disturbances at these seconds
disturbance_torque = 0.2*force_switch  # Small torque perturbation

# Time settings
dt = 0.05  # Time step (s)
t_max = 15  # Max time (s)
n_steps = int(t_max / dt)  # Number of steps

# State variables
phi = np.zeros(n_steps)
phi_dot = np.zeros(n_steps)
theta = np.zeros(n_steps)
theta_dot = np.zeros(n_steps)
control_signal = np.zeros(n_steps)

# Initial conditions
phi[0] = 0
phi_dot[0] = 0
theta[0] = np.radians(3)
theta_dot[0] = 0

# PID Controller states
integral_arm = 0
prev_error_arm = 0
integral_pendulum = 0
prev_error_pendulum = 0

# Simulation loop
for i in range(1, n_steps):
    # Compute errors
    error_arm = arm_setpoint - phi[i - 1]
    error_pendulum = pendulum_setpoint - theta[i - 1]

    # PID calculations
    integral_arm += error_arm * dt
    derivative_arm = (error_arm - prev_error_arm) / dt
    arm_torque = Kp_arm * error_arm + Ki_arm * integral_arm + Kd_arm * derivative_arm
    prev_error_arm = error_arm

    integral_pendulum += error_pendulum * dt
    derivative_pendulum = (error_pendulum - prev_error_pendulum) / dt
    pendulum_torque = Kp_pendulum * error_pendulum + Ki_pendulum * integral_pendulum + Kd_pendulum * derivative_pendulum
    prev_error_pendulum = error_pendulum

    # Total control signal
    control_signal[i] = np.clip(arm_torque + pendulum_torque, -255, 255)

    # Motor calculations
    V_motor = (control_signal[i] / 255) * V_supply
    I_motor = (V_motor - k_e * phi_dot[i - 1]) / R_motor
    tau_m = k_t * I_motor
    tau_p = (tau_m * l_p) / l_a

    # Update the system inertia
    I_total = I_a + (I_p + m_p * l_p ** 2) * (np.cos(theta[i - 1])) ** 2


    # Apply a switch to tau for the lower two quadrants
    if theta[i - 1] <= np.radians(-90) or theta[i - 1] >= np.radians(90):
        switch = -1
    else:
        switch = 1

    # **Apply Disturbance Torque at Specified Times**
    if any(abs(i * dt - t) < dt / 2 for t in disturbance_times):
        disturbance = disturbance_torque * np.sign(np.random.randn())  # Random direction
    else:
        disturbance = 0

    # Equations of motion (Euler integration)
    theta_ddot = (switch * tau_p - m_p * g * l_p * 0.5 * np.sin(theta[i - 1] + np.pi) - b * theta_dot[i - 1] + disturbance) / (I_p + m_p * l_p ** 2)
    phi_ddot = (tau_m - b * phi_dot[i - 1]) / (I_total)

    theta_dot[i] = theta_dot[i - 1] + theta_ddot * dt
    theta[i] = theta[i - 1] + theta_dot[i] * dt

    phi_dot[i] = phi_dot[i - 1] + phi_ddot * dt
    phi[i] = phi[i - 1] + phi_dot[i] * dt

# Time array
time = np.linspace(0, t_max, n_steps)

# Plot results
plt.figure(figsize=(12, 7))

# Arm Angle Plot
plt.subplot(3, 1, 1)
plt.plot(time, phi * 180 / np.pi, label='Arm Angle (deg)')
plt.axhline(arm_setpoint * 180 / np.pi, color='r', linestyle='--', label=f'Target ({arm_setpoint * 180 / np.pi:.1f}°)')
plt.xlabel('Time (s)')
plt.ylabel('Arm Angle (deg)')
plt.title('Arm Angle Over Time')
plt.legend()
plt.grid()

# Pendulum Angle Plot
plt.subplot(3, 1, 2)
plt.plot(time, theta * 180 / np.pi, label='Pendulum Angle (deg)')
plt.axhline(pendulum_setpoint * 180 / np.pi, color='r', linestyle='--', label=f'Target ({pendulum_setpoint * 180 / np.pi:.1f}°)')
plt.xlabel('Time (s)')
plt.ylabel('Pendulum Angle (deg)')
plt.title('Pendulum Angle Over Time')
plt.legend()
plt.grid()

if force_switch==1:
  # Mark Disturbance Times
  for t in disturbance_times:
      plt.axvline(x=t, color='k', linestyle='--', label='Disturbance')

# Control Signal Plot
plt.subplot(3, 1, 3)
plt.plot(time, control_signal, label='Control Signal (PWM)', color='purple')
plt.axhline(0, color='r', linestyle='--', label='Zero Control')
plt.xlabel('Time (s)')
plt.ylabel('Control Signal (PWM)')
plt.title('Control Signal Over Time')
plt.legend()
plt.grid()

plt.tight_layout()
plt.show()

The only thing I'm not sure about at this point are the inertias. Are these correct? Can someone derive these? I'm using the chatGPT result for these and I'm not 100% sure they are correct.

 

[haruspex]

It is impossible to answer without a clear definition of your variables, but I can say that a moment of inertia is all about mass distribution and, given that, never depends on the velocity.
Above response, merged from an earlier thread, is no longer relevant.

 

[Remusco]

It is impossible to answer without a clear definition of your variables, but I can say that a moment of inertia is all about mass distribution and, given that, never depends on the velocity.

# System Parameters
m_p = 0.1 # Mass of pendulum (kg)
m_a = 0.5 # Mass of arm (kg)
l_p = 0.2 # Length of pendulum (m)
l_a = 0.15 # Length of arm (m)
g = 9.81 # Gravity (m/s^2)
b = 0.00 # Damping coefficient
I_a = (1/3) * m_a * l_a**2 # Arm moment of inertia
I_p = (1/3) * m_p * l_p**2 # Pendulum moment of inertia

 

[pbuk]

I am getting really good results now with this code:

This code is totally different from the previous code (and is still not posted as Python code with appropriate syntax highlighting). Are you perhaps using AI to generate code and asking us to check it for you?

 

[Remusco]

This code is totally different from the previous code (and is still not posted as Python code with appropriate syntax highlighting). Are you perhaps using AI to generate code and asking us to check it for you?

No, I wrote all of this myself. The only thing I'm not sure about are the inertias.