PD Cont. Actuator Sandbox
2025-05-09
PD Controller. Using RK4 integrator.
Work in progress.
Code snippet:
// RK4 integrator
//Apply an external torque of 0.3Nm and using a moment of inertia of 0.2 kg*m^2
function rk4(t, state, dt, deriv) {
const k1 = deriv(t, state);
const k2 = deriv(t + dt/2, state.map((s, i) => s + dt * k1[i] / 2));
const k3 = deriv(t + dt/2, state.map((s, i) => s + dt * k2[i] / 2));
const k4 = deriv(t + dt, state.map((s, i) => s + dt * k3[i]));
return state.map((s, i) =>
s + dt * (k1[i] + 2*k2[i] + 2*k3[i] + k4[i]) / 6
);
}Interactive Simulation:
Experiment One
Experiment Two
Experiment Three
Experiment Four
For a better simulator of PD controller behavior, use Scipy's integrator.
Here is an example snippet that I used to sanity check PD controller behavior away from the actuator or MuJoCo simulation.
def system_dynamics(t, y):
# y[0] = position [rad], y[1] = velocity [rad/s]
position_rad, velocity_rad_s = y
position_error_rad = const_target_position_rad - position_rad # [rad]
velocity_error_rad_s = const_target_velocity_rad_s - velocity_rad_s # [rad/s]
#* Model MIT controller as used in Robstride actuators
control_input_Nm = (
kp * position_error_rad # [Nm/rad] * [rad] = [Nm]
+ kd * velocity_error_rad_s # [Nm/(rad/s)] * [rad/s] = [Nm]
+ const_target_torque_Nm # [Nm]
)
#* Estimate 0.5 Nm from passive params of motor (damping and coulomb friction)
#* I see avg 0.3 Nm when rotor by itself is holding position (it fluctuates).
global external_load_Nm
global passive_load_Nm
#* this is 2.5lb mass 0.14m away from center
inertia_kgm2 = 1.13398 * (0.140316**2) # [kg·m²]
# Acceleration with external load [rad/s²]
acceleration_rad_s2 = (control_input_Nm - (external_load_Nm + passive_load_Nm)) / inertia_kgm2
return [velocity_rad_s, acceleration_rad_s2]
Full Python Snippet
import numpy as np
import matplotlib.pyplot as plt
from scipy.integrate import solve_ivp
# Define the system parameters
kp = 300 # Proportional gain [Nm/rad]
kd = 24.722 # Derivative gain [Nm/(rad/s)]
external_load_Nm = 0.0# 2.976 # [Nm] *8kg 0.372m away from center
passive_load_Nm = 0.3 # [Nm]
# Target command variables
const_target_position_rad = np.radians(10.0) # [radians] (from 10 degrees)
const_target_velocity_deg_s = 0.0 # [degrees/s]
const_target_velocity_rad_s = np.radians(const_target_velocity_deg_s) # [radians/s]
const_target_torque_Nm = 0.0 # [Nm]
def system_dynamics(t, y):
# y[0] = position [rad], y[1] = velocity [rad/s]
position_rad, velocity_rad_s = y
position_error_rad = const_target_position_rad - position_rad # [rad]
velocity_error_rad_s = const_target_velocity_rad_s - velocity_rad_s # [rad/s]
#* Model MIT controller as used in Robstride actuators
control_input_Nm = (
kp * position_error_rad # [Nm/rad] * [rad] = [Nm]
+ kd * velocity_error_rad_s # [Nm/(rad/s)] * [rad/s] = [Nm]
+ const_target_torque_Nm # [Nm]
)
#* Estimate 0.5 Nm from passive params of motor (damping and coulomb friction)
#* I see avg 0.3 Nm when rotor by itself is holding position (it fluctuates).
global external_load_Nm
global passive_load_Nm
#* this is 2.5lb mass 0.14m away from center
inertia_kgm2 = 1.13398 * (0.140316**2) # [kg·m²]
# Acceleration with external load [rad/s²]
acceleration_rad_s2 = (control_input_Nm - (external_load_Nm + passive_load_Nm)) / inertia_kgm2
return [velocity_rad_s, acceleration_rad_s2]
# Time span for the simulation [s]
t_span = (0, 6) # [s]
# Initial conditions [position [rad], velocity [rad/s]]
y0 = [0, 0]
# Solve the differential equation
solution = solve_ivp(system_dynamics, t_span, y0, method='RK45',
t_eval=np.linspace(t_span[0], t_span[1], 1000))
time_s = solution.t # [s]
position_rad = solution.y[0] # [rad]
velocity_rad_s = solution.y[1] # [rad/s]
# Get final position value
final_position_rad = position_rad[-1] # [rad]
# Calculate reference signals
position_reference_rad = np.ones_like(time_s) * const_target_position_rad # [rad]
velocity_reference_rad_s = np.ones_like(time_s) * const_target_velocity_rad_s # [rad/s]
# Calculate errors
position_error_rad = position_reference_rad - position_rad # [rad]
velocity_error_rad_s = velocity_reference_rad_s - velocity_rad_s # [rad/s]
# Convert all angular values to degrees for plotting
position_deg = np.degrees(position_rad)
position_reference_deg = np.degrees(position_reference_rad)
final_position_deg = np.degrees(final_position_rad)
velocity_deg_s = np.degrees(velocity_rad_s)
velocity_reference_deg_s = np.degrees(velocity_reference_rad_s)
target_position_deg = np.degrees(const_target_position_rad)
target_velocity_deg_s = np.degrees(const_target_velocity_rad_s)
# Plot the results
plt.figure(figsize=(12, 12))
# Position plot
plt.subplot(3, 1, 1)
plt.plot(time_s, position_deg, 'b-', label='Position')
plt.plot(time_s, position_reference_deg, 'r--', label=f'Reference ({target_position_deg:.1f}°)')
plt.axhline(y=final_position_deg, color='g', linestyle=':', label=f'Final Position ({final_position_deg:.1f}°)')
plt.grid(True)
plt.legend()
plot_title = (f'PD Controller Response (Kp={kp} Nm/rad, Kd={kd} Nm/(rad/s))
'
f'Cmd_Pos={target_position_deg:.1f}°, Cmd_Vel={target_velocity_deg_s:.1f}°/s, Cmd_Torque={const_target_torque_Nm} Nm
'
f'External+Passive Load={external_load_Nm + passive_load_Nm} Nm')
plt.title(plot_title)
plt.xlabel('Time (s)')
plt.ylabel('Position (degrees)')
# Velocity plot
plt.subplot(3, 1, 2)
plt.plot(time_s, velocity_deg_s, 'g-', label='Velocity')
plt.plot(time_s, velocity_reference_deg_s, 'r--', label=f'Reference ({target_velocity_deg_s:.1f}°/s)')
plt.grid(True)
plt.legend()
plt.xlabel('Time (s)')
plt.ylabel('Velocity (degrees/s)')
plt.tight_layout()
plt.savefig('pd_controller_response.png')
plt.show()
Example Plots from PD Controller Simulation:
