def test_Vertices_Nonsym(self):
"""Tests the vertices of a nonsymetric space."""
self.k = numpy.matrix([[6], [12], [2], [10]])
self.p = polytope.HPolytope(self.H, self.k)
vertices = self.p.Vertices()
self.AreVertices(self.p, vertices)
self.HasSamePoints(
numpy.matrix([[6., 2.], [6., -10.], [-12., -10.], [-12., 2.]]),
vertices)
def test_Skewed_Nonsym_Vertices(self):
"""Tests the vertices of a severely skewed space."""
self.H = numpy.matrix([[10, -1], [-1, -1], [-1, 10], [10, 10]])
self.k = numpy.matrix([[2], [2], [2], [2]])
self.p = polytope.HPolytope(self.H, self.k)
vertices = self.p.Vertices()
self.AreVertices(self.p, vertices)
self.HasSamePoints(
numpy.matrix([[0., 0.2], [0.2, 0.], [-2., 0.], [0., -2.]]),
vertices)
def __init__(self):
self.drivetrain_low_low = VelocityDrivetrainModel(
left_low=True, right_low=True, name='VelocityDrivetrainLowLow')
self.drivetrain_low_high = VelocityDrivetrainModel(left_low=True, right_low=False, name='VelocityDrivetrainLowHigh')
self.drivetrain_high_low = VelocityDrivetrainModel(left_low=False, right_low=True, name = 'VelocityDrivetrainHighLow')
self.drivetrain_high_high = VelocityDrivetrainModel(left_low=False, right_low=False, name = 'VelocityDrivetrainHighHigh')
# X is [lvel, rvel]
self.X = numpy.matrix(
[[0.0],
[0.0]])
self.U_poly = polytope.HPolytope(
numpy.matrix([[1, 0],
[-1, 0],
[0, 1],
[0, -1]]),
numpy.matrix([[12],
[12],
[12],
[12]]))
self.U_max = numpy.matrix(
[[12.0],
[12.0]])
self.U_min = numpy.matrix(
[[-12.0000000000],
[-12.0000000000]])
self.dt = 0.005
self.R = numpy.matrix(
[[0.0],
[0.0]])
self.U_ideal = numpy.matrix(
[[0.0],
[0.0]])
# ttrust is the comprimise between having full throttle negative inertia,
# and having no throttle negative inertia. A value of 0 is full throttle
# inertia. A value of 1 is no throttle negative inertia.
self.ttrust = 1.0
self.left_gear = VelocityDrivetrain.LOW
self.right_gear = VelocityDrivetrain.LOW
self.left_shifter_position = 0.0
self.right_shifter_position = 0.0
self.left_cim = drivetrain.CIM()
self.right_cim = drivetrain.CIM()
def setUp(self):
"""Builds a simple box polytope."""
self.H = numpy.matrix([[1, 0], [-1, 0], [0, 1], [0, -1]])
self.k = numpy.matrix([[12], [12], [12], [12]])
self.p = polytope.HPolytope(self.H, self.k)
def MakePWithDims(self, num_constraints, num_dims):
"""Makes a zeroed out polytope with the correct size."""
self.p = polytope.HPolytope(
numpy.matrix(numpy.zeros((num_constraints, num_dims))),
numpy.matrix(numpy.zeros((num_constraints, 1))))
def DoCoerceGoal(region, K, w, R):
if region.IsInside(R):
return (R, True)
perpendicular_vector = K.T / numpy.linalg.norm(K)
parallel_vector = numpy.matrix([[perpendicular_vector[1, 0]],
[-perpendicular_vector[0, 0]]])
# We want to impose the constraint K * X = w on the polytope H * X <= k.
# We do this by breaking X up into parallel and perpendicular components to
# the half plane. This gives us the following equation.
#
# parallel * (parallel.T \dot X) + perpendicular * (perpendicular \dot X)) = X
#
# Then, substitute this into the polytope.
#
# H * (parallel * (parallel.T \dot X) + perpendicular * (perpendicular \dot X)) <= k
#
# Substitute K * X = w
#
# H * parallel * (parallel.T \dot X) + H * perpendicular * w <= k
#
# Move all the knowns to the right side.
#
# H * parallel * ([parallel1 parallel2] * X) <= k - H * perpendicular * w
#
# Let t = parallel.T \dot X, the component parallel to the surface.
#
# H * parallel * t <= k - H * perpendicular * w
#
# This is a polytope which we can solve, and use to figure out the range of X
# that we care about!
t_poly = polytope.HPolytope(
region.H * parallel_vector,
region.k - region.H * perpendicular_vector * w)
vertices = t_poly.Vertices()
if vertices.shape[0]:
# The region exists!
# Find the closest vertex
min_distance = numpy.infty
closest_point = None
for vertex in vertices:
point = parallel_vector * vertex + perpendicular_vector * w
length = numpy.linalg.norm(R - point)
if length < min_distance:
min_distance = length
closest_point = point
return (closest_point, True)
else:
# Find the vertex of the space that is closest to the line.
region_vertices = region.Vertices()
min_distance = numpy.infty
closest_point = None
for vertex in region_vertices:
point = vertex.T
length = numpy.abs((perpendicular_vector.T * point)[0, 0])
if length < min_distance:
min_distance = length
closest_point = point
return (closest_point, False)
def Update(self, throttle, steering):
# Shift into the gear which sends the most power to the floor.
# This is the same as sending the most torque down to the floor at the
# wheel.
self.left_gear = self.right_gear = True
if True:
self.left_gear = self.ComputeGear(self.X[0, 0], should_print=True,
current_gear=self.left_gear,
gear_name="left")
self.right_gear = self.ComputeGear(self.X[1, 0], should_print=True,
current_gear=self.right_gear,
gear_name="right")
if self.IsInGear(self.left_gear):
self.left_cim.X[0, 0] = self.MotorRPM(self.left_shifter_position, self.X[0, 0])
if self.IsInGear(self.right_gear):
self.right_cim.X[0, 0] = self.MotorRPM(self.right_shifter_position, self.X[0, 0])
if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
# Filter the throttle to provide a nicer response.
fvel = self.FilterVelocity(throttle)
# Constant radius means that angualar_velocity / linear_velocity = constant.
# Compute the left and right velocities.
steering_velocity = numpy.abs(fvel) * steering
left_velocity = fvel - steering_velocity
right_velocity = fvel + steering_velocity
# Write this constraint in the form of K * R = w
# angular velocity / linear velocity = constant
# (left - right) / (left + right) = constant
# left - right = constant * left + constant * right
# (fvel - steering * numpy.abs(fvel) - fvel - steering * numpy.abs(fvel)) /
# (fvel - steering * numpy.abs(fvel) + fvel + steering * numpy.abs(fvel)) =
# constant
# (- 2 * steering * numpy.abs(fvel)) / (2 * fvel) = constant
# (-steering * sign(fvel)) = constant
# (-steering * sign(fvel)) * (left + right) = left - right
# (steering * sign(fvel) + 1) * left + (steering * sign(fvel) - 1) * right = 0
equality_k = numpy.matrix(
[[1 + steering * numpy.sign(fvel), -(1 - steering * numpy.sign(fvel))]])
equality_w = 0.0
self.R[0, 0] = left_velocity
self.R[1, 0] = right_velocity
# Construct a constraint on R by manipulating the constraint on U
# Start out with H * U <= k
# U = FF * R + K * (R - X)
# H * (FF * R + K * R - K * X) <= k
# H * (FF + K) * R <= k + H * K * X
R_poly = polytope.HPolytope(
self.U_poly.H * (self.CurrentDrivetrain().K + self.CurrentDrivetrain().FF),
self.U_poly.k + self.U_poly.H * self.CurrentDrivetrain().K * self.X)
# Limit R back inside the box.
self.boxed_R = CoerceGoal(R_poly, equality_k, equality_w, self.R)
FF_volts = self.CurrentDrivetrain().FF * self.boxed_R
self.U_ideal = self.CurrentDrivetrain().K * (self.boxed_R - self.X) + FF_volts
else:
glog.debug('Not all in gear')
if not self.IsInGear(self.left_gear) and not self.IsInGear(self.right_gear):
# TODO(austin): Use battery volts here.
R_left = self.MotorRPM(self.left_shifter_position, self.X[0, 0])
self.U_ideal[0, 0] = numpy.clip(
self.left_cim.K * (R_left - self.left_cim.X) + R_left / self.left_cim.Kv,
self.left_cim.U_min, self.left_cim.U_max)
self.left_cim.Update(self.U_ideal[0, 0])
R_right = self.MotorRPM(self.right_shifter_position, self.X[1, 0])
self.U_ideal[1, 0] = numpy.clip(
self.right_cim.K * (R_right - self.right_cim.X) + R_right / self.right_cim.Kv,
self.right_cim.U_min, self.right_cim.U_max)
self.right_cim.Update(self.U_ideal[1, 0])
else:
assert False
self.U = numpy.clip(self.U_ideal, self.U_min, self.U_max)
# TODO(austin): Model the robot as not accelerating when you shift...
# This hack only works when you shift at the same time.
if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
self.X = self.CurrentDrivetrain().A * self.X + self.CurrentDrivetrain().B * self.U
self.left_gear, self.left_shifter_position = self.SimShifter(
self.left_gear, self.left_shifter_position)
self.right_gear, self.right_shifter_position = self.SimShifter(
self.right_gear, self.right_shifter_position)
glog.debug('U is %s %s', str(self.U[0, 0]), str(self.U[1, 0]))
glog.debug('Left shifter %s %d Right shifter %s %d',
self.left_gear, self.left_shifter_position,
self.right_gear, self.right_shifter_position)