# Import Libraries:
import pydrake.symbolic
from pydrake.symbolic import Variable
from pydrake.systems.primitives import SymbolicVectorSystem
# Parameters:
g = 9.81
L = 1
# Define Symbolic Variables: States = [theta, dtheta]
th = Variable("th")
dth = Variable("dth")
# States and State-space Form:
states = [th, dth]
odes = [dth, -g / L * pydrake.symbolic.sin(th)]
# Define the System:
continuous_vector_system = SymbolicVectorSystem(state=states, dynamics=odes, output=states)
import numpy as np
from pydrake.systems.analysis import Simulator
from pydrake.systems.framework import DiagramBuilder
from pydrake.systems.primitives import LogVectorOutput
# Create Block Diagram containing our system:
builder = DiagramBuilder()
system = builder.AddSystem(continuous_vector_system)
logger = LogVectorOutput(system.get_output_port(0), builder)
diagram = builder.Build()
# Set Initial Conditions: [theta(0) = pi / 4, dtheta(0) = 0]
context = diagram.CreateDefaultContext()
context.SetContinuousState([np.pi / 4.0, 0.0])
# Create Simulator and simulate for 10 seconds:
simulator = Simulator(diagram, context)
simulator.AdvanceTo(10)
# Grab results from Logger:
log = logger.FindLog(context)
time = log.sample_times()
data = log.data().transpose()
theta = data[:, 0]
dtheta = data[:, 1]
# Find X and Y states for animation:
simulation_size = len(time)
# Initialize and Solve Animation States:
x = np.zeros(simulation_size)
y = np.zeros(simulation_size)
for i in range(0, simulation_size):
x[i] = L * np.sin(theta[i])
y[i] = -L * np.cos(theta[i])
# Find the average timestep of the simulation:
timesteps = []
average_timestep = 0
for i in range(simulation_size - 1):
timestep = abs(time[i] - time[i+1])
timesteps.append(timestep)
average_timestep = sum(timesteps)/len(timesteps)
import os
import matplotlib.pyplot as plt
from matplotlib.animation import FFMpegWriter
from matplotlib.patches import Circle
# Animate Results:
fig, ax = plt.subplots()
p, = ax.plot([],[], color='cornflowerblue') # initializes an empty plot
ax.axis('equal')
ax.set_xlim([-3, 3]) # X Lim
ax.set_ylim([-3, 3]) # Y Lim
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_title('Drake: Pendulum Simulation')
video_title = "pendulum"
# Patch:
c = Circle((0, 0), radius=0.1, color='cornflowerblue')
ax.add_patch(c)
# Animation Parameters
FPS = 20
dt = average_timestep
sample_rate = int(1 / (FPS*dt))
dpi = 300 # Quality of the Video
writerObj = FFMpegWriter(fps=FPS)
video_path_1 = "/work/" + video_title + ".mp4"
# Plot and Create Animation
with writerObj.saving(fig, video_path_1, dpi):
for i in range(0, simulation_size, sample_rate):
# Update Plot:
x_data_points = [0, x[i]]
y_data_points = [0, y[i]]
p.set_data(x_data_points, y_data_points)
# Update Patch:
patch_center = x[i], y[i]
c.center = patch_center
# Update Drawing
fig.canvas.draw()
# Save Frame:
writerObj.grab_frame()
from IPython.display import Video
Video(video_path_1, embed=True, width=640, height=480)
from pydrake.systems.controllers import PidController
# Redefine Pendulum System but with an Input:
# Parameters:
g = 9.81
L = 1.0
m = 1.0
# Define Symbolic Variables: States = [theta, dtheta]
th = Variable("th")
dth = Variable("dth")
# Input Variable: Input = [u]
u = Variable("u")
# States:
states = [th, dth]
odes = [dth, -g / L * pydrake.symbolic.sin(th) + u / (m * L ** 2)]
# Define the System:
pendulum_system = SymbolicVectorSystem(state=states, input=[u], dynamics=odes, output=states)
# Creat Diagram Builder:
builder = DiagramBuilder()
# Add Pendulum System to Diagram:
pendulum = builder.AddSystem(pendulum_system)
pendulum.set_name("pendulum")
# Add PID Controller to Diagram:
controller = builder.AddSystem(PidController(kp=[10.0], ki=[1.0], kd=[1.0]))
controller.set_name("controller")
# Connect the Controller to the Plant:
builder.Connect(pendulum.get_output_port(), controller.get_input_port_estimated_state())
builder.Connect(controller.get_output_port_control(), pendulum.get_input_port())
# Make the Desired State Input of the Controller an Input to the Diagram:
builder.ExportInput(controller.get_input_port_desired_state())
# Log the States of the Pendulum:
logger = LogVectorOutput(pendulum.get_output_port(0), builder)
logger.set_name("logger")
diagram = builder.Build()
diagram.set_name("diagram")
# Set Simulator to Run Diagram:
simulator = Simulator(diagram)
context = simulator.get_mutable_context()
# Desired Pendulum Angle:
desired_angle = np.pi / 2.0
# First Extract the subsystem context for the pendulum:
pendulum_context = diagram.GetMutableSubsystemContext(pendulum, context)
# Set Pendulum State: [theta, dtheta]:
pendulum_context.get_mutable_continuous_state_vector().SetFromVector([0.0, 0.0])
# Set Desired State to Diagram Input Port:
diagram.get_input_port(0).FixValue(context, [desired_angle, 0.])
# Clear the Logger (so we can re-run this cell if we want)
logger.FindMutableLog(context).Clear()
# Simulate for 20 seconds.
simulator.AdvanceTo(20);
# Grab results from Logger:
log = logger.FindLog(simulator.get_context())
time = log.sample_times()
data = log.data().transpose()
theta = data[:, 0]
dtheta = data[:, 1]
# Plot the Results:
plt.figure()
# Plot theta.
plt.plot(time, theta,'.-')
# Draw a line for the desired angle:
plt.plot([time[0], time[-1]], [desired_angle, desired_angle], 'g' )
plt.xlabel('time (seconds)')
plt.ylabel('theta (rad)')
plt.title('PID Control of the Pendulum')
# Find X and Y states for animation:
simulation_size = len(time)
# Initialize and Solve Animation States:
x = np.zeros(simulation_size)
y = np.zeros(simulation_size)
for i in range(0, simulation_size):
x[i] = L * np.sin(theta[i])
y[i] = -L * np.cos(theta[i])
# Find the average timestep of the simulation:
timesteps = []
average_timestep = 0
for i in range(simulation_size - 1):
timestep = abs(time[i] - time[i+1])
timesteps.append(timestep)
average_timestep = sum(timesteps)/len(timesteps)
# Animate Results:
fig, ax = plt.subplots()
p, = ax.plot([],[], color='cornflowerblue') # initializes an empty plot
ax.axis('equal')
ax.set_xlim([-3, 3]) # X Lim
ax.set_ylim([-3, 3]) # Y Lim
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_title('Drake: Pendulum Simulation')
video_title = "pendulum_pid"
# Patch:
c = Circle((0, 0), radius=0.1, color='cornflowerblue')
ax.add_patch(c)
# Animation Parameters
FPS = 20
dt = average_timestep
sample_rate = int(1 / (FPS*dt))
dpi = 300 # Quality of the Video
writerObj = FFMpegWriter(fps=FPS)
video_path_2 = "/work/" + video_title + ".mp4"
# Plot and Create Animation
with writerObj.saving(fig, video_path_2, dpi):
for i in range(0, simulation_size, sample_rate):
# Update Plot:
x_data_points = [0, x[i]]
y_data_points = [0, y[i]]
p.set_data(x_data_points, y_data_points)
# Update Patch:
patch_center = x[i], y[i]
c.center = patch_center
# Update Drawing
fig.canvas.draw()
# Save Frame:
writerObj.grab_frame()
from IPython.display import Video
Video(video_path_2, embed=True, width=640, height=480)