def test_lqr_discrete(self): """Test overloading of lqr operator for discrete time systems""" csys = ct.rss(2, 1, 1) dsys = ct.drss(2, 1, 1) Q = np.eye(2) R = np.eye(1) # Calling with a system versus explicit A, B should be the sam K_csys, S_csys, E_csys = ct.lqr(csys, Q, R) K_expl, S_expl, E_expl = ct.lqr(csys.A, csys.B, Q, R) np.testing.assert_almost_equal(K_csys, K_expl) np.testing.assert_almost_equal(S_csys, S_expl) np.testing.assert_almost_equal(E_csys, E_expl) # Calling lqr() with a discrete time system should call dlqr() K_lqr, S_lqr, E_lqr = ct.lqr(dsys, Q, R) K_dlqr, S_dlqr, E_dlqr = ct.dlqr(dsys, Q, R) np.testing.assert_almost_equal(K_lqr, K_dlqr) np.testing.assert_almost_equal(S_lqr, S_dlqr) np.testing.assert_almost_equal(E_lqr, E_dlqr) # Calling lqr() with no timebase should call lqr() asys = ct.ss(csys.A, csys.B, csys.C, csys.D, dt=None) K_asys, S_asys, E_asys = ct.lqr(asys, Q, R) K_expl, S_expl, E_expl = ct.lqr(csys.A, csys.B, Q, R) np.testing.assert_almost_equal(K_asys, K_expl) np.testing.assert_almost_equal(S_asys, S_expl) np.testing.assert_almost_equal(E_asys, E_expl) # Calling dlqr() with a continuous time system should raise an error with pytest.raises(ControlArgument, match="dsys must be discrete"): K, S, E = ct.dlqr(csys, Q, R)
def gainCallBack(self, req): # start_time = time.time() self.updateModel(req.l) k1, _, _ = lqr(self.sys1, self.Q1, self.R1) k2, _, _ = lqr(self.sys2, self.Q2, self.R2) res = GainResponse(k1.flatten().tolist() + k2.flatten().tolist()) print(res) # rospy.loginfo("--- %s seconds ---" % (time.time() - start_time)) return res
def control(self, dt, x, goal_x, est_params): if hasattr(self, '_N'): K, S, E = control.lqr(self._model.A, self._model.B, self._Q, self._R, self._N) else: K, S, E = control.lqr(self._model.A, self._model.B, self._Q, self._R) self._K = K output = - self._K @ x return output
def getGainK(self): #S = self.getRiccatiSolution() #S1 = self.B.T.dot(S) #if self.N is not None: S1 += self.N.T #K = np.linalg.inv(self.R).dot(S1) if self.N is None: K, S, E = control.lqr(self.A, self.B, self.Q, self.R) else: K, S, E = control.lqr(self.A, self.B, self.Q, self.R, self.N) return K
def test_lqr_integral_discrete(self): # Generate a discrete time system for testing sys = ct.drss(4, 4, 2, strictly_proper=True) sys.C = np.eye(4) # reset output to be full state C_int = np.eye(2, 4) # integrate outputs for first two states nintegrators = C_int.shape[0] # Generate a controller with integral action K, _, _ = ct.lqr(sys, np.eye(sys.nstates + nintegrators), np.eye(sys.ninputs), integral_action=C_int) Kp, Ki = K[:, :sys.nstates], K[:, sys.nstates:] # Create an I/O system for the controller ctrl, clsys = ct.create_statefbk_iosystem(sys, K, integral_action=C_int) # Construct the state space matrices by hand A_ctrl = np.eye(nintegrators) B_ctrl = np.block( [[-C_int, np.zeros((nintegrators, sys.ninputs)), C_int]]) C_ctrl = -K[:, sys.nstates:] D_ctrl = np.block([[Kp, np.eye(nintegrators), -Kp]]) # Check to make sure everything matches assert ct.isdtime(clsys) np.testing.assert_array_almost_equal(ctrl.A, A_ctrl) np.testing.assert_array_almost_equal(ctrl.B, B_ctrl) np.testing.assert_array_almost_equal(ctrl.C, C_ctrl) np.testing.assert_array_almost_equal(ctrl.D, D_ctrl)
def lqr_control(): #ignore the platform mass and interia m = 10 #blue bass mass mw = 0.4 #wheel mass h = 0.8 #center of mass height(roughly) g = 9.8 #gravity coefficient r = 0.1 #wheel radius Iw = 0.15 #moment of inertia of wheel about y Iz = 0.5 * mw * r**2 #moment of inertia of robotbase about z # reference chapter 4 lqr model t1 = t2 = t3 = t4 = t5 = A = np.array([[0,1,0,0,0,0], [t1,0,0,0,0,0], [0,0,0,1,0,0], [t2,0,0,0,0,0], [0,0,0,0,0,1], [0,0,0,0,0,0]]) B = np.array([[0,0], [t3,0], [0,0], [t4,0], [0,0], [0,t5]]) Q = np.eye(6) R = np.eye(2) K,S,E = lqr(A,B,Q,R) print(K) return K
def lqr_control(self, traj, t): """ Get the control input for a given instant, given a desired trajectory Based on a Taylor linearization, so we're assuming the actual location is pretty close to the desired one. """ A = np.array([[ 0, 0, -traj.vdes(t) * np.sin(traj.theta_des(t)), np.cos(traj.theta_des(t)), 0 ], [ 0, 0, traj.vdes(t) * np.cos(traj.theta_des(t)), np.sin(traj.theta_des(t)), 0 ], [0, 0, 0, 0, 1], [0, 0, 0, 0, 0], [0, 0, 0, 0, 0]]) B = np.array([[0, 0], [0, 0], [0, 0], [1, 0], [0, 1]]) Q = np.array([[1, 0, 0, 0, 0], [0, 1, 0, 0, 0], [0, 0, 1, 0, 0], [0, 0, 0, 1, 0], [0, 0, 0, 0, 1]]) R = np.array([[1, 0], [0, 1]]) K, S, E = control.lqr(A, B, Q, R) x = self.current_extended_state(traj, t) u = np.matmul(-K, x) return u
def lqr_sys(A, B, C, D, in_q, r=1, num_q=6): #parameter sys = control.StateSpace(A, B, C, D) Q = np.zeros((num_q, num_q)) R = [r] for i in range(num_q): Q[i, i] = in_q[i] K, solu_R, Eig_sys = control.lqr(sys, Q, r) kr = -1 / np.matmul(np.matmul(C, npl.inv((A - np.matmul(B, K)))), B)[0] pole, eig_v = npl.eig(A - B * K) pole_real = [a.real for a in pole] minpol = -3 + min(pole_real) repol = [] for i in range(num_q): repol.append(minpol + i + 1) repol = np.array(repol) L = scis.place_poles(A.transpose(), C.transpose(), repol).gain_matrix L = L.transpose() upCA = np.column_stack(((A - np.matmul(B, K)), (np.matmul(B, K)))) downCA = np.column_stack((np.zeros((num_q, num_q)), (A - np.matmul(L, C)))) CA = np.row_stack((upCA, downCA)) CB = np.row_stack((B * kr, np.zeros((num_q, 1)))) CC = np.column_stack((C, np.zeros((1, num_q)))) CD = np.array([0]) return CA, CB, CC, CD
def test_lqr(): control_spec = { 'feedback': 'LQR', 'params': { 'feedback': { 'Q': np.array([[1, 0], [0, 1]]), 'R': 1 } } } model = Model(np.array([[0, 1], [1, 0]]), [[0], [1]]) dt = 0.01 x = np.array([[1], [1]]) goal_x = np.array([[0], [0]]) est_params = [] test_controller = controller.Controller(control_spec, model) output = test_controller.control(dt, x, goal_x, est_params) # expected results K, S, E = control.lqr(model.A, model.B, control_spec['params']['feedback']['Q'], control_spec['params']['feedback']['R']) expec_output = -K @ x assert output == expec_output
def policy_rollout(self): A, B = self.get_system() Q = np.eye(self.hidden_dim) R = np.array([[0.01]]) K, _, _ = control.lqr(A, B, Q, R) ref = torch.FloatTensor([[1.0, 0., 0.]]) ref = model.encoder(ref).detach().numpy() obs_old = self.transform_state(self.env.reset()) #obs_old[2] = obs_old[2] / 8.0 for _ in range(100): if np.random.random() > 0.5: state = torch.FloatTensor(obs_old.reshape((1, -1))) y = model.encoder(state).detach().numpy() action = -np.dot(K, (y-ref).T) action = np.clip(np.array([action.item()]), self.env.action_space.low, self.env.action_space.high) else: action = self.env.action_space.sample() * 0.4 #self.env.render() obs, reward, done, info = self.env.step(action) #obs[2] = obs[2] / 8.0 obs = self.transform_state(obs) self.replay_buffer.push((obs_old, action / 2.0, obs)) obs_old = obs
def LQR(plant, x): A = np.matrix( [[0, 1, 0, 0], [0, 0, plant.params['g'] * plant.params['m'] / plant.params['M'], 0], [0, 0, 0, 1], [ 0, 0, (plant.params['M'] + plant.params['m']) * plant.params['g'] / (plant.params['l'] * plant.params['M']), 0 ]]) B = np.matrix([[0], [1 / plant.params['M']], [0], [1 / (plant.params['l'] * plant.params['M'])]]) # The Q and R matrices are emperically tuned. It is described further in the report Q = np.matrix([[1, 0, 0, 0], [0, 0.01, 0, 0], [0, 0, 15, 0], [0, 0, 0, 11]]) R = np.matrix([5]) # The K matrix is calculated using the lqr function from the controls library K, S, E = control.lqr(A, B, Q, R) np.matrix(K) x = np.matrix([[-np.squeeze(np.asarray(x[0]))], [np.squeeze(np.asarray(x[1]))], [np.squeeze(np.asarray(np.arcsin(x[3])))], [-np.squeeze(np.asarray(x[2]))]]) desired = np.matrix([[0], [0], [0], [0]]) F = (K * (x - desired)) return np.array(F)
def clfqp_a(self, X, Xd, Ud): #Xn = np.array([1,np.pi/3,1,np.pi/3]) #An = self.sdcA(Xn) #Bn = self.dynamicsg(Xn) e = X - Xd B = self.dynamicsg(X) Bd = self.dynamicsg(Xd) f = self.dynamicsf(X) + B @ Ud fd = self.dynamicsf(Xd) + Bd @ Ud Q = np.identity(self.n_states) * 10 R = np.identity(self.m_inputs) A = self.sdcA(X) try: K, P, _ = control.lqr(A, B, Q, R) invR = np.linalg.inv(R) phi0 = 2 * e @ P @ (f - fd) + e @ (P @ B @ invR @ B.T @ P + Q) @ e phi1 = (2 * e @ P @ B).T if phi0 > 0: U = -(phi0 / (phi1.T @ phi1)) * phi1 U = Ud + U.ravel() else: U = Ud except: U = Ud print("clfqp infeasible") return U
def callback(self, data): self.K, self.S, self.e = lqr(A, B, self.Q, self.R) y = data.orientation.y * 180 / 3.1416 if y > self.yMax: self.yMax = y elif y < self.yMin: self.yMin = y vel = Twist() diff_yaw = y - self.y_ np_x = np.array([[y], [diff_yaw]]) xvel = self.K.dot(np_x)[0, 0] if xvel > self.xvelMax: self.xvelMax = xvel elif xvel < self.xvelMin: self.xvelMin = xvel vel.linear.x = xvel vel.linear.y = 0 vel.linear.z = 0 vel.angular.x = 0 vel.angular.y = 0 vel.angular.z = 0 self.pub.publish(vel) #print "Max vel " + str(self.xvelMax) + " & Min vel " + str(self.xvelMin) + " Max y " + str(self.yMax*180/3.1416) +" & Min y" + str(self.yMin*180/3.1416) #print "Velocity "+ str(xvel)+ " & yaw " + str(y) self.y_ = y self.pub1.publish(data.orientation.y)
def compute_whipple_lqr_gain(velocity): _, A, B = benchmark_state_space_vs_speed(*benchmark_matrices(), velocity) Q = np.diag([1e5, 1e3, 1e3, 1e2]) R = np.eye(2) gains = [control.lqr(Ai, Bi, Q, R)[0] for Ai, Bi in zip(A, B)] return gains
def update(self): t = self.simulationHandler.currentTime x, dx = self.controlInputStateSpaceHandler.getX_dx(t) q, v, a = self.stateHandler.getPositionVelocityAcceleration() A = self.linearizedModelHandler.getA() B = self.linearizedModelHandler.getB() F = self.constraintsHandler.getJacobian(DM(q)) dF = self.constraintsHandler.getJacobianDerivative(DM(q), DM(v)) G = horzcat(F, dF) N = scipy.linalg.null_space(G) An = N.T @ A @ N Bn = N.T @ B.T Qn = N.T @ self.Q @ N Kn, S, CLP = lqr(An, Bn, Qn, self.R) K = Kn @ N.T e = (self.stateSpaceHandler.x - x) u_FB = -K @ e u_FF = self.inverseDynamicsHandler.u self.u = u_FB + u_FF
def lqr_gain(rotor): A = np.array([ [0, 1, 0, 0, 0, 0], [0, 0, 0, 0, -rotor.g, 0], [0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 1], [0, 0, 0, 0, 0, 0], ], dtype=float) B = np.array([ [0, 0], [0, 0], [0, 0], [1, 1], [0, 0], [-rotor.r / rotor.I, rotor.r / rotor.I], ], dtype=float) Q = np.eye(A.shape[1]) R = np.eye(B.shape[1]) K, _, _ = control.lqr(A, B, Q, R) return K
def diffeomorphism_full_state_control(self): d = 1 xi_bar = np.array([[0, 0, 1]]).T zeta_bar = xi_bar + np.array([[0, 0, d]]).T m = self.M g = self.G R = self.rot_mat x = self.state xi = x[0:3] eta = x[3:6] xi_dot = x[6:9] eta_dot = x[9:12] phi = eta[0] theta = eta[1] psi = eta[2] phi_dot = eta_dot[0] theta_dot = eta_dot[1] psi_dot = eta_dot[2] zeta = (self * rigid.Vector(*(0, 0, -d))).array chi = np.vstack([zeta - zeta_bar, psi, xi_dot, psi_dot]) W = mechanics.att_vel_to_ang_vel_jacobian(phi, theta) W_inv = np.linalg.inv(W) W_dot = mechanics.time_derivative_att_vel_to_ang_vel_jacobian( phi, theta, phi_dot, theta_dot) A = np.hstack( [np.zeros((8, 4)), np.vstack([np.identity(4), np.zeros((4, 4))])]) b = np.array([[-1 / m * R[0, 2], d * R[0, 1], -d * R[0, 0]], [-1 / m * R[1, 2], d * R[1, 1], -d * R[1, 0]], [-1 / m * R[2, 2], d * R[2, 1], -d * R[2, 0]]]) B = np.vstack([ np.zeros((4, 4)), np.hstack([b, np.zeros((3, 1))]), np.array([[0, 0, 0, 1]]) ]) G = np.array([[0, 0, 0, 0, 0, 0, -g, 0]]).T QQ = np.diag([100, 100, 10, 1, 1, 1, 1, 1]) RR = np.diag(np.array([1, 1, 1, 1])) K, _, _ = lqr(A, B, QQ, RR) # mu_bar = np.array([[-g*m*R[2, 2], g/d*R[2, 1], -g/d*R[2, 0], 0]]).T mu_bar = np.array([[g * m, 0, 0, 0]]).T mu_delta = -np.dot(K, chi) mu = mu_bar + mu_delta F = mu[0, 0] alpha = mu[1:4] J = generialized_coordinate_inertia(self.I, phi, theta, psi) C = quad_coriolis(self.I, phi, theta, phi_dot, theta_dot, psi_dot) tau_eta = np.dot(J, np.dot(W_inv, (alpha - np.dot(W_dot, eta_dot)))) + np.dot( C, eta_dot) return np.vstack([F, tau_eta])
def LQRControl(plant, Q, R): plantStateSpace = control.tf2ss(plant) A, B, C, D = control.ssdata(plantStateSpace) print(A, B, C, D) K, P, E = control.lqr(plantStateSpace, Q, R) output = control.ss(A - B * K, B * K, C, D) return control.step_response(output, T)
def lqr(self,x,A,B,Q,R): K, S, E = control.lqr(A, B, Q, R) u = -scipy.linalg.inv(R)*(B.T*(S*x)) #if B.size==2: #print("S",S) #print("x",x) #print("B",self.B) return u
def calculate_LQR_gains(A, B, C, D, Q, R): # calculate LQR gains (K, X, E) = ctl.lqr(A, B, Q, R) # calculate Nu, Nx matrices for including reference input Nu, Nx = getInputMatrices(A, B, C, D) return K, Nu, Nx
def get_linear_quadratic_regulator(linear_model, q=numpy.identity(3), r=numpy.identity(1)): ctrb_matrix = control.ctrb(linear_model.A, linear_model.B) if ctrb_matrix.shape[0] != linear_model.A.shape[0]: raise ValueError("System is not controllable!") k_matrix, s_matrix, e_matrix = control.lqr(linear_model.A, linear_model.B, q, r) return k_matrix, s_matrix, e_matrix
def diffeomorphism_full_state_control(self): d = 1 xi_bar = np.array([[0, 0, 1]]).T zeta_bar = xi_bar + np.array([[0, 0, d]]).T m = self.M g = self.G R = self.rot_mat x = self.state xi = x[0:3] eta = x[3:6] xi_dot = x[6:9] eta_dot = x[9:12] phi = eta[0] theta = eta[1] psi = eta[2] phi_dot = eta_dot[0] theta_dot = eta_dot[1] psi_dot = eta_dot[2] zeta = (self * rigid.Vector(*(0, 0, -d))).array chi = np.vstack([zeta-zeta_bar, psi, xi_dot, psi_dot]) W = mechanics.att_vel_to_ang_vel_jacobian(phi, theta) W_inv = np.linalg.inv(W) W_dot = mechanics.time_derivative_att_vel_to_ang_vel_jacobian(phi, theta, phi_dot, theta_dot) A = np.hstack([np.zeros((8, 4)), np.vstack([np.identity(4), np.zeros((4, 4))])]) b = np.array([[-1/m*R[0, 2], d*R[0, 1], -d*R[0, 0]], [-1/m*R[1, 2], d*R[1, 1], -d*R[1, 0]], [-1/m*R[2, 2], d*R[2, 1], -d*R[2, 0]]]) B = np.vstack([np.zeros((4, 4)), np.hstack([b, np.zeros((3, 1))]), np.array([[0, 0, 0, 1]])]) G = np.array([[0, 0, 0, 0, 0, 0, -g, 0]]).T QQ = np.diag([100, 100, 10, 1, 1, 1, 1, 1]) RR = np.diag(np.array([1, 1, 1, 1])) K,_,_ = lqr(A, B, QQ, RR) # mu_bar = np.array([[-g*m*R[2, 2], g/d*R[2, 1], -g/d*R[2, 0], 0]]).T mu_bar = np.array([[g*m, 0, 0, 0]]).T mu_delta = -np.dot(K, chi) mu = mu_bar + mu_delta F = mu[0, 0] alpha = mu[1:4] J = generialized_coordinate_inertia(self.I, phi, theta, psi) C = quad_coriolis(self.I, phi, theta, phi_dot, theta_dot, psi_dot) tau_eta = np.dot(J, np.dot(W_inv, (alpha - np.dot(W_dot, eta_dot)))) + np.dot(C, eta_dot) return np.vstack([F, tau_eta])
def _test_lqr_random(rng, n): # pylint: disable=import-outside-toplevel import control from jax.tree_util import tree_multimap A, B, Q, R, N = random_env(rng, n) actual = lqr_continuous_time_infinite_horizon(A, B, Q, R, N) expected = control.lqr(A, B, Q, R, N) assert tree_multimap(jp.allclose, actual, expected)
def one_step_sim(self,y): runge_num = self.runge_num A = self.Aekf(self.Z,self.t) R = 1*np.eye(1) Q = 100*np.eye(3) K,P,E = control.lqr(A.T,C.T,Q,R) L = K.T for num in range(0, runge_num) self.Z, self.t = self.rk4(L,y) return self.Z, self.t
def test_discrete_lqr(): # oscillator model defined in 2D # Source: https://www.mpt3.org/UI/RegulationProblem A = [[0.5403, -0.8415], [0.8415, 0.5403]] B = [[-0.4597], [0.8415]] C = [[1, 0]] D = [[0]] # Linear discrete-time model with sample time 1 sys = ct.ss2io(ct.ss(A, B, C, D, 1)) # Include weights on states/inputs Q = np.eye(2) R = 1 K, S, E = ct.lqr(A, B, Q, R) # note: *continuous* time LQR # Compute the integral and terminal cost integral_cost = opt.quadratic_cost(sys, Q, R) terminal_cost = opt.quadratic_cost(sys, S, None) # Formulate finite horizon MPC problem time = np.arange(0, 5, 1) x0 = np.array([1, 1]) optctrl = opt.OptimalControlProblem(sys, time, integral_cost, terminal_cost=terminal_cost) res1 = optctrl.compute_trajectory(x0, return_states=True) with pytest.xfail("discrete LQR not implemented"): # Result should match LQR K, S, E = ct.dlqr(A, B, Q, R) lqr_sys = ct.ss2io(ct.ss(A - B @ K, B, C, D, 1)) _, _, lqr_x = ct.input_output_response(lqr_sys, time, 0, x0, return_x=True) np.testing.assert_almost_equal(res1.states, lqr_x) # Add state and input constraints trajectory_constraints = [ (sp.optimize.LinearConstraint, np.eye(3), [-10, -10, -1], [10, 10, 1]), ] # Re-solve res2 = opt.solve_ocp(sys, time, x0, integral_cost, constraints, terminal_cost=terminal_cost) # Make sure we got a different solution assert np.any(np.abs(res1.inputs - res2.inputs) > 0.1)
def lqr_attitude_control(self): Q_rot = np.diag([80, 80, 2, 1, 1, 1]) R_rot = np.diag(np.array([1000, 1000, 1000])) K_rot,_,_ = lqr(self.A_rot, self.B_rot, Q_rot, R_rot) X_rot = np.vstack([self.state[3:6], self.state[9:12]]) torque_attitude = -np.dot(K_rot, X_rot) force = self.G*self.M return np.vstack([force, torque_attitude])
def __init__(self, plant, state0, t0, Q=None, R=None): A = plant.df(state0, t0) B = plant.control_matrix(state0, t0) Q = Q or np.identity(A.shape[1]) * 1.0 R = R or np.identity(B.shape[1]) * 0.00000001 K, S, E = control.lqr(A, B, Q, R) self.state0 = state0.copy() self.K = K self.S = S self.E = E
def __init__(self, intopic, outtopic, A, B, target=None, Q=None, R=None, realizable=True): if Q is None: Q = np.eye(A.shape[0]) if R is None: R = np.eye(B.shape[1]) self.target_state = np.zeros(A.shape[0]) if target is not None: self.target_state[:target.shape[0]] = target K, S, E = lqr(A,B,Q,R) StateFeedback.__init__(self, intopic, outtopic, K, realizable=realizable)
def LQR(): A = np.array([[0, 1], [g / lp, 0]]) B = np.array([[0], [1]]) Q = np.array([[3000., 0], [0, 1.]]) R = .1 K, S, E = lqr(A, B, Q, R) return np.array(K)
def lqr_attitude_control(self): Q_rot = np.diag([80, 80, 2, 1, 1, 1]) R_rot = np.diag(np.array([1000, 1000, 1000])) K_rot, _, _ = lqr(self.A_rot, self.B_rot, Q_rot, R_rot) X_rot = np.vstack([self.state[3:6], self.state[9:12]]) torque_attitude = -np.dot(K_rot, X_rot) force = self.G * self.M return np.vstack([force, torque_attitude])
def calc_lqr_gain(self): # Choose state and input weights for cost function self.Q = np.eye(4) self.R = 0.0001 # Derive the optimal controller K, S, E = control.lqr(self.A, self.B, self.Q, self.R) # Set gain matrix self.gain[:] = K
def solve_lqr(self, A, B, Q, R): ''' Returns: K: 2-d array : State feedback gains S: 2-d array : Solution to Riccati equation E: 1-d array : Eigenvalues of the closed loop system ''' return control.lqr(A, B, Q, R)
def generateLQRTable(A_table, B_table, Q_table, R_table): count = A_table.shape[0] n = A_table.shape[2] m = B_table.shape[2] K_table = np.zeros((count, m, n)) for i in range(count): K, S, CLP = lqr(A_table[i], B_table[i], Q_table[i], R_table[i]) K_table[i] = K return K_table
def construct(self): d_stat = 9 d_ctrl = self.n_contacts*6 self.A = np.zeros((d_stat,d_stat)) self.A[:3,3:6] = 1./self.mass*np.eye(3) self.A[6:,:3] = self.cross_mat(self.mass*np.array([0., 0., -9.81])) self.B = np.zeros((d_stat,d_ctrl)) for c in range(self.n_contacts): self.B[3:,6*c:6*c+6] = np.eye(6) self.B[6:,6*c:6*c+3] = self.cross_mat(self.cog_to_cont[c]) self.K,self.p,self.e = lqr(self.A,self.B,self.Q,self.R) # dh = Gain*err + h_des self.Gain = np.array(np.matrix(self.B[3:,:])*np.matrix(self.K))
def control_eq(self): self.equilibrium_point = hstack(( 0, pi / 2 * ones(len(self.q) - 1), zeros(len(self.u)) )) equilibrium_dict = dict(zip(self.q + self.u, self.equilibrium_point)) parameter_dict = dict(zip(self.parameters, self.parameter_vals)) # symbolically linearize about arbitrary equilibrium self.linear_state_matrix, self.linear_input_matrix, inputs = self.kane.linearize() # sub in the equilibrium point and the parameters self.f_A_lin = self.linear_state_matrix.subs(parameter_dict).subs(equilibrium_dict) self.f_B_lin = self.linear_input_matrix.subs(parameter_dict).subs(equilibrium_dict) m_mat = self.kane.mass_matrix_full.subs(parameter_dict).subs(equilibrium_dict) # compute A and B from numpy import matrix A = matrix(m_mat.inv() * self.f_A_lin) B = matrix(m_mat.inv() * self.f_B_lin) assert matrix_rank(control.ctrb(A, B)) == A.shape[0] self.K, self.X, self.E = control.lqr(A, B, ones(A.shape), 1);
def calcK(self, Plant): #Calculates the K matrix using python control systems and lqr pole placement #Define A matrix A = [ [0, 1], [-Plant.g / Plant.l, -Plant.b / (Plant.m * (Plant.l ** 2))] ] #Define B matrix B = [ [0], [1 / (Plant.m * (Plant.l ** 2))] ] #define C matrix. It's just the identity C = np.identity(2) #The D matrix is just 0 D = [ [0], [0] ] #Create the statespace representation sys = control.ss(A, B, C, D) #Create the state weight matrix, weight position 20000, and speed 100 Q = [ [20000, 0], [0, 100] ] #Create input weight matrix R = [ [1] ] #Compute LQR LQR_vals = control.lqr(sys, Q, R) return LQR_vals[0] #return K values of LQR return
def lqr_position_attitude_control(self): m = self.M g = self.G q = self.state xi = q[0:3] xi_dot = q[6:9] phi = q[3,0] theta = q[4,0] psi = mmath.wrap_to_2pi(q[5,0]) eta = np.array([[phi, theta, psi]]).T chi = np.vstack([xi, xi_dot]) xi_bar = self.state_desired[0:3] psi_bar = mmath.wrap_to_2pi(self.state_desired[5,0]) xi_dot_bar = self.state_desired[6:9] chi_bar = np.vstack([xi_bar, xi_dot_bar]) chi_delta = (chi - chi_bar) A_pos = np.vstack([np.hstack([np.zeros((3,3)), np.identity(3)]), np.zeros((3, 6))]) B_pos = np.vstack([np.zeros((3, 3)), np.identity(3)]) Q_pos = np.diag([2, 2, 2, 20, 20, 20]) R_pos = np.diag(np.array([5, 5, 5])) K_pos,_,_ = lqr(A_pos, B_pos, Q_pos, R_pos) acc_vec = -np.dot(K_pos, chi_delta) + np.array([[0, 0, g]]).T Z = -acc_vec / np.linalg.norm(acc_vec) a = Z[0, 0]*sin(psi_bar) + Z[1, 0]*cos(psi_bar) b = -Z[2, 0] c = -a if np.allclose(a, 0): X3 = 0 elif abs((-b+sqrt(b**2 - 4*a*c))/(2*a)) <= 1: X3 = (-b+sqrt(b**2 - 4*a*c))/(2*a) else: X3 = (-b-sqrt(b**2 - 4*a*c))/(2*a) d = sqrt(1 - X3**2) if psi_bar >= 0 and psi_bar < PI: X1 = sqrt(d**2 * (sin(psi_bar))**2) else: X1 = -sqrt(d**2 * (sin(psi_bar))**2) if (psi_bar >= 0 and psi_bar < PI/2) or psi_bar >= 3*PI/2: X2 = sqrt(d**2 * (cos(psi_bar))**2) else: X2 = -sqrt(d**2 * (cos(psi_bar))**2) X = np.array([[X1, X2, X3]]).T X = X / np.linalg.norm(X) Y = np.atleast_2d(np.cross(np.squeeze(Z), np.squeeze(X))).T Y = Y / np.linalg.norm(Y) R = np.hstack([X, Y, Z]) (phi_bar, theta_bar, _) = mmath.rot2euler(np.dot(self.nominal_frame.rot_mat, R)) eta_bar = np.array([[phi_bar, theta_bar, psi_bar]]).T eta_delta = eta - eta_bar eta_delta = np.array([[mmath.wrap_to_pi(ang) for ang in eta_delta]]).T Q_rot = np.diag([1, 1, 1, 1, 1, 1]) R_rot = np.diag(np.array([5, 5, 5])) K_rot,_,_ = lqr(self.A_rot, self.B_rot, Q_rot, R_rot) X_rot = np.vstack([eta_delta, self.state[9:12]]) torque_attitude = -np.dot(K_rot, X_rot) force = np.linalg.norm(acc_vec)*m u = np.vstack([force, torque_attitude]) return u
[0, -(I+m*l**2)*b/p, (m**2*g*l**2)/p, 0], [0, 0, 0, 1], [0, -(m*l*b)/p, m*g*l*(M+m)/p, 0]]) B = array([[0], [(I+m*l**2)/p], [0], [m*l/p]]) C = array([[1, 0, 0, 0], [0, 0, 1, 0]]) D = array([[0], [0]]) poles,vect = eig(A) # Print vectors to verify that the system is unstable print poles K = control.place(A,B, [-1,-1,-1,-1]) print 'K(place)=',K Q = C.transpose().dot(C) Q[1,1]=5000 Q[3,3] = 100 R = 1 K,S,E = control.lqr(A,B,Q,R) print 'K(lqr)=',K # New control matrix Ac = A-B.dot(K) poles,vect = eig(Ac) print "Ac poles=", poles