Python SimpleCar Examples

Example #1

File: car_ekf.py Project: jakebarnwell/PythonGenerator

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

Example #2

File: car_localizer_demo.py Project: jakebarnwell/PythonGenerator

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

Example #3

File: car_belief_space_obstacles.py Project: jakebarnwell/PythonGenerator

'''
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

Example #4

File: car_belief_space_obstacles2.py Project: jakebarnwell/PythonGenerator

]
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

Example #5

File: car_ekf.py Project: jakebarnwell/PythonGenerator

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

Example #6

File: car_demo.py Project: jakebarnwell/PythonGenerator

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])

Example #7

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

Example #8

File: car_demo.py Project: jakebarnwell/PythonGenerator

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])