44 次代码提交

作者 SHA1 备注 提交日期
0aba9e4f67 real robot not working 2023-12-15 00:09:10 -08:00
a0905ad3b5 async query works in sim 2023-12-14 23:48:42 -08:00
c3c9deeaa3 uncommit random rotation, colorjitter rand 2023-12-14 23:13:36 -08:00
ea3aad108d Merge branch 'main' of github.com:MarkFzp/act-plus-plus into main 2023-12-14 23:09:30 -08:00
ad44c2ad3d reduce train_num_workers if os.getlogin() == 'zfu' 2023-12-14 23:09:24 -08:00
09098015e7 add aloha_mobile_wipe_wine_2 2023-12-14 22:57:52 -08:00
3794f96a42 add augmentation for diffusion 2023-12-14 21:21:34 -08:00
47270bd18f Merge branch 'main' of github.com:MarkFzp/act-plus-plus into main 2023-12-12 21:28:08 -08:00
7a8f14492c add base delay to diffusion eval 2023-12-12 21:28:05 -08:00
d065a7b84d add data set flag for vinn cache feature 2023-12-12 21:26:14 -08:00
7d3f51cfcd no EMA and commands 2023-12-12 21:22:42 -08:00
2234dd5ce7 change qpos plot size 2023-12-12 18:22:10 -05:00
1dd85c07c1 Merge branch 'main' of github.com:MarkFzp/act-plus-plus into main 2023-12-12 17:28:58 -05:00
547a734f7b log qpos during eval 2023-12-12 17:28:50 -05:00
61ffa1bdb1 vinn_eval pipeline 2023-12-11 21:52:59 -08:00
0247d3669c add decompress 2023-12-11 17:29:55 -08:00
0ad73fc50f real expr 2023-12-11 16:28:05 -08:00
91f1bbf6e2 VINN works for sim transfer cube 2023-12-10 20:18:46 -08:00
c460687c33 fix diffusion EMA 2023-12-10 14:40:35 -08:00
66e07fff99 truncate_data.py 2023-11-30 19:41:18 -05:00
45fd925b29 Merge branch 'main' of github.com:MarkFzp/act-plus-plus into main 2023-11-26 16:11:40 -08:00
4df9486efe add BASE_DELAY to eval_bc, elevator task 2023-11-26 16:11:36 -08:00
20ab6fa14e delete v1 co-training data pipeline 2023-11-25 17:11:32 -08:00
d90fd74aae num_workers=2 & prefetch_factor=2, print norm stats dir 2023-11-25 20:01:44 -05:00
265dad7cba refactor co-training data loading pipeline 2023-11-25 16:16:32 -08:00
4228c61cd9 add co-training options: sample_weights, list of dataset_dir, stats_dir, train_ratio 2023-11-25 00:08:10 -08:00
30d172203f add FPS to constants.py 2023-11-23 06:29:10 -05:00
a5e127a58e Merge branch 'main' of github.com:MarkFzp/act-plus-plus into main 2023-11-23 01:16:07 -08:00
953f952439 FPS 50 in imitate_episode.py, truncate aloha_mobile_wash_pan task to 1100 steps, add setup.py 2023-11-23 01:16:00 -08:00
dfae097b89 debug actuator_net in imitate_episodes.py 2023-11-21 17:56:41 -05:00
ad52735336 Merge branch 'main' of github.com:MarkFzp/act-plus-plus into main 2023-11-21 12:27:36 -08:00
de1c00235f dxl_test dynamixel_client, modify eval_bc for real 2023-11-21 12:25:55 -08:00
c593d1746b actuator eval pipeline in imitation_episodes eval_bc() 2023-11-21 00:43:34 -05:00
cad6d302ab default lr=1e-4 in train_actuator_network.py 2023-11-20 18:18:21 -05:00
4154aad37c add prediction_len to train_actuator_network.py 2023-11-20 16:21:44 -05:00
ee4433d812 actuator network 2023-11-18 20:50:56 -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
15ac53a6b0 compress_data.py 2023-11-10 20:43:50 -08:00
共有 22 个文件被更改,包括 2753 次插入 和 147 次删除

4
.gitignore vendored
查看文件

@ -141,3 +141,7 @@ dmypy.json
ckpts/
*log*
*.png
*.npy
play.ipynb

查看文件

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

1
__init__.py 普通文件
查看文件

@ -0,0 +1 @@
w

1
byol_pytorch 子模块

子模块 byol_pytorch 已添加到 bcd1b066c3

查看文件

@ -58,7 +58,7 @@ CUDA_VISIBLE_DEVICES=0 python3 imitate_episodes.py \
--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
--num_steps 100000 --eval_every 2000 --validate_every 2000 --save_every 2000 --no_encoder
# launch experiment on all data
@ -70,26 +70,355 @@ CUDA_VISIBLE_DEVICES=0 python3 imitate_episodes.py \
--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
--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
## Nov 10 history
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_insertion_scripted_mirror \
--ckpt_dir /scr/tonyzhao/train_logs/cube_scripted_test_history \
--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 100000 --validate_every 5000 --save_every 2000 \
--skip_mirrored_data --no_encoder --history 100 --eval
--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
# Dec 10
######################## more diffusion ########################
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_chunk64 \
--policy_class Diffusion --chunk_size 64 \
--batch_size 32 --lr 1e-4 --seed 0 \
--num_steps 200000 --eval_every 4000 --validate_every 4000 --save_every 4000
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_regressionTest \
--policy_class Diffusion --chunk_size 32 \
--batch_size 32 --lr 1e-4 --seed 0 \
--num_steps 200000 --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=0 python3 imitate_episodes.py \
--task_name sim_transfer_cube_scripted \
--ckpt_dir /scr/tonyzhao/train_logs/cube_scripted_diffusion_sweep_5_noEMA \
--policy_class Diffusion --chunk_size 32 \
--batch_size 32 --lr 1e-4 --seed 0 \
--num_steps 200000 --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_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
###### Diffusion Real ######
## deploy
python3 imitate_episodes.py --task_name aloha_mobile_wipe_wine --ckpt_dir /home/mobile-aloha/interbotix_ws/src/act/ckpts/wipe_wine_diffusion_augmentation_seed0/ --policy_class Diffusion --chunk_size 32 --batch_size 32 --lr 1e-4 --seed 0 --num_steps 1000000 --eval_every 1000000 --validate_every 5000 --save_every 5000 --eval
conda activate mobile
export MUJOCO_GL=egl
cd /home/tonyzhao/Research/act-plus-plus
CUDA_VISIBLE_DEVICES=1 python3 imitate_episodes.py \
--task_name aloha_mobile_wipe_wine \
--ckpt_dir /scr/tonyzhao/train_logs/wipe_wine_diffusion_seed0 \
--policy_class Diffusion --chunk_size 32 \
--batch_size 32 --lr 1e-4 --seed 0 \
--num_steps 1000000 --eval_every 1000000 --validate_every 5000 --save_every 5000
## Cotrain
conda activate mobile
export MUJOCO_GL=egl
cd /home/tonyzhao/Research/act-plus-plus
CUDA_VISIBLE_DEVICES=1 python3 imitate_episodes.py \
--task_name aloha_mobile_wipe_wine_cotrain \
--ckpt_dir /scr/tonyzhao/train_logs/wipe_wine_cotrain_diffusion_seed0 \
--policy_class Diffusion --chunk_size 32 \
--batch_size 32 --lr 1e-4 --seed 0 \
--num_steps 1000000 --eval_every 1000000 --validate_every 5000 --save_every 5000
# train no cotrain again with augmentations
conda activate mobile
export MUJOCO_GL=egl
cd /home/tonyzhao/Research/act-plus-plus
CUDA_VISIBLE_DEVICES=0 python3 imitate_episodes.py \
--task_name aloha_mobile_wipe_wine \
--ckpt_dir /scr/tonyzhao/train_logs/wipe_wine_diffusion_augmentation_seed0 \
--policy_class Diffusion --chunk_size 32 \
--batch_size 32 --lr 1e-4 --seed 0 \
--num_steps 1000000 --eval_every 1000000 --validate_every 5000 --save_every 5000
## Cotrain with augmentations
conda activate mobile
export MUJOCO_GL=egl
cd /home/tonyzhao/Research/act-plus-plus
CUDA_VISIBLE_DEVICES=1 python3 imitate_episodes.py \
--task_name aloha_mobile_wipe_wine_cotrain \
--ckpt_dir /scr/tonyzhao/train_logs/wipe_wine_cotrain_diffusion_augmentation_seed0 \
--policy_class Diffusion --chunk_size 32 \
--batch_size 32 --lr 1e-4 --seed 0 \
--num_steps 1000000 --eval_every 1000000 --validate_every 5000 --save_every 5000
######################## VINN ########################
conda activate mobile
cd /home/tonyzhao/Research/act-plus-plus/byol_pytorch/examples/lightning
CUDA_VISIBLE_DEVICES=1 python3 train.py --dataset_dir /scr/tonyzhao/datasets/sim_transfer_cube_scripted --cam_name top --seed 0
conda activate mobile
cd /home/tonyzhao/Research/act-plus-plus/byol_pytorch/examples/lightning
CUDA_VISIBLE_DEVICES=0 python3 train.py --dataset_dir /scr/tonyzhao/datasets/sim_transfer_cube_scripted --cam_name left_wrist --seed 0
conda activate mobile
cd /home/tonyzhao/Research/act-plus-plus/byol_pytorch/examples/lightning
CUDA_VISIBLE_DEVICES=1 python3 train.py --dataset_dir /scr/tonyzhao/datasets/sim_transfer_cube_scripted --cam_name right_wrist --seed 0
conda activate mobile
cd /home/tonyzhao/Research/act-plus-plus
TASK_NAME=sim_transfer_cube_scripted
python3 vinn_cache_feature.py --ckpt_path /home/tonyzhao/Research/act-plus-plus/byol_pytorch/examples/lightning/byol-${TASK_NAME}-DUMMY-seed-0.pt
TASK_NAME=sim_transfer_cube_scripted
python3 vinn_select_k.py \
--dataset_dir /scr/tonyzhao/datasets/sim_transfer_cube_scripted \
--ckpt_dir /scr/tonyzhao/train_logs/VINN-eval-seed-0-test
python3 vinn_eval.py \
--dataset_dir /scr/tonyzhao/datasets/sim_transfer_cube_scripted \
--model_dir /home/tonyzhao/Research/act-plus-plus/byol_pytorch/examples/lightning/byol-${TASK_NAME}-DUMMY-seed-0.pt \
--ckpt_dir /scr/tonyzhao/train_logs/VINN-eval-seed-0-test \
--task_name $TASK_NAME
## TODO
make sure env is consistent
tune a bit more
######################## VINN Real ########################
### test backward compatibility
conda activate mobile
cd /home/tonyzhao/Research/act-plus-plus/byol_pytorch/examples/lightning
CUDA_VISIBLE_DEVICES=1 python3 train.py --task sim_transfer_cube_scripted --cam_name top --seed 0
CUDA_VISIBLE_DEVICES=1 python3 train.py --task sim_transfer_cube_scripted --cam_name left_wrist --seed 0
conda activate mobile
cd /home/tonyzhao/Research/act-plus-plus/byol_pytorch/examples/lightning
CUDA_VISIBLE_DEVICES=1 python3 train.py --task sim_transfer_cube_scripted --cam_name right_wrist --seed 0
conda activate mobile
cd /home/tonyzhao/Research/act-plus-plus
TASK_NAME=sim_transfer_cube_scripted
python3 vinn_cache_feature.py --ckpt_path /home/tonyzhao/Research/act-plus-plus/byol_pytorch/examples/lightning/byol-${TASK_NAME}-DUMMY-seed-0.pt
TASK_NAME=sim_transfer_cube_scripted
python3 vinn_select_k.py \
--dataset_dir /scr/tonyzhao/datasets/sim_transfer_cube_scripted \
--ckpt_dir /scr/tonyzhao/train_logs/VINN-eval-seed-0-test
python3 vinn_eval.py \
--dataset_dir /scr/tonyzhao/datasets/sim_transfer_cube_scripted \
--model_dir /home/tonyzhao/Research/act-plus-plus/byol_pytorch/examples/lightning/byol-${TASK_NAME}-DUMMY-seed-0.pt \
--ckpt_dir /scr/tonyzhao/train_logs/VINN-eval-seed-0-test \
--task_name $TASK_NAME
### new data loader passed backward compatibility
conda activate mobile
cd /home/tonyzhao/Research/act-plus-plus/byol_pytorch/examples/lightning
#CUDA_VISIBLE_DEVICES=1 python3 train.py --task aloha_mobile_wipe_wine --cam_name cam_high --seed 0
#CUDA_VISIBLE_DEVICES=1 python3 train.py --task aloha_mobile_wipe_wine --cam_name cam_left_wrist --seed 0
#CUDA_VISIBLE_DEVICES=1 python3 train.py --task aloha_mobile_wipe_wine --cam_name cam_right_wrist --seed 0
#CUDA_VISIBLE_DEVICES=1 python3 train.py --task aloha_mobile_wipe_wine_cotrain --cam_name cam_high --seed 0
#CUDA_VISIBLE_DEVICES=1 python3 train.py --task aloha_mobile_wipe_wine_cotrain --cam_name cam_left_wrist --seed 0
CUDA_VISIBLE_DEVICES=1 python3 train.py --task aloha_mobile_wipe_wine_cotrain --cam_name cam_right_wrist --seed 0
conda activate mobile
cd /home/tonyzhao/Research/act-plus-plus/byol_pytorch/examples/lightning
#CUDA_VISIBLE_DEVICES=1 python3 train.py --task aloha_mobile_wash_pan --cam_name cam_high --seed 0
#CUDA_VISIBLE_DEVICES=1 python3 train.py --task aloha_mobile_wash_pan --cam_name cam_left_wrist --seed 0
#CUDA_VISIBLE_DEVICES=1 python3 train.py --task aloha_mobile_wash_pan --cam_name cam_right_wrist --seed 0
#CUDA_VISIBLE_DEVICES=1 python3 train.py --task aloha_mobile_wash_pan_cotrain --cam_name cam_high --seed 0
#CUDA_VISIBLE_DEVICES=1 python3 train.py --task aloha_mobile_wash_pan_cotrain --cam_name cam_left_wrist --seed 0
CUDA_VISIBLE_DEVICES=1 python3 train.py --task aloha_mobile_wash_pan_cotrain --cam_name cam_right_wrist --seed 0
conda activate mobile
cd /home/tonyzhao/Research/act-plus-plus/byol_pytorch/examples/lightning
CUDA_VISIBLE_DEVICES=1 python3 train.py --task aloha_mobile_wipe_wine_cotrain --cam_name cam_right_wrist --seed 0
CUDA_VISIBLE_DEVICES=1 python3 train.py --task aloha_mobile_elevator_truncated --cam_name cam_high --seed 0
CUDA_VISIBLE_DEVICES=1 python3 train.py --task aloha_mobile_elevator_truncated --cam_name cam_left_wrist --seed 0
CUDA_VISIBLE_DEVICES=1 python3 train.py --task aloha_mobile_elevator_truncated --cam_name cam_right_wrist --seed 0
conda activate mobile
cd /home/tonyzhao/Research/act-plus-plus/byol_pytorch/examples/lightning
CUDA_VISIBLE_DEVICES=1 python3 train.py --task aloha_mobile_wash_pan_cotrain --cam_name cam_right_wrist --seed 0
CUDA_VISIBLE_DEVICES=1 python3 train.py --task aloha_mobile_elevator_truncated_cotrain --cam_name cam_high --seed 0
CUDA_VISIBLE_DEVICES=1 python3 train.py --task aloha_mobile_elevator_truncated_cotrain --cam_name cam_left_wrist --seed 0
CUDA_VISIBLE_DEVICES=1 python3 train.py --task aloha_mobile_elevator_truncated_cotrain --cam_name cam_right_wrist --seed 0
conda activate mobile
export CUDA_VISIBLE_DEVICES=1
cd /home/tonyzhao/Research/act-plus-plus
TASK_NAME=aloha_mobile_wipe_wine
DATA_NAME=aloha_mobile_wipe_wine
python3 vinn_cache_feature.py --ckpt_path /home/tonyzhao/Research/act-plus-plus/byol_pytorch/examples/lightning/byol-${TASK_NAME}-DUMMY-seed-0.pt \
--dataset_dir /scr/tonyzhao/mobile_aloha_datasets/${DATA_NAME}
conda activate mobile
export CUDA_VISIBLE_DEVICES=1
cd /home/tonyzhao/Research/act-plus-plus
TASK_NAME=aloha_mobile_wipe_wine_cotrain
DATA_NAME=aloha_mobile_wipe_wine
python3 vinn_cache_feature.py --ckpt_path /home/tonyzhao/Research/act-plus-plus/byol_pytorch/examples/lightning/byol-${TASK_NAME}-DUMMY-seed-0.pt \
--dataset_dir /scr/tonyzhao/mobile_aloha_datasets/${DATA_NAME}
conda activate mobile
export CUDA_VISIBLE_DEVICES=1
cd /home/tonyzhao/Research/act-plus-plus
TASK_NAME=aloha_mobile_wash_pan
DATA_NAME=aloha_mobile_wash_pan
python3 vinn_cache_feature.py --ckpt_path /home/tonyzhao/Research/act-plus-plus/byol_pytorch/examples/lightning/byol-${TASK_NAME}-DUMMY-seed-0.pt \
--dataset_dir /scr/tonyzhao/mobile_aloha_datasets/${DATA_NAME}
conda activate mobile
export CUDA_VISIBLE_DEVICES=1
cd /home/tonyzhao/Research/act-plus-plus
TASK_NAME=aloha_mobile_wash_pan_cotrain
DATA_NAME=aloha_mobile_wash_pan
python3 vinn_cache_feature.py --ckpt_path /home/tonyzhao/Research/act-plus-plus/byol_pytorch/examples/lightning/byol-${TASK_NAME}-DUMMY-seed-0.pt \
--dataset_dir /scr/tonyzhao/mobile_aloha_datasets/${DATA_NAME}
conda activate mobile
export CUDA_VISIBLE_DEVICES=1
cd /home/tonyzhao/Research/act-plus-plus
TASK_NAME=aloha_mobile_elevator_truncated
DATA_NAME=aloha_mobile_elevator_truncated
python3 vinn_cache_feature.py --ckpt_path /home/tonyzhao/Research/act-plus-plus/byol_pytorch/examples/lightning/byol-${TASK_NAME}-DUMMY-seed-0.pt \
--dataset_dir /scr/tonyzhao/mobile_aloha_datasets/${DATA_NAME}
conda activate mobile
export CUDA_VISIBLE_DEVICES=1
cd /home/tonyzhao/Research/act-plus-plus
TASK_NAME=aloha_mobile_elevator_truncated_cotrain
DATA_NAME=aloha_mobile_elevator_truncated
python3 vinn_cache_feature.py --ckpt_path /home/tonyzhao/Research/act-plus-plus/byol_pytorch/examples/lightning/byol-${TASK_NAME}-DUMMY-seed-0.pt \
--dataset_dir /scr/tonyzhao/mobile_aloha_datasets/${DATA_NAME}
# eval on real robot
conda activate aloha
cd /home/mobile-aloha/interbotix_ws/src/act
TASK_NAME=aloha_mobile_wipe_wine
python3 vinn_cache_feature.py --ckpt_path /home/mobile-aloha/interbotix_ws/src/act/ckpts/vinn_ckpts/byol-${TASK_NAME}-DUMMY-seed-0.pt
TASK_NAME=aloha_mobile_wipe_wine
python3 vinn_select_k.py \
--dataset_dir /home/mobile-aloha/data/${TASK_NAME} \
--ckpt_dir /home/mobile-aloha/interbotix_ws/src/act/ckpts/vinn_ckpts/VINN-eval-seed-0-test \
TASK_NAME=aloha_mobile_wipe_wine
python3 vinn_eval.py \
--dataset_dir /home/mobile-aloha/data/${TASK_NAME} \
--model_dir /home/mobile-aloha/interbotix_ws/src/act/ckpts/vinn_ckpts/byol-${TASK_NAME}-DUMMY-seed-0.pt \
--ckpt_dir /home/mobile-aloha/interbotix_ws/src/act/ckpts/vinn_ckpts/VINN-eval-seed-0-test \
--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
TODO: Add history, EMA at test time
conda activate mobile
cd /home/tonyzhao/Research/act-plus-plus
CUDA_VISIBLE_DEVICES=1 python3 train_actuator_network.py

查看文件

@ -99,7 +99,7 @@ def save_videos(video, dt, video_path=None):
images = []
for cam_name in cam_names:
image = image_dict[cam_name]
# image = image[:, :, [2, 1, 0]] # swap B and R channel
image = image[:, :, [2, 1, 0]] # swap B and R channel
images.append(image)
images = np.concatenate(images, axis=1)
out.write(images)
@ -119,7 +119,7 @@ def save_videos(video, dt, video_path=None):
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
image = image[:, :, [2, 1, 0]] # swap B and R channel
out.write(image)
out.release()
print(f'Saved video to: {video_path}')
@ -178,5 +178,5 @@ if __name__ == '__main__':
# 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)
load_and_save_first_episode_video(output_dataset_dir, video_path)

查看文件

@ -57,6 +57,7 @@ SIM_TASK_CONFIGS = {
### Simulation envs fixed constants
DT = 0.02
FPS = 50
JOINT_NAMES = ["waist", "shoulder", "elbow", "forearm_roll", "wrist_angle", "wrist_rotate"]
START_ARM_POSE = [0, -0.96, 1.16, 0, -0.3, 0, 0.02239, -0.02239, 0, -0.96, 1.16, 0, -0.3, 0, 0.02239, -0.02239]
@ -69,8 +70,8 @@ PUPPET_GRIPPER_POSITION_OPEN = 0.05800
PUPPET_GRIPPER_POSITION_CLOSE = 0.01844
# Gripper joint limits (qpos[6])
MASTER_GRIPPER_JOINT_OPEN = 0.3083
MASTER_GRIPPER_JOINT_CLOSE = -0.6842
MASTER_GRIPPER_JOINT_OPEN = -0.8
MASTER_GRIPPER_JOINT_CLOSE = -1.65
PUPPET_GRIPPER_JOINT_OPEN = 1.4910
PUPPET_GRIPPER_JOINT_CLOSE = -0.6213

查看文件

@ -75,7 +75,10 @@ def get_args_parser():
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')
parser.add_argument('--history', action='store', type=int, required=False, default=0) # default no history
parser.add_argument('--actuator_network_dir', action='store', type=str, help='actuator_network_dir', required=False)
parser.add_argument('--history_len', action='store', type=int)
parser.add_argument('--future_len', action='store', type=int)
parser.add_argument('--prediction_len', action='store', type=int)
return parser

查看文件

@ -111,22 +111,6 @@ class Joiner(nn.Sequential):
return out, pos
class JoinerChannelConcat(nn.Sequential):
def __init__(self, backbone, position_embedding, conv):
super().__init__(backbone, position_embedding, conv)
def forward(self, tensor_list: NestedTensor):
tensor_list = self[2](tensor_list) # 6 to 3 channels
xs = self[0](tensor_list)
out: List[NestedTensor] = []
pos = []
for name, x in xs.items():
out.append(x)
# position encoding
pos.append(self[1](x).to(x.dtype))
return out, pos
def build_backbone(args):
position_embedding = build_position_encoding(args)
@ -136,15 +120,3 @@ def build_backbone(args):
model = Joiner(backbone, position_embedding)
model.num_channels = backbone.num_channels
return model
def build_backbone_channel_concat(args):
position_embedding = build_position_encoding(args)
train_backbone = args.lr_backbone > 0
return_interm_layers = args.masks
backbone = Backbone(args.backbone, train_backbone, return_interm_layers, args.dilation)
conv = nn.Conv2d(in_channels=6, out_channels=3, kernel_size=3, stride=1, padding=1)
model = JoinerChannelConcat(backbone, position_embedding, conv)
model.num_channels = backbone.num_channels
return model

查看文件

@ -6,7 +6,7 @@ import torch
from torch import nn
from torch.autograd import Variable
import torch.nn.functional as F
from .backbone import build_backbone, build_backbone_channel_concat
from .backbone import build_backbone
from .transformer import build_transformer, TransformerEncoder, TransformerEncoderLayer
import numpy as np
@ -274,12 +274,8 @@ def build(args):
# From image
backbones = []
for _ in args.camera_names:
if args.history >= 1:
backbone = build_backbone_channel_concat(args)
backbones.append(backbone)
else:
backbone = build_backbone(args)
backbones.append(backbone)
backbone = build_backbone(args)
backbones.append(backbone)
transformer = build_transformer(args)

4
dxl_test.py 普通文件
查看文件

@ -0,0 +1,4 @@
from dynamixel_client import DynamixelClient
client = DynamixelClient([1, 2], port='/dev/ttyDXL_wheels', lazy_connect=True)
print(client.read_pos_vel_cur())

604
dynamixel_client.py 普通文件
查看文件

@ -0,0 +1,604 @@
"""Communication using the DynamixelSDK."""
##This is based off of the dynamixel SDK
import atexit
import logging
import time
from typing import Optional, Sequence, Union, Tuple
import numpy as np
PROTOCOL_VERSION = 2.0
# The following addresses assume XH motors.
ADDR_TORQUE_ENABLE = 64
ADDR_GOAL_POSITION = 116
ADDR_PRESENT_POSITION = 132
ADDR_PRESENT_VELOCITY = 128
ADDR_PRESENT_CURRENT = 126
ADDR_PRESENT_POS_VEL_CUR = 126
# Data Byte Length
LEN_PRESENT_POSITION = 4
LEN_PRESENT_VELOCITY = 4
LEN_PRESENT_CURRENT = 2
LEN_PRESENT_POS_VEL_CUR = 10
LEN_GOAL_POSITION = 4
DEFAULT_POS_SCALE = 2.0 * np.pi / 4096 # 0.088 degrees
# See http://emanual.robotis.com/docs/en/dxl/x/xh430-v210/#goal-velocity
DEFAULT_VEL_SCALE = 0.229 * 2.0 * np.pi / 60.0 # 0.229 rpm
DEFAULT_CUR_SCALE = 1.34
def dynamixel_cleanup_handler():
"""Cleanup function to ensure Dynamixels are disconnected properly."""
open_clients = list(DynamixelClient.OPEN_CLIENTS)
for open_client in open_clients:
if open_client.port_handler.is_using:
logging.warning('Forcing client to close.')
open_client.port_handler.is_using = False
open_client.disconnect()
def signed_to_unsigned(value: int, size: int) -> int:
"""Converts the given value to its unsigned representation."""
if value < 0:
bit_size = 8 * size
max_value = (1 << bit_size) - 1
value = max_value + value
return value
def unsigned_to_signed(value: int, size: int) -> int:
"""Converts the given value from its unsigned representation."""
bit_size = 8 * size
if (value & (1 << (bit_size - 1))) != 0:
value = -((1 << bit_size) - value)
return value
class DynamixelClient:
"""Client for communicating with Dynamixel motors.
NOTE: This only supports Protocol 2.
"""
# The currently open clients.
OPEN_CLIENTS = set()
def __init__(self,
motor_ids: Sequence[int],
port: str = '/dev/ttyUSB0',
baudrate: int = 1000000,
lazy_connect: bool = False,
pos_scale: Optional[float] = None,
vel_scale: Optional[float] = None,
cur_scale: Optional[float] = None):
"""Initializes a new client.
Args:
motor_ids: All motor IDs being used by the client.
port: The Dynamixel device to talk to. e.g.
- Linux: /dev/ttyUSB0
- Mac: /dev/tty.usbserial-*
- Windows: COM1
baudrate: The Dynamixel baudrate to communicate with.
lazy_connect: If True, automatically connects when calling a method
that requires a connection, if not already connected.
pos_scale: The scaling factor for the positions. This is
motor-dependent. If not provided, uses the default scale.
vel_scale: The scaling factor for the velocities. This is
motor-dependent. If not provided uses the default scale.
cur_scale: The scaling factor for the currents. This is
motor-dependent. If not provided uses the default scale.
"""
import dynamixel_sdk
self.dxl = dynamixel_sdk
self.motor_ids = list(motor_ids)
self.port_name = port
self.baudrate = baudrate
self.lazy_connect = lazy_connect
self.port_handler = self.dxl.PortHandler(port)
self.packet_handler = self.dxl.PacketHandler(PROTOCOL_VERSION)
self._pos_vel_cur_reader = DynamixelPosVelCurReader(
self,
self.motor_ids,
pos_scale=pos_scale if pos_scale is not None else DEFAULT_POS_SCALE,
vel_scale=vel_scale if vel_scale is not None else DEFAULT_VEL_SCALE,
cur_scale=cur_scale if cur_scale is not None else DEFAULT_CUR_SCALE,
)
self._pos_reader = DynamixelPosReader(
self,
self.motor_ids,
pos_scale=pos_scale if pos_scale is not None else DEFAULT_POS_SCALE,
vel_scale=vel_scale if vel_scale is not None else DEFAULT_VEL_SCALE,
cur_scale=cur_scale if cur_scale is not None else DEFAULT_CUR_SCALE,
)
self._vel_reader = DynamixelVelReader(
self,
self.motor_ids,
pos_scale=pos_scale if pos_scale is not None else DEFAULT_POS_SCALE,
vel_scale=vel_scale if vel_scale is not None else DEFAULT_VEL_SCALE,
cur_scale=cur_scale if cur_scale is not None else DEFAULT_CUR_SCALE,
)
self._cur_reader = DynamixelCurReader(
self,
self.motor_ids,
pos_scale=pos_scale if pos_scale is not None else DEFAULT_POS_SCALE,
vel_scale=vel_scale if vel_scale is not None else DEFAULT_VEL_SCALE,
cur_scale=cur_scale if cur_scale is not None else DEFAULT_CUR_SCALE,
)
self._sync_writers = {}
self.OPEN_CLIENTS.add(self)
@property
def is_connected(self) -> bool:
return self.port_handler.is_open
def connect(self):
"""Connects to the Dynamixel motors.
NOTE: This should be called after all DynamixelClients on the same
process are created.
"""
assert not self.is_connected, 'Client is already connected.'
if self.port_handler.openPort():
logging.info('Succeeded to open port: %s', self.port_name)
else:
raise OSError(
('Failed to open port at {} (Check that the device is powered '
'on and connected to your computer).').format(self.port_name))
if self.port_handler.setBaudRate(self.baudrate):
logging.info('Succeeded to set baudrate to %d', self.baudrate)
else:
raise OSError(
('Failed to set the baudrate to {} (Ensure that the device was '
'configured for this baudrate).').format(self.baudrate))
# Start with all motors enabled. NO, I want to set settings before enabled
#self.set_torque_enabled(self.motor_ids, True)
def disconnect(self):
"""Disconnects from the Dynamixel device."""
if not self.is_connected:
return
if self.port_handler.is_using:
logging.error('Port handler in use; cannot disconnect.')
return
# Ensure motors are disabled at the end.
self.set_torque_enabled(self.motor_ids, False, retries=0)
self.port_handler.closePort()
if self in self.OPEN_CLIENTS:
self.OPEN_CLIENTS.remove(self)
def set_torque_enabled(self,
motor_ids: Sequence[int],
enabled: bool,
retries: int = -1,
retry_interval: float = 0.25):
"""Sets whether torque is enabled for the motors.
Args:
motor_ids: The motor IDs to configure.
enabled: Whether to engage or disengage the motors.
retries: The number of times to retry. If this is <0, will retry
forever.
retry_interval: The number of seconds to wait between retries.
"""
remaining_ids = list(motor_ids)
while remaining_ids:
remaining_ids = self.write_byte(
remaining_ids,
int(enabled),
ADDR_TORQUE_ENABLE,
)
if remaining_ids:
logging.error('Could not set torque %s for IDs: %s',
'enabled' if enabled else 'disabled',
str(remaining_ids))
if retries == 0:
break
time.sleep(retry_interval)
retries -= 1
def read_pos_vel_cur(self) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
"""Returns the current positions and velocities."""
return self._pos_vel_cur_reader.read()
def read_pos(self) -> np.ndarray:
"""Returns the current positions and velocities."""
return self._pos_reader.read()
def read_vel(self) -> np.ndarray:
"""Returns the current positions and velocities."""
return self._vel_reader.read()
def read_cur(self) -> np.ndarray:
"""Returns the current positions and velocities."""
return self._cur_reader.read()
def write_desired_pos(self, motor_ids: Sequence[int],
positions: np.ndarray):
"""Writes the given desired positions.
Args:
motor_ids: The motor IDs to write to.
positions: The joint angles in radians to write.
"""
assert len(motor_ids) == len(positions)
# Convert to Dynamixel position space.
positions = positions / self._pos_vel_cur_reader.pos_scale
self.sync_write(motor_ids, positions, ADDR_GOAL_POSITION,
LEN_GOAL_POSITION)
def write_byte(
self,
motor_ids: Sequence[int],
value: int,
address: int,
) -> Sequence[int]:
"""Writes a value to the motors.
Args:
motor_ids: The motor IDs to write to.
value: The value to write to the control table.
address: The control table address to write to.
Returns:
A list of IDs that were unsuccessful.
"""
self.check_connected()
errored_ids = []
for motor_id in motor_ids:
comm_result, dxl_error = self.packet_handler.write1ByteTxRx(
self.port_handler, motor_id, address, value)
success = self.handle_packet_result(
comm_result, dxl_error, motor_id, context='write_byte')
if not success:
errored_ids.append(motor_id)
return errored_ids
def sync_write(self, motor_ids: Sequence[int],
values: Sequence[Union[int, float]], address: int,
size: int):
"""Writes values to a group of motors.
Args:
motor_ids: The motor IDs to write to.
values: The values to write.
address: The control table address to write to.
size: The size of the control table value being written to.
"""
self.check_connected()
key = (address, size)
if key not in self._sync_writers:
self._sync_writers[key] = self.dxl.GroupSyncWrite(
self.port_handler, self.packet_handler, address, size)
sync_writer = self._sync_writers[key]
errored_ids = []
for motor_id, desired_pos in zip(motor_ids, values):
value = signed_to_unsigned(int(desired_pos), size=size)
value = value.to_bytes(size, byteorder='little')
success = sync_writer.addParam(motor_id, value)
if not success:
errored_ids.append(motor_id)
if errored_ids:
logging.error('Sync write failed for: %s', str(errored_ids))
comm_result = sync_writer.txPacket()
self.handle_packet_result(comm_result, context='sync_write')
sync_writer.clearParam()
def check_connected(self):
"""Ensures the robot is connected."""
if self.lazy_connect and not self.is_connected:
self.connect()
if not self.is_connected:
raise OSError('Must call connect() first.')
def handle_packet_result(self,
comm_result: int,
dxl_error: Optional[int] = None,
dxl_id: Optional[int] = None,
context: Optional[str] = None):
"""Handles the result from a communication request."""
error_message = None
if comm_result != self.dxl.COMM_SUCCESS:
error_message = self.packet_handler.getTxRxResult(comm_result)
elif dxl_error is not None:
error_message = self.packet_handler.getRxPacketError(dxl_error)
if error_message:
if dxl_id is not None:
error_message = '[Motor ID: {}] {}'.format(
dxl_id, error_message)
if context is not None:
error_message = '> {}: {}'.format(context, error_message)
logging.error(error_message)
return False
return True
def convert_to_unsigned(self, value: int, size: int) -> int:
"""Converts the given value to its unsigned representation."""
if value < 0:
max_value = (1 << (8 * size)) - 1
value = max_value + value
return value
def __enter__(self):
"""Enables use as a context manager."""
if not self.is_connected:
self.connect()
return self
def __exit__(self, *args):
"""Enables use as a context manager."""
self.disconnect()
def __del__(self):
"""Automatically disconnect on destruction."""
self.disconnect()
class DynamixelReader:
"""Reads data from Dynamixel motors.
This wraps a GroupBulkRead from the DynamixelSDK.
"""
def __init__(self, client: DynamixelClient, motor_ids: Sequence[int],
address: int, size: int):
"""Initializes a new reader."""
self.client = client
self.motor_ids = motor_ids
self.address = address
self.size = size
self._initialize_data()
self.operation = self.client.dxl.GroupBulkRead(client.port_handler,
client.packet_handler)
for motor_id in motor_ids:
success = self.operation.addParam(motor_id, address, size)
if not success:
raise OSError(
'[Motor ID: {}] Could not add parameter to bulk read.'
.format(motor_id))
def read(self, retries: int = 1):
"""Reads data from the motors."""
self.client.check_connected()
success = False
while not success and retries >= 0:
comm_result = self.operation.txRxPacket()
success = self.client.handle_packet_result(
comm_result, context='read')
retries -= 1
# If we failed, send a copy of the previous data.
if not success:
return self._get_data()
errored_ids = []
for i, motor_id in enumerate(self.motor_ids):
# Check if the data is available.
available = self.operation.isAvailable(motor_id, self.address,
self.size)
if not available:
errored_ids.append(motor_id)
continue
self._update_data(i, motor_id)
if errored_ids:
logging.error('Bulk read data is unavailable for: %s',
str(errored_ids))
return self._get_data()
def _initialize_data(self):
"""Initializes the cached data."""
self._data = np.zeros(len(self.motor_ids), dtype=np.float32)
def _update_data(self, index: int, motor_id: int):
"""Updates the data index for the given motor ID."""
self._data[index] = self.operation.getData(motor_id, self.address,
self.size)
def _get_data(self):
"""Returns a copy of the data."""
return self._data.copy()
class DynamixelPosVelCurReader(DynamixelReader):
"""Reads positions and velocities."""
def __init__(self,
client: DynamixelClient,
motor_ids: Sequence[int],
pos_scale: float = 1.0,
vel_scale: float = 1.0,
cur_scale: float = 1.0):
super().__init__(
client,
motor_ids,
address=ADDR_PRESENT_POS_VEL_CUR,
size=LEN_PRESENT_POS_VEL_CUR,
)
self.pos_scale = pos_scale
self.vel_scale = vel_scale
self.cur_scale = cur_scale
def _initialize_data(self):
"""Initializes the cached data."""
self._pos_data = np.zeros(len(self.motor_ids), dtype=np.float32)
self._vel_data = np.zeros(len(self.motor_ids), dtype=np.float32)
self._cur_data = np.zeros(len(self.motor_ids), dtype=np.float32)
def _update_data(self, index: int, motor_id: int):
"""Updates the data index for the given motor ID."""
cur = self.operation.getData(motor_id, ADDR_PRESENT_CURRENT,
LEN_PRESENT_CURRENT)
vel = self.operation.getData(motor_id, ADDR_PRESENT_VELOCITY,
LEN_PRESENT_VELOCITY)
pos = self.operation.getData(motor_id, ADDR_PRESENT_POSITION,
LEN_PRESENT_POSITION)
cur = unsigned_to_signed(cur, size=2)
vel = unsigned_to_signed(vel, size=4)
pos = unsigned_to_signed(pos, size=4)
self._pos_data[index] = float(pos) * self.pos_scale
self._vel_data[index] = float(vel) * self.vel_scale
self._cur_data[index] = float(cur) * self.cur_scale
def _get_data(self):
"""Returns a copy of the data."""
return (self._pos_data.copy(), self._vel_data.copy(),
self._cur_data.copy())
class DynamixelPosReader(DynamixelReader):
"""Reads positions and velocities."""
def __init__(self,
client: DynamixelClient,
motor_ids: Sequence[int],
pos_scale: float = 1.0,
vel_scale: float = 1.0,
cur_scale: float = 1.0):
super().__init__(
client,
motor_ids,
address=ADDR_PRESENT_POS_VEL_CUR,
size=LEN_PRESENT_POS_VEL_CUR,
)
self.pos_scale = pos_scale
def _initialize_data(self):
"""Initializes the cached data."""
self._pos_data = np.zeros(len(self.motor_ids), dtype=np.float32)
def _update_data(self, index: int, motor_id: int):
"""Updates the data index for the given motor ID."""
pos = self.operation.getData(motor_id, ADDR_PRESENT_POSITION,
LEN_PRESENT_POSITION)
pos = unsigned_to_signed(pos, size=4)
self._pos_data[index] = float(pos) * self.pos_scale
def _get_data(self):
"""Returns a copy of the data."""
return self._pos_data.copy()
class DynamixelVelReader(DynamixelReader):
"""Reads positions and velocities."""
def __init__(self,
client: DynamixelClient,
motor_ids: Sequence[int],
pos_scale: float = 1.0,
vel_scale: float = 1.0,
cur_scale: float = 1.0):
super().__init__(
client,
motor_ids,
address=ADDR_PRESENT_POS_VEL_CUR,
size=LEN_PRESENT_POS_VEL_CUR,
)
self.pos_scale = pos_scale
self.vel_scale = vel_scale
self.cur_scale = cur_scale
def _initialize_data(self):
"""Initializes the cached data."""
self._vel_data = np.zeros(len(self.motor_ids), dtype=np.float32)
def _update_data(self, index: int, motor_id: int):
"""Updates the data index for the given motor ID."""
vel = self.operation.getData(motor_id, ADDR_PRESENT_VELOCITY,
LEN_PRESENT_VELOCITY)
vel = unsigned_to_signed(vel, size=4)
self._vel_data[index] = float(vel) * self.vel_scale
def _get_data(self):
"""Returns a copy of the data."""
return self._vel_data.copy()
class DynamixelCurReader(DynamixelReader):
"""Reads positions and velocities."""
def __init__(self,
client: DynamixelClient,
motor_ids: Sequence[int],
pos_scale: float = 1.0,
vel_scale: float = 1.0,
cur_scale: float = 1.0):
super().__init__(
client,
motor_ids,
address=ADDR_PRESENT_POS_VEL_CUR,
size=LEN_PRESENT_POS_VEL_CUR,
)
self.cur_scale = cur_scale
def _initialize_data(self):
"""Initializes the cached data."""
self._cur_data = np.zeros(len(self.motor_ids), dtype=np.float32)
def _update_data(self, index: int, motor_id: int):
"""Updates the data index for the given motor ID."""
cur = self.operation.getData(motor_id, ADDR_PRESENT_CURRENT,
LEN_PRESENT_CURRENT)
cur = unsigned_to_signed(cur, size=2)
self._cur_data[index] = float(cur) * self.cur_scale
def _get_data(self):
"""Returns a copy of the data."""
return self._cur_data.copy()
# Register global cleanup function.
atexit.register(dynamixel_cleanup_handler)
if __name__ == '__main__':
import argparse
import itertools
parser = argparse.ArgumentParser()
parser.add_argument(
'-m',
'--motors',
required=True,
help='Comma-separated list of motor IDs.')
parser.add_argument(
'-d',
'--device',
default='/dev/ttyUSB0',
help='The Dynamixel device to connect to.')
parser.add_argument(
'-b', '--baud', default=1000000, help='The baudrate to connect with.')
parsed_args = parser.parse_args()
motors = [int(motor) for motor in parsed_args.motors.split(',')]
way_points = [np.zeros(len(motors)), np.full(len(motors), np.pi)]
with DynamixelClient(motors, parsed_args.device,
parsed_args.baud) as dxl_client:
for step in itertools.count():
if step > 0 and step % 50 == 0:
way_point = way_points[(step // 100) % len(way_points)]
print('Writing: {}'.format(way_point.tolist()))
dxl_client.write_desired_pos(motors, way_point)
read_start = time.time()
pos_now, vel_now, cur_now = dxl_client.read_pos_vel_cur()
if step % 5 == 0:
print('[{}] Frequency: {:.2f} Hz'.format(
step, 1.0 / (time.time() - read_start)))
print('> Pos: {}'.format(pos_now.tolist()))
print('> Vel: {}'.format(vel_now.tolist()))
print('> Cur: {}'.format(cur_now.tolist()))

查看文件

@ -8,15 +8,18 @@ from copy import deepcopy
from itertools import repeat
from tqdm import tqdm
from einops import rearrange
from collections import deque
import wandb
import time
import ray
from ray.util.queue import Queue
from torchvision import transforms
from constants import DT
from constants import FPS
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, calibrate_linear_vel, postprocess_base_action # helper functions
from policy import ACTPolicy, CNNMLPPolicy
from policy import ACTPolicy, CNNMLPPolicy, DiffusionPolicy
from visualize_episodes import save_videos
from detr.models.latent_model import Latent_Model_Transformer
@ -26,6 +29,15 @@ from sim_env import BOX_POSE
import IPython
e = IPython.embed
ray.init()
def get_auto_index(dataset_dir):
max_idx = 1000
for i in range(max_idx+1):
if not os.path.isfile(os.path.join(dataset_dir, f'qpos_{i}.npy')):
return i
raise Exception(f"Error getting auto index, or more than {max_idx} episodes")
def main(args):
set_seed(1)
# command line parameters
@ -41,7 +53,6 @@ def main(args):
validate_every = args['validate_every']
save_every = args['save_every']
resume_ckpt_path = args['resume_ckpt_path']
history = args['history']
# get task parameters
is_sim = task_name[:4] == 'sim_'
@ -52,9 +63,12 @@ def main(args):
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']
# num_episodes = task_config['num_episodes']
episode_len = task_config['episode_len']
camera_names = task_config['camera_names']
stats_dir = task_config.get('stats_dir', None)
sample_weights = task_config.get('sample_weights', None)
train_ratio = task_config.get('train_ratio', 0.99)
name_filter = task_config.get('name_filter', lambda n: True)
# fixed parameters
@ -81,7 +95,19 @@ def main(args):
'vq_dim': args['vq_dim'],
'action_dim': 16,
'no_encoder': args['no_encoder'],
'history': history,
}
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,
@ -89,6 +115,13 @@ def main(args):
else:
raise NotImplementedError
actuator_config = {
'actuator_network_dir': args['actuator_network_dir'],
'history_len': args['history_len'],
'future_len': args['future_len'],
'prediction_len': args['prediction_len'],
}
config = {
'num_steps': num_steps,
'eval_every': eval_every,
@ -108,7 +141,7 @@ def main(args):
'camera_names': camera_names,
'real_robot': not is_sim,
'load_pretrain': args['load_pretrain'],
'history': history,
'actuator_config': actuator_config,
}
if not os.path.isdir(ckpt_dir):
@ -116,7 +149,7 @@ def main(args):
config_path = os.path.join(ckpt_dir, 'config.pkl')
expr_name = ckpt_dir.split('/')[-1]
if not is_eval:
wandb.init(project="mobile-aloha", reinit=True, entity="mobile-aloha", name=expr_name)
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)
@ -124,7 +157,7 @@ def main(args):
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, num_rollouts=1)
success_rate, avg_return = eval_bc(config, ckpt_name, save_episode=True, num_rollouts=10)
# wandb.log({'success_rate': success_rate, 'avg_return': avg_return})
results.append([ckpt_name, success_rate, avg_return])
@ -133,7 +166,7 @@ def main(args):
print()
exit()
train_dataloader, val_dataloader, stats, _ = load_data(dataset_dir, name_filter, camera_names, batch_size_train, batch_size_val, args['chunk_size'], history, args['skip_mirrored_data'], config['load_pretrain'])
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, stats_dir_l=stats_dir, sample_weights=sample_weights, train_ratio=train_ratio)
# save dataset stats
stats_path = os.path.join(ckpt_dir, f'dataset_stats.pkl')
@ -155,6 +188,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
@ -165,21 +200,51 @@ 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
def get_image(ts, camera_names):
def get_image(ts, camera_names, rand_crop_resize=False):
curr_images = []
for cam_name in camera_names:
curr_image = rearrange(ts.observation['images'][cam_name], 'h w c -> c h w')
curr_images.append(curr_image)
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)
if rand_crop_resize:
print('rand crop resize is used!')
original_size = curr_image.shape[-2:]
ratio = 0.95
curr_image = curr_image[..., int(original_size[0] * (1 - ratio) / 2): int(original_size[0] * (1 + ratio) / 2),
int(original_size[1] * (1 - ratio) / 2): int(original_size[1] * (1 + ratio) / 2)]
curr_image = curr_image.squeeze(0)
resize_transform = transforms.Resize(original_size, antialias=True)
curr_image = resize_transform(curr_image)
curr_image = curr_image.unsqueeze(0)
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):
set_seed(1000)
ckpt_dir = config['ckpt_dir']
@ -192,15 +257,17 @@ def eval_bc(config, ckpt_name, save_episode=True, num_rollouts=50):
max_timesteps = config['episode_len']
task_name = config['task_name']
temporal_agg = config['temporal_agg']
history = config['history']
onscreen_cam = 'angle'
vq = config['policy_config']['vq']
has_history = history >= 1
actuator_config = config['actuator_config']
use_actuator_net = actuator_config['actuator_network_dir'] is not None
async_query = 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()
@ -209,7 +276,7 @@ def eval_bc(config, ckpt_name, save_episode=True, num_rollouts=50):
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}')
@ -218,9 +285,46 @@ def eval_bc(config, ckpt_name, save_episode=True, num_rollouts=50):
stats_path = os.path.join(ckpt_dir, f'dataset_stats.pkl')
with open(stats_path, 'rb') as f:
stats = pickle.load(f)
# if use_actuator_net:
# prediction_len = actuator_config['prediction_len']
# future_len = actuator_config['future_len']
# history_len = actuator_config['history_len']
# actuator_network_dir = actuator_config['actuator_network_dir']
# from act.train_actuator_network import ActuatorNetwork
# actuator_network = ActuatorNetwork(prediction_len)
# 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))
# actuator_network.eval()
# actuator_network.cuda()
# print(f'Loaded actuator network from: {actuator_network_path}, {loading_status}')
# actuator_stats_path = os.path.join(actuator_network_dir, 'actuator_net_stats.pkl')
# with open(actuator_stats_path, 'rb') as f:
# actuator_stats = pickle.load(f)
# 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']
# def collect_base_action(all_actions, norm_episode_all_base_actions):
# post_processed_actions = post_process(all_actions.squeeze(0).cpu().numpy())
# 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']
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']
# 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
if real_robot:
@ -233,16 +337,21 @@ def eval_bc(config, ckpt_name, save_episode=True, num_rollouts=50):
env = make_sim_env(task_name)
env_max_reward = env.task.max_reward
query_frequency = policy_config['num_queries']
query_frequency = num_queries = policy_config['num_queries']
if temporal_agg:
query_frequency = 1
num_queries = policy_config['num_queries']
if real_robot:
BASE_DELAY = 13
query_frequency -= BASE_DELAY
max_timesteps = int(max_timesteps * 1) # may increase for real-world tasks
episode_returns = []
highest_rewards = []
for rollout_id in range(num_rollouts):
if real_robot:
e()
rollout_id += 0
### set task
if 'sim_transfer_cube' in task_name:
@ -259,18 +368,23 @@ def eval_bc(config, ckpt_name, save_episode=True, num_rollouts=50):
plt.ion()
### evaluation loop
if temporal_agg:
all_time_actions = torch.zeros([max_timesteps, max_timesteps+num_queries, 16]).cuda()
if temporal_agg or async_query:
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))
image_list = [] # for visualization
qpos_list = []
target_qpos_list = []
rewards = []
if has_history:
image_obs_history = deque(maxlen=history+1)
# if use_actuator_net:
# norm_episode_all_base_actions = [actuator_norm(np.zeros(history_len, 2)).tolist()]
with torch.inference_mode():
time0 = time.time()
DT = 1 / FPS
culmulated_delay = 0
for t in range(max_timesteps):
time1 = time.time()
### update onscreen render and wait for DT
if onscreen_render:
image = env._physics.render(height=480, width=640, camera_id=onscreen_cam)
@ -278,21 +392,45 @@ def eval_bc(config, ckpt_name, save_episode=True, num_rollouts=50):
plt.pause(DT)
### process previous timestep to get qpos and image_list
time2 = time.time()
obs = ts.observation
if 'images' in obs:
image_list.append(obs['images'])
else:
image_list.append({'main': obs['image']})
qpos_numpy = np.array(obs['qpos'])
qpos_history_raw[t] = qpos_numpy
qpos = pre_process(qpos_numpy)
qpos = torch.from_numpy(qpos).float().cuda().unsqueeze(0)
qpos_history[:, t] = qpos
curr_image = get_image(ts, camera_names)
qpos = torch.from_numpy(qpos).float().unsqueeze(0)
# qpos_history[:, t] = qpos
if t % query_frequency == 0:
curr_image = get_image(ts, camera_names, rand_crop_resize=(config['policy_class'] == 'Diffusion'))
# print('get image: ', time.time() - time2)
if not async_query:
qpos = qpos.cuda()
curr_image = curr_image.cuda()
if has_history:
image_obs_history.append(curr_image)
if t == 0:
print('Warming up')
if async_query:
num_warmup_steps = 10
for _ in tqdm(range(num_warmup_steps)):
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
time3 = time.time()
if config['policy_class'] == "ACT":
if t % query_frequency == 0:
if vq:
@ -303,10 +441,12 @@ def eval_bc(config, ckpt_name, save_episode=True, num_rollouts=50):
vq_sample = latent_model.generate(1, temperature=1, x=None)
all_actions = policy(qpos, curr_image, vq_sample=vq_sample)
else:
if has_history:
history_image = image_obs_history[0]
curr_image = torch.cat([curr_image, history_image], dim=2)
# e()
all_actions = policy(qpos, curr_image)
# 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)
if temporal_agg:
all_time_actions[[t], t:t+num_queries] = all_actions
actions_for_curr_step = all_time_actions[:, t]
@ -315,38 +455,126 @@ def eval_bc(config, ckpt_name, save_episode=True, num_rollouts=50):
k = 0.01
exp_weights = np.exp(-k * np.arange(len(actions_for_curr_step)))
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)
else:
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":
if async_query:
freq_mult = 0.8
if t == 0 or (t % int(query_frequency*freq_mult) == 0):
print(f'{t}: Put')
input_queue.put({'qpos': qpos, 'obs': curr_image, 'query_wall_t': time.time(), 'query_t': t})
# 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']
if real_robot:
all_actions = torch.cat([all_actions[:, :-BASE_DELAY, :-2], all_actions[:, BASE_DELAY:, -2:]], dim=2)
all_time_actions[query_t, query_t:query_t+len(all_actions)] = 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":
raw_action = policy(qpos, curr_image)
all_actions = raw_action.unsqueeze(0)
# if use_actuator_net:
# collect_base_action(all_actions, norm_episode_all_base_actions)
else:
raise NotImplementedError
# print('query policy: ', time.time() - time3)
### post-process actions
time4 = time.time()
raw_action = raw_action.squeeze(0).cpu().numpy()
action = post_process(raw_action)
target_qpos = action[:-2]
# if use_actuator_net:
# assert(not temporal_agg)
# if t % prediction_len == 0:
# 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 = torch.from_numpy(actuator_net_in).float().unsqueeze(dim=0).cuda()
# pred = actuator_network(actuator_net_in)
# base_action_chunk = actuator_unnorm(pred.detach().cpu().numpy()[0])
# base_action = base_action_chunk[t % prediction_len]
# else:
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)
# print('post process: ', time.time() - time4)
### step the environment
time5 = time.time()
if real_robot:
ts = env.step(target_qpos, base_action)
else:
ts = env.step(target_qpos)
# print('step env: ', time.time() - time5)
### for visualization
qpos_list.append(qpos_numpy)
target_qpos_list.append(target_qpos)
rewards.append(ts.reward)
duration = time.time() - time1
sleep_time = max(0, DT - duration)
# print(sleep_time)
time.sleep(sleep_time)
# time.sleep(max(0, DT - duration - culmulated_delay))
if duration >= DT and real_robot:
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()
if real_robot:
move_grippers([env.puppet_bot_left, env.puppet_bot_right], [PUPPET_GRIPPER_JOINT_OPEN] * 2, move_time=0.5) # open
pass
# save qpos_history_raw
log_id = get_auto_index(ckpt_dir)
np.save(os.path.join(ckpt_dir, f'qpos_{log_id}.npy'), qpos_history_raw)
plt.figure(figsize=(10, 20))
# plot qpos_history_raw for each qpos dim using subplots
for i in range(state_dim):
plt.subplot(state_dim, 1, i+1)
plt.plot(qpos_history_raw[:, i])
# remove x axis
if i != state_dim - 1:
plt.xticks([])
plt.tight_layout()
plt.savefig(os.path.join(ckpt_dir, f'qpos_{log_id}.png'))
plt.close()
rewards = np.array(rewards)
episode_return = np.sum(rewards[rewards!=None])
@ -355,8 +583,8 @@ def eval_bc(config, ckpt_name, save_episode=True, num_rollouts=50):
highest_rewards.append(episode_highest_reward)
print(f'Rollout {rollout_id}\n{episode_return=}, {episode_highest_reward=}, {env_max_reward=}, Success: {episode_highest_reward==env_max_reward}')
if save_episode:
save_videos(image_list, DT, video_path=os.path.join(ckpt_dir, f'video{rollout_id}.mp4'))
# if save_episode:
# save_videos(image_list, DT, video_path=os.path.join(ckpt_dir, f'video{rollout_id}.mp4'))
success_rate = np.mean(np.array(highest_rewards) == env_max_reward)
avg_return = np.mean(episode_returns)
@ -399,10 +627,10 @@ def train_bc(train_dataloader, val_dataloader, config):
policy = make_policy(policy_class, policy_config)
if config['load_pretrain']:
loading_status = policy.load_state_dict(torch.load(os.path.join('/home/zfu/interbotix_ws/src/act/ckpts/pretrain_all', 'policy_step_50000_seed_0.ckpt')))
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.load_state_dict(torch.load(config['resume_ckpt_path']))
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)
@ -422,14 +650,16 @@ def train_bc(train_dataloader, val_dataloader, config):
for batch_idx, data in enumerate(val_dataloader):
forward_dict = forward_pass(data, policy)
validation_dicts.append(forward_dict)
if batch_idx > 50:
break
validation_summary = compute_dict_mean(validation_dicts)
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}')
@ -443,7 +673,7 @@ def train_bc(train_dataloader, val_dataloader, config):
# 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.state_dict(), ckpt_path)
torch.save(policy.serialize(), ckpt_path)
success, _ = eval_bc(config, ckpt_name, save_episode=True, num_rollouts=10)
wandb.log({'success': success}, step=step)
@ -460,10 +690,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')
@ -498,6 +728,10 @@ if __name__ == '__main__':
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')
parser.add_argument('--actuator_network_dir', action='store', type=str, help='actuator_network_dir', required=False)
parser.add_argument('--history_len', action='store', type=int)
parser.add_argument('--future_len', action='store', type=int)
parser.add_argument('--prediction_len', action='store', type=int)
# for ACT
parser.add_argument('--kl_weight', action='store', type=int, help='KL Weight', required=False)
@ -509,6 +743,5 @@ if __name__ == '__main__':
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')
parser.add_argument('--history', action='store', type=int, required=False, default=0) # default no history
main(vars(parser.parse_args()))

205
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 = False
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__()
@ -19,15 +207,8 @@ class ACTPolicy(nn.Module):
def __call__(self, qpos, image, actions=None, is_pad=None, vq_sample=None):
env_state = None
if image.shape[2] == 6:
normalize = transforms.Normalize(mean=[0.485, 0.456, 0.406] * 2,
std=[0.229, 0.224, 0.225] * 2)
else:
assert image.shape[2] == 3
normalize = transforms.Normalize(mean=[0.485, 0.456, 0.406],
std=[0.229, 0.224, 0.225])
normalize = transforms.Normalize(mean=[0.485, 0.456, 0.406],
std=[0.229, 0.224, 0.225])
image = normalize(image)
if actions is not None: # training time
actions = actions[:, :self.model.num_queries]
@ -63,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):

查看文件

@ -26,6 +26,7 @@ def load_hdf5(dataset_dir, dataset_name):
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'][()]
@ -37,17 +38,30 @@ def load_hdf5(dataset_dir, dataset_name):
base_action = root['/base_action'][()]
else:
base_action = None
if compressed:
compress_len = root['/compress_len'][()]
return qpos, qvel, action, base_action, image_dict
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']
for episode_idx in range(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 = load_hdf5(dataset_dir, dataset_name)
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)
@ -59,7 +73,7 @@ def main(args):
# 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':
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')
@ -86,18 +100,54 @@ def main(args):
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'] = True
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():
_ = image.create_dataset(cam_name, (max_timesteps, 480, 640, 3), dtype='uint8',
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), )
# compression='gzip',compression_opts=2,)
# compression=32001, compression_opts=(0, 0, 0, 0, 9, 1, 1), shuffle=False)
qpos = obs.create_dataset('qpos', (max_timesteps, 14))
qvel = obs.create_dataset('qvel', (max_timesteps, 14))
action = root.create_dataset('action', (max_timesteps, 14))
@ -106,11 +156,17 @@ def main(args):
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')
# 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 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()

10
setup.py 普通文件
查看文件

@ -0,0 +1,10 @@
from distutils.core import setup
from setuptools import find_packages
setup(
name='act',
version='0.0.0',
packages=find_packages(),
license='MIT License',
long_description=open('README.md').read(),
)

367
train_actuator_network.py 普通文件
查看文件

@ -0,0 +1,367 @@
import numpy as np
import torch
from torch import nn
from torch.nn import functional as F
from torch.utils.data import DataLoader
import os
import h5py
import math
import wandb
import pickle
import matplotlib.pyplot as plt
from copy import deepcopy
from tqdm import tqdm
from utils import find_all_hdf5
from imitate_episodes import repeater, compute_dict_mean
import IPython
e = IPython.embed
def main():
### Idea
# input : o o o o o o # observed speed
# target: a a a a a a # commanded speed
# at test time, input desired speed profile and convert that to command
#########################################################
history_len = 50
future_len = 50
prediction_len = 50
batch_size_train = 16
batch_size_val = 16
lr = 1e-4
weight_decay = 1e-4
num_steps = 10000
validate_every = 2000
save_every = 2000
expr_name = f'actuator_network_test_{history_len}_{future_len}_{prediction_len}'
ckpt_dir = f'/scr/tonyzhao/train_logs/{expr_name}' if os.getlogin() == 'tonyzhao' else f'./ckpts/{expr_name}'
dataset_dir = '/scr/tonyzhao/compressed_datasets/aloha_mobile_fork/' if os.getlogin() == 'tonyzhao' else '/home/zfu/data/aloha_mobile_fork/'
#########################################################
assert(history_len + future_len >= prediction_len)
assert(future_len % prediction_len == 0)
wandb.init(project="mobile-aloha2", reinit=True, entity="mobile-aloha2", name=expr_name) # mode='disabled',
if not os.path.isdir(ckpt_dir):
os.makedirs(ckpt_dir)
dataset_path_list = find_all_hdf5(dataset_dir, skip_mirrored_data=True)
dataset_path_list = [n for n in dataset_path_list if 'replayed' in n]
num_episodes = len(dataset_path_list)
# obtain train test split
train_ratio = 0.9
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]
assert(all_episode_len[0] % prediction_len == 0)
# save dataset stats
stats_path = os.path.join(ckpt_dir, f'actuator_net_stats.pkl')
with open(stats_path, 'wb') as f:
pickle.dump(norm_stats, f)
# construct dataset and dataloader
train_dataset = EpisodicDataset(dataset_path_list, norm_stats, train_episode_ids, train_episode_len, history_len, future_len, prediction_len)
val_dataset = EpisodicDataset(dataset_path_list, norm_stats, val_episode_ids, val_episode_len, history_len, future_len, prediction_len)
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)
policy = ActuatorNetwork(prediction_len).cuda()
optimizer = torch.optim.AdamW(policy.parameters(), lr=lr, weight_decay=weight_decay)
n_parameters = sum(p.numel() for p in policy.parameters() if p.requires_grad)
print("number of parameters: %.2fM" % (n_parameters/1e6,))
min_val_loss = np.inf
best_ckpt_info = None
train_dataloader = repeater(train_dataloader)
for step in tqdm(range(num_steps+1)):
# validation
if step % validate_every == 0:
print('validating')
with torch.inference_mode():
policy.eval()
validation_dicts = []
for batch_idx, data in enumerate(val_dataloader):
observed_speed, commanded_speed = data
out, forward_dict = policy(observed_speed.cuda(), commanded_speed.cuda())
validation_dicts.append(forward_dict)
validation_summary = compute_dict_mean(validation_dicts)
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 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}')
summary_string = ''
for k, v in validation_summary.items():
summary_string += f'{k}: {v.item():.3f} '
print(summary_string)
visualize_prediction(dataset_path_list, val_episode_ids, policy, norm_stats, history_len, future_len, prediction_len, ckpt_dir, step, 'val')
visualize_prediction(dataset_path_list, train_episode_ids, policy, norm_stats, history_len, future_len, prediction_len, ckpt_dir, step, 'train')
# training
policy.train()
optimizer.zero_grad()
data = next(train_dataloader)
observed_speed, commanded_speed = data
out, forward_dict = policy(observed_speed.cuda(), commanded_speed.cuda())
# backward
loss = forward_dict['loss']
loss.backward()
optimizer.step()
wandb.log(forward_dict, step=step) # not great, make training 1-2% slower
if step % save_every == 0:
ckpt_path = os.path.join(ckpt_dir, f'actuator_net_step_{step}.ckpt')
torch.save(policy.state_dict(), ckpt_path)
ckpt_path = os.path.join(ckpt_dir, f'actuator_net_last.ckpt')
torch.save(policy.state_dict(), ckpt_path)
best_step, min_val_loss, best_state_dict = best_ckpt_info
ckpt_path = os.path.join(ckpt_dir, f'actuator_net_step_{best_step}.ckpt')
torch.save(best_state_dict, ckpt_path)
print(f'Training finished:\nval loss {min_val_loss:.6f} at step {best_step}')
def visualize_prediction(dataset_path_list, episode_ids, policy, norm_stats, history_len, future_len, prediction_len, ckpt_dir, step, name):
num_vis = 2
episode_ids = episode_ids[:num_vis]
vis_path = [dataset_path_list[i] for i in episode_ids]
for i, dataset_path in enumerate(vis_path):
try:
with h5py.File(dataset_path, 'r') as root:
commanded_speed = root['/base_action'][()]
observed_speed = root['/obs_tracer'][()]
except Exception as ee:
print(f'Error loading {dataset_path} in get_norm_stats')
print(ee)
quit()
# commanded_speed = (commanded_speed - norm_stats["commanded_speed_mean"]) / norm_stats["commanded_speed_std"]
norm_observed_speed = (observed_speed - norm_stats["observed_speed_mean"]) / norm_stats["observed_speed_std"]
out_unnorm_fn = lambda x: (x * norm_stats["commanded_speed_std"]) + norm_stats["commanded_speed_mean"]
history_pad = np.zeros((history_len, 2))
future_pad = np.zeros((future_len, 2))
norm_observed_speed = np.concatenate([history_pad, norm_observed_speed, future_pad], axis=0)
episode_len = commanded_speed.shape[0]
all_pred = []
for t in range(0, episode_len, prediction_len):
offset_start_ts = t + history_len
policy_input = norm_observed_speed[offset_start_ts-history_len: offset_start_ts+future_len]
policy_input = torch.from_numpy(policy_input).float().unsqueeze(dim=0).cuda()
pred = policy(policy_input)
pred = pred.detach().cpu().numpy()[0]
all_pred += out_unnorm_fn(pred).tolist()
all_pred = np.array(all_pred)
plot_path = os.path.join(ckpt_dir, f'{name}{i}_step{step}_linear')
plt.figure()
plt.plot(commanded_speed[:, 0], label='commanded_speed_linear')
plt.plot(observed_speed[:, 0], label='observed_speed_linear')
plt.plot(all_pred[:, 0], label='pred_commanded_speed_linear')
# plot vertical grey dotted lines every prediction_len
for t in range(0, episode_len, prediction_len):
plt.axvline(t, linestyle='--', color='grey')
plt.legend()
plt.savefig(plot_path)
plt.close()
plot_path = os.path.join(ckpt_dir, f'{name}{i}_step{step}_angular')
plt.figure()
plt.plot(commanded_speed[:, 1], label='commanded_speed_angular')
plt.plot(observed_speed[:, 1], label='observed_speed_angular')
plt.plot(all_pred[:, 1], label='pred_commanded_speed_angular')
# plot vertical dotted lines every prediction_len
for t in range(0, episode_len, prediction_len):
plt.axvline(t, linestyle='--', color='grey')
plt.legend()
plt.savefig(plot_path)
plt.close()
class ActuatorNetwork(nn.Module):
def __init__(self, prediction_len):
super().__init__()
d_model = 256
encoder_layer = nn.TransformerEncoderLayer(d_model=d_model, nhead=8)
self.transformer = nn.TransformerEncoder(encoder_layer, num_layers=3)
self.pe = PositionalEncoding(d_model)
self.in_proj = nn.Linear(2, d_model)
self.out_proj = nn.Linear(d_model, 2)
self.prediction_len = prediction_len
def forward(self, src, tgt=None):
if tgt is not None: # training time
# (batch, seq, feature) -> (seq, batch, feature)
src = self.in_proj(src)
src = torch.einsum('b s d -> s b d', src)
src = self.pe(src)
out = self.transformer(src)
tgt = torch.einsum('b s d -> s b d', tgt)
assert(self.prediction_len == tgt.shape[0])
out = out[0: self.prediction_len] # take first few tokens only for prediction
out = self.out_proj(out)
l2_loss = loss = F.mse_loss(out, tgt)
loss_dict = {'loss': l2_loss}
out = torch.einsum('s b d -> b s d', out)
return out, loss_dict
else:
src = self.in_proj(src)
src = torch.einsum('b s d -> s b d', src)
src = self.pe(src)
out = self.transformer(src)
out = out[0: self.prediction_len] # take first few tokens only for prediction
out = self.out_proj(out)
out = torch.einsum('s b d -> b s d', out)
return out
class PositionalEncoding(nn.Module):
def __init__(self, d_model: int, dropout: float = 0.1, max_len: int = 5000):
super().__init__()
self.dropout = nn.Dropout(p=dropout)
position = torch.arange(max_len).unsqueeze(1)
div_term = torch.exp(torch.arange(0, d_model, 2) * (-math.log(10000.0) / d_model))
pe = torch.zeros(max_len, 1, d_model)
pe[:, 0, 0::2] = torch.sin(position * div_term)
pe[:, 0, 1::2] = torch.cos(position * div_term)
self.register_buffer('pe', pe)
def forward(self, x):
"""
Arguments:
x: Tensor, shape ``[seq_len, batch_size, embedding_dim]``
"""
x = x + self.pe[:x.size(0)]
return self.dropout(x)
def get_norm_stats(dataset_path_list):
all_commanded_speed = []
all_observed_speed = []
all_episode_len = []
for dataset_path in dataset_path_list:
try:
with h5py.File(dataset_path, 'r') as root:
commanded_speed = root['/base_action'][()]
observed_speed = root['/obs_tracer'][()]
except Exception as e:
print(f'Error loading {dataset_path} in get_norm_stats')
print(e)
quit()
all_commanded_speed.append(torch.from_numpy(commanded_speed))
all_observed_speed.append(torch.from_numpy(observed_speed))
all_episode_len.append(len(commanded_speed))
all_commanded_speed = torch.cat(all_commanded_speed, dim=0)
all_observed_speed = torch.cat(all_observed_speed, dim=0)
# normalize all_commanded_speed
commanded_speed_mean = all_commanded_speed.mean(dim=[0]).float()
commanded_speed_std = all_commanded_speed.std(dim=[0]).float()
commanded_speed_std = torch.clip(commanded_speed_std, 1e-2, np.inf) # clipping
# normalize all_observed_speed
observed_speed_mean = all_observed_speed.mean(dim=[0]).float()
observed_speed_std = all_observed_speed.std(dim=[0]).float()
observed_speed_std = torch.clip(observed_speed_std, 1e-2, np.inf) # clipping
stats = {"commanded_speed_mean": commanded_speed_mean.numpy(), "commanded_speed_std": commanded_speed_std.numpy(),
"observed_speed_mean": observed_speed_mean.numpy(), "observed_speed_std": observed_speed_std.numpy()}
return stats, all_episode_len
class EpisodicDataset(torch.utils.data.Dataset):
def __init__(self, dataset_path_list, norm_stats, episode_ids, episode_len, history_len, future_len, prediction_len):
super(EpisodicDataset).__init__()
self.episode_ids = episode_ids
self.dataset_path_list = dataset_path_list
self.norm_stats = norm_stats
self.episode_len = episode_len
self.cumulative_len = np.cumsum(self.episode_len)
self.max_episode_len = max(episode_len)
self.history_len = history_len
self.future_len = future_len
self.prediction_len = prediction_len
self.is_sim = False
self.history_pad = np.zeros((self.history_len, 2))
self.future_pad = np.zeros((self.future_len, 2))
self.prediction_pad = np.zeros((self.prediction_len, 2))
self.__getitem__(0) # initialize self.is_sim
def __len__(self):
return sum(self.episode_len)
def _locate_transition(self, index):
assert index < self.cumulative_len[-1]
episode_index = np.argmax(self.cumulative_len > index) # argmax returns first True index
start_ts = index - (self.cumulative_len[episode_index] - self.episode_len[episode_index])
episode_id = self.episode_ids[episode_index]
return episode_id, start_ts
def __getitem__(self, index):
episode_id, start_ts = self._locate_transition(index)
dataset_path = self.dataset_path_list[episode_id]
try:
# print(dataset_path)
with h5py.File(dataset_path, 'r') as root:
commanded_speed = root['/base_action'][()]
observed_speed = root['/obs_tracer'][()]
observed_speed = np.concatenate([self.history_pad, observed_speed, self.future_pad], axis=0)
commanded_speed = np.concatenate([commanded_speed, self.prediction_pad], axis=0)
offset_start_ts = start_ts + self.history_len
commanded_speed = commanded_speed[start_ts: start_ts+self.prediction_len]
observed_speed = observed_speed[offset_start_ts-self.history_len: offset_start_ts+self.future_len]
commanded_speed = torch.from_numpy(commanded_speed).float()
observed_speed = torch.from_numpy(observed_speed).float()
# normalize to mean 0 std 1
commanded_speed = (commanded_speed - self.norm_stats["commanded_speed_mean"]) / self.norm_stats["commanded_speed_std"]
observed_speed = (observed_speed - self.norm_stats["observed_speed_mean"]) / self.norm_stats["observed_speed_std"]
except:
print(f'Error loading {dataset_path} in __getitem__')
quit()
# print(image_data.dtype, qpos_data.dtype, action_data.dtype, is_pad.dtype)
return observed_speed, commanded_speed
if __name__ == '__main__':
main()

158
truncate_data.py 普通文件
查看文件

@ -0,0 +1,158 @@
"""
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"]
TRUNCATE_LEN = 2250
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:
outfile.attrs['sim'] = infile.attrs['sim']
outfile.attrs['compress'] = True
# Copy non-image data directly
for key in infile.keys():
if key != 'observations' and key != 'compress_len':
data = infile[key][:TRUNCATE_LEN]
out_data = outfile.create_dataset(key, (TRUNCATE_LEN, data.shape[1]))
out_data[:] = data
data_compress_len = infile['compress_len']
out_data_compress_len = outfile.create_dataset('compress_len', data_compress_len.shape)
out_data_compress_len[:] = data_compress_len
# Create observation group in the output
obs_group = infile['observations']
out_obs_group = outfile.create_group('observations')
for key in obs_group.keys():
if key != 'images':
data = obs_group[key][:TRUNCATE_LEN]
out_data = out_obs_group.create_dataset(key, (TRUNCATE_LEN, data.shape[1]))
out_data[:] = data
image_group = obs_group['images']
out_image_group = out_obs_group.create_group('images')
for cam_name in image_group.keys():
data = image_group[cam_name][:TRUNCATE_LEN]
out_data = out_image_group.create_dataset(cam_name, (TRUNCATE_LEN, data.shape[1]), dtype='uint8')
out_data[:] = data
print(f"Truncated 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 + '_truncated'
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="Truncating 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(output_dataset_dir, video_path)

142
utils.py
查看文件

@ -4,13 +4,19 @@ import os
import h5py
import pickle
import fnmatch
import cv2
from time import time
from torch.utils.data import TensorDataset, DataLoader
import torchvision.transforms as transforms
import IPython
e = IPython.embed
def flatten_list(l):
return [item for sublist in l for item in sublist]
class EpisodicDataset(torch.utils.data.Dataset):
def __init__(self, dataset_path_list, camera_names, norm_stats, episode_ids, episode_len, chunk_size, history):
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
@ -18,14 +24,19 @@ class EpisodicDataset(torch.utils.data.Dataset):
self.norm_stats = norm_stats
self.episode_len = episode_len
self.chunk_size = chunk_size
self.history = history
self.cumulative_len = np.cumsum(self.episode_len)
self.max_episode_len = max(episode_len)
self.__getitem__(0) # initialize self.is_sim
self.policy_class = policy_class
if self.policy_class == 'Diffusion':
self.augment_images = True
else:
self.augment_images = False
self.transformations = None
self.__getitem__(0) # initialize self.is_sim and self.transformations
self.is_sim = False
def __len__(self):
return sum(self.episode_len)
# def __len__(self):
# return sum(self.episode_len)
def _locate_transition(self, index):
assert index < self.cumulative_len[-1]
@ -44,6 +55,7 @@ class EpisodicDataset(torch.utils.data.Dataset):
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)
@ -59,13 +71,13 @@ class EpisodicDataset(torch.utils.data.Dataset):
qvel = root['/observations/qvel'][start_ts]
image_dict = dict()
for cam_name in self.camera_names:
if self.history >= 1:
curr_frame = root[f'/observations/images/{cam_name}'][start_ts]
history_frame = root[f'/observations/images/{cam_name}'][max(0, start_ts - self.history)]
concat_image = np.concatenate([curr_frame, history_frame], axis=2)
image_dict[cam_name] = concat_image
else:
image_dict[cam_name] = root[f'/observations/images/{cam_name}'][start_ts]
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:]
@ -80,7 +92,6 @@ class EpisodicDataset(torch.utils.data.Dataset):
is_pad = np.zeros(self.max_episode_len)
is_pad[action_len:] = 1
padded_action = padded_action[:self.chunk_size]
padded_action = padded_action[:self.chunk_size]
# new axis for different cameras
@ -98,9 +109,32 @@ class EpisodicDataset(torch.utils.data.Dataset):
# channel last
image_data = torch.einsum('k h w c -> k c h w', image_data)
# augmentation
if self.transformations is None:
print('Initializing transformations')
original_size = image_data.shape[2:]
ratio = 0.95
self.transformations = [
transforms.RandomCrop(size=[int(original_size[0] * ratio), int(original_size[1] * ratio)]),
transforms.Resize(original_size, antialias=True),
transforms.RandomRotation(degrees=[-5.0, 5.0], expand=False),
transforms.ColorJitter(brightness=0.3, contrast=0.4, saturation=0.5) #, hue=0.08)
]
if self.augment_images:
for transform in self.transformations:
image_data = transform(image_data)
# 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:
@ -149,7 +183,12 @@ 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}
@ -159,39 +198,74 @@ 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 'features' in filename: continue
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 BatchSampler(batch_size, episode_len_l, sample_weights):
sample_probs = np.array(sample_weights) / np.sum(sample_weights) if sample_weights is not None else None
sum_dataset_len_l = np.cumsum([0] + [np.sum(episode_len) for episode_len in episode_len_l])
while True:
batch = []
for _ in range(batch_size):
episode_idx = np.random.choice(len(episode_len_l), p=sample_probs)
step_idx = np.random.randint(sum_dataset_len_l[episode_idx], sum_dataset_len_l[episode_idx + 1])
batch.append(step_idx)
yield batch
def load_data(dataset_dir, name_filter, camera_names, batch_size_train, batch_size_val, chunk_size, history, skip_mirrored_data=False, load_pretrain=False):
dataset_path_list = find_all_hdf5(dataset_dir, skip_mirrored_data)
def load_data(dataset_dir_l, name_filter, camera_names, batch_size_train, batch_size_val, chunk_size, skip_mirrored_data=False, load_pretrain=False, policy_class=None, stats_dir_l=None, sample_weights=None, train_ratio=0.99):
if type(dataset_dir_l) == str:
dataset_dir_l = [dataset_dir_l]
dataset_path_list_list = [find_all_hdf5(dataset_dir, skip_mirrored_data) for dataset_dir in dataset_dir_l]
num_episodes_0 = len(dataset_path_list_list[0])
dataset_path_list = flatten_list(dataset_path_list_list)
dataset_path_list = [n for n in dataset_path_list if name_filter(n)]
num_episodes = len(dataset_path_list)
num_episodes_l = [len(dataset_path_list) for dataset_path_list in dataset_path_list_list]
num_episodes_cumsum = np.cumsum(num_episodes_l)
# obtain train test split
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 train test split on dataset_dir_l[0]
shuffled_episode_ids_0 = np.random.permutation(num_episodes_0)
train_episode_ids_0 = shuffled_episode_ids_0[:int(train_ratio * num_episodes_0)]
val_episode_ids_0 = shuffled_episode_ids_0[int(train_ratio * num_episodes_0):]
train_episode_ids_l = [train_episode_ids_0] + [np.arange(num_episodes) + num_episodes_cumsum[idx] for idx, num_episodes in enumerate(num_episodes_l[1:])]
val_episode_ids_l = [val_episode_ids_0]
train_episode_ids = np.concatenate(train_episode_ids_l)
val_episode_ids = np.concatenate(val_episode_ids_l)
print(f'\n\nData from: {dataset_dir_l}\n- Train on {[len(x) for x in train_episode_ids_l]} episodes\n- Test on {[len(x) for x in val_episode_ids_l]} 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]
_, all_episode_len = get_norm_stats(dataset_path_list)
train_episode_len_l = [[all_episode_len[i] for i in train_episode_ids] for train_episode_ids in train_episode_ids_l]
val_episode_len_l = [[all_episode_len[i] for i in val_episode_ids] for val_episode_ids in val_episode_ids_l]
train_episode_len = flatten_list(train_episode_len_l)
val_episode_len = flatten_list(val_episode_len_l)
if stats_dir_l is None:
stats_dir_l = dataset_dir_l
elif type(stats_dir_l) == str:
stats_dir_l = [stats_dir_l]
norm_stats, _ = get_norm_stats(flatten_list([find_all_hdf5(stats_dir, skip_mirrored_data) for stats_dir in stats_dir_l]))
print(f'Norm stats from: {stats_dir_l}')
batch_sampler_train = BatchSampler(batch_size_train, train_episode_len_l, sample_weights)
batch_sampler_val = BatchSampler(batch_size_val, val_episode_len_l, None)
# print(f'train_episode_len: {train_episode_len}, val_episode_len: {val_episode_len}, train_episode_ids: {train_episode_ids}, val_episode_ids: {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, history)
val_dataset = EpisodicDataset(dataset_path_list, camera_names, norm_stats, val_episode_ids, val_episode_len, chunk_size, history)
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)
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_num_workers = (8 if os.getlogin() == 'zfu' else 16) if train_dataset.augment_images else 2
val_num_workers = 8 if train_dataset.augment_images else 2
print(f'Augment images: {train_dataset.augment_images}, train_num_workers: {train_num_workers}, val_num_workers: {val_num_workers}')
train_dataloader = DataLoader(train_dataset, batch_sampler=batch_sampler_train, pin_memory=True, num_workers=train_num_workers, prefetch_factor=2)
val_dataloader = DataLoader(val_dataset, batch_sampler=batch_sampler_val, pin_memory=True, num_workers=val_num_workers, prefetch_factor=2)
return train_dataloader, val_dataloader, norm_stats, train_dataset.is_sim
@ -215,9 +289,13 @@ def preprocess_base_action(base_action):
return base_action
def postprocess_base_action(self, base_action):
def postprocess_base_action(base_action):
linear_vel, angular_vel = base_action
angular_vel *= 0.9
linear_vel *= 1.0
angular_vel *= 1.0
# angular_vel = 0
# if np.abs(linear_vel) < 0.05:
# linear_vel = 0
return np.array([linear_vel, angular_vel])
### env utils

143
vinn_cache_feature.py 普通文件
查看文件

@ -0,0 +1,143 @@
import torch
import argparse
import pathlib
from torch import nn
import torchvision
import os
import time
import h5py
import h5py
from torchvision import models, transforms
from PIL import Image
from tqdm import tqdm
import cv2
import numpy as np
import IPython
e = IPython.embed
def chunks(lst, n):
"""Yield successive n-sized chunks from lst."""
for i in range(0, len(lst), n):
yield lst[i:i + n]
def expand_greyscale(t):
return t.expand(3, -1, -1)
def main(args):
#################################################
batch_size = 256
#################################################
ckpt_path = args.ckpt_path
dataset_dir = args.dataset_dir
ckpt_name = pathlib.PurePath(ckpt_path).name
dataset_name = ckpt_name.split('-')[1]
repr_type = ckpt_name.split('-')[0]
seed = int(ckpt_name.split('-')[-1][:-3])
episode_idxs = [int(name.split('_')[1].split('.')[0]) for name in os.listdir(dataset_dir) if ('.hdf5' in name) and ('features' not in name)]
episode_idxs.sort()
assert len(episode_idxs) == episode_idxs[-1] + 1 # no holes
num_episodes = len(episode_idxs)
feature_extractors = {}
for episode_idx in range(num_episodes):
# load all images
dataset_path = os.path.join(dataset_dir, f'episode_{episode_idx}.hdf5')
with h5py.File(dataset_path, 'r') as root:
image_dict = {}
camera_names = list(root[f'/observations/images/'].keys())
print(f'Camera names: {camera_names}')
for cam_name in camera_names:
image = root[f'/observations/images/{cam_name}'][:]
uncompressed_image = []
for im in image:
im = np.array(cv2.imdecode(im, 1))
uncompressed_image.append(im)
image = np.stack(uncompressed_image, axis=0)
image_dict[cam_name] = image
# load pretrain nets after cam names are known
if not feature_extractors:
for cam_name in camera_names:
resnet = torchvision.models.resnet18(pretrained=True)
loading_status = resnet.load_state_dict(torch.load(ckpt_path.replace('DUMMY', cam_name)))
print(cam_name, loading_status)
resnet = nn.Sequential(*list(resnet.children())[:-1])
resnet = resnet.cuda()
resnet.eval()
feature_extractors[cam_name] = resnet
# inference with resnet
feature_dict = {}
for cam_name, images in image_dict.items():
# Preprocess images
image_size = 120 # TODO NOTICE: reduced resolution
transform = transforms.Compose([
transforms.Resize(image_size), # will scale the image
transforms.CenterCrop(image_size),
transforms.ToTensor(),
transforms.Lambda(expand_greyscale),
transforms.Normalize(
mean=torch.tensor([0.485, 0.456, 0.406]),
std=torch.tensor([0.229, 0.224, 0.225])),
])
processed_images = []
for image in tqdm(images):
image = Image.fromarray(image)
image = transform(image)
processed_images.append(image)
processed_images = torch.stack(processed_images).cuda()
# query the model
all_features = []
with torch.inference_mode():
for batch in chunks(processed_images, batch_size):
print('inference')
features = feature_extractors[cam_name](batch)
features = features.squeeze(axis=3).squeeze(axis=2)
all_features.append(features)
all_features = torch.cat(all_features, axis=0)
max_timesteps = all_features.shape[0]
feature_dict[cam_name] = all_features
# TODO START diagnostics
# first_image = images[0]
# first_processed_image = processed_images[0].cpu().numpy()
# first_feature = all_features[0].cpu().numpy()
# import numpy as np
# np.save('first_image.npy', first_image)
# np.save('first_processed_image.npy', first_processed_image)
# np.save('first_feature.npy', first_feature)
# torch.save(resnet.state_dict(), 'rn.ckpt')
# e()
# exit()
# TODO END diagnostics
# save
dataset_path = os.path.join(dataset_dir, f'{repr_type}_features_seed{seed}_episode_{episode_idx}.hdf5')
print(dataset_path)
# HDF5
t0 = time.time()
with h5py.File(dataset_path, 'w', rdcc_nbytes=1024 ** 2 * 2) as root:
features = root.create_group('features')
for cam_name, array in feature_dict.items():
cam_feature = features.create_dataset(cam_name, (max_timesteps, 512))
features[cam_name][...] = array.cpu().numpy()
print(f'Saving: {time.time() - t0:.1f} secs\n')
if __name__ == '__main__':
parser = argparse.ArgumentParser(description='cache features')
parser.add_argument('--ckpt_path', type=str, required=True, help='ckpt_path')
parser.add_argument('--dataset_dir', type=str, required=True, help='dataset_dir')
args = parser.parse_args()
main(args)

331
vinn_eval.py 普通文件
查看文件

@ -0,0 +1,331 @@
import torch
from torch import nn
import torch.nn.functional as F
import numpy as np
import h5py
import pathlib
import os
import argparse
import matplotlib.pyplot as plt
from PIL import Image
import torchvision
from torchvision import transforms
# from visualize_episodes import visualize_joints
from utils import set_seed, sample_box_pose
# from imitate_episodes import get_image
from sim_env import BOX_POSE
from constants import DT
from imitate_episodes import save_videos
from einops import rearrange
import time
DT = 0.02
import IPython
e = IPython.embed
# modified from https://github.com/jyopari/VINN/blob/main/nearest-neighbor-eval/handle_nn.ipynb
def calculate_nearest_neighbors(curr_feature, support_inputs, support_targets, k, state_weight):
has_skip = len(support_targets.shape) == 3
if has_skip: # when there is action skip
num_targets, skip, a_dim = support_targets.shape
support_targets = support_targets.view((num_targets, -1))
curr_vis_feature, curr_s_feature = curr_feature
support_vis_feature, support_s_feature = support_inputs
pairwise_dist_vis = torch.norm(curr_vis_feature - support_vis_feature, dim=1).unsqueeze(0)
pairwise_dist_s = torch.norm(curr_s_feature - support_s_feature, dim=1).unsqueeze(0)
pairwise_dist = pairwise_dist_vis + pairwise_dist_s * state_weight
sorted_dist, index = torch.sort(pairwise_dist, dim=1) # sort the support axis
permuted_support_targets = support_targets[index]
topk_dist = pairwise_dist[:, :k]
topk_support_targets = permuted_support_targets[:, :k]
weights = F.softmax(-topk_dist, dim=1)
weighted_support_targets = weights.unsqueeze(2) * topk_support_targets
prediction = torch.sum(weighted_support_targets, dim=1)
if has_skip:
num_predictions = prediction.shape[0]
prediction = prediction.reshape((num_predictions, skip, a_dim))
return prediction
def main(args):
# TODO ######################
k = None # for scripted box transfer
skip = 100
real_robot = True
save_episode = True
# TODO ######################
onscreen_cam = 'main'
state_dim = 14
dataset_dir = args['dataset_dir']
onscreen_render = args['onscreen_render']
ckpt_dir = args['ckpt_dir']
model_dir = args['model_dir']
task_name = args['task_name']
if 'insertion' in task_name:
sim_episode_len = 400
env_max_reward = 4
ks = [None]
elif 'transfer_cube' in task_name:
sim_episode_len = 400
env_max_reward = 4
ks = [1, 1, 1]
if 'human' in dataset_dir:
state_weight = 5
else:
state_weight = 10
print(f'{state_weight=}')
elif task_name == 'ziploc_slide':
env_max_reward = 1
ks = [71]
state_weight = 0
elif task_name == 'aloha_mobile_wipe_wine':
sim_episode_len = 1300
env_max_reward = 4
ks = [2, 2, 2]
state_weight = 5
print(f'{state_weight=}')
else:
raise NotImplementedError
model_name = pathlib.PurePath(model_dir).name
seed = int(model_name.split('-')[-1][:-3])
k = ks[seed]
if real_robot:
BASE_DELAY = 15
query_freq = skip - BASE_DELAY
# load train data
vis_features = []
state_features = []
Y = []
for episode_id in range(0, 40):
dataset_path = os.path.join(dataset_dir, f'episode_{episode_id}.hdf5')
with h5py.File(dataset_path, 'r') as root:
action = root['/action'][:]
base_action = root['/base_action'][:]
action = np.concatenate([action, base_action], axis=1)
camera_names = list(root[f'/observations/images/'].keys())
# Visual feature
all_cam_feature = []
for cam_name in camera_names:
feature_dataset_path = os.path.join(dataset_dir, f'byol_features_seed{seed}_episode_{episode_id}.hdf5')
with h5py.File(feature_dataset_path, 'r') as root:
cam_feature = root[f'/features/{cam_name}'][:]
all_cam_feature.append(cam_feature)
vis_fea = np.concatenate(all_cam_feature, axis=1)
## State feature
dataset_path = os.path.join(dataset_dir, f'episode_{episode_id}.hdf5')
with h5py.File(dataset_path, 'r') as root:
s_fea = root['/observations/qpos'][:]
# stack actions together
eps_len = len(action)
indices = np.tile(np.arange(skip), eps_len).reshape(eps_len, skip) # each row is 0, 1, ... skip
offset = np.expand_dims(np.arange(eps_len), axis=1)
indices = indices + offset # row1: 0, 1, ... skip; row2: 1, 2, ... skip+1
# indices will exceed eps_len, thus clamp to eps_len-1
indices = np.clip(indices, 0, eps_len-1)
# stack action
action = action[indices] # new shape: eps_len, skip, a_dim
vis_features.append(vis_fea)
state_features.append(s_fea)
Y.append(action)
vis_features = np.concatenate(vis_features)
state_features = np.concatenate(state_features)
Y = np.concatenate(Y)
train_inputs = [torch.from_numpy(vis_features).cuda(), torch.from_numpy(state_features).cuda()]
train_targets = torch.from_numpy(Y).cuda()
set_seed(1000)
feature_extractors = {}
for cam_name in camera_names:
resnet = torchvision.models.resnet18(pretrained=True)
loading_status = resnet.load_state_dict(torch.load(model_dir.replace('DUMMY', cam_name)))
print(cam_name, loading_status)
resnet = nn.Sequential(*list(resnet.children())[:-1])
resnet = resnet.cuda()
resnet.eval()
feature_extractors[cam_name] = resnet
# load environment
if real_robot:
from aloha_scripts.real_env import make_real_env #### TODO TODO
env = make_real_env(init_node=True, setup_robots=True, setup_base=True)
max_timesteps = sim_episode_len
camera_names = ['cam_high', 'cam_left_wrist', 'cam_right_wrist']
else:
from sim_env import make_sim_env
env = make_sim_env(task_name)
max_timesteps = sim_episode_len
num_rollouts = 50
episode_returns = []
max_rewards = []
for rollout_id in range(num_rollouts):
### set task
BOX_POSE[0] = sample_box_pose() # used in sim reset
ts = env.reset()
### evaluation loop
qpos_history = torch.zeros((1, max_timesteps, state_dim)).cuda()
image_list = [] # for visualization
qpos_list = []
target_qpos_list = []
rewards = []
with torch.inference_mode():
for t in range(sim_episode_len):
start_time = time.time()
if t % 100 == 0: print(t)
if t % query_freq == 0:
### process previous timestep to get qpos and image_list
obs = ts.observation
if 'images' in obs:
image_list.append(obs['images'])
else:
image_list.append({'main': obs['image']})
qpos_numpy = np.array(obs['qpos'])
# qpos = pre_process(qpos_numpy)
qpos = torch.from_numpy(qpos_numpy).float().cuda().unsqueeze(0)
qpos_history[:, t] = qpos
_, curr_image_raw = get_image(ts, camera_names)
image_size = 120
transform = transforms.Compose([
transforms.Resize(image_size), # will scale the image
transforms.CenterCrop(image_size),
transforms.ToTensor(),
transforms.Lambda(expand_greyscale),
transforms.Normalize(
mean=torch.tensor([0.485, 0.456, 0.406]),
std=torch.tensor([0.229, 0.224, 0.225])),
])
all_cam_features = []
for cam_id, curr_image in enumerate(curr_image_raw):
curr_image = Image.fromarray(curr_image) # TODO only one camera
curr_image = transform(curr_image)
curr_image = curr_image.unsqueeze(dim=0).cuda()
curr_image_feature = feature_extractors[camera_names[cam_id]](curr_image)
curr_image_feature = curr_image_feature.squeeze(3).squeeze(2)
all_cam_features.append(curr_image_feature)
curr_image_feature = torch.cat(all_cam_features, dim=1)
### Visual feature
# curr_feature = curr_image_feature
### State feature
# curr_feature = qpos
### Both features
curr_feature = [curr_image_feature, qpos]
action = calculate_nearest_neighbors(curr_feature, train_inputs, train_targets, k, state_weight) # TODO use this
action = action.squeeze(0).cpu().numpy()
action = np.concatenate([action[:-BASE_DELAY, :-2], action[BASE_DELAY:, -2:]], axis=1)
print(f'Query: {(time.time() - start_time):.3f}s')
curr_action = action[t % query_freq]
target_qpos = curr_action[:-2]
base_action = curr_action[-2:]
# ### SAFETY
# max_a = 0.05
# curr_qpos = qpos.squeeze().cpu().numpy()
# target_qpos = target_qpos.clip(curr_qpos - max_a, curr_qpos + max_a)
# ### SAFETY
### step the environment
ts = env.step(target_qpos, base_action=base_action)
duration = time.time() - start_time
# print(f'{duration:.3f}')
time.sleep(max(0, DT - duration))
### save things for visualization
qpos_list.append(qpos_numpy)
target_qpos_list.append(target_qpos)
rewards.append(ts.reward)
# if real_robot and t != 0 and t % 60 == 0:
# e()
plt.close()
if real_robot:
env.puppet_bot_left.dxl.robot_set_operating_modes("single", "gripper", "position")
env.puppet_bot_right.dxl.robot_set_operating_modes("single", "gripper", "position")
env.puppet_bot_left.dxl.robot_set_operating_modes("single", "gripper", "pwm")
env.puppet_bot_right.dxl.robot_set_operating_modes("single", "gripper", "pwm")
rewards = np.array(rewards)
episode_return = np.sum(rewards[rewards!=None])
episode_returns.append(episode_return)
max_reward = np.max(rewards)
max_rewards.append(max_reward)
print(f'{episode_return=}, {max_reward=}')
if save_episode:
save_videos(image_list, DT, video_path=os.path.join(ckpt_dir, f'video{rollout_id}.mp4'))
# visualize_joints(qpos_list, target_qpos_list, plot_path=os.path.join(ckpt_dir, f'qpos{rollout_id}.png'))
# visualize_joints(qpos_list, example_qpos, plot_path=os.path.join(ckpt_dir, f'qpos_reference{rollout_id}.png'), label_overwrite=("policy", "dataset"))
success_rate = np.mean(np.array(max_rewards) == env_max_reward)
avg_return = np.mean(episode_returns)
summary_str = f'\nSuccess rate: {success_rate}\nAverage return: {avg_return}\n\n'
for r in range(env_max_reward+1):
more_or_equal_r = (np.array(max_rewards) >= r).sum()
more_or_equal_r_rate = more_or_equal_r / num_rollouts
summary_str += f'Reward >= {r}: {more_or_equal_r}/{num_rollouts} = {more_or_equal_r_rate*100}%\n'
print(summary_str)
# save success rate to txt
result_file_name = f'result_{skip}_{k}' + '.txt'
with open(os.path.join(ckpt_dir, result_file_name), 'w') as f:
f.write(summary_str)
f.write(repr(episode_returns))
f.write('\n\n')
f.write(repr(max_rewards))
return success_rate, avg_return
def get_image(ts, camera_names):
if 'images' in ts.observation:
curr_images = []
for cam_name in camera_names:
curr_image = rearrange(ts.observation['images'][cam_name], 'h w c -> c h w')
curr_images.append(curr_image)
curr_image_raw = np.stack(curr_images, axis=0)
else:
curr_image_raw = rearrange(ts.observation['image'], 'h w c -> c h w')
curr_image = torch.from_numpy(curr_image_raw / 255.0).float().cuda().unsqueeze(0)
curr_image_raw = rearrange(curr_image_raw, 'b c h w -> b h w c')
return curr_image, curr_image_raw
def expand_greyscale(t):
return t.expand(3, -1, -1)
if __name__ == '__main__':
parser = argparse.ArgumentParser()
parser.add_argument('--onscreen_render', action='store_true')
parser.add_argument('--dataset_dir', action='store', type=str, help='The text to parse.', required=True)
parser.add_argument('--model_dir', action='store', type=str, help='model_dir', required=True)
parser.add_argument('--task_name', action='store', type=str, help='task_name', required=True)
parser.add_argument('--ckpt_dir', action='store', type=str, help='The text to parse.', required=True)
main(vars(parser.parse_args()))

129
vinn_select_k.py 普通文件
查看文件

@ -0,0 +1,129 @@
import torch
import torch.nn.functional as F
import numpy as np
import h5py
import pathlib
import os
import argparse
import matplotlib.pyplot as plt
import IPython
e = IPython.embed
# modified from https://github.com/jyopari/VINN/blob/main/nearest-neighbor-eval/handle_nn.ipynb
def calculate_nearest_neighbors(query_inputs, query_targets, support_inputs, support_targets, max_k):
with torch.no_grad():
pairwise_dist = []
for q_in in query_inputs:
diff = support_inputs - q_in.unsqueeze(0)
dist = torch.norm(diff, dim=1)
pairwise_dist.append(dist)
pairwise_dist = torch.stack(pairwise_dist)
sorted_dist, index = torch.sort(pairwise_dist, dim=1) # sort the support axis
permuted_support_targets = support_targets[index]
errors = []
for k in range(1, max_k):
topk_dist = pairwise_dist[:, :k]
topk_support_targets = permuted_support_targets[:, :k]
weights = F.softmax(-topk_dist, dim=1)
weighted_support_targets = weights.unsqueeze(2) * topk_support_targets
prediction = torch.sum(weighted_support_targets, dim=1)
error = F.mse_loss(prediction, query_targets)
errors.append(error)
return errors
def chunks(lst, n):
"""Yield successive n-sized chunks from lst."""
for i in range(0, len(lst), n):
yield lst[i:i + n]
def main(args):
# TODO ######################
dataset_dir = args['dataset_dir']
ckpt_dir = args['ckpt_dir']
seed = 0
max_k = 400
batch_size = 100
# TODO ######################
if not os.path.isdir(ckpt_dir):
os.makedirs(ckpt_dir)
episode_idxs = [int(name.split('_')[1].split('.')[0]) for name in os.listdir(dataset_dir) if ('.hdf5' in name) and ('features' not in name)]
episode_idxs.sort()
assert len(episode_idxs) == episode_idxs[-1] + 1 # no holes
num_episodes = len(episode_idxs)
val_split = int(num_episodes * 0.8)
# load train data
X = []
Y = []
for episode_id in range(0, val_split):
dataset_path = os.path.join(dataset_dir, f'episode_{episode_id}.hdf5')
with h5py.File(dataset_path, 'r') as root:
action = root['/action'][:]
camera_names = list(root[f'/observations/images/'].keys())
all_cam_feature = []
feature_dataset_path = os.path.join(dataset_dir, f'byol_features_seed{seed}_episode_{episode_id}.hdf5')
with h5py.File(feature_dataset_path, 'r') as root:
for cam_name in camera_names:
cam_feature = root[f'/features/{cam_name}'][:]
all_cam_feature.append(cam_feature)
cam_feature = np.concatenate(all_cam_feature, axis=1)
X.append(cam_feature)
Y.append(action)
X = np.concatenate(X)
Y = np.concatenate(Y)
train_inputs = torch.from_numpy(X).cuda()
train_targets = torch.from_numpy(Y).cuda()
print(f'All features: {train_inputs.shape}')
# load test data
X = []
Y = []
for episode_id in range(val_split, num_episodes):
dataset_path = os.path.join(dataset_dir, f'episode_{episode_id}.hdf5')
with h5py.File(dataset_path, 'r') as root:
action = root['/action'][:]
all_cam_feature = []
feature_dataset_path = os.path.join(dataset_dir, f'byol_features_seed{seed}_episode_{episode_id}.hdf5')
with h5py.File(feature_dataset_path, 'r') as root:
for cam_name in camera_names:
cam_feature = root[f'/features/{cam_name}'][:]
all_cam_feature.append(cam_feature)
cam_feature = np.concatenate(all_cam_feature, axis=1)
X.append(cam_feature)
Y.append(action)
X = np.concatenate(X)
Y = np.concatenate(Y)
val_inputs = torch.from_numpy(X).cuda()
val_targets = torch.from_numpy(Y).cuda()
val_losses = []
for inputs, targets in zip(chunks(val_inputs, batch_size), chunks(val_targets, batch_size)):
val_loss = calculate_nearest_neighbors(inputs, targets, train_inputs, train_targets, max_k)
val_loss = torch.stack(val_loss)
val_losses.append(val_loss)
val_losses = torch.mean(torch.stack(val_losses), dim=0)
val_loss = val_losses
val_loss = torch.tensor(val_loss).cpu().numpy()
print(f'min val loss of {np.min(val_loss)} at k={np.argmin(val_loss)}')
plt.plot(np.arange(1, max_k), val_loss)
plt.savefig(os.path.join(ckpt_dir, f'k_select-seed{seed}.png'))
if __name__ == '__main__':
parser = argparse.ArgumentParser()
parser.add_argument('--dataset_dir', action='store', type=str, help='The text to parse.', required=True)
parser.add_argument('--ckpt_dir', action='store', type=str, help='The text to parse.', required=True)
main(vars(parser.parse_args()))