def __init__(self, dt, name, num_stages_per_side, num_motors_per_side, stall_current, free_current, stall_torque, free_speed, gear_ratio_per_side, moment_of_inertia, wheel_radius, drivebase_radius, mass, C = numpy.matrix([[1 , 0, 0, 0], [0,1,0,0]])): """Instantiates the drive template. Units should be SI. Free speed should be radians per second, gear ratio should be greater than one, stall current, free current, stall torque just refer to your motor specs for one of that type which should be found on Vex Motors. The dt refers to the robot's loop speed which as of 2018, is 5 ms (so dt = 0.005), and name refers to what you want to call the subsystem. Drivebase radius is a fudge factor for the effects of the drivetrain on each side, a reasonable approximation is half the length of the drivetrain base.""" state_space_controller.__init__(self, dt, name) #Input values self.N = num_stages_per_side self.n = num_motors_per_side self.I_stall = stall_current * self.n self.I_free = free_current * self.n self.tau_stall = stall_torque * self.n self.omega_free = free_speed self.J = moment_of_inertia self.G = gear_ratio_per_side self.r_w = wheel_radius self.r_b = drivebase_radius self.m = mass #Derived Values #efficiency self.mu = 0.95 ** self.N #resistance self.R = 12.0 / self.I_stall self.k_t = self.I_stall / self.tau_stall self.k_v = (12.0 - self.I_free * self.R) / self.omega_free self.gamma_1 = (1.0 / self.m + self.r_b * self.r_b / self.J)#just placeholder constant so I don't retype all the variables self.gamma_2 = (1.0 / self.m - self.r_b * self.r_b / self.J) #coefficient of velocity self.k_vel = -self.k_v * self.G * self.G / (self.k_t * self.R * self.r_w * self.r_w) #coefficient of input self.k_u = self.mu * self.G / (self.k_t * self.R * self.r_w) #State is [[pos_left],[pos_right],[vel_left],[vel_right]] self.A_c = numpy.matrix([[0, 0, 1.0, 0], [0, 0, 0, 1.0], [0, 0, self.gamma_1 * self.k_vel, self.gamma_2 * self.k_vel], [0 ,0, self.gamma_2 * self.k_vel, self.gamma_1 * self.k_vel]]) self.B_c = numpy.matrix([[0, 0], [0, 0], [self.gamma_1 * self.k_u, self.gamma_2 * self.k_u], [self.gamma_2 * self.k_u, self.gamma_1 * self.k_u]]) self.C = C self.D = numpy.matrix([[0, 0], [0, 0]]) #converts continuous to discrete matrices self.A_d, self.B_d = controls.c2d(self.A_c,self.B_c,self.dt) self.minU = numpy.matrix([[-12.0], [-12.0]]) self.maxU = numpy.matrix([[12.0], [12.0]]) self.initialize_state()
def __init__(self, dt, name, num_stages, num_motors, stall_current, free_current, stall_torque, free_speed, moment_of_inertia, gear_ratio, C=numpy.matrix([[1, 0]])): state_space_controller.__init__(self, dt, name) """Instantiates the arm template. Units should be SI. Free speed should be radians per second, gear ratio should be greater than one, stall current, free current, stall torque just refer to your motor specs for one of that type which should be found on Vex Motors. The dt refers to the robot's loop speed which as of 2018, is 5 ms (so dt = 0.005), and name refers to what you want to call the subsystem.""" #Input values self.N = num_stages self.n = num_motors self.I_stall = stall_current * num_motors self.I_free = free_current * num_motors self.tau_stall = stall_torque * num_motors self.omega_free = free_speed self.J = moment_of_inertia self.G = gear_ratio #Derived Values #efficiency self.mu = 0.95**self.N #resistance self.R = 12.0 / self.I_stall self.k_t = self.I_stall / self.tau_stall self.k_v = (12.0 - self.I_free * self.R) / self.omega_free #coefficient of velocity self.k_omega = -self.k_v * self.G * self.G / (self.k_t * self.R * self.J) #coefficient of input self.k_u = self.mu * self.G / (self.k_t * self.R * self.J) #State is [[angle],[angular_velocity]] self.A_c = numpy.matrix([[0, 1], [0, self.k_omega]]) self.B_c = numpy.matrix([[0], [self.k_u]]) self.C = C self.D = numpy.matrix([[0]]) #converts continuous to discrete matrices self.A_d, self.B_d = controls.c2d(self.A_c, self.B_c, self.dt) self.minU = numpy.matrix([[-12.0]]) self.maxU = numpy.matrix([[12.0]]) self.time_elapsed = 0.0 self.initialize_state()
def ContinuousToDiscrete(self, A_continuous, B_continuous, dt): """Calculates the discrete time values for A and B. Args: A_continuous: numpy.matrix, The continuous time A matrix B_continuous: numpy.matrix, The continuous time B matrix dt: float, The time step of the control loop Returns: (A, B), numpy.matrix, the control matricies. """ return controls.c2d(A_continuous, B_continuous, dt)
def error_u_kalman(plant, Q_c = None, R_c = None, Q_d = None, R_d = None): augmented_system = _augment(plant) if Q_c is not None and R_c is not None: _, _, Q_d, R_d = controls.c2d(augmented_system.A_c, augmented_system.B_c, augmented_system.dt, Q_c, R_c) elif Q_d is not None and R_d is not None: pass else: Q_d = plant.Q_d R_d = plant.R_d augmented_system.Q_c = Q_c augmented_system.R_c = R_c augmented_system.Q_d = Q_d augmented_system.R_d = R_d return state_space_observer.kalman(augmented_system)
def error_u_lqr(plant, u_min = None, u_max = None, Q_c = None, R_c = None, Q_d = None, R_d = None): augmented_system = _augment(plant) if Q_c is not None and R_c is not None: _, _, Q_d, R_d = controls.c2d(augmented_system.A_c, augmented_system.B_c, augmented_system.dt, Q_c, R_c) elif Q_d is not None and R_d is not None: pass else: Q_d = plant.Q_d R_d = plant.R_d augmented_system.Q_c = Q_c augmented_system.R_c = R_c augmented_system.Q_d = Q_d augmented_system.R_d = R_d return state_space_controller.lqr(augmented_system, u_min, u_max)
def set_system(self, dt, A, B, C, D=0, Q=None, R=None): self.A_c = np.asmatrix(A) self.B_c = np.asmatrix(B) self.C = np.asmatrix(C) self.D = np.asmatrix(D) if Q is None: Q = np.zeros(self.A_c.shape) self.Q_c = np.asmatrix(Q) if R is None: R = mat.zeros((self.num_outputs, self.num_outputs)) self.R_c = np.asmatrix(R) self.dt = dt self.A_d, self.B_d, self.Q_d, self.R_d = controls.c2d( self.A_c, self.B_c, dt, self.Q_c, self.R_c)
def main(): rod_weight = 4.0 * 0.293 * 0.453592 radius = 3.0 / 16.0 * 0.0254 pitch = 16.0/ (1 * 0.0254) load = calculate_load_torque(40. * 4.4482, 2.0 * radius, pitch, 0.19) test = linear_actuator(0.005, "test", 1.0, 1.0, 105.0, 1.8, 2.6, 5676 * 2.0 * math.pi / 60.0, 0.5 * rod_weight * radius * radius, 1.0, pitch, load) #test.run_custom_test(numpy.matrix([[0.0],[0]]), numpy.matrix([[24.0],[0]]), max_func, True, False, True, 800) #test.get_stats() best_G = calculate_ideal_gear_ratio(load, 1.3) print(best_G) #Neo @ 1:3 overdrive running at 10 volts works best drive_gear_ratio = 7.083 * 24.0 / 60.0 * 64.0 / 20.0;#1.13333#5.10#50.0 /34.0 2.16 ratio spread 54 : 30 j = 6.0 #2778 2000 2907 3269 3211 3099 3287 3573 2705 #drivetrain = drive(0.005, "drivetrain", 4.0, 2.0, 131.0, 2.7, 2.41, 5330 * 2.0 * math.pi / 60.0, # drive_gear_ratio, j, 3.0 * 0.0254, 28.0 * 0.0254 * 0.5, 154.0 * 0.454) #radius base might be 25.5 * 0.0254 * 0.5, but not likely, more like 0.45 drivetrain = drive(0.005, "drivetrain", 3.0, 2.0, 131.0, 2.7, 2.41, 5330 * 2.0 * math.pi / 60.0, drive_gear_ratio, j, 3.0 * 0.0254, 25.5 * 0.0254 * 0.5 , (125.0) * 0.454) #drivetrain.run_pid_test([1.0, 0.0,0],numpy.matrix([[0.0],[0],[0],[0]]), numpy.matrix([[55.0],[55.0], [0.0],[0.0]]), True, True, 4000) #drivetrain.run_custom_test(numpy.matrix([[0.0],[0],[0],[0]]), numpy.matrix([[12.0],[12.0], [0.0],[0.0]]), full_volts, True, False, True, 2000) drivetrain.get_stats(True) driveQ_c = numpy.matrix([[1.0 / (0.14)**2.0 , 0, 0, 0], [0, 1.0 / (0.14)**2.0, 0, 0], [0, 0, 1.0 / (1.0)**2.0, 0], [0, 0, 0, 1.0 / (1.0)**2.0]]) driveR_c = numpy.matrix([[0.0001**2, 0.0], [0.0, 0.0001 **2]]) # [0 , 1.0 / (.1 * 0.0254)**2]]) driveA_d, driveB_d, driveQ_d, driveR_d = controls.c2d(drivetrain.A_c, drivetrain.B_c, 0.005, driveQ_c, driveR_c) driveK = controls.dlqr(driveA_d, driveB_d, driveQ_d, driveR_d) print(driveK) elev = elevator(0.005, "elev", 2.0, 2.0, 134.0, 0.7, 0.71, 18730.0 * 2.0 * math.pi / 60.0, 63.0, 3.0 * (0.8755 - 0.16) * 0.0254, 18.8* 0.454)#11.59) #elev.export("../python") elevQ_c = numpy.matrix([[1.0 / (0.0254 * 0.01)**2.0 , 0], [0 , 1.0 / (.1 * 0.0254)**2]]) elevR_c = numpy.matrix([[1.0 / 13.0**2]]) elevA_d, elevB_d, elevQ_d, elevR_d = controls.c2d(elev.A_c, elev.B_c, 0.005, elevQ_c, elevR_c) elevK = controls.dlqr(elevA_d, elevB_d, elevQ_d, elevR_d) elevL = controls.dkalman(elevA_d, elevB_d, elevQ_d, elevR_d) #print(elevK) elev.run_test([[0.],[0.]],[[0.0],[60.0]])
def __init__(self, dt, name): ss.StateSpaceController.__init__(self, dt) self.origin = (0, 0) self.time_elapsed = 0.0 self.has_cube = False self.free_speed = 18730.0 * 2.0 * math.pi / 60.0 #radians per second self.free_current = 0.7 #amps self.stall_torque = 0.71 # newton meters self.stall_current = 134.0 self.resistence = 12.0 / self.stall_current self.k_t = self.stall_torque / self.stall_current self.k_v = (12.0 - self.free_current * self.resistence) / self.free_speed self.gear_ratio = 833.33 self.mass = 10.0 * 0.454 #kilograms if (self.has_cube): self.mass += 1.59 self.radius = 17.0 * 0.0254 #meters self.moment_of_inertia = self.mass * self.radius * self.radius #10.0 #this is speculative self.efficiency = 0.65 self.a = (-self.k_t * self.k_v * self.gear_ratio * self.gear_ratio) / (self.moment_of_inertia * self.resistence) self.b = (self.efficiency * self.k_t * self.gear_ratio) / (self.moment_of_inertia * self.resistence) #state is [angle, # angular_velocity] #input is [voltage] self.A_c = numpy.matrix([[0, 1], [0, self.a]]) self.B_c = numpy.matrix([[0], [self.b]]) self.C = numpy.matrix([[1, 0]]) self.D = numpy.matrix([[0]]) q_1 = 1.0 / ((math.pi / 90.0)**2) q_2 = 1.0 / ((math.pi / 90.0)**2) self.Q_c = numpy.matrix([[q_1, 0], [0, q_2]]) r_1 = 1.0 / (0.1**2) self.R_c = numpy.matrix([[r_1]]) self.temp = controls.clqr(self.A_c, self.B_c, self.Q_c, self.R_c) print(controls.eig(self.A_c - self.B_c * self.temp)) self.A_d, self.B_d, self.Q_d, self.R_d = controls.c2d( self.A_c, self.B_c, self.dt, self.Q_c, self.R_c) #s = 0,-295.41341225 are open loop poles #converting s to z is through e^(sT) where T is the discrete time step. #(0.995 , 0.874) these are when s = -1, and s = -27 #tried with poles at 4.2, -295.41341225 poles = (cmath.exp(0.005 * (-27.0)), cmath.exp(0.005 * (-28.0))) polesd = (-27.0 + 3.0j, -27.0 - 3.0j) self.K = controls.place(self.A_d, self.B_d, poles) self.Kff = controls.feedforwards(self.A_d, self.B_d, self.Q_d) #These should be chosen 10 times faster in the s domain, and then converted #to the z domain #(0.951, 0.259) these are when s = -10, s= -270 self.L = controls.dkalman(self.A_d, self.C, self.Q_d, self.R_d) self.minU = numpy.matrix([[-12.0]]) self.maxU = numpy.matrix([[12.0]]) self.initialize_state()
def __init__(self): n = 2 tau_stall = 0.71 * n #N/m I_stall = 134 * n# A omega_free = 18730.0 * 2.0 * math.pi / 60.0 # rad / s V_max = 12.0 #V I_free = 0.7 * n mu = 1.0 G = 63.0 #resistance R = V_max / I_stall #voltage constant k_v = (V_max - I_free * R) / omega_free #torque constant k_t = I_stall / tau_stall #pulley_radius r_p = 0.0254 m = 10.0 * 0.454 A_11 = -(k_v * G * G) / (k_t * R * r_p * r_p * m) B_01 = (G * mu) / (k_t * R * r_p * m) A_c = np.asmatrix([[0.0, 1.0], [0.0, A_11]]) B_c = np.asmatrix([[0.0], [B_01]]) C = np.asmatrix([[1.0, 0.0]]) D = np.asmatrix([[0.0]]) Q_lqr = np.asmatrix([[3.0 ** 2, 0.0], [0.0, 1.5 ** 2]]) Q_kal = np.asmatrix([[.01 ** 2, 0.0], [0.0, .01 ** 2]]) R = np.asmatrix([[0.001 ** 2]]) self.minU = np.asmatrix([[-V_max]]) self.maxU = np.asmatrix([[V_max]]) A, B, Q_d, R_d = controls.c2d(A_c, B_c, 0.005, Q_lqr, R) A, B, Q_kalman, R_d = controls.c2d(A_c, B_c, 0.005, Q_kal, R) self.dt = 0.005 self.controller = ss.state_space_controller(A, B, C, D) #pole placement #self.K = controls.place(A, B, (0.7, 0.1)) #self.Kff = controls.feedforwards(A, B) #self.controller.L = controls.place(A.T, C.T, (-0.61939275, 0.98497246)).T #LQG self.K = controls.dlqr(A, B, Q_d, R_d) print(np.linalg.eig(A - B * self.K)) self.Kff = controls.feedforwards(A, B, Q_d) self.controller.L = controls.dkalman(A, C, Q_kalman, R_d) print(np.linalg.eig(A.T - C.T * self.controller.L.T))
def __init__(self, name="RawClaw"): super(Claw, self).__init__(name) # Stall Torque in N m self.stall_torque = 2.42 # Stall Current in Amps self.stall_current = 133 # Free Speed in RPM self.free_speed = 5500.0 # Free Current in Amps self.free_current = 2.7 # Moment of inertia of the claw in kg m^2 self.J_top = 2.8 self.J_bottom = 3.0 # Resistance of the motor self.R = 12.0 / self.stall_current # Motor velocity constant self.Kv = (self.free_speed / 60.0 * 2.0 * numpy.pi) / (13.5 - self.R * self.free_current) # Torque constant self.Kt = self.stall_torque / self.stall_current # Gear ratio self.G = 14.0 / 48.0 * 18.0 / 32.0 * 18.0 / 66.0 * 12.0 / 60.0 # Control loop time step self.dt = 0.01 # State is [bottom position, bottom velocity, top - bottom position, # top - bottom velocity] # Input is [bottom power, top power - bottom power * J_top / J_bottom] # Motor time constants. difference_bottom refers to the constant for how the # bottom velocity affects the difference of the top and bottom velocities. self.common_motor_constant = -self.Kt / self.Kv / (self.G * self.G * self.R) self.bottom_bottom = self.common_motor_constant / self.J_bottom self.difference_bottom = -self.common_motor_constant * (1 / self.J_bottom - 1 / self.J_top) self.difference_difference = self.common_motor_constant / self.J_top # State feedback matrices self.A_continuous = numpy.matrix( [ [0, 0, 1, 0], [0, 0, 0, 1], [0, 0, self.bottom_bottom, 0], [0, 0, self.difference_bottom, self.difference_difference], ] ) self.A_bottom_cont = numpy.matrix([[0, 1], [0, self.bottom_bottom]]) self.A_diff_cont = numpy.matrix([[0, 1], [0, self.difference_difference]]) self.motor_feedforward = self.Kt / (self.G * self.R) self.motor_feedforward_bottom = self.motor_feedforward / self.J_bottom self.motor_feedforward_difference = self.motor_feedforward / self.J_top self.motor_feedforward_difference_bottom = self.motor_feedforward * (1 / self.J_bottom - 1 / self.J_top) self.B_continuous = numpy.matrix( [ [0, 0], [0, 0], [self.motor_feedforward_bottom, 0], [-self.motor_feedforward_bottom, self.motor_feedforward_difference], ] ) print "Cont X_ss", self.motor_feedforward / -self.common_motor_constant self.B_bottom_cont = numpy.matrix([[0], [self.motor_feedforward_bottom]]) self.B_diff_cont = numpy.matrix([[0], [self.motor_feedforward_difference]]) self.C = numpy.matrix([[1, 0, 0, 0], [1, 1, 0, 0]]) self.D = numpy.matrix([[0, 0], [0, 0]]) self.A, self.B = self.ContinuousToDiscrete(self.A_continuous, self.B_continuous, self.dt) self.A_bottom, self.B_bottom = controls.c2d(self.A_bottom_cont, self.B_bottom_cont, self.dt) self.A_diff, self.B_diff = controls.c2d(self.A_diff_cont, self.B_diff_cont, self.dt) self.K_bottom = controls.dplace(self.A_bottom, self.B_bottom, [0.75 + 0.1j, 0.75 - 0.1j]) self.K_diff = controls.dplace(self.A_diff, self.B_diff, [0.75 + 0.1j, 0.75 - 0.1j]) print "K_diff", self.K_diff print "K_bottom", self.K_bottom print "A" print self.A print "B" print self.B self.Q = numpy.matrix( [ [(1.0 / (0.10 ** 2.0)), 0.0, 0.0, 0.0], [0.0, (1.0 / (0.06 ** 2.0)), 0.0, 0.0], [0.0, 0.0, 0.10, 0.0], [0.0, 0.0, 0.0, 0.1], ] ) self.R = numpy.matrix([[(1.0 / (40.0 ** 2.0)), 0.0], [0.0, (1.0 / (5.0 ** 2.0))]]) # self.K = controls.dlqr(self.A, self.B, self.Q, self.R) self.K = numpy.matrix( [[self.K_bottom[0, 0], 0.0, self.K_bottom[0, 1], 0.0], [0.0, self.K_diff[0, 0], 0.0, self.K_diff[0, 1]]] ) # Compute the feed forwards aceleration term. self.K[1, 0] = -self.B[1, 0] * self.K[0, 0] / self.B[1, 1] lstsq_A = numpy.identity(2) lstsq_A[0, :] = self.B[1, :] lstsq_A[1, :] = self.B[3, :] print "System of Equations coefficients:" print lstsq_A print "det", numpy.linalg.det(lstsq_A) out_x = numpy.linalg.lstsq(lstsq_A, numpy.matrix([[self.A[1, 2]], [self.A[3, 2]]]))[0] self.K[1, 2] = -lstsq_A[0, 0] * (self.K[0, 2] - out_x[0]) / lstsq_A[0, 1] + out_x[1] print "K unaugmented" print self.K print "B * K unaugmented" print self.B * self.K F = self.A - self.B * self.K print "A - B * K unaugmented" print F print "eigenvalues" print numpy.linalg.eig(F)[0] self.rpl = 0.05 self.ipl = 0.010 self.PlaceObserverPoles( [self.rpl + 1j * self.ipl, self.rpl + 1j * self.ipl, self.rpl - 1j * self.ipl, self.rpl - 1j * self.ipl] ) # The box formed by U_min and U_max must encompass all possible values, # or else Austin's code gets angry. self.U_max = numpy.matrix([[12.0], [12.0]]) self.U_min = numpy.matrix([[-12.0], [-12.0]]) # For the tests that check the limits, these are (upper, lower) for both # claws. self.hard_pos_limits = None self.pos_limits = None # Compute the steady state velocities for a given applied voltage. # The top and bottom of the claw should spin at the same rate if the # physics is right. X_ss = numpy.matrix([[0], [0], [0.0], [0]]) U = numpy.matrix([[1.0], [1.0]]) A = self.A B = self.B # X_ss[2, 0] = X_ss[2, 0] * A[2, 2] + B[2, 0] * U[0, 0] X_ss[2, 0] = 1 / (1 - A[2, 2]) * B[2, 0] * U[0, 0] # X_ss[3, 0] = X_ss[3, 0] * A[3, 3] + X_ss[2, 0] * A[3, 2] + B[3, 0] * U[0, 0] + B[3, 1] * U[1, 0] # X_ss[3, 0] * (1 - A[3, 3]) = X_ss[2, 0] * A[3, 2] + B[3, 0] * U[0, 0] + B[3, 1] * U[1, 0] X_ss[3, 0] = 1 / (1 - A[3, 3]) * (X_ss[2, 0] * A[3, 2] + B[3, 1] * U[1, 0] + B[3, 0] * U[0, 0]) # X_ss[3, 0] = 1 / (1 - A[3, 3]) / (1 - A[2, 2]) * B[2, 0] * U[0, 0] * A[3, 2] + B[3, 0] * U[0, 0] + B[3, 1] * U[1, 0] X_ss[0, 0] = A[0, 2] * X_ss[2, 0] + B[0, 0] * U[0, 0] X_ss[1, 0] = A[1, 2] * X_ss[2, 0] + A[1, 3] * X_ss[3, 0] + B[1, 0] * U[0, 0] + B[1, 1] * U[1, 0] print "X_ss", X_ss self.InitializeState()
C = np.asmatrix([1, 0]) D = np.asmatrix([0]) dt = .01 x_initial = np.asmatrix([0, 0]).T # Controller weighting factors Q_c = np.asmatrix([[10, 0], [0, 100]]) R_c = np.asmatrix([[1]]) # Noise properties Q_o = np.asmatrix([[1, 0], [0, 1]]) R_o = np.asmatrix([[0.3]]) # Calculate gain matrices and discrete-time matrices A_d, B_d, Q_d, R_d = controls.c2d(A, B, dt, Q_o, R_o) K = controls.clqr(A, B, Q_c, R_c) Kff = controls.feedforwards(A_d, B_d) L = controls.dkalman(A_d, C, Q_d, R_d) # Create a single set of gains gains = StateSpaceGains('test_gains', dt, A_d, B_d, C, D, Q_d, R_o, K, Kff, L) gains.add_writable_constant('A_c', A) gains.add_writable_constant('B_c', B) u_max = np.asmatrix([[12]]) plant = StateSpacePlant(gains, x_initial) controller = StateSpaceController(gains, -u_max, u_max) observer = StateSpaceObserver(gains, x_initial)