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)