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
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?
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
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?
#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;
w = f = 500
localizer.attach_sensor(ExtendedCamera2D(f, localizer.x, w, ks=[-0.33, 0.1]),
lambda x: localizer.fov_state(x))
#localizer.attach_sensor(PinholeCamera2D(f, localizer.x, w),
#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
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])
ball = np.array([1.35, 0])
#ball = np.array([0.8, 0.35])
ball = np.array([1.3, 0.8])
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))
#localizer.attach_sensor(FOVSensor(localizer.x, fov_angle=2*pi, decay_coeff=25), lambda x: localizer.fov_state(x))
w = 0.05
f = 0.1
localizer.attach_sensor(ExtendedCamera2D(f, localizer.x, w, ks=[-0.33, 0.1],
radial_distortion=True, field_of_view=True, depth_of_field=False),
lambda x: localizer.fov_state(x))
#localizer.attach_sensor(PinholeCamera2D(f, localizer.x, w),
#lambda x: localizer.fov_state(x))
s.add_robot(localizer)
# Number of timesteps
T = 30 #arg
