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
ax = plt.gca()
s.draw(ax=ax)
links.draw_trajectory(mat2tuple(X_bar.T))
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
ax = plt.gca()
s.draw(ax=ax)
links.draw_trajectory(mat2tuple(X_bar.T))
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), lambda x: links.forward_kinematics(origin, x)) #links.attach_sensor(BeaconSensor(decay_coeff=25), lambda x: x[0:2]) s.add_robot(links) T = 20 num_states = links.NX
from objects import Point2D
from sensors import ExtendedCamera2D
from numpy import pi, array, mat, cos, sin, zeros, ones
from optimize import scp_solver
s = SimEnv2D()
s.add_object(Point2D(array([0.8, 0.8]), Sigma=mat('0.01 0; 0 0.01')))
w = f = 500
# 2-link arm
xy = [0, 0]
#thetas = [pi/2, 5*pi/6]
thetas = [0, 0]
l1 = 0.5; l2 = 0.5
two_link = Links(xy, thetas, l1, l2)
# Sensor
cam = ExtendedCamera2D(f, 0, 0, -pi/4, w, ks=array([-0.25, 0.11]),\
color='k')
# Attach sensor to elbow of arm facing direction of second link
def pos_fn(x):
return x[0] + cos(x[2]+pi/2)*l1,\
x[1] + sin(x[2]+pi/2)*l1,\
x[2] + x[3]
two_link.attach_sensor(cam, pos_fn)
s.add_robot(two_link)
# Draw scene
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.52, 0.1]) start_pt = np.array([0.10, -0.3]) s = SimEnv2D(bounds=[-1, 1, -1, 1], beacons=beacons) #s = SimEnv2D(bounds=[-3, 3, -3, 3], 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)) print xN #xN = links.forward_kinematics(origin, thetaN) #xN = np.mat(xN).T links.attach_sensor(BeaconSensor(decay_coeff=15),
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 print xN #xN = links.forward_kinematics(origin, thetaN) #xN = np.mat(xN).T
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),
lambda x: links.forward_kinematics(origin, x))
#links.attach_sensor(BeaconSensor(decay_coeff=25), lambda x: x[0:2])
s.add_robot(links)
T = 20
