import numpy as np import matplotlib.pyplot as plt from sim_env import SimEnv2D, Beacon 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
import os, sys
up_path = os.path.abspath('..')
sys.path.append(up_path)
from numpy import *
from sensors import PinholeCamera2D
from sim_env import SimEnv2D
from world import World2D
from objects import Point2D
from pylab import *
from utils import *
from covar import draw_ellipsoid
from kalman_filter import ekf_update
# World setup
s = SimEnv2D()
w = f = 500
px = 0
py = 0
s.add_object(Point2D(array([px, py])))
cams = list()
cams.append(PinholeCamera2D(f, [px - 0.5, py - .5, pi / 4], w,
color=COLORS[1]))
cams.append(PinholeCamera2D(f, [px - 0.5, py, pi / 4], w, color=COLORS[2]))
cams.append(PinholeCamera2D(f, [px - 0.5, py, 0], w, color=COLORS[3]))
cams.append(PinholeCamera2D(f, [px - 0.75, py, 0], w, color=COLORS[4]))
world = World2D(cams, s)
# Kalman updates
# mus, x in world coordinates
from covar import cov2vec, vec2cov
from optimize import value_iteration_solver
from scipy.io import loadmat
# 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
up_path = os.path.abspath('..')
sys.path.append(up_path)
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.cm as cm
from sim_env import SimEnv2D
from sensors import FOVSensor
from objects import Point2D
import random
from numpy import pi
# Set up environment #args
bounds = [-1, 1, -1, 1]
s = SimEnv2D(bounds=bounds)
fov_sensor = FOVSensor(np.array([0, 0, pi/4]), fov_angle=pi/2)
s.draw()
fov_sensor.draw()
s.objects.append(None)
delta = (bounds[1] - bounds[0])/100.0
xs = np.arange(bounds[0], bounds[1], delta)
ys = np.arange(bounds[2], bounds[3], delta)
Z = np.zeros((len(ys), len(xs)))
for j in xrange(len(xs)):
for k in xrange(len(ys)):
x = xs[j]; y = ys[k];
s.objects[0] = Point2D(np.array([x,y]))
Z[len(ys)-1-k,j] = np.linalg.norm(fov_sensor.observe(s),2)
from robots 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 from covar import cov2vec, vec2cov from optimize import scp_solver_beliefs from scipy.io import loadmat import matplotlib.animation as animation # Set up environment #args beacons = [Beacon(np.array([0.3, 0.7])), Beacon(np.array([1, 0.7]))] s = SimEnv2D(bounds=[-0.1, 1.3, -0.1, 1], beacons=beacons) x0 = np.array([0, 0, np.pi / 4, 0]) car = SimpleCar(x0) car.attach_sensor(BeaconSensor(decay_coeff=100), 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-3] * num_states)) #arg
up_path = os.path.abspath('..')
sys.path.append(up_path)
import numpy as np
import matplotlib.pyplot as plt
from sim_env import SimEnv2D
from robots import Links
from optimize import scp_solver_states
from utils import mat2tuple
########################################
#TODO WRAP THIS ALL UP INTO A FUNCTION #
########################################
# Set up environment
s = SimEnv2D(bounds=[-1, 1, -1, 1])
x0 = np.array([0.1, 0.2])
links = Links(x0)
s.add_robot(links)
T = 50
X_bar = np.mat(np.zeros((links.NX, T)))
U_bar = np.mat(np.random.random_sample((links.NU, T - 1))) / 2
print U_bar
for t in xrange(1, T):
X_bar[:, t] = links.dynamics(X_bar[:, t - 1], U_bar[:, t - 1])
# Plot nominal trajectory
from sim_env import SimEnv2D, Beacon from robots import Links, LocalizerBot, Dot from optimize import scp_solver_beliefs from utils import mat2tuple from sensors import BeaconSensor, FOVSensor from kalman_filter import ekf_update from covar import cov2vec, vec2cov from math import pi colors = ['b', 'g', 'r', 'c', 'm', 'y'] # Set up environment beacons = [Beacon(np.array([-0.0, 0.6]))] ball = np.array([-0.4, 0.15]) start_pt = np.array([0.10, -0.3]) #s = SimEnv2D(bounds=[-1, 1, -1, 1], beacons=beacons) s = SimEnv2D(bounds=[-4, 4, 0, 4], beacons=beacons) #theta0 = np.array([-2.11, 2.65]) #x0 is broken, just set to 0 theta0 = np.array([-2.26, 2.03]) #x0 is broken, just set to 0 origin = np.array([0.0, 0.0]) 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)) xN = np.mat(thetaN).T
sys.path.append(up_path) import numpy as np import matplotlib.pyplot as plt from sim_env import SimEnv2D, Beacon from robots import Links from optimize import scp_solver_beliefs from utils import mat2tuple from sensors import BeaconSensor from kalman_filter import ekf_update from covar import cov2vec, vec2cov from math import pi # Set up environment beacons = [Beacon(np.array([0.8, 0.7])), Beacon(np.array([0.7, 0.7]))] s = SimEnv2D(bounds=[-1, 1, -1, 1], beacons=beacons) #s = SimEnv2D(bounds=[-3, 3, -3, 3], beacons=beacons) theta0 = np.array([-0.5, -0.2]) #x0 is broken, just set to 0 origin = np.array([0.0, 0.0]) links = Links(theta0, origin=origin, state_rep='angles') x0 = np.mat(theta0).T #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 xN = np.mat(thetaN).T #xN = links.forward_kinematics(origin, thetaN) #xN = np.mat(xN).T links.attach_sensor(BeaconSensor(decay_coeff=15),