Austin Schuh | 085eab9 | 2020-11-26 13:54:51 -0800 | [diff] [blame] | 1 | #!/usr/bin/python3 |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 2 | |
| 3 | import numpy |
| 4 | from frc971.control_loops.python import polytope |
| 5 | import frc971.control_loops.python.drivetrain |
| 6 | from frc971.control_loops.python import control_loop |
| 7 | from frc971.control_loops.python import controls |
| 8 | from frc971.control_loops.python.cim import CIM |
| 9 | from matplotlib import pylab |
| 10 | |
| 11 | import glog |
| 12 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 13 | |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 14 | def CoerceGoal(region, K, w, R): |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 15 | """Intersects a line with a region, and finds the closest point to R. |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 16 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 17 | Finds a point that is closest to R inside the region, and on the line |
| 18 | defined by K X = w. If it is not possible to find a point on the line, |
| 19 | finds a point that is inside the region and closest to the line. This |
| 20 | function assumes that |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 21 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 22 | Args: |
| 23 | region: HPolytope, the valid goal region. |
| 24 | K: numpy.matrix (2 x 1), the matrix for the equation [K1, K2] [x1; x2] = w |
| 25 | w: float, the offset in the equation above. |
| 26 | R: numpy.matrix (2 x 1), the point to be closest to. |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 27 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 28 | Returns: |
| 29 | numpy.matrix (2 x 1), the point. |
| 30 | """ |
| 31 | return DoCoerceGoal(region, K, w, R)[0] |
| 32 | |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 33 | |
| 34 | def DoCoerceGoal(region, K, w, R): |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 35 | if region.IsInside(R): |
| 36 | return (R, True) |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 37 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 38 | perpendicular_vector = K.T / numpy.linalg.norm(K) |
| 39 | parallel_vector = numpy.matrix([[perpendicular_vector[1, 0]], |
| 40 | [-perpendicular_vector[0, 0]]]) |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 41 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 42 | # We want to impose the constraint K * X = w on the polytope H * X <= k. |
| 43 | # We do this by breaking X up into parallel and perpendicular components to |
| 44 | # the half plane. This gives us the following equation. |
| 45 | # |
| 46 | # parallel * (parallel.T \dot X) + perpendicular * (perpendicular \dot X)) = X |
| 47 | # |
| 48 | # Then, substitute this into the polytope. |
| 49 | # |
| 50 | # H * (parallel * (parallel.T \dot X) + perpendicular * (perpendicular \dot X)) <= k |
| 51 | # |
| 52 | # Substitute K * X = w |
| 53 | # |
| 54 | # H * parallel * (parallel.T \dot X) + H * perpendicular * w <= k |
| 55 | # |
| 56 | # Move all the knowns to the right side. |
| 57 | # |
| 58 | # H * parallel * ([parallel1 parallel2] * X) <= k - H * perpendicular * w |
| 59 | # |
| 60 | # Let t = parallel.T \dot X, the component parallel to the surface. |
| 61 | # |
| 62 | # H * parallel * t <= k - H * perpendicular * w |
| 63 | # |
| 64 | # This is a polytope which we can solve, and use to figure out the range of X |
| 65 | # that we care about! |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 66 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 67 | t_poly = polytope.HPolytope(region.H * parallel_vector, |
| 68 | region.k - region.H * perpendicular_vector * w) |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 69 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 70 | vertices = t_poly.Vertices() |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 71 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 72 | if vertices.shape[0]: |
| 73 | # The region exists! |
| 74 | # Find the closest vertex |
| 75 | min_distance = numpy.infty |
| 76 | closest_point = None |
| 77 | for vertex in vertices: |
| 78 | point = parallel_vector * vertex + perpendicular_vector * w |
| 79 | length = numpy.linalg.norm(R - point) |
| 80 | if length < min_distance: |
| 81 | min_distance = length |
| 82 | closest_point = point |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 83 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 84 | return (closest_point, True) |
| 85 | else: |
| 86 | # Find the vertex of the space that is closest to the line. |
| 87 | region_vertices = region.Vertices() |
| 88 | min_distance = numpy.infty |
| 89 | closest_point = None |
| 90 | for vertex in region_vertices: |
| 91 | point = vertex.T |
| 92 | length = numpy.abs((perpendicular_vector.T * point)[0, 0]) |
| 93 | if length < min_distance: |
| 94 | min_distance = length |
| 95 | closest_point = point |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 96 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 97 | return (closest_point, False) |
| 98 | |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 99 | |
| 100 | class VelocityDrivetrainModel(control_loop.ControlLoop): |
Ravago Jones | 5127ccc | 2022-07-31 16:32:45 -0700 | [diff] [blame] | 101 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 102 | def __init__(self, |
| 103 | drivetrain_params, |
| 104 | left_low=True, |
| 105 | right_low=True, |
| 106 | name="VelocityDrivetrainModel"): |
| 107 | super(VelocityDrivetrainModel, self).__init__(name) |
| 108 | self._drivetrain = frc971.control_loops.python.drivetrain.Drivetrain( |
| 109 | left_low=left_low, |
| 110 | right_low=right_low, |
| 111 | drivetrain_params=drivetrain_params) |
| 112 | self.dt = drivetrain_params.dt |
Ravago Jones | 5127ccc | 2022-07-31 16:32:45 -0700 | [diff] [blame] | 113 | self.A_continuous = numpy.matrix([[ |
| 114 | self._drivetrain.A_continuous[1, 1], |
| 115 | self._drivetrain.A_continuous[1, 3] |
| 116 | ], |
| 117 | [ |
| 118 | self._drivetrain.A_continuous[3, |
| 119 | 1], |
| 120 | self._drivetrain.A_continuous[3, |
| 121 | 3] |
| 122 | ]]) |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 123 | |
Ravago Jones | 5127ccc | 2022-07-31 16:32:45 -0700 | [diff] [blame] | 124 | self.B_continuous = numpy.matrix([[ |
| 125 | self._drivetrain.B_continuous[1, 0], |
| 126 | self._drivetrain.B_continuous[1, 1] |
| 127 | ], |
| 128 | [ |
| 129 | self._drivetrain.B_continuous[3, |
| 130 | 0], |
| 131 | self._drivetrain.B_continuous[3, |
| 132 | 1] |
| 133 | ]]) |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 134 | self.C = numpy.matrix(numpy.eye(2)) |
| 135 | self.D = numpy.matrix(numpy.zeros((2, 2))) |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 136 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 137 | self.A, self.B = self.ContinuousToDiscrete(self.A_continuous, |
| 138 | self.B_continuous, self.dt) |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 139 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 140 | # FF * X = U (steady state) |
| 141 | self.FF = self.B.I * (numpy.eye(2) - self.A) |
Austin Schuh | 7442515 | 2018-12-21 11:37:14 +1100 | [diff] [blame] | 142 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 143 | self.PlaceControllerPoles(drivetrain_params.controller_poles) |
| 144 | # Build a kalman filter for the velocity. We don't care what the gains |
| 145 | # are, but the hybrid kalman filter that we want to write to disk to get |
| 146 | # access to A_continuous and B_continuous needs this for completeness. |
Ravago Jones | 5127ccc | 2022-07-31 16:32:45 -0700 | [diff] [blame] | 147 | self.Q_continuous = numpy.matrix([[(0.5**2.0), 0.0], [0.0, |
| 148 | (0.5**2.0)]]) |
| 149 | self.R_continuous = numpy.matrix([[(0.1**2.0), 0.0], [0.0, |
| 150 | (0.1**2.0)]]) |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 151 | self.PlaceObserverPoles(drivetrain_params.observer_poles) |
Ravago Jones | 5127ccc | 2022-07-31 16:32:45 -0700 | [diff] [blame] | 152 | _, _, self.Q, self.R = controls.kalmd(A_continuous=self.A_continuous, |
| 153 | B_continuous=self.B_continuous, |
| 154 | Q_continuous=self.Q_continuous, |
| 155 | R_continuous=self.R_continuous, |
| 156 | dt=self.dt) |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 157 | |
Ravago Jones | 5127ccc | 2022-07-31 16:32:45 -0700 | [diff] [blame] | 158 | self.KalmanGain, self.P_steady_state = controls.kalman(A=self.A, |
| 159 | B=self.B, |
| 160 | C=self.C, |
| 161 | Q=self.Q, |
| 162 | R=self.R) |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 163 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 164 | self.G_high = self._drivetrain.G_high |
| 165 | self.G_low = self._drivetrain.G_low |
| 166 | self.resistance = self._drivetrain.resistance |
| 167 | self.r = self._drivetrain.r |
| 168 | self.Kv = self._drivetrain.Kv |
| 169 | self.Kt = self._drivetrain.Kt |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 170 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 171 | self.U_max = self._drivetrain.U_max |
| 172 | self.U_min = self._drivetrain.U_min |
James Kuszmaul | 9ea3ff9 | 2024-06-14 15:02:15 -0700 | [diff] [blame] | 173 | self.wrap_point = numpy.matrix(numpy.zeros((2, 1))) |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 174 | |
| 175 | @property |
| 176 | def robot_radius_l(self): |
| 177 | return self._drivetrain.robot_radius_l |
| 178 | |
| 179 | @property |
| 180 | def robot_radius_r(self): |
| 181 | return self._drivetrain.robot_radius_r |
| 182 | |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 183 | |
| 184 | class VelocityDrivetrain(object): |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 185 | HIGH = 'high' |
| 186 | LOW = 'low' |
| 187 | SHIFTING_UP = 'up' |
| 188 | SHIFTING_DOWN = 'down' |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 189 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 190 | def __init__(self, drivetrain_params, name='VelocityDrivetrain'): |
| 191 | self.drivetrain_low_low = VelocityDrivetrainModel( |
| 192 | left_low=True, |
| 193 | right_low=True, |
| 194 | name=name + 'LowLow', |
| 195 | drivetrain_params=drivetrain_params) |
| 196 | self.drivetrain_low_high = VelocityDrivetrainModel( |
| 197 | left_low=True, |
| 198 | right_low=False, |
| 199 | name=name + 'LowHigh', |
| 200 | drivetrain_params=drivetrain_params) |
| 201 | self.drivetrain_high_low = VelocityDrivetrainModel( |
| 202 | left_low=False, |
| 203 | right_low=True, |
| 204 | name=name + 'HighLow', |
| 205 | drivetrain_params=drivetrain_params) |
| 206 | self.drivetrain_high_high = VelocityDrivetrainModel( |
| 207 | left_low=False, |
| 208 | right_low=False, |
| 209 | name=name + 'HighHigh', |
| 210 | drivetrain_params=drivetrain_params) |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 211 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 212 | # X is [lvel, rvel] |
| 213 | self.X = numpy.matrix([[0.0], [0.0]]) |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 214 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 215 | self.U_poly = polytope.HPolytope( |
| 216 | numpy.matrix([[1, 0], [-1, 0], [0, 1], [0, -1]]), |
| 217 | numpy.matrix([[12], [12], [12], [12]])) |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 218 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 219 | self.U_max = numpy.matrix([[12.0], [12.0]]) |
| 220 | self.U_min = numpy.matrix([[-12.0000000000], [-12.0000000000]]) |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 221 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 222 | self.dt = 0.00505 |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 223 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 224 | self.R = numpy.matrix([[0.0], [0.0]]) |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 225 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 226 | self.U_ideal = numpy.matrix([[0.0], [0.0]]) |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 227 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 228 | # ttrust is the comprimise between having full throttle negative inertia, |
| 229 | # and having no throttle negative inertia. A value of 0 is full throttle |
| 230 | # inertia. A value of 1 is no throttle negative inertia. |
| 231 | self.ttrust = 1.0 |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 232 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 233 | self.left_gear = VelocityDrivetrain.LOW |
| 234 | self.right_gear = VelocityDrivetrain.LOW |
| 235 | self.left_shifter_position = 0.0 |
| 236 | self.right_shifter_position = 0.0 |
| 237 | self.left_cim = CIM() |
| 238 | self.right_cim = CIM() |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 239 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 240 | def IsInGear(self, gear): |
| 241 | return gear is VelocityDrivetrain.HIGH or gear is VelocityDrivetrain.LOW |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 242 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 243 | def MotorRPM(self, shifter_position, velocity): |
| 244 | if shifter_position > 0.5: |
| 245 | return (velocity / self.CurrentDrivetrain().G_high / |
| 246 | self.CurrentDrivetrain().r) |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 247 | else: |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 248 | return (velocity / self.CurrentDrivetrain().G_low / |
| 249 | self.CurrentDrivetrain().r) |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 250 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 251 | def CurrentDrivetrain(self): |
| 252 | if self.left_shifter_position > 0.5: |
| 253 | if self.right_shifter_position > 0.5: |
| 254 | return self.drivetrain_high_high |
| 255 | else: |
| 256 | return self.drivetrain_high_low |
| 257 | else: |
| 258 | if self.right_shifter_position > 0.5: |
| 259 | return self.drivetrain_low_high |
| 260 | else: |
| 261 | return self.drivetrain_low_low |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 262 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 263 | def SimShifter(self, gear, shifter_position): |
| 264 | if gear is VelocityDrivetrain.HIGH or gear is VelocityDrivetrain.SHIFTING_UP: |
| 265 | shifter_position = min(shifter_position + 0.5, 1.0) |
| 266 | else: |
| 267 | shifter_position = max(shifter_position - 0.5, 0.0) |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 268 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 269 | if shifter_position == 1.0: |
| 270 | gear = VelocityDrivetrain.HIGH |
| 271 | elif shifter_position == 0.0: |
| 272 | gear = VelocityDrivetrain.LOW |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 273 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 274 | return gear, shifter_position |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 275 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 276 | def ComputeGear(self, |
| 277 | wheel_velocity, |
| 278 | should_print=False, |
| 279 | current_gear=False, |
| 280 | gear_name=None): |
| 281 | high_omega = (wheel_velocity / self.CurrentDrivetrain().G_high / |
| 282 | self.CurrentDrivetrain().r) |
| 283 | low_omega = (wheel_velocity / self.CurrentDrivetrain().G_low / |
| 284 | self.CurrentDrivetrain().r) |
| 285 | #print gear_name, "Motor Energy Difference.", 0.5 * 0.000140032647 * (low_omega * low_omega - high_omega * high_omega), "joules" |
Ravago Jones | 5127ccc | 2022-07-31 16:32:45 -0700 | [diff] [blame] | 286 | high_torque = ((12.0 - high_omega / self.CurrentDrivetrain().Kv) * |
| 287 | self.CurrentDrivetrain().Kt / |
| 288 | self.CurrentDrivetrain().resistance) |
| 289 | low_torque = ((12.0 - low_omega / self.CurrentDrivetrain().Kv) * |
| 290 | self.CurrentDrivetrain().Kt / |
| 291 | self.CurrentDrivetrain().resistance) |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 292 | high_power = high_torque * high_omega |
| 293 | low_power = low_torque * low_omega |
| 294 | #if should_print: |
| 295 | # print gear_name, "High omega", high_omega, "Low omega", low_omega |
| 296 | # print gear_name, "High torque", high_torque, "Low torque", low_torque |
| 297 | # print gear_name, "High power", high_power, "Low power", low_power |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 298 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 299 | # Shift algorithm improvements. |
| 300 | # TODO(aschuh): |
| 301 | # It takes time to shift. Shifting down for 1 cycle doesn't make sense |
| 302 | # because you will end up slower than without shifting. Figure out how |
| 303 | # to include that info. |
| 304 | # If the driver is still in high gear, but isn't asking for the extra power |
| 305 | # from low gear, don't shift until he asks for it. |
| 306 | goal_gear_is_high = high_power > low_power |
| 307 | #goal_gear_is_high = True |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 308 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 309 | if not self.IsInGear(current_gear): |
| 310 | glog.debug('%s Not in gear.', gear_name) |
| 311 | return current_gear |
| 312 | else: |
| 313 | is_high = current_gear is VelocityDrivetrain.HIGH |
| 314 | if is_high != goal_gear_is_high: |
| 315 | if goal_gear_is_high: |
| 316 | glog.debug('%s Shifting up.', gear_name) |
| 317 | return VelocityDrivetrain.SHIFTING_UP |
| 318 | else: |
| 319 | glog.debug('%s Shifting down.', gear_name) |
| 320 | return VelocityDrivetrain.SHIFTING_DOWN |
| 321 | else: |
| 322 | return current_gear |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 323 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 324 | def FilterVelocity(self, throttle): |
| 325 | # Invert the plant to figure out how the velocity filter would have to work |
| 326 | # out in order to filter out the forwards negative inertia. |
| 327 | # This math assumes that the left and right power and velocity are equal. |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 328 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 329 | # The throttle filter should filter such that the motor in the highest gear |
| 330 | # should be controlling the time constant. |
| 331 | # Do this by finding the index of FF that has the lowest value, and computing |
| 332 | # the sums using that index. |
| 333 | FF_sum = self.CurrentDrivetrain().FF.sum(axis=1) |
| 334 | min_FF_sum_index = numpy.argmin(FF_sum) |
| 335 | min_FF_sum = FF_sum[min_FF_sum_index, 0] |
| 336 | min_K_sum = self.CurrentDrivetrain().K[min_FF_sum_index, :].sum() |
| 337 | # Compute the FF sum for high gear. |
| 338 | high_min_FF_sum = self.drivetrain_high_high.FF[0, :].sum() |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 339 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 340 | # U = self.K[0, :].sum() * (R - x_avg) + self.FF[0, :].sum() * R |
| 341 | # throttle * 12.0 = (self.K[0, :].sum() + self.FF[0, :].sum()) * R |
| 342 | # - self.K[0, :].sum() * x_avg |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 343 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 344 | # R = (throttle * 12.0 + self.K[0, :].sum() * x_avg) / |
| 345 | # (self.K[0, :].sum() + self.FF[0, :].sum()) |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 346 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 347 | # U = (K + FF) * R - K * X |
| 348 | # (K + FF) ^-1 * (U + K * X) = R |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 349 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 350 | # Scale throttle by min_FF_sum / high_min_FF_sum. This will make low gear |
| 351 | # have the same velocity goal as high gear, and so that the robot will hold |
| 352 | # the same speed for the same throttle for all gears. |
| 353 | adjusted_ff_voltage = numpy.clip( |
| 354 | throttle * 12.0 * min_FF_sum / high_min_FF_sum, -12.0, 12.0) |
| 355 | return ((adjusted_ff_voltage + self.ttrust * min_K_sum * |
| 356 | (self.X[0, 0] + self.X[1, 0]) / 2.0) / |
| 357 | (self.ttrust * min_K_sum + min_FF_sum)) |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 358 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 359 | def Update(self, throttle, steering): |
| 360 | # Shift into the gear which sends the most power to the floor. |
| 361 | # This is the same as sending the most torque down to the floor at the |
| 362 | # wheel. |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 363 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 364 | self.left_gear = self.right_gear = True |
| 365 | if True: |
Ravago Jones | 5127ccc | 2022-07-31 16:32:45 -0700 | [diff] [blame] | 366 | self.left_gear = self.ComputeGear(self.X[0, 0], |
| 367 | should_print=True, |
| 368 | current_gear=self.left_gear, |
| 369 | gear_name="left") |
| 370 | self.right_gear = self.ComputeGear(self.X[1, 0], |
| 371 | should_print=True, |
| 372 | current_gear=self.right_gear, |
| 373 | gear_name="right") |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 374 | if self.IsInGear(self.left_gear): |
Ravago Jones | 5127ccc | 2022-07-31 16:32:45 -0700 | [diff] [blame] | 375 | self.left_cim.X[0, |
| 376 | 0] = self.MotorRPM(self.left_shifter_position, |
| 377 | self.X[0, 0]) |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 378 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 379 | if self.IsInGear(self.right_gear): |
| 380 | self.right_cim.X[0, 0] = self.MotorRPM( |
| 381 | self.right_shifter_position, self.X[0, 0]) |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 382 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 383 | if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear): |
| 384 | # Filter the throttle to provide a nicer response. |
| 385 | fvel = self.FilterVelocity(throttle) |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 386 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 387 | # Constant radius means that angualar_velocity / linear_velocity = constant. |
| 388 | # Compute the left and right velocities. |
| 389 | steering_velocity = numpy.abs(fvel) * steering |
| 390 | left_velocity = fvel - steering_velocity |
| 391 | right_velocity = fvel + steering_velocity |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 392 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 393 | # Write this constraint in the form of K * R = w |
| 394 | # angular velocity / linear velocity = constant |
| 395 | # (left - right) / (left + right) = constant |
| 396 | # left - right = constant * left + constant * right |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 397 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 398 | # (fvel - steering * numpy.abs(fvel) - fvel - steering * numpy.abs(fvel)) / |
| 399 | # (fvel - steering * numpy.abs(fvel) + fvel + steering * numpy.abs(fvel)) = |
| 400 | # constant |
| 401 | # (- 2 * steering * numpy.abs(fvel)) / (2 * fvel) = constant |
| 402 | # (-steering * sign(fvel)) = constant |
| 403 | # (-steering * sign(fvel)) * (left + right) = left - right |
| 404 | # (steering * sign(fvel) + 1) * left + (steering * sign(fvel) - 1) * right = 0 |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 405 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 406 | equality_k = numpy.matrix([[ |
| 407 | 1 + steering * numpy.sign(fvel), |
| 408 | -(1 - steering * numpy.sign(fvel)) |
| 409 | ]]) |
| 410 | equality_w = 0.0 |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 411 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 412 | self.R[0, 0] = left_velocity |
| 413 | self.R[1, 0] = right_velocity |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 414 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 415 | # Construct a constraint on R by manipulating the constraint on U |
| 416 | # Start out with H * U <= k |
| 417 | # U = FF * R + K * (R - X) |
| 418 | # H * (FF * R + K * R - K * X) <= k |
| 419 | # H * (FF + K) * R <= k + H * K * X |
| 420 | R_poly = polytope.HPolytope( |
| 421 | self.U_poly.H * |
| 422 | (self.CurrentDrivetrain().K + self.CurrentDrivetrain().FF), |
| 423 | self.U_poly.k + |
| 424 | self.U_poly.H * self.CurrentDrivetrain().K * self.X) |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 425 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 426 | # Limit R back inside the box. |
| 427 | self.boxed_R = CoerceGoal(R_poly, equality_k, equality_w, self.R) |
| 428 | |
| 429 | FF_volts = self.CurrentDrivetrain().FF * self.boxed_R |
Ravago Jones | 5127ccc | 2022-07-31 16:32:45 -0700 | [diff] [blame] | 430 | self.U_ideal = self.CurrentDrivetrain().K * (self.boxed_R - |
| 431 | self.X) + FF_volts |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 432 | else: |
| 433 | glog.debug('Not all in gear') |
| 434 | if not self.IsInGear(self.left_gear) and not self.IsInGear( |
| 435 | self.right_gear): |
| 436 | # TODO(austin): Use battery volts here. |
Ravago Jones | 5127ccc | 2022-07-31 16:32:45 -0700 | [diff] [blame] | 437 | R_left = self.MotorRPM(self.left_shifter_position, self.X[0, |
| 438 | 0]) |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 439 | self.U_ideal[0, 0] = numpy.clip( |
| 440 | self.left_cim.K * (R_left - self.left_cim.X) + |
| 441 | R_left / self.left_cim.Kv, self.left_cim.U_min, |
| 442 | self.left_cim.U_max) |
| 443 | self.left_cim.Update(self.U_ideal[0, 0]) |
| 444 | |
Ravago Jones | 5127ccc | 2022-07-31 16:32:45 -0700 | [diff] [blame] | 445 | R_right = self.MotorRPM(self.right_shifter_position, self.X[1, |
| 446 | 0]) |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 447 | self.U_ideal[1, 0] = numpy.clip( |
| 448 | self.right_cim.K * (R_right - self.right_cim.X) + |
| 449 | R_right / self.right_cim.Kv, self.right_cim.U_min, |
| 450 | self.right_cim.U_max) |
| 451 | self.right_cim.Update(self.U_ideal[1, 0]) |
| 452 | else: |
| 453 | assert False |
| 454 | |
| 455 | self.U = numpy.clip(self.U_ideal, self.U_min, self.U_max) |
| 456 | |
| 457 | # TODO(austin): Model the robot as not accelerating when you shift... |
| 458 | # This hack only works when you shift at the same time. |
| 459 | if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear): |
| 460 | self.X = self.CurrentDrivetrain( |
| 461 | ).A * self.X + self.CurrentDrivetrain().B * self.U |
| 462 | |
| 463 | self.left_gear, self.left_shifter_position = self.SimShifter( |
| 464 | self.left_gear, self.left_shifter_position) |
| 465 | self.right_gear, self.right_shifter_position = self.SimShifter( |
| 466 | self.right_gear, self.right_shifter_position) |
| 467 | |
| 468 | glog.debug('U is %s %s', str(self.U[0, 0]), str(self.U[1, 0])) |
| 469 | glog.debug('Left shifter %s %d Right shifter %s %d', self.left_gear, |
| 470 | self.left_shifter_position, self.right_gear, |
| 471 | self.right_shifter_position) |
| 472 | |
| 473 | |
| 474 | def WritePolyDrivetrain(drivetrain_files, |
| 475 | motor_files, |
| 476 | hybrid_files, |
| 477 | year_namespace, |
| 478 | drivetrain_params, |
Austin Schuh | 7442515 | 2018-12-21 11:37:14 +1100 | [diff] [blame] | 479 | scalar_type='double'): |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 480 | vdrivetrain = VelocityDrivetrain(drivetrain_params) |
Ravago Jones | 5127ccc | 2022-07-31 16:32:45 -0700 | [diff] [blame] | 481 | hybrid_vdrivetrain = VelocityDrivetrain(drivetrain_params, |
| 482 | name="HybridVelocityDrivetrain") |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 483 | if isinstance(year_namespace, list): |
| 484 | namespaces = year_namespace |
| 485 | else: |
| 486 | namespaces = [year_namespace, 'control_loops', 'drivetrain'] |
Ravago Jones | 5127ccc | 2022-07-31 16:32:45 -0700 | [diff] [blame] | 487 | dog_loop_writer = control_loop.ControlLoopWriter("VelocityDrivetrain", [ |
| 488 | vdrivetrain.drivetrain_low_low, vdrivetrain.drivetrain_low_high, |
| 489 | vdrivetrain.drivetrain_high_low, vdrivetrain.drivetrain_high_high |
| 490 | ], |
| 491 | namespaces=namespaces, |
| 492 | scalar_type=scalar_type) |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 493 | |
James Kuszmaul | eeb98e9 | 2024-01-14 22:15:32 -0800 | [diff] [blame] | 494 | dog_loop_writer.Write(drivetrain_files[0], drivetrain_files[1], |
James Kuszmaul | 62c3bd8 | 2024-01-17 20:03:05 -0800 | [diff] [blame] | 495 | drivetrain_files[2], "velocity_drivetrain_loop") |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 496 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 497 | hybrid_loop_writer = control_loop.ControlLoopWriter( |
| 498 | "HybridVelocityDrivetrain", [ |
| 499 | hybrid_vdrivetrain.drivetrain_low_low, |
| 500 | hybrid_vdrivetrain.drivetrain_low_high, |
| 501 | hybrid_vdrivetrain.drivetrain_high_low, |
| 502 | hybrid_vdrivetrain.drivetrain_high_high |
| 503 | ], |
| 504 | namespaces=namespaces, |
| 505 | scalar_type=scalar_type, |
| 506 | plant_type='StateFeedbackHybridPlant', |
| 507 | observer_type='HybridKalman') |
Austin Schuh | 7442515 | 2018-12-21 11:37:14 +1100 | [diff] [blame] | 508 | |
James Kuszmaul | 62c3bd8 | 2024-01-17 20:03:05 -0800 | [diff] [blame] | 509 | hybrid_loop_writer.Write(hybrid_files[0], hybrid_files[1], hybrid_files[2], |
| 510 | "hybrid_velocity_drivetrain_loop") |
Austin Schuh | 7442515 | 2018-12-21 11:37:14 +1100 | [diff] [blame] | 511 | |
Ravago Jones | 5127ccc | 2022-07-31 16:32:45 -0700 | [diff] [blame] | 512 | cim_writer = control_loop.ControlLoopWriter("CIM", [CIM()], |
| 513 | scalar_type=scalar_type) |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 514 | |
James Kuszmaul | eeb98e9 | 2024-01-14 22:15:32 -0800 | [diff] [blame] | 515 | cim_writer.Write(motor_files[0], motor_files[1], motor_files[2]) |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 516 | |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 517 | |
| 518 | def PlotPolyDrivetrainMotions(drivetrain_params): |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 519 | vdrivetrain = VelocityDrivetrain(drivetrain_params) |
| 520 | vl_plot = [] |
| 521 | vr_plot = [] |
| 522 | ul_plot = [] |
| 523 | ur_plot = [] |
| 524 | radius_plot = [] |
| 525 | t_plot = [] |
| 526 | left_gear_plot = [] |
| 527 | right_gear_plot = [] |
| 528 | vdrivetrain.left_shifter_position = 0.0 |
| 529 | vdrivetrain.right_shifter_position = 0.0 |
| 530 | vdrivetrain.left_gear = VelocityDrivetrain.LOW |
| 531 | vdrivetrain.right_gear = VelocityDrivetrain.LOW |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 532 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 533 | glog.debug('K is %s', str(vdrivetrain.CurrentDrivetrain().K)) |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 534 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 535 | if vdrivetrain.left_gear is VelocityDrivetrain.HIGH: |
| 536 | glog.debug('Left is high') |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 537 | else: |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 538 | glog.debug('Left is low') |
| 539 | if vdrivetrain.right_gear is VelocityDrivetrain.HIGH: |
| 540 | glog.debug('Right is high') |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 541 | else: |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 542 | glog.debug('Right is low') |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 543 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 544 | for t in numpy.arange(0, 1.7, vdrivetrain.dt): |
| 545 | if t < 0.5: |
| 546 | vdrivetrain.Update(throttle=0.00, steering=1.0) |
| 547 | elif t < 1.2: |
| 548 | vdrivetrain.Update(throttle=0.5, steering=1.0) |
| 549 | else: |
| 550 | vdrivetrain.Update(throttle=0.00, steering=1.0) |
| 551 | t_plot.append(t) |
| 552 | vl_plot.append(vdrivetrain.X[0, 0]) |
| 553 | vr_plot.append(vdrivetrain.X[1, 0]) |
| 554 | ul_plot.append(vdrivetrain.U[0, 0]) |
| 555 | ur_plot.append(vdrivetrain.U[1, 0]) |
| 556 | left_gear_plot.append( |
| 557 | (vdrivetrain.left_gear is VelocityDrivetrain.HIGH) * 2.0 - 10.0) |
| 558 | right_gear_plot.append( |
| 559 | (vdrivetrain.right_gear is VelocityDrivetrain.HIGH) * 2.0 - 10.0) |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 560 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 561 | fwd_velocity = (vdrivetrain.X[1, 0] + vdrivetrain.X[0, 0]) / 2 |
| 562 | turn_velocity = (vdrivetrain.X[1, 0] - vdrivetrain.X[0, 0]) |
| 563 | if abs(fwd_velocity) < 0.0000001: |
| 564 | radius_plot.append(turn_velocity) |
| 565 | else: |
| 566 | radius_plot.append(turn_velocity / fwd_velocity) |
Campbell Crowley | 33e0e3d | 2017-12-27 17:55:40 -0800 | [diff] [blame] | 567 | |
Tyler Chatow | 6738c36 | 2019-02-16 14:12:30 -0800 | [diff] [blame] | 568 | # TODO(austin): |
| 569 | # Shifting compensation. |
| 570 | |
| 571 | # Tighten the turn. |
| 572 | # Closed loop drive. |
| 573 | |
| 574 | pylab.plot(t_plot, vl_plot, label='left velocity') |
| 575 | pylab.plot(t_plot, vr_plot, label='right velocity') |
| 576 | pylab.plot(t_plot, ul_plot, label='left voltage') |
| 577 | pylab.plot(t_plot, ur_plot, label='right voltage') |
| 578 | pylab.plot(t_plot, radius_plot, label='radius') |
| 579 | pylab.plot(t_plot, left_gear_plot, label='left gear high') |
| 580 | pylab.plot(t_plot, right_gear_plot, label='right gear high') |
| 581 | pylab.legend() |
| 582 | pylab.show() |