def draw_beacons(self): if not self.beacons: return bs = BeaconSensor() delta = (self.bounds[1] - self.bounds[0])/100.0 xs = np.arange(self.bounds[0], self.bounds[1], delta) ys = np.arange(self.bounds[2], self.bounds[3], delta) Z = np.zeros((len(ys), len(xs))) #links = Links(np.array([0, 0]), state_rep='angles') for j in xrange(len(xs)): for k in xrange(len(ys)): x = xs[j]; y = ys[k]; #xyef = links.forward_kinematics(np.array([0,0]), np.array([x, y])) Z[len(ys)-1-k,j] = np.linalg.norm(bs.observe(self, np.array([x,y])),2) #Z[len(ys)-1-k,j] = np.linalg.norm(bs.observe(self, xyef),2) ax = plt.gca() plt.imshow(Z, cmap=cm.gray, extent=self.bounds)
def draw_beacons(self): if not self.beacons: return bs = BeaconSensor() delta = (self.bounds[1] - self.bounds[0]) / 100.0 xs = np.arange(self.bounds[0], self.bounds[1], delta) ys = np.arange(self.bounds[2], self.bounds[3], delta) Z = np.zeros((len(ys), len(xs))) #links = Links(np.array([0, 0]), state_rep='angles') for j in xrange(len(xs)): for k in xrange(len(ys)): x = xs[j] y = ys[k] #xyef = links.forward_kinematics(np.array([0,0]), np.array([x, y])) Z[len(ys) - 1 - k, j] = np.linalg.norm(bs.observe(self, np.array([x, y])), 2) #Z[len(ys)-1-k,j] = np.linalg.norm(bs.observe(self, xyef),2) ax = plt.gca() plt.imshow(Z, cmap=cm.gray, extent=self.bounds)
from sensors import BeaconSensor from utils import mat2tuple import random from math import log from numpy.random import multivariate_normal as mvn from kalman_filter import ekf_update # Set up environment #args beacons = [Beacon(np.array([-1, -1])), Beacon(np.array([1, 1]))] beacons = beacons[1:2] s = SimEnv2D(bounds=[-2, 2, -2, 2], beacons=beacons) x0 = np.array([-1, 1, -np.pi / 4, 0]) car = SimpleCar(x0) car.attach_sensor(BeaconSensor(), lambda x: x[0:2]) s.add_robot(car) # Number of timesteps T = 20 #arg # Dynamics and measurement noise num_states = car.NX num_ctrls = car.NU num_measure = len(beacons) #arg/make part of robot observe Q = np.mat(np.diag([0.0005] * num_states)) #arg Q[2, 2] = 0 # Gets out of hand if noise in theta or phi Q[3, 3] = 0 R = np.mat(np.diag([0.00005] * num_measure)) * 0.001 #arg # Sample noise
# Set up environment #args beacons = [ Beacon(np.array([0.2, 0.2])), Beacon(np.array([1.2, 0.5])), Beacon(np.array([0.2, 0.8])) ] #obstacles = [RectangularObstacle(np.array([[0.75, 0.2], [0.75, 0.4], [0.85, 0.4], [0.85,0.2]], float).T),\ # RectangularObstacle(np.array([[0.5,0.85], [1.15,0.85], [1.15,0.6], [0.5,0.6]], float).T)] obstacles = [] s = SimEnv2D(bounds=[-0.1, 1.5, -0.1, 1], beacons=beacons, obstacles=obstacles) x0 = np.array([0, 0.5, 0, 0]) car = SimpleCar(x0) car.attach_sensor(BeaconSensor(decay_coeff=25), lambda x: x[0:2]) s.add_robot(car) # Number of timesteps T = 30 #arg # Dynamics and measurement noise num_states = car.NX num_ctrls = car.NU num_measure = len(beacons) + 1 #arg/make part of robot observe Q = np.mat(np.diag([1e-5] * num_states)) #arg Q[2, 2] = 1e-8 # Gets out of hand if noise in theta or phi Q[3, 3] = 1e-8 # Can also add theta/phi to measurement like Sameep #TODO? R = np.mat(np.diag([0.005] * num_measure)) #arg R[3, 3] = 1e-9
x0 = np.mat(theta0).T print links.forward_kinematics(origin, theta0) #x0 = links.forward_kinematics(origin, theta0) #x0 = np.mat(x0).T # hack xN #thetaN = np.array([-3.8, -1.9]) # can be looked up using IK thetaN = links.inverse_kinematics(origin, ball) #xN = np.vstack((thetaN.T, ball.T)) #xN = np.reshape(xN, (4,1)) xN = np.mat(thetaN).T print xN #xN = links.forward_kinematics(origin, thetaN) #xN = np.mat(xN).T links.attach_sensor(BeaconSensor(decay_coeff=15), lambda x: links.forward_kinematics(origin, x)) #links.attach_sensor(BeaconSensor(decay_coeff=25), lambda x: x[0:2]) localizer = links # HAAAAAACK!!!!!! #localizer = LocalizerBot(links, ball) #x0 = np.mat(localizer.x).T; #localizer.attach_sensor(FOVSensor(localizer.x, fov_angle=2*pi, decay_coeff=25), lambda x: localizer.fov_state(x)) s.add_robot(localizer) T = 20 num_states = localizer.NX num_ctrls = localizer.NU num_measure = len(beacons) #+1 #arg/make part of robot observe Q = np.mat(np.diag([1e-5] * num_states)) #arg
links = Links(theta0, origin=origin, state_rep='angles') x0 = np.mat(theta0).T print links.forward_kinematics(origin, theta0) #x0 = links.forward_kinematics(origin, theta0) #x0 = np.mat(x0).T # hack xN #thetaN = np.array([-3.8, -1.9]) # can be looked up using IK thetaN = links.inverse_kinematics(origin, ball) xN = np.vstack((thetaN.T, ball.T)) xN = np.reshape(xN, (4,1)) print xN #xN = links.forward_kinematics(origin, thetaN) #xN = np.mat(xN).T links.attach_sensor(BeaconSensor(decay_coeff=15), lambda x: links.forward_kinematics(origin, x)) #links.attach_sensor(BeaconSensor(decay_coeff=25), lambda x: x[0:2]) localizer = LocalizerBot(links, ball) x0 = np.mat(localizer.x).T; localizer.attach_sensor(FOVSensor(localizer.x, fov_angle=2*pi, decay_coeff=25), lambda x: localizer.fov_state(x)) s.add_robot(localizer) T = 20 num_states = localizer.NX num_ctrls = localizer.NU num_measure = len(beacons)+1#+1 #arg/make part of robot observe Q = np.mat(np.diag([1e-5]*num_states)) #arg Q[2,2] = 1e-10