def __init__(self,
world,
clock,
name,
position=VectorN(np.array([0.0, 0.0])),
velocity=None,
acceleration=None,
measurement_range=30.0,
gps_noise=0.2,
imu_noise=0.2,
motion_noise=0.2):
self.world = world
if world.dimensions != position.points.size:
print(
"Robot position does not match world dimensions! Deleting robot!"
)
self.position = VectorN(position.points[:world.dimensions])
# del self
# return
else:
self.position = position
print(self.position.points)
self.clock = clock
self.measurement_range = measurement_range
# self.measurement_noise = measurement_noise
self.gps_noise = gps_noise
self.imu_noise = imu_noise
self.motion_noise = motion_noise
if velocity is None:
self.velocity = VectorN(
np.zeros(shape=self.world.dimensions, dtype=float))
else:
self.velocity = velocity
if acceleration is None:
self.acceleration = VectorN(
np.zeros(shape=self.world.dimensions, dtype=float))
else:
self.acceleration = acceleration
self.name = name
self.world.add_actor(self)
#! Memory of past movement times
self.lastMoveTime = self.clock.get_time()
#! Generate minimums and maximums of degrees of motion
self.maxAcc = 2.0 # 2 m/s
self.minAcc = -2.0 # -2 m/s
def update(self):
dt = self.clock.get_time() - self.lastUpdateTime
self.lastUpdateTime = self.clock.get_time()
robot = self.world.actors["robot1"]
self.world.count += 1
# self.robot1.randMoveByAcc()
robot.moveByAcc(
VectorN(np.zeros(shape=self.world.dimensions, dtype=float)))
gpsLoc = robot.runGPS()
# ! Kalman Filter updates
self.kalman.pos = self.kalman.predict(self.kalman.pos[0],
self.kalman.pos[1],
robot.velocity.points[0],
robot.motion_noise)
self.kalman.variance.append(self.kalman.pos[1])
self.kalman.pos = self.kalman.update(self.kalman.pos[0],
self.kalman.pos[1], gpsLoc.points,
robot.gps_noise)
self.kalman.positions.append(self.kalman.pos)
if self.world.count >= 100000:
viewGraph = GraphViewer()
viewGraph.plotKalman(self.kalman.positions)
print("Final Position: {0}".format(robot.position.points))
if self.visual:
self.visSim.update()
def moveByAcc(self, acc):
dt = self.clock.get_time() - self.lastMoveTime
self.lastMoveTime = self.clock.get_time()
# print(dt)
# Apply some noise to the acceleration value, so the models can mimic
# having error
velocity = self.velocity + \
(acc + self.randNoise(self.motion_noise)) * dt
# print(velocity.points)
# des_position = self.position + (velocity + self.randNoise(self.motion_noise)) * dt
des_position = self.position + velocity * dt
if not self.world.checkCollision(self.position, des_position):
self.position = des_position
self.velocity = velocity
self.acceleration = acc
if self.world.count % 10000 == 0 or self.world.count == 1:
print(self.world.count)
print("Position: {0}, Velocity: {1}, Acceleration: {2}".format(
self.position.points, self.velocity.points, acc.points))
else:
self.velocity = VectorN(
np.zeros(shape=self.world.dimensions, dtype=float))
# if self.world.count % 10000 == 0 or self.world.count == 1:
# print(self.world.count)
# print("Noise: {0}".format(np.random.rand(self.world.dimensions) * self.motion_noise))
print("Collision! Position: {0}, Velocity: {1}, Acceleration: {2}".
format(des_position.points, velocity.points, acc.points))
def SimThread(self):
self.sim.setVel(VectorN(np.array([1.0])), "robot1")
while True:
try:
self.sim.update()
except KeyboardInterrupt:
print("^C")
break
def randMoveByAcc(self):
# Generate random vector that is within minimum and maximum
# acceleration
acc = VectorN(
np.random.rand(self.world.dimensions) *
(self.maxAcc - self.minAcc) + self.minAcc)
# print(acc.points)
self.moveByAcc(acc)
def __init__(self,
origin=VectorN(np.array([0.0, 0.0])),
world_size=np.array([10.0, 10.0]),
wind=False,
landmarks=np.array([])):
self.dimensions = world_size.size
self.world_size = world_size
self.area = self.set_area(world_size)
self.landmarks = landmarks
self.origin = origin
self.actors = {}
# TODO: Add way to represent infinite worlds, use math.inf and
# float("inf") to check
self.halfworld_size = VectorN((self.world_size / 2))
self.upbounds = self.origin + self.halfworld_size
self.lowbounds = self.origin - self.halfworld_size
print(self.upbounds.points)
print(self.lowbounds.points)
self.count = 0
def create_landmarks(self):
for i in range(5):
values = np.array([])
for length in world_size:
np.append(values, random.random() * length)
np.append(self.landmarks, VectorN(values) + self.origin)