def test_case1(self):
length = 20.0
bearing_noise = 0.0
steering_noise = 0.0
distance_noise = 0.0
myrobot = task.robot(length)
myrobot.set(0.0, 0.0, 0.0)
myrobot.set_noise(bearing_noise, steering_noise, distance_noise)
motions = [[0.0, 10.0], [pi / 6.0, 10], [0.0, 20.0]]
expected_repr = ["[x=0.0 y=0.0 orient=0.0]",
"[x=10.0 y=0.0 orient=0.0]",
"[x=19.861 y=1.4333 orient=0.2886]",
"[x=39.034 y=7.1270 orient=0.2886]"]
self.assertEquals(
expected_repr[0],
repr(myrobot),
"On initialization step: expected %s got %s" % (expected_repr[0],
repr(myrobot)))
for m in range(len(motions)):
myrobot = myrobot.move(motions[m])
self.assertEquals(
expected_repr[m+1],
repr(myrobot),
"On step %d: expected %s got %s" % (m,
expected_repr[m+1],
repr(myrobot)))
def test_case1(self):
length = 20.0
bearing_noise = 0.0
steering_noise = 0.0
distance_noise = 0.0
myrobot = task.robot(length)
myrobot.set(0.0, 0.0, 0.0)
myrobot.set_noise(bearing_noise, steering_noise, distance_noise)
motions = [[0.0, 10.0], [pi / 6.0, 10], [0.0, 20.0]]
expected_repr = ["[x=0.0 y=0.0 orient=0.0]",
"[x=10.0 y=0.0 orient=0.0]",
"[x=19.861 y=1.4333 orient=0.2886]",
"[x=39.034 y=7.1270 orient=0.2886]"]
self.assertEquals(
expected_repr[0],
repr(myrobot),
"On initialization step: expected %s got %s" % (expected_repr[0],
repr(myrobot)))
for m in range(len(motions)):
myrobot = myrobot.move(motions[m])
self.assertEquals(
expected_repr[m+1],
repr(myrobot),
"On step %d: expected %s got %s" % (m,
expected_repr[m+1],
repr(myrobot)))
def test_racetrack(self):
radius = 25.0
params = [10.0, 15.0, 0.0]
myrobot = robot()
myrobot.set(0.0, radius, pi / 2.0)
speed = 1.0 # motion distance is equal to speed (we assume time = 1)
err = 0.0
int_crosstrack_error = 0.0
N = 200
crosstrack_error = myrobot.cte(radius)
for i in range(N * 2):
diff_crosstrack_error = -crosstrack_error
crosstrack_error = myrobot.cte(radius)
diff_crosstrack_error += crosstrack_error
int_crosstrack_error += crosstrack_error
steer = - params[0] * crosstrack_error \
- params[1] * diff_crosstrack_error \
- params[2] * int_crosstrack_error
myrobot = myrobot.move(steer, speed)
if i >= N:
err += crosstrack_error**2
self.assertAlmostEqual(
expected_values[i][0], myrobot.x, 3,
"Robot X coordinate differs at point %d: "
"expected %.3f, got %.3f" %
(i, expected_values[i][0], myrobot.x))
self.assertAlmostEqual(
expected_values[i][1], myrobot.y, 3,
"Robot Y coordinate differs at point %d: "
"expected %.3f, got %.3f" %
(i, expected_values[i][1], myrobot.x))
self.assertAlmostEqual(
expected_values[i][2], myrobot.orientation, 3,
"Robot orientation coordinate differs at point %d: "
"expected %.3f, got %.3f" %
(i, expected_values[i][2], myrobot.x))
self.assertAlmostEqual(
expected_values[i][3], crosstrack_error, 3,
"Crosstrack error differs at point %d: "
"expected %.3f, got %.3f" %
(i, expected_values[i][3], myrobot.x))
self.assertAlmostEqual(
expected_values[i][4], steer, 3,
"Steering angle differs at point %d: "
"expected %.3f, got %.3f" %
(i, expected_values[i][4], myrobot.x))
result = err / float(N)
expected_result = 0.00586850481282
if result < expected_result:
print "INFO: your result %.14f is less than expected %.14f " % (result, expected_result), \
"therefore it might be accepted"
self.assertAlmostEqual(
result, expected_result, 5,
"Your average CTE differs from the expected one. "
"Expected %.5f, got %.5f" % (expected_result, result))
def test_racetrack(self):
radius = 25.0
params = [10.0, 15.0, 0.0]
myrobot = robot()
myrobot.set(0.0, radius, pi / 2.0)
speed = 1.0 # motion distance is equal to speed (we assume time = 1)
err = 0.0
int_crosstrack_error = 0.0
N = 200
crosstrack_error = myrobot.cte(radius)
for i in range(N*2):
diff_crosstrack_error = - crosstrack_error
crosstrack_error = myrobot.cte(radius)
diff_crosstrack_error += crosstrack_error
int_crosstrack_error += crosstrack_error
steer = - params[0] * crosstrack_error \
- params[1] * diff_crosstrack_error \
- params[2] * int_crosstrack_error
myrobot = myrobot.move(steer, speed)
if i >= N:
err += crosstrack_error ** 2
self.assertAlmostEqual(expected_values[i][0], myrobot.x, 3,
"Robot X coordinate differs at point %d: "
"expected %.3f, got %.3f" % (i, expected_values[i][0], myrobot.x))
self.assertAlmostEqual(expected_values[i][1], myrobot.y, 3,
"Robot Y coordinate differs at point %d: "
"expected %.3f, got %.3f" % (i, expected_values[i][1], myrobot.x))
self.assertAlmostEqual(expected_values[i][2], myrobot.orientation, 3,
"Robot orientation coordinate differs at point %d: "
"expected %.3f, got %.3f" % (i, expected_values[i][2], myrobot.x))
self.assertAlmostEqual(expected_values[i][3], crosstrack_error, 3,
"Crosstrack error differs at point %d: "
"expected %.3f, got %.3f" % (i, expected_values[i][3], myrobot.x))
self.assertAlmostEqual(expected_values[i][4], steer, 3,
"Steering angle differs at point %d: "
"expected %.3f, got %.3f" % (i, expected_values[i][4], myrobot.x))
result = err / float(N)
expected_result = 0.00586850481282
if result < expected_result:
print "INFO: your result %.14f is less than expected %.14f " % (result, expected_result), \
"therefore it might be accepted"
self.assertAlmostEqual(result, expected_result, 5,
"Your average CTE differs from the expected one. "
"Expected %.5f, got %.5f" % (expected_result, result))
def test_case1(self):
length = 20.0
bearing_noise = 0.0
steering_noise = 0.0
distance_noise = 0.0
myrobot = task.robot(length)
myrobot.set(30.0, 20.0, 0.0)
myrobot.set_noise(bearing_noise, steering_noise, distance_noise)
expected = [6.004885648174475, 3.7295952571373605, 1.9295669970654687, 0.8519663271732721]
result = myrobot.sense()
self.assertEquals(len(expected), len(result),
"Measurement lengths differ: expected %d got %d" % (len(expected),
len(result)))
for i, j in zip(expected, result):
self.assertAlmostEquals(i, j, 7,
"Measurements differ: expected %s, got %s" % (repr(expected),
repr(result)))
def test_case2(self):
length = 20.0
bearing_noise = 0.0
steering_noise = 0.0
distance_noise = 0.0
myrobot = task.robot(length)
myrobot.set(30.0, 20.0, pi / 5.0)
myrobot.set_noise(bearing_noise, steering_noise, distance_noise)
expected = [5.376567117456516, 3.101276726419402, 1.3012484663475101, 0.22364779645531352]
result = myrobot.sense()
self.assertEquals(len(expected), len(result),
"Measurement lengths differ: expected %d got %d" % (len(expected),
len(result)))
for i, j in zip(expected, result):
self.assertAlmostEquals(i, j, 7,
"Measurements differ: expected %s, got %s" % (repr(expected),
repr(result)))
def test_case1(self):
length = 20.0
bearing_noise = 0.0
steering_noise = 0.0
distance_noise = 0.0
myrobot = task.robot(length)
myrobot.set(30.0, 20.0, 0.0)
myrobot.set_noise(bearing_noise, steering_noise, distance_noise)
expected = [
6.004885648174475, 3.7295952571373605, 1.9295669970654687,
0.8519663271732721
]
result = myrobot.sense()
self.assertEquals(
len(expected), len(result),
"Measurement lengths differ: expected %d got %d" %
(len(expected), len(result)))
for i, j in zip(expected, result):
self.assertAlmostEquals(
i, j, 7, "Measurements differ: expected %s, got %s" %
(repr(expected), repr(result)))
def test_case2(self):
length = 20.0
bearing_noise = 0.0
steering_noise = 0.0
distance_noise = 0.0
myrobot = task.robot(length)
myrobot.set(30.0, 20.0, pi / 5.0)
myrobot.set_noise(bearing_noise, steering_noise, distance_noise)
expected = [
5.376567117456516, 3.101276726419402, 1.3012484663475101,
0.22364779645531352
]
result = myrobot.sense()
self.assertEquals(
len(expected), len(result),
"Measurement lengths differ: expected %d got %d" %
(len(expected), len(result)))
for i, j in zip(expected, result):
self.assertAlmostEquals(
i, j, 7, "Measurements differ: expected %s, got %s" %
(repr(expected), repr(result)))
def test_case2(self):
length = 20.0
bearing_noise = 0.0
steering_noise = 0.0
distance_noise = 0.0
myrobot = task.robot(length)
myrobot.set(0.0, 0.0, 0.0)
myrobot.set_noise(bearing_noise, steering_noise, distance_noise)
motions = [[0.2, 10.] for row in range(10)]
expected_repr = ["[x=0.0 y=0.0 orient=0.0]",
"[x=9.9828 y=0.5063 orient=0.1013]",
"[x=19.863 y=2.0201 orient=0.2027]",
"[x=29.539 y=4.5259 orient=0.3040]",
"[x=38.913 y=7.9979 orient=0.4054]",
"[x=47.887 y=12.400 orient=0.5067]",
"[x=56.369 y=17.688 orient=0.6081]",
"[x=64.273 y=23.807 orient=0.7094]",
"[x=71.517 y=30.695 orient=0.8108]",
"[x=78.027 y=38.280 orient=0.9121]",
"[x=83.736 y=46.485 orient=1.0135]"]
self.assertEquals(
expected_repr[0],
repr(myrobot),
"On initialization step: expected %s got %s" % (expected_repr[0],
repr(myrobot)))
for m in range(len(motions)):
myrobot = myrobot.move(motions[m])
self.assertEquals(
expected_repr[m+1],
repr(myrobot),
"On step %d: expected %s got %s" % (m,
expected_repr[m+1],
repr(myrobot)))
def test_case2(self):
length = 20.0
bearing_noise = 0.0
steering_noise = 0.0
distance_noise = 0.0
myrobot = task.robot(length)
myrobot.set(0.0, 0.0, 0.0)
myrobot.set_noise(bearing_noise, steering_noise, distance_noise)
motions = [[0.2, 10.] for row in range(10)]
expected_repr = ["[x=0.0 y=0.0 orient=0.0]",
"[x=9.9828 y=0.5063 orient=0.1013]",
"[x=19.863 y=2.0201 orient=0.2027]",
"[x=29.539 y=4.5259 orient=0.3040]",
"[x=38.913 y=7.9979 orient=0.4054]",
"[x=47.887 y=12.400 orient=0.5067]",
"[x=56.369 y=17.688 orient=0.6081]",
"[x=64.273 y=23.807 orient=0.7094]",
"[x=71.517 y=30.695 orient=0.8108]",
"[x=78.027 y=38.280 orient=0.9121]",
"[x=83.736 y=46.485 orient=1.0135]"]
self.assertEquals(
expected_repr[0],
repr(myrobot),
"On initialization step: expected %s got %s" % (expected_repr[0],
repr(myrobot)))
for m in range(len(motions)):
myrobot = myrobot.move(motions[m])
self.assertEquals(
expected_repr[m+1],
repr(myrobot),
"On step %d: expected %s got %s" % (m,
expected_repr[m+1],
repr(myrobot)))
y_track = [sin((t*pi/100)+(pi/2))*radius + radius for t in range(100,0,-1)] \
+ [sin((t*pi/100)-(pi/2))*radius + radius for t in range(100,0,-1)]
x_track.append(radius)
y_track.append(0.0)
#Draw the path
x_path = [
0.0,
]
y_path = [
radius,
]
myrobot = robot()
myrobot.set(0.0, radius, pi / 2.0)
speed = 1.0 # motion distance is equal to speed (we assume time = 1)
err = 0.0
int_crosstrack_error = 0.0
N = 200
crosstrack_error = myrobot.cte(radius)
for i in range(N * 2):
diff_crosstrack_error = -crosstrack_error
crosstrack_error = myrobot.cte(radius)
diff_crosstrack_error += crosstrack_error
int_crosstrack_error += crosstrack_error
steer = - params[0] * crosstrack_error \
- params[1] * diff_crosstrack_error \
params = [10.0, 15.0, 0.0]
x_track = [cos((t*pi/100)+(pi/2))*radius + radius for t in range(100)] \
+ [cos((t*pi/100)-(pi/2))*radius + 3*radius for t in range(100)]
y_track = [sin((t*pi/100)+(pi/2))*radius + radius for t in range(100,0,-1)] \
+ [sin((t*pi/100)-(pi/2))*radius + radius for t in range(100,0,-1)]
x_track.append(radius)
y_track.append(0.0)
#Draw the path
x_path = [0.0,]
y_path = [radius,]
myrobot = robot()
myrobot.set(0.0, radius, pi / 2.0)
speed = 1.0 # motion distance is equal to speed (we assume time = 1)
err = 0.0
int_crosstrack_error = 0.0
N = 200
crosstrack_error = myrobot.cte(radius)
for i in range(N*2):
diff_crosstrack_error = - crosstrack_error
crosstrack_error = myrobot.cte(radius)
diff_crosstrack_error += crosstrack_error
int_crosstrack_error += crosstrack_error
steer = - params[0] * crosstrack_error \
- params[1] * diff_crosstrack_error \
