Merge "Handle one node going away better"
diff --git a/aos/events/event_loop_param_test.cc b/aos/events/event_loop_param_test.cc
index d92927b..a5b6b19 100644
--- a/aos/events/event_loop_param_test.cc
+++ b/aos/events/event_loop_param_test.cc
@@ -1053,23 +1053,31 @@
// Verify that we can change a timer's parameters during execution.
TEST_P(AbstractEventLoopTest, TimerChangeParameters) {
auto loop = MakePrimary();
- ::std::vector<::aos::monotonic_clock::time_point> iteration_list;
+ std::vector<monotonic_clock::time_point> iteration_list;
auto test_timer = loop->AddTimer([&iteration_list, &loop]() {
- iteration_list.push_back(loop->monotonic_now());
+ iteration_list.push_back(loop->context().monotonic_event_time);
});
- auto modifier_timer = loop->AddTimer([&loop, &test_timer]() {
- test_timer->Setup(loop->monotonic_now(), ::std::chrono::milliseconds(30));
+ monotonic_clock::time_point s;
+ auto modifier_timer = loop->AddTimer([&test_timer, &s]() {
+ test_timer->Setup(s + chrono::milliseconds(45), chrono::milliseconds(30));
});
- test_timer->Setup(loop->monotonic_now(), ::std::chrono::milliseconds(20));
- modifier_timer->Setup(loop->monotonic_now() +
- ::std::chrono::milliseconds(45));
- EndEventLoop(loop.get(), ::std::chrono::milliseconds(150));
+ s = loop->monotonic_now();
+ test_timer->Setup(s, chrono::milliseconds(20));
+ modifier_timer->Setup(s + chrono::milliseconds(45));
+ EndEventLoop(loop.get(), chrono::milliseconds(150));
Run();
EXPECT_EQ(iteration_list.size(), 7);
+ EXPECT_EQ(iteration_list[0], s);
+ EXPECT_EQ(iteration_list[1], s + chrono::milliseconds(20));
+ EXPECT_EQ(iteration_list[2], s + chrono::milliseconds(40));
+ EXPECT_EQ(iteration_list[3], s + chrono::milliseconds(45));
+ EXPECT_EQ(iteration_list[4], s + chrono::milliseconds(75));
+ EXPECT_EQ(iteration_list[5], s + chrono::milliseconds(105));
+ EXPECT_EQ(iteration_list[6], s + chrono::milliseconds(135));
}
// Verify that we can disable a timer during execution.
diff --git a/aos/events/logging/logger.cc b/aos/events/logging/logger.cc
index eb7221d..e10d911 100644
--- a/aos/events/logging/logger.cc
+++ b/aos/events/logging/logger.cc
@@ -1437,12 +1437,18 @@
// simulation.
state->Send(std::move(timestamped_message));
} else if (!ignore_missing_data_ &&
+ // When starting up, we can have data which was sent before the
+ // log starts, but the timestamp was after the log starts. This
+ // is unreasonable to avoid, so ignore the missing data.
+ timestamped_message.monotonic_remote_time >=
+ state->monotonic_remote_start_time(
+ timestamped_message.channel_index) &&
!FLAGS_skip_missing_forwarding_entries) {
- // We've found a timestamp without data. This likely means that we are
- // at the end of the log file. Record it and CHECK that in the rest of
- // the log file, we don't find any more data on that channel. Not all
- // channels will end at the same point in time since they can be in
- // different files.
+ // We've found a timestamp without data that we expect to have data for.
+ // This likely means that we are at the end of the log file. Record it
+ // and CHECK that in the rest of the log file, we don't find any more
+ // data on that channel. Not all channels will end at the same point in
+ // time since they can be in different files.
VLOG(1) << "Found the last message on channel "
<< timestamped_message.channel_index;
@@ -1454,7 +1460,8 @@
// Now that we found the end of one channel, artificially stop the
// rest. It is confusing when part of your data gets replayed but not
- // all.
+ // all. Read the rest of the messages and drop them on the floor while
+ // doing some basic validation.
while (state->OldestMessageTime() != monotonic_clock::max_time) {
TimestampedMessage next = state->PopOldest();
// Make sure that once we have seen the last message on a channel,
@@ -2034,8 +2041,16 @@
<< ": Unsupported mix of timestamps and no timestamps.";
sender_.Send(std::move(remote_message));
} else {
- remote_timestamps_.emplace_back(std::move(remote_message),
- monotonic_timestamp_time);
+ remote_timestamps_.emplace(
+ std::upper_bound(
+ remote_timestamps_.begin(), remote_timestamps_.end(),
+ monotonic_timestamp_time,
+ [](const aos::monotonic_clock::time_point monotonic_timestamp_time,
+ const Timestamp ×tamp) {
+ return monotonic_timestamp_time <
+ timestamp.monotonic_timestamp_time;
+ }),
+ std::move(remote_message), monotonic_timestamp_time);
ScheduleTimestamp();
}
}
@@ -2098,6 +2113,10 @@
// mapper directly.
filter->Pop(event_loop_->node(), result.monotonic_event_time);
}
+ VLOG(1) << "Popped " << result
+ << configuration::CleanedChannelToString(
+ event_loop_->configuration()->channels()->Get(
+ factory_channel_index_[result.channel_index]));
return result;
}
diff --git a/aos/events/logging/logger.h b/aos/events/logging/logger.h
index 3cb44f6..d142bd8 100644
--- a/aos/events/logging/logger.h
+++ b/aos/events/logging/logger.h
@@ -457,9 +457,9 @@
// Returns true if the channel exists on the node and was logged.
template <typename T>
bool HasLoggedChannel(std::string_view name, const Node *node = nullptr) {
- const Channel *channel = configuration::GetChannel(logged_configuration(), name,
- T::GetFullyQualifiedName(), "", node,
- true);
+ const Channel *channel =
+ configuration::GetChannel(logged_configuration(), name,
+ T::GetFullyQualifiedName(), "", node, true);
if (channel == nullptr) return false;
return channel->logger() != LoggerConfig::NOT_LOGGED;
}
@@ -497,7 +497,7 @@
// Class to manage sending RemoteMessages on the provided node after the
// correct delay.
- class RemoteMessageSender{
+ class RemoteMessageSender {
public:
RemoteMessageSender(aos::Sender<message_bridge::RemoteMessage> sender,
EventLoop *event_loop);
@@ -606,6 +606,12 @@
->node_event_loop_factory_->monotonic_now();
}
+ // Returns the start time of the remote for the provided channel.
+ monotonic_clock::time_point monotonic_remote_start_time(
+ size_t channel_index) {
+ return channel_source_state_[channel_index]->monotonic_start_time();
+ }
+
distributed_clock::time_point RemoteToDistributedClock(
size_t channel_index, monotonic_clock::time_point time) {
return channel_source_state_[channel_index]
diff --git a/aos/network/timestamp_filter.cc b/aos/network/timestamp_filter.cc
index b4455a5..6479607 100644
--- a/aos/network/timestamp_filter.cc
+++ b/aos/network/timestamp_filter.cc
@@ -520,16 +520,15 @@
std::tuple<monotonic_clock::time_point, chrono::nanoseconds>>
NoncausalTimestampFilter::FindTimestamps(monotonic_clock::time_point ta) const {
CHECK_GT(timestamps_size(), 1u);
- // Linear search until this is proven to be a measurable slowdown.
- // If ta is outside our timestamp range, return the closest pair
- size_t index = 0;
- while (index < timestamps_size() - 2u) {
- if (std::get<0>(timestamp(index + 1)) > ta) {
- break;
- }
- ++index;
- }
- return std::make_pair(timestamp(index), timestamp(index + 1));
+ auto it = std::upper_bound(timestamps_.begin() + 1, timestamps_.end() - 1, ta,
+ [](monotonic_clock::time_point ta,
+ std::tuple<aos::monotonic_clock::time_point,
+ std::chrono::nanoseconds, bool>
+ t) { return ta < std::get<0>(t); });
+
+ const size_t index = std::distance(timestamps_.begin(), it);
+
+ return std::make_pair(timestamp(index - 1), timestamp(index));
}
chrono::nanoseconds NoncausalTimestampFilter::ExtrapolateOffset(
diff --git a/aos/vision/download/downloader.py b/aos/vision/download/downloader.py
index 3aea634..be8fda2 100644
--- a/aos/vision/download/downloader.py
+++ b/aos/vision/download/downloader.py
@@ -34,7 +34,7 @@
jetson_fnames = [ToJetsonFname(fname) for fname in fnames]
checksums = GetChecksums(fnames)
jetson_checksums = GetJetsonChecksums(ssh_target, jetson_fnames)
- for i in xrange(len(fnames)):
+ for i in range(len(fnames)):
if (checksums[i] != jetson_checksums[i]):
# if empty, unlink
subprocess.check_call(["ssh", ssh_target, "unlink " + jetson_fnames[i]])
diff --git a/frc971/control_loops/python/angular_system.py b/frc971/control_loops/python/angular_system.py
index bd4447b..397e6ee 100755
--- a/frc971/control_loops/python/angular_system.py
+++ b/frc971/control_loops/python/angular_system.py
@@ -216,7 +216,7 @@
U_last = numpy.matrix(numpy.zeros((1, 1)))
iterations = int(duration / plant.dt)
- for i in xrange(iterations):
+ for i in range(iterations):
t = i * plant.dt
observer.Y = plant.Y
observer.CorrectObserver(U_last)
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 1f1cacd..c3dc1a2 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -353,8 +353,8 @@
' Eigen::Matrix<%s, %d, %d> %s;\n' % (scalar_type, matrix.shape[0],
matrix.shape[1], matrix_name)
]
- for x in xrange(matrix.shape[0]):
- for y in xrange(matrix.shape[1]):
+ for x in range(matrix.shape[0]):
+ for y in range(matrix.shape[1]):
write_type = repr(matrix[x, y])
if scalar_type == 'float':
if '.' not in write_type:
diff --git a/frc971/control_loops/python/controls.py b/frc971/control_loops/python/controls.py
index 7505413..defdbd2 100644
--- a/frc971/control_loops/python/controls.py
+++ b/frc971/control_loops/python/controls.py
@@ -117,7 +117,7 @@
n = A.shape[0]
output = B
intermediate = B
- for i in xrange(0, n):
+ for i in range(0, n):
intermediate = A * intermediate
output = numpy.concatenate((output, intermediate), axis=1)
diff --git a/frc971/control_loops/python/down_estimator.py b/frc971/control_loops/python/down_estimator.py
index 8efcff6..c4b8e7e 100644
--- a/frc971/control_loops/python/down_estimator.py
+++ b/frc971/control_loops/python/down_estimator.py
@@ -83,7 +83,7 @@
real_velocities = [0]
estimated_angles = [0]
estimated_velocities = [0]
- for _ in xrange(100):
+ for _ in range(100):
estimator.Predict(0)
estimator.Update(numpy.sqrt(2) / 2.0, numpy.sqrt(2) / 2.0, 0, 0)
real_angles.append(math.pi / 2)
@@ -92,7 +92,7 @@
estimated_velocities.append(estimator.X_hat[1, 0])
angle = math.pi / 2
velocity = 1
- for i in xrange(100):
+ for i in range(100):
measured_velocity = velocity + (random.random() - 0.5) * 0.01 + 0.05
estimator.Predict(measured_velocity)
estimator.Update(
@@ -111,7 +111,7 @@
pylab.show()
if len(argv) != 3:
- print "Expected .h file name and .cc file name"
+ glog.error("Expected .h file name and .cc file name")
else:
namespaces = ['frc971', 'control_loops', 'drivetrain']
kf_loop_writer = control_loop.ControlLoopWriter(
diff --git a/frc971/control_loops/python/drivetrain.py b/frc971/control_loops/python/drivetrain.py
index 1aec459..a020276 100644
--- a/frc971/control_loops/python/drivetrain.py
+++ b/frc971/control_loops/python/drivetrain.py
@@ -540,7 +540,7 @@
left_power = []
right_power = []
R = numpy.matrix([[0.0], [0.0], [0.0], [0.0], [0.0], [0.0], [0.0]])
- for _ in xrange(300):
+ for _ in range(300):
U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat), drivetrain.U_min,
drivetrain.U_max)
drivetrain.UpdateObserver(U)
@@ -564,7 +564,7 @@
left_low=False, right_low=False, drivetrain_params=drivetrain_params)
simulated_left = []
simulated_right = []
- for _ in xrange(100):
+ for _ in range(100):
drivetrain.Update(numpy.matrix([[12.0], [12.0]]))
simulated_left.append(drivetrain.X[0, 0])
simulated_right.append(drivetrain.X[2, 0])
@@ -584,7 +584,7 @@
left_power = []
right_power = []
R = numpy.matrix([[1.0], [0.0], [1.0], [0.0], [0.0], [0.0], [0.0]])
- for _ in xrange(300):
+ for _ in range(300):
U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat), drivetrain.U_min,
drivetrain.U_max)
drivetrain.UpdateObserver(U)
@@ -608,7 +608,7 @@
close_loop_left = []
close_loop_right = []
R = numpy.matrix([[-1.0], [0.0], [1.0], [0.0], [0.0], [0.0], [0.0]])
- for _ in xrange(200):
+ for _ in range(200):
U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat), drivetrain.U_min,
drivetrain.U_max)
drivetrain.UpdateObserver(U)
@@ -628,7 +628,7 @@
close_loop_left = []
close_loop_right = []
R = numpy.matrix([[0.0], [0.0], [1.0], [0.0], [0.0], [0.0], [0.0]])
- for _ in xrange(300):
+ for _ in range(300):
U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat), drivetrain.U_min,
drivetrain.U_max)
drivetrain.UpdateObserver(U)
diff --git a/frc971/control_loops/python/haptic_wheel.py b/frc971/control_loops/python/haptic_wheel.py
index 088b204..79c9c35 100755
--- a/frc971/control_loops/python/haptic_wheel.py
+++ b/frc971/control_loops/python/haptic_wheel.py
@@ -253,7 +253,7 @@
observer.X_hat[0, 0] = data_radians[0]
last_request_current = data_request_current[0]
- kf_torques = [[] for i in xrange(num_kf)]
+ kf_torques = [[] for i in range(num_kf)]
for angle, current, request_current in zip(data_radians, data_current,
data_request_current):
# Predict and correct all the parameter swept observers.
diff --git a/frc971/control_loops/python/linear_system.py b/frc971/control_loops/python/linear_system.py
index 105093b..a2e37f0 100755
--- a/frc971/control_loops/python/linear_system.py
+++ b/frc971/control_loops/python/linear_system.py
@@ -215,7 +215,7 @@
U_last = numpy.matrix(numpy.zeros((1, 1)))
iterations = int(duration / plant.dt)
- for i in xrange(iterations):
+ for i in range(iterations):
t = i * plant.dt
observer.Y = plant.Y
observer.CorrectObserver(U_last)
diff --git a/frc971/control_loops/python/polytope.py b/frc971/control_loops/python/polytope.py
index 0b65e38..a1cba57 100644
--- a/frc971/control_loops/python/polytope.py
+++ b/frc971/control_loops/python/polytope.py
@@ -40,9 +40,9 @@
if height < min_height:
pad_array = [' ' * len(padded_array[0])]
height_error = min_height - height
- return (pad_array * ((height_error) / 2) +
+ return (pad_array * int((height_error) / 2) +
padded_array +
- pad_array * ((height_error + 1) / 2))
+ pad_array * int((height_error + 1) / 2))
return padded_array
@@ -101,9 +101,9 @@
try:
# Copy the data into the matrix.
- for i in xrange(self.num_constraints):
+ for i in range(self.num_constraints):
libcdd.dd_set_d(matrix.matrix[i][0], self._k[i, 0])
- for j in xrange(self.ndim):
+ for j in range(self.ndim):
libcdd.dd_set_d(matrix.matrix[i][j + 1], -self._H[i, j])
# Set enums to the correct values.
@@ -132,7 +132,7 @@
# Count the number of vertices and rays in the result.
num_vertices = 0
num_rays = 0
- for i in xrange(vertex_matrix.rowsize):
+ for i in range(vertex_matrix.rowsize):
if libcdd.dd_get_d(vertex_matrix.matrix[i][0]) == 0:
num_rays += 1
else:
@@ -148,14 +148,14 @@
vertex_index = 0
# Copy the data out of the matrix.
- for index in xrange(vertex_matrix.rowsize):
+ for index in range(vertex_matrix.rowsize):
if libcdd.dd_get_d(vertex_matrix.matrix[index][0]) == 0.0:
- for j in xrange(vertex_matrix.colsize - 1):
+ for j in range(vertex_matrix.colsize - 1):
rays[ray_index, j] = libcdd.dd_get_d(
vertex_matrix.matrix[index][j + 1])
ray_index += 1
else:
- for j in xrange(vertex_matrix.colsize - 1):
+ for j in range(vertex_matrix.colsize - 1):
vertices[vertex_index, j] = libcdd.dd_get_d(
vertex_matrix.matrix[index][j + 1])
vertex_index += 1
@@ -203,7 +203,7 @@
if self.ndim == 1:
x_strings = ["[[x0]] "]
else:
- for index in xrange(self.ndim):
+ for index in range(self.ndim):
if index == 0:
x = "[[x%d] " % index
elif index == self.ndim - 1:
@@ -216,8 +216,8 @@
def _MakeCmpStrings(self, height):
"""Builds an array of strings with the comparison in it for printing."""
cmp_strings = []
- for index in xrange(height):
- if index == (height - 1) / 2:
+ for index in range(height):
+ if index == int((height - 1) / 2):
cmp_strings.append("<= ")
else:
cmp_strings.append(" ")
diff --git a/frc971/control_loops/python/polytope_test.py b/frc971/control_loops/python/polytope_test.py
index 51bf6fd..f5e4783 100755
--- a/frc971/control_loops/python/polytope_test.py
+++ b/frc971/control_loops/python/polytope_test.py
@@ -9,185 +9,185 @@
__author__ = 'Austin Schuh (austin.linux@gmail.com)'
def MakePoint(*args):
- """Makes a point from a set of arguments."""
- return numpy.matrix([[arg] for arg in args])
+ """Makes a point from a set of arguments."""
+ return numpy.matrix([[arg] for arg in args])
class TestHPolytope(unittest.TestCase):
- 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 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 test_Hk(self):
- """Tests that H and k are saved correctly."""
- assert_array_equal(self.p.H, self.H)
- assert_array_equal(self.p.k, self.k)
+ def test_Hk(self):
+ """Tests that H and k are saved correctly."""
+ assert_array_equal(self.p.H, self.H)
+ assert_array_equal(self.p.k, self.k)
- def test_IsInside(self):
- """Tests IsInside for various points."""
- inside_points = [
- MakePoint(0, 0),
- MakePoint(6, 6),
- MakePoint(12, 6),
- MakePoint(-6, 10)]
- outside_points = [
- MakePoint(14, 0),
- MakePoint(-14, 0),
- MakePoint(0, 14),
- MakePoint(0, -14),
- MakePoint(14, -14)]
+ def test_IsInside(self):
+ """Tests IsInside for various points."""
+ inside_points = [
+ MakePoint(0, 0),
+ MakePoint(6, 6),
+ MakePoint(12, 6),
+ MakePoint(-6, 10)]
+ outside_points = [
+ MakePoint(14, 0),
+ MakePoint(-14, 0),
+ MakePoint(0, 14),
+ MakePoint(0, -14),
+ MakePoint(14, -14)]
- for inside_point in inside_points:
- self.assertTrue(self.p.IsInside(inside_point),
- msg='Point is' + str(inside_point))
+ for inside_point in inside_points:
+ self.assertTrue(self.p.IsInside(inside_point),
+ msg='Point is' + str(inside_point))
- for outside_point in outside_points:
- self.assertFalse(self.p.IsInside(outside_point),
- msg='Point is' + str(outside_point))
+ for outside_point in outside_points:
+ self.assertFalse(self.p.IsInside(outside_point),
+ msg='Point is' + str(outside_point))
- def AreVertices(self, p, vertices):
- """Checks that all the vertices are on corners of the set."""
- for i in xrange(vertices.shape[0]):
- # Check that all the vertices have the correct number of active
- # constraints.
- lmda = p.H * vertices[i,:].T - p.k
- num_active_constraints = 0
- for j in xrange(lmda.shape[0]):
- # Verify that the constraints are either active, or not violated.
- if numpy.abs(lmda[j, 0]) <= 1e-9:
- num_active_constraints += 1
- else:
- self.assertLessEqual(lmda[j, 0], 0.0)
+ def AreVertices(self, p, vertices):
+ """Checks that all the vertices are on corners of the set."""
+ for i in range(vertices.shape[0]):
+ # Check that all the vertices have the correct number of active
+ # constraints.
+ lmda = p.H * vertices[i,:].T - p.k
+ num_active_constraints = 0
+ for j in range(lmda.shape[0]):
+ # Verify that the constraints are either active, or not violated.
+ if numpy.abs(lmda[j, 0]) <= 1e-9:
+ num_active_constraints += 1
+ else:
+ self.assertLessEqual(lmda[j, 0], 0.0)
- self.assertEqual(p.ndim, num_active_constraints)
+ self.assertEqual(p.ndim, num_active_constraints)
- def HasSamePoints(self, expected, actual):
- """Verifies that the points in expected are in actual."""
- found_points = set()
- self.assertEqual(expected.shape, actual.shape)
- for index in xrange(expected.shape[0]):
- expected_point = expected[index, :]
- for actual_index in xrange(actual.shape[0]):
- actual_point = actual[actual_index, :]
- if numpy.abs(expected_point - actual_point).max() <= 1e-4:
- found_points.add(actual_index)
- break
+ def HasSamePoints(self, expected, actual):
+ """Verifies that the points in expected are in actual."""
+ found_points = set()
+ self.assertEqual(expected.shape, actual.shape)
+ for index in range(expected.shape[0]):
+ expected_point = expected[index, :]
+ for actual_index in range(actual.shape[0]):
+ actual_point = actual[actual_index, :]
+ if numpy.abs(expected_point - actual_point).max() <= 1e-4:
+ found_points.add(actual_index)
+ break
- self.assertEqual(len(found_points), actual.shape[0],
- msg="Expected:\n" + str(expected) + "\nActual:\n" + str(actual))
+ self.assertEqual(len(found_points), actual.shape[0],
+ msg="Expected:\n" + str(expected) + "\nActual:\n" + str(actual))
- 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)
+ 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)
+ self.HasSamePoints(
+ numpy.matrix([[0., 0.2],
+ [0.2, 0.],
+ [-2., 0.],
+ [0., -2.]]),
+ vertices)
- 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)
+ 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)
+ self.HasSamePoints(
+ numpy.matrix([[6., 2.],
+ [6., -10.],
+ [-12., -10.],
+ [-12., 2.]]),
+ vertices)
- def test_Vertices(self):
- """Tests the vertices of a nonsymetric space."""
- self.HasSamePoints(self.p.Vertices(),
- numpy.matrix([[12., 12.],
- [12., -12.],
- [-12., -12.],
- [-12., 12.]]))
+ def test_Vertices(self):
+ """Tests the vertices of a nonsymetric space."""
+ self.HasSamePoints(self.p.Vertices(),
+ numpy.matrix([[12., 12.],
+ [12., -12.],
+ [-12., -12.],
+ [-12., 12.]]))
- def test_concat(self):
- """Tests that the concat function works for simple inputs."""
- self.assertEqual(["asd", "qwe"],
- polytope._PiecewiseConcat(["a", "q"],
- ["s", "w"],
- ["d", "e"]))
+ def test_concat(self):
+ """Tests that the concat function works for simple inputs."""
+ self.assertEqual(["asd", "qwe"],
+ list(
+ polytope._PiecewiseConcat(["a", "q"], ["s", "w"],
+ ["d", "e"])))
- def test_str(self):
- """Verifies that the str method works for the provided p."""
- self.assertEqual('[[ 1 0] [[12] \n'
- ' [-1 0] [[x0] <= [12] \n'
- ' [ 0 1] [x1]] [12] \n'
- ' [ 0 -1]] [12]] ',
- str(self.p))
+ def test_str(self):
+ """Verifies that the str method works for the provided p."""
+ self.assertEqual('[[ 1 0] [[12] \n'
+ ' [-1 0] [[x0] <= [12] \n'
+ ' [ 0 1] [x1]] [12] \n'
+ ' [ 0 -1]] [12]] ',
+ str(self.p))
- 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 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 test_few_constraints_odd_constraint_even_dims_str(self):
- """Tests printing out the set with odd constraints and even dimensions."""
- self.MakePWithDims(num_constraints=5, num_dims=2)
- self.assertEqual('[[ 0. 0.] [[ 0.] \n'
- ' [ 0. 0.] [[x0] [ 0.] \n'
- ' [ 0. 0.] [x1]] <= [ 0.] \n'
- ' [ 0. 0.] [ 0.] \n'
- ' [ 0. 0.]] [ 0.]] ',
- str(self.p))
+ def test_few_constraints_odd_constraint_even_dims_str(self):
+ """Tests printing out the set with odd constraints and even dimensions."""
+ self.MakePWithDims(num_constraints=5, num_dims=2)
+ self.assertEqual('[[ 0. 0.] [[ 0.] \n'
+ ' [ 0. 0.] [[x0] [ 0.] \n'
+ ' [ 0. 0.] [x1]] <= [ 0.] \n'
+ ' [ 0. 0.] [ 0.] \n'
+ ' [ 0. 0.]] [ 0.]] ',
+ str(self.p))
- def test_few_constraints_odd_constraint_small_dims_str(self):
- """Tests printing out the set with odd constraints and odd dimensions."""
- self.MakePWithDims(num_constraints=5, num_dims=1)
- self.assertEqual('[[ 0.] [[ 0.] \n'
- ' [ 0.] [ 0.] \n'
- ' [ 0.] [[x0]] <= [ 0.] \n'
- ' [ 0.] [ 0.] \n'
- ' [ 0.]] [ 0.]] ',
- str(self.p))
+ def test_few_constraints_odd_constraint_small_dims_str(self):
+ """Tests printing out the set with odd constraints and odd dimensions."""
+ self.MakePWithDims(num_constraints=5, num_dims=1)
+ self.assertEqual('[[ 0.] [[ 0.] \n'
+ ' [ 0.] [ 0.] \n'
+ ' [ 0.] [[x0]] <= [ 0.] \n'
+ ' [ 0.] [ 0.] \n'
+ ' [ 0.]] [ 0.]] ',
+ str(self.p))
- def test_few_constraints_odd_constraint_odd_dims_str(self):
- """Tests printing out the set with odd constraints and odd dimensions."""
- self.MakePWithDims(num_constraints=5, num_dims=3)
- self.assertEqual('[[ 0. 0. 0.] [[ 0.] \n'
- ' [ 0. 0. 0.] [[x0] [ 0.] \n'
- ' [ 0. 0. 0.] [x1] <= [ 0.] \n'
- ' [ 0. 0. 0.] [x2]] [ 0.] \n'
- ' [ 0. 0. 0.]] [ 0.]] ',
- str(self.p))
+ def test_few_constraints_odd_constraint_odd_dims_str(self):
+ """Tests printing out the set with odd constraints and odd dimensions."""
+ self.MakePWithDims(num_constraints=5, num_dims=3)
+ self.assertEqual('[[ 0. 0. 0.] [[ 0.] \n'
+ ' [ 0. 0. 0.] [[x0] [ 0.] \n'
+ ' [ 0. 0. 0.] [x1] <= [ 0.] \n'
+ ' [ 0. 0. 0.] [x2]] [ 0.] \n'
+ ' [ 0. 0. 0.]] [ 0.]] ',
+ str(self.p))
- def test_many_constraints_even_constraint_odd_dims_str(self):
- """Tests printing out the set with even constraints and odd dimensions."""
- self.MakePWithDims(num_constraints=2, num_dims=3)
- self.assertEqual('[[ 0. 0. 0.] [[x0] [[ 0.] \n'
- ' [ 0. 0. 0.]] [x1] <= [ 0.]] \n'
- ' [x2]] ',
- str(self.p))
+ def test_many_constraints_even_constraint_odd_dims_str(self):
+ """Tests printing out the set with even constraints and odd dimensions."""
+ self.MakePWithDims(num_constraints=2, num_dims=3)
+ self.assertEqual('[[ 0. 0. 0.] [[x0] [[ 0.] \n'
+ ' [ 0. 0. 0.]] [x1] <= [ 0.]] \n'
+ ' [x2]] ',
+ str(self.p))
if __name__ == '__main__':
- unittest.main()
+ unittest.main()
diff --git a/frc971/control_loops/python/spline.py b/frc971/control_loops/python/spline.py
index b428c9a..890ef9f 100644
--- a/frc971/control_loops/python/spline.py
+++ b/frc971/control_loops/python/spline.py
@@ -28,7 +28,7 @@
"""4th order RungeKutta integration of dy/dt = f(t, y) starting at X."""
y1 = y0
dh = h / float(count)
- for x in xrange(count):
+ for x in range(count):
k1 = dh * f(t + dh * x, y1)
k2 = dh * f(t + dh * x + dh / 2.0, y1 + k1 / 2.0)
k3 = dh * f(t + dh * x + dh / 2.0, y1 + k2 / 2.0)
@@ -738,7 +738,7 @@
distances = numpy.linspace(0.0, path.length(), distance_count)
- for i in xrange(len(distances) - 1):
+ for i in range(len(distances) - 1):
position += velocity * (distances[i + 1] - distances[i])
velocity += path.ddxy(distances[i]) * (distances[i + 1] - distances[i])
iposition_plot[:, i + 1] = position
@@ -868,7 +868,7 @@
velocity_drivetrain.robot_radius_r)
Ter = numpy.matrix([[0.5, 0.5], [-1.0 / width, 1.0 / width]])
- for i in xrange(len(length_plan_t)):
+ for i in range(len(length_plan_t)):
xva_plan[0, i] = length_plan_x[i][0, 0]
xva_plan[1, i] = length_plan_v[i]
xva_plan[2, i] = length_plan_a[i]
@@ -895,7 +895,7 @@
R = numpy.matrix(numpy.diag([1.0 / (12.0**2), 1.0 / (12.0**2)]))
kMinVelocity = 0.1
- for i in xrange(len(length_plan_t)):
+ for i in range(len(length_plan_t)):
states[:, i] = state
theta = state[2, 0]
@@ -983,7 +983,7 @@
return 2.0 + 0.0001 * x
v = 0.0
- for _ in xrange(10):
+ for _ in range(10):
dx = 4.0 / 10.0
v = integrate_accel_for_distance(a, v, 0.0, dx)
print('v', v)
diff --git a/motors/pistol_grip/generate_cogging.py b/motors/pistol_grip/generate_cogging.py
index 9cecfe2..9eaf537 100644
--- a/motors/pistol_grip/generate_cogging.py
+++ b/motors/pistol_grip/generate_cogging.py
@@ -28,30 +28,30 @@
data = [0.0] * 4096
min_zero = 4096
max_zero = 0
- for i in xrange(0, 4096):
+ for i in range(0, 4096):
if data_count[i] == 0:
min_zero = min(i, min_zero)
max_zero = max(i, min_zero)
- for i in xrange(0, 4096):
+ for i in range(0, 4096):
if data_count[i] != 0:
data[i] = data_sum[i] / data_count[i]
if min_zero == 0 and max_zero == 4095:
- for i in xrange(0, 4096):
+ for i in range(0, 4096):
if data_count[i] != 0:
while i > 0:
data[i - 1] = data[i]
i -= 1
break;
- for i in reversed(xrange(0, 4096)):
+ for i in reversed(range(0, 4096)):
if data_count[i] != 0:
while i < 4095:
data[i + 1] = data[i]
i += 1
break;
else:
- for i in xrange(0, 4096):
+ for i in range(0, 4096):
if data_count[i] == 0:
if i < (min_zero + max_zero) / 2:
data[i] = data[min_zero - 1]
diff --git a/y2014/control_loops/python/claw.py b/y2014/control_loops/python/claw.py
index cade03d..346a188 100755
--- a/y2014/control_loops/python/claw.py
+++ b/y2014/control_loops/python/claw.py
@@ -401,7 +401,7 @@
upper_bound = (initial_X[1, 0] if initial_X[1, 0] > goal[1, 0] else
goal[1, 0]) + max_separation_error
- for i in xrange(iterations):
+ for i in range(iterations):
U = claw.K * (goal - claw.X)
U = ScaleU(claw, U, claw.K, goal - claw.X)
claw.Update(U)
diff --git a/y2014/control_loops/python/shooter.py b/y2014/control_loops/python/shooter.py
index 9287dae..e25c72b 100755
--- a/y2014/control_loops/python/shooter.py
+++ b/y2014/control_loops/python/shooter.py
@@ -217,7 +217,7 @@
[[goal_position], [0.0],
[-sprung_shooter.A[1, 0] / sprung_shooter.A[1, 2] * goal_position]])
voltage = numpy.matrix([[0.0]])
- for _ in xrange(500):
+ for _ in range(500):
U = sprung_shooter.K * (R - sprung_shooter.X_hat)
U = ClipDeltaU(sprung_shooter, voltage, U)
sprung_shooter.Y = raw_sprung_shooter.Y + 0.01
@@ -240,7 +240,7 @@
R = numpy.matrix([[goal_position], [0.0],
[-shooter.A[1, 0] / shooter.A[1, 2] * goal_position]])
voltage = numpy.matrix([[0.0]])
- for _ in xrange(500):
+ for _ in range(500):
U = shooter.K * (R - shooter.X_hat)
U = ClipDeltaU(shooter, voltage, U)
shooter.Y = raw_shooter.Y + 0.01
diff --git a/y2016/control_loops/python/arm.py b/y2016/control_loops/python/arm.py
index 96975bb..9e6de34 100755
--- a/y2016/control_loops/python/arm.py
+++ b/y2016/control_loops/python/arm.py
@@ -261,7 +261,7 @@
shooter_profile.SetGoal(goal[2, 0])
U_last = numpy.matrix(numpy.zeros((2, 1)))
- for i in xrange(iterations):
+ for i in range(iterations):
X_hat = arm.X
if observer is not None:
diff --git a/y2016/control_loops/python/shooter.py b/y2016/control_loops/python/shooter.py
index b3c988c..5e3eddb 100755
--- a/y2016/control_loops/python/shooter.py
+++ b/y2016/control_loops/python/shooter.py
@@ -190,7 +190,7 @@
else:
initial_t = 0
- for i in xrange(iterations):
+ for i in range(iterations):
X_hat = shooter.X
if observer_shooter is not None:
diff --git a/y2016/control_loops/python/shoulder.py b/y2016/control_loops/python/shoulder.py
index 6c07725..e17bc7e 100755
--- a/y2016/control_loops/python/shoulder.py
+++ b/y2016/control_loops/python/shoulder.py
@@ -188,7 +188,7 @@
else:
initial_t = 0
- for i in xrange(iterations):
+ for i in range(iterations):
X_hat = shoulder.X
if observer_shoulder is not None:
diff --git a/y2016/control_loops/python/wrist.py b/y2016/control_loops/python/wrist.py
index 6de8551..a775494 100755
--- a/y2016/control_loops/python/wrist.py
+++ b/y2016/control_loops/python/wrist.py
@@ -184,7 +184,7 @@
else:
initial_t = 0
- for i in xrange(iterations):
+ for i in range(iterations):
X_hat = wrist.X
if observer_wrist is not None:
diff --git a/y2017/control_loops/python/column.py b/y2017/control_loops/python/column.py
index 70cd649..7c34ee6 100755
--- a/y2017/control_loops/python/column.py
+++ b/y2017/control_loops/python/column.py
@@ -273,7 +273,7 @@
profile.SetGoal(goal[2, 0])
U_last = numpy.matrix(numpy.zeros((2, 1)))
- for i in xrange(iterations):
+ for i in range(iterations):
observer_column.Y = column.Y
observer_column.CorrectObserver(U_last)
diff --git a/y2017/control_loops/python/hood.py b/y2017/control_loops/python/hood.py
index 58bd15e..7f875c9 100755
--- a/y2017/control_loops/python/hood.py
+++ b/y2017/control_loops/python/hood.py
@@ -230,7 +230,7 @@
profile.SetGoal(goal[0, 0])
U_last = numpy.matrix(numpy.zeros((1, 1)))
- for i in xrange(iterations):
+ for i in range(iterations):
observer_hood.Y = hood.Y
observer_hood.CorrectObserver(U_last)
diff --git a/y2017/control_loops/python/indexer.py b/y2017/control_loops/python/indexer.py
index 53b856c..0c5cb6b 100755
--- a/y2017/control_loops/python/indexer.py
+++ b/y2017/control_loops/python/indexer.py
@@ -215,7 +215,7 @@
else:
initial_t = 0
- for i in xrange(iterations):
+ for i in range(iterations):
X_hat = indexer.X
if observer_indexer is not None:
diff --git a/y2017/control_loops/python/shooter.py b/y2017/control_loops/python/shooter.py
index a825ff0..69f0af4 100755
--- a/y2017/control_loops/python/shooter.py
+++ b/y2017/control_loops/python/shooter.py
@@ -285,7 +285,7 @@
initial_t = 0
last_U = numpy.matrix([[0.0]])
- for i in xrange(iterations):
+ for i in range(iterations):
X_hat = shooter.X
if observer_shooter is not None:
diff --git a/y2017/control_loops/python/turret.py b/y2017/control_loops/python/turret.py
index e8829eb..c5e4269 100755
--- a/y2017/control_loops/python/turret.py
+++ b/y2017/control_loops/python/turret.py
@@ -201,7 +201,7 @@
profile.SetGoal(goal[0, 0])
U_last = numpy.matrix(numpy.zeros((1, 1)))
- for i in xrange(iterations):
+ for i in range(iterations):
observer_turret.Y = turret.Y
observer_turret.CorrectObserver(U_last)
diff --git a/y2018/control_loops/python/arm_trajectory.py b/y2018/control_loops/python/arm_trajectory.py
index 464d16e..3531dc7 100755
--- a/y2018/control_loops/python/arm_trajectory.py
+++ b/y2018/control_loops/python/arm_trajectory.py
@@ -509,7 +509,7 @@
int_d = 0.0
int_vel = prev_velocity
num_steps = 10
- for _ in xrange(num_steps):
+ for _ in range(num_steps):
int_accel_t = self.compute_feasable_back_acceleration(
dynamics, prev_distance - int_d, int_vel, vmax,
alpha_unitizer)
@@ -585,7 +585,7 @@
int_d = 0.0
int_vel = prev_velocity
num_steps = 10
- for _ in xrange(num_steps):
+ for _ in range(num_steps):
int_accel_t = self.compute_feasable_forwards_acceleration(
dynamics, prev_distance + int_d, int_vel, vmax,
alpha_unitizer)
diff --git a/y2018/control_loops/python/intake.py b/y2018/control_loops/python/intake.py
index b76016a..3802505 100755
--- a/y2018/control_loops/python/intake.py
+++ b/y2018/control_loops/python/intake.py
@@ -276,7 +276,7 @@
# intake.Y[0,0] = intake.X[0,0]
# observer_intake.X_hat[0,0] = intake.X[0,0]
- for i in xrange(iterations):
+ for i in range(iterations):
X_hat = intake.X
if observer_intake is not None:
diff --git a/y2020/control_loops/python/flywheel.py b/y2020/control_loops/python/flywheel.py
index f284450..caaee96 100755
--- a/y2020/control_loops/python/flywheel.py
+++ b/y2020/control_loops/python/flywheel.py
@@ -205,7 +205,7 @@
else:
initial_t = 0
- for i in xrange(iterations):
+ for i in range(iterations):
X_hat = flywheel.X
if observer_flywheel is not None: