def make_noisy_robot():
t = Variable("t") # time
# state of the robot (in cartesian coordinates)
z1 = Variable("z1") # ball angle (theta)
z2 = Variable("z2") # body angle
z3 = Variable("z3") # ball angular velocity
z4 = Variable("z4") # body angular velocity
cartesian_state = [z1, z2, z3, z4]
# control input of the robot
u1 = Variable("u1") # wheel torque
ctrl_input = [u1]
# nonlinear dynamics, the whole state is measured (output = state)
mb = 1 # mass of the ball
mB = 1 # mass of the body
rb = 1 # radius of the ball
l = 1 # height of the center of gravity
Ib = 1 # inertia of the ball
IB = 1 # inertia of the body
g = 1.5 # gravitational acceleration
theta = z1
theta_dot = z3
phi = z1 - z2
phi_dot = z3 - z4
gamma1 = Ib + IB + mb*rb**2 + mB*rb**2 + mB*l**2
gamma2 = mB*l**2 + IB
m1 = gamma1 + 2*mB*rb*l*cos(theta + phi)
m2 = gamma2 + mB*rb*l*cos(theta + phi)
m3 = gamma2 + mB*rb*l*cos(theta + phi)
m4 = gamma2
Minv = np.array([[m4, -m2],[-m3, m1]])/(m1*m4 - m2*m3)
C = np.array([[-mB*rb*l*sin(theta + phi)*(theta_dot + phi_dot)**2], [0]])
G = np.array([[-mB*g*l*sin(theta+phi)], [-mB*g*l*sin(theta+phi)]])
B = np.array([[0],[u1]])
D = np.array([[1.5*theta_dot], [phi_dot]])
qddot = (Minv*(B-C-G-D)).T[0]
theta_ddot, phi_ddot = qddot
z5 = theta_ddot
z6 = theta_ddot - phi_ddot
dynamics = np.array([z3, z4, z5, z6])
# dynamics = [u1*cos(z3), u1*sin(z3), u2]
robot = SymbolicVectorSystem(
state=cartesian_state,
input=ctrl_input,
output=cartesian_state,
dynamics=dynamics,
)
return robot
def create_symbolic_dynamics():
# state of the robot (in cartesian coordinates)
x1 = Variable("x1")
x2 = Variable("x2")
x3 = Variable("x3")
cartesian_state = [x1, x2, x3]
u1 = Variable("u1") # angular velocity
input = [u1]
dynamics = [cos(x3), sin(x3), u1]
uav = SymbolicVectorSystem(
state=cartesian_state,
input=input,
output=cartesian_state,
dynamics=dynamics,
)
return uav
]
def pendulum_with_motor_dynamics(x, u, p):
q = x[0]
qdot = x[1]
tau = u[0]
arm_intertia = (-p["m"] * p["g"] * p["l"] * sin(q) + tau) / (p["m"] *
p["l"]**2)
# Note: should tau actually be tau_motor? Which one do we get as input?
q_dd_motor = (p["g"] / p["N"] + tau) / (p["I"] + arm_intertia / p["N"]**2)
q_dd = q_dd_motor / p["N"]
return [qdot, q_dd]
x = [Variable("theta"), Variable("thetadot")]
u = [Variable("tau")]
# Example parameters of pendulum dynamics
p = {"m": 1.0, "g": 9.81, "l": 0.5}
system = SymbolicVectorSystem(state=x,
output=x,
input=u,
dynamics=pendulum_dynamics(x, u, p))
context = system.CreateDefaultContext()
print(context)
import matplotlib as mp
mp.use("Qt5Agg") # apt install python3-pyqt5
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.ticker import StrMethodFormatter
from pydrake.all import (DiagramBuilder, LogOutput, Simulator,
SymbolicVectorSystem, Variable)
builder = DiagramBuilder()
x = Variable("x")
plant = builder.AddSystem(
SymbolicVectorSystem(state=[x],
dynamics=[4 * x * (1 - x)],
output=[x],
time_period=1))
log = LogOutput(plant.get_output_port(0), builder)
diagram = builder.Build()
simulator = Simulator(diagram)
context = simulator.get_mutable_context()
random_state = np.random.RandomState() # see drake #12632.
fig, ax = plt.subplots(2, 1)
for i in range(2):
context.SetTime(0)
context.SetDiscreteState([random_state.uniform()])
log.reset()
simulator.Initialize()
print(H)
print('P(Lyapunov)=')
print(P)
assert isPositiveDefinite(-H), "Vdot is not negative definite at the fixed point."
#V = FixedLyapunovMaximizeLevelSet(prog, x, V, Vdot)
V = FixedLyapunovSearchRho(prog, x, V, Vdot)
# Put V back into global coordinates
V = V.Substitute(dict(zip(x,x-x0)))
return V
from pydrake.all import SymbolicVectorSystem, Variable, plot_sublevelset_expression
# Time-reversed van der Pol oscillator
mu = 1;
q = Variable('q')
qdot = Variable('qdot')
#vdp = SymbolicVectorSystem(state=[q,qdot], dynamics=[-qdot, mu*(q*q - 1)*qdot + q]) # orig
q0 = math.pi
#vdp = SymbolicVectorSystem(state=[q,qdot], dynamics=[qdot, -10.0*(q-q0-((q-q0)**3)/6.0+((q-q0)**5)/120.0) - 0.1*qdot]) # pendulum
vdp = SymbolicVectorSystem(state=[q,qdot], dynamics=[qdot, -10.0*(q-((q)**3)/6.0+((q)**5)/120.0) - 0.1*qdot]) # pendulum
context = vdp.CreateDefaultContext()
context.SetContinuousState([q0, 0])
V = RegionOfAttraction(vdp, context)
#import pdb; pdb.set_trace()
plot_sublevelset_expression(ax, V)
plt.show()
# state of the robot (in cartesian coordinates)
z1 = Variable("z1") # horizontal position
z2 = Variable("z2") # vertical position
z3 = Variable("z3") # angular position
cartesian_state = [z1, z2, z3]
# control input of the robot
u1 = Variable("u1") # linear velocity
u2 = Variable("u2") # angular velocity
input = [u1, u2]
# nonlinear dynamics, the whole state is measured (output = state)
dynamics = [u1*cos(z3), u1*sin(z3), u2]
robot = SymbolicVectorSystem(
state=cartesian_state,
input=input,
output=cartesian_state,
dynamics=dynamics,
)
"""## Write the Control Law
It is time to write the controller.
To do that we construct a `VectorSystem` with a method called `DoCalcVectorOutput`.
In the latter, we write all the steps needed to compute the instantaneous control action from the vehicle state.
"""
# you will write the control law in it in the next cell
# by defining the function called "lyapunov_controller"
class ParkingController(VectorSystem):
def __init__(self, lyapunov_controller):
# 3 inputs (robot state)