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 num_ctrls = links.NU num_measure = len(beacons)#+1 #arg/make part of robot observe Q = np.mat(np.diag([1e-5]*num_states)) #arg #Q[2,2] = 1e-4 #Q[3,3] = 1e-4 R = np.mat(np.diag([0.0005]*num_measure)) #arg #R[1,1] = 1e-3 #R[2,2] = 1e-3
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
links.attach_sensor(BeaconSensor(decay_coeff=15),
lambda x: links.forward_kinematics(origin, x))
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),
lambda x: links.forward_kinematics(origin, x))
#links.attach_sensor(BeaconSensor(decay_coeff=25), lambda x: x[0:2])
#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
num_ctrls = links.NU
num_measure = len(beacons) #+1 #arg/make part of robot observe
Q = np.mat(np.diag([1e-5] * num_states)) #arg
#Q[2,2] = 1e-4
#Q[3,3] = 1e-4
R = np.mat(np.diag([0.0005] * num_measure)) #arg
#R[1,1] = 1e-3
#R[2,2] = 1e-3
