# python libraries
import numpy as np
import matplotlib.pyplot as plt, mpld3
import pydot
from IPython.display import HTML, SVG, display
# pydrake imports
from pydrake.all import (DiagramBuilder, VectorLogSink, Variable, PidController,
Simulator, SymbolicVectorSystem, ConstantVectorSource, sin)
from manipulation import running_as_notebook
# enable mpld3 notebook
if running_as_notebook:
mpld3.enable_notebook()
def pendulum_dynamics(x, u, p):
q = x[0]
qdot = x[1]
tau = u[0]
return [
qdot,
((-p["m"] * p["g"] * p["l"] * sin(q) + tau) / (p["m"] * p["l"] ** 2))
]
# Symbolic Variables from pydrake.symbolic
x = [Variable("theta"), Variable("thetadot")]
u = [Variable("tau")]
# Example parameters of pendulum dynamics
p = {"m": 1.0, # kg
"g": 9.81, # m / s^2
"l": 0.5 # m
}
# Declaration of a symbolic vector system
system = SymbolicVectorSystem(state = x,
output = x,
input = u,
dynamics = pendulum_dynamics(x, u, p))
context = system.CreateDefaultContext()
print(context)
::_ Context
------------
Time: 0
States:
2 continuous states
0 0
# Modify this function. you may not change the function name, inputs, or size of the output.
def pendulum_with_motor_dynamics(x, u, p):
q = x[0]
qdot = x[1]
tau = u[0]
return [
0., # modify here
0. # modify here
]
from manipulation.exercises.robot.test_reflected_inertia import TestSimplePendulumWithGearbox
from manipulation.exercises.grader import Grader
Grader.grade_output([TestSimplePendulumWithGearbox], [locals()], 'results.json')
Grader.print_test_results('results.json')
Total score is 0/5.
Score for Test state space dynamics is 0/5.
- Test Failed: 41.52907242353008 not less than or equal to 1e-06 : The pendulum with motor dynamics are not corret.
# Add motor and gearbox parameters
p = {"N": 160,
"I": 3.46e-4,
"m": 1.0, # kg
"g": 9.81, # m / s^2
"l": 0.5 # m
}
def BuildAndSimulate(q_d, pendulum_params, gains, visualize=False):
# This defines the plant using the pendulum with motor dynamics.
system = SymbolicVectorSystem(state = x,
output = x,
input = u,
dynamics = pendulum_with_motor_dynamics(x, u, pendulum_params))
kp, ki, kd = gains
builder = DiagramBuilder()
# Add all the systems into the diagram
pendulum = builder.AddSystem(system)
logger = builder.AddSystem(VectorLogSink(2))
pid_controller = builder.AddSystem(PidController([kp], [ki], [kd]))
desired_state = builder.AddSystem(ConstantVectorSource([q_d, 0.]))
# Connect the IO ports of the systems.
builder.Connect(pid_controller.get_output_port(0),
system.get_input_port(0))
builder.Connect(system.get_output_port(0),
logger.get_input_port(0))
builder.Connect(system.get_output_port(0),
pid_controller.get_input_port(0))
builder.Connect(desired_state.get_output_port(0),
pid_controller.get_input_port(1))
diagram = builder.Build()
diagram.set_name("diagram")
# Plot the diagram if visualize is true.
if(visualize): (
display(SVG(pydot.graph_from_dot_data(
diagram.GetGraphvizString(max_depth=1))[0].create_svg()))
)
simulator = Simulator(diagram)
context = simulator.get_mutable_context()
# The simulator has three states since the plant has 2, and the PID controller
# keeps an additional state for the integral term. We set all of them to zero
# initialization.
context.SetContinuousState([0., 0., 0.])
return simulator, logger.FindLog(context)
gains = [5, 2, 1]
simulator, logger = BuildAndSimulate(0., p, gains, visualize = True)
q_d = (5./8.) * np.pi # Feel free to play around with different final positions.
gains = [5, 2, 1] # [P, I, D] gains.
p["N"] = 1
simulator, logger = BuildAndSimulate(q_d, p, gains)
simulator.Initialize()
simulator.AdvanceTo(20.0)
time = logger.sample_times()
traj = logger.data()
plt.figure()
plt.plot(time, traj[0,:], 'b-')
plt.plot(time, q_d * np.ones(traj.shape[1]), 'r--')
plt.xlabel('time (s)')
plt.ylabel('q (rads)')
mpld3.display()
plt.figure()
p["N"] = 1
q_d_lst = np.linspace(-np.pi, np.pi, 10)
for i in range(len(q_d_lst)):
simulator, logger = BuildAndSimulate(q_d_lst[i], p, gains)
simulator.Initialize()
simulator.AdvanceTo(20.0)
time = logger.sample_times()
traj = logger.data()
plt.plot(time, traj[0,:], 'b--')
plt.plot(time, q_d_lst[i] * np.ones(traj.shape[1]), 'r--')
plt.xlabel('time (s)')
plt.ylabel('q (rads)')
mpld3.display()
q_d = np.pi/1 # Feel free to play around with different gains.
gains = [5, 2, 1] # [P, I, D] gains
p["N"] = 160
simulator, logger = BuildAndSimulate(q_d, p, gains)
simulator.Initialize()
simulator.AdvanceTo(20.0)
time = logger.sample_times()
traj = logger.data()
plt.figure()
plt.plot(time, traj[0,:], 'b-')
plt.plot(time, q_d * np.ones(traj.shape[1]), 'r--')
plt.xlabel('time (s)')
plt.ylabel('q (rads)')
mpld3.display()
plt.figure()
p["N"] = 160
q_d_lst = np.linspace(-np.pi, np.pi, 10)
for i in range(len(q_d_lst)):
simulator, logger = BuildAndSimulate(q_d_lst[i], p, gains)
simulator.Initialize()
simulator.AdvanceTo(20.0)
time = logger.sample_times()
traj = logger.data()
plt.plot(time, traj[0,:], 'b--')
plt.plot(time, q_d_lst[i] * np.ones(traj.shape[1]), 'r--')
plt.xlabel('time (s)')
plt.ylabel('q (rads)')
mpld3.display()