add BASE_DELAY to eval_bc, elevator task
这个提交包含在:
@ -240,29 +240,29 @@ def eval_bc(config, ckpt_name, save_episode=True, num_rollouts=50):
|
|||||||
stats_path = os.path.join(ckpt_dir, f'dataset_stats.pkl')
|
stats_path = os.path.join(ckpt_dir, f'dataset_stats.pkl')
|
||||||
with open(stats_path, 'rb') as f:
|
with open(stats_path, 'rb') as f:
|
||||||
stats = pickle.load(f)
|
stats = pickle.load(f)
|
||||||
if use_actuator_net:
|
# if use_actuator_net:
|
||||||
prediction_len = actuator_config['prediction_len']
|
# prediction_len = actuator_config['prediction_len']
|
||||||
future_len = actuator_config['future_len']
|
# future_len = actuator_config['future_len']
|
||||||
history_len = actuator_config['history_len']
|
# history_len = actuator_config['history_len']
|
||||||
actuator_network_dir = actuator_config['actuator_network_dir']
|
# actuator_network_dir = actuator_config['actuator_network_dir']
|
||||||
|
|
||||||
from act.train_actuator_network import ActuatorNetwork
|
# from act.train_actuator_network import ActuatorNetwork
|
||||||
actuator_network = ActuatorNetwork(prediction_len)
|
# actuator_network = ActuatorNetwork(prediction_len)
|
||||||
actuator_network_path = os.path.join(actuator_network_dir, 'actuator_net_last.ckpt')
|
# actuator_network_path = os.path.join(actuator_network_dir, 'actuator_net_last.ckpt')
|
||||||
loading_status = actuator_network.load_state_dict(torch.load(actuator_network_path))
|
# loading_status = actuator_network.load_state_dict(torch.load(actuator_network_path))
|
||||||
actuator_network.eval()
|
# actuator_network.eval()
|
||||||
actuator_network.cuda()
|
# actuator_network.cuda()
|
||||||
print(f'Loaded actuator network from: {actuator_network_path}, {loading_status}')
|
# print(f'Loaded actuator network from: {actuator_network_path}, {loading_status}')
|
||||||
|
|
||||||
actuator_stats_path = os.path.join(actuator_network_dir, 'actuator_net_stats.pkl')
|
# actuator_stats_path = os.path.join(actuator_network_dir, 'actuator_net_stats.pkl')
|
||||||
with open(actuator_stats_path, 'rb') as f:
|
# with open(actuator_stats_path, 'rb') as f:
|
||||||
actuator_stats = pickle.load(f)
|
# actuator_stats = pickle.load(f)
|
||||||
|
|
||||||
actuator_unnorm = lambda x: x * actuator_stats['commanded_speed_std'] + actuator_stats['commanded_speed_std']
|
# actuator_unnorm = lambda x: x * actuator_stats['commanded_speed_std'] + actuator_stats['commanded_speed_std']
|
||||||
actuator_norm = lambda x: (x - actuator_stats['observed_speed_mean']) / actuator_stats['observed_speed_mean']
|
# actuator_norm = lambda x: (x - actuator_stats['observed_speed_mean']) / actuator_stats['observed_speed_mean']
|
||||||
def collect_base_action(all_actions, norm_episode_all_base_actions):
|
# def collect_base_action(all_actions, norm_episode_all_base_actions):
|
||||||
post_processed_actions = post_process(all_actions.squeeze(0).cpu().numpy())
|
# post_processed_actions = post_process(all_actions.squeeze(0).cpu().numpy())
|
||||||
norm_episode_all_base_actions += actuator_norm(post_processed_actions[:, -2:]).tolist()
|
# norm_episode_all_base_actions += actuator_norm(post_processed_actions[:, -2:]).tolist()
|
||||||
|
|
||||||
pre_process = lambda s_qpos: (s_qpos - stats['qpos_mean']) / stats['qpos_std']
|
pre_process = lambda s_qpos: (s_qpos - stats['qpos_mean']) / stats['qpos_std']
|
||||||
if policy_class == 'Diffusion':
|
if policy_class == 'Diffusion':
|
||||||
@ -285,6 +285,9 @@ def eval_bc(config, ckpt_name, save_episode=True, num_rollouts=50):
|
|||||||
if temporal_agg:
|
if temporal_agg:
|
||||||
query_frequency = 1
|
query_frequency = 1
|
||||||
num_queries = policy_config['num_queries']
|
num_queries = policy_config['num_queries']
|
||||||
|
if real_robot:
|
||||||
|
BASE_DELAY = 15
|
||||||
|
query_frequency -= BASE_DELAY
|
||||||
|
|
||||||
max_timesteps = int(max_timesteps * 1) # may increase for real-world tasks
|
max_timesteps = int(max_timesteps * 1) # may increase for real-world tasks
|
||||||
|
|
||||||
@ -317,10 +320,12 @@ def eval_bc(config, ckpt_name, save_episode=True, num_rollouts=50):
|
|||||||
qpos_list = []
|
qpos_list = []
|
||||||
target_qpos_list = []
|
target_qpos_list = []
|
||||||
rewards = []
|
rewards = []
|
||||||
norm_episode_all_base_actions = [actuator_norm(np.zeros(history_len, 2)).tolist()]
|
# if use_actuator_net:
|
||||||
|
# norm_episode_all_base_actions = [actuator_norm(np.zeros(history_len, 2)).tolist()]
|
||||||
with torch.inference_mode():
|
with torch.inference_mode():
|
||||||
time0 = time.time()
|
time0 = time.time()
|
||||||
DT = 1 / FPS
|
DT = 1 / FPS
|
||||||
|
culmulated_delay = 0
|
||||||
for t in range(max_timesteps):
|
for t in range(max_timesteps):
|
||||||
time1 = time.time()
|
time1 = time.time()
|
||||||
### update onscreen render and wait for DT
|
### update onscreen render and wait for DT
|
||||||
@ -330,6 +335,7 @@ def eval_bc(config, ckpt_name, save_episode=True, num_rollouts=50):
|
|||||||
plt.pause(DT)
|
plt.pause(DT)
|
||||||
|
|
||||||
### process previous timestep to get qpos and image_list
|
### process previous timestep to get qpos and image_list
|
||||||
|
time2 = time.time()
|
||||||
obs = ts.observation
|
obs = ts.observation
|
||||||
if 'images' in obs:
|
if 'images' in obs:
|
||||||
image_list.append(obs['images'])
|
image_list.append(obs['images'])
|
||||||
@ -339,9 +345,19 @@ def eval_bc(config, ckpt_name, save_episode=True, num_rollouts=50):
|
|||||||
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().cuda().unsqueeze(0)
|
||||||
qpos_history[:, t] = qpos
|
qpos_history[:, t] = qpos
|
||||||
curr_image = get_image(ts, camera_names)
|
if t % query_frequency == 0:
|
||||||
|
curr_image = get_image(ts, camera_names)
|
||||||
|
# print('get image: ', time.time() - time2)
|
||||||
|
|
||||||
|
if t == 0:
|
||||||
|
# warm up
|
||||||
|
for _ in range(10):
|
||||||
|
policy(qpos, curr_image)
|
||||||
|
print('network warm up done')
|
||||||
|
time1 = time.time()
|
||||||
|
|
||||||
### query policy
|
### query policy
|
||||||
|
time3 = time.time()
|
||||||
if config['policy_class'] == "ACT":
|
if config['policy_class'] == "ACT":
|
||||||
if t % query_frequency == 0:
|
if t % query_frequency == 0:
|
||||||
if vq:
|
if vq:
|
||||||
@ -354,8 +370,10 @@ def eval_bc(config, ckpt_name, save_episode=True, num_rollouts=50):
|
|||||||
else:
|
else:
|
||||||
# e()
|
# e()
|
||||||
all_actions = policy(qpos, curr_image)
|
all_actions = policy(qpos, curr_image)
|
||||||
if use_actuator_net:
|
# if use_actuator_net:
|
||||||
collect_base_action(all_actions, norm_episode_all_base_actions)
|
# 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)
|
||||||
if temporal_agg:
|
if temporal_agg:
|
||||||
all_time_actions[[t], t:t+num_queries] = all_actions
|
all_time_actions[[t], t:t+num_queries] = all_actions
|
||||||
actions_for_curr_step = all_time_actions[:, t]
|
actions_for_curr_step = all_time_actions[:, t]
|
||||||
@ -368,51 +386,67 @@ def eval_bc(config, ckpt_name, save_episode=True, num_rollouts=50):
|
|||||||
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]
|
||||||
|
# if t % query_frequency == query_frequency - 1:
|
||||||
|
# # zero out base actions to avoid overshooting
|
||||||
|
# raw_action[0, -2:] = 0
|
||||||
elif config['policy_class'] == "Diffusion":
|
elif config['policy_class'] == "Diffusion":
|
||||||
if t % query_frequency == 0:
|
if t % query_frequency == 0:
|
||||||
all_actions = policy(qpos, curr_image)
|
all_actions = policy(qpos, curr_image)
|
||||||
if use_actuator_net:
|
# if use_actuator_net:
|
||||||
collect_base_action(all_actions, norm_episode_all_base_actions)
|
# collect_base_action(all_actions, norm_episode_all_base_actions)
|
||||||
raw_action = all_actions[:, t % query_frequency]
|
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)
|
||||||
if use_actuator_net:
|
# if use_actuator_net:
|
||||||
collect_base_action(all_actions, norm_episode_all_base_actions)
|
# collect_base_action(all_actions, norm_episode_all_base_actions)
|
||||||
else:
|
else:
|
||||||
raise NotImplementedError
|
raise NotImplementedError
|
||||||
|
# print('query policy: ', time.time() - time3)
|
||||||
|
|
||||||
### post-process actions
|
### post-process actions
|
||||||
|
time4 = time.time()
|
||||||
raw_action = raw_action.squeeze(0).cpu().numpy()
|
raw_action = raw_action.squeeze(0).cpu().numpy()
|
||||||
action = post_process(raw_action)
|
action = post_process(raw_action)
|
||||||
target_qpos = action[:-2]
|
target_qpos = action[:-2]
|
||||||
|
|
||||||
if use_actuator_net:
|
# if use_actuator_net:
|
||||||
assert(not temporal_agg)
|
# assert(not temporal_agg)
|
||||||
if t % prediction_len == 0:
|
# if t % prediction_len == 0:
|
||||||
offset_start_ts = t + history_len
|
# offset_start_ts = t + history_len
|
||||||
actuator_net_in = np.array(norm_episode_all_base_actions[offset_start_ts - history_len: offset_start_ts + future_len])
|
# actuator_net_in = np.array(norm_episode_all_base_actions[offset_start_ts - history_len: offset_start_ts + future_len])
|
||||||
actuator_net_in = torch.from_numpy(actuator_net_in).float().unsqueeze(dim=0).cuda()
|
# actuator_net_in = torch.from_numpy(actuator_net_in).float().unsqueeze(dim=0).cuda()
|
||||||
pred = actuator_network(actuator_net_in)
|
# pred = actuator_network(actuator_net_in)
|
||||||
base_action_chunk = actuator_unnorm(pred.detach().cpu().numpy()[0])
|
# base_action_chunk = actuator_unnorm(pred.detach().cpu().numpy()[0])
|
||||||
base_action = base_action_chunk[t % prediction_len]
|
# base_action = base_action_chunk[t % prediction_len]
|
||||||
else:
|
# else:
|
||||||
base_action = action[-2:]
|
base_action = action[-2:]
|
||||||
# base_action = calibrate_linear_vel(base_action, c=0.19)
|
# base_action = calibrate_linear_vel(base_action, c=0.19)
|
||||||
# base_action = postprocess_base_action(base_action)
|
# base_action = postprocess_base_action(base_action)
|
||||||
|
# print('post process: ', time.time() - time4)
|
||||||
|
|
||||||
### step the environment
|
### step the environment
|
||||||
|
time5 = time.time()
|
||||||
if real_robot:
|
if real_robot:
|
||||||
ts = env.step(target_qpos, base_action)
|
ts = env.step(target_qpos, base_action)
|
||||||
else:
|
else:
|
||||||
ts = env.step(target_qpos)
|
ts = env.step(target_qpos)
|
||||||
|
# print('step env: ', time.time() - time5)
|
||||||
|
|
||||||
### for visualization
|
### for visualization
|
||||||
qpos_list.append(qpos_numpy)
|
qpos_list.append(qpos_numpy)
|
||||||
target_qpos_list.append(target_qpos)
|
target_qpos_list.append(target_qpos)
|
||||||
rewards.append(ts.reward)
|
rewards.append(ts.reward)
|
||||||
time.sleep(max(0, DT - (time.time() - time1)))
|
duration = time.time() - time1
|
||||||
|
time.sleep(max(0, DT - duration))
|
||||||
|
# time.sleep(max(0, DT - duration - culmulated_delay))
|
||||||
|
if 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')
|
||||||
|
# else:
|
||||||
|
# culmulated_delay = max(0, culmulated_delay - (DT - duration))
|
||||||
|
|
||||||
|
print(f'Avg fps {max_timesteps / (time.time() - time0)}')
|
||||||
plt.close()
|
plt.close()
|
||||||
if real_robot:
|
if real_robot:
|
||||||
move_grippers([env.puppet_bot_left, env.puppet_bot_right], [PUPPET_GRIPPER_JOINT_OPEN] * 2, move_time=0.5) # open
|
move_grippers([env.puppet_bot_left, env.puppet_bot_right], [PUPPET_GRIPPER_JOINT_OPEN] * 2, move_time=0.5) # open
|
||||||
|
|||||||
8
utils.py
8
utils.py
@ -231,11 +231,11 @@ def preprocess_base_action(base_action):
|
|||||||
|
|
||||||
def postprocess_base_action(base_action):
|
def postprocess_base_action(base_action):
|
||||||
linear_vel, angular_vel = base_action
|
linear_vel, angular_vel = base_action
|
||||||
linear_vel *= 0.8
|
linear_vel *= 1.0
|
||||||
angular_vel *= 0.70
|
angular_vel *= 1.0
|
||||||
# angular_vel = 0
|
# angular_vel = 0
|
||||||
if np.abs(linear_vel) < 0.05:
|
# if np.abs(linear_vel) < 0.05:
|
||||||
linear_vel = 0
|
# linear_vel = 0
|
||||||
return np.array([linear_vel, angular_vel])
|
return np.array([linear_vel, angular_vel])
|
||||||
|
|
||||||
### env utils
|
### env utils
|
||||||
|
|||||||
在新工单中引用
屏蔽一个用户