async query works in sim

这个提交包含在:
Tony Zhao
2023-12-14 23:48:42 -08:00
父节点 c3c9deeaa3
当前提交 a0905ad3b5
共有 3 个文件被更改,包括 116 次插入 和 19 次删除

查看文件

@ -43,6 +43,7 @@ You can find all scripted/human demo for simulated environments [here](https://d
pip install packaging pip install packaging
pip install h5py pip install h5py
pip install ipython pip install ipython
pip install ray==2.7.1
cd act/detr && pip install -e . cd act/detr && pip install -e .
### Example Usages ### Example Usages

查看文件

@ -391,6 +391,23 @@ python3 vinn_eval.py \
--task_name $TASK_NAME --task_name $TASK_NAME
# debug async model query
conda activate mobile
export MUJOCO_GL=egl
cd /home/tonyzhao/Research/act-plus-plus
CUDA_VISIBLE_DEVICES=0 python3 imitate_episodes.py \
--task_name sim_transfer_cube_scripted \
--ckpt_dir /scr/tonyzhao/train_logs/cube_scripted_diffusion_sweep_6_noEMA_seed1 \
--policy_class Diffusion --chunk_size 32 \
--batch_size 32 --lr 1e-4 --seed 1 \
--num_steps 200000 --eval_every 6000 --validate_every 6000 --save_every 6000 --eval
--------------------------------------------------------------------------------------- ---------------------------------------------------------------------------------------
NOTE: chunk size cannot be any number, try before launching NOTE: chunk size cannot be any number, try before launching

查看文件

@ -10,6 +10,8 @@ from tqdm import tqdm
from einops import rearrange from einops import rearrange
import wandb import wandb
import time import time
import ray
from ray.util.queue import Queue
from constants import FPS from constants import FPS
from constants import PUPPET_GRIPPER_JOINT_OPEN from constants import PUPPET_GRIPPER_JOINT_OPEN
@ -26,6 +28,8 @@ from sim_env import BOX_POSE
import IPython import IPython
e = IPython.embed e = IPython.embed
ray.init()
def get_auto_index(dataset_dir): def get_auto_index(dataset_dir):
max_idx = 1000 max_idx = 1000
for i in range(max_idx+1): for i in range(max_idx+1):
@ -208,10 +212,26 @@ def get_image(ts, camera_names):
curr_image = rearrange(ts.observation['images'][cam_name], 'h w c -> c h w') curr_image = rearrange(ts.observation['images'][cam_name], 'h w c -> c h w')
curr_images.append(curr_image) curr_images.append(curr_image)
curr_image = np.stack(curr_images, axis=0) curr_image = np.stack(curr_images, axis=0)
curr_image = torch.from_numpy(curr_image / 255.0).float().cuda().unsqueeze(0) curr_image = torch.from_numpy(curr_image / 255.0).float().unsqueeze(0)
return curr_image return curr_image
@ray.remote(num_gpus=0.1)
class PolicyLoop:
def __init__(self, policy, input_queue, action_queue):
self.input_queue = input_queue
self.action_queue = action_queue
self.policy = policy # Replace with your AI model initialization
def run(self):
while True:
input = self.input_queue.get()
obs = input['obs'].cuda()
qpos = input['qpos'].cuda()
action = self.policy(qpos, obs)
self.action_queue.put({'action': action.cpu(), 'query_wall_t': input['query_wall_t'], 'query_t': input['query_t']})
def eval_bc(config, ckpt_name, save_episode=True, num_rollouts=50): def eval_bc(config, ckpt_name, save_episode=True, num_rollouts=50):
set_seed(1000) set_seed(1000)
ckpt_dir = config['ckpt_dir'] ckpt_dir = config['ckpt_dir']
@ -229,6 +249,8 @@ def eval_bc(config, ckpt_name, save_episode=True, num_rollouts=50):
actuator_config = config['actuator_config'] actuator_config = config['actuator_config']
use_actuator_net = actuator_config['actuator_network_dir'] is not None use_actuator_net = actuator_config['actuator_network_dir'] is not None
async_query = True
# load policy and stats # load policy and stats
ckpt_path = os.path.join(ckpt_dir, ckpt_name) ckpt_path = os.path.join(ckpt_dir, ckpt_name)
policy = make_policy(policy_class, policy_config) policy = make_policy(policy_class, policy_config)
@ -280,6 +302,17 @@ def eval_bc(config, ckpt_name, save_episode=True, num_rollouts=50):
else: else:
post_process = lambda a: a * stats['action_std'] + stats['action_mean'] post_process = lambda a: a * stats['action_std'] + stats['action_mean']
# async
if async_query:
ray.put(policy)
# Create the communication queues
input_queue = Queue()
action_queue = Queue()
# Start the robot AI loop actor
for _ in range(2):
policy_loop = PolicyLoop.remote(policy, input_queue, action_queue)
policy_loop.run.remote()
# load environment # load environment
if real_robot: if real_robot:
from aloha_scripts.robot_utils import move_grippers # requires aloha from aloha_scripts.robot_utils import move_grippers # requires aloha
@ -291,7 +324,7 @@ def eval_bc(config, ckpt_name, save_episode=True, num_rollouts=50):
env = make_sim_env(task_name) env = make_sim_env(task_name)
env_max_reward = env.task.max_reward env_max_reward = env.task.max_reward
query_frequency = policy_config['num_queries'] query_frequency = num_queries = policy_config['num_queries']
if temporal_agg: if temporal_agg:
query_frequency = 1 query_frequency = 1
num_queries = policy_config['num_queries'] num_queries = policy_config['num_queries']
@ -322,8 +355,8 @@ def eval_bc(config, ckpt_name, save_episode=True, num_rollouts=50):
plt.ion() plt.ion()
### evaluation loop ### evaluation loop
if temporal_agg: if temporal_agg or async_query:
all_time_actions = torch.zeros([max_timesteps, max_timesteps+num_queries, 16]).cuda() all_time_actions = torch.zeros([max_timesteps, max_timesteps+num_queries, 16])
# qpos_history = torch.zeros((1, max_timesteps, state_dim)).cuda() # qpos_history = torch.zeros((1, max_timesteps, state_dim)).cuda()
qpos_history_raw = np.zeros((max_timesteps, state_dim)) qpos_history_raw = np.zeros((max_timesteps, state_dim))
@ -355,18 +388,33 @@ def eval_bc(config, ckpt_name, save_episode=True, num_rollouts=50):
qpos_numpy = np.array(obs['qpos']) qpos_numpy = np.array(obs['qpos'])
qpos_history_raw[t] = qpos_numpy qpos_history_raw[t] = qpos_numpy
qpos = pre_process(qpos_numpy) qpos = pre_process(qpos_numpy)
qpos = torch.from_numpy(qpos).float().cuda().unsqueeze(0) qpos = torch.from_numpy(qpos).float().unsqueeze(0)
# qpos_history[:, t] = qpos # qpos_history[:, t] = qpos
if t % query_frequency == 0: if t % query_frequency == 0:
curr_image = get_image(ts, camera_names) curr_image = get_image(ts, camera_names)
# print('get image: ', time.time() - time2) # print('get image: ', time.time() - time2)
if not async_query:
qpos = qpos.cuda()
curr_image = curr_image.cuda()
if t == 0: if t == 0:
# warm up print('Warming up')
for _ in range(10): if async_query:
policy(qpos, curr_image) num_warmup_steps = 10
print('network warm up done') for _ in tqdm(range(num_warmup_steps)):
time1 = time.time() input_queue.put({'qpos': qpos, 'obs': curr_image, 'query_wall_t': time.time(), 'query_t': t})
while action_queue.qsize() == 0:
time.sleep(0.01)
if action_queue.qsize() > 0:
while action_queue.qsize() > 0:
res_dict = action_queue.get()
query_wall_t = res_dict['query_wall_t']
query_t = res_dict['query_t']
all_actions = res_dict['action']
else:
for _ in range(10):
policy(qpos, curr_image)
time1 = time.time()
### query policy ### query policy
time3 = time.time() time3 = time.time()
@ -394,7 +442,7 @@ def eval_bc(config, ckpt_name, save_episode=True, num_rollouts=50):
k = 0.01 k = 0.01
exp_weights = np.exp(-k * np.arange(len(actions_for_curr_step))) exp_weights = np.exp(-k * np.arange(len(actions_for_curr_step)))
exp_weights = exp_weights / exp_weights.sum() exp_weights = exp_weights / exp_weights.sum()
exp_weights = torch.from_numpy(exp_weights).cuda().unsqueeze(dim=1) exp_weights = torch.from_numpy(exp_weights).unsqueeze(dim=1)
raw_action = (actions_for_curr_step * exp_weights).sum(dim=0, keepdim=True) raw_action = (actions_for_curr_step * exp_weights).sum(dim=0, keepdim=True)
else: else:
raw_action = all_actions[:, t % query_frequency] raw_action = all_actions[:, t % query_frequency]
@ -402,13 +450,44 @@ def eval_bc(config, ckpt_name, save_episode=True, num_rollouts=50):
# # zero out base actions to avoid overshooting # # zero out base actions to avoid overshooting
# raw_action[0, -2:] = 0 # raw_action[0, -2:] = 0
elif config['policy_class'] == "Diffusion": elif config['policy_class'] == "Diffusion":
if t % query_frequency == 0: if async_query:
all_actions = policy(qpos, curr_image) freq_mult = 0.5
# if use_actuator_net: if t == 0 or (t % int(query_frequency*freq_mult) == 0):
# collect_base_action(all_actions, norm_episode_all_base_actions) print(f'{t}: Put')
if real_robot: input_queue.put({'qpos': qpos, 'obs': curr_image, 'query_wall_t': time.time(), 'query_t': t})
all_actions = torch.cat([all_actions[:, :-BASE_DELAY, :-2], all_actions[:, BASE_DELAY:, -2:]], dim=2)
raw_action = all_actions[:, t % query_frequency] # Wait if no action is produced for current step and nothing in the queue
no_action = not all_time_actions[:, t].any()
no_queue = action_queue.qsize() == 0
while no_action and no_queue:
time.sleep(0.01)
print(f'{t}: wait')
no_action = not all_time_actions[:, t].any()
no_queue = action_queue.qsize() == 0
# Get action
if action_queue.qsize() > 0:
print(f'{t}: Get')
while action_queue.qsize() > 0:
res_dict = action_queue.get()
query_wall_t = res_dict['query_wall_t']
query_t = res_dict['query_t']
all_actions = res_dict['action']
all_time_actions[query_t, query_t:query_t+num_queries] = all_actions
all_time_curr_actions = all_time_actions[:, t]
has_action = all_time_curr_actions.any(dim=1)
first_index = torch.arange(len(has_action))[has_action].max().item()
# print(first_index)
raw_action = all_time_curr_actions[first_index]
else:
if t % query_frequency == 0:
all_actions = policy(qpos.cuda(), curr_image.cuda())
# if use_actuator_net:
# collect_base_action(all_actions, norm_episode_all_base_actions)
if real_robot:
all_actions = torch.cat([all_actions[:, :-BASE_DELAY, :-2], all_actions[:, BASE_DELAY:, -2:]], dim=2)
raw_action = all_actions[:, t % query_frequency]
elif config['policy_class'] == "CNNMLP": elif config['policy_class'] == "CNNMLP":
raw_action = policy(qpos, curr_image) raw_action = policy(qpos, curr_image)
all_actions = raw_action.unsqueeze(0) all_actions = raw_action.unsqueeze(0)
@ -454,7 +533,7 @@ def eval_bc(config, ckpt_name, save_episode=True, num_rollouts=50):
duration = time.time() - time1 duration = time.time() - time1
time.sleep(max(0, DT - duration)) time.sleep(max(0, DT - duration))
# time.sleep(max(0, DT - duration - culmulated_delay)) # time.sleep(max(0, DT - duration - culmulated_delay))
if duration >= DT: if duration >= DT and real_robot:
culmulated_delay += (duration - DT) culmulated_delay += (duration - DT)
print(f'Warning: step duration: {duration:.3f} s at step {t} longer than DT: {DT} s, culmulated delay: {culmulated_delay:.3f} s') print(f'Warning: step duration: {duration:.3f} s at step {t} longer than DT: {DT} s, culmulated delay: {culmulated_delay:.3f} s')
# else: # else: