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