def game_process(conn): env = HuskyNavigateEnv(config='gibson_configs/beechwood_c0_rgb_skip50_random_initial_separate.yaml') conn.send(0) done = False while True: cmd, arg = conn.recv() if cmd=='reset': ob = env.reset() done = False conn.send(ob) elif cmd=='action': ob, reward, done, info = env.step(arg) r, p, yaw = env.robot.body_rpy rot_speed = np.array( [[np.cos(-yaw), -np.sin(-yaw), 0], [np.sin(-yaw), np.cos(-yaw), 0], [ 0, 0, 1]] ) vx, vy, vz = np.dot(rot_speed, env.robot.robot_body.speed()) avx, avy, avz = np.dot(rot_speed, env.robot.robot_body.angular_speed()) vel = [vx, vy, vz, avx, avy, avz] conn.send([ob, reward, running, vel]) elif cmd=='observe': eye_pos, eye_quat = env.get_eye_pos_orientation() pose = [eye_pos, eye_quat] ob = env.render_observations(pose) conn.send([ob]) elif cmd=='running': running = not done conn.send([running]) elif cmd=='stop': break env.close() conn.send(0) conn.close()
from gibson.envs.husky_env import HuskyNavigateEnv from gibson.utils.play import play from gibson.core.render.profiler import Profiler import os config_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), '..', 'configs', 'benchmark.yaml') print(config_file) if __name__ == '__main__': import argparse parser = argparse.ArgumentParser( formatter_class=argparse.ArgumentDefaultsHelpFormatter) parser.add_argument('--config', type=str, default=config_file) parser.add_argument('--gpu', type=int, default=0) args = parser.parse_args() #env = HuskyNavigateEnv(human=True, timestep=timestep, frame_skip=frame_skip, mode="RGB", is_discrete = True, resolution=args.resolution) env = HuskyNavigateEnv(config=args.config, gpu_idx=args.gpu) env.reset() frame = 0 while frame < 10000: with Profiler("env step"): env.step(2) frame += 1 print("frame {}".format(frame))
## -45, -30, -15, 0, 15, 30, 45 for i in range(7): theta = -3/12.0*math.pi + i * math.pi/12 diff_theta_list.append(theta) source_node_idx = random.randint(0, num_nodes-1) x0 = path_finder.nodes_x[source_node_idx] y0 = path_finder.nodes_y[source_node_idx] theta0 = pose_theta_list[random.randint(0, 23)] left_pose = [x0, y0, theta0] file_addr = '/home/reza/Datasets/GibsonEnv/my_code/visual_servoing/sample_image_pairs' current_pose = left_pose pos, orn = func_pose2posAndorn(current_pose, mapper_scene2z[scene_name]) env.robot.reset_new_pose(pos, orn) obs, _, _, _ = env.step(4) obs_rgb = obs['rgb_filled'] cv2.imwrite('{}/left_img.png'.format(file_addr), obs_rgb[:,:,::-1]) right_pose_list = [] for i in range(len(theta_list)): for j in range(len(dist_list)): #temp_theta = plus_theta_fn(theta0, theta_list[random.randint(0, 6)]) #temp_dist = dist_list[random.randint(0, 6)] location_theta = plus_theta_fn(theta0, theta_list[i]) location_dist = dist_list[j] x1 = x0 + location_dist * math.cos(location_theta) y1 = y0 + location_dist * math.sin(location_theta) left_pixel = path_finder.point_to_pixel(left_pose) right_pixel = path_finder.point_to_pixel((x1, y1))
control_signal = -0.5 control_signal_omega = 0.5 v = 0 omega = 0 kp = 100 ki = 0.1 kd = 25 ie = 0 de = 0 olde = 0 ie_omega = 0 de_omega = 0 olde_omage = 0 for i in range(1000): obs, _, _, _ = env.step([0,0,0,0]) vs = [] control_signals = [] omegas = [] control_signals_omega = [] for i in range(12000): e = control_signal - v de = e - olde ie += e olde = e pid_v = kp * e + ki * ie + kd * de e_omega = control_signal_omega - omega de_omega = e_omega - olde_omage ie_omega += e_omega pid_omega = kp * e_omega + ki * ie_omega + kd * de_omega