def __init__(self):
# k_p and k_d gains for swing up controller
# TODO: choose appropriate k_p and k_d gains
self.kp = 0
self.kd = 0
self.g = 9.81
# mass of the pole and mass of the part
self.mc = 1
self.mp = 1
self.L = 1
# Computes the lqr gains for the final stabilizing controller
A = np.array(
[[0, 0, 1, 0], [0, 0, 0, 1],
[0, self.g * (self.mc / self.mp), 1, 0],
[0, self.g * (self.mc + self.mp) / (self.L * self.mc), 0, 0]])
B = np.array([0, 0, 1 / self.mc, 1 / (self.L * self.mc)]).T
Q = np.eye((4))
Q[3, 3] = 10
R = np.array([1])
N = np.zeros(4)
self.K, self.s = LinearQuadraticRegulator(A, B, Q, R, N)
self.x_des = np.array([0, pi, 0, 0])
def test_linear_quadratic_regulator(self):
A = np.array([[0, 1], [0, 0]])
B = np.array([[0], [1]])
C = np.identity(2)
D = np.array([[0], [0]])
double_integrator = LinearSystem(A, B, C, D)
Q = np.identity(2)
R = np.identity(1)
K_expected = np.array([[1, math.sqrt(3.)]])
S_expected = np.array([[math.sqrt(3), 1.], [1., math.sqrt(3)]])
(K, S) = LinearQuadraticRegulator(A, B, Q, R)
np.testing.assert_almost_equal(K, K_expected)
np.testing.assert_almost_equal(S, S_expected)
controller = LinearQuadraticRegulator(double_integrator, Q, R)
np.testing.assert_almost_equal(controller.D(), -K_expected)
context = double_integrator.CreateDefaultContext()
double_integrator.get_input_port(0).FixValue(context, [0])
controller = LinearQuadraticRegulator(
double_integrator,
context,
Q,
R,
input_port_index=double_integrator.get_input_port().get_index())
np.testing.assert_almost_equal(controller.D(), -K_expected)
def QuadrotorLQR(plant):
context = plant.CreateDefaultContext()
context.SetContinuousState(np.zeros([6, 1]))
context.FixInputPort(0, plant.mass * plant.gravity / 2. * np.array([1, 1]))
Q = np.diag([10, 10, 10, 1, 1, (plant.length / 2. / np.pi)])
R = np.array([[0.1, 0.05], [0.05, 0.1]])
return LinearQuadraticRegulator(plant, context, Q, R)
def control(ref, meas, x):
global z_est
global u_est
global L
# observer gain (only for 1 measurement, if you have more, compute in matlab).
#wn = 2.*np.pi*1. # 1Hz observer
#xi = 0.7 # damping factor
#LL = np.array([[-2.*xi*wn], [-wn*wn]]) # pole-placement
# create the range estimation for each sensor
N_sensors = meas.shape[0]
meas_est = np.zeros([N_sensors, 1])
for i, sensor in enumerate(sensor_array):
north_wall = 1.0
if sensor < 0.:
north_wall = -1.0
y_est = wall_y_location(x, y_dir=north_wall) - z_est[0][0]
if (np.abs(sensor) < 0.001):
meas_est[i][0] = np.inf
else:
meas_est[i][0] = y_est / np.sin(np.deg2rad(sensor))
# Luenberger Observer
f = np.array([[z_est[1][0]], \
[1./m * u_est] ])
# Estimator equations
zdot_est = f + L.dot(meas - meas_est)
# Euler integration
z_est = z_est + dt * zdot_est
# now we can do state feedback control (x-axis doesn't really play here)
Af = np.array([[0., 1.], [0., 0.]])
Bf = np.array([[0.], [1. / m]])
Q = np.eye(2)
R = np.eye(1)
Kf, Qf = LinearQuadraticRegulator(Af, Bf, Q, R)
u_est = ref - Kf.dot(z_est)[0][0]
# do nothing (for debug)
#u_est = 0.
return u_est
def time_varying_lqr(
trajectory_optimization, Q = None, R = None) -> np.ndarray:
"""
Policy for implementing TVLQR on an existing trajectory
Arguments:
trajectory_optimization: Trajectory with trajectory dtype
Q: Q matrix in LQR (must match dimension of system state)
R: R matrix in LQR (must match dimension of system input)
Returns:
States and inputs followed over time
"""
# Extract desired trajectory information
trajectory = trajectory_optimization.solution
system = trajectory_optimization.system
traj_state = trajectory['state']
traj_input = trajectory['input']
traj_t = trajectory['t']
n_steps = len(traj_t)
# Initialize data structure to hold output data
path = np.zeros(n_steps, dtype = trajectory_dtype(
system.n_state, system.n_u))
path['t'] = traj_t
path['state'][0] = x_initial
if Q is None:
Q = np.eye((system.n_state, system.n_state))
if R is None:
R = np.eye((system.n_u, system.n_u))
# Simulate system with policy
for i in range(0, n_steps - 1):
current_time = traj_t[i]
xbar = path[i,:] - x_traj[i];
(A_lin, B_lin) = system.linearized_dynamics(
current_time, traj_state[i,:], traj_input[i])
K, S = LinearQuadraticRegulator(Alin, Blin, Q, R)
ubar = -np.dot(K, xbar)
u = ubar + u_traj[ii]
path['input'][i] = u
path['state'][i+1,:] = step(path[i], u, traj_t[i+1] - current_time)
return path
def calcTvlqrStabilization(self):
"""
Compute time-varying LQR stabilization around trajectory. Linearize with nominal state, input at each
knot point and compute LQR based on that linearization.
"""
if self.mp_result != SolutionResult.kSolutionFound:
print "Optimal traj not yet computed, can't stablize"
return None
xd = self.xdtraj
ud = self.udtraj
# # V1 (used here)
# define Q, R for LQR cost
# calc df/dx, df/du with pydrake autodiff
# calculate A(t), B(t) at knot points
# calculate K(t) using A, B, Q, R
# # V2 (to implement if needed)
# define Q, R for LQR cost
# calc df/dx, df/du with pydrake autodiff
# calculate A(t), B(t)
# calculate S(t) by solving ODE backwards in time, potentially w/sqrt method
# calculate K(t)
n = self.num_states
m = self.num_inputs
k = ud.shape[0] # time steps
Q = np.eye(n) # quadratic state cost
R = 0.1 * np.eye(m) # quadratic input cost
K = np.zeros((k, m, n))
S = np.zeros((k, n, n))
for i in range(k):
A = self.A(xd[i, :], ud[i, :])
B = self.B(xd[i, :], ud[i, :])
K[i], S[i] = LinearQuadraticRegulator(A, B, Q, R)
self.K = K
self.S_lqr = S
return K
def BalancingLQR(diagram):
# 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 = diagram.CreateDefaultContext()
diagram.get_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(
diagram,
context,
Q,
R,
input_port_index=diagram.get_input_port().get_index())
def __init__(self, plant, contacts_per_frame, is_wbc=False):
LeafSystem.__init__(self)
self.plant = plant
self.contacts_per_frame = contacts_per_frame
self.is_wbc = is_wbc
self.upright_context = self.plant.CreateDefaultContext()
self.q_nom = self.plant.GetPositions(
self.upright_context) # Nominal upright pose
self.input_q_v_idx = self.DeclareVectorInputPort(
"q_v",
BasicVector(self.plant.num_positions() +
self.plant.num_velocities())).get_index()
self.output_tau_idx = self.DeclareVectorOutputPort(
"tau", BasicVector(Atlas.NUM_ACTUATED_DOF),
self.calcTorqueOutput).get_index()
if self.is_wbc:
com_dim = 3
self.x_size = 2 * com_dim
self.u_size = com_dim
self.input_r_des_idx = self.DeclareVectorInputPort(
"r_des", BasicVector(com_dim)).get_index()
self.input_rd_des_idx = self.DeclareVectorInputPort(
"rd_des", BasicVector(com_dim)).get_index()
self.input_rdd_des_idx = self.DeclareVectorInputPort(
"rdd_des", BasicVector(com_dim)).get_index()
self.input_q_des_idx = self.DeclareVectorInputPort(
"q_des", BasicVector(self.plant.num_positions())).get_index()
self.input_v_des_idx = self.DeclareVectorInputPort(
"v_des", BasicVector(self.plant.num_velocities())).get_index()
self.input_vd_des_idx = self.DeclareVectorInputPort(
"vd_des",
BasicVector(self.plant.num_velocities())).get_index()
Q = 1.0 * np.identity(self.x_size)
R = 0.1 * np.identity(self.u_size)
A = np.vstack([
np.hstack([0 * np.identity(com_dim),
1 * np.identity(com_dim)]),
np.hstack([0 * np.identity(com_dim), 0 * np.identity(com_dim)])
])
B_1 = np.vstack(
[0 * np.identity(com_dim), 1 * np.identity(com_dim)])
K, S = LinearQuadraticRegulator(A, B_1, Q, R)
def V_full(x, u, r, rd, rdd):
x_bar = x - np.concatenate([r, rd])
u_bar = u - rdd
'''
xd_bar = d(x - [r, rd].T)/dt
= xd - [rd, rdd].T
= Ax + Bu - [rd, rdd].T
'''
xd_bar = A.dot(x) + B_1.dot(u) - np.concatenate([rd, rdd])
return x_bar.T.dot(Q).dot(x_bar) + u_bar.T.dot(R).dot(
u_bar) + 2 * x_bar.T.dot(S).dot(xd_bar)
self.V_full = V_full
else:
# Only x, y coordinates of COM is considered
com_dim = 2
self.x_size = 2 * com_dim
self.u_size = com_dim
self.input_y_des_idx = self.DeclareVectorInputPort(
"y_des", BasicVector(zmp_state_size)).get_index()
''' Eq(1) '''
A = np.vstack([
np.hstack([0 * np.identity(com_dim),
1 * np.identity(com_dim)]),
np.hstack([0 * np.identity(com_dim), 0 * np.identity(com_dim)])
])
B_1 = np.vstack(
[0 * np.identity(com_dim), 1 * np.identity(com_dim)])
''' Eq(4) '''
C_2 = np.hstack([np.identity(2), np.zeros((2, 2))]) # C in Eq(2)
D = -z_com / Atlas.g * np.identity(zmp_state_size)
Q = 1.0 * np.identity(zmp_state_size)
''' Eq(6) '''
'''
y.T*Q*y
= (C*x+D*u)*Q*(C*x+D*u).T
= x.T*C.T*Q*C*x + u.T*D.T*Q*D*u + x.T*C.T*Q*D*u + u.T*D.T*Q*C*X
= .. + 2*x.T*C.T*Q*D*u
'''
K, S = LinearQuadraticRegulator(A, B_1,
C_2.T.dot(Q).dot(C_2),
D.T.dot(Q).dot(D),
C_2.T.dot(Q).dot(D))
# Use original formulation
def V_full(x, u,
y_des): # Assume constant z_com, we don't need tvLQR
y = C_2.dot(x) + D.dot(u)
def dJ_dx(x):
return x.T.dot(
S.T + S
) # https://math.stackexchange.com/questions/20694/vector-derivative-w-r-t-its-transpose-fracdaxdxt
y_bar = y - y_des
x_bar = x - np.concatenate(
[y_des, [0.0, 0.0]]) # FIXME: This doesn't seem right...
# FIXME: xd_bar should depend on yd_des
xd_bar = A.dot(x_bar) + B_1.dot(u)
return y_bar.T.dot(Q).dot(y_bar) + dJ_dx(x_bar).dot(xd_bar)
self.V_full = V_full
# Calculate values that don't depend on context
self.B_7 = self.plant.MakeActuationMatrix()
# From np.sort(np.nonzero(B_7)[0]) we know that indices 0-5 are the unactuated 6 DOF floating base and 6-35 are the actuated 30 DOF robot joints
self.v_idx_act = 6 # Start index of actuated joints in generalized velocities
self.B_a = self.B_7[self.v_idx_act:, :]
self.B_a_inv = np.linalg.inv(self.B_a)
# Sort joint effort limits to be the same order as tau in Eq(13)
self.sorted_max_efforts = np.array([
entry[1].effort
for entry in getJointLimitsSortedByActuator(self.plant)
])
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"--num_samples",
type=int,
default=50,
help="Number of Monte Carlo samples to use to estimate performance.")
parser.add_argument("--torque_limit",
type=float,
default=2.0,
help="Torque limit of the pendulum.")
args = parser.parse_args()
if args.torque_limit < 0:
raise InvalidArgumentError("Please supply a nonnegative torque limit.")
# Assemble the Pendulum plant.
builder = DiagramBuilder()
pendulum = builder.AddSystem(MultibodyPlant(0.0))
file_name = FindResourceOrThrow("drake/examples/pendulum/Pendulum.urdf")
Parser(pendulum).AddModelFromFile(file_name)
pendulum.WeldFrames(pendulum.world_frame(),
pendulum.GetFrameByName("base"))
pendulum.Finalize()
# Set the pendulum to start at uniformly random
# positions (but always zero velocity).
elbow = pendulum.GetMutableJointByName("theta")
upright_theta = np.pi
theta_expression = Variable(name="theta",
type=Variable.Type.RANDOM_UNIFORM) * 2. * np.pi
elbow.set_random_angle_distribution(theta_expression)
# Set up LQR, with high position gains to try to ensure the
# ROA is close to the theoretical torque-limited limit.
Q = np.diag([100., 1.])
R = np.identity(1) * 0.01
linearize_context = pendulum.CreateDefaultContext()
linearize_context.SetContinuousState(np.array([upright_theta, 0.]))
actuation_port = pendulum.get_actuation_input_port()
actuation_port.FixValue(linearize_context, 0)
controller = builder.AddSystem(
LinearQuadraticRegulator(pendulum, linearize_context, Q, R,
np.zeros(0), actuation_port.get_index()))
# Apply the torque limit.
torque_limit = args.torque_limit
torque_limiter = builder.AddSystem(
Saturation(min_value=np.array([-torque_limit]),
max_value=np.array([torque_limit])))
builder.Connect(controller.get_output_port(0),
torque_limiter.get_input_port(0))
builder.Connect(torque_limiter.get_output_port(0),
pendulum.get_actuation_input_port())
builder.Connect(pendulum.get_state_output_port(),
controller.get_input_port(0))
diagram = builder.Build()
# Perform the Monte Carlo simulation.
def make_simulator(generator):
''' Create a simulator for the system
using the given generator. '''
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(0)
simulator.Initialize()
return simulator
def calc_wrapped_error(system, context):
''' Given a context from the end of the simulation,
calculate an error -- which for this stabilizing
task is the distance from the
fixed point. '''
state = diagram.GetSubsystemContext(
pendulum, context).get_continuous_state_vector()
error = state.GetAtIndex(0) - upright_theta
# Wrap error to [-pi, pi].
return (error + np.pi) % (2 * np.pi) - np.pi
num_samples = args.num_samples
results = MonteCarloSimulation(make_simulator=make_simulator,
output=calc_wrapped_error,
final_time=1.0,
num_samples=num_samples,
generator=RandomGenerator())
# Compute results.
# The "success" region is fairly large since some "stabilized" trials
# may still be oscillating around the fixed point. Failed examples are
# consistently much farther from the fixed point than this.
binary_results = np.array([abs(res.output) < 0.1 for res in results])
passing_ratio = float(sum(binary_results)) / len(results)
# 95% confidence interval for the passing ratio.
passing_ratio_var = 1.96 * np.sqrt(passing_ratio *
(1. - passing_ratio) / len(results))
print("Monte-Carlo estimated performance across %d samples: "
"%.2f%% +/- %0.2f%%" %
(num_samples, passing_ratio * 100, passing_ratio_var * 100))
# Analytically compute the best possible ROA, for comparison, but
# calculating where the torque needed to lift the pendulum exceeds
# the torque limit.
arm_radius = 0.5
arm_mass = 1.0
# torque = r x f = r * (m * 9.81 * sin(theta))
# theta = asin(torque / (r * m))
if torque_limit <= (arm_radius * arm_mass * 9.81):
roa_half_width = np.arcsin(torque_limit /
(arm_radius * arm_mass * 9.81))
else:
roa_half_width = np.pi
roa_as_fraction_of_state_space = roa_half_width / np.pi
print("Max possible ROA = %0.2f%% of state space, which should"
" match with the above estimate." %
(100 * roa_as_fraction_of_state_space))
import numpy as np
from pydrake.systems.controllers import LinearQuadraticRegulator
# Define the double integrator's state space matrices.
A = np.array([[0, 1], [0, 0]])
B = np.array([[0], [1]])
Q = np.eye(2)
R = np.eye(1)
(K, S) = LinearQuadraticRegulator(A, B, Q, R)
print("K = " + str(K))
print("S = " + str(S))