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)] 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)
time.sleep(0.01) """ 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])
time.sleep(0.01) """ 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])
while not robot.GetController().IsDone(): time.sleep(0.01) """ 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])
#diverge_cost = 0 return 1.0 * numpy.linalg.norm(u_t)**2 + 0.1 * diverge_cost**2 """ def waitrobot(robot): while not robot.GetController().IsDone(): time.sleep(0.01) """ env = Environment() # create the environment env.SetViewer('qtcoin') # start the viewer env.Load(os.getcwd() + '/data/wam_scene.env.xml') # load a scene robot = env.GetRobots()[0] # get the first robot arm = BarretWAM(robot, env) x0 = numpy.array([0.0] * arm.NX) du = numpy.array([0.0] * arm.NU) du = numpy.array([0.0, 0.1, -0.02, -0.05, 0.04, 0.02, 0.1]) du = numpy.mat(du) #U_bar = np.mat(np.random.random_sample((car.NU, T-1)))/5 T = 20 X = numpy.mat(numpy.zeros((arm.NX, T))) U = numpy.mat(numpy.zeros((arm.NU, T - 1))) U = numpy.mat(numpy.random.random_sample((arm.NU, T - 1)) / 10) U[:, 10:] = -2 * U[:, 10:] for t in range(T - 1): #U[:,t] = du.T
import struct import _Getch """ def waitrobot(robot): while not robot.GetController().IsDone(): time.sleep(0.01) """ env = Environment() # create the environment env.SetViewer('qtcoin') # start the viewer env.Load(os.getcwd() + '/data/wam_scene.env.xml') # load a scene robot = env.GetRobots()[0] # get the first robot arm = BarretWAM(robot,env) x0 = numpy.array([0.0] * arm.NX) du = numpy.array([0.0] * arm.NU) du = numpy.array([0.0, 0.1, -0.02, -0.05, 0.04, 0.02, 0.1]) du = numpy.mat(du) #U_bar = np.mat(np.random.random_sample((car.NU, T-1)))/5 T = 20 X = numpy.mat(numpy.zeros((arm.NX, T))) U = numpy.mat(numpy.zeros((arm.NU, T-1))) U = numpy.mat(numpy.random.random_sample((arm.NU, T-1))/5) U[:,10:] = -2 * U[:,10:] for t in range(T-1):