32 次代码提交

作者 SHA1 备注 提交日期
637d4f9a93 ema fixed 2023-11-23 18:19:45 -08:00
71c46d8649 fix compress_data.py 2023-11-14 17:13:33 -08:00
b7e387a9b9 Merge branch 'main' of github.com:MarkFzp/act-plus-plus into main 2023-11-14 14:43:05 -08:00
8f66f3bc88 use mobile-aloha2 2023-11-14 14:38:11 -08:00
79e5032002 Diffusion policy 2023-11-14 11:02:33 -08:00
61decb1e13 add compress to postprocess_episodes.py 2023-11-11 19:06:55 -08:00
5376392e66 dataloader __getitem__ supported compressed 2023-11-11 18:16:15 -05:00
329a237290 fix video viz bgr 2023-11-10 21:12:11 -08:00
d89430f987 compress_data.py 2023-11-05 00:54:36 -07:00
4cec62314b default deploy with c=0.19 calibration, add postprocess_base_action to utils.py for shrinked angular vel 2023-11-02 23:46:29 -07:00
5ab307d5c4 Merge branch 'main' of github.com:MarkFzp/act-plus-plus into main 2023-11-01 18:54:11 -07:00
b401b8f9d6 align.py for cam and arm symmetry calibration 2023-11-01 18:54:06 -07:00
902dae2236 remove wandb when is_eval, change smooth action to conv 5 2023-11-01 21:53:04 -04:00
60d0da09f8 Merge branch 'mirror' into main 2023-11-01 18:08:43 -07:00
bc860dd58c fix smooth_base_action in utils.py 2023-11-01 15:32:34 -07:00
e8aa7976ba update postprocess_episodes for base_action, skip_mirrored_data option 2023-11-01 03:54:06 -04:00
7678af7021 no encoder option 2023-10-31 13:23:35 -07:00
dc756964a2 Merge branch 'mirror' of github.com:MarkFzp/act-plus-plus into mirror 2023-10-31 02:41:42 -04:00
02a6a2e4e5 add resume_ckpt_path option, sim_insertion_scripted_mirror task 2023-10-31 02:41:36 -04:00
10c445adb0 small 2023-10-30 22:51:11 -07:00
6afe28b795 load pretrain flag 2023-10-30 02:35:34 -07:00
cbc01948c8 fix pretraining on all data 2023-10-29 23:30:52 -07:00
128701a22f fix bug and add mirroring 2023-10-29 23:02:40 -07:00
588727d415 fix env shaking at first step 2023-10-29 12:34:26 -07:00
33e4c86341 improved 2023-10-28 23:44:40 -07:00
8119013853 debug smooth_base_action astype(np.float32), train_ratio = 0.99 2023-10-28 16:10:19 -07:00
d6bb889d7d Merge branch 'main' of github.com:MarkFzp/act-plus-plus into main 2023-10-26 00:57:42 -07:00
d0aa344ff0 remove preprocess base_action calibration, add postprocess option in eval_bc() 2023-10-26 00:56:33 -07:00
8f22418a7b Merge branch 'main' of github.com:MarkFzp/act-plus-plus into main 2023-10-26 01:44:36 -04:00
654248d96f fix in-place calibrate_linear_vel 2023-10-26 01:44:31 -04:00
3474cc38e7 Merge branch 'main' of github.com:MarkFzp/act-plus-plus into main 2023-10-25 21:56:24 -07:00
ca82042c1c make imitate_episodes.py runnable for mobile_aloha eval 2023-10-25 21:56:15 -07:00
共有 18 个文件被更改,包括 1005 次插入 和 122 次删除

31
align.py 普通文件
查看文件

@ -0,0 +1,31 @@
from interbotix_xs_modules.arm import InterbotixManipulatorXS
from aloha_scripts.robot_utils import move_arms, torque_on, move_grippers
from constants import PUPPET_GRIPPER_JOINT_OPEN, PUPPET_GRIPPER_JOINT_CLOSE
import argparse
import numpy as np
# for calibrating head cam and arms being symmetrical
def main():
argparser = argparse.ArgumentParser()
argparser.add_argument('--all', action='store_true', default=False)
args = argparser.parse_args()
puppet_bot_left = InterbotixManipulatorXS(robot_model="vx300s", group_name="arm", gripper_name="gripper", robot_name=f'puppet_left', init_node=True)
puppet_bot_right = InterbotixManipulatorXS(robot_model="vx300s", group_name="arm", gripper_name="gripper", robot_name=f'puppet_right', init_node=False)
all_bots = [puppet_bot_left, puppet_bot_right]
for bot in all_bots:
torque_on(bot)
multiplier = np.array([-1, 1, 1, -1, 1, 1])
puppet_sleep_position_left = np.array([-0.8, -0.5, 0.5, 0, 0.65, 0])
puppet_sleep_position_right = puppet_sleep_position_left * multiplier
all_positions = [puppet_sleep_position_left, puppet_sleep_position_right]
move_arms(all_bots, all_positions, move_time=2)
# move_grippers(all_bots, [PUPPET_GRIPPER_JOINT_OPEN] * 2, move_time=1) # open
if __name__ == '__main__':
main()

查看文件

@ -29,7 +29,7 @@
<site pos="0.15 0 0" size="0.003 0.003 0.03" type="box" name="cali_left_site1" rgba="0 0 1 0"/>
<site pos="0.15 0 0" size="0.003 0.03 0.003" type="box" name="cali_left_site2" rgba="0 0 1 0"/>
<site pos="0.15 0 0" size="0.03 0.003 0.003" type="box" name="cali_left_site3" rgba="0 0 1 0"/>
<camera name="left_wrist" pos="-0.1 0 0.16" fovy="20" mode="targetbody" target="vx300s_left/camera_focus"/>
<camera name="left_wrist" pos="-0.1 0 0.16" fovy="20" mode="fixed" euler="0 -1.05 -1.5707" />
<inertial pos="0.0395662 -2.56311e-07 0.00400649" quat="0.62033 0.619916 -0.339682 0.339869" mass="0.251652" diaginertia="0.000689546 0.000650316 0.000468142" />
<joint name="vx300s_left/wrist_rotate" pos="0 0 0" axis="1 0 0" limited="true" range="-3.14158 3.14158" frictionloss="30" />
<geom pos="-0.02 0 0" quat="0.707107 0 0 0.707107" type="mesh" mesh="vx300s_7_gripper" name="vx300s_left/7_gripper" />

查看文件

@ -29,7 +29,7 @@
<site pos="0.15 0 0" size="0.003 0.003 0.03" type="box" name="cali_right_site1" rgba="0 0 1 0"/>
<site pos="0.15 0 0" size="0.003 0.03 0.003" type="box" name="cali_right_site2" rgba="0 0 1 0"/>
<site pos="0.15 0 0" size="0.03 0.003 0.003" type="box" name="cali_right_site3" rgba="0 0 1 0"/>
<camera name="right_wrist" pos="-0.1 0 0.16" fovy="20" mode="targetbody" target="vx300s_right/camera_focus"/>
<camera name="right_wrist" pos="-0.1 0 0.16" fovy="20" mode="fixed" euler="0 -1.05 -1.5707" />
<inertial pos="0.0395662 -2.56311e-07 0.00400649" quat="0.62033 0.619916 -0.339682 0.339869" mass="0.251652" diaginertia="0.000689546 0.000650316 0.000468142" />
<joint name="vx300s_right/wrist_rotate" pos="0 0 0" axis="1 0 0" limited="true" range="-3.14158 3.14158" frictionloss="30" />
<geom pos="-0.02 0 0" quat="0.707107 0 0 0.707107" type="mesh" mesh="vx300s_7_gripper" name="vx300s_right/7_gripper" />

查看文件

@ -18,6 +18,7 @@ CUDA_VISIBLE_DEVICES=0 python3 imitate_episodes.py \
--hidden_dim 512 --batch_size 24 --dim_feedforward 3200 --num_epochs 5000 --lr 1e-4 --seed 0
#### NOTE to reproduce this experiment, uncomment the sim data filtering in utils.py
conda activate mobile
cd /home/tonyzhao/Research/act-plus-plus
CUDA_VISIBLE_DEVICES=0 python3 imitate_episodes.py \
@ -27,4 +28,133 @@ CUDA_VISIBLE_DEVICES=0 python3 imitate_episodes.py \
--hidden_dim 512 --batch_size 24 --dim_feedforward 3200 --lr 1e-4 --seed 0 \
--num_steps 1000000 --eval_every 10000000000 --validate_every 2000 --save_every 5000
# generate mirrored data
conda activate mobile
cd /home/tonyzhao/Research/act-plus-plus
python3 record_sim_episodes.py --task_name sim_transfer_cube_scripted_mirror --dataset_dir /scr/tonyzhao/datasets/sim_transfer_cube_scripted_mirror --num_episodes 50
python3 postprocess_episodes.py --dataset_dir /scr/tonyzhao/datasets/sim_transfer_cube_scripted_mirror --num_episodes 50
# the sim_transfer_cube_scripted_mirror will have 100 episodes
# I then copy the whole dir to sim_transfer_cube_scripted then removed all mirrored episodes
# this gives sim_transfer_cube_scripted_mirror (100 episodes) and sim_transfer_cube_scripted (50 episodes)
# visualize the original data
python3 visualize_episodes.py --dataset_dir /scr/tonyzhao/datasets/sim_transfer_cube_scripted_mirror --episode_idx 0
# visualize the artificially mirrored data
python3 visualize_episodes.py --dataset_dir /scr/tonyzhao/datasets/sim_transfer_cube_scripted_mirror --episode_idx 0 --ismirror
# sanity check
# replay the mirrored data action in the original env
python3 replay_episodes.py --dataset_path /scr/tonyzhao/datasets/sim_transfer_cube_scripted_mirror/mirror_episode_0.hdf5
# replay the original data action in the original env
python3 replay_episodes.py --dataset_path /scr/tonyzhao/datasets/sim_transfer_cube_scripted_mirror/episode_0.hdf5
# launch experiment on original data
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 \
--policy_class ACT --kl_weight 10 --chunk_size 50 \
--hidden_dim 512 --batch_size 12 --dim_feedforward 3200 --lr 1e-5 --seed 0 \
--num_steps 100000 --eval_every 2000 --validate_every 2000 --save_every 2000 --no_encoder
# launch experiment on all data
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_mirror \
--ckpt_dir /scr/tonyzhao/train_logs/cube_scripted_mirror \
--policy_class ACT --kl_weight 10 --chunk_size 50 \
--hidden_dim 512 --batch_size 12 --dim_feedforward 3200 --lr 1e-5 --seed 0 \
--num_steps 100000 --eval_every 2000 --validate_every 2000 --save_every 2000 --no_encoder
####### DIFFUSION POLICY
- first install https://github.com/ARISE-Initiative/robomimic/tree/r2d2 (note the r2d2 branch)
- on top of it pip install the current repo requirements
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_0 \
--policy_class Diffusion --chunk_size 32 \
--batch_size 32 --lr 1e-5 --seed 0 \
--num_steps 100000 --eval_every 2000 --validate_every 2000 --save_every 2000
conda activate mobile
export MUJOCO_GL=egl
cd /home/tonyzhao/Research/act-plus-plus
CUDA_VISIBLE_DEVICES=1 python3 imitate_episodes.py \
--task_name sim_transfer_cube_scripted \
--ckpt_dir /scr/tonyzhao/train_logs/cube_scripted_diffusion_sweep_1 \
--policy_class Diffusion --chunk_size 16 \
--batch_size 32 --lr 1e-5 --seed 0 \
--num_steps 100000 --eval_every 2000 --validate_every 2000 --save_every 2000
# above are all 100 train diffusion steps, 1e-5
conda activate mobile
export MUJOCO_GL=egl
cd /home/tonyzhao/Research/act-plus-plus
CUDA_VISIBLE_DEVICES=1 python3 imitate_episodes.py \
--task_name sim_transfer_cube_scripted \
--ckpt_dir /scr/tonyzhao/train_logs/cube_scripted_diffusion_sweep_2_50step_1e-4 \
--policy_class Diffusion --chunk_size 32 \
--batch_size 32 --lr 1e-4 --seed 0 \
--num_steps 100000 --eval_every 2000 --validate_every 2000 --save_every 2000
# use EMA and train for longer (code changes)
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_3_ema \
--policy_class Diffusion --chunk_size 32 \
--batch_size 32 --lr 1e-4 --seed 0 \
--num_steps 300000 --eval_every 6000 --validate_every 6000 --save_every 6000
# code regression test: no EMA
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_4_regression_no_ema \
--policy_class Diffusion --chunk_size 32 \
--batch_size 32 --lr 1e-4 --seed 0 \
--num_steps 300000 --eval_every 6000 --validate_every 6000 --save_every 6000
conda activate mobile
export MUJOCO_GL=egl
cd /home/tonyzhao/Research/act-plus-plus
CUDA_VISIBLE_DEVICES=1 python3 imitate_episodes.py \
--task_name sim_transfer_cube_scripted \
--ckpt_dir /scr/tonyzhao/train_logs/cube_scripted_diffusion_sweep_5_ema_again \
--policy_class Diffusion --chunk_size 32 \
--batch_size 32 --lr 1e-4 --seed 0 \
--num_steps 300000 --eval_every 6000 --validate_every 6000 --save_every 6000
---------------------------------------------------------------------------------------
NOTE: chunk size cannot be any number, try before launching
TODO: Add history, EMA at test time

185
compress_data.py 普通文件
查看文件

@ -0,0 +1,185 @@
"""
Example usage:
$ python3 script/compress_data.py --dataset_dir /scr/lucyshi/dataset/aloha_test
"""
import os
import h5py
import cv2
import numpy as np
import argparse
from tqdm import tqdm
# Constants
DT = 0.02
JOINT_NAMES = ["waist", "shoulder", "elbow", "forearm_roll", "wrist_angle", "wrist_rotate"]
STATE_NAMES = JOINT_NAMES + ["gripper"]
def compress_dataset(input_dataset_path, output_dataset_path):
# Check if output path exists
if os.path.exists(output_dataset_path):
print(f"The file {output_dataset_path} already exists. Exiting...")
return
# Load the uncompressed dataset
with h5py.File(input_dataset_path, 'r') as infile:
# Create the compressed dataset
with h5py.File(output_dataset_path, 'w') as outfile:
if 'sim' in infile.attrs:
outfile.attrs['sim'] = infile.attrs['sim']
else:
outfile.attrs['sim'] = False
outfile.attrs['compress'] = True
# Copy non-image data directly
for key in infile.keys():
if key != 'observations':
outfile.copy(infile[key], key)
obs_group = infile['observations']
# Create observation group in the output
out_obs_group = outfile.create_group('observations')
# Copy non-image data in observations directly
for key in obs_group.keys():
if key != 'images':
out_obs_group.copy(obs_group[key], key)
image_group = obs_group['images']
out_image_group = out_obs_group.create_group('images')
# JPEG compression parameters
encode_param = [int(cv2.IMWRITE_JPEG_QUALITY), 50]
compressed_lens = [] # List to store compressed lengths for each camera
for cam_name in image_group.keys():
if "_depth" in cam_name: # Depth images are not compressed
out_image_group.copy(image_group[cam_name], cam_name)
else:
images = image_group[cam_name]
compressed_images = []
cam_compressed_lens = [] # List to store compressed lengths for this camera
# Compress each image
for image in images:
result, encoded_image = cv2.imencode('.jpg', image, encode_param)
compressed_images.append(encoded_image)
cam_compressed_lens.append(len(encoded_image)) # Store the length
compressed_lens.append(cam_compressed_lens)
# Find the maximum length of the compressed images
max_len = max(len(img) for img in compressed_images)
# Create dataset to store compressed images
compressed_dataset = out_image_group.create_dataset(cam_name, (len(compressed_images), max_len), dtype='uint8')
# Store compressed images
for i, img in enumerate(compressed_images):
compressed_dataset[i, :len(img)] = img
# Save the compressed lengths to the HDF5 file
compressed_lens = np.array(compressed_lens)
_ = outfile.create_dataset('compress_len', compressed_lens.shape)
outfile['/compress_len'][...] = compressed_lens
print(f"Compressed dataset saved to {output_dataset_path}")
def save_videos(video, dt, video_path=None):
if isinstance(video, list):
cam_names = list(video[0].keys())
h, w, _ = video[0][cam_names[0]].shape
w = w * len(cam_names)
fps = int(1/dt)
out = cv2.VideoWriter(video_path, cv2.VideoWriter_fourcc(*'mp4v'), fps, (w, h))
# bitrate = 1000000
# out.set(cv2.VIDEOWRITER_PROP_BITRATE, bitrate)
for ts, image_dict in enumerate(video):
images = []
for cam_name in cam_names:
image = image_dict[cam_name]
image = image[:, :, [2, 1, 0]] # swap B and R channel
images.append(image)
images = np.concatenate(images, axis=1)
out.write(images)
out.release()
print(f'Saved video to: {video_path}')
elif isinstance(video, dict):
cam_names = list(video.keys())
# Remove depth images
cam_names = [cam_name for cam_name in cam_names if '_depth' not in cam_name]
all_cam_videos = []
for cam_name in cam_names:
all_cam_videos.append(video[cam_name])
all_cam_videos = np.concatenate(all_cam_videos, axis=2) # width dimension
n_frames, h, w, _ = all_cam_videos.shape
fps = int(1 / dt)
out = cv2.VideoWriter(video_path, cv2.VideoWriter_fourcc(*'mp4v'), fps, (w, h))
for t in range(n_frames):
image = all_cam_videos[t]
image = image[:, :, [2, 1, 0]] # swap B and R channel
out.write(image)
out.release()
print(f'Saved video to: {video_path}')
def load_and_save_first_episode_video(dataset_dir, video_path):
dataset_name = 'episode_0'
_, _, _, _, image_dict = load_hdf5(dataset_dir, dataset_name)
save_videos(image_dict, DT, video_path=video_path)
def load_hdf5(dataset_dir, dataset_name):
dataset_path = os.path.join(dataset_dir, dataset_name + '.hdf5')
if not os.path.isfile(dataset_path):
print(f'Dataset does not exist at \n{dataset_path}\n')
exit()
with h5py.File(dataset_path, 'r') as root:
compressed = root.attrs.get('compress', False)
image_dict = dict()
for cam_name in root[f'/observations/images/'].keys():
image_dict[cam_name] = root[f'/observations/images/{cam_name}'][()]
if compressed:
compress_len = root['/compress_len'][()]
if compressed:
for cam_id, cam_name in enumerate(image_dict.keys()):
padded_compressed_image_list = image_dict[cam_name]
image_list = []
for frame_id, padded_compressed_image in enumerate(padded_compressed_image_list):
image_len = int(compress_len[cam_id, frame_id])
compressed_image = padded_compressed_image
image = cv2.imdecode(compressed_image, 1)
image_list.append(image)
image_dict[cam_name] = image_list
return None, None, None, None, image_dict # Return only the image dict for this application
if __name__ == '__main__':
parser = argparse.ArgumentParser(description="Compress all HDF5 datasets in a directory.")
parser.add_argument('--dataset_dir', action='store', type=str, required=True, help='Directory containing the uncompressed datasets.')
args = parser.parse_args()
output_dataset_dir = args.dataset_dir + '_compressed'
os.makedirs(output_dataset_dir, exist_ok=True)
# Iterate over each file in the directory
for filename in tqdm(os.listdir(args.dataset_dir), desc="Compressing data"):
if filename.endswith('.hdf5'):
input_path = os.path.join(args.dataset_dir, filename)
output_path = os.path.join(output_dataset_dir, filename)
compress_dataset(input_path, output_path)
# After processing all datasets, load and save the video for the first episode
print(f'Saving video for episode 0 in {output_dataset_dir}')
video_path = os.path.join(output_dataset_dir, 'episode_0_video.mp4')
load_and_save_first_episode_video(args.dataset_dir, video_path)

查看文件

@ -1,13 +1,14 @@
import pathlib
import os
### Task parameters
DATA_DIR = '/scr/tonyzhao/datasets'
DATA_DIR = '/home/zfu/interbotix_ws/src/act/data' if os.getlogin() == 'zfu' else '/scr/tonyzhao/datasets'
SIM_TASK_CONFIGS = {
'sim_transfer_cube_scripted':{
'dataset_dir': DATA_DIR + '/sim_transfer_cube_scripted',
'num_episodes': 50,
'episode_len': 400,
'camera_names': ['top']
'camera_names': ['top', 'left_wrist', 'right_wrist']
},
'sim_transfer_cube_human':{
@ -21,7 +22,7 @@ SIM_TASK_CONFIGS = {
'dataset_dir': DATA_DIR + '/sim_insertion_scripted',
'num_episodes': 50,
'episode_len': 400,
'camera_names': ['top']
'camera_names': ['top', 'left_wrist', 'right_wrist']
},
'sim_insertion_human': {
@ -30,14 +31,27 @@ SIM_TASK_CONFIGS = {
'episode_len': 500,
'camera_names': ['top']
},
'all': {
'dataset_dir': DATA_DIR + '/',
'num_episodes': None,
'episode_len': None,
'name_filter': lambda n: 'sim' not in n,
'camera_names': ['cam_high', 'cam_left_wrist', 'cam_right_wrist']
},
'sim_transfer_cube_scripted_mirror':{
'dataset_dir': DATA_DIR + '/sim_transfer_cube_scripted_mirror',
'num_episodes': None,
'episode_len': 400,
'camera_names': ['top', 'left_wrist', 'right_wrist']
},
'sim_insertion_scripted_mirror': {
'dataset_dir': DATA_DIR + '/sim_insertion_scripted_mirror',
'num_episodes': None,
'episode_len': 400,
'camera_names': ['top', 'left_wrist', 'right_wrist']
},
}

查看文件

@ -67,11 +67,15 @@ def get_args_parser():
parser.add_argument('--use_vq', action='store_true')
parser.add_argument('--vq_class', action='store', type=int, help='vq_class', required=False)
parser.add_argument('--vq_dim', action='store', type=int, help='vq_dim', required=False)
parser.add_argument('--load_pretrain', action='store_true', default=False)
parser.add_argument('--action_dim', action='store', type=int, required=False)
parser.add_argument('--eval_every', action='store', type=int, default=500, help='eval_every', required=False)
parser.add_argument('--validate_every', action='store', type=int, default=500, help='validate_every', required=False)
parser.add_argument('--save_every', action='store', type=int, default=500, help='save_every', required=False)
parser.add_argument('--resume_ckpt_path', action='store', type=str, help='load_ckpt_path', required=False)
parser.add_argument('--no_encoder', action='store_true')
parser.add_argument('--skip_mirrored_data', action='store_true')
return parser

查看文件

@ -88,55 +88,59 @@ class DETRVAE(nn.Module):
def encode(self, qpos, actions=None, is_pad=None, vq_sample=None):
# cvae encoder
is_training = actions is not None # train or val
bs, _ = qpos.shape
### Obtain latent z from action sequence
if is_training:
# project action sequence to embedding dim, and concat with a CLS token
action_embed = self.encoder_action_proj(actions) # (bs, seq, hidden_dim)
qpos_embed = self.encoder_joint_proj(qpos) # (bs, hidden_dim)
qpos_embed = torch.unsqueeze(qpos_embed, axis=1) # (bs, 1, hidden_dim)
cls_embed = self.cls_embed.weight # (1, hidden_dim)
cls_embed = torch.unsqueeze(cls_embed, axis=0).repeat(bs, 1, 1) # (bs, 1, hidden_dim)
encoder_input = torch.cat([cls_embed, qpos_embed, action_embed], axis=1) # (bs, seq+1, hidden_dim)
encoder_input = encoder_input.permute(1, 0, 2) # (seq+1, bs, hidden_dim)
# do not mask cls token
cls_joint_is_pad = torch.full((bs, 2), False).to(qpos.device) # False: not a padding
is_pad = torch.cat([cls_joint_is_pad, is_pad], axis=1) # (bs, seq+1)
# obtain position embedding
pos_embed = self.pos_table.clone().detach()
pos_embed = pos_embed.permute(1, 0, 2) # (seq+1, 1, hidden_dim)
# query model
encoder_output = self.encoder(encoder_input, pos=pos_embed, src_key_padding_mask=is_pad)
encoder_output = encoder_output[0] # take cls output only
latent_info = self.latent_proj(encoder_output)
if self.vq:
logits = latent_info.reshape([*latent_info.shape[:-1], self.vq_class, self.vq_dim])
probs = torch.softmax(logits, dim=-1)
binaries = F.one_hot(torch.multinomial(probs.view(-1, self.vq_dim), 1).squeeze(-1), self.vq_dim).view(-1, self.vq_class, self.vq_dim).float()
binaries_flat = binaries.view(-1, self.vq_class * self.vq_dim)
probs_flat = probs.view(-1, self.vq_class * self.vq_dim)
straigt_through = binaries_flat - probs_flat.detach() + probs_flat
latent_input = self.latent_out_proj(straigt_through)
mu = logvar = None
else:
probs = binaries = None
mu = latent_info[:, :self.latent_dim]
logvar = latent_info[:, self.latent_dim:]
latent_sample = reparametrize(mu, logvar)
latent_input = self.latent_out_proj(latent_sample)
if self.encoder is None:
latent_sample = torch.zeros([bs, self.latent_dim], dtype=torch.float32).to(qpos.device)
latent_input = self.latent_out_proj(latent_sample)
probs = binaries = mu = logvar = None
else:
mu = logvar = binaries = probs = None
if self.vq:
latent_input = self.latent_out_proj(vq_sample.view(-1, self.vq_class * self.vq_dim))
else:
latent_sample = torch.zeros([bs, self.latent_dim], dtype=torch.float32).to(qpos.device)
latent_input = self.latent_out_proj(latent_sample)
# cvae encoder
is_training = actions is not None # train or val
### Obtain latent z from action sequence
if is_training:
# project action sequence to embedding dim, and concat with a CLS token
action_embed = self.encoder_action_proj(actions) # (bs, seq, hidden_dim)
qpos_embed = self.encoder_joint_proj(qpos) # (bs, hidden_dim)
qpos_embed = torch.unsqueeze(qpos_embed, axis=1) # (bs, 1, hidden_dim)
cls_embed = self.cls_embed.weight # (1, hidden_dim)
cls_embed = torch.unsqueeze(cls_embed, axis=0).repeat(bs, 1, 1) # (bs, 1, hidden_dim)
encoder_input = torch.cat([cls_embed, qpos_embed, action_embed], axis=1) # (bs, seq+1, hidden_dim)
encoder_input = encoder_input.permute(1, 0, 2) # (seq+1, bs, hidden_dim)
# do not mask cls token
cls_joint_is_pad = torch.full((bs, 2), False).to(qpos.device) # False: not a padding
is_pad = torch.cat([cls_joint_is_pad, is_pad], axis=1) # (bs, seq+1)
# obtain position embedding
pos_embed = self.pos_table.clone().detach()
pos_embed = pos_embed.permute(1, 0, 2) # (seq+1, 1, hidden_dim)
# query model
encoder_output = self.encoder(encoder_input, pos=pos_embed, src_key_padding_mask=is_pad)
encoder_output = encoder_output[0] # take cls output only
latent_info = self.latent_proj(encoder_output)
if self.vq:
logits = latent_info.reshape([*latent_info.shape[:-1], self.vq_class, self.vq_dim])
probs = torch.softmax(logits, dim=-1)
binaries = F.one_hot(torch.multinomial(probs.view(-1, self.vq_dim), 1).squeeze(-1), self.vq_dim).view(-1, self.vq_class, self.vq_dim).float()
binaries_flat = binaries.view(-1, self.vq_class * self.vq_dim)
probs_flat = probs.view(-1, self.vq_class * self.vq_dim)
straigt_through = binaries_flat - probs_flat.detach() + probs_flat
latent_input = self.latent_out_proj(straigt_through)
mu = logvar = None
else:
probs = binaries = None
mu = latent_info[:, :self.latent_dim]
logvar = latent_info[:, self.latent_dim:]
latent_sample = reparametrize(mu, logvar)
latent_input = self.latent_out_proj(latent_sample)
else:
mu = logvar = binaries = probs = None
if self.vq:
latent_input = self.latent_out_proj(vq_sample.view(-1, self.vq_class * self.vq_dim))
else:
latent_sample = torch.zeros([bs, self.latent_dim], dtype=torch.float32).to(qpos.device)
latent_input = self.latent_out_proj(latent_sample)
return latent_input, probs, binaries, mu, logvar
def forward(self, qpos, image, env_state, actions=None, is_pad=None, vq_sample=None):
@ -275,7 +279,10 @@ def build(args):
transformer = build_transformer(args)
encoder = build_encoder(args)
if args.no_encoder:
encoder = None
else:
encoder = build_transformer(args)
model = DETRVAE(
backbones,

查看文件

@ -83,10 +83,10 @@ class BimanualViperXEETask(base.Task):
# (2) get env._physics.named.data.xpos['vx300s_left/gripper_link']
# get env._physics.named.data.xquat['vx300s_left/gripper_link']
# repeat the same for right side
np.copyto(physics.data.mocap_pos[0], [-0.31718881, 0.5, 0.29525084])
np.copyto(physics.data.mocap_pos[0], [-0.31718881+0.1, 0.5, 0.29525084])
np.copyto(physics.data.mocap_quat[0], [1, 0, 0, 0])
# right
np.copyto(physics.data.mocap_pos[1], np.array([0.31718881, 0.49999888, 0.29525084]))
np.copyto(physics.data.mocap_pos[1], np.array([0.31718881-0.1, 0.49999888, 0.29525084]))
np.copyto(physics.data.mocap_quat[1], [1, 0, 0, 0])
# reset gripper control
@ -136,8 +136,8 @@ class BimanualViperXEETask(base.Task):
obs['env_state'] = self.get_env_state(physics)
obs['images'] = dict()
obs['images']['top'] = physics.render(height=480, width=640, camera_id='top')
obs['images']['angle'] = physics.render(height=480, width=640, camera_id='angle')
obs['images']['vis'] = physics.render(height=480, width=640, camera_id='front_close')
# obs['images']['angle'] = physics.render(height=480, width=640, camera_id='angle')
# obs['images']['vis'] = physics.render(height=480, width=640, camera_id='front_close')
# used in scripted policy to obtain starting pose
obs['mocap_pose_left'] = np.concatenate([physics.data.mocap_pos[0], physics.data.mocap_quat[0]]).copy()
obs['mocap_pose_right'] = np.concatenate([physics.data.mocap_pos[1], physics.data.mocap_quat[1]]).copy()

查看文件

@ -14,8 +14,8 @@ from constants import DT
from constants import PUPPET_GRIPPER_JOINT_OPEN
from utils import load_data # data functions
from utils import sample_box_pose, sample_insertion_pose # robot functions
from utils import compute_dict_mean, set_seed, detach_dict # helper functions
from policy import ACTPolicy, CNNMLPPolicy
from utils import compute_dict_mean, set_seed, detach_dict, calibrate_linear_vel, postprocess_base_action # helper functions
from policy import ACTPolicy, CNNMLPPolicy, DiffusionPolicy
from visualize_episodes import save_videos
from detr.models.latent_model import Latent_Model_Transformer
@ -39,21 +39,21 @@ def main(args):
eval_every = args['eval_every']
validate_every = args['validate_every']
save_every = args['save_every']
resume_ckpt_path = args['resume_ckpt_path']
# get task parameters
is_sim = task_name[:4] == 'sim_'
# if is_sim:
# from constants import SIM_TASK_CONFIGS
# task_config = SIM_TASK_CONFIGS[task_name]
# else:
# from aloha_scripts.constants import TASK_CONFIGS
# task_config = TASK_CONFIGS[task_name]
from constants import SIM_TASK_CONFIGS
task_config = SIM_TASK_CONFIGS[task_name]
if is_sim or task_name == 'all':
from constants import SIM_TASK_CONFIGS
task_config = SIM_TASK_CONFIGS[task_name]
else:
from aloha_scripts.constants import TASK_CONFIGS
task_config = TASK_CONFIGS[task_name]
dataset_dir = task_config['dataset_dir']
num_episodes = task_config['num_episodes']
episode_len = task_config['episode_len']
camera_names = task_config['camera_names']
name_filter = task_config.get('name_filter', lambda n: True)
# fixed parameters
state_dim = 14
@ -77,7 +77,21 @@ def main(args):
'vq': args['use_vq'],
'vq_class': args['vq_class'],
'vq_dim': args['vq_dim'],
'action_dim': 16
'action_dim': 16,
'no_encoder': args['no_encoder'],
}
elif policy_class == 'Diffusion':
policy_config = {'lr': args['lr'],
'camera_names': camera_names,
'action_dim': 16,
'observation_horizon': 1,
'action_horizon': 8,
'prediction_horizon': args['chunk_size'],
'num_queries': args['chunk_size'],
'num_inference_timesteps': 10,
'ema_power': 0.75,
'vq': False,
}
elif policy_class == 'CNNMLP':
policy_config = {'lr': args['lr'], 'lr_backbone': lr_backbone, 'backbone' : backbone, 'num_queries': 1,
@ -91,6 +105,7 @@ def main(args):
'validate_every': validate_every,
'save_every': save_every,
'ckpt_dir': ckpt_dir,
'resume_ckpt_path': resume_ckpt_path,
'episode_len': episode_len,
'state_dim': state_dim,
'lr': args['lr'],
@ -101,22 +116,25 @@ def main(args):
'seed': args['seed'],
'temporal_agg': args['temporal_agg'],
'camera_names': camera_names,
'real_robot': not is_sim
'real_robot': not is_sim,
'load_pretrain': args['load_pretrain']
}
if not os.path.isdir(ckpt_dir):
os.makedirs(ckpt_dir)
config_path = os.path.join(ckpt_dir, 'config.pkl')
wandb.init(project="mobile-aloha", reinit=True, entity="tonyzhao")
wandb.config.update(config)
expr_name = ckpt_dir.split('/')[-1]
if not is_eval:
wandb.init(project="mobile-aloha2", reinit=True, entity="mobile-aloha2", name=expr_name)
wandb.config.update(config)
with open(config_path, 'wb') as f:
pickle.dump(config, f)
if is_eval:
ckpt_names = [f'policy_last.ckpt']
results = []
for ckpt_name in ckpt_names:
success_rate, avg_return = eval_bc(config, ckpt_name, save_episode=True)
wandb.log({'success_rate': success_rate, 'avg_return': avg_return})
success_rate, avg_return = eval_bc(config, ckpt_name, save_episode=True, num_rollouts=1)
# wandb.log({'success_rate': success_rate, 'avg_return': avg_return})
results.append([ckpt_name, success_rate, avg_return])
for ckpt_name, success_rate, avg_return in results:
@ -124,7 +142,7 @@ def main(args):
print()
exit()
train_dataloader, val_dataloader, stats, _ = load_data(dataset_dir, num_episodes, camera_names, batch_size_train, batch_size_val, args['chunk_size'])
train_dataloader, val_dataloader, stats, _ = load_data(dataset_dir, name_filter, camera_names, batch_size_train, batch_size_val, args['chunk_size'], args['skip_mirrored_data'], config['load_pretrain'], policy_class)
# save dataset stats
stats_path = os.path.join(ckpt_dir, f'dataset_stats.pkl')
@ -146,6 +164,8 @@ def make_policy(policy_class, policy_config):
policy = ACTPolicy(policy_config)
elif policy_class == 'CNNMLP':
policy = CNNMLPPolicy(policy_config)
elif policy_class == 'Diffusion':
policy = DiffusionPolicy(policy_config)
else:
raise NotImplementedError
return policy
@ -156,6 +176,8 @@ def make_optimizer(policy_class, policy):
optimizer = policy.configure_optimizers()
elif policy_class == 'CNNMLP':
optimizer = policy.configure_optimizers()
elif policy_class == 'Diffusion':
optimizer = policy.configure_optimizers()
else:
raise NotImplementedError
return optimizer
@ -171,7 +193,7 @@ def get_image(ts, camera_names):
return curr_image
def eval_bc(config, ckpt_name, save_episode=True):
def eval_bc(config, ckpt_name, save_episode=True, num_rollouts=50):
set_seed(1000)
ckpt_dir = config['ckpt_dir']
state_dim = config['state_dim']
@ -189,7 +211,7 @@ def eval_bc(config, ckpt_name, save_episode=True):
# load policy and stats
ckpt_path = os.path.join(ckpt_dir, ckpt_name)
policy = make_policy(policy_class, policy_config)
loading_status = policy.load_state_dict(torch.load(ckpt_path))
loading_status = policy.deserialize(torch.load(ckpt_path))
print(loading_status)
policy.cuda()
policy.eval()
@ -198,7 +220,7 @@ def eval_bc(config, ckpt_name, save_episode=True):
vq_class = config['policy_config']['vq_class']
latent_model = Latent_Model_Transformer(vq_dim, vq_dim, vq_class)
latent_model_ckpt_path = os.path.join(ckpt_dir, 'latent_model_last.ckpt')
latent_model.load_state_dict(torch.load(latent_model_ckpt_path))
latent_model.deserialize(torch.load(latent_model_ckpt_path))
latent_model.eval()
latent_model.cuda()
print(f'Loaded policy from: {ckpt_path}, latent model from: {latent_model_ckpt_path}')
@ -209,13 +231,16 @@ def eval_bc(config, ckpt_name, save_episode=True):
stats = pickle.load(f)
pre_process = lambda s_qpos: (s_qpos - stats['qpos_mean']) / stats['qpos_std']
post_process = lambda a: a * stats['action_std'] + stats['action_mean']
if policy_class == 'Diffusion':
post_process = lambda a: ((a + 1) / 2) * (stats['action_max'] - stats['action_min']) + stats['action_min']
else:
post_process = lambda a: a * stats['action_std'] + stats['action_mean']
# load environment
if real_robot:
from aloha_scripts.robot_utils import move_grippers # requires aloha
from aloha_scripts.real_env import make_real_env # requires aloha
env = make_real_env(init_node=True, setup_base=True)
env = make_real_env(init_node=True, setup_robots=True, setup_base=True)
env_max_reward = 0
else:
from sim_env import make_sim_env
@ -229,7 +254,6 @@ def eval_bc(config, ckpt_name, save_episode=True):
max_timesteps = int(max_timesteps * 1) # may increase for real-world tasks
num_rollouts = 50
episode_returns = []
highest_rewards = []
for rollout_id in range(num_rollouts):
@ -250,7 +274,7 @@ def eval_bc(config, ckpt_name, save_episode=True):
### evaluation loop
if temporal_agg:
all_time_actions = torch.zeros([max_timesteps, max_timesteps+num_queries, state_dim]).cuda()
all_time_actions = torch.zeros([max_timesteps, max_timesteps+num_queries, 16]).cuda()
qpos_history = torch.zeros((1, max_timesteps, state_dim)).cuda()
image_list = [] # for visualization
@ -288,6 +312,7 @@ def eval_bc(config, ckpt_name, save_episode=True):
vq_sample = latent_model.generate(1, temperature=1, x=None)
all_actions = policy(qpos, curr_image, vq_sample=vq_sample)
else:
# e()
all_actions = policy(qpos, curr_image)
if temporal_agg:
all_time_actions[[t], t:t+num_queries] = all_actions
@ -301,6 +326,10 @@ def eval_bc(config, ckpt_name, save_episode=True):
raw_action = (actions_for_curr_step * exp_weights).sum(dim=0, keepdim=True)
else:
raw_action = all_actions[:, t % query_frequency]
elif config['policy_class'] == "Diffusion":
if t % query_frequency == 0:
all_actions = policy(qpos, curr_image)
raw_action = all_actions[:, t % query_frequency]
elif config['policy_class'] == "CNNMLP":
raw_action = policy(qpos, curr_image)
else:
@ -311,9 +340,14 @@ def eval_bc(config, ckpt_name, save_episode=True):
action = post_process(raw_action)
target_qpos = action[:-2]
base_action = action[-2:]
base_action = calibrate_linear_vel(base_action, c=0.19)
# base_action = postprocess_base_action(base_action)
### step the environment
ts = env.step(target_qpos, base_action)
if real_robot:
ts = env.step(target_qpos, base_action)
else:
ts = env.step(target_qpos)
### for visualization
qpos_list.append(qpos_numpy)
@ -375,6 +409,12 @@ def train_bc(train_dataloader, val_dataloader, config):
set_seed(seed)
policy = make_policy(policy_class, policy_config)
if config['load_pretrain']:
loading_status = policy.deserialize(torch.load(os.path.join('/home/zfu/interbotix_ws/src/act/ckpts/pretrain_all', 'policy_step_50000_seed_0.ckpt')))
print(f'loaded! {loading_status}')
if config['resume_ckpt_path'] is not None:
loading_status = policy.deserialize(torch.load(config['resume_ckpt_path']))
print(f'Resume policy from: {config["resume_ckpt_path"]}, Status: {loading_status}')
policy.cuda()
optimizer = make_optimizer(policy_class, policy)
@ -399,8 +439,8 @@ def train_bc(train_dataloader, val_dataloader, config):
epoch_val_loss = validation_summary['loss']
if epoch_val_loss < min_val_loss:
min_val_loss = epoch_val_loss
best_ckpt_info = (step, min_val_loss, deepcopy(policy.state_dict()))
for k in validation_summary.keys():
best_ckpt_info = (step, min_val_loss, deepcopy(policy.serialize()))
for k in list(validation_summary.keys()):
validation_summary[f'val_{k}'] = validation_summary.pop(k)
wandb.log(validation_summary, step=step)
print(f'Val loss: {epoch_val_loss:.5f}')
@ -411,10 +451,12 @@ def train_bc(train_dataloader, val_dataloader, config):
# evaluation
if (step > 0) and (step % eval_every == 0):
state_dict = deepcopy(policy.state_dict())
success_with_te, _ = eval_bc(config, f'TE_step{step}', state_dict, temporal_agg=True, save_episode=True, num_rollouts=5)
success_without_te, _ = eval_bc(config, f'step{step}', state_dict, temporal_agg=False, save_episode=True, num_rollouts=5)
wandb.log({'success_with_te':success_with_te, 'success_without_te':success_without_te}, step=step)
# first save then eval
ckpt_name = f'policy_step_{step}_seed_{seed}.ckpt'
ckpt_path = os.path.join(ckpt_dir, ckpt_name)
torch.save(policy.serialize(), ckpt_path)
success, _ = eval_bc(config, ckpt_name, save_episode=True, num_rollouts=10)
wandb.log({'success': success}, step=step)
# training
policy.train()
@ -429,10 +471,10 @@ def train_bc(train_dataloader, val_dataloader, config):
if step % save_every == 0:
ckpt_path = os.path.join(ckpt_dir, f'policy_step_{step}_seed_{seed}.ckpt')
torch.save(policy.state_dict(), ckpt_path)
torch.save(policy.serialize(), ckpt_path)
ckpt_path = os.path.join(ckpt_dir, f'policy_last.ckpt')
torch.save(policy.state_dict(), ckpt_path)
torch.save(policy.serialize(), ckpt_path)
best_step, min_val_loss, best_state_dict = best_ckpt_info
ckpt_path = os.path.join(ckpt_dir, f'policy_step_{best_step}_seed_{seed}.ckpt')
@ -461,9 +503,12 @@ if __name__ == '__main__':
parser.add_argument('--seed', action='store', type=int, help='seed', required=True)
parser.add_argument('--num_steps', action='store', type=int, help='num_steps', required=True)
parser.add_argument('--lr', action='store', type=float, help='lr', required=True)
parser.add_argument('--load_pretrain', action='store_true', default=False)
parser.add_argument('--eval_every', action='store', type=int, default=500, help='eval_every', required=False)
parser.add_argument('--validate_every', action='store', type=int, default=500, help='validate_every', required=False)
parser.add_argument('--save_every', action='store', type=int, default=500, help='save_every', required=False)
parser.add_argument('--resume_ckpt_path', action='store', type=str, help='resume_ckpt_path', required=False)
parser.add_argument('--skip_mirrored_data', action='store_true')
# for ACT
parser.add_argument('--kl_weight', action='store', type=int, help='KL Weight', required=False)
@ -474,5 +519,6 @@ if __name__ == '__main__':
parser.add_argument('--use_vq', action='store_true')
parser.add_argument('--vq_class', action='store', type=int, help='vq_class')
parser.add_argument('--vq_dim', action='store', type=int, help='vq_dim')
parser.add_argument('--no_encoder', action='store_true')
main(vars(parser.parse_args()))

199
policy.py
查看文件

@ -2,11 +2,199 @@ import torch.nn as nn
from torch.nn import functional as F
import torchvision.transforms as transforms
import torch
import numpy as np
from detr.main import build_ACT_model_and_optimizer, build_CNNMLP_model_and_optimizer
import IPython
e = IPython.embed
from collections import OrderedDict
from robomimic.models.base_nets import ResNet18Conv, SpatialSoftmax
from robomimic.algo.diffusion_policy import replace_bn_with_gn, ConditionalUnet1D
from diffusers.schedulers.scheduling_ddpm import DDPMScheduler
from diffusers.schedulers.scheduling_ddim import DDIMScheduler
from diffusers.training_utils import EMAModel
class DiffusionPolicy(nn.Module):
def __init__(self, args_override):
super().__init__()
self.camera_names = args_override['camera_names']
self.observation_horizon = args_override['observation_horizon'] ### TODO TODO TODO DO THIS
self.action_horizon = args_override['action_horizon'] # apply chunk size
self.prediction_horizon = args_override['prediction_horizon'] # chunk size
self.num_inference_timesteps = args_override['num_inference_timesteps']
self.ema_power = args_override['ema_power']
self.lr = args_override['lr']
self.weight_decay = 0
self.num_kp = 32
self.feature_dimension = 64
self.ac_dim = args_override['action_dim'] # 14 + 2
self.obs_dim = self.feature_dimension * len(self.camera_names) + 14 # camera features and proprio
backbones = []
pools = []
linears = []
for _ in self.camera_names:
backbones.append(ResNet18Conv(**{'input_channel': 3, 'pretrained': False, 'input_coord_conv': False}))
pools.append(SpatialSoftmax(**{'input_shape': [512, 15, 20], 'num_kp': self.num_kp, 'temperature': 1.0, 'learnable_temperature': False, 'noise_std': 0.0}))
linears.append(torch.nn.Linear(int(np.prod([self.num_kp, 2])), self.feature_dimension))
backbones = nn.ModuleList(backbones)
pools = nn.ModuleList(pools)
linears = nn.ModuleList(linears)
backbones = replace_bn_with_gn(backbones) # TODO
noise_pred_net = ConditionalUnet1D(
input_dim=self.ac_dim,
global_cond_dim=self.obs_dim*self.observation_horizon
)
nets = nn.ModuleDict({
'policy': nn.ModuleDict({
'backbones': backbones,
'pools': pools,
'linears': linears,
'noise_pred_net': noise_pred_net
})
})
nets = nets.float().cuda()
ENABLE_EMA = True
if ENABLE_EMA:
ema = EMAModel(model=nets, power=self.ema_power)
else:
ema = None
self.nets = nets
self.ema = ema
# setup noise scheduler
self.noise_scheduler = DDIMScheduler(
num_train_timesteps=50,
beta_schedule='squaredcos_cap_v2',
clip_sample=True,
set_alpha_to_one=True,
steps_offset=0,
prediction_type='epsilon'
)
n_parameters = sum(p.numel() for p in self.parameters())
print("number of parameters: %.2fM" % (n_parameters/1e6,))
def configure_optimizers(self):
optimizer = torch.optim.AdamW(self.nets.parameters(), lr=self.lr, weight_decay=self.weight_decay)
return optimizer
def __call__(self, qpos, image, actions=None, is_pad=None):
B = qpos.shape[0]
if actions is not None: # training time
nets = self.nets
all_features = []
for cam_id in range(len(self.camera_names)):
cam_image = image[:, cam_id]
cam_features = nets['policy']['backbones'][cam_id](cam_image)
pool_features = nets['policy']['pools'][cam_id](cam_features)
pool_features = torch.flatten(pool_features, start_dim=1)
out_features = nets['policy']['linears'][cam_id](pool_features)
all_features.append(out_features)
obs_cond = torch.cat(all_features + [qpos], dim=1)
# sample noise to add to actions
noise = torch.randn(actions.shape, device=obs_cond.device)
# sample a diffusion iteration for each data point
timesteps = torch.randint(
0, self.noise_scheduler.config.num_train_timesteps,
(B,), device=obs_cond.device
).long()
# add noise to the clean actions according to the noise magnitude at each diffusion iteration
# (this is the forward diffusion process)
noisy_actions = self.noise_scheduler.add_noise(
actions, noise, timesteps)
# predict the noise residual
noise_pred = nets['policy']['noise_pred_net'](noisy_actions, timesteps, global_cond=obs_cond)
# L2 loss
loss = F.mse_loss(noise_pred, noise)
loss_dict = {}
loss_dict['l2_loss'] = loss
loss_dict['loss'] = loss
if self.training and self.ema is not None:
self.ema.step(nets)
return loss_dict
else: # inference time
To = self.observation_horizon
Ta = self.action_horizon
Tp = self.prediction_horizon
action_dim = self.ac_dim
nets = self.nets
if self.ema is not None:
nets = self.ema.averaged_model
all_features = []
for cam_id in range(len(self.camera_names)):
cam_image = image[:, cam_id]
cam_features = nets['policy']['backbones'][cam_id](cam_image)
pool_features = nets['policy']['pools'][cam_id](cam_features)
pool_features = torch.flatten(pool_features, start_dim=1)
out_features = nets['policy']['linears'][cam_id](pool_features)
all_features.append(out_features)
obs_cond = torch.cat(all_features + [qpos], dim=1)
# initialize action from Guassian noise
noisy_action = torch.randn(
(B, Tp, action_dim), device=obs_cond.device)
naction = noisy_action
# init scheduler
self.noise_scheduler.set_timesteps(self.num_inference_timesteps)
for k in self.noise_scheduler.timesteps:
# predict noise
noise_pred = nets['policy']['noise_pred_net'](
sample=naction,
timestep=k,
global_cond=obs_cond
)
# inverse diffusion step (remove noise)
naction = self.noise_scheduler.step(
model_output=noise_pred,
timestep=k,
sample=naction
).prev_sample
return naction
def serialize(self):
return {
"nets": self.nets.state_dict(),
"ema": self.ema.averaged_model.state_dict() if self.ema is not None else None,
}
def deserialize(self, model_dict):
status = self.nets.load_state_dict(model_dict["nets"])
print('Loaded model')
if model_dict.get("ema", None) is not None:
print('Loaded EMA')
status_ema = self.ema.averaged_model.load_state_dict(model_dict["ema"])
status = [status, status_ema]
return status
class ACTPolicy(nn.Module):
def __init__(self, args_override):
super().__init__()
@ -28,11 +216,12 @@ class ACTPolicy(nn.Module):
loss_dict = dict()
a_hat, is_pad_hat, (mu, logvar), probs, binaries = self.model(qpos, image, env_state, actions, is_pad, vq_sample)
if self.vq:
if self.vq or self.model.encoder is None:
total_kld = [torch.tensor(0.0)]
loss_dict['vq_discrepancy'] = F.l1_loss(probs, binaries, reduction='mean')
else:
total_kld, dim_wise_kld, mean_kld = kl_divergence(mu, logvar)
if self.vq:
loss_dict['vq_discrepancy'] = F.l1_loss(probs, binaries, reduction='mean')
all_l1 = F.l1_loss(actions, a_hat, reduction='none')
l1 = (all_l1 * ~is_pad.unsqueeze(-1)).mean()
loss_dict['l1'] = l1
@ -55,7 +244,11 @@ class ACTPolicy(nn.Module):
return binaries
def serialize(self):
return self.state_dict()
def deserialize(self, model_dict):
return self.load_state_dict(model_dict)
class CNNMLPPolicy(nn.Module):

175
postprocess_episodes.py 普通文件
查看文件

@ -0,0 +1,175 @@
import os
import numpy as np
import cv2
import h5py
import argparse
import time
from visualize_episodes import visualize_joints, visualize_timestamp, save_videos
import matplotlib.pyplot as plt
from constants import DT
import IPython
e = IPython.embed
JOINT_NAMES = ["waist", "shoulder", "elbow", "forearm_roll", "wrist_angle", "wrist_rotate"]
STATE_NAMES = JOINT_NAMES + ["gripper"]
MIRROR_STATE_MULTIPLY = np.array([-1, 1, 1, -1, 1, -1, 1]).astype('float32')
MIRROR_BASE_MULTIPLY = np.array([1, -1]).astype('float32')
def load_hdf5(dataset_dir, dataset_name):
dataset_path = os.path.join(dataset_dir, dataset_name + '.hdf5')
if not os.path.isfile(dataset_path):
print(f'Dataset does not exist at \n{dataset_path}\n')
exit()
with h5py.File(dataset_path, 'r') as root:
is_sim = root.attrs['sim']
compressed = root.attrs.get('compress', False)
qpos = root['/observations/qpos'][()]
qvel = root['/observations/qvel'][()]
action = root['/action'][()]
image_dict = dict()
for cam_name in root[f'/observations/images/'].keys():
image_dict[cam_name] = root[f'/observations/images/{cam_name}'][()]
if 'base_action' in root.keys():
print('base_action exists')
base_action = root['/base_action'][()]
else:
base_action = None
if compressed:
compress_len = root['/compress_len'][()]
if compressed:
for cam_id, cam_name in enumerate(image_dict.keys()):
# un-pad and uncompress
padded_compressed_image_list = image_dict[cam_name]
image_list = []
for padded_compressed_image in padded_compressed_image_list: # [:1000] to save memory
image = cv2.imdecode(padded_compressed_image, 1)
image_list.append(image)
image_dict[cam_name] = np.array(image_list)
return qpos, qvel, action, base_action, image_dict, is_sim
def main(args):
dataset_dir = args['dataset_dir']
num_episodes = args['num_episodes']
start_idx = 0
for episode_idx in range(start_idx, start_idx + num_episodes):
dataset_name = f'episode_{episode_idx}'
qpos, qvel, action, base_action, image_dict, is_sim = load_hdf5(dataset_dir, dataset_name)
# process proprioception
qpos = np.concatenate([qpos[:, 7:] * MIRROR_STATE_MULTIPLY, qpos[:, :7] * MIRROR_STATE_MULTIPLY], axis=1)
qvel = np.concatenate([qvel[:, 7:] * MIRROR_STATE_MULTIPLY, qvel[:, :7] * MIRROR_STATE_MULTIPLY], axis=1)
action = np.concatenate([action[:, 7:] * MIRROR_STATE_MULTIPLY, action[:, :7] * MIRROR_STATE_MULTIPLY], axis=1)
if base_action is not None:
base_action = base_action * MIRROR_BASE_MULTIPLY
# mirror image obs
if 'left_wrist' in image_dict.keys():
image_dict['left_wrist'], image_dict['right_wrist'] = image_dict['right_wrist'][:, :, ::-1], image_dict['left_wrist'][:, :, ::-1]
elif 'cam_left_wrist' in image_dict.keys():
image_dict['cam_left_wrist'], image_dict['cam_right_wrist'] = image_dict['cam_right_wrist'][:, :, ::-1], image_dict['cam_left_wrist'][:, :, ::-1]
else:
raise Exception('No left_wrist or cam_left_wrist in image_dict')
if 'top' in image_dict.keys():
image_dict['top'] = image_dict['top'][:, :, ::-1]
elif 'cam_high' in image_dict.keys():
image_dict['cam_high'] = image_dict['cam_high'][:, :, ::-1]
else:
raise Exception('No top or cam_high in image_dict')
# saving
data_dict = {
'/observations/qpos': qpos,
'/observations/qvel': qvel,
'/action': action,
'/base_action': base_action,
} if base_action is not None else {
'/observations/qpos': qpos,
'/observations/qvel': qvel,
'/action': action,
}
for cam_name in image_dict.keys():
data_dict[f'/observations/images/{cam_name}'] = image_dict[cam_name]
max_timesteps = len(qpos)
COMPRESS = True
if COMPRESS:
# JPEG compression
t0 = time.time()
encode_param = [int(cv2.IMWRITE_JPEG_QUALITY), 50] # tried as low as 20, seems fine
compressed_len = []
for cam_name in image_dict.keys():
image_list = data_dict[f'/observations/images/{cam_name}']
compressed_list = []
compressed_len.append([])
for image in image_list:
result, encoded_image = cv2.imencode('.jpg', image, encode_param) # 0.02 sec # cv2.imdecode(encoded_image, 1)
compressed_list.append(encoded_image)
compressed_len[-1].append(len(encoded_image))
data_dict[f'/observations/images/{cam_name}'] = compressed_list
print(f'compression: {time.time() - t0:.2f}s')
# pad so it has same length
t0 = time.time()
compressed_len = np.array(compressed_len)
padded_size = compressed_len.max()
for cam_name in image_dict.keys():
compressed_image_list = data_dict[f'/observations/images/{cam_name}']
padded_compressed_image_list = []
for compressed_image in compressed_image_list:
padded_compressed_image = np.zeros(padded_size, dtype='uint8')
image_len = len(compressed_image)
padded_compressed_image[:image_len] = compressed_image
padded_compressed_image_list.append(padded_compressed_image)
data_dict[f'/observations/images/{cam_name}'] = padded_compressed_image_list
print(f'padding: {time.time() - t0:.2f}s')
# HDF5
t0 = time.time()
dataset_path = os.path.join(dataset_dir, f'mirror_episode_{episode_idx}')
with h5py.File(dataset_path + '.hdf5', 'w', rdcc_nbytes=1024 ** 2 * 2) as root:
root.attrs['sim'] = is_sim
root.attrs['compress'] = COMPRESS
obs = root.create_group('observations')
image = obs.create_group('images')
for cam_name in image_dict.keys():
if COMPRESS:
_ = image.create_dataset(cam_name, (max_timesteps, padded_size), dtype='uint8',
chunks=(1, padded_size), )
else:
_ = image.create_dataset(cam_name, (max_timesteps, 480, 640, 3), dtype='uint8',
chunks=(1, 480, 640, 3), )
qpos = obs.create_dataset('qpos', (max_timesteps, 14))
qvel = obs.create_dataset('qvel', (max_timesteps, 14))
action = root.create_dataset('action', (max_timesteps, 14))
if base_action is not None:
base_action = root.create_dataset('base_action', (max_timesteps, 2))
for name, array in data_dict.items():
root[name][...] = array
if COMPRESS:
_ = root.create_dataset('compress_len', (len(image_dict.keys()), max_timesteps))
root['/compress_len'][...] = compressed_len
print(f'Saving {dataset_path}: {time.time() - t0:.1f} secs\n')
if episode_idx == start_idx:
save_videos(image_dict, DT, video_path=os.path.join(dataset_dir, dataset_name + '_mirror_video.mp4'))
# visualize_joints(qpos, action, plot_path=os.path.join(dataset_dir, dataset_name + '_mirror_qpos.png'))
# visualize_timestamp(t_list, dataset_path) # TODO addn timestamp back
if __name__ == '__main__':
parser = argparse.ArgumentParser()
parser.add_argument('--dataset_dir', action='store', type=str, help='Dataset dir.', required=True)
parser.add_argument('--num_episodes', action='store', type=int, help='Number of episodes.', required=True)
main(vars(parser.parse_args()))

查看文件

@ -28,7 +28,7 @@ def main(args):
num_episodes = args['num_episodes']
onscreen_render = args['onscreen_render']
inject_noise = False
render_cam_name = 'angle'
render_cam_name = 'top'
if not os.path.isdir(dataset_dir):
os.makedirs(dataset_dir, exist_ok=True)
@ -39,6 +39,8 @@ def main(args):
policy_cls = PickAndTransferPolicy
elif task_name == 'sim_insertion_scripted':
policy_cls = InsertionPolicy
elif task_name == 'sim_transfer_cube_scripted_mirror':
policy_cls = PickAndTransferPolicy
else:
raise NotImplementedError

48
replay_episodes.py 普通文件
查看文件

@ -0,0 +1,48 @@
import os
import h5py
import argparse
from collections import defaultdict
from sim_env import make_sim_env
from utils import sample_box_pose, sample_insertion_pose
from sim_env import BOX_POSE
from constants import DT
from visualize_episodes import save_videos
import IPython
e = IPython.embed
def main(args):
dataset_path = args['dataset_path']
if not os.path.isfile(dataset_path):
print(f'Dataset does not exist at \n{dataset_path}\n')
exit()
with h5py.File(dataset_path, 'r') as root:
actions = root['/action'][()]
env = make_sim_env('sim_transfer_cube')
BOX_POSE[0] = sample_box_pose() # used in sim reset
ts = env.reset()
episode_replay = [ts]
for action in actions:
ts = env.step(action)
episode_replay.append(ts)
# saving
image_dict = defaultdict(lambda: [])
while episode_replay:
ts = episode_replay.pop(0)
for cam_name, image in ts.observation['images'].items():
image_dict[cam_name].append(image)
video_path = dataset_path.replace('episode_', 'replay_episode_').replace('hdf5', 'mp4')
save_videos(image_dict, DT, video_path=video_path)
if __name__ == '__main__':
parser = argparse.ArgumentParser()
parser.add_argument('--dataset_path', action='store', type=str, help='Dataset path.', required=True)
main(vars(parser.parse_args()))

查看文件

@ -108,8 +108,10 @@ class BimanualViperXTask(base.Task):
obs['env_state'] = self.get_env_state(physics)
obs['images'] = dict()
obs['images']['top'] = physics.render(height=480, width=640, camera_id='top')
obs['images']['angle'] = physics.render(height=480, width=640, camera_id='angle')
obs['images']['vis'] = physics.render(height=480, width=640, camera_id='front_close')
obs['images']['left_wrist'] = physics.render(height=480, width=640, camera_id='left_wrist')
obs['images']['right_wrist'] = physics.render(height=480, width=640, camera_id='right_wrist')
# obs['images']['angle'] = physics.render(height=480, width=640, camera_id='angle')
# obs['images']['vis'] = physics.render(height=480, width=640, camera_id='front_close')
return obs

查看文件

@ -47,6 +47,7 @@ def main(args):
num_episodes = task_config['num_episodes']
episode_len = task_config['episode_len']
camera_names = task_config['camera_names']
name_filter = task_config.get('name_filter', lambda n: True)
# fixed parameters
state_dim = 14
@ -105,7 +106,7 @@ def main(args):
# print()
# exit()
train_dataloader, val_dataloader, stats, _ = load_data(dataset_dir, num_episodes, camera_names, batch_size_train, batch_size_val)
train_dataloader, val_dataloader, stats, _ = load_data(dataset_dir, name_filter, camera_names, batch_size_train, batch_size_val)
# save dataset stats
# if not os.path.isdir(ckpt_dir):

查看文件

@ -2,14 +2,16 @@ import numpy as np
import torch
import os
import h5py
import pickle
import fnmatch
import cv2
from torch.utils.data import TensorDataset, DataLoader
import IPython
e = IPython.embed
class EpisodicDataset(torch.utils.data.Dataset):
def __init__(self, dataset_path_list, camera_names, norm_stats, episode_ids, episode_len, chunk_size):
def __init__(self, dataset_path_list, camera_names, norm_stats, episode_ids, episode_len, chunk_size, policy_class):
super(EpisodicDataset).__init__()
self.episode_ids = episode_ids
self.dataset_path_list = dataset_path_list
@ -19,6 +21,7 @@ class EpisodicDataset(torch.utils.data.Dataset):
self.chunk_size = chunk_size
self.cumulative_len = np.cumsum(self.episode_len)
self.max_episode_len = max(episode_len)
self.policy_class = policy_class
self.__getitem__(0) # initialize self.is_sim
self.is_sim = False
@ -38,7 +41,11 @@ class EpisodicDataset(torch.utils.data.Dataset):
try:
# print(dataset_path)
with h5py.File(dataset_path, 'r') as root:
is_sim = False # root.attrs['sim']
try: # some legacy data does not have this attribute
is_sim = root.attrs['sim']
except:
is_sim = False
compressed = root.attrs.get('compress', False)
if '/base_action' in root:
base_action = root['/base_action'][()]
base_action = preprocess_base_action(base_action)
@ -55,6 +62,12 @@ class EpisodicDataset(torch.utils.data.Dataset):
image_dict = dict()
for cam_name in self.camera_names:
image_dict[cam_name] = root[f'/observations/images/{cam_name}'][start_ts]
if compressed:
for cam_name in image_dict.keys():
decompressed_image = cv2.imdecode(image_dict[cam_name], 1)
image_dict[cam_name] = np.array(decompressed_image)
# get all actions after and including start_ts
if is_sim:
action = action[start_ts:]
@ -89,11 +102,18 @@ class EpisodicDataset(torch.utils.data.Dataset):
# normalize image and change dtype to float
image_data = image_data / 255.0
action_data = (action_data - self.norm_stats["action_mean"]) / self.norm_stats["action_std"]
if self.policy_class == 'Diffusion':
# normalize to [-1, 1]
action_data = ((action_data - self.norm_stats["action_min"]) / (self.norm_stats["action_max"] - self.norm_stats["action_min"])) * 2 - 1
else:
# normalize to mean 0 std 1
action_data = (action_data - self.norm_stats["action_mean"]) / self.norm_stats["action_std"]
qpos_data = (qpos_data - self.norm_stats["qpos_mean"]) / self.norm_stats["qpos_std"]
except:
print(f'Error loading {dataset_path}')
print(f'Error loading {dataset_path} in __getitem__')
quit()
# print(image_data.dtype, qpos_data.dtype, action_data.dtype, is_pad.dtype)
@ -118,8 +138,9 @@ def get_norm_stats(dataset_path_list):
action = root['/action'][()]
dummy_base_action = np.zeros([action.shape[0], 2])
action = np.concatenate([action, dummy_base_action], axis=-1)
except:
print(f'Error loading {dataset_path}')
except Exception as e:
print(f'Error loading {dataset_path} in get_norm_stats')
print(e)
quit()
all_qpos_data.append(torch.from_numpy(qpos))
all_action_data.append(torch.from_numpy(action))
@ -137,65 +158,82 @@ def get_norm_stats(dataset_path_list):
qpos_std = all_qpos_data.std(dim=[0]).float()
qpos_std = torch.clip(qpos_std, 1e-2, np.inf) # clipping
action_min = all_action_data.min(dim=0).values.float()
action_max = all_action_data.max(dim=0).values.float()
eps = 0.0001
stats = {"action_mean": action_mean.numpy(), "action_std": action_std.numpy(),
"action_min": action_min.numpy() - eps,"action_max": action_max.numpy() + eps,
"qpos_mean": qpos_mean.numpy(), "qpos_std": qpos_std.numpy(),
"example_qpos": qpos}
return stats, all_episode_len
def find_all_hdf5(dataset_dir):
def find_all_hdf5(dataset_dir, skip_mirrored_data):
hdf5_files = []
for root, dirs, files in os.walk(dataset_dir):
for filename in fnmatch.filter(files, '*.hdf5'):
if skip_mirrored_data and 'mirror' in filename:
continue
hdf5_files.append(os.path.join(root, filename))
print(f'Found {len(hdf5_files)} hdf5 files')
return hdf5_files
def load_data(dataset_dir, num_episodes, camera_names, batch_size_train, batch_size_val, chunk_size):
del num_episodes
dataset_path_list = find_all_hdf5(dataset_dir)
dataset_path_list = [p for p in dataset_path_list if 'sim' not in p] ### TODO filter out sim data
def load_data(dataset_dir, name_filter, camera_names, batch_size_train, batch_size_val, chunk_size, skip_mirrored_data=False, load_pretrain=False, policy_class=None):
dataset_path_list = find_all_hdf5(dataset_dir, skip_mirrored_data)
dataset_path_list = [n for n in dataset_path_list if name_filter(n)]
num_episodes = len(dataset_path_list)
# obtain train test split
train_ratio = 0.998
train_ratio = 0.995
shuffled_episode_ids = np.random.permutation(num_episodes)
train_episode_ids = shuffled_episode_ids[:int(train_ratio * num_episodes)]
val_episode_ids = shuffled_episode_ids[int(train_ratio * num_episodes):]
print(f'\n\nData from: {dataset_dir}\n- Train on {len(train_episode_ids)} episodes\n- Test on {len(val_episode_ids)} episodes\n\n')
# obtain normalization stats for qpos and action
# if load_pretrain:
# with open(os.path.join('/home/zfu/interbotix_ws/src/act/ckpts/pretrain_all', 'dataset_stats.pkl'), 'rb') as f:
# norm_stats = pickle.load(f)
# print('Loaded pretrain dataset stats')
norm_stats, all_episode_len = get_norm_stats(dataset_path_list)
train_episode_len = [all_episode_len[i] for i in train_episode_ids]
val_episode_len = [all_episode_len[i] for i in val_episode_ids]
# construct dataset and dataloader
train_dataset = EpisodicDataset(dataset_path_list, camera_names, norm_stats, train_episode_ids, train_episode_len, chunk_size)
val_dataset = EpisodicDataset(dataset_path_list, camera_names, norm_stats, val_episode_ids, val_episode_len, chunk_size)
train_dataset = EpisodicDataset(dataset_path_list, camera_names, norm_stats, train_episode_ids, train_episode_len, chunk_size, policy_class)
val_dataset = EpisodicDataset(dataset_path_list, camera_names, norm_stats, val_episode_ids, val_episode_len, chunk_size, policy_class)
train_dataloader = DataLoader(train_dataset, batch_size=batch_size_train, shuffle=True, pin_memory=True, num_workers=1, prefetch_factor=1)
val_dataloader = DataLoader(val_dataset, batch_size=batch_size_val, shuffle=True, pin_memory=True, num_workers=1, prefetch_factor=1)
return train_dataloader, val_dataloader, norm_stats, train_dataset.is_sim
def calibrate_linear_vel(base_action):
c = 0.19
v = base_action[:, 0]
w = base_action[:, 1]
base_action[:, 0] = v - c * w
def calibrate_linear_vel(base_action, c=None):
if c is None:
c = 0.0 # 0.19
v = base_action[..., 0]
w = base_action[..., 1]
base_action = base_action.copy()
base_action[..., 0] = v - c * w
return base_action
def smooth_base_action(base_action):
return np.stack([
np.convolve(base_action[:, i], np.ones(20)/20, mode='same') for i in range(base_action.shape[1])
], axis=-1, dtype=np.float32)
np.convolve(base_action[:, i], np.ones(5)/5, mode='same') for i in range(base_action.shape[1])
], axis=-1).astype(np.float32)
def preprocess_base_action(base_action):
base_action = calibrate_linear_vel(base_action)
# base_action = calibrate_linear_vel(base_action)
base_action = smooth_base_action(base_action)
return base_action
def postprocess_base_action(self, base_action):
linear_vel, angular_vel = base_action
angular_vel *= 0.9
return np.array([linear_vel, angular_vel])
### env utils
def sample_box_pose():

查看文件

@ -33,7 +33,11 @@ def load_hdf5(dataset_dir, dataset_name):
def main(args):
dataset_dir = args['dataset_dir']
episode_idx = args['episode_idx']
dataset_name = f'episode_{episode_idx}'
ismirror = args['ismirror']
if ismirror:
dataset_name = f'mirror_episode_{episode_idx}'
else:
dataset_name = f'episode_{episode_idx}'
qpos, qvel, action, image_dict = load_hdf5(dataset_dir, dataset_name)
save_videos(image_dict, DT, video_path=os.path.join(dataset_dir, dataset_name + '_video.mp4'))
@ -44,6 +48,7 @@ def main(args):
def save_videos(video, dt, video_path=None):
if isinstance(video, list):
cam_names = list(video[0].keys())
cam_names = sorted(cam_names)
h, w, _ = video[0][cam_names[0]].shape
w = w * len(cam_names)
fps = int(1/dt)
@ -60,6 +65,7 @@ def save_videos(video, dt, video_path=None):
print(f'Saved video to: {video_path}')
elif isinstance(video, dict):
cam_names = list(video.keys())
cam_names = sorted(cam_names)
all_cam_videos = []
for cam_name in cam_names:
all_cam_videos.append(video[cam_name])
@ -144,4 +150,5 @@ if __name__ == '__main__':
parser = argparse.ArgumentParser()
parser.add_argument('--dataset_dir', action='store', type=str, help='Dataset dir.', required=True)
parser.add_argument('--episode_idx', action='store', type=int, help='Episode index.', required=False)
parser.add_argument('--ismirror', action='store_true')
main(vars(parser.parse_args()))