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 s.draw() plt.gca().set_xlim(-1, 1); plt.gca().set_ylim(-1, 1); # Generate a nominal trajectory T = 100 X_bar = mat(zeros((two_link.NX, T))) U_bar = mat(ones((two_link.NU, T-1))/10) for t in xrange(T): X_bar[:,t] = two_link.dynamics(X_bar[:,t-1], U_bar[:, t-1])
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
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)) #links.attach_sensor(BeaconSensor(decay_coeff=25), lambda x: x[0:2]) localizer = links # HAAAAAACK!!!!!! #localizer = LocalizerBot(links, ball) #x0 = np.mat(localizer.x).T; #localizer.attach_sensor(FOVSensor(localizer.x, fov_angle=2*pi, decay_coeff=25), 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 #arg/make part of robot observe Q = np.mat(np.diag([1e-5] * num_states)) #arg #Q[2,2] = 1e-10