"""
env = Environment() # create the environment
env.SetViewer("qtcoin") # start the viewer
env.Load(os.getcwd() + "/data/wam_scene.env.xml") # load a scene
beacons = [Beacon(np.mat(np.array([-1.3, 0.4, 2.0])).T)]
beacons.append(Beacon(np.mat(np.array([-0.0, 0.2, 1.0])).T))
# beacons = [Beacon(np.mat(np.array([0.0, 2.0, 2.0])).T)]
s = SuperRaveEnv(rave_env=env, beacons=beacons)
s.draw()
robot = env.GetRobots()[0] # get the first robot
arm = BarretWAM(robot, env)
arm.attach_sensor(BeaconSensor3D(decay_coeff=25), lambda x: arm.traj_pos(x))
s.add_robot(arm)
num_states = arm.NX
num_ctrls = arm.NU
num_measure = len(beacons) # arg/make part of robot observe
Q = np.mat(np.diag([1e-7] * num_states)) # arg
# Q[2,2] = 1e-8 # Gets out of hand if noise in theta or phi
R = np.mat(np.diag([1e-5] * num_measure)) # arg
# R[3,3] = 5e-7
T = 20
x0 = np.array([-0.5] * arm.NX)
du = np.array([0.0, 0.1, -0.02, -0.05, 0.04, 0.02, 0.1])
du = np.mat(du).T
for t in range(T - 1):
#U[:,t] = du.T
X[:, t + 1] = arm.dynamics(X[:, t], U[:, t])
arm.draw_trajectory(X)
rho_x = 0.1
rho_u = 0.1
N_iter = 5
print X
X_copy = X.copy()
U_copy = U.copy()
cost_t_fns = []
for t in range(T - 1):
arm_pos = arm.traj_pos(X[:, t])
cost_t_fns.append(lambda x, u, pos=arm_pos.copy(): cost_t(x, u, pos))
fcost = lambda x, pos=X[:, T - 1].copy(), bot=arm: final_cost(x, pos, bot)
opt_states, opt_ctrls, opt_vals = scp_solver_states_quadratize(arm, X_copy, U_copy,\
rho_x, rho_u, cost_t_fns, fcost, N_iter, method='shooting')
arm.draw_trajectory(opt_states, color=numpy.array((0.0, 1.0, 0.0)))
drawRobot = opt_states
getch = _Getch._Getch()
t = 0
while True:
c = getch()
print c
beacons.append(Beacon(np.mat(np.array([-0.0, 0.2, 1.0])).T))
#beacons = [Beacon(np.mat(np.array([0.0, 2.0, 2.0])).T)]
mug_pos = np.mat(np.array([0.0, 1.0, 0.8])).T
mug = env.GetKinBody('mug2')
mugTransform = mug.GetTransform()
mugTransform[0, 3] = mug_pos[0]
mugTransform[1, 3] = mug_pos[1]
mugTransform[2, 3] = mug_pos[2]
mug.SetTransform(mugTransform)
s = SuperRaveEnv(rave_env=env, beacons=beacons)
s.draw()
robot = env.GetRobots()[0] # get the first robot
arm = BarretWAM(robot, env)
arm.attach_sensor(BeaconSensor3D(decay_coeff=25), lambda x: arm.traj_pos(x))
localizer = RaveLocalizerBot(arm, mug_pos)
localizer.attach_sensor(
OpenRAVECamera(KK=np.mat(
np.array([(640.0, 0.0, 320.0), (0.0, 480.0, 240.0), (0.0, 0.0,
1.0)]))),
lambda x: localizer.camera_obj_state(x))
s.add_robot(localizer)
for attachedsensor in robot.GetAttachedSensors():
if attachedsensor.GetSensor().Supports(Sensor.Type.Camera):
attachedsensor.GetSensor().Configure(Sensor.ConfigureCommand.PowerOn)
attachedsensor.GetSensor().Configure(
Sensor.ConfigureCommand.RenderDataOn)
arm.draw_trajectory(opt_states, color=numpy.array((0.0,1.0,0.0)))
drawRobot = opt_states
getch = _Getch._Getch()
t = 0
while True:
c = getch()
print c
if c == 'x':
break;
elif ord(c) == 3 or ord(c) == 4:
exit(0)
elif c == 'n':
print 'drawRobot = X'
drawRobot = X
elif c == 'm':
drawRobot = opt_states
elif c == ',' or c == '<':
t = t - 1
elif c == '.' or c == '>':
t = t + 1
if t >= T-1:
t = T-1
elif t <= 0:
t = 0
print arm.traj_pos(drawRobot[:,t])
env.UpdatePublishedBodies()
