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 __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 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 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 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 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))