from robot.car import SimpleCar 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
colors = ['b', 'g', 'r', 'c', 'm', 'y']
# 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)
ball = np.array([1.4, 0.30])
x0 = np.array([0, 0.5, 0, 0])
car = SimpleCar(x0)
car.attach_sensor(BeaconSensor(decay_coeff=25), lambda x: x[0:2])
localizer = LocalizerBot(car,ball)
x0 = np.mat(localizer.x)
localizer.attach_sensor(FOVSensor(localizer.x, fov_angle=2*pi, decay_coeff=25), lambda x: localizer.fov_state(x))
s.add_robot(localizer)
# Number of timesteps
T = 30 #arg
# Dynamics and measurement noise
num_states = localizer.NX
num_ctrls = localizer.NU
num_measure = len(beacons)+1+1 #arg/make part of robot observe
'''
print d
stop
print d['dat_states'].shape
print d['dat_ctrls'].shape
'''
# Set up environment #args
beacons=[Beacon(np.array([0.3,0.7])),
Beacon(np.array([0.8,-0.1]))]
obstacles = [CircularObstacle(0.5, 0.63, 0.08)]
s = SimEnv2D(bounds=[-0.1, 1.3, -0.1, 1], beacons=beacons, obstacles=obstacles)
x0 = d['dat_states'][:,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)+2 #arg/make part of robot observe
Q = np.mat(np.diag([0.000001]*num_states)) #arg
Q[2,2] = 0 # Gets out of hand if noise in theta or phi
Q[3,3] = 0 # Can also add theta/phi to measurement like Sameep #TODO?
R = np.mat(np.diag([0.0005]*num_measure)) #arg
]
beacons = beacons[1:]
obstacles = [
RectangularObstacle(
np.array([[-0.5, 0.3], [-0.5, 0.5], [-0.10, 0.5], [-0.10, 0.3]],
float).T),
RectangularObstacle(
np.array([[0.5, 0.5], [0.5, 0.7], [0.10, 0.7], [0.10, 0.5]], float).T)
]
obstacles = obstacles[0:1]
s = SimEnv2D(bounds=[-0.5, 0.5, -0.1, 1.2],
beacons=beacons,
obstacles=obstacles)
x0 = np.array([-0.4, 0.2, 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([0.0000001] * num_states)) #arg
R = np.mat(np.diag([0.001] * num_measure)) #arg
R[2, 2] = 0.000005
# Sample noise
dynamics_noise = mvn([0] * num_states, Q, T - 1).T * 0 #FIXME
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
from sim_env import SimEnv2D
from robot.car import SimpleCar
from optimize import scp_solver_states
from utils import mat2tuple
from random import random
########################################
#TODO WRAP THIS ALL UP INTO A FUNCTION #
########################################
# Set up environment
s = SimEnv2D(bounds=[-1, 1, -1, 1])
x0 = np.array([0, 0, 0, np.pi / 2])
car = SimpleCar(x0)
s.add_robot(car)
T = 30
X_bar = np.mat(np.zeros((car.NX, T)))
U_bar = np.mat(np.zeros((car.NU, T - 1)))
#U_bar = np.mat(np.random.random_sample((car.NU, T-1)))/5
for t in xrange(1, T):
U_bar[0, t - 1] = 0.3
U_bar[1, t - 1] = 0.1
if t > T / 2:
U_bar[0, t - 1] = 0
U_bar[1, t - 1] = -U_bar[1, t - 1]
X_bar[:, t] = car.dynamics(X_bar[:, t - 1], U_bar[:, t - 1])
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)
ball = np.array([1.4, 0.30])
x0 = np.array([0, 0.5, 0, 0])
car = SimpleCar(x0)
car.attach_sensor(BeaconSensor(decay_coeff=25), lambda x: x[0:2])
localizer = LocalizerBot(car, ball)
x0 = np.mat(localizer.x)
localizer.attach_sensor(
FOVSensor(localizer.x, fov_angle=2 * pi, decay_coeff=25),
lambda x: localizer.fov_state(x))
s.add_robot(localizer)
# Number of timesteps
T = 30 #arg
# Dynamics and measurement noise
num_states = localizer.NX
from sim_env import SimEnv2D
from robot.car import SimpleCar
from optimize import scp_solver_states
from utils import mat2tuple
from random import random
########################################
#TODO WRAP THIS ALL UP INTO A FUNCTION #
########################################
# Set up environment
s = SimEnv2D(bounds=[-1, 1, -1, 1])
x0 = np.array([0, 0, 0, np.pi/2])
car = SimpleCar(x0)
s.add_robot(car)
T = 30
X_bar = np.mat(np.zeros((car.NX, T)))
U_bar = np.mat(np.zeros((car.NU, T-1)))
#U_bar = np.mat(np.random.random_sample((car.NU, T-1)))/5
for t in xrange(1,T):
U_bar[0,t-1] = 0.3
U_bar[1,t-1] = 0.1
if t > T / 2:
U_bar[0,t-1] = 0
U_bar[1,t-1] = -U_bar[1,t-1]
X_bar[:,t] = car.dynamics(X_bar[:,t-1], U_bar[:, t-1])
