def calculate_lqr(self):
upright_plant = AcrobotPlant()
upright_plant_context = upright_plant.CreateDefaultContext()
upright_plant_context.get_mutable_continuous_state() \
.SetFromVector(np.array([np.pi, 0, 0, 0]))
upright_plant.GetInputPort("elbow_torque") \
.FixValue(upright_plant_context, 0)
lqr = LinearQuadraticRegulator(upright_plant, upright_plant_context,
self.Q, self.R)
return lqr.D()
def __init__(self,
quadrotor_plant,
xf=np.array(
[0., 0., 0., math.pi, math.pi, 0., 0., 0., 0., 0.])):
self.plant = quadrotor_plant
self.mb = self.plant.mb
self.lb = self.plant.lb
self.Ib = self.plant.Ib
self.m1 = self.plant.m1
self.l1 = self.plant.l1
self.I1 = self.plant.I1
self.m1 = self.plant.m2
self.l1 = self.plant.l2
self.I1 = self.plant.I2
self.g = self.plant.g
self.input_max = self.plant.input_max
self.uf = np.array([15., 15.])
self.xf = xf
A, B = quadrotor_plant.GetLinearizedDynamics(self.uf, self.xf)
self.Q = np.diag([1., 1., 2.1, 2., 2., 1., 1., 2., 2., 2.])
self.R = np.diag([1., 1.]) * .5
from pydrake.all import LinearQuadraticRegulator
self.K, self.S = LinearQuadraticRegulator(A, B, self.Q, self.R)
def __init__(self, quadrotor_plant,
uf = np.array([10., 10.]),
xf = np.array([0., 0., 0., math.pi, 0., 0., 0., 0.]) ):
self.plant = quadrotor_plant
self.mb = self.plant.mb
self.lb = self.plant.lb
self.Ib = self.plant.Ib
self.m1 = self.plant.m1
self.l1 = self.plant.l1
self.I1 = self.plant.I1
self.g = self.plant.g
self.input_max = self.plant.input_max
self.uf = uf
self.xf = xf
self.kp = 10
self.kb = 8
A, B = quadrotor_plant.GetLinearizedDynamics(self.uf, self.xf)
Q = np.eye(8)
R = np.eye(2)*2
from pydrake.all import LinearQuadraticRegulator
self.K, self.S = LinearQuadraticRegulator(A, B, Q, R)
self.A = A
self.B = B
self.lqr_switch = 0
self.cnt =0
self.u_step = 12
self.J = 1000000
self.switch_t = 0
def create_reduced_lqr(A, B):
'''
Code submission for 3.3: Fill in the missing
details of this function to produce a control
matrix K, and cost-to-go matrix S, for the full
(4-state) system.
'''
Q = np.zeros((3, 3))
# Not clear what these gains will do,
# but as long as Q is positive semidefinite
# this should find a solution.
Q1 = np.random.random((3, 3))
Q = (Q1.T + Q1) # make symmetric and thus psd
print('Q',Q)
R = [1.]
A=np.delete(A, 1, 0)
A=np.delete(A,1,1)
B=np.delete(B, 1, 0)
print('A',A)
print('B',B)
K, S = LinearQuadraticRegulator(A, B, Q, R)
K=np.insert(K,1,0,axis=1)
S=np.insert(S,1,0,axis=1)
S=np.insert(S,1,0,axis=0)
# Refer to create_lqr() to see how invocations
# to LinearQuadraticRegulator work.
# https://docs.scipy.org/doc/numpy/reference/generated/numpy.delete.html
# and
# https://docs.scipy.org/doc/numpy/reference/generated/numpy.insert.html
# might be useful for helping with the state reduction.
#K = np.zeros((1, 4))
#S = np.eye(4)
return (K, S)
def LQR(self, ztraj, utraj, Q, R, Qf):
tspan = utraj.get_segment_times()
dztrajdt = ztraj.derivative(1)
context = self.CreateDefaultContext()
nZ = context.num_continuous_states()
nU = self.GetInputPort('u').size()
sym_system = self.ToSymbolic()
sym_context = sym_system.CreateDefaultContext()
prog = MathematicalProgram()
z = prog.NewIndeterminates(nZ,'z')
ucon = prog.NewIndeterminates(nU,'u')
#nY = self.GetOutputPort('z').size()
#N = np.zeros([nX, nU])
times = ztraj.get_segment_times()
K = []
S = []
for t in times:
# option 1
z0 = ztraj.value(t).transpose()[0]
u0 = utraj.value(t).transpose()[0]
sym_context.SetContinuousState(z0+z)
sym_context.FixInputPort(0, u0+ucon )
# zdot=f(z,u)==>zhdot=f(zh+z0,uh+u0)-z0dot
f = sym_system.EvalTimeDerivatives(sym_context).CopyToVector() # - dztrajdt.value(t).transpose()
mapping = dict(zip(z, z0))
mapping.update(dict(zip(ucon, u0)))
A = Evaluate(Jacobian(f, z), mapping)
B = Evaluate(Jacobian(f, ucon), mapping)
k, s = LinearQuadraticRegulator(A, B, Q, R)
import pdb; pdb.set_trace()
if(len(K) == 0):
K = np.ravel(k).reshape(nU*nZ,1)
S = np.ravel(s).reshape(nZ*nZ,1)
else:
K = np.hstack( (K, np.ravel(k).reshape(nU*nZ,1)) )
S = np.hstack( (S, np.ravel(s).reshape(nZ*nZ,1)) )
#
# option 2
#context.SetContinuousState(xtraj.value(t) )
#context.FixInputPort(0, utraj.value(t) )
#linearized_plant = Linearize(self, context)
#K.append(LinearQuadraticRegulator(linearized_plant.A(),
# linearized_plant.B(),
# Q, R)) #self, context, Q, R)
Kpp = PiecewisePolynomial.FirstOrderHold(times, K)
return Kpp
def create_LQR(self): # as initialization
Q = np.eye(2)
R = np.eye(2)
A = np.zeros([2, 2])
B = np.eye(2)
K, S = LinearQuadraticRegulator(A, B, Q, R)
return K, S
def lqr_controller(self, x, x_des):
#Robot parameters manually set according to actual measurements on 5/13/19
m_s = 0.2 #kg
d = 0.085 #m
m_c = 0.056
I_3 = 0.000228373 #.0000989844 #kg*m^2
R = 0.0333375
g = -9.81 #may need to be set as -9.81; test to see
u = np.zeros((self.plant.num_actuators()))
theta = math.asin(2 * (x[0] * x[2] - x[1] * x[3]))
theta_dot = x[
10] #Shown to be ~1.5% below true theta_dot on average in experiments
x_mod = np.asarray(
[
x[4] - x_des[0], theta - x_des[1], x[12] - x_des[2],
theta_dot - x_des[3]
]
) #[x, theta, x_dot, theta_dot] - need to verify positions of theta and theta_dot
#Assumes that all zeros is optimal state; may add optimal_state variable and subtract from current state to change
actuator_limit = .05 #must determine limits
A = np.zeros((4, 4))
A[0, 2] = 1.
A[1, 3] = 1.
A[2, 1] = (m_s**2 * d**2 * g) / (3 * m_c * I_3 + 3 * m_c * m_s * d**2 +
m_s * I_3) * 180 / math.pi
A[3, 1] = (m_s * d * g *
(3 * m_c + m_s)) / (3 * m_c * I_3 + 3 * m_c * m_s * d**2 +
m_s * I_3)
B = np.zeros((4, 1))
B[2,
0] = (-(m_s * d**2 + I_3) / R -
m_s * d) / (3 * m_c * I_3 + 3 * m_c * m_s * d**2 + m_s * I_3)
B[3, 0] = (-m_s * d / R - 3 * m_c * m_s) / (
3 * m_c * I_3 + 3 * m_c * m_s * d**2 + m_s * I_3) * math.pi / 180
Q = np.zeros((4, 4))
Q[0, 0] = 3.
Q[1, 1] = .1
Q[2, 2] = .1
Q[3, 3] = 4.
R = np.asarray([1.]) #0 => costless control
K, S = LinearQuadraticRegulator(A, B, Q, R)
#print('A',A)
#print('B',B)
#print('K',K)
u[0] = np.matmul(K, x_mod)
u[0] = np.clip(u[0], -actuator_limit, actuator_limit)
u[1] = -u[0]
#print('u',u)
return u
def QuadrotorLQR(plant):
context = plant.CreateDefaultContext()
context.SetContinuousState(np.zeros([6, 1]))
context.SetContinuousState(np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]))
context.FixInputPort(0, plant.mass * plant.gravity / 2. * np.array([1, 1]))
Q = np.diag([
100000, 100000, 100000, 10000, 10000,
10000 * (plant.length / 2. / np.pi)
])
R = np.array([[0.1, 0.05], [0.05, 0.1]])
return LinearQuadraticRegulator(plant, context, Q, R)
def get_LQR_params(self):
quadroter = QuadrotorPlant()
context = quadroter.CreateDefaultContext()
hover_thrust = self.m * self.g
quadroter.get_input_port(0).FixValue(context, [
hover_thrust / 4.0,
] * 4)
context.get_mutable_continuous_state_vector()\
.SetFromVector(self.nominal_state)
linear_sys = Linearize(quadroter, context, InputPortIndex(0),
OutputPortIndex(0))
return LinearQuadraticRegulator(linear_sys.A(), linear_sys.B(), self.Q,
self.R)
def BalancingLQR(robot):
# Design an LQR controller for stabilizing the CartPole around the upright.
# Returns a (static) AffineSystem that implements the controller (in
# the original CartPole coordinates).
context = robot.CreateDefaultContext()
context.FixInputPort(0, BasicVector([0]))
context.get_mutable_continuous_state_vector().SetFromVector(UprightState())
Q = np.diag((10., 10., 1., 1.))
R = [1]
return LinearQuadraticRegulator(robot, context, Q, R)
def BalancingLQR(plant):
context = plant.CreateDefaultContext()
plant.get_actuation_input_port().FixValue(context, [0])
context.get_mutable_continuous_state_vector().SetFromVector(UPRIGHT_STATE)
Q = np.diag((10.0, 10.0, 1.0, 1.0))
R = [1]
# MultibodyPlant has many (optional) input ports, so we must pass the
# input_port_index to LQR.
return LinearQuadraticRegulator(
plant,
context,
Q,
R,
input_port_index=plant.get_actuation_input_port().get_index(),
)
def BalancingLQR():
# Design an LQR controller for stabilizing the Acrobot around the upright.
# Returns a (static) AffineSystem that implements the controller (in
# the original AcrobotState coordinates).
acrobot = AcrobotPlant()
context = acrobot.CreateDefaultContext()
input = AcrobotInput()
input.set_tau(0.)
context.FixInputPort(0, input)
context.get_mutable_continuous_state_vector()\
.SetFromVector(UprightState().CopyToVector())
Q = np.diag((10., 10., 1., 1.))
R = [1]
return LinearQuadraticRegulator(acrobot, context, Q, R)
def LQR(self, xtraj, utraj, Q, R, Qf):
tspan = utraj.get_segment_times()
context = self.CreateDefaultContext()
nX = context.num_continuous_states()
nU = self.GetInputPort('u').size()
sym_system = self.ToSymbolic()
sym_context = sym_system.CreateDefaultContext()
prog = MathematicalProgram()
x = prog.NewIndeterminates(nX, 'x')
ucon = prog.NewIndeterminates(nU, 'u')
#nY = self.GetOutputPort('x').size()
#N = np.zeros([nX, nU])
times = xtraj.get_segment_times()
K = []
import pdb
pdb.set_trace()
for t in times:
# option 1
x0 = xtraj.value(t).transpose()[0]
u0 = utraj.value(t).transpose()[0]
sym_context.SetContinuousState(x0 + x)
sym_context.FixInputPort(0, u0 + ucon)
f = sym_system.EvalTimeDerivatives(sym_context).CopyToVector()
A = Evaluate(Jacobian(f, x), dict(zip(x, x0)))
B = Evaluate(Jacobian(f, ucon), dict(zip(ucon, u0)))
K.append(LinearQuadraticRegulator(A, B, Q, R))
# option 2
#context.SetContinuousState(xtraj.value(t) )
#context.FixInputPort(0, utraj.value(t) )
#linearized_plant = Linearize(self, context)
#K.append(LinearQuadraticRegulator(linearized_plant.A(),
# linearized_plant.B(),
# Q, R)) #self, context, Q, R)
Kpp = PiecewisePolynomial.FirstOrderHold(times, K)
return Kpp
def BalancingLQR(plant):
# Design an LQR controller for stabilizing the CartPole around the upright.
# Returns a (static) AffineSystem that implements the controller (in
# the original CartPole coordinates).
context = plant.CreateDefaultContext()
plant.get_actuation_input_port().FixValue(context, [0])
context.get_mutable_continuous_state_vector().SetFromVector(UprightState())
Q = np.diag((10., 10., 1., 1.))
R = [1]
# MultibodyPlant has many (optional) input ports, so we must pass the
# input_port_index to LQR.
return LinearQuadraticRegulator(
plant,
context,
Q,
R,
input_port_index=plant.get_actuation_input_port().get_index())
def BalancingLQR():
# Design an LQR controller for stabilizing the Acrobot around the upright.
# Returns a (static) AffineSystem that implements the controller (in
# the original AcrobotState coordinates).
pendulum = PendulumPlant()
context = pendulum.CreateDefaultContext()
input = PendulumInput()
input.set_tau(0.)
context.FixInputPort(0, input)
context.get_mutable_continuous_state_vector().SetFromVector(
UprightState().CopyToVector())
Q = np.diag((10., 1.))
R = [1]
linearized_pendulum = Linearize(pendulum, context)
(K, S) = LinearQuadraticRegulator(linearized_pendulum.A(),
linearized_pendulum.B(), Q, R)
return (K, S)
cartpole, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
urdf_path = FindResource('models/undamped_cartpole.urdf')
Parser(cartpole).AddModelFromFile(urdf_path)
cartpole.Finalize()
# set the operating point (vertical unstable equilibrium)
context = cartpole.CreateDefaultContext()
context.get_mutable_continuous_state_vector().SetFromVector(x_star)
# fix the input port to zero and get its index for the lqr function
cartpole.get_actuation_input_port().FixValue(context, [0])
input_i = cartpole.get_actuation_input_port().get_index()
# synthesize lqr controller directly from
# the nonlinear system and the operating point
lqr = LinearQuadraticRegulator(cartpole, context, Q, R, input_port_index=input_i)
lqr = builder.AddSystem(lqr)
# the following two lines are not needed here...
output_i = cartpole.get_state_output_port().get_index()
cartpole_lin = Linearize(cartpole, context, input_port_index=input_i, output_port_index=output_i)
# wire cart-pole and lqr
builder.Connect(cartpole.get_state_output_port(), lqr.get_input_port(0))
builder.Connect(lqr.get_output_port(0), cartpole.get_actuation_input_port())
# add a visualizer and wire it
visualizer = builder.AddSystem(
PlanarSceneGraphVisualizer(scene_graph, xlim=[-3., 3.], ylim=[-1.2, 1.2], show=False)
)
builder.Connect(scene_graph.get_pose_bundle_output_port(), visualizer.get_input_port(0))
Ae[2,3] = r_hat
Ae[3,0] = n_bar[2]
Ae[3,2] = -1* r_hat
Ae[0:4,4:5] = B0_f
Ae[4,4] = -1*sigma_motor
Be[4] = 1/sigma_motor
#the failed matrix u
# u_f = np.zeros([2,1])
Q[2,2]*= 1000
Q[3,3]*= 2
# get LQR controller about the fixed point
K0, S0 = LinearQuadraticRegulator(Ae, Be, Q, R)
# simulate stabilizing about fixed point using LQR controller
dt = 0.001
N = int(5.0/dt)
x = np.zeros((N+1, n))
forces = np.zeros((N+1, 4))
forces[0] = np.zeros(4)
x0 = np.zeros(n)
x0[0] = 5
x0[1] = 5
x[0] = x0
# here, assume the nx and ny are initially zero
# additional motor values are also set to zero
def one_rotor_loss():
# simulate quadrotor w/ LQR controller using forward Euler integration.
# fixed point
n = 12
m = 4
xd = np.zeros(n)
xd[0:2] = [2, 1]
ud = np.zeros(m)
ud[:] = mass * g / 4
x_u = np.hstack((xd, ud))
partials = jacobian(CalcF, x_u)
A0 = partials[:, 0:n]
print(A0)
B0 = partials[:, n:n + m]
print(B0)
Q = 10 * np.eye(n)
R = np.eye(m)
# get LQR controller about the fixed point
K0, S0 = LinearQuadraticRegulator(A0, B0, Q, R)
# simulate stabilizing about fixed point using LQR controller
# dt = 0.001
# N = int(2.0/dt)
x = np.zeros((N + 1, n))
x0 = np.zeros(n)
x[0] = x0
timeVec = np.zeros(2 * N + 1)
all_p = np.zeros(2 * N)
all_q = np.zeros(2 * N)
# keeping track of all forces on the quadrotors to show how they adapt
forces = np.zeros((2 * N, 4))
for i in range(N):
x_u = np.hstack((x[i], -K0.dot(x[i] - xd) + ud))
all_p[i] = x_u[9]
all_q[i] = x_u[10]
forces[i] = x_u[12:16]
print(forces[i])
x[i + 1] = x[i] + dt * CalcF(x_u)
timeVec[i] = timeVec[i - 1] + dt
#%% open meshact
# vis = meshcat.Visualizer()
# vis.open
#%% Meshcat animation
# PlotTrajectoryMeshcat(x, timeVec, vis)
# at timestamp N=5.0 seconds, rotor 4 fails and we switch control systems
n = 6
m = 2
#for failure of rotor 4, omega_hat_4 = 0
n_bar = [0.0, 0.289, 0.958]
sigma_motor = 0.015
#from eq 51
xd = np.zeros(n)
xd[1] = 5.69 #q
xd[2] = n_bar[0] #nx
xd[3] = n_bar[1] #ny
ud = np.zeros(m) #zero because u is in error coordinates
x_u = np.hstack((xd, ud))
#based on eq 51
force_bar = np.array([2.05, 1.02, 2.05, 0])
# force_bar = np.array([1.02, 2.05, 1.02, 0])
omega_bar = np.sqrt(force_bar / kF)
#define the A matrix for failed quad
Ae = np.zeros([6, 6])
B0_f = np.zeros([4, 2])
#extended state
Be = np.zeros([6, 2])
Q = np.eye(n)
R = np.eye(m)
r_hat = ((kF * kM) / gamma) * (omega_bar[0]**2 - omega_bar[1]**2 +
omega_bar[2]**2 - omega_bar[3]**2)
#define coupling constant
a_bar = (I[0, 0] - I[2, 2]) / I[0, 0]
B0_f[1, 0] = 1
B0_f[0, 1] = 1
B0_f = (l / I[0, 0]) * B0_f
Ae[0, 1] = a_bar
Ae[1, 0] = -1 * a_bar
Ae[1, 0] = -1 * a_bar
Ae[2, 1] = -1 * n_bar[2]
Ae[2, 3] = r_hat
Ae[3, 0] = n_bar[2]
Ae[3, 2] = -1 * r_hat
Ae[0:4, 4:6] = B0_f
Ae[4:6, 4:6] = -1 * np.eye(2) / sigma_motor
Be[4:6, 0:2] = np.eye(2) / sigma_motor
#the failed matrix u
# u_f = np.zeros([2,1])
Q[2:4, 2:4] *= 20
Q[4:6, 4:6] *= 0
# get LQR controller about the fixed point
K0, S0 = LinearQuadraticRegulator(Ae, Be, Q, R)
# simulate stabilizing about fixed point using LQR controller
x_mesh = np.zeros((N + 1, 12))
x_mesh[0] = x[N]
x = np.zeros((N + 1, n))
x0 = np.zeros(n)
x0[0] = 5
x0[1] = 5
x[0] = x0
for i in range(N):
x_u = np.hstack((x[i], -K0.dot(x[i] - xd) + ud))
all_p[i + N] = x[i, 0]
all_q[i + N] = x[i, 1]
xDot = Ae.dot(x[i]) + Be.dot(x_u[6:8])
x[i + 1] = x[i] + dt * xDot
# print(x[i,1])
u_i = -K0.dot(x[i] - xd) + ud
f = get_forces(u_i, force_bar)
# x_mesh[i+1] = x_mesh[i] + dt*CalcF(np.hstack((x_mesh[i], f)))
forces[i + N] = f
timeVec[i + N] = timeVec[i - 1 + N] + dt
x_mesh[i + 1] = x_mesh[i] + dt * CalcF(np.hstack([x_mesh[i], f]))
x_mesh[i + 1, 9] = x[i + 1, 0]
x_mesh[i + 1, 10] = x[i + 1, 1]
print(x_mesh[i + 1])
# plot_forces(forces, timeVec, all_p, all_q, 1, force_bar, omega_bar[1])
#%% open meshact
vis = meshcat.Visualizer()
vis.open
#%% Meshcat animation
PlotTrajectoryMeshcat(x_mesh, timeVec[N:2 * N + 1], vis)
#%% simulate and plot
dt = 0.001
T = 20000
t = dt*np.arange(T+1)
x = np.zeros((T+1, 2))
x[0] = [0, 0.1]
# desired fixed point
xd = np.array([-np.pi, 0])
# linearize about upright fixed point
f_x_u = jacobian(CalcF, np.hstack((xd, [0])))
A0 = f_x_u[:, 0:2]
B0 = f_x_u[:, 2:3]
K0, S0 = LinearQuadraticRegulator(A0, B0, 10*np.diag([1,1]), 1*np.eye(1))
for i in range(T):
x_u = np.hstack((x[i], Tau(x[i])))
x[i+1] = x[i] + dt*CalcF(x_u)
fig = plt.figure(figsize=(6,12), dpi = 100)
ax_x = fig.add_subplot(311)
ax_x.set_ylabel("x")
ax_x.plot(t, x[:,0])
ax_x.axhline(np.pi, color='r', ls='--')
ax_y = fig.add_subplot(312)
ax_y.set_ylabel("theta")
ax_y.plot(t, x[:,1])
ax_y.axhline(color='r', ls='--')
def two_rotor_loss():
# simulate quadrotor w/ LQR controller using forward Euler integration.
# fixed point
n = 12
m = 4
xd = np.zeros(n)
xd[0:2] = [2, 1]
ud = np.zeros(m)
ud[:] = mass * g / 4
x_u = np.hstack((xd, ud))
partials = jacobian(CalcF, x_u)
A0 = partials[:, 0:n]
B0 = partials[:, n:n + m]
Q = 10 * np.eye(n)
R = np.eye(m)
# get LQR controller about the fixed point
K0, S0 = LinearQuadraticRegulator(A0, B0, Q, R)
# simulate stabilizing about fixed point using LQR controller
# dt = 0.001
# N = int(2.0/dt)
x = np.zeros((N + 1, n))
x0 = np.zeros(n)
x[0] = x0
timeVec = np.zeros(2 * N + 1)
all_p = np.zeros(2 * N)
all_q = np.zeros(2 * N)
# keeping track of all forces on the quadrotors to show how they adapt
forces = np.zeros((2 * N, 4))
for i in range(N):
x_u = np.hstack((x[i], -K0.dot(x[i] - xd) + ud))
all_p[i] = x_u[9]
all_q[i] = x_u[10]
forces[i] = x_u[12:16]
print(forces[i])
x[i + 1] = x[i] + dt * CalcF(x_u)
timeVec[i] = timeVec[i - 1] + dt
#%% open meshact
# vis = meshcat.Visualizer()
# vis.open
#%% Meshcat animation
# PlotTrajectoryMeshcat(x, timeVec, vis)
#for failure of rotor 4 and 2, omega_hat_4 = omega_hat_2= 0
n = 5
m = 1
kF = 6.41e-6
n_bar = [0.0, 0.0, 1.0]
sigma_motor = 0.015
#from eq 51
xd = np.zeros(n) #p,q are set to 0
xd[2] = n_bar[0] #nx
xd[3] = n_bar[1] #ny
ud = np.zeros(m) #zero because u is in error coordinates
x_u = np.hstack((xd, ud))
#based on eq 51
force_bar = np.array([2.45, 0.0, 2.45, 0.0])
#omega_bar = np.sqrt(force_bar/kF) explicitly calcualted in paper
omega_bar = [-619.0, 0.0, -619.0, 0.0]
#define the A matrix for failed quad
Ae = np.zeros([5, 5])
B0_f = np.zeros([4, 1])
#extended state
Be = np.zeros([5, 1])
Q = np.eye(n)
R = np.eye(m)
R *= 0.75
r_hat = ((kF * kM) / gamma) * (omega_bar[0]**2 - omega_bar[1]**2 +
omega_bar[2]**2 - omega_bar[3]**2)
#define coupling constant
a_bar = (I[0, 0] - I[2, 2]) / I[0, 0]
B0_f[1] = 1
B0_f = (l / I[0, 0]) * B0_f
Ae[0, 1] = a_bar
Ae[1, 0] = -1 * a_bar
Ae[1, 0] = -1 * a_bar
Ae[2, 1] = -1 * n_bar[2]
Ae[2, 3] = r_hat
Ae[3, 0] = n_bar[2]
Ae[3, 2] = -1 * r_hat
Ae[0:4, 4:5] = B0_f
Ae[4, 4] = -1 * sigma_motor
Be[4] = 1 / sigma_motor
#the failed matrix u
# u_f = np.zeros([2,1])
Q[2, 2] *= 1000
Q[3, 3] *= 2
# get LQR controller about the fixed point
K0, S0 = LinearQuadraticRegulator(Ae, Be, Q, R)
# simulate stabilizing about fixed point using LQR controller
# dt = 0.001
# N = int(5.0/dt)
x = np.zeros((N + 1, n))
# forces = np.zeros((N+1, 4))
# forces[0] = np.zeros(4)
x0 = np.zeros(n)
x0[0] = 5
x0[1] = 5
x[0] = x0
# here, assume the nx and ny are initially zero
# additional motor values are also set to zero
# timeVec = np.zeros(N+1)
for i in range(N):
x_u = np.hstack((x[i], -K0.dot(x[i] - xd) + ud))
all_p[i + N] = x[i, 0]
all_q[i + N] = x[i, 1]
xDot = Ae.dot(x[i]) + Be.dot(x_u[n:n + m])
x[i + 1] = x[i] + dt * xDot
# print(x[i,1])
u_i = -K0.dot(x[i] - xd) + ud
f = np.zeros(4)
f[2] = (u_i[0] + 2 * force_bar[2]) / 2
f[0] = np.sum(force_bar) - f[2]
forces[i + N] = f
timeVec[i + N] = timeVec[i - 1 + N] + dt
# PlotFailedTraj(x.copy(), dt, xd, ud, timeVec, forces)
print(timeVec.shape)
print(force_bar.shape)
plot_forces(forces, timeVec, all_p, all_q, 2, force_bar, omega_bar[1])
def RegionOfAttraction(self, xtraj, utraj, V=None):
# Construct a polynomial V that contains all monomials with s,c,thetadot up to degree 2.
#deg_V = 2
do_normalization = False
do_balancing = False #True
do_use_Slti = True
# Construct a polynomial L representing the "Lagrange multiplier".
deg_L = 4
taylor_order = 3
Q = 10.0*np.eye(self.nX)
R = 1.0*np.eye(self.nU)
Qf = 1.0*np.eye(self.nX)
rho_f = 1.0
xdotraj = xtraj.derivative(1)
# set some constraints on inputs
#context.SetContinuousState(xtraj.value(xtraj.end_time()))
#context.FixInputPort(0, utraj.value(utraj.end_time()))
#x0 = context.get_continuous_state_vector().CopyToVector()
#if(xdotraj is None):
# xdotraj = xtraj.Derivative(1)
# Check that x0 is a "fixed point" (on the trajectory).
#xdot0 = self.EvalTimeDerivatives(context).CopyToVector()
#assert np.allclose(xdot0, xdotraj), "context does not describe valid path." #0*xdot0),
prog = MathematicalProgram()
x = prog.NewIndeterminates(self.nX, 'x')
ucon = prog.NewIndeterminates(self.nU, 'u')
#sym_system = self.ToSymbolic()
#sym_context = sym_system.CreateDefaultContext()
#sym_context.SetContinuousState(x)
#sym_context.FixInputPort(0, ucon )
#f = sym_system.EvalTimeDerivativesTaylor(sym_context).CopyToVector()
times = xtraj.get_segment_times()
all_V = [] #might be transformed
all_Vd = [] #might be transformed
all_Vd2 = []
all_fcl = [] #might be transformed
all_T = [] #might be transformed
all_x0 = [] #might be transformed
sumV = 0.0
zero_map = dict(zip(x,np.zeros(self.nX)))
if(do_use_Slti):
# get the final ROA to be the initial condition (of t=end) of the S from Ricatti)
for tf in [times[-1]]:
#tf = times[-1]
xf = xtraj.value(tf).transpose()[0]
xdf = xdotraj.value(tf).transpose()[0]
uf = utraj.value(tf).transpose()[0]
Af, Bf = self.PlantDerivatives(xf, uf) #0.0*uf)
Kf, __ = LinearQuadraticRegulator(Af, Bf, Q, R)
# get a polynomial representation of f_closedloop, xdot = f_cl(x)
# where x is actually in rel. coord.
f_Lcl = self.EvalClosedLoopDynamics(x, ucon, xf, uf, xdf, Kf, order=1) # linearization CL
Af = Evaluate(Jacobian(f_Lcl, x), zero_map) # because this is rel. coord.
Qf = RealContinuousLyapunovEquation(Af, Q)
f_cl = self.EvalClosedLoopDynamics(x, ucon, xf, uf, xdf, Kf, order=taylor_order) # for dynamics CL
# make sure Lyapunov candidate is pd
if (self.isPositiveDefinite(Qf)==False):
assert False, '******\nQf is not PD for t=%f\n******' %(tf)
Vf = (x-xf).transpose().dot(Qf.dot((x-xf)))
#import pdb; pdb.set_trace()
if(do_normalization):
#coeffs = Polynomial(Vf).monomial_to_coefficient_map().values()
#sumV = 0.
#for coeff in coeffs:
# sumV = sumV + np.abs(coeff.Evaluate())
sumV = Vf.Evaluate(dict(zip(x, xf+np.ones(self.nX)))) #do: V(1,1,...)=1
Vf = Vf / sumV #normalize coefficient sum to one
Qf = Qf / sumV
Vfdot = Vf.Jacobian(x).dot(f_cl) # we're just doing the static final point to get Rho_f
#H = Evaluate(0.5*Jacobian(Vfdot.Jacobian(x),x), zero_map)
#if (self.isPositiveDefinite(-H)==False):
# assert False, '******\nVdot is not ND at the end point, for t=%f\n******' %(tf)
if(do_balancing):
#import pdb; pdb.set_trace()
S1 = Evaluate(0.5*Jacobian(Vf.Jacobian(x),x), zero_map)
S2 = Evaluate(0.5*Jacobian(Vfdot.Jacobian(x),x), zero_map)
T = self.balanceQuadraticForm(S1, S2)
balanced_x = T.dot(x)
balance_map = dict(zip(x, balanced_x))
Vf = Vf.Substitute(balance_map)
Vfdot = Vfdot.Substitute(balance_map)
for i in range(len(f_cl)):
f_cl[i] = f_cl[i].Substitute(balance_map)
xf = np.linalg.inv(T).dot(xf) #the new coordinates of the equilibrium point
rhomin = 0.0
rhomax = 1.0
#import pdb; pdb.set_trace()
#deg_L = Polynomial(Vfdot).TotalDegree()
# First bracket the solution
while self.CheckLevelSet(x, xf, Vf, Vfdot, rhomax, multiplier_degree=deg_L) > 0:
rhomin = rhomax
rhomax = 1.2*rhomax
#print('Rho_max = %f' %(rhomax))
tolerance = 1e-4
slack = -1.0
while rhomax - rhomin > tolerance:
rho_f = (rhomin + rhomax)/2
slack = self.CheckLevelSet(x, xf, Vf, Vfdot, rho_f, multiplier_degree=deg_L)
if slack >= 0:
rhomin = rho_f
else:
rhomax = rho_f
rho_f = (rhomin + rhomax)/2
rho_f = rho_f*0.8 # just to be on the safe-side. REMOVE WHEN WORKING
print('Rho_final(t=%f) = %f; slack=%f' %(tf, rho_f, slack))
#import pdb; pdb.set_trace()
#import pdb; pdb.set_trace()
# end of getting initial conditions
if V is None:
# Do optimization to find the Lyapunov candidate.
#print('******\nRunning SOS to find Lyapunov function ...')
#V, Vdot = self.findLyapunovFunctionSOS(xtraj, utraj, deg_V, deg_L)
# or Do tvlqr to get S
print('******\nRunning TVLQR ...')
K, S = self.TVLQR(xtraj, utraj, Q, R, Qf)
#S0 = S.value(times[-1]).reshape(self.nX, self.nX)
#print(Polynomial(x.dot(S0.dot(x))).RemoveTermsWithSmallCoefficients(1e-6))
print('Done\n******')
for t in times:
x0 = xtraj.value(t).transpose()[0]
xd0 = xdotraj.value(t).transpose()[0]
u0 = utraj.value(t).transpose()[0]
K0 = K.value(t).reshape(self.nU, self.nX)
S0 = S.value(t).reshape(self.nX, self.nX)
S0d = S.derivative(1).value(t).reshape(self.nX, self.nX) # Sdot
# make sure Lyapunov candidate is pd
if (self.isPositiveDefinite(S0)==False):
assert False, '******\nS is not PD for t=%f\n******' %(t)
# for debugging
#S0 = np.eye(self.nX)
#V = x.dot(S0.dot(x))
V = (x-x0).transpose().dot(S0.dot((x-x0)))
# normalization of the lyapunov function
if(do_normalization):
#coeffs = Polynomial(V).monomial_to_coefficient_map().values()
#sumV = 0.
#for coeff in coeffs:
# sumV = sumV + np.abs(coeff.Evaluate())
#sumV = V.Evaluate(dict(zip(x, x0+np.ones(self.nX)))) #do: V(1,1,...)=1
V = V / sumV #normalize coefficient sum to one
# get a polynomial representation of f_closedloop, xdot = f_cl(x)
f_cl = self.EvalClosedLoopDynamics(x, ucon, x0, u0, xd0, K0, order=taylor_order)
#import pdb; pdb.set_trace()
# vdot = x'*Sdot*x + dV/dx*fcl_poly
#Vdot = (x.transpose().dot(S0d)).dot(x) + V.Jacobian(x).dot(f_cl(xbar))
Vdot = ((x-x0).transpose().dot(S0d)).dot((x-x0)) + V.Jacobian(x).dot(f_cl)
#deg_L = np.max([Polynomial(Vdot).TotalDegree(), deg_L])
if(do_balancing):
#import pdb; pdb.set_trace()
S1 = Evaluate(0.5*Jacobian(V.Jacobian(x),x), zero_map)
S2 = Evaluate(0.5*Jacobian(Vdot.Jacobian(x),x), zero_map)
T = self.balanceQuadraticForm(S1, S2)
balanced_x = T.dot(x)
balance_map = dict(zip(x, balanced_x))
V = V.Substitute(balance_map)
Vdot = Vdot.Substitute(balance_map)
for i in range(len(f_cl)):
f_cl[i] = f_cl[i].Substitute(balance_map)
x0 = np.linalg.inv(T).dot(x0) #the new coordinates of the equilibrium point
else:
T = np.eye(self.nX)
# store it for later use
all_V.append(V)
all_fcl.append(f_cl)
all_Vd.append(Vdot)
all_T.append(T)
all_x0.append(x0)
for i in range(len(times)-1):
xd0 = xdotraj.value(times[i]).transpose()[0]
Vdot = (all_V[i+1]-all_V[i])/(times[i+1]-times[i]) + all_V[i].Jacobian(x).dot(all_fcl[i])
all_Vd2.append(Vdot)
#import pdb; pdb.set_trace()
#rho_f = 1.0
# time-varying PolynomialLyapunovFunction who's one-level set defines the verified invariant region
V = self.TimeVaryingLyapunovSearchRho(x, all_V, all_Vd, all_T, times, xtraj, utraj, rho_f, \
multiplier_degree=deg_L)
# Check Hessian of Vdot at origin
#H = Evaluate(0.5*Jacobian(Vdot.Jacobian(x),x), dict(zip(x, x0)))
#assert isPositiveDefinite(-H), "Vdot is not negative definite at the fixed point."
return V
PlotTrajectoryMeshcat)
from ilqr_quadrotor_3D import traj_specs, planner
import meshcat
#%% get LQR controller about goal point
# fixed point
xd = np.zeros(n)
ud = np.zeros(m)
ud[:] = mass * g / 4
x_u = np.hstack((xd, ud))
partials = jacobian(CalcF, x_u)
A0 = partials[:, 0:n]
B0 = partials[:, n:n + m]
Q = 10 * np.eye(n)
R = np.eye(m)
K0, S0 = LinearQuadraticRegulator(A0, B0, Q, R)
#%% Build drake diagram system and simulate.
builder = DiagramBuilder()
quad = builder.AddSystem(Quadrotor())
# controller system
class QuadLqrController(LeafSystem):
def __init__(self):
LeafSystem.__init__(self)
self._DeclareInputPort(PortDataType.kVectorValued, n)
self._DeclareVectorOutputPort(BasicVector(m), self._DoCalcVectorOutput)
self._DeclareDiscreteState(m) # state of the controller system is u
self._DeclarePeriodicDiscreteUpdate(
period_sec=0.005) # update u at 200Hz
fh_lqr_phi_data = []
fh_lqr_phi_dot_data = []
fh_lqr_theta_data = []
# Set up LQR
lqr_plant_vector_system, lqr_position_system = get_plant_and_pos()
lqr_builder = DiagramBuilder()
plant = lqr_builder.AddSystem(lqr_plant_vector_system)
position = lqr_builder.AddSystem(lqr_position_system)
lqr_context = plant.CreateDefaultContext()
plant.get_input_port(0).FixValue(lqr_context, u_bar)
lqr_context.SetContinuousState(x_bar)
Q = np.diag([1, 100, 100, 1, 1])
R = np.diag([0.5, 0.5, 0.1])
LQR_Controller = LinearQuadraticRegulator(plant, lqr_context, Q, R)
lqr_t, lqr_r_data, lqr_r_dot_data, lqr_theta_dot_data, lqr_phi_data, \
lqr_phi_dot_data, lqr_theta_data, = simulate(
lqr_builder, lqr_plant_vector_system, lqr_position_system, LQR_Controller, ss_x0, simulation_time - fh_lqr_time)
if fh_lqr_time > 0:
lqr_t = lqr_t + fh_lqr_t[-1]
t = np.concatenate((fh_lqr_t, lqr_t))
r_data = np.concatenate((fh_lqr_r_data, lqr_r_data))
r_dot_data = np.concatenate((fh_lqr_r_dot_data, lqr_r_dot_data))
theta_dot_data = np.concatenate((fh_lqr_theta_dot_data, lqr_theta_dot_data))
phi_data = np.concatenate((fh_lqr_phi_data, lqr_phi_data))
phi_dot_data = np.concatenate((fh_lqr_phi_dot_data, lqr_phi_dot_data))
theta_data = np.concatenate((fh_lqr_theta_data, lqr_theta_data))
# print("A:", A, "B:", B)
Q = 1 * np.eye(13)
Q[0:3, 0:3] = 10 * np.eye(3)
Q[10:13, 10:13] = 1 * np.eye(3)
R = np.eye(4)
N = np.zeros((13, 4))
M_lqr = solve_continuous_are(A, B, Q, R)
# print("M_lqr:", M_lqr)
K_py = np.dot(np.dot(np.linalg.inv(R), B.T), M_lqr)
print("K_py", K_py)
# print("K_py size:", K_py.shape)
K_, S_ = LinearQuadraticRegulator(A, B, Q, R)
# print("K_:", K_, "S_:", S_)
ControllabilityMatrix = np.zeros((13, 4 * 13))
for i in range(0, 13):
if i == 0:
TestAB = B
ControllabilityMatrix = TestAB
# print("ControllabilityMatrix:", ControllabilityMatrix)
else:
TestAB = np.dot(A, TestAB)
ControllabilityMatrix = np.hstack([ControllabilityMatrix, TestAB])
# print("ContrbM: ", ControllabilityMatrix)
# print("Contrb size:", ControllabilityMatrix.shape)
rankContrb = np.linalg.matrix_rank(ControllabilityMatrix)
#print CalcFu(x_u)
#print jacobian(CalcF, x_u)
#%% simulate and plot
dt = 0.01
T = 4000
t = dt * np.arange(T + 1)
x = np.zeros((T + 1, 4))
x[0] = [0, 3.19, 0, 0]
xd = np.array([0, np.pi, 0, 0])
f_x_u = jacobian(CalcF, np.hstack((xd, [0])))
A0 = f_x_u[:, 0:4]
B0 = f_x_u[:, 4:5]
K0, S0 = LinearQuadraticRegulator(A0, B0, 100 * np.diag([1, 1, 1, 1]),
1 * np.eye(1))
for i in range(T):
x_u = np.hstack((x[i], -K0.dot(x[i] - xd)))
x[i + 1] = x[i] + dt * CalcF(x_u)
fig = plt.figure(figsize=(6, 12), dpi=100)
ax_x = fig.add_subplot(311)
ax_x.set_ylabel("x")
ax_x.plot(t, x[:, 0])
ax_x.axhline(color='r', ls='--')
ax_y = fig.add_subplot(312)
ax_y.set_ylabel("theta")
ax_y.plot(t, x[:, 1])
ax_y.axhline(color='r', ls='--')
if __name__ == '__main__':
# simulate quadrotor w/ LQR controller using forward Euler integration.
# fixed point
xd = np.zeros(n)
xd[0:2] = [2, 1]
ud = np.zeros(m)
ud[:] = mass * g / 4
x_u = np.hstack((xd, ud))
partials = jacobian(CalcF, x_u)
A0 = partials[:, 0:n]
B0 = partials[:, n:n + m]
Q = 10 * np.eye(n)
R = np.eye(m)
# get LQR controller about the fixed point
K0, S0 = LinearQuadraticRegulator(A0, B0, Q, R)
# simulate stabilizing about fixed point using LQR controller
dt = 0.001
N = int(4.0 / dt)
x = np.zeros((N + 1, n))
x0 = np.zeros(n)
x[0] = x0
timeVec = np.zeros(N + 1)
for i in range(N):
x_u = np.hstack((x[i], -K0.dot(x[i] - xd) + ud))
x[i + 1] = x[i] + dt * CalcF(x_u)
timeVec[i] = timeVec[i - 1] + dt
def CallLQR(x, u, Q, R):
f_x_u = jacobian(self.CalcF, np.hstack((x, u)))
A = f_x_u[:, 0:self.n]
B = f_x_u[:, self.n:self.n + self.m]
K, P = LinearQuadraticRegulator(A, B, Q, R)
return K, P