├── .gitignore ├── LICENSE ├── README.md ├── equibot ├── envs │ ├── sim_mobile │ │ ├── assets │ │ │ ├── closing │ │ │ │ ├── template.urdf │ │ │ │ └── template_real.urdf │ │ │ ├── covering │ │ │ │ ├── .gitignore │ │ │ │ ├── box.mtl │ │ │ │ └── box.obj │ │ │ ├── folding │ │ │ │ ├── .gitignore │ │ │ │ ├── towel.jpg │ │ │ │ ├── towel.mtl │ │ │ │ ├── towel.obj │ │ │ │ └── towel_small.obj │ │ │ ├── robots │ │ │ │ └── kinova │ │ │ │ │ ├── base.urdf │ │ │ │ │ ├── base_with_kinova_gripper.urdf │ │ │ │ │ └── meshes │ │ │ │ │ ├── arm │ │ │ │ │ ├── base_link.STL │ │ │ │ │ ├── bracelet_no_vision_link.STL │ │ │ │ │ ├── bracelet_with_vision_link.STL │ │ │ │ │ ├── end_effector_link.STL │ │ │ │ │ ├── forearm_link.STL │ │ │ │ │ ├── half_arm_1_link.STL │ │ │ │ │ ├── half_arm_2_link.STL │ │ │ │ │ ├── shoulder_link.STL │ │ │ │ │ ├── spherical_wrist_1_link.STL │ │ │ │ │ └── spherical_wrist_2_link.STL │ │ │ │ │ ├── arm_col │ │ │ │ │ ├── base_link.STL │ │ │ │ │ ├── bracelet_no_vision_link.STL │ │ │ │ │ ├── bracelet_with_vision_link.STL │ │ │ │ │ ├── end_effector_link.STL │ │ │ │ │ ├── forearm_link.STL │ │ │ │ │ ├── half_arm_1_link.STL │ │ │ │ │ ├── half_arm_2_link.STL │ │ │ │ │ ├── shoulder_link.STL │ │ │ │ │ ├── spherical_wrist_1_link.STL │ │ │ │ │ └── spherical_wrist_2_link.STL │ │ │ │ │ ├── base │ │ │ │ │ ├── mobile_base.obj │ │ │ │ │ └── mobile_base_with_arm_base_link.obj │ │ │ │ │ ├── gripper │ │ │ │ │ ├── collision │ │ │ │ │ │ ├── robotiq_arg2f_85_base_link.stl │ │ │ │ │ │ ├── robotiq_arg2f_85_inner_finger.dae │ │ │ │ │ │ ├── robotiq_arg2f_85_inner_knuckle.dae │ │ │ │ │ │ ├── robotiq_arg2f_85_outer_finger.dae │ │ │ │ │ │ ├── robotiq_arg2f_85_outer_knuckle.dae │ │ │ │ │ │ ├── robotiq_arg2f_85_pad.dae │ │ │ │ │ │ ├── robotiq_arg2f_85_pad_large.stl │ │ │ │ │ │ ├── robotiq_arg2f_base_link.stl │ │ │ │ │ │ └── robotiq_arg2f_base_link_kinova_frame.stl │ │ │ │ │ └── visual │ │ │ │ │ │ ├── robotiq_arg2f_85_base_link.dae │ │ │ │ │ │ ├── robotiq_arg2f_85_base_link_v3000_large.obj │ │ │ │ │ │ ├── robotiq_arg2f_85_inner_finger.dae │ │ │ │ │ │ ├── robotiq_arg2f_85_inner_knuckle.dae │ │ │ │ │ │ ├── robotiq_arg2f_85_outer_finger.dae │ │ │ │ │ │ ├── robotiq_arg2f_85_outer_knuckle.dae │ │ │ │ │ │ ├── robotiq_arg2f_85_pad.dae │ │ │ │ │ │ ├── robotiq_arg2f_85_pad.stl │ │ │ │ │ │ └── robotiq_gripper_coupling.stl │ │ │ │ │ └── tools │ │ │ │ │ ├── small_sphere.jpg │ │ │ │ │ ├── small_sphere.mtl │ │ │ │ │ └── small_sphere.obj │ │ │ └── textures │ │ │ │ ├── comforter.png │ │ │ │ ├── dark_carpet.jpg │ │ │ │ └── wood.jpg │ │ ├── base_env.py │ │ ├── closing_env.py │ │ ├── covering_env.py │ │ ├── folding_env.py │ │ ├── generate_demos.py │ │ └── utils │ │ │ ├── anchors.py │ │ │ ├── bullet_robot.py │ │ │ ├── console.py │ │ │ ├── convert_coords.py │ │ │ ├── frame_converter.py │ │ │ ├── info.py │ │ │ ├── init_utils.py │ │ │ ├── multi_camera.py │ │ │ ├── plan_control_traj.py │ │ │ ├── project.py │ │ │ ├── render.py │ │ │ └── transformations.py │ ├── subproc_vec_env.py │ └── vec_env.py └── policies │ ├── agents │ ├── dp_agent.py │ ├── dp_policy.py │ ├── equibot_agent.py │ └── equibot_policy.py │ ├── configs │ ├── base.yaml │ ├── close_mobile_dp.yaml │ ├── close_mobile_equibot.yaml │ ├── cover_mobile_dp.yaml │ ├── cover_mobile_equibot.yaml │ ├── fold_mobile_dp.yaml │ └── fold_mobile_equibot.yaml │ ├── datasets │ └── dataset.py │ ├── eval.py │ ├── train.py │ ├── utils │ ├── diffusion │ │ ├── conditional_unet1d.py │ │ ├── conv1d_components.py │ │ ├── ema_model.py │ │ ├── lr_scheduler.py │ │ ├── positional_embedding.py │ │ └── resnet_with_gn.py │ ├── equivariant_diffusion │ │ ├── conditional_unet1d.py │ │ └── conv1d_components.py │ ├── media.py │ ├── misc.py │ └── norm.py │ ├── vec_eval.py │ └── vision │ ├── misc.py │ ├── pointnet_encoder.py │ ├── sim3_encoder.py │ ├── vec_layers.py │ └── vec_pointnet.py └── setup.py /.gitignore: -------------------------------------------------------------------------------- 1 | logs/ 2 | **/__pycache__ 3 | .DS_Store 4 | *.egg-info 5 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2024 Stanford Interactive Perception and Robot Learning Lab 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # EquiBot: SIM(3)-Equivariant Diffusion Policy for Generalizable and Data Efficient Learning 2 | 3 | Jingyun Yang*, Zi-ang Cao*, Congyue Deng, Rika Antonova, Shuran Song, Jeannette Bohg 4 | 5 | [![YouTube](https://badges.aleen42.com/src/youtube.svg)](https://youtu.be/FFrl_TEXrUw) 6 | 7 | ![Overview figure](https://equi-bot.github.io/images/teaser.jpg) 8 | 9 | This repository includes: 10 | 11 | * Implementation of the EquiBot method and a Diffusion Policy baseline that takes point clouds as input. 12 | * A set of three simulated mobile manipulation environments: Cloth Folding, Object Covering, and Box Closing. 13 | * Data generation, training, and evaluation scripts that accompany the above algorithms and environments. 14 | 15 | ## Getting Started 16 | 17 | ### Installation 18 | 19 | This codebase is tested with the following setup: Ubuntu 20.04, an RTX 4090 GPU, CUDA 11.8. In the root directory of the repository, run the following commands: 20 | 21 | ``` 22 | conda create -n lfd python=3.10 -y 23 | conda activate lfd 24 | 25 | conda install -y fvcore iopath ffmpeg -c iopath -c fvcore 26 | pip install torch==2.1.0 torchvision==0.16.0 torchaudio==2.1.0 --index-url https://download.pytorch.org/whl/cu118 27 | pip install "git+https://github.com/facebookresearch/pytorch3d.git" 28 | 29 | pip install -e . 30 | ``` 31 | 32 | Then, in the last two lines of [this config file](equibot/policies/configs/base.yaml), enter the wandb entity and project names for logging purposes. If you do not have a wandb account yet, you can register [here](https://wandb.ai). 33 | 34 | ### Demonstration Generation 35 | 36 | The following code generates demonstrations for simulated mobile environments. To change the number of generated demos, change `--num_demos 50` to a different number. 37 | 38 | ``` 39 | python -m equibot.envs.sim_mobile.generate_demos --data_out_dir ../data/fold \ 40 | --num_demos 50 --cam_dist 2 --cam_pitches -75 --task_name fold 41 | 42 | python -m equibot.envs.sim_mobile.generate_demos --data_out_dir ../data/cover \ 43 | --num_demos 50 --cam_dist 2 --cam_pitches -75 --task_name cover 44 | 45 | python -m equibot.envs.sim_mobile.generate_demos --data_out_dir ../data/close \ 46 | --num_demos 50 --cam_dist 1.5 --cam_pitches -45 --task_name close 47 | ``` 48 | 49 | ### Training 50 | 51 | The following code runs training for our method and the Diffusion Policy baseline. Fill the dataset path with the `data_out_dir` argument in the previous section. Make sure the dataset path ends with `pcs`. To run this code for the `cover` and `close` environments, substitute occurrences of `fold` with `cover` or `close`. 52 | 53 | ``` 54 | # diffusion policy baseline (takes point clouds as input) 55 | python -m equibot.policies.train --config-name fold_mobile_dp \ 56 | prefix=sim_mobile_fold_7dof_dp \ 57 | data.dataset.path=[data out dir in the last section]/pcs 58 | 59 | # our method (equibot) 60 | python -m equibot.policies.train --config-name fold_mobile_equibot \ 61 | prefix=sim_mobile_fold_7dof_equibot \ 62 | data.dataset.path=[data out dir in the last section]/pcs 63 | ``` 64 | 65 | ### Evaluation 66 | 67 | The commands below evaluate the trained EquiBot policy on the four different setups mentioned in the paper: `Original`, `R+Su`, `R+Sn`, and `R+Sn+P`. To run these evaluations for the DP baseline, replace all occurrences of `equibot` to`dp`. For the log directory, fill `[log_dir]` with the absolute path to the log directory. By default, this directory is `./log`. 68 | 69 | ``` 70 | # Original setup 71 | python -m equibot.policies.eval --config-name fold_mobile_equibot \ 72 | prefix="eval_original_sim_mobile_fold_equibot_s1" mode=eval \ 73 | training.ckpt="[log_dir]/train/sim_mobile_fold_7dof_equibot_s1/ckpt01999.pth" \ 74 | env.args.max_episode_length=50 env.vectorize=true 75 | 76 | # R+Su setup 77 | python -m equibot.policies.eval --config-name fold_mobile_equibot \ 78 | prefix="eval_rsu_sim_mobile_fold_7dof_equibot_s1" mode=eval \ 79 | training.ckpt="[log_dir]/train/sim_mobile_fold_7dof_equibot_s1/ckpt01999.pth" \ 80 | env.args.scale_high=2 env.args.uniform_scaling=true \ 81 | env.args.randomize_rotation=true env.args.randomize_scale=true env.vectorize=true 82 | 83 | # R+Sn setup 84 | python -m equibot.policies.eval --config-name fold_mobile_equibot \ 85 | prefix="eval_rsn_sim_mobile_fold_7dof_equibot_s1" mode=eval \ 86 | training.ckpt="[log_dir]/train/sim_mobile_fold_7dof_equibot_s1/ckpt01999.pth" \ 87 | env.args.scale_high=2 env.args.scale_aspect_limit=1.33 \ 88 | env.args.randomize_rotation=true env.args.randomize_scale=true env.vectorize=true 89 | 90 | # R+Sn+P setup 91 | python -m equibot.policies.eval --config-name fold_mobile_equibot \ 92 | prefix="eval_rsnp_sim_mobile_fold_7dof_equibot_s1" mode=eval \ 93 | training.ckpt="[log_dir]/train/sim_mobile_fold_7dof_equibot_s1/ckpt01999.pth" \ 94 | env.args.scale_high=2 env.args.scale_aspect_limit=1.33 \ 95 | env.args.randomize_rotation=true env.args.randomize_scale=true \ 96 | +env.args.randomize_position=true +env.args.rand_pos_scale=0.5 env.vectorize=true 97 | ``` 98 | 99 | ## License 100 | 101 | This codebase is licensed under the terms of the MIT License. 102 | -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/assets/closing/template.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/assets/closing/template_real.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/assets/covering/.gitignore: -------------------------------------------------------------------------------- 1 | box_scaled_*.obj 2 | -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/assets/covering/box.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 1 3 | 4 | newmtl Material 5 | Ns 323.999994 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 0.800000 0.800000 0.800000 8 | Ks 0.500000 0.500000 0.500000 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.450000 11 | d 1.000000 12 | illum 2 13 | -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/assets/folding/.gitignore: -------------------------------------------------------------------------------- 1 | towel_scaled_*.obj 2 | towel_small_scaled_*.obj -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/assets/folding/towel.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yjy0625/equibot/dc70059191beb29903d78fc4d0193392be5134fc/equibot/envs/sim_mobile/assets/folding/towel.jpg -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/assets/folding/towel.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 1 3 | 4 | newmtl Towel 5 | Ns 225.000000 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 0.800000 0.800000 0.800000 8 | Ks 0.500000 0.500000 0.500000 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.450000 11 | d 1.000000 12 | illum 2 13 | map_Kd ./towel.jpg -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/assets/robots/kinova/base.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/assets/robots/kinova/meshes/arm/base_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yjy0625/equibot/dc70059191beb29903d78fc4d0193392be5134fc/equibot/envs/sim_mobile/assets/robots/kinova/meshes/arm/base_link.STL -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/assets/robots/kinova/meshes/arm/bracelet_no_vision_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yjy0625/equibot/dc70059191beb29903d78fc4d0193392be5134fc/equibot/envs/sim_mobile/assets/robots/kinova/meshes/arm/bracelet_no_vision_link.STL -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/assets/robots/kinova/meshes/arm/bracelet_with_vision_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yjy0625/equibot/dc70059191beb29903d78fc4d0193392be5134fc/equibot/envs/sim_mobile/assets/robots/kinova/meshes/arm/bracelet_with_vision_link.STL -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/assets/robots/kinova/meshes/arm/end_effector_link.STL: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/assets/robots/kinova/meshes/arm/forearm_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yjy0625/equibot/dc70059191beb29903d78fc4d0193392be5134fc/equibot/envs/sim_mobile/assets/robots/kinova/meshes/arm/forearm_link.STL -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/assets/robots/kinova/meshes/arm/half_arm_1_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yjy0625/equibot/dc70059191beb29903d78fc4d0193392be5134fc/equibot/envs/sim_mobile/assets/robots/kinova/meshes/arm/half_arm_1_link.STL -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/assets/robots/kinova/meshes/arm/half_arm_2_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yjy0625/equibot/dc70059191beb29903d78fc4d0193392be5134fc/equibot/envs/sim_mobile/assets/robots/kinova/meshes/arm/half_arm_2_link.STL -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/assets/robots/kinova/meshes/arm/shoulder_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yjy0625/equibot/dc70059191beb29903d78fc4d0193392be5134fc/equibot/envs/sim_mobile/assets/robots/kinova/meshes/arm/shoulder_link.STL -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/assets/robots/kinova/meshes/arm/spherical_wrist_1_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yjy0625/equibot/dc70059191beb29903d78fc4d0193392be5134fc/equibot/envs/sim_mobile/assets/robots/kinova/meshes/arm/spherical_wrist_1_link.STL -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/assets/robots/kinova/meshes/arm/spherical_wrist_2_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yjy0625/equibot/dc70059191beb29903d78fc4d0193392be5134fc/equibot/envs/sim_mobile/assets/robots/kinova/meshes/arm/spherical_wrist_2_link.STL -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/assets/robots/kinova/meshes/arm_col/base_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yjy0625/equibot/dc70059191beb29903d78fc4d0193392be5134fc/equibot/envs/sim_mobile/assets/robots/kinova/meshes/arm_col/base_link.STL -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/assets/robots/kinova/meshes/arm_col/bracelet_no_vision_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yjy0625/equibot/dc70059191beb29903d78fc4d0193392be5134fc/equibot/envs/sim_mobile/assets/robots/kinova/meshes/arm_col/bracelet_no_vision_link.STL -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/assets/robots/kinova/meshes/arm_col/bracelet_with_vision_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yjy0625/equibot/dc70059191beb29903d78fc4d0193392be5134fc/equibot/envs/sim_mobile/assets/robots/kinova/meshes/arm_col/bracelet_with_vision_link.STL -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/assets/robots/kinova/meshes/arm_col/end_effector_link.STL: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/assets/robots/kinova/meshes/arm_col/forearm_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yjy0625/equibot/dc70059191beb29903d78fc4d0193392be5134fc/equibot/envs/sim_mobile/assets/robots/kinova/meshes/arm_col/forearm_link.STL -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/assets/robots/kinova/meshes/arm_col/half_arm_1_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yjy0625/equibot/dc70059191beb29903d78fc4d0193392be5134fc/equibot/envs/sim_mobile/assets/robots/kinova/meshes/arm_col/half_arm_1_link.STL -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/assets/robots/kinova/meshes/arm_col/half_arm_2_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yjy0625/equibot/dc70059191beb29903d78fc4d0193392be5134fc/equibot/envs/sim_mobile/assets/robots/kinova/meshes/arm_col/half_arm_2_link.STL -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/assets/robots/kinova/meshes/arm_col/shoulder_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yjy0625/equibot/dc70059191beb29903d78fc4d0193392be5134fc/equibot/envs/sim_mobile/assets/robots/kinova/meshes/arm_col/shoulder_link.STL -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/assets/robots/kinova/meshes/arm_col/spherical_wrist_1_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yjy0625/equibot/dc70059191beb29903d78fc4d0193392be5134fc/equibot/envs/sim_mobile/assets/robots/kinova/meshes/arm_col/spherical_wrist_1_link.STL -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/assets/robots/kinova/meshes/arm_col/spherical_wrist_2_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yjy0625/equibot/dc70059191beb29903d78fc4d0193392be5134fc/equibot/envs/sim_mobile/assets/robots/kinova/meshes/arm_col/spherical_wrist_2_link.STL -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/assets/robots/kinova/meshes/gripper/collision/robotiq_arg2f_85_base_link.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yjy0625/equibot/dc70059191beb29903d78fc4d0193392be5134fc/equibot/envs/sim_mobile/assets/robots/kinova/meshes/gripper/collision/robotiq_arg2f_85_base_link.stl -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/assets/robots/kinova/meshes/gripper/collision/robotiq_arg2f_85_pad.dae: -------------------------------------------------------------------------------- 1 | 2 | 3 | 2016-07-17T22:25:43.361178 4 | 2016-07-17T22:25:43.361188 5 | Z_UP 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 0.0 0.0 0.0 1.0 14 | 15 | 16 | 0.0 0.0 0.0 1.0 17 | 18 | 19 | 0.7 0.7 0.7 1.0 20 | 21 | 22 | 1 1 1 1.0 23 | 24 | 25 | 0.0 26 | 27 | 28 | 0.0 0.0 0.0 1.0 29 | 30 | 31 | 0.0 32 | 33 | 34 | 0.0 0.0 0.0 1.0 35 | 36 | 37 | 1.0 38 | 39 | 40 | 41 | 42 | 43 | 0 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 4.38531e-14 -1 -4.451336e-05 4.38531e-14 -1 -4.451336e-05 -4.38531e-14 1 4.451336e-05 -4.38531e-14 1 4.451336e-05 -1 -4.385301e-14 -2.011189e-15 -1 -4.385301e-14 -2.011189e-15 -2.009237e-15 -4.451336e-05 1 -2.009237e-15 -4.451336e-05 1 1 4.385301e-14 2.011189e-15 1 4.385301e-14 2.011189e-15 2.009237e-15 4.451336e-05 -1 2.009237e-15 4.451336e-05 -1 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -10 -23.90175 13.51442 10 -23.9033 48.51442 -10 -23.9033 48.51442 10 -23.90175 13.51442 -10 -18.90175 13.51464 -10 -18.9033 48.51464 10 -18.90175 13.51464 10 -18.9033 48.51464 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 |

0 0 1 0 2 0 3 1 1 1 0 1 4 2 5 2 6 2 5 3 7 3 6 3 2 4 5 4 4 4 2 5 4 5 0 5 5 6 2 6 1 6 5 7 1 7 7 7 7 8 1 8 6 8 1 9 3 9 6 9 0 10 4 10 3 10 4 11 6 11 3 11

79 |
80 |
81 |
82 |
83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 |
105 | -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/assets/robots/kinova/meshes/gripper/collision/robotiq_arg2f_85_pad_large.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yjy0625/equibot/dc70059191beb29903d78fc4d0193392be5134fc/equibot/envs/sim_mobile/assets/robots/kinova/meshes/gripper/collision/robotiq_arg2f_85_pad_large.stl -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/assets/robots/kinova/meshes/gripper/collision/robotiq_arg2f_base_link.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yjy0625/equibot/dc70059191beb29903d78fc4d0193392be5134fc/equibot/envs/sim_mobile/assets/robots/kinova/meshes/gripper/collision/robotiq_arg2f_base_link.stl -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/assets/robots/kinova/meshes/gripper/collision/robotiq_arg2f_base_link_kinova_frame.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yjy0625/equibot/dc70059191beb29903d78fc4d0193392be5134fc/equibot/envs/sim_mobile/assets/robots/kinova/meshes/gripper/collision/robotiq_arg2f_base_link_kinova_frame.stl -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/assets/robots/kinova/meshes/gripper/visual/robotiq_arg2f_85_pad.dae: -------------------------------------------------------------------------------- 1 | 2 | 3 | 2016-07-17T22:25:43.361178 4 | 2016-07-17T22:25:43.361188 5 | Z_UP 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 0.0 0.0 0.0 1.0 14 | 15 | 16 | 0.0 0.0 0.0 1.0 17 | 18 | 19 | 0.7 0.7 0.7 1.0 20 | 21 | 22 | 1 1 1 1.0 23 | 24 | 25 | 0.0 26 | 27 | 28 | 0.0 0.0 0.0 1.0 29 | 30 | 31 | 0.0 32 | 33 | 34 | 0.0 0.0 0.0 1.0 35 | 36 | 37 | 1.0 38 | 39 | 40 | 41 | 42 | 43 | 0 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 4.38531e-14 -1 -4.451336e-05 4.38531e-14 -1 -4.451336e-05 -4.38531e-14 1 4.451336e-05 -4.38531e-14 1 4.451336e-05 -1 -4.385301e-14 -2.011189e-15 -1 -4.385301e-14 -2.011189e-15 -2.009237e-15 -4.451336e-05 1 -2.009237e-15 -4.451336e-05 1 1 4.385301e-14 2.011189e-15 1 4.385301e-14 2.011189e-15 2.009237e-15 4.451336e-05 -1 2.009237e-15 4.451336e-05 -1 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -10 -23.90175 13.51442 10 -23.9033 48.51442 -10 -23.9033 48.51442 10 -23.90175 13.51442 -10 -18.90175 13.51464 -10 -18.9033 48.51464 10 -18.90175 13.51464 10 -18.9033 48.51464 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 |

0 0 1 0 2 0 3 1 1 1 0 1 4 2 5 2 6 2 5 3 7 3 6 3 2 4 5 4 4 4 2 5 4 5 0 5 5 6 2 6 1 6 5 7 1 7 7 7 7 8 1 8 6 8 1 9 3 9 6 9 0 10 4 10 3 10 4 11 6 11 3 11

79 |
80 |
81 |
82 |
83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 |
105 | -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/assets/robots/kinova/meshes/gripper/visual/robotiq_arg2f_85_pad.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yjy0625/equibot/dc70059191beb29903d78fc4d0193392be5134fc/equibot/envs/sim_mobile/assets/robots/kinova/meshes/gripper/visual/robotiq_arg2f_85_pad.stl -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/assets/robots/kinova/meshes/gripper/visual/robotiq_gripper_coupling.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yjy0625/equibot/dc70059191beb29903d78fc4d0193392be5134fc/equibot/envs/sim_mobile/assets/robots/kinova/meshes/gripper/visual/robotiq_gripper_coupling.stl -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/assets/robots/kinova/meshes/tools/small_sphere.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yjy0625/equibot/dc70059191beb29903d78fc4d0193392be5134fc/equibot/envs/sim_mobile/assets/robots/kinova/meshes/tools/small_sphere.jpg -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/assets/robots/kinova/meshes/tools/small_sphere.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 1 3 | 4 | newmtl Sphere 5 | Ns 225.000000 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 0.800000 0.800000 0.800000 8 | Ks 0.500000 0.500000 0.500000 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.450000 11 | d 1.000000 12 | illum 2 13 | map_Kd ./sphere.jpg -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/assets/robots/kinova/meshes/tools/small_sphere.obj: -------------------------------------------------------------------------------- 1 | # Blender v3.0.0 OBJ File: '' 2 | # www.blender.org 3 | mtllib small_sphere.mtl 4 | o Icosphere 5 | v 0.000000 0.000000 -0.010000 6 | v 0.007236 -0.005257 -0.004472 7 | v -0.002764 -0.008506 -0.004472 8 | v -0.008944 0.000000 -0.004472 9 | v -0.002764 0.008506 -0.004472 10 | v 0.007236 0.005257 -0.004472 11 | v 0.002764 -0.008506 0.004472 12 | v -0.007236 -0.005257 0.004472 13 | v -0.007236 0.005257 0.004472 14 | v 0.002764 0.008506 0.004472 15 | v 0.008944 0.000000 0.004472 16 | v 0.000000 0.000000 0.010000 17 | v -0.001625 -0.005000 -0.008507 18 | v 0.004253 -0.003090 -0.008507 19 | v 0.002629 -0.008090 -0.005257 20 | v 0.008506 0.000000 -0.005257 21 | v 0.004253 0.003090 -0.008507 22 | v -0.005257 0.000000 -0.008507 23 | v -0.006882 -0.005000 -0.005257 24 | v -0.001625 0.005000 -0.008507 25 | v -0.006882 0.005000 -0.005257 26 | v 0.002629 0.008090 -0.005257 27 | v 0.009511 -0.003090 0.000000 28 | v 0.009511 0.003090 0.000000 29 | v 0.000000 -0.010000 0.000000 30 | v 0.005878 -0.008090 0.000000 31 | v -0.009511 -0.003090 0.000000 32 | v -0.005878 -0.008090 0.000000 33 | v -0.005878 0.008090 0.000000 34 | v -0.009511 0.003090 0.000000 35 | v 0.005878 0.008090 0.000000 36 | v 0.000000 0.010000 0.000000 37 | v 0.006882 -0.005000 0.005257 38 | v -0.002629 -0.008090 0.005257 39 | v -0.008506 0.000000 0.005257 40 | v -0.002629 0.008090 0.005257 41 | v 0.006882 0.005000 0.005257 42 | v 0.001625 -0.005000 0.008507 43 | v 0.005257 0.000000 0.008507 44 | v -0.004253 -0.003090 0.008507 45 | v -0.004253 0.003090 0.008507 46 | v 0.001625 0.005000 0.008507 47 | vt 0.181819 0.000000 48 | vt 0.227273 0.078731 49 | vt 0.136365 0.078731 50 | vt 0.272728 0.157461 51 | vt 0.318182 0.078731 52 | vt 0.363637 0.157461 53 | vt 0.909091 0.000000 54 | vt 0.954545 0.078731 55 | vt 0.863636 0.078731 56 | vt 0.727273 0.000000 57 | vt 0.772727 0.078731 58 | vt 0.681818 0.078731 59 | vt 0.545455 0.000000 60 | vt 0.590909 0.078731 61 | vt 0.500000 0.078731 62 | vt 0.318182 0.236191 63 | vt 0.090910 0.157461 64 | vt 0.181819 0.157461 65 | vt 0.136365 0.236191 66 | vt 0.818182 0.157461 67 | vt 0.909091 0.157461 68 | vt 0.863636 0.236191 69 | vt 0.636364 0.157461 70 | vt 0.727273 0.157461 71 | vt 0.681818 0.236191 72 | vt 0.454546 0.157461 73 | vt 0.545455 0.157461 74 | vt 0.500000 0.236191 75 | vt 0.227273 0.236191 76 | vt 0.045455 0.236191 77 | vt 0.772727 0.236191 78 | vt 0.590909 0.236191 79 | vt 0.409092 0.236191 80 | vt 0.181819 0.314921 81 | vt 0.272728 0.314921 82 | vt 0.227273 0.393651 83 | vt 0.000000 0.314921 84 | vt 0.090910 0.314921 85 | vt 0.045455 0.393651 86 | vt 0.727273 0.314921 87 | vt 0.818182 0.314921 88 | vt 0.772727 0.393651 89 | vt 0.545455 0.314921 90 | vt 0.636364 0.314921 91 | vt 0.590909 0.393651 92 | vt 0.363637 0.314921 93 | vt 0.454546 0.314921 94 | vt 0.409092 0.393651 95 | vt 0.500000 0.393651 96 | vt 0.454546 0.472382 97 | vt 0.681818 0.393651 98 | vt 0.636364 0.472382 99 | vt 0.863636 0.393651 100 | vt 0.818182 0.472382 101 | vt 0.909091 0.314921 102 | vt 0.136365 0.393651 103 | vt 0.090910 0.472382 104 | vt 0.318182 0.393651 105 | vt 0.272728 0.472382 106 | vt 0.954545 0.236191 107 | vt 1.000000 0.157461 108 | vt 0.409092 0.078731 109 | vt 0.363637 0.000000 110 | vn 0.1024 -0.3151 -0.9435 111 | vn 0.7002 -0.2680 -0.6617 112 | vn -0.2680 -0.1947 -0.9435 113 | vn -0.2680 0.1947 -0.9435 114 | vn 0.1024 0.3151 -0.9435 115 | vn 0.9050 -0.2680 -0.3304 116 | vn 0.0247 -0.9435 -0.3304 117 | vn -0.8897 -0.3151 -0.3304 118 | vn -0.5746 0.7488 -0.3304 119 | vn 0.5346 0.7779 -0.3304 120 | vn 0.8026 -0.5831 -0.1256 121 | vn -0.3066 -0.9435 -0.1256 122 | vn -0.9921 0.0000 -0.1256 123 | vn -0.3066 0.9435 -0.1256 124 | vn 0.8026 0.5831 -0.1256 125 | vn 0.4089 -0.6284 0.6617 126 | vn -0.4713 -0.5831 0.6617 127 | vn -0.7002 0.2680 0.6617 128 | vn 0.0385 0.7488 0.6617 129 | vn 0.7240 0.1947 0.6617 130 | vn 0.2680 0.1947 0.9435 131 | vn 0.4911 0.3568 0.7947 132 | vn 0.4089 0.6284 0.6617 133 | vn -0.1024 0.3151 0.9435 134 | vn -0.1876 0.5773 0.7947 135 | vn -0.4713 0.5831 0.6617 136 | vn -0.3313 0.0000 0.9435 137 | vn -0.6071 0.0000 0.7947 138 | vn -0.7002 -0.2680 0.6617 139 | vn -0.1024 -0.3151 0.9435 140 | vn -0.1876 -0.5773 0.7947 141 | vn 0.0385 -0.7488 0.6617 142 | vn 0.2680 -0.1947 0.9435 143 | vn 0.4911 -0.3568 0.7947 144 | vn 0.7240 -0.1947 0.6617 145 | vn 0.8897 0.3151 0.3304 146 | vn 0.7947 0.5773 0.1876 147 | vn 0.5746 0.7488 0.3304 148 | vn -0.0247 0.9435 0.3304 149 | vn -0.3035 0.9342 0.1876 150 | vn -0.5346 0.7779 0.3304 151 | vn -0.9050 0.2680 0.3304 152 | vn -0.9822 0.0000 0.1876 153 | vn -0.9050 -0.2680 0.3304 154 | vn -0.5346 -0.7779 0.3304 155 | vn -0.3035 -0.9342 0.1876 156 | vn -0.0247 -0.9435 0.3304 157 | vn 0.5746 -0.7488 0.3304 158 | vn 0.7947 -0.5773 0.1876 159 | vn 0.8897 -0.3151 0.3304 160 | vn 0.3066 0.9435 0.1256 161 | vn 0.3035 0.9342 -0.1876 162 | vn 0.0247 0.9435 -0.3304 163 | vn -0.8026 0.5831 0.1256 164 | vn -0.7947 0.5773 -0.1876 165 | vn -0.8897 0.3151 -0.3304 166 | vn -0.8026 -0.5831 0.1256 167 | vn -0.7947 -0.5773 -0.1876 168 | vn -0.5746 -0.7488 -0.3304 169 | vn 0.3066 -0.9435 0.1256 170 | vn 0.3035 -0.9342 -0.1876 171 | vn 0.5346 -0.7779 -0.3304 172 | vn 0.9921 0.0000 0.1256 173 | vn 0.9822 0.0000 -0.1876 174 | vn 0.9050 0.2680 -0.3304 175 | vn 0.4713 0.5831 -0.6617 176 | vn 0.1876 0.5773 -0.7947 177 | vn -0.0385 0.7488 -0.6617 178 | vn -0.4089 0.6284 -0.6617 179 | vn -0.4911 0.3568 -0.7947 180 | vn -0.7240 0.1947 -0.6617 181 | vn -0.7240 -0.1947 -0.6617 182 | vn -0.4911 -0.3568 -0.7947 183 | vn -0.4089 -0.6284 -0.6617 184 | vn 0.7002 0.2680 -0.6617 185 | vn 0.6071 0.0000 -0.7947 186 | vn 0.3313 0.0000 -0.9435 187 | vn -0.0385 -0.7488 -0.6617 188 | vn 0.1876 -0.5773 -0.7947 189 | vn 0.4713 -0.5831 -0.6617 190 | usemtl None 191 | s off 192 | f 1/1/1 14/2/1 13/3/1 193 | f 2/4/2 14/5/2 16/6/2 194 | f 1/7/3 13/8/3 18/9/3 195 | f 1/10/4 18/11/4 20/12/4 196 | f 1/13/5 20/14/5 17/15/5 197 | f 2/4/6 16/6/6 23/16/6 198 | f 3/17/7 15/18/7 25/19/7 199 | f 4/20/8 19/21/8 27/22/8 200 | f 5/23/9 21/24/9 29/25/9 201 | f 6/26/10 22/27/10 31/28/10 202 | f 2/4/11 23/16/11 26/29/11 203 | f 3/17/12 25/19/12 28/30/12 204 | f 4/20/13 27/22/13 30/31/13 205 | f 5/23/14 29/25/14 32/32/14 206 | f 6/26/15 31/28/15 24/33/15 207 | f 7/34/16 33/35/16 38/36/16 208 | f 8/37/17 34/38/17 40/39/17 209 | f 9/40/18 35/41/18 41/42/18 210 | f 10/43/19 36/44/19 42/45/19 211 | f 11/46/20 37/47/20 39/48/20 212 | f 39/48/21 42/49/21 12/50/21 213 | f 39/48/22 37/47/22 42/49/22 214 | f 37/47/23 10/43/23 42/49/23 215 | f 42/45/24 41/51/24 12/52/24 216 | f 42/45/25 36/44/25 41/51/25 217 | f 36/44/26 9/40/26 41/51/26 218 | f 41/42/27 40/53/27 12/54/27 219 | f 41/42/28 35/41/28 40/53/28 220 | f 35/41/29 8/55/29 40/53/29 221 | f 40/39/30 38/56/30 12/57/30 222 | f 40/39/31 34/38/31 38/56/31 223 | f 34/38/32 7/34/32 38/56/32 224 | f 38/36/33 39/58/33 12/59/33 225 | f 38/36/34 33/35/34 39/58/34 226 | f 33/35/35 11/46/35 39/58/35 227 | f 24/33/36 37/47/36 11/46/36 228 | f 24/33/37 31/28/37 37/47/37 229 | f 31/28/38 10/43/38 37/47/38 230 | f 32/32/39 36/44/39 10/43/39 231 | f 32/32/40 29/25/40 36/44/40 232 | f 29/25/41 9/40/41 36/44/41 233 | f 30/31/42 35/41/42 9/40/42 234 | f 30/31/43 27/22/43 35/41/43 235 | f 27/22/44 8/55/44 35/41/44 236 | f 28/30/45 34/38/45 8/37/45 237 | f 28/30/46 25/19/46 34/38/46 238 | f 25/19/47 7/34/47 34/38/47 239 | f 26/29/48 33/35/48 7/34/48 240 | f 26/29/49 23/16/49 33/35/49 241 | f 23/16/50 11/46/50 33/35/50 242 | f 31/28/51 32/32/51 10/43/51 243 | f 31/28/52 22/27/52 32/32/52 244 | f 22/27/53 5/23/53 32/32/53 245 | f 29/25/54 30/31/54 9/40/54 246 | f 29/25/55 21/24/55 30/31/55 247 | f 21/24/56 4/20/56 30/31/56 248 | f 27/22/57 28/60/57 8/55/57 249 | f 27/22/58 19/21/58 28/60/58 250 | f 19/21/59 3/61/59 28/60/59 251 | f 25/19/60 26/29/60 7/34/60 252 | f 25/19/61 15/18/61 26/29/61 253 | f 15/18/62 2/4/62 26/29/62 254 | f 23/16/63 24/33/63 11/46/63 255 | f 23/16/64 16/6/64 24/33/64 256 | f 16/6/65 6/26/65 24/33/65 257 | f 17/15/66 22/27/66 6/26/66 258 | f 17/15/67 20/14/67 22/27/67 259 | f 20/14/68 5/23/68 22/27/68 260 | f 20/12/69 21/24/69 5/23/69 261 | f 20/12/70 18/11/70 21/24/70 262 | f 18/11/71 4/20/71 21/24/71 263 | f 18/9/72 19/21/72 4/20/72 264 | f 18/9/73 13/8/73 19/21/73 265 | f 13/8/74 3/61/74 19/21/74 266 | f 16/6/75 17/62/75 6/26/75 267 | f 16/6/76 14/5/76 17/62/76 268 | f 14/5/77 1/63/77 17/62/77 269 | f 13/3/78 15/18/78 3/17/78 270 | f 13/3/79 14/2/79 15/18/79 271 | f 14/2/80 2/4/80 15/18/80 272 | -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/assets/textures/comforter.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yjy0625/equibot/dc70059191beb29903d78fc4d0193392be5134fc/equibot/envs/sim_mobile/assets/textures/comforter.png -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/assets/textures/dark_carpet.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yjy0625/equibot/dc70059191beb29903d78fc4d0193392be5134fc/equibot/envs/sim_mobile/assets/textures/dark_carpet.jpg -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/assets/textures/wood.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yjy0625/equibot/dc70059191beb29903d78fc4d0193392be5134fc/equibot/envs/sim_mobile/assets/textures/wood.jpg -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/closing_env.py: -------------------------------------------------------------------------------- 1 | import os 2 | import re 3 | import pybullet as p 4 | import numpy as np 5 | from tempfile import NamedTemporaryFile 6 | 7 | from equibot.envs.sim_mobile.base_env import BaseEnv 8 | from equibot.envs.sim_mobile.utils.init_utils import rotate_around_z 9 | 10 | 11 | def evaluate_and_replace_expressions(input_file, output_file, local_vars): 12 | with open(input_file, "r") as file: 13 | content = file.read() 14 | 15 | def eval_expression(match): 16 | expression = match.group(1) 17 | try: 18 | locals().update(local_vars) 19 | return str(eval(expression)) 20 | except Exception as e: 21 | return f"ERROR: {e}" 22 | 23 | # find all "${...}" expressions and replace with evaluated results 24 | processed_content = re.sub(r"\${(.*?)}", eval_expression, content) 25 | with open(output_file, "w") as file: 26 | file.write(processed_content) 27 | 28 | 29 | class ClosingEnv(BaseEnv): 30 | BASE_INIT_ROT = 0 31 | OTHER_BASE_INIT_ROT = np.pi 32 | 33 | @property 34 | def robot_config(self): 35 | L, W, H = self._box_size 36 | left_pos = np.array([-L / 2 - L * 0.35, W * 0.1, 0.005]) 37 | right_pos = np.array([L / 2 + L * 0.35, W * 0.1, 0.005]) 38 | init_base_pos = np.stack([left_pos, right_pos]) 39 | init_base_pos[0, 0] -= 0.75 40 | init_base_pos[1, 0] += 0.75 41 | init_base_pos[0, 0] -= L / 3 42 | init_base_pos[1, 0] += L / 3 43 | init_base_pos = rotate_around_z(init_base_pos, self._object_rotation[-1]) 44 | init_base_pos[:, :2] += self.scene_offset[None] 45 | init_base_rot = [self._object_rotation[-1] + np.pi, self._object_rotation[-1]] 46 | rest_arm_pos = np.array([0.75, 0.0, H * 1.0 - self.ARM_MOUNTING_HEIGHT]) 47 | rest_arm_rot = np.array([np.pi * 0.5, np.pi, np.pi / 2]) 48 | robots = [ 49 | { 50 | "sim_robot_name": "kinova", 51 | "rest_base_pose": np.array( 52 | [init_base_pos[0, 0], init_base_pos[0, 1], init_base_rot[0]] 53 | ), 54 | "rest_arm_pos": rest_arm_pos, 55 | "rest_arm_rot": rest_arm_rot, 56 | }, 57 | { 58 | "sim_robot_name": "kinova", 59 | "rest_base_pose": np.array( 60 | [init_base_pos[1, 0], init_base_pos[1, 1], init_base_rot[1]] 61 | ), 62 | "rest_arm_pos": rest_arm_pos, 63 | "rest_arm_rot": rest_arm_rot, 64 | }, 65 | ] 66 | return robots 67 | 68 | @property 69 | def default_camera_config(self): 70 | cfg = super().default_camera_config 71 | cfg["pitch"] = -45 72 | cfg["yaw"] = 45 73 | cfg["distance"] = 1.5 * np.max(self._box_size / np.array([0.2, 0.2, 0.1])) 74 | cfg["target"] = [self.scene_offset[0], self.scene_offset[1], 0.1] 75 | return cfg 76 | 77 | @property 78 | def rigid_objects(self): 79 | return [] 80 | 81 | @property 82 | def anchor_config(self): 83 | return [] 84 | 85 | @property 86 | def soft_objects(self): 87 | return [] 88 | 89 | @property 90 | def name(self): 91 | return "close" 92 | 93 | def visualize_anchor(self, pos): 94 | return super().visualize_anchor(pos + np.array([[0, 0, 0.09]])) 95 | 96 | def visualize_pc(self, pos): 97 | return super().visualize_pc(pos + np.array([[0, 0, 0.09]])) 98 | 99 | def _init_robots(self): 100 | self._box_size = np.array([0.0, 0.0, 0.0]) 101 | super()._init_robots() 102 | 103 | def _reset_sim(self): 104 | # constants 105 | box_size = np.array([0.145, 0.12, 0.115]) * self._rigid_object_scale 106 | self._box_size = box_size 107 | self._box_thickness = 0.005 108 | 109 | super()._reset_sim() 110 | 111 | # add box to sim 112 | self.rigid_ids.append(self._init_box()) 113 | self._rigid_graspable.append(True) 114 | 115 | def _init_box(self): 116 | dir_path = os.path.dirname(__file__) 117 | template_path = os.path.join(dir_path, "assets/closing/template.urdf") 118 | local_vars = dict( 119 | L=self._box_size[0], 120 | W=self._box_size[1], 121 | H=self._box_size[2], 122 | T=self._box_thickness, 123 | ) 124 | with NamedTemporaryFile(mode="w", suffix=".urdf") as f: 125 | evaluate_and_replace_expressions(template_path, f.name, local_vars) 126 | box_id = p.loadURDF( 127 | f.name, 128 | np.zeros([3]), 129 | useFixedBase=True, 130 | flags=p.URDF_MAINTAIN_LINK_ORDER 131 | | p.URDF_USE_SELF_COLLISION 132 | | p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS, 133 | ) 134 | 135 | p.changeVisualShape(box_id, -1, rgbaColor=[0.678, 0.573, 0.439, 1.0]) 136 | 137 | init_pos = np.array([0, 0, self._box_size[2] / 2]) 138 | init_pos[:2] += self.scene_offset 139 | rotation_quaternion = p.getQuaternionFromEuler( 140 | [0, 0, self._object_rotation[-1]] 141 | ) 142 | p.resetBasePositionAndOrientation(box_id, init_pos, rotation_quaternion) 143 | 144 | for joint_index in range(8): 145 | p.changeDynamics( 146 | box_id, 147 | joint_index, 148 | lateralFriction=0.0, 149 | spinningFriction=0.0, 150 | rollingFriction=0.0, 151 | linearDamping=0.0, 152 | angularDamping=0.0, 153 | ) 154 | p.setJointMotorControl2( 155 | box_id, joint_index, controlMode=p.VELOCITY_CONTROL, force=0 156 | ) 157 | 158 | return box_id 159 | 160 | def _get_obs(self, dummy_obs=False): 161 | obs = super()._get_obs(dummy_obs=dummy_obs) 162 | return obs 163 | 164 | def compute_reward(self): 165 | joint_states = [p.getJointState(self.rigid_ids[0], ix)[0] for ix in [5, 6, 7]] 166 | return np.clip( 167 | 1.0 - np.abs(np.array(joint_states) - 3.14).mean() / 3.14, 0.0, 1.0 168 | ) 169 | 170 | def _get_rigid_body_mesh(self, obj_id, link_index=None): 171 | if obj_id in self.rigid_ids: 172 | mesh_vertices = [] 173 | num_links = p.getNumJoints(obj_id) 174 | link_idxs = list(range(num_links)) if link_index is None else [link_index] 175 | for link_idx in link_idxs: 176 | col_data = p.getCollisionShapeData(obj_id, link_idx) 177 | size = col_data[0][3] 178 | local_pos = col_data[0][5] 179 | link_state = p.getLinkState(obj_id, link_idx) 180 | pos, ori = link_state[0], link_state[1] 181 | 182 | get_num_pts = lambda s: ( 183 | 20 if s > self._box_thickness * 4 else 2 184 | ) # max(2, np.round(s / 0.01).astype(int)) 185 | for dx in np.linspace( 186 | -0.5 * size[0], 0.5 * size[0], get_num_pts(size[0]) 187 | ): 188 | for dy in np.linspace( 189 | -0.5 * size[1], 0.5 * size[1], get_num_pts(size[1]) 190 | ): 191 | for dz in np.linspace( 192 | -0.5 * size[2], 0.5 * size[2], get_num_pts(size[2]) 193 | ): 194 | dxyz = tuple((np.array([dx, dy, dz]) + local_pos).tolist()) 195 | vertex = p.multiplyTransforms(pos, ori, dxyz, [0, 0, 0, 1])[ 196 | 0 197 | ] 198 | mesh_vertices.append(vertex) 199 | verts = np.array(mesh_vertices) 200 | verts[:, :2] += self.scene_offset 201 | return verts 202 | else: 203 | return super()._get_rigid_body_mesh(obj_id) 204 | -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/covering_env.py: -------------------------------------------------------------------------------- 1 | import os 2 | import pybullet 3 | import numpy as np 4 | import trimesh 5 | 6 | from equibot.envs.sim_mobile.base_env import BaseEnv 7 | from equibot.envs.sim_mobile.utils.init_utils import scale_mesh, rotate_around_z 8 | 9 | 10 | class CoveringEnv(BaseEnv): 11 | @property 12 | def name(self): 13 | return "covering" 14 | 15 | @property 16 | def robot_config(self): 17 | init_base_pos = np.array([[-0.2, -0.1, 0.005], [+0.2, -0.1, 0.005]]) 18 | init_base_pos[:, :2] *= self._soft_object_scale[None] 19 | init_base_pos[0, 0] -= 0.7 / np.sqrt(2) 20 | init_base_pos[1, 0] += 0.7 / np.sqrt(2) 21 | init_base_pos[0, 1] -= 0.7 / np.sqrt(2) 22 | init_base_pos[1, 1] -= 0.7 / np.sqrt(2) 23 | init_base_pos = rotate_around_z(init_base_pos, self._object_rotation[-1]) 24 | init_base_pos[:, :2] += self.scene_offset[None] 25 | init_base_rot = [ 26 | self._object_rotation[-1] + np.pi * 1.25, 27 | self._object_rotation[-1] - np.pi * 0.25, 28 | ] 29 | rest_arm_pos = np.array([0.7, 0.0, 0.01 - self.ARM_MOUNTING_HEIGHT]) 30 | rest_arm_rot = np.array([np.pi * 0.6, 0.0, np.pi / 2]) 31 | robots = [ 32 | { 33 | "sim_robot_name": "kinova", 34 | "rest_base_pose": np.array( 35 | [init_base_pos[0, 0], init_base_pos[0, 1], init_base_rot[0]] 36 | ), 37 | "rest_arm_pos": rest_arm_pos, 38 | "rest_arm_rot": rest_arm_rot, 39 | }, 40 | { 41 | "sim_robot_name": "kinova", 42 | "rest_base_pose": np.array( 43 | [init_base_pos[1, 0], init_base_pos[1, 1], init_base_rot[1]] 44 | ), 45 | "rest_arm_pos": rest_arm_pos, 46 | "rest_arm_rot": rest_arm_rot, 47 | }, 48 | ] 49 | return robots 50 | 51 | def _randomize_object_scales(self): 52 | # override object scale variable so default size is equal to source size 53 | super()._randomize_object_scales() 54 | self._rigid_object_scale *= np.array([1.0, 0.7, 0.5]) * 1 55 | self._soft_object_scale *= np.array([0.6875, 0.6875]) * 1 56 | 57 | @property 58 | def default_camera_config(self): 59 | cfg = super().default_camera_config 60 | cfg["pitch"] = -75 61 | cfg["distance"] = np.max(self._soft_object_scale) * 2 62 | cfg["target"] = [self.scene_offset[0], self.scene_offset[1], 0.1] 63 | return cfg 64 | 65 | @property 66 | def anchor_config(self): 67 | return [] 68 | 69 | @property 70 | def rigid_objects(self): 71 | ang = self._object_rotation[-1] 72 | self._box_size = 0.05 73 | print(f"Randomizing rigid object scale to {self._rigid_object_scale}!") 74 | obj_path = os.path.join(self.data_path, "covering/box.obj") 75 | obj_path = scale_mesh(obj_path, self._rigid_object_scale) 76 | obj_path = "/".join(obj_path.split("/")[-2:]) 77 | return [ 78 | { 79 | "path": obj_path, 80 | "scale": 1.0, 81 | "pos": [ 82 | self.scene_offset[0], 83 | self.scene_offset[1], 84 | self._box_size * self._rigid_object_scale[-1], 85 | ], 86 | "orn": self._object_rotation, 87 | } 88 | ] 89 | 90 | @property 91 | def soft_objects(self): 92 | scale = 2.0 93 | mass = 0.2 94 | collision_margin = 0.001 95 | xy_scale = self._soft_object_scale 96 | print(f"Randomizing object scale to {xy_scale}!") 97 | obj_path = os.path.join(self.data_path, "folding/towel.obj") 98 | obj_path = scale_mesh(obj_path, np.array([xy_scale[0], xy_scale[1], 1.0])) 99 | obj_path = "/".join(obj_path.split("/")[-2:]) 100 | ang = self._object_rotation[-1] 101 | init_y = ( 102 | -0.3 * self._soft_object_scale[1] * np.cos(ang) + self.scene_offset[1] 103 | ) 104 | return [ 105 | { 106 | "path": obj_path, 107 | "scale": scale, 108 | "pos": [ 109 | 0.3 * self._soft_object_scale[1] * np.sin(ang) 110 | + self.scene_offset[0], 111 | init_y, 112 | 0.001, 113 | ], 114 | "orn": self._object_rotation, 115 | "mass": mass, 116 | "collision_margin": collision_margin, 117 | } 118 | ] 119 | 120 | def compute_reward(self): 121 | obj_id = self.soft_ids[0] 122 | cloth_mesh_xyzs = np.array(self.sim.getMeshData(obj_id)[1]) 123 | cloth_vol = trimesh.convex.convex_hull(cloth_mesh_xyzs) 124 | rigid_mesh_xyzs = self._get_rigid_body_mesh(self.rigid_ids[0]) 125 | rigid_vol = trimesh.convex.convex_hull(rigid_mesh_xyzs) 126 | rigid_volume = rigid_vol.volume 127 | intersect_volume = rigid_vol.intersection(cloth_vol).volume 128 | return intersect_volume / rigid_volume 129 | 130 | def _reset_sim(self): 131 | self.args.deform_elastic_stiffness = 100.0 132 | self.args.deform_friction_coeff = 1.0 133 | super()._reset_sim() 134 | 135 | # make floor friction higher 136 | pybullet.changeDynamics(self.rigid_ids[0], -1, lateralFriction=0.3) 137 | 138 | # preload box mesh vertices 139 | # this is required because pybullet by default doesn't save all vertices 140 | # of the object 141 | obj_path = os.path.join(self.data_path, "covering/box.obj") 142 | mesh = trimesh.load(obj_path).vertices * self._rigid_object_scale 143 | self._box_vertices = mesh 144 | 145 | texture_id = self.sim.loadTexture("textures/comforter.png") 146 | self.sim.changeVisualShape( 147 | self.soft_ids[0], -1, rgbaColor=[1, 1, 1, 1], textureUniqueId=texture_id 148 | ) 149 | 150 | def _get_rigid_body_mesh(self, obj_id): 151 | assert obj_id == self.rigid_ids[0] 152 | mesh = self._box_vertices.copy() 153 | mesh = rotate_around_z(mesh, self._object_rotation[-1]) 154 | mesh += np.array(self._rigid_objects[0]["pos"])[None] 155 | return mesh 156 | -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/folding_env.py: -------------------------------------------------------------------------------- 1 | import os 2 | import numpy as np 3 | 4 | from equibot.envs.sim_mobile.base_env import BaseEnv 5 | from equibot.envs.sim_mobile.utils.init_utils import scale_mesh, rotate_around_z 6 | 7 | 8 | class FoldingEnv(BaseEnv): 9 | BASE_INIT_ROT = 0 10 | OTHER_BASE_INIT_ROT = np.pi 11 | INITIAL_HEIGHT = 0.02 12 | 13 | @property 14 | def robot_config(self): 15 | init_base_pos = np.array([[0.21, 0.2, 0.01], [-0.21, 0.2, 0.01]]) 16 | init_base_pos[:, :2] *= self._soft_object_scale[None] 17 | init_base_pos[0, 0] += 0.7 / np.sqrt(2) 18 | init_base_pos[0, 1] += 0.7 / np.sqrt(2) 19 | init_base_pos[1, 0] -= 0.7 / np.sqrt(2) 20 | init_base_pos[1, 1] += 0.7 / np.sqrt(2) 21 | init_base_pos = rotate_around_z(init_base_pos, self._object_rotation[-1]) 22 | init_base_pos[:, :2] += self.scene_offset[None] 23 | init_base_rot = [ 24 | self._object_rotation[-1] + np.pi / 4, 25 | self._object_rotation[-1] + np.pi / 4 * 3, 26 | ] 27 | rest_arm_pos = np.array( 28 | [0.7, 0.0, self.INITIAL_HEIGHT - self.ARM_MOUNTING_HEIGHT] 29 | ) 30 | rest_arm_rot = np.array([[np.pi * 0.6, 0.0, np.pi / 2]] * 2) 31 | robots = [ 32 | { 33 | "sim_robot_name": "kinova", 34 | "rest_base_pose": np.array( 35 | [init_base_pos[0, 0], init_base_pos[0, 1], init_base_rot[0]] 36 | ), 37 | "rest_arm_pos": rest_arm_pos, 38 | "rest_arm_rot": rest_arm_rot[0], 39 | }, 40 | { 41 | "sim_robot_name": "kinova", 42 | "rest_base_pose": np.array( 43 | [init_base_pos[1, 0], init_base_pos[1, 1], init_base_rot[1]] 44 | ), 45 | "rest_arm_pos": rest_arm_pos, 46 | "rest_arm_rot": rest_arm_rot[1], 47 | }, 48 | ] 49 | return robots 50 | 51 | @property 52 | def rigid_objects(self): 53 | return [] 54 | 55 | @property 56 | def default_camera_config(self): 57 | cfg = super().default_camera_config 58 | cfg["pitch"] = -75 59 | cfg["distance"] = np.max(self._soft_object_scale) * 2 60 | cfg["target"] = [self.scene_offset[0], self.scene_offset[1], 0.1] 61 | return cfg 62 | 63 | @property 64 | def anchor_config(self): 65 | return [] 66 | 67 | @property 68 | def soft_objects(self): 69 | xy_scale = self._soft_object_scale 70 | print(f"Randomizing object scale to {xy_scale}!") 71 | obj_path = os.path.join( 72 | self.data_path, 73 | "folding/towel_small.obj" 74 | ) 75 | obj_path = scale_mesh(obj_path, np.array([xy_scale[0], xy_scale[1], 1.0])) 76 | obj_path = "/".join(obj_path.split("/")[-2:]) 77 | return [ 78 | { 79 | "path": obj_path, 80 | "scale": 2.0, 81 | "pos": [self.scene_offset[0], self.scene_offset[1], 0.001], 82 | "orn": self._object_rotation, 83 | "mass": 0.5, 84 | "collision_margin": 0.005, 85 | } 86 | ] 87 | 88 | @property 89 | def name(self): 90 | return "fold" 91 | 92 | def _randomize_object_scales(self): 93 | super()._randomize_object_scales() 94 | self._soft_object_scale *= np.array([0.6875, 0.6875]) 95 | 96 | def _reset_sim(self): 97 | super()._reset_sim() 98 | 99 | vertices = self.sim.getMeshData(self.soft_ids[0])[1] 100 | self._corner_idxs = self._get_corner_vertex_indices(self.soft_ids[0]) 101 | mesh_xyzs = np.array(self.sim.getMeshData(self.soft_ids[0])[1]) 102 | self._init_corner_positions = mesh_xyzs[self._corner_idxs] 103 | 104 | texture_id = self.sim.loadTexture("textures/comforter.png") 105 | self.sim.changeVisualShape( 106 | self.soft_ids[0], -1, rgbaColor=[1, 1, 1, 1], textureUniqueId=texture_id 107 | ) 108 | 109 | def compute_reward(self): 110 | obj_id = self.soft_ids[0] 111 | mesh_xyzs = np.array(self.sim.getMeshData(obj_id)[1]) 112 | corner_positions = mesh_xyzs[self._corner_idxs] 113 | dist = 0.0 114 | dist += np.linalg.norm(corner_positions[1] - self._init_corner_positions[0]) / 2 115 | dist += np.linalg.norm(corner_positions[3] - self._init_corner_positions[2]) / 2 116 | min_dist = self.INITIAL_HEIGHT 117 | max_dist = self._soft_object_scale[1] * 0.4 118 | dist = 1.0 - max(0, dist - min_dist) / (max_dist - min_dist) 119 | return max(0.0, min(1.0, dist)) 120 | 121 | def _get_corner_vertex_indices(self, obj_id): 122 | mesh_xyzs = np.array(self.sim.getMeshData(obj_id)[1]) 123 | mesh_xyzs = rotate_around_z(mesh_xyzs, -self._object_rotation[-1]) 124 | max_xy = np.max(mesh_xyzs, axis=0)[:2] 125 | min_xy = np.min(mesh_xyzs, axis=0)[:2] 126 | 127 | minx = np.where(mesh_xyzs[:, 0] <= min_xy[0] + 0.001)[0] 128 | maxx = np.where(mesh_xyzs[:, 0] >= max_xy[0] - 0.001)[0] 129 | miny = np.where(mesh_xyzs[:, 1] <= min_xy[1] + 0.001)[0] 130 | maxy = np.where(mesh_xyzs[:, 1] >= max_xy[1] - 0.001)[0] 131 | 132 | find_overlapping_index = lambda a, b: list(set(a).intersection(set(b)))[0] 133 | corners = [ 134 | find_overlapping_index(minx, miny), 135 | find_overlapping_index(minx, maxy), 136 | find_overlapping_index(maxx, miny), 137 | find_overlapping_index(maxx, maxy), 138 | ] 139 | return corners 140 | -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/utils/anchors.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import pybullet 3 | 4 | from .plan_control_traj import plan_min_jerk_trajectory 5 | 6 | 7 | def make_anchors(anchor_config, sim): 8 | anchor_ids = [] 9 | for anchor_info in anchor_config: 10 | radius, rgba = anchor_info["radius"], anchor_info["rgba"] 11 | mass, pos = 0.0, anchor_info["pos"] 12 | visual_shape = sim.createVisualShape( 13 | pybullet.GEOM_SPHERE, radius=radius, rgbaColor=rgba 14 | ) 15 | anchor_id = sim.createMultiBody( 16 | baseMass=mass, 17 | basePosition=pos, 18 | baseCollisionShapeIndex=-1, 19 | baseVisualShapeIndex=visual_shape, 20 | useMaximalCoordinates=True, 21 | ) 22 | anchor_ids.append(anchor_id) 23 | return anchor_ids 24 | 25 | 26 | def get_closest_mesh_vertex(pos, obj_ids, sim): 27 | """Get the closest point from a position among several meshes.""" 28 | min_dist, selected_obj_id, selected_vertex_id = np.inf, None, None 29 | kwargs = {} 30 | if hasattr(pybullet, "MESH_DATA_SIMULATION_MESH"): 31 | kwargs["flags"] = pybullet.MESH_DATA_SIMULATION_MESH 32 | for obj_id in obj_ids: 33 | _, mesh_vertices = sim.getMeshData(obj_id, **kwargs) 34 | vertex_id = get_closest(pos, mesh_vertices)[0] 35 | vertex_pos = np.array(mesh_vertices[vertex_id]) 36 | vertex_dist = np.linalg.norm(pos - vertex_pos) 37 | if vertex_dist < min_dist: 38 | selected_obj_id = obj_id 39 | selected_vertex_id = vertex_id 40 | min_dist = vertex_dist 41 | if min_dist > 0.25: 42 | return None, None 43 | return selected_obj_id, selected_vertex_id 44 | 45 | 46 | def get_closest(point, vertices, max_dist=None): 47 | """Find mesh points closest to the given point.""" 48 | point = np.array(point).reshape(1, -1) 49 | vertices = np.array(vertices) 50 | num_pins_per_pt = max(1, vertices.shape[0] // 50) 51 | num_to_pin = min(vertices.shape[0], num_pins_per_pt) 52 | dists = np.linalg.norm(vertices - point, axis=1) 53 | closest_ids = np.argpartition(dists, num_to_pin)[0:num_to_pin] 54 | if max_dist is not None: 55 | closest_ids = closest_ids[dists[closest_ids] <= max_dist] 56 | return closest_ids 57 | 58 | 59 | def create_trajectory(waypoints, steps_per_waypoint, frequency): 60 | """Creates a smoothed trajectory through the given waypoints.""" 61 | assert len(waypoints) == len(steps_per_waypoint) 62 | num_wpts = len(waypoints) 63 | tot_steps = sum(steps_per_waypoint[:-1]) 64 | dt = 1.0 / frequency 65 | traj = np.zeros([tot_steps, 3 + 3]) # 3D pos , 3D vel 66 | prev_pos = waypoints[0] # start at the 0th waypoint 67 | t = 0 68 | for wpt in range(1, num_wpts): 69 | tgt_pos = waypoints[wpt] 70 | dur = steps_per_waypoint[wpt - 1] 71 | if dur == 0: 72 | continue 73 | Y, Yd, Ydd = plan_min_jerk_trajectory(prev_pos, tgt_pos, dur, dt) 74 | traj[t : t + dur, 0:3] = Y[:] 75 | traj[t : t + dur, 3:6] = Yd[:] # vel 76 | # traj[t:t+dur,6:9] = Ydd[:] # acc 77 | t += dur 78 | prev_pos = tgt_pos 79 | if t < tot_steps: 80 | traj[t:, :] = traj[t - 1, :] # set rest to last entry 81 | # print('create_trajectory(): traj', traj) 82 | return traj 83 | -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/utils/console.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | from contextlib import contextmanager 4 | 5 | 6 | @contextmanager 7 | def suppress_stdout(): 8 | fd = sys.stdout.fileno() 9 | 10 | def _redirect_stdout(to): 11 | sys.stdout.close() # + implicit flush() 12 | os.dup2(to.fileno(), fd) # fd writes to 'to' file 13 | sys.stdout = os.fdopen(fd, "w") # Python writes to fd 14 | 15 | with os.fdopen(os.dup(fd), "w") as old_stdout: 16 | with open(os.devnull, "w") as file: 17 | _redirect_stdout(to=file) 18 | try: 19 | yield # allow code to be run with the redirected stdout 20 | finally: 21 | _redirect_stdout(to=old_stdout) # restore stdout. 22 | -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/utils/convert_coords.py: -------------------------------------------------------------------------------- 1 | """ 2 | Utilities to convert between local coordinates that are relative to the 3 | base and global world coordinates. 4 | 5 | @contactrika 6 | 7 | """ 8 | 9 | import numpy as np 10 | import pybullet 11 | 12 | 13 | def wrap_angle(x): # wrap to range [-pi, pi] 14 | return np.mod(np.array(x) + np.pi, np.pi * 2) - np.pi 15 | 16 | 17 | def flip_rot(rot): 18 | flipped_rot = rot + np.pi 19 | if flipped_rot > np.pi: 20 | flipped_rot = rot - np.pi 21 | return flipped_rot 22 | 23 | 24 | def global_to_local_pose( 25 | global_pos, global_quat, base_xy, base_rot, height_offset, debug=False 26 | ): 27 | # Convert the global pose (in world coordinates) to local coordinates 28 | # that are relative to the base. 29 | # github.com/bulletphysics/bullet3/blob/master/examples/pybullet/gym/ 30 | # pybullet_envs/minitaur/robots/utilities/kinematics_utils.py 31 | # link_position_in_base_frame() 32 | arm_ori = [0, 0, np.pi] # x for base is -x for arm in reality 33 | base_arm_pos, base_arm_quat = pybullet.multiplyTransforms( 34 | [*base_xy, height_offset], 35 | pybullet.getQuaternionFromEuler([0, 0, base_rot]), 36 | [0, 0, 0], 37 | pybullet.getQuaternionFromEuler(arm_ori), 38 | ) 39 | inv_trans, inv_rot = pybullet.invertTransform(base_arm_pos, base_arm_quat) 40 | local_pos, local_quat = pybullet.multiplyTransforms( 41 | inv_trans, inv_rot, global_pos, global_quat 42 | ) 43 | local_ori = wrap_angle(pybullet.getEulerFromQuaternion(local_quat)) 44 | if debug: 45 | print( 46 | "global_to_local_pose: base_xy", 47 | base_xy, 48 | "base_rot deg", 49 | base_rot / np.pi * 180, 50 | "\nglobal_pos", 51 | global_pos, 52 | "global_ori deg", 53 | np.array(pybullet.getEulerFromQuaternion(global_quat)) / np.pi * 180, 54 | "\nlocal_pos", 55 | local_pos, 56 | "local_ori deg", 57 | np.array(local_ori) / np.pi * 180, 58 | ) 59 | return local_pos, local_ori, local_quat 60 | 61 | 62 | def local_to_global_pose( 63 | local_pos, local_ori, base_xy, base_rot, height_offset, debug=False 64 | ): 65 | # Converts the given local pos and ori to global coordinates. 66 | arm_ori = [0, 0, np.pi] # x for base is -x for arm in reality 67 | base_arm_pos, base_arm_quat = pybullet.multiplyTransforms( 68 | [*base_xy, height_offset], 69 | pybullet.getQuaternionFromEuler([0, 0, base_rot]), 70 | [0, 0, 0], 71 | pybullet.getQuaternionFromEuler(arm_ori), 72 | ) 73 | local_quat = pybullet.getQuaternionFromEuler(local_ori) 74 | global_pos, global_quat = pybullet.multiplyTransforms( 75 | base_arm_pos, base_arm_quat, local_pos, local_quat 76 | ) 77 | global_ori = wrap_angle(pybullet.getEulerFromQuaternion(global_quat)) 78 | if debug: 79 | print( 80 | "local_to_global_pose: base_xy", 81 | base_xy, 82 | "base_rot deg", 83 | base_rot / np.pi * 180, 84 | "\nlocal_pos", 85 | local_pos, 86 | "local_ori deg", 87 | np.array(pybullet.getEulerFromQuaternion(local_quat)) / np.pi * 180, 88 | "\nglobal_pos", 89 | global_pos, 90 | "global_ori deg", 91 | np.array(global_ori) / np.pi * 180, 92 | ) 93 | return global_pos, global_ori, global_quat 94 | -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/utils/frame_converter.py: -------------------------------------------------------------------------------- 1 | """ 2 | Utilities to convert coordinate frames for odometry from cameras. 3 | 4 | @yjy0625, @jimmyyhwu. 5 | 6 | """ 7 | 8 | import numpy as np 9 | 10 | 11 | def wrap_angle(x, ref): 12 | return ref + np.mod(x - ref + np.pi, np.pi * 2) - np.pi 13 | 14 | 15 | class CoordFrameConverter(object): 16 | # Author: Jimmy Wu. Date: September 2022. 17 | def __init__(self, pose_in_map=np.zeros(3), pose_in_odom=np.zeros(3)): 18 | self.origin = None 19 | self.basis = None 20 | self.update(pose_in_map, pose_in_odom) 21 | 22 | def update(self, pose_in_map, pose_in_odom): 23 | self.basis = pose_in_map[2] - pose_in_odom[2] 24 | dx = pose_in_odom[0] * np.cos(self.basis) - pose_in_odom[1] * np.sin(self.basis) 25 | dy = pose_in_odom[0] * np.sin(self.basis) + pose_in_odom[1] * np.cos(self.basis) 26 | self.origin = (pose_in_map[0] - dx, pose_in_map[1] - dy) 27 | 28 | def convert_position(self, position): 29 | x, y = position 30 | x = x - self.origin[0] 31 | y = y - self.origin[1] 32 | xp = x * np.cos(-self.basis) - y * np.sin(-self.basis) 33 | yp = x * np.sin(-self.basis) + y * np.cos(-self.basis) 34 | return (xp, yp) 35 | 36 | def convert_heading(self, th): 37 | return th - self.basis 38 | 39 | def convert_pose(self, pose): 40 | x, y, th = pose 41 | return (*self.convert_position((x, y)), self.convert_heading(th)) 42 | -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/utils/info.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import pybullet 3 | 4 | 5 | DEFAULT_ANCHOR_CONFIG = [ 6 | {"pos": np.array([0.0, 0.0, 0.0]), "radius": 0.02, "rgba": (0.0, 0.0, 0.0, 0.0)}, 7 | {"pos": np.array([1.0, 0.0, 0.0]), "radius": 0.02, "rgba": (1.0, 0.0, 0.0, 0.0)}, 8 | {"pos": np.array([0.0, 1.0, 0.0]), "radius": 0.02, "rgba": (1.0, 0.3, 0.0, 0.0)}, 9 | {"pos": np.array([0.0, 0.0, -10.0]), "radius": 0.02, "rgba": (0.3, 0.8, 0.6, 1.0)}, 10 | ] 11 | 12 | 13 | KINOVA_HOME_QPOS = np.array([0.0, 0.26, 3.14, -2.27, 0.0, 0.96, 1.57]) 14 | 15 | 16 | SIM_ROBOT_INFO = { 17 | "kinova": { 18 | "file_name": "kinova/base_with_kinova_gripper.urdf", 19 | "ee_joint_name": "end_effector", 20 | "ee_link_name": "tool_frame", 21 | "rest_arm_qpos": KINOVA_HOME_QPOS, 22 | }, 23 | "kinova_tta": { 24 | "file_name": "kinova/base_with_kinova_tta.urdf", 25 | "ee_joint_name": "end_effector", 26 | "ee_link_name": "tool_frame", 27 | "rest_arm_qpos": KINOVA_HOME_QPOS, 28 | }, 29 | "kinova_ladle": { 30 | "file_name": "kinova/base_with_kinova_ladle.urdf", 31 | "ee_joint_name": "end_effector", 32 | "ee_link_name": "tool_frame", 33 | "rest_arm_qpos": KINOVA_HOME_QPOS, 34 | }, 35 | "kinova_pan": { 36 | "file_name": "kinova/base_with_kinova_pan.urdf", 37 | "ee_joint_name": "end_effector", 38 | "ee_link_name": "tool_frame", 39 | "rest_arm_qpos": KINOVA_HOME_QPOS, 40 | }, 41 | } 42 | -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/utils/init_utils.py: -------------------------------------------------------------------------------- 1 | # 2 | # Utilities for deform sim in PyBullet. 3 | # 4 | # @contactrika 5 | # 6 | import os 7 | from glob import glob 8 | 9 | import numpy as np 10 | 11 | np.set_printoptions(precision=2, linewidth=150, threshold=10000, suppress=True) 12 | import pybullet 13 | import pybullet_data 14 | import pybullet_utils.bullet_client as bclient 15 | 16 | from .multi_camera import MultiCamera 17 | 18 | 19 | def load_rigid_object(sim, obj_file_name, scale, init_pos, init_ori, mass=0.0): 20 | """Load a rigid object from file, create visual and collision shapes.""" 21 | assert obj_file_name.endswith(".obj") # assume mesh info 22 | viz_shape_id = sim.createVisualShape( 23 | shapeType=pybullet.GEOM_MESH, 24 | rgbaColor=None, 25 | fileName=obj_file_name, 26 | meshScale=scale, 27 | ) 28 | col_shape_id = sim.createCollisionShape( 29 | shapeType=pybullet.GEOM_MESH, fileName=obj_file_name, meshScale=scale 30 | ) 31 | rigid_custom_id = sim.createMultiBody( 32 | baseMass=mass, # mass==0 => fixed at the position where it is loaded 33 | basePosition=init_pos, 34 | baseCollisionShapeIndex=col_shape_id, 35 | baseVisualShapeIndex=viz_shape_id, 36 | baseOrientation=pybullet.getQuaternionFromEuler(init_ori), 37 | ) 38 | return rigid_custom_id 39 | 40 | 41 | def load_soft_object( 42 | sim, 43 | obj_file_name, 44 | scale, 45 | init_pos, 46 | init_ori, 47 | bending_stiffness, 48 | damping_stiffness, 49 | elastic_stiffness, 50 | friction_coeff, 51 | mass=1.0, 52 | collision_margin=0.002, 53 | fuzz_stiffness=False, 54 | use_self_collision=True, 55 | debug=False, 56 | ): 57 | """Load object from obj file with pybullet's loadSoftBody().""" 58 | if fuzz_stiffness: 59 | elastic_stiffness += (np.random.rand() - 0.5) * 2 * 20 60 | bending_stiffness += (np.random.rand() - 0.5) * 2 * 20 61 | friction_coeff += (np.random.rand() - 0.5) * 2 * 0.3 62 | scale += (np.random.rand() - 0.5) * 2 * 0.2 63 | if elastic_stiffness < 10.0: 64 | elastic_stiffness = 10.0 65 | if bending_stiffness < 10.0: 66 | bending_stiffness = 10.0 67 | scale = np.clip(scale, 0.6, 1.5) 68 | print( 69 | "fuzzed", 70 | f"elastic_stiffness {elastic_stiffness:0.4f}", 71 | f"bending_stiffness {bending_stiffness:0.4f}", 72 | f"friction_coeff {friction_coeff:0.4f} scale {scale:0.4f}", 73 | ) 74 | # Note: do not set very small mass (e.g. 0.01 causes instabilities). 75 | deform_id = sim.loadSoftBody( 76 | scale=scale, 77 | mass=mass, 78 | fileName=obj_file_name, 79 | basePosition=init_pos, 80 | baseOrientation=pybullet.getQuaternionFromEuler(init_ori), 81 | collisionMargin=collision_margin, 82 | springElasticStiffness=elastic_stiffness, 83 | springDampingStiffness=damping_stiffness, 84 | springBendingStiffness=bending_stiffness, 85 | frictionCoeff=friction_coeff, 86 | useSelfCollision=use_self_collision, 87 | useNeoHookean=0, 88 | useMassSpring=1, 89 | useBendingSprings=1, 90 | ) 91 | kwargs = {} 92 | if hasattr(pybullet, "MESH_DATA_SIMULATION_MESH"): 93 | kwargs["flags"] = pybullet.MESH_DATA_SIMULATION_MESH 94 | num_mesh_vertices, _ = sim.getMeshData(deform_id, **kwargs) 95 | if debug: 96 | print("Loaded deform_id", deform_id, "with", num_mesh_vertices, "mesh vertices") 97 | # Pybullet will struggle with very large meshes, so we should keep mesh 98 | # sizes to a limited number of vertices and faces. 99 | # Large meshes will load on Linux/Ubuntu, but sim will run too slowly. 100 | # Meshes with >2^13=8196 verstices will fail to load on OS X due to shared 101 | # memory limits, as noted here: 102 | # https://github.com/bulletphysics/bullet3/issues/1965 103 | assert num_mesh_vertices < 2**13 # make sure mesh has less than ~8K verts 104 | return deform_id 105 | 106 | 107 | def create_spheres( 108 | id, 109 | radius=0.01, 110 | mass=0.0, 111 | batch_positions=[[0, 0, 0]], 112 | visual=True, 113 | collision=True, 114 | rgba=[0, 1, 1, 1], 115 | ): 116 | """ 117 | Reference: https://github.com/Healthcare-Robotics/assistive-gym/blob/ 118 | 41d7059f0df0976381b2544b6bcfc2b84e1be008/assistive_gym/envs/base_env.py#L127 119 | """ 120 | sphere_collision = -1 121 | if collision: 122 | sphere_collision = pybullet.createCollisionShape( 123 | shapeType=pybullet.GEOM_SPHERE, radius=radius, physicsClientId=id 124 | ) 125 | 126 | sphere_visual = ( 127 | pybullet.createVisualShape( 128 | shapeType=pybullet.GEOM_SPHERE, 129 | radius=radius, 130 | rgbaColor=rgba, 131 | physicsClientId=id, 132 | ) 133 | if visual 134 | else -1 135 | ) 136 | 137 | last_sphere_id = pybullet.createMultiBody( 138 | baseMass=mass, 139 | baseCollisionShapeIndex=sphere_collision, 140 | baseVisualShapeIndex=sphere_visual, 141 | basePosition=[0, 0, 0], 142 | useMaximalCoordinates=False, 143 | batchPositions=batch_positions, 144 | physicsClientId=id, 145 | ) 146 | 147 | spheres = [] 148 | for body in list( 149 | range(last_sphere_id[-1] - len(batch_positions) + 1, last_sphere_id[-1] + 1) 150 | ): 151 | # sphere = Agent() 152 | # sphere.init(body, id, self.np_random, indices=-1) 153 | spheres.append(body) 154 | return spheres 155 | 156 | 157 | def init_bullet(args, sim=None, cam_on=False, cam_configs={}): 158 | """Initialize pybullet simulation.""" 159 | curr_dir = os.path.dirname(os.path.realpath(__file__)) 160 | parent_dir = os.path.dirname(curr_dir) 161 | args.data_path = os.path.join(parent_dir, "assets") 162 | if args.viz: 163 | if sim is None: 164 | sim = bclient.BulletClient(connection_mode=pybullet.GUI) 165 | # toggle aux menus in the gui 166 | pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_GUI, cam_on) 167 | pybullet.configureDebugVisualizer( 168 | pybullet.COV_ENABLE_RGB_BUFFER_PREVIEW, cam_on 169 | ) 170 | pybullet.configureDebugVisualizer( 171 | pybullet.COV_ENABLE_DEPTH_BUFFER_PREVIEW, cam_on 172 | ) 173 | pybullet.configureDebugVisualizer( 174 | pybullet.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, cam_on 175 | ) 176 | # don't render during init 177 | pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RENDERING, 0) 178 | # Keep camera distance and target same as defaults for MultiCamera, 179 | # so that debug views look similar to those recorded by the camera pans. 180 | # Camera pitch and yaw can be set as desired, since they don't 181 | # affect MultiCamera panning strategy. 182 | cam_args = { 183 | "cameraDistance": 0.85, 184 | "cameraYaw": -30, 185 | "cameraPitch": -70, 186 | "cameraTargetPosition": np.array([0.35, 0, 0]), 187 | } 188 | cam_args.update(cam_configs) 189 | sim.resetDebugVisualizerCamera(**cam_args) 190 | else: 191 | if sim is None: 192 | sim = bclient.BulletClient(connection_mode=pybullet.DIRECT) 193 | sim.resetSimulation(pybullet.RESET_USE_DEFORMABLE_WORLD) 194 | sim.setGravity(0, 0, args.sim_gravity) 195 | sim.setTimeStep(1.0 / args.sim_frequency) 196 | return sim 197 | 198 | 199 | def scale_mesh(obj_file, scale): 200 | # Load the OBJ file 201 | with open(obj_file, "r") as f: 202 | lines = f.readlines() 203 | lines = [l.strip() for l in lines] 204 | v_lines_idxs = [k for k in range(len(lines)) if lines[k].startswith("v ")] 205 | v_lines = [lines[idx] for idx in v_lines_idxs] 206 | vertices = [[float(x) for x in l.split(" ")[1:]] for l in v_lines] 207 | vertices = np.array(vertices) 208 | vertices = vertices * np.array(scale)[None] 209 | for i, idx in enumerate(v_lines_idxs): 210 | x, y, z = vertices[i] 211 | lines[idx] = f"v {x:.6f} {y:.6f} {z:.6f}" 212 | 213 | # Save the scaled mesh to a new OBJ file 214 | pid = os.getpid() 215 | output_file = obj_file.replace( 216 | ".obj", f"_scaled_x{scale[0]:.2f}_y{scale[1]:.2f}_pid{pid}.obj" 217 | ) 218 | existing_generated_files = glob( 219 | os.path.join(os.path.dirname(obj_file), f"*_scaled*_pid{pid}.obj") 220 | ) 221 | for f in existing_generated_files: 222 | os.remove(f) 223 | with open(output_file, "w") as f: 224 | for line in lines: 225 | f.write(f"{line}\n") 226 | return output_file 227 | 228 | 229 | def rotate_around_z( 230 | points, 231 | angle_rad=0.0, 232 | center=np.array([0.0, 0.0, 0.0]), 233 | scale=np.array([1.0, 1.0, 1.0]), 234 | ): 235 | # Check if the input points have the correct shape (N, 3) 236 | assert (len(points.shape) == 1 and len(points) == 3) or points.shape[-1] == 3 237 | p_shape = points.shape 238 | points = points.reshape(-1, 3) - center[None] 239 | 240 | # Create the rotation matrix 241 | cos_theta = np.cos(angle_rad) 242 | sin_theta = np.sin(angle_rad) 243 | rotation_matrix = np.array( 244 | [[cos_theta, -sin_theta, 0], [sin_theta, cos_theta, 0], [0, 0, 1]] 245 | ) 246 | 247 | # Apply the rotation to all points using matrix multiplication 248 | rotated_points = np.dot(points, rotation_matrix.T) * scale[None] + center[None] 249 | rotated_points = rotated_points.reshape(p_shape) 250 | 251 | return rotated_points 252 | -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/utils/multi_camera.py: -------------------------------------------------------------------------------- 1 | # 2 | # MultiCamera class that processes depth and RGB camera input from PyBullet. 3 | # The code follows basic recommendations from PyBullet forums. 4 | # Note that it uses assumptions of the camera setup, which work in the 5 | # current pybullet versions, but ultimately might change in the future. 6 | # Using pybullet versions from 2.6.4 to 2.8.1 should work fine. 7 | # 8 | # @contactrika 9 | # 10 | import os 11 | import sys 12 | import math 13 | import time 14 | 15 | import numpy as np 16 | 17 | np.set_printoptions(precision=4, linewidth=150, threshold=np.inf, suppress=True) 18 | import pybullet 19 | 20 | 21 | def assert_close(ars, ars0): 22 | for ar, ar0 in zip(ars, ars0): 23 | assert np.linalg.norm(np.array(ar) - np.array(ar0)) < 1e-6 24 | 25 | 26 | def get_camera_info(args, aux=False): 27 | if not aux: 28 | yaws, pitches = [], [] 29 | for y in args.cam_yaws: 30 | for p in args.cam_pitches: 31 | yaws.append(y) 32 | pitches.append(p) 33 | num_views = len(yaws) 34 | else: 35 | yaws = [90] 36 | pitches = [-20] 37 | num_views = 1 38 | cam_info = { 39 | "yaws": yaws, 40 | "pitches": pitches, 41 | "dist": args.cam_dist, 42 | "views": list(np.arange(num_views)), 43 | "fov": args.cam_fov, 44 | "width": args.cam_resolution, 45 | "height": args.cam_resolution, 46 | } 47 | return cam_info 48 | 49 | 50 | class MultiCamera: 51 | # In non-GUI mode we will render without X11 context but *with* GPU accel. 52 | # examples/pybullet/examples/testrender_egl.py 53 | # Note: use alpha=1 (in rgba), otherwise depth readings are not good 54 | # Using defaults from PyBullet. 55 | # See examples/pybullet/examples/pointCloudFromCameraImage.py 56 | PYBULLET_FAR_PLANE = 10000 57 | PYBULLET_NEAR_VAL = 0.01 58 | PYBULLET_FAR_VAL = 1000.0 59 | 60 | @staticmethod 61 | def init(viz): 62 | if viz: 63 | pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_GUI, 1) 64 | pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RGB_BUFFER_PREVIEW, 1) 65 | pybullet.configureDebugVisualizer( 66 | pybullet.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 1 67 | ) 68 | pybullet.configureDebugVisualizer( 69 | pybullet.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 1 70 | ) 71 | 72 | @staticmethod 73 | def get_cam_vals( 74 | cam_rolls, cam_yaws, cam_pitches, cam_dist, cam_target, fov, aspect_ratio=1.0 75 | ): 76 | # load static variables 77 | near_val = MultiCamera.PYBULLET_NEAR_VAL 78 | far_val = MultiCamera.PYBULLET_FAR_VAL 79 | far_plane = MultiCamera.PYBULLET_FAR_PLANE 80 | 81 | # compute cam vals 82 | cam_vals = [] 83 | for cam_roll, cam_yaw, cam_pitch in zip(cam_rolls, cam_yaws, cam_pitches): 84 | view_matrix = pybullet.computeViewMatrixFromYawPitchRoll( 85 | cam_target, cam_dist, cam_yaw, cam_pitch, cam_roll, upAxisIndex=2 86 | ) 87 | np_view_matrix = np.array(view_matrix).reshape(4, 4) 88 | proj_matrix = pybullet.computeProjectionMatrixFOV( 89 | fov, aspect_ratio, near_val, far_val 90 | ) 91 | forward_vec = -np_view_matrix[:3, 2] 92 | horizon = np_view_matrix[:3, 0] * far_plane * 2 * aspect_ratio 93 | vertical = np_view_matrix[:3, 1] * far_plane * 2 94 | cam_vals.append( 95 | [ 96 | view_matrix, 97 | proj_matrix, 98 | forward_vec, 99 | horizon, 100 | vertical, 101 | cam_dist, 102 | cam_target, 103 | ] 104 | ) 105 | 106 | return cam_vals 107 | 108 | def soft_ptcloud(sim, softbody_ids, debug=False): 109 | """Add SoftBody vertex positions to the point cloud.""" 110 | deform_ptcloud = [] 111 | deform_tracking_ids = [] 112 | if softbody_ids is not None: 113 | for i in range(len(softbody_ids)): 114 | num_verts, verts = sim.getMeshData(softbody_ids[i]) 115 | for v in verts: 116 | deform_ptcloud.append(np.array(v)) 117 | deform_tracking_ids.append(softbody_ids[i]) 118 | deform_ptcloud = np.array(deform_ptcloud) 119 | deform_tracking_ids = np.array(deform_tracking_ids) 120 | return deform_ptcloud, deform_tracking_ids 121 | 122 | def render( 123 | sim, 124 | object_ids, 125 | cam_rolls=[0] * 7, 126 | cam_yaws=[-30, 10, 50, 90, 130, 170, 210], 127 | cam_pitches=[-70, -10, -65, -40, -10, -25, -60], 128 | cam_dist=0.85, 129 | cam_target=np.array([0.35, 0, 0]), 130 | fov=90, 131 | views=[2], 132 | width=100, 133 | height=100, 134 | return_seg=False, 135 | return_depth=False, 136 | debug=False, 137 | ): 138 | imgs, depths, segs = [], [], [] 139 | cam_vals = MultiCamera.get_cam_vals( 140 | cam_rolls, 141 | cam_yaws, 142 | cam_pitches, 143 | cam_dist, 144 | cam_target, 145 | fov, 146 | aspect_ratio=float(width / height), 147 | ) 148 | 149 | # render views 150 | for i in views: 151 | ( 152 | view_matrix, 153 | proj_matrix, 154 | cam_forward, 155 | cam_horiz, 156 | cam_vert, 157 | cam_dist, 158 | cam_tgt, 159 | ) = cam_vals[i] 160 | w, h, rgba_px, depth_raw_cam_dists, segment_mask = sim.getCameraImage( 161 | width=width, 162 | height=height, 163 | viewMatrix=view_matrix, 164 | projectionMatrix=proj_matrix, 165 | lightDirection=np.array([0.0, 0.0, 5.0]), 166 | lightColor=np.array([1.0, 1.0, 1.0]), 167 | lightDistance=2.0, 168 | shadow=1, 169 | renderer=pybullet.ER_BULLET_HARDWARE_OPENGL, 170 | # renderer=pybullet.ER_TINY_RENDERER, 171 | flags=pybullet.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX, 172 | ) 173 | imgs.append(np.array(rgba_px).reshape(height, width, 4)) 174 | depths.append(np.array(depth_raw_cam_dists).reshape(height, width)) 175 | segs.append(np.array(segment_mask).reshape(height, width)) 176 | 177 | # prepare return 178 | return_dict = dict(images=imgs) 179 | if return_depth: 180 | for i, depth in enumerate(depths): 181 | depth = np.array(depth).reshape(height, width).T 182 | near_val = MultiCamera.PYBULLET_NEAR_VAL 183 | far_val = MultiCamera.PYBULLET_FAR_VAL 184 | depth = far_val * near_val / (far_val - (far_val - near_val) * depth) 185 | depths[i] = depth 186 | return_dict["depths"] = depths 187 | if return_seg: 188 | return_dict["segs"] = segs 189 | return return_dict 190 | -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/utils/plan_control_traj.py: -------------------------------------------------------------------------------- 1 | """ 2 | Utilities to make trajectories for smoother cotrol. 3 | 4 | From https://github.com/contactrika/bo-svae-dc/blob/master/ 5 | gym-bullet-extensions/gym_bullet_extensions/control/control_util.py 6 | 7 | Akshara Rai wrote parts of this code for ours CoRL2019 paper. 8 | 9 | @contactrika modified the code for this repo. 10 | 11 | """ 12 | 13 | import numpy as np 14 | import math 15 | import pybullet 16 | 17 | 18 | # Conforming to pybullet's conventions on reporting angular velocity. 19 | # Different from pybullet.GetQuaternionFromEuler(). 20 | def quaternion_to_euler(q): 21 | x = q[0] 22 | y = q[1] 23 | z = q[2] 24 | w = q[3] 25 | 26 | sinr_cosp = 2.0 * (w * x + y * z) 27 | cosr_cosp = 1.0 - 2.0 * (x * x + y * y) 28 | roll = math.atan2(sinr_cosp, cosr_cosp) 29 | 30 | sinp = 2.0 * (w * y - z * x) 31 | if np.abs(sinp) >= 1: 32 | pitch = np.pi / 2.0 33 | else: 34 | pitch = math.asin(sinp) 35 | 36 | siny_cosp = 2.0 * (w * z + x * y) 37 | cosy_cosp = 1.0 - 2.0 * (y * y + z * z) 38 | yaw = math.atan2(siny_cosp, cosy_cosp) 39 | 40 | return np.array([pitch, -roll, yaw]) 41 | 42 | 43 | def euler_to_quaternion(theta): 44 | """ 45 | Assuming that theta is from the quaternion to euler function above 46 | :param theta: 47 | :return: 48 | """ 49 | 50 | roll = -theta[1] 51 | pitch = theta[0] 52 | yaw = theta[2] 53 | 54 | quat = pybullet.getQuaternionFromEuler([roll, pitch, yaw]) 55 | return quat 56 | 57 | 58 | def plan_linear_orientation_trajectory(th0, th_goal, N): 59 | shortest_path = (((th_goal - th0) + np.pi) % (2.0 * np.pi)) - np.pi 60 | amount = np.linspace(0.0, 1.0, num=N) 61 | traj = np.zeros((N, th0.shape[0])) 62 | for i in range(N): 63 | traj[i] = th0 + (shortest_path * amount[i]) 64 | return traj 65 | 66 | 67 | def plan_min_jerk_trajectory(y0, goal, num_steps, freq, yd0=None): 68 | dt = 1.0 / freq 69 | dur = num_steps * dt 70 | nDim = np.shape(y0)[0] 71 | Y = np.zeros((num_steps, nDim)) 72 | Yd = np.zeros((num_steps, nDim)) 73 | Ydd = np.zeros((num_steps, nDim)) 74 | Y[0, :] = y0 75 | if yd0 is not None: 76 | Yd[0, :] = yd0 77 | rem_dur = dur 78 | for n in range(1, num_steps): 79 | y_curr = Y[n - 1, :] 80 | yd_curr = Yd[n - 1, :] 81 | ydd_curr = Ydd[n - 1, :] 82 | for d in range(nDim): 83 | Y[n, d], Yd[n, d], Ydd[n, d] = calculate_min_jerk_step( 84 | y_curr[d], yd_curr[d], ydd_curr[d], goal[d], rem_dur, dt 85 | ) 86 | rem_dur = rem_dur - dt 87 | return Y, Yd, Ydd 88 | 89 | 90 | def calculate_min_jerk_step(y_curr, yd_curr, ydd_curr, goal, rem_dur, dt): 91 | 92 | if rem_dur < 0: 93 | return 94 | 95 | if dt > rem_dur: 96 | dt = rem_dur 97 | 98 | t1 = dt 99 | t2 = t1 * dt 100 | t3 = t2 * dt 101 | t4 = t3 * dt 102 | t5 = t4 * dt 103 | 104 | dur1 = rem_dur 105 | dur2 = dur1 * rem_dur 106 | dur3 = dur2 * rem_dur 107 | dur4 = dur3 * rem_dur 108 | dur5 = dur4 * rem_dur 109 | 110 | dist = goal - y_curr 111 | a1t2 = 0.0 # goaldd 112 | a0t2 = ydd_curr * dur2 113 | v1t1 = 0.0 # goald 114 | v0t1 = yd_curr * dur1 115 | 116 | c1 = (6.0 * dist + (a1t2 - a0t2) / 2.0 - 3.0 * (v0t1 + v1t1)) / dur5 117 | c2 = ( 118 | -15.0 * dist + (3.0 * a0t2 - 2.0 * a1t2) / 2.0 + 8.0 * v0t1 + 7.0 * v1t1 119 | ) / dur4 120 | c3 = (10.0 * dist + (a1t2 - 3.0 * a0t2) / 2.0 - 6.0 * v0t1 - 4.0 * v1t1) / dur3 121 | c4 = ydd_curr / 2.0 122 | c5 = yd_curr 123 | c6 = y_curr 124 | 125 | y = c1 * t5 + c2 * t4 + c3 * t3 + c4 * t2 + c5 * t1 + c6 126 | yd = 5 * c1 * t4 + 4 * c2 * t3 + 3 * c3 * t2 + 2 * c4 * t1 + c5 127 | ydd = 20 * c1 * t3 + 12 * c2 * t2 + 6 * c3 * t1 + 2 * c4 128 | 129 | return y, yd, ydd 130 | 131 | 132 | def plan_control_traj( 133 | target_pos, 134 | target_quat, 135 | num_steps, 136 | freq, 137 | curr_pos, 138 | curr_quat, 139 | curr_vel=None, 140 | ori_type="quat", 141 | ): 142 | # Compute minimum jerk trajectory. 143 | # From https://github.com/contactrika/bo-svae-dc/blob/master/ 144 | # gym-bullet-extensions/gym_bullet_extensions/control/waypts_policy.py 145 | # Note: using custom function to convert to euler in order to match 146 | # pybullet's conventions on how angular velocity is reported. 147 | assert ori_type in ["quat", "euler"] 148 | if ori_type == "quat": 149 | curr_orient = quaternion_to_euler(curr_quat) 150 | ee_orient = quaternion_to_euler(target_quat) 151 | else: 152 | curr_orient = curr_quat 153 | ee_orient = target_quat 154 | Y, Yd, Ydd = plan_min_jerk_trajectory( # Y[:] is x,y,z traj 155 | curr_pos, target_pos, num_steps, freq, yd0=curr_vel 156 | ) 157 | th = plan_linear_orientation_trajectory( 158 | curr_orient, ee_orient, num_steps 159 | ) # 3 euler angles (custom) 160 | if ori_type == "quat": 161 | ee_quat_traj = np.zeros([num_steps, 4]) 162 | for tmp_i in range(num_steps): 163 | ee_quat_traj[tmp_i, :] = euler_to_quaternion(th[tmp_i]) 164 | return Y, ee_quat_traj 165 | else: 166 | ee_ori_traj = np.array(th) 167 | return Y, ee_ori_traj 168 | -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/utils/project.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | def vectorized_unproject( 5 | depth, intrinsics, rgb=None, depth_scale=1.0, filter_pixels=None 6 | ): 7 | # parse intrinsics and scale depth 8 | fx, fy, cx, cy = np.array(intrinsics)[[0, 1, 0, 1], [0, 1, 2, 2]] 9 | depth /= depth_scale 10 | 11 | # form the mesh grid 12 | if filter_pixels is None: 13 | xv, yv = np.meshgrid( 14 | np.arange(depth.shape[1], dtype=float), 15 | np.arange(depth.shape[0], dtype=float), 16 | ) 17 | else: 18 | xv, yv = filter_pixels[:, 1], filter_pixels[:, 0] 19 | depth = depth[filter_pixels[:, 0], filter_pixels[:, 1]] 20 | 21 | # transform coordinates and concatenate xyz on 2nd axis 22 | xv = (xv - cx) / fx * depth 23 | yv = (yv - cy) / fy * depth 24 | points = np.c_[xv.flatten(), yv.flatten(), depth.flatten()] 25 | 26 | # attach rgb values if provided 27 | if rgb is not None: 28 | rgb = rgb.reshape(-1, 3) 29 | points = np.concatenate([points, rgb], axis=1) 30 | 31 | return points 32 | 33 | 34 | def unproject_depth( 35 | depths, 36 | intrinsics, 37 | extrinsics, 38 | clip_radius=1.0, 39 | filter_xyz=True, 40 | min_x=-1.0, 41 | max_x=1.0, 42 | min_y=-1.0, 43 | max_y=1.0, 44 | min_z=-1.0, 45 | max_z=1.0, 46 | filter_pixels=None, 47 | ): 48 | """ 49 | Converts depth images to point cloud in world coordinate system. 50 | Args: 51 | depths: all depth images 52 | intrinsics: all intrinsic matrices from camera coordinates to pixels 53 | extrinsics: all extrinsic matrices from camera to world coordinates 54 | clip_radius: clipping radius for filtering out sky box 55 | filter_xyz: perform point cloud filtering by axis limits 56 | [min,max]_[x,y,z]: axis limits for point cloud items 57 | filter_pixels: only unproject depth at the specified pixels 58 | """ 59 | camera_xyzs = [] 60 | world_xyzs = [] 61 | if filter_pixels is None: 62 | filter_pixels = [None] * len(depths) 63 | for i in range(len(depths)): 64 | # filter mask 65 | if filter_pixels[i] is None: 66 | clip_mask = depths[i].flatten() < clip_radius 67 | else: 68 | clip_mask = ( 69 | depths[i][filter_pixels[i][:, 0], filter_pixels[i][:, 1]] < clip_radius 70 | ) 71 | 72 | # unproject depths into camera coordinate 73 | camera_xyz = vectorized_unproject( 74 | depths[i], intrinsics[i], filter_pixels=filter_pixels[i] 75 | ) 76 | camera_xyz_homo = np.c_[camera_xyz, np.ones(len(camera_xyz))] 77 | camera_xyzs.append(camera_xyz[clip_mask]) 78 | 79 | # convert positions to world coordinate 80 | world_xyz = np.dot(extrinsics[i], camera_xyz_homo.T).T 81 | filtered_world_xyz = world_xyz[clip_mask][:, :3] 82 | if filter_xyz: 83 | filtered_world_xyz = filtered_world_xyz[filtered_world_xyz[..., 0] > min_x] 84 | filtered_world_xyz = filtered_world_xyz[filtered_world_xyz[..., 0] < max_x] 85 | filtered_world_xyz = filtered_world_xyz[filtered_world_xyz[..., 1] > min_y] 86 | filtered_world_xyz = filtered_world_xyz[filtered_world_xyz[..., 1] < max_y] 87 | filtered_world_xyz = filtered_world_xyz[filtered_world_xyz[..., 2] > min_z] 88 | filtered_world_xyz = filtered_world_xyz[filtered_world_xyz[..., 2] < max_z] 89 | world_xyzs.append(filtered_world_xyz) 90 | 91 | return np.concatenate(world_xyzs, axis=0) 92 | -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/utils/render.py: -------------------------------------------------------------------------------- 1 | """ 2 | Utilities for rendering and visualization. 3 | 4 | Authors: @yjy0625, @contactrika 5 | 6 | """ 7 | 8 | import numpy as np 9 | import pybullet 10 | 11 | 12 | def add_debug_region(sim, mn, mx, clr=(1, 0, 0), z=0.01): 13 | sim.addUserDebugLine([mn[0], mn[1], z], [mn[0], mx[1], z], clr) 14 | sim.addUserDebugLine([mn[0], mn[1], z], [mx[0], mn[1], z], clr) 15 | sim.addUserDebugLine([mn[0], mx[1], z], [mx[0], mx[1], z], clr) 16 | sim.addUserDebugLine([mx[0], mn[1], z], [mx[0], mx[1], z], clr) 17 | -------------------------------------------------------------------------------- /equibot/envs/sim_mobile/utils/transformations.py: -------------------------------------------------------------------------------- 1 | import math 2 | 3 | import numpy as np 4 | from scipy.spatial.transform import Rotation as R 5 | 6 | 7 | PI = np.pi 8 | EPS = np.finfo(float).eps * 4.0 9 | 10 | 11 | _NEXT_AXIS = [1, 2, 0, 1] 12 | 13 | 14 | # map axes strings to/from tuples of inner axis, parity, repetition, frame 15 | _AXES2TUPLE = { 16 | "sxyz": (0, 0, 0, 0), 17 | "sxyx": (0, 0, 1, 0), 18 | "sxzy": (0, 1, 0, 0), 19 | "sxzx": (0, 1, 1, 0), 20 | "syzx": (1, 0, 0, 0), 21 | "syzy": (1, 0, 1, 0), 22 | "syxz": (1, 1, 0, 0), 23 | "syxy": (1, 1, 1, 0), 24 | "szxy": (2, 0, 0, 0), 25 | "szxz": (2, 0, 1, 0), 26 | "szyx": (2, 1, 0, 0), 27 | "szyz": (2, 1, 1, 0), 28 | "rzyx": (0, 0, 0, 1), 29 | "rxyx": (0, 0, 1, 1), 30 | "ryzx": (0, 1, 0, 1), 31 | "rxzx": (0, 1, 1, 1), 32 | "rxzy": (1, 0, 0, 1), 33 | "ryzy": (1, 0, 1, 1), 34 | "rzxy": (1, 1, 0, 1), 35 | "ryxy": (1, 1, 1, 1), 36 | "ryxz": (2, 0, 0, 1), 37 | "rzxz": (2, 0, 1, 1), 38 | "rxyz": (2, 1, 0, 1), 39 | "rzyz": (2, 1, 1, 1), 40 | } 41 | 42 | 43 | def vec(values): 44 | """ 45 | Converts value tuple into a numpy vector. 46 | 47 | Args: 48 | values (n-array): a tuple of numbers 49 | 50 | Returns: 51 | np.array: vector of given values 52 | """ 53 | return np.array(values, dtype=np.float32) 54 | 55 | 56 | def euler2mat(euler): 57 | """ 58 | Converts euler angles into rotation matrix form 59 | 60 | Args: 61 | euler (np.array): (r,p,y) angles 62 | 63 | Returns: 64 | np.array: 3x3 rotation matrix 65 | 66 | Raises: 67 | AssertionError: [Invalid input shape] 68 | """ 69 | 70 | euler = np.asarray(euler, dtype=np.float64) 71 | assert euler.shape[-1] == 3, "Invalid shaped euler {}".format(euler) 72 | 73 | ai, aj, ak = -euler[..., 2], -euler[..., 1], -euler[..., 0] 74 | si, sj, sk = np.sin(ai), np.sin(aj), np.sin(ak) 75 | ci, cj, ck = np.cos(ai), np.cos(aj), np.cos(ak) 76 | cc, cs = ci * ck, ci * sk 77 | sc, ss = si * ck, si * sk 78 | 79 | mat = np.empty(euler.shape[:-1] + (3, 3), dtype=np.float64) 80 | mat[..., 2, 2] = cj * ck 81 | mat[..., 2, 1] = sj * sc - cs 82 | mat[..., 2, 0] = sj * cc + ss 83 | mat[..., 1, 2] = cj * sk 84 | mat[..., 1, 1] = sj * ss + cc 85 | mat[..., 1, 0] = sj * cs - sc 86 | mat[..., 0, 2] = -sj 87 | mat[..., 0, 1] = cj * si 88 | mat[..., 0, 0] = cj * ci 89 | return mat 90 | 91 | 92 | def mat2euler(rmat, axes="sxyz"): 93 | """ 94 | Converts given rotation matrix to euler angles in radian. 95 | 96 | Args: 97 | rmat (np.array): 3x3 rotation matrix 98 | axes (str): One of 24 axis sequences as string or encoded tuple (see top of this module) 99 | 100 | Returns: 101 | np.array: (r,p,y) converted euler angles in radian vec3 float 102 | """ 103 | try: 104 | firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()] 105 | except (AttributeError, KeyError): 106 | firstaxis, parity, repetition, frame = axes 107 | 108 | i = firstaxis 109 | j = _NEXT_AXIS[i + parity] 110 | k = _NEXT_AXIS[i - parity + 1] 111 | 112 | M = np.array(rmat, dtype=np.float32, copy=False)[:3, :3] 113 | if repetition: 114 | sy = math.sqrt(M[i, j] * M[i, j] + M[i, k] * M[i, k]) 115 | if sy > EPS: 116 | ax = math.atan2(M[i, j], M[i, k]) 117 | ay = math.atan2(sy, M[i, i]) 118 | az = math.atan2(M[j, i], -M[k, i]) 119 | else: 120 | ax = math.atan2(-M[j, k], M[j, j]) 121 | ay = math.atan2(sy, M[i, i]) 122 | az = 0.0 123 | else: 124 | cy = math.sqrt(M[i, i] * M[i, i] + M[j, i] * M[j, i]) 125 | if cy > EPS: 126 | ax = math.atan2(M[k, j], M[k, k]) 127 | ay = math.atan2(-M[k, i], cy) 128 | az = math.atan2(M[j, i], M[i, i]) 129 | else: 130 | ax = math.atan2(-M[j, k], M[j, j]) 131 | ay = math.atan2(-M[k, i], cy) 132 | az = 0.0 133 | 134 | if parity: 135 | ax, ay, az = -ax, -ay, -az 136 | if frame: 137 | ax, az = az, ax 138 | return vec((ax, ay, az)) 139 | 140 | 141 | def quat_conjugate(quat): 142 | """ Return the conjugate of a quaternion. """ 143 | x, y, z, w = quat 144 | return np.array([x, y, z, -w], dtype=np.float32) 145 | 146 | def quat_multiply(quaternion1, quaternion0): 147 | """ 148 | Return multiplication of two quaternions (q1 * q0). 149 | 150 | E.g.: 151 | >>> q = quat_multiply([1, -2, 3, 4], [-5, 6, 7, 8]) 152 | >>> np.allclose(q, [-44, -14, 48, 28]) 153 | True 154 | 155 | Args: 156 | quaternion1 (np.array): (x,y,z,w) quaternion 157 | quaternion0 (np.array): (x,y,z,w) quaternion 158 | 159 | Returns: 160 | np.array: (x,y,z,w) multiplied quaternion 161 | """ 162 | x0, y0, z0, w0 = quaternion0 163 | x1, y1, z1, w1 = quaternion1 164 | return np.array( 165 | ( 166 | x1 * w0 + y1 * z0 - z1 * y0 + w1 * x0, 167 | -x1 * z0 + y1 * w0 + z1 * x0 + w1 * y0, 168 | x1 * y0 - y1 * x0 + z1 * w0 + w1 * z0, 169 | -x1 * x0 - y1 * y0 - z1 * z0 + w1 * w0, 170 | ), 171 | dtype=np.float32, 172 | ) 173 | 174 | 175 | def quat2axisangle(quat): 176 | """ 177 | Converts quaternion to axis-angle format. 178 | Returns a unit vector direction scaled by its angle in radians. 179 | 180 | Args: 181 | quat (np.array): (x,y,z,w) vec4 float angles 182 | 183 | Returns: 184 | np.array: (ax,ay,az) axis-angle exponential coordinates 185 | """ 186 | # clip quaternion 187 | if quat[3] > 1.0: 188 | quat[3] = 1.0 189 | elif quat[3] < -1.0: 190 | quat[3] = -1.0 191 | 192 | den = np.sqrt(1.0 - quat[3] * quat[3]) 193 | if math.isclose(den, 0.0): 194 | # This is (close to) a zero degree rotation, immediately return 195 | return np.zeros(3) 196 | 197 | return (quat[:3] * 2.0 * math.acos(quat[3])) / den 198 | 199 | 200 | def axisangle2quat(vec): 201 | """ 202 | Converts scaled axis-angle to quat. 203 | 204 | Args: 205 | vec (np.array): (ax,ay,az) axis-angle exponential coordinates 206 | 207 | Returns: 208 | np.array: (x,y,z,w) vec4 float angles 209 | """ 210 | # Grab angle 211 | angle = np.linalg.norm(vec) 212 | 213 | # handle zero-rotation case 214 | if math.isclose(angle, 0.0): 215 | return np.array([0.0, 0.0, 0.0, 1.0]) 216 | 217 | # make sure that axis is a unit vector 218 | axis = vec / angle 219 | 220 | q = np.zeros(4) 221 | q[3] = np.cos(angle / 2.0) 222 | q[:3] = axis * np.sin(angle / 2.0) 223 | return q 224 | 225 | 226 | def mat2quat(rmat): 227 | """ 228 | Converts given rotation matrix to quaternion. 229 | 230 | Args: 231 | rmat (np.array): 3x3 rotation matrix 232 | 233 | Returns: 234 | np.array: (x,y,z,w) float quaternion angles 235 | """ 236 | M = np.asarray(rmat).astype(np.float32)[:3, :3] 237 | 238 | m00 = M[0, 0] 239 | m01 = M[0, 1] 240 | m02 = M[0, 2] 241 | m10 = M[1, 0] 242 | m11 = M[1, 1] 243 | m12 = M[1, 2] 244 | m20 = M[2, 0] 245 | m21 = M[2, 1] 246 | m22 = M[2, 2] 247 | # symmetric matrix K 248 | K = np.array( 249 | [ 250 | [m00 - m11 - m22, np.float32(0.0), np.float32(0.0), np.float32(0.0)], 251 | [m01 + m10, m11 - m00 - m22, np.float32(0.0), np.float32(0.0)], 252 | [m02 + m20, m12 + m21, m22 - m00 - m11, np.float32(0.0)], 253 | [m21 - m12, m02 - m20, m10 - m01, m00 + m11 + m22], 254 | ] 255 | ) 256 | K /= 3.0 257 | # quaternion is Eigen vector of K that corresponds to largest eigenvalue 258 | w, V = np.linalg.eigh(K) 259 | inds = np.array([3, 0, 1, 2]) 260 | q1 = V[inds, np.argmax(w)] 261 | if q1[0] < 0.0: 262 | np.negative(q1, q1) 263 | inds = np.array([1, 2, 3, 0]) 264 | return q1[inds] 265 | 266 | 267 | def quat2mat(quaternion): 268 | """ 269 | Converts given quaternion to matrix. 270 | 271 | Args: 272 | quaternion (np.array): (x,y,z,w) vec4 float angles 273 | 274 | Returns: 275 | np.array: 3x3 rotation matrix 276 | """ 277 | # awkward semantics for use with numba 278 | inds = np.array([3, 0, 1, 2]) 279 | q = np.asarray(quaternion).copy().astype(np.float32)[inds] 280 | 281 | n = np.dot(q, q) 282 | if n < EPS: 283 | return np.identity(3) 284 | q *= math.sqrt(2.0 / n) 285 | q2 = np.outer(q, q) 286 | return np.array( 287 | [ 288 | [1.0 - q2[2, 2] - q2[3, 3], q2[1, 2] - q2[3, 0], q2[1, 3] + q2[2, 0]], 289 | [q2[1, 2] + q2[3, 0], 1.0 - q2[1, 1] - q2[3, 3], q2[2, 3] - q2[1, 0]], 290 | [q2[1, 3] - q2[2, 0], q2[2, 3] + q2[1, 0], 1.0 - q2[1, 1] - q2[2, 2]], 291 | ] 292 | ) 293 | 294 | 295 | def quat_multiply(quaternion1, quaternion0): 296 | """ 297 | Return multiplication of two quaternions (q1 * q0). 298 | 299 | E.g.: 300 | >>> q = quat_multiply([1, -2, 3, 4], [-5, 6, 7, 8]) 301 | >>> np.allclose(q, [-44, -14, 48, 28]) 302 | True 303 | 304 | Args: 305 | quaternion1 (np.array): (x,y,z,w) quaternion 306 | quaternion0 (np.array): (x,y,z,w) quaternion 307 | 308 | Returns: 309 | np.array: (x,y,z,w) multiplied quaternion 310 | """ 311 | x0, y0, z0, w0 = quaternion0 312 | x1, y1, z1, w1 = quaternion1 313 | return np.array( 314 | ( 315 | x1 * w0 + y1 * z0 - z1 * y0 + w1 * x0, 316 | -x1 * z0 + y1 * w0 + z1 * x0 + w1 * y0, 317 | x1 * y0 - y1 * x0 + z1 * w0 + w1 * z0, 318 | -x1 * x0 - y1 * y0 - z1 * z0 + w1 * w0, 319 | ), 320 | dtype=np.float32, 321 | ) 322 | 323 | 324 | ## Transformation code from DROID 325 | 326 | 327 | def add_angles(delta, source, degrees=False): 328 | delta_rot = R.from_euler("xyz", delta, degrees=degrees) 329 | source_rot = R.from_euler("xyz", source, degrees=degrees) 330 | new_rot = delta_rot * source_rot 331 | return new_rot.as_euler("xyz", degrees=degrees) 332 | 333 | 334 | def angle_diff(target, source, degrees=False): 335 | target_rot = R.from_euler("xyz", target, degrees=degrees) 336 | source_rot = R.from_euler("xyz", source, degrees=degrees) 337 | result = target_rot * source_rot.inv() 338 | return result.as_euler("xyz") 339 | 340 | 341 | def euler_to_quat(euler, degrees=False): 342 | return R.from_euler("xyz", euler, degrees=degrees).as_quat() 343 | 344 | 345 | def euler_to_axisangle(euler, degrees=False): 346 | return R.from_euler("xyz", euler, degrees=degrees).as_rotvec() 347 | 348 | 349 | def quat_to_euler(quat, degrees=False): 350 | euler = R.from_quat(quat).as_euler("xyz", degrees=degrees) 351 | return euler 352 | 353 | 354 | def quat_diff(target, source): 355 | result = R.from_quat(target) * R.from_quat(source).inv() 356 | return result.as_quat() 357 | 358 | 359 | def quat_add(target, source): 360 | result = R.from_quat(target) * R.from_quat(source) 361 | return result.as_quat() 362 | 363 | 364 | def rmat_to_quat(rot_mat, degrees=False): 365 | quat = R.from_matrix(rot_mat).as_quat() 366 | return quat 367 | 368 | 369 | def rmat_to_euler(rot_mat, degrees=False): 370 | euler = R.from_matrix(rot_mat).as_euler("xyz", degrees=degrees) 371 | return euler 372 | -------------------------------------------------------------------------------- /equibot/envs/subproc_vec_env.py: -------------------------------------------------------------------------------- 1 | import os 2 | import multiprocessing 3 | from collections import OrderedDict 4 | from typing import Sequence 5 | 6 | import gym 7 | import numpy as np 8 | 9 | from equibot.envs.vec_env import VecEnv, CloudpickleWrapper 10 | 11 | 12 | def _worker(remote, parent_remote, env_fn_wrapper): 13 | parent_remote.close() 14 | env = env_fn_wrapper.var() 15 | while True: 16 | try: 17 | cmd, data = remote.recv() 18 | if cmd == "step": 19 | observation, reward, done, info = env.step( 20 | data[0], dummy_reward=data[1] 21 | ) 22 | remote.send((observation, reward, done, info)) 23 | elif cmd == "seed": 24 | remote.send(env.seed(data)) 25 | elif cmd == "reset": 26 | observation = env.reset() 27 | remote.send(observation) 28 | elif cmd == "render": 29 | remote.send(env.render(data)) 30 | elif cmd == "close": 31 | env.close() 32 | remote.close() 33 | break 34 | elif cmd == "get_spaces": 35 | remote.send((env.observation_space, env.action_space)) 36 | elif cmd == "env_method": 37 | method = getattr(env, data[0]) 38 | remote.send(method(*data[1], **data[2])) 39 | elif cmd == "has_attr": 40 | remote.send(hasattr(env, data)) 41 | elif cmd == "get_attr": 42 | remote.send(getattr(env, data)) 43 | elif cmd == "set_attr": 44 | remote.send(setattr(env, data[0], data[1])) 45 | else: 46 | raise NotImplementedError( 47 | "`{}` is not implemented in the worker".format(cmd) 48 | ) 49 | except EOFError: 50 | break 51 | 52 | 53 | class SubprocVecEnv(VecEnv): 54 | """ 55 | Creates a multiprocess vectorized wrapper for multiple environments, distributing each environment to its own 56 | process, allowing significant speed up when the environment is computationally complex. 57 | 58 | For performance reasons, if your environment is not IO bound, the number of environments should not exceed the 59 | number of logical cores on your CPU. 60 | 61 | .. warning:: 62 | 63 | Only 'forkserver' and 'spawn' start methods are thread-safe, 64 | which is important when TensorFlow sessions or other non thread-safe 65 | libraries are used in the parent (see issue #217). However, compared to 66 | 'fork' they incur a small start-up cost and have restrictions on 67 | global variables. With those methods, users must wrap the code in an 68 | ``if __name__ == "__main__":`` block. 69 | For more information, see the multiprocessing documentation. 70 | 71 | :param env_fns: ([callable]) A list of functions that will create the environments 72 | (each callable returns a `Gym.Env` instance when called). 73 | :param start_method: (str) method used to start the subprocesses. 74 | Must be one of the methods returned by multiprocessing.get_all_start_methods(). 75 | Defaults to 'forkserver' on available platforms, and 'spawn' otherwise. 76 | """ 77 | 78 | def __init__(self, env_fns, start_method=None): 79 | self.waiting = False 80 | self.closed = False 81 | n_envs = len(env_fns) 82 | 83 | # In some cases (like on GitHub workflow machine when running tests), 84 | # "forkserver" method results in an "connection error" (probably due to mpi) 85 | # We allow to bypass the default start method if an environment variable 86 | # is specified by the user 87 | if start_method is None: 88 | start_method = os.environ.get("DEFAULT_START_METHOD") 89 | 90 | # No DEFAULT_START_METHOD was specified, start_method may still be None 91 | if start_method is None: 92 | # Fork is not a thread safe method (see issue #217) 93 | # but is more user friendly (does not require to wrap the code in 94 | # a `if __name__ == "__main__":`) 95 | forkserver_available = ( 96 | "forkserver" in multiprocessing.get_all_start_methods() 97 | ) 98 | start_method = "forkserver" if forkserver_available else "spawn" 99 | ctx = multiprocessing.get_context(start_method) 100 | 101 | self.remotes, self.work_remotes = zip( 102 | *[ctx.Pipe(duplex=True) for _ in range(n_envs)] 103 | ) 104 | self.processes = [] 105 | for work_remote, remote, env_fn in zip( 106 | self.work_remotes, self.remotes, env_fns 107 | ): 108 | args = (work_remote, remote, CloudpickleWrapper(env_fn)) 109 | # daemon=True: if the main process crashes, we should not cause things to hang 110 | process = ctx.Process( 111 | target=_worker, args=args, daemon=True 112 | ) # pytype:disable=attribute-error 113 | process.start() 114 | self.processes.append(process) 115 | work_remote.close() 116 | 117 | self.remotes[0].send(("get_spaces", None)) 118 | observation_space, action_space = self.remotes[0].recv() 119 | VecEnv.__init__(self, len(env_fns), observation_space, action_space) 120 | 121 | def step_async(self, actions, dummy_reward=False): 122 | for remote, action in zip(self.remotes, actions): 123 | remote.send(("step", (action, dummy_reward))) 124 | self.waiting = True 125 | 126 | def step_wait(self): 127 | results = [remote.recv() for remote in self.remotes] 128 | self.waiting = False 129 | obs, rews, dones, infos = zip(*results) 130 | return ( 131 | _flatten_obs(obs, self.observation_space), 132 | np.stack(rews), 133 | np.stack(dones), 134 | infos, 135 | ) 136 | 137 | def seed(self, seed=None): 138 | for idx, remote in enumerate(self.remotes): 139 | remote.send(("seed", seed + idx)) 140 | return [remote.recv() for remote in self.remotes] 141 | 142 | def reset(self): 143 | for remote in self.remotes: 144 | remote.send(("reset", None)) 145 | obs = [remote.recv() for remote in self.remotes] 146 | return _flatten_obs(obs, self.observation_space) 147 | 148 | def close(self): 149 | if self.closed: 150 | return 151 | if self.waiting: 152 | for remote in self.remotes: 153 | remote.recv() 154 | for remote in self.remotes: 155 | remote.send(("close", None)) 156 | for process in self.processes: 157 | process.join() 158 | self.closed = True 159 | 160 | def get_images(self) -> Sequence[np.ndarray]: 161 | for pipe in self.remotes: 162 | # gather images from subprocesses 163 | # `mode` will be taken into account later 164 | pipe.send(("render", "rgb_array")) 165 | imgs = [pipe.recv() for pipe in self.remotes] 166 | return imgs 167 | 168 | def get(self, attr_name): 169 | return self.get_attr(attr_name, [0])[0] 170 | 171 | def has_attr(self, attr_name): 172 | self.remotes[0].send(("has_attr", attr_name)) 173 | return self.remotes[0].recv() 174 | 175 | def get_attr(self, attr_name, indices=None): 176 | """Return attribute from vectorized environment (see base class).""" 177 | target_remotes = self._get_target_remotes(indices) 178 | for remote in target_remotes: 179 | remote.send(("get_attr", attr_name)) 180 | return [remote.recv() for remote in target_remotes] 181 | 182 | def set_attr(self, attr_name, value, indices=None): 183 | """Set attribute inside vectorized environments (see base class).""" 184 | target_remotes = self._get_target_remotes(indices) 185 | for remote in target_remotes: 186 | remote.send(("set_attr", (attr_name, value))) 187 | for remote in target_remotes: 188 | remote.recv() 189 | 190 | def env_method(self, method_name, *method_args, indices=None, **method_kwargs): 191 | """Call instance methods of vectorized environments.""" 192 | target_remotes = self._get_target_remotes(indices) 193 | for remote in target_remotes: 194 | remote.send(("env_method", (method_name, method_args, method_kwargs))) 195 | return [remote.recv() for remote in target_remotes] 196 | 197 | def _get_target_remotes(self, indices): 198 | """ 199 | Get the connection object needed to communicate with the wanted 200 | envs that are in subprocesses. 201 | 202 | :param indices: (None,int,Iterable) refers to indices of envs. 203 | :return: ([multiprocessing.Connection]) Connection object to communicate between processes. 204 | """ 205 | indices = self._get_indices(indices) 206 | return [self.remotes[i] for i in indices] 207 | 208 | 209 | def _flatten_obs(obs, space): 210 | """ 211 | Flatten observations, depending on the observation space. 212 | 213 | :param obs: (list or tuple where X is dict, tuple or ndarray) observations. 214 | A list or tuple of observations, one per environment. 215 | Each environment observation may be a NumPy array, or a dict or tuple of NumPy arrays. 216 | :return (OrderedDict, tuple or ndarray) flattened observations. 217 | A flattened NumPy array or an OrderedDict or tuple of flattened numpy arrays. 218 | Each NumPy array has the environment index as its first axis. 219 | """ 220 | assert isinstance( 221 | obs, (list, tuple) 222 | ), "expected list or tuple of observations per environment" 223 | assert len(obs) > 0, "need observations from at least one environment" 224 | 225 | if isinstance(space, gym.spaces.Dict): 226 | assert isinstance( 227 | space.spaces, OrderedDict 228 | ), "Dict space must have ordered subspaces" 229 | assert isinstance( 230 | obs[0], dict 231 | ), "non-dict observation for environment with Dict observation space" 232 | return OrderedDict( 233 | [(k, np.stack([o[k] for o in obs])) for k in space.spaces.keys()] 234 | ) 235 | elif isinstance(space, gym.spaces.Tuple): 236 | assert isinstance( 237 | obs[0], tuple 238 | ), "non-tuple observation for environment with Tuple observation space" 239 | obs_len = len(space.spaces) 240 | return tuple((np.stack([o[i] for o in obs]) for i in range(obs_len))) 241 | else: 242 | return np.stack(obs) 243 | -------------------------------------------------------------------------------- /equibot/policies/agents/dp_policy.py: -------------------------------------------------------------------------------- 1 | import copy 2 | import hydra 3 | import torch 4 | from torch import nn 5 | 6 | from equibot.policies.vision.pointnet_encoder import PointNetEncoder 7 | from equibot.policies.utils.diffusion.ema_model import EMAModel 8 | from equibot.policies.utils.diffusion.conditional_unet1d import ConditionalUnet1D 9 | from equibot.policies.utils.diffusion.resnet_with_gn import get_resnet, replace_bn_with_gn 10 | 11 | 12 | class DPPolicy(nn.Module): 13 | def __init__(self, cfg, device="cpu"): 14 | super().__init__() 15 | self.cfg = cfg 16 | self.hidden_dim = hidden_dim = cfg.model.hidden_dim 17 | self.obs_mode = cfg.model.obs_mode 18 | self.device = device 19 | 20 | # |o|o| observations: 2 21 | # | |a|a|a|a|a|a|a|a| actions executed: 8 22 | # |p|p|p|p|p|p|p|p|p|p|p|p|p|p|p|p| actions predicted: 16 23 | self.pred_horizon = cfg.model.pred_horizon 24 | self.obs_horizon = cfg.model.obs_horizon 25 | self.action_horizon = cfg.model.ac_horizon 26 | 27 | if hasattr(cfg.model, "num_diffusion_iters"): 28 | self.num_diffusion_iters = cfg.model.num_diffusion_iters 29 | else: 30 | self.num_diffusion_iters = cfg.model.noise_scheduler.num_train_timesteps 31 | 32 | self.num_eef = cfg.env.num_eef 33 | self.eef_dim = cfg.env.eef_dim 34 | self.dof = cfg.env.dof 35 | if cfg.model.obs_mode == "state": 36 | self.obs_dim = self.num_eef * self.eef_dim 37 | elif cfg.model.obs_mode == "rgb": 38 | self.obs_dim = 512 + self.num_eef * self.eef_dim 39 | else: 40 | self.obs_dim = hidden_dim + self.num_eef * self.eef_dim 41 | self.action_dim = self.dof * cfg.env.num_eef 42 | 43 | if self.obs_mode.startswith("pc"): 44 | self.encoder = PointNetEncoder( 45 | h_dim=hidden_dim, 46 | c_dim=hidden_dim, 47 | num_layers=cfg.model.encoder.backbone_args.num_layers, 48 | ) 49 | elif self.obs_mode == "rgb": 50 | self.encoder = replace_bn_with_gn(get_resnet("resnet18")) 51 | else: 52 | self.encoder = nn.Identity() 53 | self.noise_pred_net = ConditionalUnet1D( 54 | input_dim=self.action_dim, 55 | diffusion_step_embed_dim=self.obs_dim * self.obs_horizon, 56 | global_cond_dim=self.obs_dim * self.obs_horizon, 57 | ) 58 | 59 | self.nets = nn.ModuleDict( 60 | {"encoder": self.encoder, "noise_pred_net": self.noise_pred_net} 61 | ) 62 | self.ema = EMAModel(model=copy.deepcopy(self.nets), power=0.75) 63 | 64 | self._init_torch_compile() 65 | 66 | self.noise_scheduler = hydra.utils.instantiate(cfg.model.noise_scheduler) 67 | 68 | num_parameters = sum(p.numel() for p in self.parameters() if p.requires_grad) 69 | print(f"Initialized DP Policy with {num_parameters} parameters") 70 | 71 | def _init_torch_compile(self): 72 | if self.cfg.model.use_torch_compile: 73 | self.encoder_handle = torch.compile(self.encoder) 74 | self.noise_pred_net_handle = torch.compile(self.noise_pred_net) 75 | 76 | def forward(self, obs, predict_action=True, debug=False): 77 | # assumes that observation has format: 78 | # - pc: [BS, obs_horizon, num_pts, 3] 79 | # - state: [BS, obs_horizon, obs_dim] 80 | # returns: 81 | # - action: [BS, pred_horizon, ac_dim] 82 | pc = obs["pc"] 83 | state = obs["state"] 84 | 85 | if self.obs_mode.startswith("pc"): 86 | pc = self.pc_normalizer.normalize(pc) 87 | state = self.state_normalizer.normalize(state) 88 | 89 | pc_shape = pc.shape 90 | batch_size = pc.shape[0] 91 | 92 | ema_nets = self.ema.averaged_model 93 | 94 | if self.obs_mode == "state": 95 | z = state 96 | else: 97 | if self.obs_mode == "rgb": 98 | rgb = obs["rgb"] 99 | rgb_shape = rgb.shape 100 | flattened_rgb = rgb.reshape( 101 | batch_size * self.obs_horizon, *rgb_shape[-3:] 102 | ) 103 | z = ema_nets["encoder"](flattened_rgb.permute(0, 3, 1, 2)) 104 | else: 105 | flattened_pc = pc.reshape(batch_size * self.obs_horizon, *pc_shape[-2:]) 106 | z = ema_nets["encoder"](flattened_pc.permute(0, 2, 1))["global"] 107 | z = z.reshape(batch_size, self.obs_horizon, -1) 108 | z = torch.cat([z, state], dim=-1) 109 | obs_cond = z.reshape(batch_size, -1) # (BS, obs_horizon * obs_dim) 110 | 111 | initial_noise_scale = 0.0 if debug else 1.0 112 | noisy_action = ( 113 | torch.randn((batch_size, self.pred_horizon, self.action_dim)).to( 114 | self.device 115 | ) 116 | * initial_noise_scale 117 | ) 118 | curr_action = noisy_action 119 | self.noise_scheduler.set_timesteps(self.num_diffusion_iters) 120 | 121 | for k in self.noise_scheduler.timesteps: 122 | # predict noise 123 | noise_pred = ema_nets["noise_pred_net"]( 124 | sample=curr_action, timestep=k, global_cond=obs_cond 125 | ) 126 | 127 | # inverse diffusion step 128 | curr_action = self.noise_scheduler.step( 129 | model_output=noise_pred, timestep=k, sample=curr_action 130 | ).prev_sample 131 | 132 | ret = dict(ac=curr_action) 133 | return ret 134 | 135 | def step_ema(self): 136 | self.ema.step(self.nets) 137 | -------------------------------------------------------------------------------- /equibot/policies/configs/base.yaml: -------------------------------------------------------------------------------- 1 | prefix: default 2 | device: cuda 3 | mode: train 4 | log_dir: logs/${mode} 5 | eval_data_path: null 6 | use_wandb: true 7 | seed: 0 8 | 9 | agent: 10 | agent_name: dp 11 | 12 | env: 13 | env_class: ??? 14 | num_eef: 2 15 | dof: 7 16 | eef_dim: 13 17 | vectorize: false 18 | args: 19 | num_eef: ${env.num_eef} 20 | dof: ${env.dof} 21 | seed: ${seed} 22 | obs_mode: "pc" 23 | ac_mode: "rel" 24 | max_episode_length: ??? 25 | num_points: ${data.dataset.num_points} 26 | randomize_rotation: false 27 | randomize_scale: false 28 | scale_low: 1.0 29 | scale_high: 1.0 30 | scale_aspect_limit: 100.0 31 | uniform_scaling: false 32 | 33 | data: 34 | dataset_class: base_dataset 35 | dataset: 36 | num_training_steps: ??? 37 | path: null 38 | num_points: 1024 39 | num_augment: 0 40 | same_aug_per_sample: true 41 | aug_keep_original: true 42 | aug_scale_low: 0.5 43 | aug_scale_high: 1.5 44 | aug_scale_aspect_limit: 1.0 45 | aug_scale_rot: -1 46 | aug_scale_pos: 0.1 47 | aug_zero_z_offset: false 48 | aug_center: [0., 0., 0.] 49 | shuffle_pc: true 50 | num_workers: 12 51 | dof: ${env.dof} 52 | num_eef: ${env.num_eef} 53 | eef_dim: ${env.eef_dim} 54 | obs_horizon: ${model.obs_horizon} 55 | pred_horizon: ${model.pred_horizon} 56 | reduce_horizon_dim: false 57 | min_demo_length: 15 58 | 59 | model: 60 | hidden_dim: 32 61 | noise_scheduler: 62 | _target_: diffusers.schedulers.scheduling_ddpm.DDPMScheduler 63 | num_train_timesteps: 100 64 | beta_schedule: squaredcos_cap_v2 65 | clip_sample: true 66 | prediction_type: epsilon 67 | obs_horizon: 2 68 | ac_horizon: 8 69 | pred_horizon: 16 70 | encoder: 71 | c_dim: ${model.hidden_dim} 72 | backbone_type: vn_pointnet 73 | backbone_args: 74 | h_dim: ${model.hidden_dim} 75 | c_dim: ${model.encoder.c_dim} 76 | num_layers: 4 77 | knn: 8 78 | obs_mode: ${env.args.obs_mode} 79 | ac_mode: ${env.args.ac_mode} 80 | use_torch_compile: false 81 | 82 | training: 83 | batch_size: 32 84 | num_epochs: 2000 85 | lr: 3e-5 86 | weight_decay: 1e-6 87 | num_eval_episodes: 10 88 | eval_interval: 1000000 # do not eval during training 89 | save_interval: 50 90 | vis_interval: 100 91 | ckpt: null 92 | 93 | eval: 94 | last_ckpt: 1999 95 | num_ckpts_to_eval: 5 96 | 97 | hydra: 98 | run: 99 | dir: ${log_dir}/${prefix} 100 | 101 | wandb: 102 | entity: [enter wandb entity name here] 103 | project: [enter wandb project name here] 104 | -------------------------------------------------------------------------------- /equibot/policies/configs/close_mobile_dp.yaml: -------------------------------------------------------------------------------- 1 | defaults: 2 | - base 3 | 4 | env: 5 | env_class: close 6 | args: 7 | max_episode_length: 100 8 | cam_resolution: 256 9 | vis: true 10 | freq: 5 11 | ac_noise: 0.0 12 | -------------------------------------------------------------------------------- /equibot/policies/configs/close_mobile_equibot.yaml: -------------------------------------------------------------------------------- 1 | defaults: 2 | - close_mobile_dp 3 | 4 | agent: 5 | agent_name: equibot 6 | 7 | model: 8 | encoder: 9 | backbone_type: vn_pointnet 10 | -------------------------------------------------------------------------------- /equibot/policies/configs/cover_mobile_dp.yaml: -------------------------------------------------------------------------------- 1 | defaults: 2 | - base 3 | 4 | env: 5 | env_class: cover 6 | args: 7 | max_episode_length: 50 8 | cam_resolution: 256 9 | deform_bending_stiffness: 0.01 10 | deform_damping_stiffness: 1.0 11 | deform_elastic_stiffness: 300.0 12 | deform_friction_coeff: 10.0 13 | vis: true 14 | freq: 5 15 | ac_noise: 0.0 16 | -------------------------------------------------------------------------------- /equibot/policies/configs/cover_mobile_equibot.yaml: -------------------------------------------------------------------------------- 1 | defaults: 2 | - cover_mobile_dp 3 | 4 | agent: 5 | agent_name: equibot 6 | 7 | model: 8 | encoder: 9 | backbone_type: vn_pointnet 10 | -------------------------------------------------------------------------------- /equibot/policies/configs/fold_mobile_dp.yaml: -------------------------------------------------------------------------------- 1 | defaults: 2 | - base 3 | 4 | env: 5 | env_class: fold 6 | args: 7 | max_episode_length: 50 8 | cam_resolution: 256 9 | deform_bending_stiffness: 0.01 10 | deform_damping_stiffness: 1.0 11 | deform_elastic_stiffness: 150.0 12 | deform_friction_coeff: 1.0 13 | vis: true 14 | freq: 5 15 | ac_noise: 0.0 16 | -------------------------------------------------------------------------------- /equibot/policies/configs/fold_mobile_equibot.yaml: -------------------------------------------------------------------------------- 1 | defaults: 2 | - fold_mobile_dp 3 | 4 | agent: 5 | agent_name: equibot 6 | 7 | model: 8 | encoder: 9 | backbone_type: vn_pointnet 10 | -------------------------------------------------------------------------------- /equibot/policies/eval.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | import time 4 | import torch 5 | import hydra 6 | import omegaconf 7 | import wandb 8 | import numpy as np 9 | import getpass as gt 10 | from glob import glob 11 | from tqdm import tqdm 12 | from omegaconf import OmegaConf 13 | 14 | from equibot.policies.utils.media import combine_videos, save_video 15 | from equibot.policies.utils.misc import get_env_class, get_dataset, get_agent 16 | from equibot.envs.subproc_vec_env import SubprocVecEnv 17 | 18 | 19 | def organize_obs(render, rgb_render, state): 20 | if type(render) is list: 21 | obs = dict( 22 | pc=[r["pc"] for r in render], 23 | rgb=np.array([r["images"][0][..., :3] for r in rgb_render]), 24 | # depth=np.array([r["depths"][0] for r in render]), 25 | state=np.array(state), 26 | ) 27 | for k in ["eef_pos", "eef_rot"]: 28 | if k in render[0]: 29 | obs[k] = [r[k] for r in render] 30 | return obs 31 | elif type(render) is dict: 32 | obs = organize_obs([render], [rgb_render], [state]) 33 | return {k: v[0] for k, v in obs.items()} 34 | 35 | 36 | def run_eval( 37 | env, 38 | agent, 39 | vis=False, 40 | num_episodes=1, 41 | log_dir=None, 42 | reduce_horizon_dim=True, 43 | verbose=False, 44 | use_wandb=False, 45 | ckpt_name=None, 46 | ): 47 | if vis: 48 | vis_frames = [] 49 | 50 | if hasattr(agent, "obs_horizon") and hasattr(agent, "ac_horizon"): 51 | obs_horizon = agent.obs_horizon 52 | ac_horizon = agent.ac_horizon 53 | pred_horizon = agent.pred_horizon 54 | else: 55 | obs_horizon = 1 56 | ac_horizon = 1 57 | pred_horizon = 1 58 | 59 | images, rews = [], [] 60 | for ep_ix in range(num_episodes): 61 | images.append([]) 62 | obs_history = [] 63 | state = env.reset() 64 | 65 | rgb_render = render = env.render() 66 | obs = organize_obs(render, rgb_render, state) 67 | for i in range(obs_horizon): 68 | obs_history.append(obs) 69 | images[-1].append(rgb_render["images"][0][..., :3]) 70 | 71 | if ep_ix == 0: 72 | sample_pc = render["pc"] 73 | 74 | done = False 75 | global_features = None 76 | prev_reward = None 77 | if log_dir is not None: 78 | history = dict(action=[], eef_pos=[]) 79 | while not done: 80 | # make obs for agent 81 | if obs_horizon == 1 and reduce_horizon_dim: 82 | agent_obs = obs 83 | else: 84 | agent_obs = dict() 85 | for k in obs.keys(): 86 | if k == "pc": 87 | # point clouds can have different number of points 88 | # so do not stack them 89 | agent_obs[k] = [o[k] for o in obs_history[-obs_horizon:]] 90 | else: 91 | agent_obs[k] = np.stack( 92 | [o[k] for o in obs_history[-obs_horizon:]] 93 | ) 94 | 95 | # predict actions 96 | st = time.time() 97 | ac, ac_dict = agent.act(agent_obs, return_dict=True) 98 | print(f"Inference time: {time.time() - st:.3f}s") 99 | if ac_dict is not None: 100 | if ( 101 | "expected_eef_pos" in ac_dict 102 | and ac_dict["expected_eef_pos"] is not None 103 | ): 104 | env.visualize_anchor(ac_dict["expected_eef_pos"][:, :3]) 105 | if "expected_pc" in ac_dict and ac_dict["expected_pc"] is not None: 106 | if hasattr(env, "visualize_pc"): 107 | env.visualize_pc(ac_dict["expected_pc"]) 108 | else: 109 | print(f"Warning: ac dict is none!") 110 | if log_dir is not None: 111 | history["action"].append(ac) 112 | history["eef_pos"].append(obs["state"]) 113 | 114 | # take actions 115 | for ac_ix in range(ac_horizon): 116 | if len(obs["pc"]) == 0 or len(obs["pc"][0]) == 0: 117 | ac_dict = None 118 | break 119 | agent_ac = ac[ac_ix] if len(ac.shape) > 1 else ac 120 | state, rew, done, info = env.step(agent_ac, dummy_reward=True) 121 | if hasattr(env, "visualize_eef_frame"): 122 | env.visualize_eef_frame(state) 123 | rgb_render = render = env.render() 124 | obs = organize_obs(render, rgb_render, state) 125 | obs_history.append(obs) 126 | if len(obs) > obs_horizon: 127 | obs_history = obs_history[-obs_horizon:] 128 | images[-1].append(rgb_render["images"][0][..., :3]) 129 | if vis: 130 | vis_frames.append(rgb_render["images"][0][..., :3]) 131 | curr_reward = env.compute_reward() 132 | if prev_reward is None or curr_reward > prev_reward: 133 | prev_reward = curr_reward 134 | if ( 135 | ac_dict is None 136 | or done 137 | or ( 138 | hasattr(env, "_failed_safety_check") 139 | and env._failed_safety_check 140 | ) 141 | ): 142 | break 143 | if ( 144 | ac_dict is None 145 | or done 146 | or (hasattr(env, "_failed_safety_check") and env._failed_safety_check) 147 | ): 148 | break 149 | if ac_dict is not None: 150 | rew = env.compute_reward() 151 | else: 152 | rew = prev_reward 153 | rews.append(rew) 154 | print(f"Episode {ep_ix + 1} reward: {rew:.4f}.") 155 | 156 | if log_dir is not None: 157 | os.makedirs(log_dir, exist_ok=True) 158 | np.savez( 159 | os.path.join(log_dir, f"eval_ep{ep_ix:02d}_rew{rew:.3f}.npz"), 160 | action=np.array(history["action"]), 161 | eef_pos=np.array(history["eef_pos"]), 162 | ) 163 | max_num_images = np.max([len(images[i]) for i in range(len(images))]) 164 | for i in range(len(images)): 165 | if len(images[i]) < max_num_images: 166 | images[i] = images[i] + [images[i][-1]] * (max_num_images - len(images[i])) 167 | images = np.array(images) 168 | rews = np.array(rews) 169 | 170 | pos_idxs, neg_idxs = np.where(rews >= 0.5)[0], np.where(rews < 0.5)[0] 171 | metrics = dict(rew=np.mean(rews)) 172 | fps = 30 if "sim_mobile" in env.__module__ else 4 173 | if use_wandb: 174 | if len(pos_idxs) > 0: 175 | metrics["video_pos"] = wandb.Video( 176 | combine_videos(images[pos_idxs][:6], num_cols=5).transpose(0, 3, 1, 2), 177 | fps=30, 178 | ) 179 | if len(neg_idxs) > 0: 180 | metrics["video_neg"] = wandb.Video( 181 | combine_videos(images[neg_idxs][:6], num_cols=5).transpose(0, 3, 1, 2), 182 | fps=30, 183 | ) 184 | if vis: 185 | metrics["vis_rollout"] = images 186 | metrics["vis_pc"] = wandb.Object3D(sample_pc) 187 | else: 188 | metrics["vis_rollout"] = images 189 | return metrics 190 | 191 | 192 | @hydra.main(config_path="configs", config_name="fold_synthetic") 193 | def main(cfg): 194 | assert cfg.mode == "eval" 195 | device = torch.device(cfg.device) 196 | if cfg.use_wandb: 197 | wandb_config = omegaconf.OmegaConf.to_container( 198 | cfg, resolve=True, throw_on_missing=False 199 | ) 200 | wandb.init( 201 | entity=cfg.wandb.entity, 202 | project=cfg.wandb.project, 203 | tags=["eval"], 204 | name=cfg.prefix, 205 | settings=wandb.Settings(code_dir="."), 206 | config=wandb_config, 207 | ) 208 | np.random.seed(cfg.seed) 209 | 210 | if cfg.env.vectorize: 211 | env_fns = [] 212 | env_class = get_env_class(cfg.env.env_class) 213 | env_args = dict(OmegaConf.to_container(cfg.env.args, resolve=True)) 214 | 215 | def create_env(env_args, i): 216 | env_args["seed"] = cfg.seed * 100 + i 217 | return env_class(OmegaConf.create(env_args)) 218 | 219 | env = SubprocVecEnv( 220 | [ 221 | lambda seed=i: create_env(env_args, seed) 222 | for i in range(cfg.training.num_eval_episodes) 223 | ] 224 | ) 225 | from equibot.policies.vec_eval import run_eval as run_vec_eval 226 | 227 | eval_fn = run_vec_eval 228 | else: 229 | env = get_env_class(cfg.env.env_class)(cfg.env.args) 230 | eval_fn = run_eval 231 | 232 | agent = get_agent(cfg.agent.agent_name)(cfg) 233 | agent.train(False) 234 | 235 | if os.path.isdir(cfg.training.ckpt): 236 | ckpt_dir = cfg.training.ckpt 237 | ckpt_paths = list(glob(os.path.join(ckpt_dir, "ckpt*.pth"))) 238 | assert len(ckpt_paths) >= cfg.eval.num_ckpts_to_eval 239 | ckpt_paths = list(sorted(ckpt_paths))[-cfg.eval.num_ckpts_to_eval :] 240 | assert f"{cfg.eval.last_ckpt}" in ckpt_paths[-1] 241 | else: 242 | ckpt_paths = [cfg.training.ckpt] 243 | 244 | rew_list = [] 245 | 246 | for ckpt_path in ckpt_paths: 247 | ckpt_name = ckpt_path.split("/")[-1].split(".")[0] 248 | agent.load_snapshot(ckpt_path) 249 | 250 | log_dir = os.getcwd() 251 | 252 | eval_metrics = eval_fn( 253 | env, 254 | agent, 255 | vis=True, 256 | num_episodes=cfg.training.num_eval_episodes, 257 | log_dir=log_dir, 258 | reduce_horizon_dim=cfg.data.dataset.reduce_horizon_dim, 259 | verbose=True, 260 | ckpt_name=ckpt_name, 261 | ) 262 | mean_rew = eval_metrics["rew"] 263 | print(f"Evaluation results: mean rew = {mean_rew}") 264 | rew_list.append(mean_rew) 265 | if cfg.use_wandb: 266 | wandb.log( 267 | {"eval/" + k: v for k, v in eval_metrics.items() if k != "vis_rollout"} 268 | ) 269 | else: 270 | save_filename = os.path.join( 271 | os.getcwd(), f"vis_{ckpt_name}_rew{mean_rew:.3f}.mp4" 272 | ) 273 | if "vis_rollout" in eval_metrics: 274 | if len(eval_metrics["vis_rollout"].shape) == 4: 275 | save_video(eval_metrics["vis_rollout"], save_filename, fps=30) 276 | else: 277 | assert len(eval_metrics["vis_rollout"][0].shape) == 4 278 | for eval_idx, eval_video in enumerate(eval_metrics["vis_rollout"]): 279 | episode_rew = eval_metrics["rew_values"][eval_idx] 280 | save_filename = os.path.join( 281 | os.getcwd(), 282 | f"vis_{ckpt_name}_ep{eval_idx}_rew{episode_rew:.3f}.mp4", 283 | ) 284 | save_video(eval_video, save_filename) 285 | del eval_metrics 286 | np.savez(os.path.join(os.getcwd(), "info.npz"), rews=np.array(rew_list)) 287 | 288 | 289 | if __name__ == "__main__": 290 | main() 291 | -------------------------------------------------------------------------------- /equibot/policies/train.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | import copy 4 | import hydra 5 | import torch 6 | import wandb 7 | import omegaconf 8 | import numpy as np 9 | import getpass as gt 10 | from tqdm import tqdm 11 | from glob import glob 12 | from omegaconf import OmegaConf 13 | 14 | from equibot.policies.utils.media import save_video 15 | from equibot.policies.utils.misc import get_env_class, get_dataset, get_agent 16 | from equibot.policies.vec_eval import run_eval 17 | from equibot.envs.subproc_vec_env import SubprocVecEnv 18 | 19 | 20 | @hydra.main(config_path="configs", config_name="fold_synthetic") 21 | def main(cfg): 22 | assert cfg.mode == "train" 23 | np.random.seed(cfg.seed) 24 | 25 | # initialize parameters 26 | batch_size = cfg.training.batch_size 27 | 28 | # setup logging 29 | if cfg.use_wandb: 30 | wandb_config = omegaconf.OmegaConf.to_container( 31 | cfg, resolve=True, throw_on_missing=False 32 | ) 33 | wandb.init( 34 | entity=cfg.wandb.entity, 35 | project=cfg.wandb.project, 36 | tags=["train"], 37 | name=cfg.prefix, 38 | settings=wandb.Settings(code_dir="."), 39 | config=wandb_config, 40 | ) 41 | log_dir = os.getcwd() 42 | 43 | # init dataloader 44 | train_dataset = get_dataset(cfg, "train") 45 | num_workers = cfg.data.dataset.num_workers 46 | train_loader = torch.utils.data.DataLoader( 47 | train_dataset, 48 | batch_size=batch_size, 49 | num_workers=num_workers, 50 | shuffle=True, 51 | drop_last=True, 52 | pin_memory=True, 53 | ) 54 | cfg.data.dataset.num_training_steps = ( 55 | cfg.training.num_epochs * len(train_dataset) // batch_size 56 | ) 57 | 58 | # init env 59 | env_fns = [] 60 | env_class = get_env_class(cfg.env.env_class) 61 | env_args = dict(OmegaConf.to_container(cfg.env.args, resolve=True)) 62 | 63 | def create_env(env_args, i): 64 | env_args.seed = cfg.seed * 100 + i 65 | return env_class(OmegaConf.create(env_args)) 66 | 67 | if cfg.training.eval_interval <= cfg.training.num_epochs: 68 | env = SubprocVecEnv( 69 | [ 70 | lambda seed=i: create_env(env_args, seed) 71 | for i in range(cfg.training.num_eval_episodes) 72 | ] 73 | ) 74 | else: 75 | env = None 76 | 77 | # init agent 78 | agent = get_agent(cfg.agent.agent_name)(cfg) 79 | if cfg.training.ckpt is not None: 80 | agent.load_snapshot(cfg.training.ckpt) 81 | start_epoch_ix = int(cfg.training.ckpt.split("/")[-1].split(".")[0][4:]) 82 | else: 83 | start_epoch_ix = 0 84 | 85 | # train loop 86 | global_step = 0 87 | for epoch_ix in tqdm(range(start_epoch_ix, cfg.training.num_epochs)): 88 | batch_ix = 0 89 | for batch in tqdm(train_loader, leave=False, desc="Batches"): 90 | train_metrics = agent.update( 91 | batch, vis=epoch_ix % cfg.training.vis_interval == 0 and batch_ix == 0 92 | ) 93 | if cfg.use_wandb: 94 | wandb.log( 95 | {"train/" + k: v for k, v in train_metrics.items()}, 96 | step=global_step, 97 | ) 98 | wandb.log({"epoch": epoch_ix}, step=global_step) 99 | del train_metrics 100 | global_step += 1 101 | batch_ix += 1 102 | if ( 103 | ( 104 | epoch_ix % cfg.training.eval_interval == 0 105 | or epoch_ix == cfg.training.num_epochs - 1 106 | ) 107 | and epoch_ix > 0 108 | and env is not None 109 | ): 110 | eval_metrics = run_eval( 111 | env, 112 | agent, 113 | vis=True, 114 | num_episodes=cfg.training.num_eval_episodes, 115 | reduce_horizon_dim=cfg.data.dataset.reduce_horizon_dim, 116 | use_wandb=cfg.use_wandb, 117 | ) 118 | if cfg.use_wandb: 119 | if epoch_ix > cfg.training.eval_interval and "vis_pc" in eval_metrics: 120 | # only save one pc per run to save space 121 | del eval_metrics["vis_pc"] 122 | wandb.log( 123 | { 124 | "eval/" + k: v 125 | for k, v in eval_metrics.items() 126 | if not k in ["vis_rollout", "rew_values"] 127 | }, 128 | step=global_step, 129 | ) 130 | if "vis_rollout" in eval_metrics: 131 | for eval_idx, eval_video in enumerate(eval_metrics["vis_rollout"]): 132 | video_path = os.path.join( 133 | log_dir, 134 | f"eval{epoch_ix:05d}_ep{eval_idx}_rew{eval_metrics['rew_values'][eval_idx]}.mp4", 135 | ) 136 | save_video(eval_video, video_path) 137 | print(f"Saved eval video to {video_path}") 138 | del eval_metrics 139 | if ( 140 | epoch_ix % cfg.training.save_interval == 0 141 | or epoch_ix == cfg.training.num_epochs - 1 142 | ): 143 | save_path = os.path.join(log_dir, f"ckpt{epoch_ix:05d}.pth") 144 | num_ckpt_to_keep = 10 145 | if len(list(glob(os.path.join(log_dir, "ckpt*.pth")))) > num_ckpt_to_keep: 146 | # remove old checkpoints 147 | for fn in list(sorted(glob(os.path.join(log_dir, "ckpt*.pth"))))[ 148 | :-num_ckpt_to_keep 149 | ]: 150 | os.remove(fn) 151 | agent.save_snapshot(save_path) 152 | 153 | 154 | if __name__ == "__main__": 155 | main() 156 | -------------------------------------------------------------------------------- /equibot/policies/utils/diffusion/conditional_unet1d.py: -------------------------------------------------------------------------------- 1 | from typing import Union 2 | import torch 3 | import torch.nn as nn 4 | import einops 5 | from einops.layers.torch import Rearrange 6 | 7 | from equibot.policies.utils.diffusion.conv1d_components import ( 8 | Downsample1d, 9 | Upsample1d, 10 | Conv1dBlock, 11 | ) 12 | from equibot.policies.utils.diffusion.positional_embedding import SinusoidalPosEmb 13 | 14 | 15 | class ConditionalResidualBlock1D(nn.Module): 16 | def __init__( 17 | self, 18 | in_channels, 19 | out_channels, 20 | cond_dim, 21 | kernel_size=3, 22 | n_groups=8, 23 | cond_predict_scale=False, 24 | ): 25 | super().__init__() 26 | 27 | self.blocks = nn.ModuleList( 28 | [ 29 | Conv1dBlock(in_channels, out_channels, kernel_size, n_groups=n_groups), 30 | Conv1dBlock(out_channels, out_channels, kernel_size, n_groups=n_groups), 31 | ] 32 | ) 33 | 34 | # FiLM modulation https://arxiv.org/abs/1709.07871 35 | # predicts per-channel scale and bias 36 | cond_channels = out_channels 37 | if cond_predict_scale: 38 | cond_channels = out_channels * 2 39 | self.cond_predict_scale = cond_predict_scale 40 | self.out_channels = out_channels 41 | self.cond_encoder = nn.Sequential( 42 | nn.Mish(), 43 | nn.Linear(cond_dim, cond_channels), 44 | Rearrange("batch t -> batch t 1"), 45 | ) 46 | 47 | # make sure dimensions compatible 48 | self.residual_conv = ( 49 | nn.Conv1d(in_channels, out_channels, 1) 50 | if in_channels != out_channels 51 | else nn.Identity() 52 | ) 53 | 54 | def forward(self, x, cond): 55 | """ 56 | x : [ batch_size x in_channels x horizon ] 57 | cond : [ batch_size x cond_dim] 58 | 59 | returns: 60 | out : [ batch_size x out_channels x horizon ] 61 | """ 62 | out = self.blocks[0](x) 63 | embed = self.cond_encoder(cond) 64 | if self.cond_predict_scale: 65 | embed = embed.reshape(embed.shape[0], 2, self.out_channels, 1) 66 | scale = embed[:, 0, ...] 67 | bias = embed[:, 1, ...] 68 | out = scale * out + bias 69 | else: 70 | out = out + embed 71 | out = self.blocks[1](out) 72 | out = out + self.residual_conv(x) 73 | return out 74 | 75 | 76 | class ConditionalUnet1D(nn.Module): 77 | def __init__( 78 | self, 79 | input_dim, 80 | local_cond_dim=None, 81 | global_cond_dim=None, 82 | diffusion_step_embed_dim=256, 83 | down_dims=[256, 512, 1024], 84 | kernel_size=3, 85 | n_groups=8, 86 | cond_predict_scale=False, 87 | ): 88 | super().__init__() 89 | all_dims = [input_dim] + list(down_dims) 90 | start_dim = down_dims[0] 91 | 92 | dsed = diffusion_step_embed_dim 93 | diffusion_step_encoder = nn.Sequential( 94 | SinusoidalPosEmb(dsed), 95 | nn.Linear(dsed, dsed * 4), 96 | nn.Mish(), 97 | nn.Linear(dsed * 4, dsed), 98 | ) 99 | cond_dim = dsed 100 | if global_cond_dim is not None: 101 | cond_dim += global_cond_dim 102 | 103 | in_out = list(zip(all_dims[:-1], all_dims[1:])) 104 | 105 | local_cond_encoder = None 106 | if local_cond_dim is not None: 107 | _, dim_out = in_out[0] 108 | dim_in = local_cond_dim 109 | local_cond_encoder = nn.ModuleList( 110 | [ 111 | # down encoder 112 | ConditionalResidualBlock1D( 113 | dim_in, 114 | dim_out, 115 | cond_dim=cond_dim, 116 | kernel_size=kernel_size, 117 | n_groups=n_groups, 118 | cond_predict_scale=cond_predict_scale, 119 | ), 120 | # up encoder 121 | ConditionalResidualBlock1D( 122 | dim_in, 123 | dim_out, 124 | cond_dim=cond_dim, 125 | kernel_size=kernel_size, 126 | n_groups=n_groups, 127 | cond_predict_scale=cond_predict_scale, 128 | ), 129 | ] 130 | ) 131 | 132 | mid_dim = all_dims[-1] 133 | self.mid_modules = nn.ModuleList( 134 | [ 135 | ConditionalResidualBlock1D( 136 | mid_dim, 137 | mid_dim, 138 | cond_dim=cond_dim, 139 | kernel_size=kernel_size, 140 | n_groups=n_groups, 141 | cond_predict_scale=cond_predict_scale, 142 | ), 143 | ConditionalResidualBlock1D( 144 | mid_dim, 145 | mid_dim, 146 | cond_dim=cond_dim, 147 | kernel_size=kernel_size, 148 | n_groups=n_groups, 149 | cond_predict_scale=cond_predict_scale, 150 | ), 151 | ] 152 | ) 153 | 154 | down_modules = nn.ModuleList([]) 155 | for ind, (dim_in, dim_out) in enumerate(in_out): 156 | is_last = ind >= (len(in_out) - 1) 157 | down_modules.append( 158 | nn.ModuleList( 159 | [ 160 | ConditionalResidualBlock1D( 161 | dim_in, 162 | dim_out, 163 | cond_dim=cond_dim, 164 | kernel_size=kernel_size, 165 | n_groups=n_groups, 166 | cond_predict_scale=cond_predict_scale, 167 | ), 168 | ConditionalResidualBlock1D( 169 | dim_out, 170 | dim_out, 171 | cond_dim=cond_dim, 172 | kernel_size=kernel_size, 173 | n_groups=n_groups, 174 | cond_predict_scale=cond_predict_scale, 175 | ), 176 | Downsample1d(dim_out) if not is_last else nn.Identity(), 177 | ] 178 | ) 179 | ) 180 | 181 | up_modules = nn.ModuleList([]) 182 | for ind, (dim_in, dim_out) in enumerate(reversed(in_out[1:])): 183 | is_last = ind >= (len(in_out) - 1) 184 | up_modules.append( 185 | nn.ModuleList( 186 | [ 187 | ConditionalResidualBlock1D( 188 | dim_out * 2, 189 | dim_in, 190 | cond_dim=cond_dim, 191 | kernel_size=kernel_size, 192 | n_groups=n_groups, 193 | cond_predict_scale=cond_predict_scale, 194 | ), 195 | ConditionalResidualBlock1D( 196 | dim_in, 197 | dim_in, 198 | cond_dim=cond_dim, 199 | kernel_size=kernel_size, 200 | n_groups=n_groups, 201 | cond_predict_scale=cond_predict_scale, 202 | ), 203 | Upsample1d(dim_in) if not is_last else nn.Identity(), 204 | ] 205 | ) 206 | ) 207 | 208 | final_conv = nn.Sequential( 209 | Conv1dBlock(start_dim, start_dim, kernel_size=kernel_size), 210 | nn.Conv1d(start_dim, input_dim, 1), 211 | ) 212 | 213 | self.diffusion_step_encoder = diffusion_step_encoder 214 | self.local_cond_encoder = local_cond_encoder 215 | self.up_modules = up_modules 216 | self.down_modules = down_modules 217 | self.final_conv = final_conv 218 | 219 | print("number of parameters: %e", sum(p.numel() for p in self.parameters())) 220 | 221 | def forward( 222 | self, 223 | sample: torch.Tensor, 224 | timestep: Union[torch.Tensor, float, int], 225 | local_cond=None, 226 | global_cond=None, 227 | **kwargs 228 | ): 229 | """ 230 | x: (B,T,input_dim) 231 | timestep: (B,) or int, diffusion step 232 | local_cond: (B,T,local_cond_dim) 233 | global_cond: (B,global_cond_dim) 234 | output: (B,T,input_dim) 235 | """ 236 | sample = einops.rearrange(sample, "b h t -> b t h") 237 | 238 | # 1. time 239 | timesteps = timestep 240 | if not torch.is_tensor(timesteps): 241 | # TODO: this requires sync between CPU and GPU. So try to pass timesteps as tensors if you can 242 | timesteps = torch.tensor( 243 | [timesteps], dtype=torch.long, device=sample.device 244 | ) 245 | elif torch.is_tensor(timesteps) and len(timesteps.shape) == 0: 246 | timesteps = timesteps[None].to(sample.device) 247 | # broadcast to batch dimension in a way that's compatible with ONNX/Core ML 248 | timesteps = timesteps.expand(sample.shape[0]) 249 | 250 | global_feature = self.diffusion_step_encoder(timesteps) 251 | 252 | if global_cond is not None: 253 | global_feature = torch.cat([global_feature, global_cond], axis=-1) 254 | 255 | # encode local features 256 | h_local = list() 257 | if local_cond is not None: 258 | local_cond = einops.rearrange(local_cond, "b h t -> b t h") 259 | resnet, resnet2 = self.local_cond_encoder 260 | x = resnet(local_cond, global_feature) 261 | h_local.append(x) 262 | x = resnet2(local_cond, global_feature) 263 | h_local.append(x) 264 | 265 | x = sample 266 | h = [] 267 | for idx, (resnet, resnet2, downsample) in enumerate(self.down_modules): 268 | x = resnet(x, global_feature) 269 | if idx == 0 and len(h_local) > 0: 270 | x = x + h_local[0] 271 | x = resnet2(x, global_feature) 272 | h.append(x) 273 | x = downsample(x) 274 | 275 | for mid_module in self.mid_modules: 276 | x = mid_module(x, global_feature) 277 | 278 | for idx, (resnet, resnet2, upsample) in enumerate(self.up_modules): 279 | x = torch.cat((x, h.pop()), dim=1) 280 | x = resnet(x, global_feature) 281 | # The correct condition should be: 282 | # if idx == (len(self.up_modules)-1) and len(h_local) > 0: 283 | # However this change will break compatibility with published checkpoints. 284 | # Therefore it is left as a comment. 285 | if idx == len(self.up_modules) and len(h_local) > 0: 286 | x = x + h_local[1] 287 | x = resnet2(x, global_feature) 288 | x = upsample(x) 289 | 290 | x = self.final_conv(x) 291 | 292 | x = einops.rearrange(x, "b t h -> b h t") 293 | return x 294 | -------------------------------------------------------------------------------- /equibot/policies/utils/diffusion/conv1d_components.py: -------------------------------------------------------------------------------- 1 | import torch 2 | import torch.nn as nn 3 | import torch.nn.functional as F 4 | 5 | 6 | class Downsample1d(nn.Module): 7 | def __init__(self, dim): 8 | super().__init__() 9 | self.conv = nn.Conv1d(dim, dim, 3, 2, 1) 10 | 11 | def forward(self, x): 12 | return self.conv(x) 13 | 14 | 15 | class Upsample1d(nn.Module): 16 | def __init__(self, dim): 17 | super().__init__() 18 | self.conv = nn.ConvTranspose1d(dim, dim, 4, 2, 1) 19 | 20 | def forward(self, x): 21 | return self.conv(x) 22 | 23 | 24 | class Conv1dBlock(nn.Module): 25 | """ 26 | Conv1d --> GroupNorm --> Mish 27 | """ 28 | 29 | def __init__(self, inp_channels, out_channels, kernel_size, n_groups=8): 30 | super().__init__() 31 | 32 | self.block = nn.Sequential( 33 | nn.Conv1d( 34 | inp_channels, out_channels, kernel_size, padding=kernel_size // 2 35 | ), 36 | # Rearrange('batch channels horizon -> batch channels 1 horizon'), 37 | nn.GroupNorm(n_groups, out_channels), 38 | # Rearrange('batch channels 1 horizon -> batch channels horizon'), 39 | nn.Mish(), 40 | ) 41 | 42 | def forward(self, x): 43 | return self.block(x) 44 | 45 | 46 | def test(): 47 | cb = Conv1dBlock(256, 128, kernel_size=3) 48 | x = torch.zeros((1, 256, 16)) 49 | o = cb(x) 50 | -------------------------------------------------------------------------------- /equibot/policies/utils/diffusion/ema_model.py: -------------------------------------------------------------------------------- 1 | import copy 2 | import torch 3 | from torch.nn.modules.batchnorm import _BatchNorm 4 | 5 | 6 | class EMAModel: 7 | """ 8 | Exponential Moving Average of models weights 9 | """ 10 | 11 | def __init__( 12 | self, 13 | model, 14 | update_after_step=0, 15 | inv_gamma=1.0, 16 | power=2 / 3, 17 | min_value=0.0, 18 | max_value=0.9999, 19 | ): 20 | """ 21 | @crowsonkb's notes on EMA Warmup: 22 | If gamma=1 and power=1, implements a simple average. gamma=1, power=2/3 are good values for models you plan 23 | to train for a million or more steps (reaches decay factor 0.999 at 31.6K steps, 0.9999 at 1M steps), 24 | gamma=1, power=3/4 for models you plan to train for less (reaches decay factor 0.999 at 10K steps, 0.9999 25 | at 215.4k steps). 26 | Args: 27 | inv_gamma (float): Inverse multiplicative factor of EMA warmup. Default: 1. 28 | power (float): Exponential factor of EMA warmup. Default: 2/3. 29 | min_value (float): The minimum EMA decay rate. Default: 0. 30 | """ 31 | 32 | self.averaged_model = model 33 | self.averaged_model.eval() 34 | self.averaged_model.requires_grad_(False) 35 | 36 | self.update_after_step = update_after_step 37 | self.inv_gamma = inv_gamma 38 | self.power = power 39 | self.min_value = min_value 40 | self.max_value = max_value 41 | 42 | self.decay = 0.0 43 | self.optimization_step = 0 44 | 45 | def get_decay(self, optimization_step): 46 | """ 47 | Compute the decay factor for the exponential moving average. 48 | """ 49 | step = max(0, optimization_step - self.update_after_step - 1) 50 | value = 1 - (1 + step / self.inv_gamma) ** -self.power 51 | 52 | if step <= 0: 53 | return 0.0 54 | 55 | return max(self.min_value, min(value, self.max_value)) 56 | 57 | @torch.no_grad() 58 | def step(self, new_model): 59 | self.decay = self.get_decay(self.optimization_step) 60 | 61 | # old_all_dataptrs = set() 62 | # for param in new_model.parameters(): 63 | # data_ptr = param.data_ptr() 64 | # if data_ptr != 0: 65 | # old_all_dataptrs.add(data_ptr) 66 | 67 | all_dataptrs = set() 68 | for module, ema_module in zip( 69 | new_model.modules(), self.averaged_model.modules() 70 | ): 71 | for param, ema_param in zip( 72 | module.parameters(recurse=False), ema_module.parameters(recurse=False) 73 | ): 74 | # iterative over immediate parameters only. 75 | if isinstance(param, dict): 76 | raise RuntimeError("Dict parameter not supported") 77 | 78 | # data_ptr = param.data_ptr() 79 | # if data_ptr != 0: 80 | # all_dataptrs.add(data_ptr) 81 | 82 | if isinstance(module, _BatchNorm): 83 | # skip batchnorms 84 | ema_param.copy_(param.to(dtype=ema_param.dtype).data) 85 | elif not param.requires_grad: 86 | ema_param.copy_(param.to(dtype=ema_param.dtype).data) 87 | else: 88 | ema_param.mul_(self.decay) 89 | ema_param.add_( 90 | param.data.to(dtype=ema_param.dtype), alpha=1 - self.decay 91 | ) 92 | 93 | # verify that iterating over module and then parameters is identical to parameters recursively. 94 | # assert old_all_dataptrs == all_dataptrs 95 | self.optimization_step += 1 96 | -------------------------------------------------------------------------------- /equibot/policies/utils/diffusion/lr_scheduler.py: -------------------------------------------------------------------------------- 1 | from diffusers.optimization import ( 2 | Union, 3 | SchedulerType, 4 | Optional, 5 | Optimizer, 6 | TYPE_TO_SCHEDULER_FUNCTION, 7 | ) 8 | 9 | 10 | def get_scheduler( 11 | name: Union[str, SchedulerType], 12 | optimizer: Optimizer, 13 | num_warmup_steps: Optional[int] = None, 14 | num_training_steps: Optional[int] = None, 15 | **kwargs, 16 | ): 17 | """ 18 | Added kwargs vs diffuser's original implementation 19 | 20 | Unified API to get any scheduler from its name. 21 | 22 | Args: 23 | name (`str` or `SchedulerType`): 24 | The name of the scheduler to use. 25 | optimizer (`torch.optim.Optimizer`): 26 | The optimizer that will be used during training. 27 | num_warmup_steps (`int`, *optional*): 28 | The number of warmup steps to do. This is not required by all schedulers (hence the argument being 29 | optional), the function will raise an error if it's unset and the scheduler type requires it. 30 | num_training_steps (`int``, *optional*): 31 | The number of training steps to do. This is not required by all schedulers (hence the argument being 32 | optional), the function will raise an error if it's unset and the scheduler type requires it. 33 | """ 34 | name = SchedulerType(name) 35 | schedule_func = TYPE_TO_SCHEDULER_FUNCTION[name] 36 | if name == SchedulerType.CONSTANT: 37 | return schedule_func(optimizer, **kwargs) 38 | 39 | # All other schedulers require `num_warmup_steps` 40 | if num_warmup_steps is None: 41 | raise ValueError( 42 | f"{name} requires `num_warmup_steps`, please provide that argument." 43 | ) 44 | 45 | if name == SchedulerType.CONSTANT_WITH_WARMUP: 46 | return schedule_func(optimizer, num_warmup_steps=num_warmup_steps, **kwargs) 47 | 48 | # All other schedulers require `num_training_steps` 49 | if num_training_steps is None: 50 | raise ValueError( 51 | f"{name} requires `num_training_steps`, please provide that argument." 52 | ) 53 | 54 | return schedule_func( 55 | optimizer, 56 | num_warmup_steps=num_warmup_steps, 57 | num_training_steps=num_training_steps, 58 | **kwargs, 59 | ) 60 | -------------------------------------------------------------------------------- /equibot/policies/utils/diffusion/positional_embedding.py: -------------------------------------------------------------------------------- 1 | import math 2 | import torch 3 | import torch.nn as nn 4 | 5 | 6 | class SinusoidalPosEmb(nn.Module): 7 | def __init__(self, dim): 8 | super().__init__() 9 | self.dim = dim 10 | 11 | def forward(self, x): 12 | device = x.device 13 | half_dim = self.dim // 2 14 | emb = math.log(10000) / (half_dim - 1) 15 | emb = torch.exp(torch.arange(half_dim, device=device) * -emb) 16 | emb = x[:, None] * emb[None, :] 17 | emb = torch.cat((emb.sin(), emb.cos()), dim=-1) 18 | return emb 19 | -------------------------------------------------------------------------------- /equibot/policies/utils/diffusion/resnet_with_gn.py: -------------------------------------------------------------------------------- 1 | import torch 2 | import torchvision 3 | from torch import nn 4 | 5 | from typing import Callable 6 | 7 | 8 | def get_resnet(name: str, weights=None, **kwargs) -> nn.Module: 9 | """ 10 | name: resnet18, resnet34, resnet50 11 | weights: "IMAGENET1K_V1", None 12 | """ 13 | # Use standard ResNet implementation from torchvision 14 | func = getattr(torchvision.models, name) 15 | resnet = func(weights=weights, **kwargs) 16 | 17 | # remove the final fully connected layer 18 | # for resnet18, the output dim should be 512 19 | resnet.fc = torch.nn.Identity() 20 | return resnet 21 | 22 | 23 | def replace_submodules( 24 | root_module: nn.Module, 25 | predicate: Callable[[nn.Module], bool], 26 | func: Callable[[nn.Module], nn.Module], 27 | ) -> nn.Module: 28 | """ 29 | Replace all submodules selected by the predicate with 30 | the output of func. 31 | 32 | predicate: Return true if the module is to be replaced. 33 | func: Return new module to use. 34 | """ 35 | if predicate(root_module): 36 | return func(root_module) 37 | 38 | bn_list = [ 39 | k.split(".") 40 | for k, m in root_module.named_modules(remove_duplicate=True) 41 | if predicate(m) 42 | ] 43 | for *parent, k in bn_list: 44 | parent_module = root_module 45 | if len(parent) > 0: 46 | parent_module = root_module.get_submodule(".".join(parent)) 47 | if isinstance(parent_module, nn.Sequential): 48 | src_module = parent_module[int(k)] 49 | else: 50 | src_module = getattr(parent_module, k) 51 | tgt_module = func(src_module) 52 | if isinstance(parent_module, nn.Sequential): 53 | parent_module[int(k)] = tgt_module 54 | else: 55 | setattr(parent_module, k, tgt_module) 56 | # verify that all modules are replaced 57 | bn_list = [ 58 | k.split(".") 59 | for k, m in root_module.named_modules(remove_duplicate=True) 60 | if predicate(m) 61 | ] 62 | assert len(bn_list) == 0 63 | return root_module 64 | 65 | 66 | def replace_bn_with_gn( 67 | root_module: nn.Module, features_per_group: int = 16 68 | ) -> nn.Module: 69 | """ 70 | Relace all BatchNorm layers with GroupNorm. 71 | """ 72 | replace_submodules( 73 | root_module=root_module, 74 | predicate=lambda x: isinstance(x, nn.BatchNorm2d), 75 | func=lambda x: nn.GroupNorm( 76 | num_groups=x.num_features // features_per_group, num_channels=x.num_features 77 | ), 78 | ) 79 | return root_module 80 | -------------------------------------------------------------------------------- /equibot/policies/utils/equivariant_diffusion/conditional_unet1d.py: -------------------------------------------------------------------------------- 1 | from typing import Union 2 | import logging 3 | import torch 4 | import torch.nn as nn 5 | import einops 6 | from einops.layers.torch import Rearrange 7 | 8 | from equibot.policies.utils.equivariant_diffusion.conv1d_components import ( 9 | VecDownsample1d, 10 | VecUpsample1d, 11 | VecConv1dBlock, 12 | ) 13 | from equibot.policies.utils.diffusion.positional_embedding import SinusoidalPosEmb 14 | from equibot.policies.vision.vec_layers import VecLinNormAct as VecLNA 15 | from equibot.policies.vision.vec_layers import VecLinear 16 | 17 | logger = logging.getLogger(__name__) 18 | 19 | 20 | class VecConditionalResidualBlock1D(nn.Module): 21 | def __init__( 22 | self, 23 | in_channels, 24 | out_channels, 25 | vec_cond_dim, 26 | scalar_cond_dim=0, 27 | kernel_size=3, 28 | cond_predict_scale=False, 29 | ): 30 | # input dimensionality: () 31 | super().__init__() 32 | 33 | self.blocks = nn.ModuleList( 34 | [ 35 | VecConv1dBlock(in_channels, out_channels, kernel_size), 36 | VecConv1dBlock(out_channels, out_channels, kernel_size), 37 | ] 38 | ) 39 | 40 | # FiLM modulation https://arxiv.org/abs/1709.07871 41 | # predicts per-channel scale and bias 42 | cond_channels = out_channels 43 | if cond_predict_scale: 44 | cond_channels = out_channels * 2 45 | self.cond_predict_scale = cond_predict_scale 46 | self.out_channels = out_channels 47 | 48 | act_func = nn.Mish() 49 | vnla_cfg = dict(mode="so3", s_in=scalar_cond_dim, return_tuple=False) 50 | self.cond_encoder_l1 = VecLNA(vec_cond_dim, cond_channels, act_func, **vnla_cfg) 51 | self.cond_encoder_l2 = VecLinear( 52 | cond_channels, out_channels, s_out=out_channels if cond_predict_scale else 0 53 | ) 54 | 55 | # make sure dimensions compatible 56 | self.residual_conv = ( 57 | VecDownsample1d(in_channels, out_channels, 1, 1, 0) 58 | if in_channels != out_channels 59 | else nn.Identity() 60 | ) 61 | 62 | def forward(self, x, vec_cond, scalar_cond=None): 63 | """ 64 | x : [ batch_size x in_channels x 3 x horizon ] 65 | vec_cond : [ batch_size x vec_cond_dim x 3] 66 | scalar_cond : [ batch_size x scalar_cond_dim ] 67 | 68 | returns: 69 | out : [ batch_size x out_channels x 3 x horizon ] 70 | """ 71 | out = self.blocks[0](x) 72 | bias, scale = self.cond_encoder_l2( 73 | self.cond_encoder_l1(vec_cond, s=scalar_cond) 74 | ) 75 | if self.cond_predict_scale: 76 | out = scale[..., None, None] * out + bias[..., None] 77 | else: 78 | out = out + bias[..., None] 79 | out = self.blocks[1](out) 80 | out = out + self.residual_conv(x) 81 | return out 82 | 83 | 84 | class VecConditionalUnet1D(nn.Module): 85 | def __init__( 86 | self, 87 | input_dim, 88 | cond_dim, 89 | scalar_input_dim=0, 90 | scalar_cond_dim=0, 91 | diffusion_step_embed_dim=256, 92 | down_dims=[256, 512, 1024], 93 | kernel_size=3, 94 | cond_predict_scale=False, 95 | ): 96 | super().__init__() 97 | all_dims = [input_dim] + list(down_dims) 98 | start_dim = down_dims[0] 99 | 100 | dsed = diffusion_step_embed_dim 101 | diffusion_step_encoder = nn.Sequential( 102 | SinusoidalPosEmb(dsed), 103 | nn.Linear(dsed, dsed * 4), 104 | nn.Mish(), 105 | nn.Linear(dsed * 4, dsed), 106 | ) 107 | 108 | vec_cond_dim = cond_dim 109 | scalar_cond_dim = dsed + scalar_cond_dim 110 | 111 | if scalar_input_dim > 0: 112 | self.scalar_fusion_module = VecLNA( 113 | input_dim, 114 | down_dims[0], 115 | act_func=nn.Mish(), 116 | s_in=scalar_input_dim, 117 | mode="so3", 118 | return_tuple=False, 119 | ) 120 | all_dims[0] = down_dims[0] 121 | 122 | in_out = list(zip(all_dims[:-1], all_dims[1:])) 123 | 124 | make_res_block = lambda din, dout: VecConditionalResidualBlock1D( 125 | din, 126 | dout, 127 | vec_cond_dim, 128 | scalar_cond_dim=scalar_cond_dim, 129 | kernel_size=kernel_size, 130 | cond_predict_scale=cond_predict_scale, 131 | ) 132 | 133 | mid_dim = all_dims[-1] 134 | self.mid_modules = nn.ModuleList( 135 | [make_res_block(mid_dim, mid_dim), make_res_block(mid_dim, mid_dim)] 136 | ) 137 | 138 | down_modules = nn.ModuleList([]) 139 | for ind, (dim_in, dim_out) in enumerate(in_out): 140 | is_last = ind >= (len(in_out) - 1) 141 | down_modules.append( 142 | nn.ModuleList( 143 | [ 144 | make_res_block(dim_in, dim_out), 145 | make_res_block(dim_out, dim_out), 146 | VecDownsample1d(dim_out) if not is_last else nn.Identity(), 147 | ] 148 | ) 149 | ) 150 | 151 | up_modules = nn.ModuleList([]) 152 | for ind, (dim_in, dim_out) in enumerate(reversed(in_out[1:])): 153 | is_last = ind >= (len(in_out) - 1) 154 | up_modules.append( 155 | nn.ModuleList( 156 | [ 157 | make_res_block(dim_out * 2, dim_in), 158 | make_res_block(dim_in, dim_in), 159 | VecUpsample1d(dim_in) if not is_last else nn.Identity(), 160 | ] 161 | ) 162 | ) 163 | 164 | final_conv = nn.Sequential( 165 | VecConv1dBlock(start_dim, start_dim, kernel_size=kernel_size), 166 | VecDownsample1d(start_dim, input_dim, 1, 1, 0), 167 | ) 168 | 169 | if scalar_input_dim > 0: 170 | self.final_linear = VecLinear(start_dim, 0, s_out=scalar_input_dim) 171 | 172 | self.diffusion_step_encoder = diffusion_step_encoder 173 | self.up_modules = up_modules 174 | self.down_modules = down_modules 175 | self.final_conv = final_conv 176 | 177 | logger.info( 178 | "number of parameters: %e", sum(p.numel() for p in self.parameters()) 179 | ) 180 | 181 | def forward( 182 | self, 183 | sample: torch.Tensor, 184 | timestep: Union[torch.Tensor, float, int], 185 | scalar_sample=None, 186 | cond=None, 187 | scalar_cond=None, 188 | **kwargs 189 | ): 190 | """ 191 | sample: (B,T,input_dim,3) 192 | timestep: (B,) or int, diffusion step 193 | scalar_sample: (B,T,input_dim) 194 | cond: (B,cond_dim,3) 195 | scalar_cond: (B,cond_dim) 196 | output: (B,T,input_dim) 197 | """ 198 | sample = einops.rearrange(sample, "b h t v -> b t v h") 199 | if scalar_sample is not None: 200 | scalar_sample = einops.rearrange(scalar_sample, "b h t -> b t h") 201 | 202 | # 1. encode timestep 203 | timesteps = timestep 204 | if not torch.is_tensor(timesteps): 205 | # TODO: this requires sync between CPU and GPU. So try to pass timesteps as tensors if you can 206 | timesteps = torch.tensor( 207 | [timesteps], dtype=torch.long, device=sample.device 208 | ) 209 | elif torch.is_tensor(timesteps) and len(timesteps.shape) == 0: 210 | timesteps = timesteps[None].to(sample.device) 211 | # broadcast to batch dimension in a way that's compatible with ONNX/Core ML 212 | timesteps = timesteps.expand(sample.shape[0]) 213 | 214 | # 2. conditioning 215 | assert cond is not None 216 | vec_feature = cond 217 | scalar_feature = self.diffusion_step_encoder(timesteps) # (B, dsed) 218 | if scalar_cond is not None: 219 | scalar_feature = torch.cat([scalar_feature, scalar_cond], dim=-1) 220 | 221 | # 3. forward pass through the unet 222 | # 3.1 fuse vector and scalar samples if needed 223 | if scalar_sample is not None: 224 | assert hasattr(self, "scalar_fusion_module") 225 | sample = self.scalar_fusion_module(sample, scalar_sample) 226 | 227 | # 3.2 unet 228 | x = sample 229 | h = [] 230 | for idx, (resnet, resnet2, downsample) in enumerate(self.down_modules): 231 | x = resnet(x, vec_feature, scalar_feature) 232 | x = resnet2(x, vec_feature, scalar_feature) 233 | h.append(x) 234 | x = downsample(x) 235 | 236 | for mid_module in self.mid_modules: 237 | x = mid_module(x, vec_feature, scalar_feature) 238 | 239 | for idx, (resnet, resnet2, upsample) in enumerate(self.up_modules): 240 | x = torch.cat((x, h.pop()), dim=1) 241 | x = resnet(x, vec_feature, scalar_feature) 242 | x = resnet2(x, vec_feature, scalar_feature) 243 | x = upsample(x) 244 | 245 | if scalar_sample is not None: 246 | assert hasattr(self, "final_linear") 247 | x_scalar = self.final_linear(x)[1] 248 | x_scalar = einops.rearrange(x_scalar, "b t h -> b h t") 249 | x = self.final_conv(x) 250 | x = einops.rearrange(x, "b t v h -> b h t v") 251 | 252 | if scalar_sample is not None: 253 | return x, x_scalar 254 | else: 255 | return x, None 256 | -------------------------------------------------------------------------------- /equibot/policies/utils/equivariant_diffusion/conv1d_components.py: -------------------------------------------------------------------------------- 1 | import torch 2 | import torch.nn as nn 3 | import torch.nn.functional as F 4 | 5 | from equibot.policies.vision.vec_layers import VecActivation 6 | 7 | 8 | class VecDownsample1d(nn.Module): 9 | def __init__(self, in_dim, out_dim=None, kernel_size=3, stride=2, padding=1): 10 | # input: (N, C_in, 3, L) 11 | super().__init__() 12 | if out_dim is None: 13 | out_dim = in_dim 14 | 15 | # note: we need to omit the bias term to maintain equivariance 16 | self.conv = nn.Conv1d(in_dim, out_dim, kernel_size, stride, padding, bias=False) 17 | 18 | def forward(self, x): 19 | # put vector dimension inside batch dimension 20 | original_shape = x.shape 21 | x_flattened = torch.transpose(x, 1, 2) 22 | x_flattened = x_flattened.reshape(-1, *x_flattened.shape[2:]) 23 | # pass processed input through convolution layer 24 | out = self.conv(x_flattened) 25 | # process convolved input back to vector format 26 | out = torch.transpose(out.reshape(-1, 3, *out.shape[1:]), 1, 2) 27 | return out 28 | 29 | 30 | class VecUpsample1d(VecDownsample1d): 31 | def __init__(self, dim): 32 | nn.Module.__init__(self) 33 | self.conv = nn.ConvTranspose1d(dim, dim, 4, 2, 1, bias=False) 34 | 35 | 36 | class VecConv1dBlock(nn.Module): 37 | """ 38 | Conv1d --> GroupNorm --> Mish 39 | """ 40 | 41 | def __init__(self, inp_channels, out_channels, kernel_size): 42 | # input dimension: (N, C_in, 3, L) 43 | super().__init__() 44 | 45 | # Note: we cannot have normalization layers here because it breaks scale equivariance 46 | self.block = nn.Sequential( 47 | VecDownsample1d( 48 | inp_channels, out_channels, kernel_size, 1, kernel_size // 2 49 | ), 50 | VecActivation( 51 | out_channels, 52 | nn.Mish(), 53 | shared_nonlinearity=False, 54 | mode="so3", 55 | cross=False, 56 | ), 57 | ) 58 | 59 | def forward(self, x): 60 | return self.block(x) 61 | 62 | 63 | def test(): 64 | cb = VecConv1dBlock(256, 128, kernel_size=3) 65 | x = torch.zeros((1, 256, 3, 16)) 66 | o = cb(x) 67 | -------------------------------------------------------------------------------- /equibot/policies/utils/media.py: -------------------------------------------------------------------------------- 1 | """ 2 | Utilities for loading, saving, and manipulating videos and images. 3 | 4 | @yjy0625 5 | 6 | """ 7 | 8 | import os 9 | import numpy as np 10 | import cv2 11 | import skvideo.io 12 | import imageio 13 | 14 | 15 | def _make_dir(filename): 16 | folder = os.path.dirname(filename) 17 | os.makedirs(folder, exist_ok=True) 18 | 19 | 20 | def save_image(image, filename): 21 | _make_dir(filename) 22 | if np.max(image) < 2: 23 | image = np.array(image * 255) 24 | image = image.astype(np.uint8) 25 | cv2.imwrite(filename, image[..., ::-1]) 26 | 27 | 28 | def save_gif(video_array, file_path, fps=10): 29 | """ 30 | Save a (T, H, W, 3) numpy array of video into a GIF file. 31 | 32 | Parameters: 33 | video_array (numpy.ndarray): The video as a 4D numpy array with shape (T, H, W, 3). 34 | file_path (str): The file path where the GIF will be saved. 35 | fps (int, optional): Frames per second for the GIF. Default is 10. 36 | """ 37 | try: 38 | # Ensure the video array is uint8 (required for GIF) 39 | video_array = (255 * (1.0 - video_array)).astype("uint8") 40 | 41 | # Save the GIF 42 | imageio.mimsave(file_path, video_array, duration=len(video_array) / fps, loop=1) 43 | print(f"Saved GIF to {file_path}") 44 | except Exception as e: 45 | print(f"Error saving GIF: {e}") 46 | 47 | 48 | def save_video(video_frames, filename, fps=10, video_format="mp4"): 49 | if len(video_frames) == 0: 50 | return False 51 | 52 | assert fps == int(fps), fps 53 | _make_dir(filename) 54 | 55 | skvideo.io.vwrite( 56 | filename, 57 | video_frames, 58 | inputdict={ 59 | "-r": str(int(fps)), 60 | }, 61 | outputdict={"-f": video_format, "-pix_fmt": "yuv420p"}, 62 | ) 63 | 64 | return True 65 | 66 | 67 | def read_video(filename): 68 | return skvideo.io.vread(filename) 69 | 70 | 71 | def get_video_framerate(filename): 72 | videometadata = skvideo.io.ffprobe(filename) 73 | frame_rate = videometadata["video"]["@avg_frame_rate"] 74 | return eval(frame_rate) 75 | 76 | 77 | def add_caption_to_img(img, info, name=None, flip_rgb=False, num_lines=5): 78 | """Adds caption to an image. info is dict with keys and text/array. 79 | :arg name: if given this will be printed as heading in the first line 80 | :arg flip_rgb: set to True for inputs with BGR color channels 81 | """ 82 | mul = 2.0 83 | offset = int(12 * mul) 84 | 85 | frame = img * 255.0 if img.max() <= 1.0 else img 86 | if flip_rgb: 87 | frame = frame[:, :, ::-1] 88 | 89 | # colors 90 | blue = (66, 133, 244) 91 | yellow = (255, 255, 0) 92 | white = (255, 255, 255) 93 | 94 | # add border to frame if success 95 | if "is_success" in info.keys() and info["is_success"]: 96 | border_size = int(10 * mul) 97 | frame[:border_size, :] = np.array(blue)[None][None] 98 | frame[-border_size:, :] = np.array(blue)[None][None] 99 | frame[:, :border_size] = np.array(blue)[None][None] 100 | frame[:, -border_size:] = np.array(blue)[None][None] 101 | 102 | fheight, fwidth = frame.shape[:2] 103 | pad_height = int(offset * (num_lines + 2)) 104 | frame = np.concatenate([frame, np.zeros((pad_height, fwidth, 3))], 0) 105 | 106 | font_size = 0.4 * mul 107 | thickness = int(1 * mul) 108 | x, y = int(5 * mul), fheight + int(10 * mul) 109 | if name is not None: 110 | cv2.putText( 111 | frame, 112 | "[{}]".format(name), 113 | (x, y), 114 | cv2.FONT_HERSHEY_SIMPLEX, 115 | font_size, 116 | yellow, 117 | thickness, 118 | cv2.LINE_AA, 119 | ) 120 | for i, k in enumerate(info.keys()): 121 | v = info[k] 122 | if ( 123 | type(v) == np.ndarray 124 | or type(v) == np.float64 125 | or type(v) == np.float32 126 | or type(v) == float 127 | ): 128 | if type(v) == np.ndarray: 129 | v = np.round(v, 3 if np.isscalar(v) or len(v) <= 3 else 2) 130 | else: 131 | v = np.round(v, 3) 132 | key_text = "{}: ".format(k) 133 | (key_width, _), _ = cv2.getTextSize( 134 | key_text, cv2.FONT_HERSHEY_SIMPLEX, font_size, thickness 135 | ) 136 | 137 | cv2.putText( 138 | frame, 139 | key_text, 140 | (x, y + offset * (i + 2)), 141 | cv2.FONT_HERSHEY_SIMPLEX, 142 | font_size, 143 | blue, 144 | thickness, 145 | cv2.LINE_AA, 146 | ) 147 | 148 | cv2.putText( 149 | frame, 150 | str(v), 151 | (x + key_width, y + offset * (i + 2)), 152 | cv2.FONT_HERSHEY_SIMPLEX, 153 | font_size, 154 | white, 155 | thickness, 156 | cv2.LINE_AA, 157 | ) 158 | 159 | if flip_rgb: 160 | frame = frame[:, :, ::-1] 161 | 162 | return frame 163 | 164 | 165 | def add_border_to_img(frame, color=(234, 67, 53)): 166 | border_size = 20 167 | if np.max(frame) <= 1.0: 168 | color = tuple([float(x) / 255 for x in color]) 169 | frame[:border_size, :] = np.array(color)[None][None] 170 | frame[-border_size:, :] = np.array(color)[None][None] 171 | frame[:, :border_size] = np.array(color)[None][None] 172 | frame[:, -border_size:] = np.array(color)[None][None] 173 | return frame 174 | 175 | 176 | def add_border_to_video(frames, color=(234, 67, 53)): 177 | border_size = 20 178 | if np.max(frames) <= 1.0: 179 | color = tuple([float(x) / 255 for x in color]) 180 | frames[:, :border_size, :] = np.array(color)[None][None][None] 181 | frames[:, -border_size:, :] = np.array(color)[None][None][None] 182 | frames[:, :, :border_size] = np.array(color)[None][None][None] 183 | frames[:, :, -border_size:] = np.array(color)[None][None][None] 184 | return frames 185 | 186 | 187 | def combine_videos(images, num_cols=5): 188 | if len(images) == 1: 189 | return np.array(images[0]) 190 | max_frames = np.max([len(im) for im in images]) 191 | images = [ 192 | np.concatenate([im[:-1], np.array([im[-1]] * (max_frames - len(im) + 1))]) 193 | for im in images 194 | ] 195 | images = np.array(images) 196 | B = images.shape[0] 197 | if B % num_cols != 0: 198 | images = np.concatenate( 199 | [images, np.zeros((num_cols - (B % num_cols),) + tuple(images.shape[1:]))] 200 | ) 201 | B, T, H, W, C = images.shape 202 | images = images.reshape(B // num_cols, num_cols, T, H, W, C).transpose( 203 | 2, 0, 3, 1, 4, 5 204 | ) 205 | images = images.reshape(T, B // num_cols * H, num_cols * W, C) 206 | return images 207 | -------------------------------------------------------------------------------- /equibot/policies/utils/misc.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import torch 3 | 4 | 5 | def to_torch(batch, device): 6 | return {k: v.to(device) for k, v in batch.items()} 7 | 8 | 9 | def rotate_around_z( 10 | points, 11 | angle_rad=0.0, 12 | center=np.array([0.0, 0.0, 0.0]), 13 | scale=np.array([1.0, 1.0, 1.0]), 14 | ): 15 | # Check if the input points have the correct shape (N, 3) 16 | assert (len(points.shape) == 1 and len(points) == 3) or points.shape[-1] == 3 17 | p_shape = points.shape 18 | points = points.reshape(-1, 3) - center[None] 19 | 20 | # Create the rotation matrix 21 | cos_theta = np.cos(angle_rad) 22 | sin_theta = np.sin(angle_rad) 23 | rotation_matrix = np.array( 24 | [[cos_theta, -sin_theta, 0], [sin_theta, cos_theta, 0], [0, 0, 1]] 25 | ) 26 | 27 | # Apply the rotation to all points using matrix multiplication 28 | rotated_points = np.dot(points, rotation_matrix.T) * scale[None] + center[None] 29 | rotated_points = rotated_points.reshape(p_shape) 30 | 31 | return rotated_points 32 | 33 | 34 | def get_env_class(env_name): 35 | if env_name == "fold": 36 | from equibot.envs.sim_mobile.folding_env import FoldingEnv 37 | return FoldingEnv 38 | elif env_name == "cover": 39 | from equibot.envs.sim_mobile.covering_env import CoveringEnv 40 | return CoveringEnv 41 | elif env_name == "close": 42 | from equibot.envs.sim_mobile.closing_env import ClosingEnv 43 | return ClosingEnv 44 | else: 45 | raise ValueError() 46 | 47 | 48 | def get_dataset(cfg, mode="train"): 49 | from equibot.policies.datasets.dataset import BaseDataset 50 | return BaseDataset(cfg.data.dataset, mode) 51 | 52 | 53 | def get_agent(agent_name): 54 | if agent_name == "dp": 55 | from equibot.policies.agents.dp_agent import DPAgent 56 | return DPAgent 57 | elif agent_name == "equibot": 58 | from equibot.policies.agents.equibot_agent import EquiBotAgent 59 | return EquiBotAgent 60 | else: 61 | raise ValueError(f"Agent with name [{agent_name}] not found.") 62 | -------------------------------------------------------------------------------- /equibot/policies/utils/norm.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import torch 3 | 4 | 5 | class Normalizer(object): 6 | def __init__(self, data, symmetric=False, indices=None): 7 | if isinstance(data, dict): 8 | # load from existing data statistics 9 | self.stats = data 10 | elif symmetric: 11 | # just scaling applied in normalization, no bias 12 | # perform the same normalization in groups 13 | if indices is None: 14 | indices = np.arange(data.shape[-1])[None] 15 | 16 | self.stats = { 17 | "min": torch.zeros([data.shape[-1]]).to(data.device), 18 | "max": torch.ones([data.shape[-1]]).to(data.device), 19 | } 20 | for group in indices: 21 | max_abs = torch.abs(data[:, group]).max(0)[0].detach() 22 | limits = torch.ones_like(max_abs) * torch.max(max_abs) 23 | self.stats["max"][group] = limits 24 | else: 25 | mask = torch.zeros([data.shape[-1]]).to(data.device) 26 | if indices is not None: 27 | mask[indices.flatten()] += 1 28 | else: 29 | mask += 1 30 | self.stats = { 31 | "min": data.min(0)[0].detach() * mask, 32 | "max": data.max(0)[0].detach() * mask + 1.0 * (1 - mask), 33 | } 34 | 35 | def normalize(self, data): 36 | nd = len(data.shape) 37 | target_shape = (1,) * (nd - 1) + (data.shape[-1],) 38 | dmin = self.stats["min"].reshape(target_shape) 39 | dmax = self.stats["max"].reshape(target_shape) 40 | return (data - dmin) / (dmax - dmin + 1e-12) 41 | 42 | def unnormalize(self, data): 43 | nd = len(data.shape) 44 | target_shape = (1,) * (nd - 1) + (data.shape[-1],) 45 | dmin = self.stats["min"].reshape(target_shape) 46 | dmax = self.stats["max"].reshape(target_shape) 47 | return data * (dmax - dmin) + dmin 48 | 49 | def state_dict(self): 50 | return self.stats 51 | 52 | def load_state_dict(self, state_dict): 53 | self.stats = state_dict 54 | -------------------------------------------------------------------------------- /equibot/policies/vec_eval.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | import torch 4 | import hydra 5 | import omegaconf 6 | import wandb 7 | import numpy as np 8 | import getpass as gt 9 | from tqdm import tqdm 10 | 11 | from equibot.policies.eval import organize_obs 12 | from equibot.policies.utils.media import combine_videos 13 | 14 | 15 | def run_eval( 16 | env, 17 | agent, 18 | vis=False, 19 | num_episodes=1, 20 | log_dir=None, 21 | reduce_horizon_dim=True, 22 | verbose=False, 23 | use_wandb=False, 24 | ckpt_name=None, 25 | ): 26 | if hasattr(agent, "obs_horizon") and hasattr(agent, "ac_horizon"): 27 | obs_horizon = agent.obs_horizon 28 | ac_horizon = agent.ac_horizon 29 | else: 30 | obs_horizon = 1 31 | ac_horizon = 1 32 | 33 | images = [] 34 | obs_history = [] 35 | num_envs = len(env.remotes) 36 | for i in range(num_envs): 37 | images.append([]) 38 | state = env.reset() 39 | 40 | env_module_name = env.get_attr("__module__")[0] 41 | 42 | pred_horizon = agent.pred_horizon if hasattr(agent, "pred_horizon") else 1 43 | rgb_render = render = env.env_method("render") 44 | obs = organize_obs(render, rgb_render, state) 45 | for i in range(obs_horizon): 46 | obs_history.append(obs) 47 | for i in range(num_envs): 48 | images[i].append(rgb_render[i]["images"][0][..., :3]) 49 | 50 | sample_pc = render[0]["pc"] 51 | mean_num_points_in_pc = np.mean([len(render[k]["pc"]) for k in range(len(render))]) 52 | 53 | done = [False] * num_envs 54 | if log_dir is not None: 55 | history = [] 56 | for i in range(num_envs): 57 | history.append(dict(action=[], eef_pos=[])) 58 | t = 0 59 | pbar = tqdm( 60 | list(range(env.get("args").max_episode_length // ac_horizon)), 61 | leave=False, 62 | desc="Vec Eval", 63 | ) 64 | while not np.all(done): 65 | # make obs for agent 66 | if obs_horizon == 1 and reduce_horizon_dim: 67 | agent_obs = obs 68 | else: 69 | agent_obs = dict() 70 | for k in obs.keys(): 71 | if k == "pc": 72 | # point clouds can have different number of points 73 | # so do not stack them 74 | agent_obs[k] = [o[k] for o in obs_history[-obs_horizon:]] 75 | else: 76 | agent_obs[k] = np.stack([o[k] for o in obs_history[-obs_horizon:]]) 77 | 78 | # predict actions 79 | ac = agent.act( 80 | agent_obs 81 | ) # (num_envs, agent.pred_horizon, num_eef * dof) 82 | 83 | if log_dir is not None: 84 | for i in range(num_envs): 85 | history[i]["action"].append(ac[i]) 86 | history[i]["eef_pos"].append(obs["state"][i]) 87 | 88 | # take actions 89 | for ac_ix in range(ac_horizon): 90 | agent_ac = ac[:, ac_ix] if len(ac.shape) > 2 else ac 91 | env.step_async(agent_ac, dummy_reward=True) 92 | state, _, done, _ = env.step_wait() 93 | rgb_render = render = env.env_method("render") 94 | obs = organize_obs(render, rgb_render, state) 95 | obs_history.append(obs) 96 | if len(obs) > obs_horizon: 97 | obs_history = obs_history[-obs_horizon:] 98 | for i in range(num_envs): 99 | images[i].append(rgb_render[i]["images"][0][..., :3]) 100 | t += 1 101 | pbar.update(1) 102 | pbar.close() 103 | rews = np.array(env.env_method("compute_reward")) 104 | print(f"Episode rewards: {rews.round(3)}.") 105 | 106 | if log_dir is not None: 107 | os.makedirs(log_dir, exist_ok=True) 108 | for ep_ix in range(num_envs): 109 | np.savez( 110 | os.path.join( 111 | log_dir, f"eval_{ckpt_name}_ep{ep_ix:02d}_rew{rews[ep_ix]:.3f}.npz" 112 | ), 113 | action=np.array(history[ep_ix]["action"]), 114 | eef_pos=np.array(history[ep_ix]["eef_pos"]), 115 | ) 116 | 117 | images = np.array(images) 118 | metrics = dict(rew=np.mean(rews)) 119 | if vis: 120 | vis_frames = images # N, T, H, W, C 121 | vis_rews = np.zeros_like(vis_frames[:, :, :20]) 122 | for i in range(num_envs): 123 | num_pixels = int(vis_frames.shape[-2] * rews[i]) 124 | vis_rews[i, :, 2:, :num_pixels] = 255 125 | vis_frames = np.concatenate([vis_frames, vis_rews], axis=2) 126 | if use_wandb: 127 | metrics["vis_pc"] = wandb.Object3D(sample_pc) 128 | metrics["rew_values"] = rews 129 | metrics["vis_rollout"] = vis_frames 130 | metrics["mean_pc_size"] = mean_num_points_in_pc 131 | return metrics 132 | -------------------------------------------------------------------------------- /equibot/policies/vision/misc.py: -------------------------------------------------------------------------------- 1 | import logging 2 | 3 | 4 | def cfg_with_default(cfg, key_list, default): 5 | root = cfg 6 | for k in key_list: 7 | if k in root.keys(): 8 | root = root[k] 9 | else: 10 | return default 11 | return root 12 | 13 | 14 | def count_param(network_dict): 15 | for k in network_dict: 16 | logging.info( 17 | "{:.3f}M params in {}".format( 18 | sum(param.numel() for param in network_dict[k].parameters()) / 1e6, k 19 | ) 20 | ) 21 | -------------------------------------------------------------------------------- /equibot/policies/vision/pointnet_encoder.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | import numpy as np 4 | import torch 5 | import torch.nn as nn 6 | import torch.nn.functional as F 7 | from pytorch3d.ops.knn import knn_points 8 | import logging 9 | 10 | 11 | def meanpool(x, dim=-1, keepdim=False): 12 | out = x.mean(dim=dim, keepdim=keepdim) 13 | return out 14 | 15 | 16 | class PointNetEncoder(nn.Module): 17 | def __init__(self, h_dim=128, c_dim=128, num_layers=4, **kwargs): 18 | super().__init__() 19 | 20 | self.h_dim = h_dim 21 | self.c_dim = c_dim 22 | self.num_layers = num_layers 23 | 24 | self.pool = meanpool 25 | 26 | act_func = nn.LeakyReLU(negative_slope=0.0, inplace=False) 27 | self.act = act_func 28 | 29 | self.conv_in = nn.Conv1d(3, h_dim, kernel_size=1) 30 | self.layers, self.global_layers = nn.ModuleList(), nn.ModuleList() 31 | for i in range(self.num_layers): 32 | self.layers.append(nn.Conv1d(h_dim, h_dim, kernel_size=1)) 33 | self.global_layers.append(nn.Conv1d(h_dim * 2, h_dim, kernel_size=1)) 34 | self.conv_out = nn.Conv1d(h_dim * self.num_layers, c_dim, kernel_size=1) 35 | 36 | def forward(self, x, ret_perpoint_feat=False): 37 | 38 | y = self.act(self.conv_in(x)) 39 | feat_list = [] 40 | for i in range(self.num_layers): 41 | y = self.act(self.layers[i](y)) 42 | y_global = y.max(-1, keepdim=True).values 43 | y = torch.cat([y, y_global.expand_as(y)], dim=1) 44 | y = self.act(self.global_layers[i](y)) 45 | feat_list.append(y) 46 | x = torch.cat(feat_list, dim=1) 47 | x = self.conv_out(x) 48 | 49 | x_global = x.max(-1).values 50 | 51 | ret = {"global": x_global} 52 | if ret_perpoint_feat: 53 | ret["per_point"] = x.transpose(1, 2) 54 | 55 | return ret 56 | -------------------------------------------------------------------------------- /equibot/policies/vision/sim3_encoder.py: -------------------------------------------------------------------------------- 1 | # using the backbone, different encoders 2 | 3 | import os 4 | import sys 5 | import numpy as np 6 | import torch 7 | import torch.nn as nn 8 | import torch.nn.functional as F 9 | import logging 10 | 11 | from equibot.policies.vision.vec_layers import VecLinear 12 | from equibot.policies.vision.vec_pointnet import VecPointNet 13 | 14 | 15 | BACKBONE_DICT = {"vn_pointnet": VecPointNet} 16 | 17 | NORMALIZATION_METHOD = {"bn": nn.BatchNorm1d, "in": nn.InstanceNorm1d} 18 | 19 | 20 | class SIM3Vec4Latent(nn.Module): 21 | """ 22 | This encoder encode the input point cloud to 4 latents 23 | Now only support so3 mode 24 | TODO: se3 and hybrid 25 | """ 26 | 27 | def __init__( 28 | self, 29 | c_dim, 30 | backbone_type, 31 | backbone_args, 32 | mode="so3", 33 | normalization_method=None, 34 | ): 35 | super().__init__() 36 | assert mode == "so3", NotImplementedError("TODO, add se3") 37 | if normalization_method is not None: 38 | backbone_args["normalization_method"] = NORMALIZATION_METHOD[ 39 | normalization_method 40 | ] 41 | 42 | self.backbone = BACKBONE_DICT[backbone_type](**backbone_args) 43 | self.fc_inv = VecLinear(c_dim, c_dim, mode=mode) 44 | 45 | def forward(self, pcl, ret_perpoint_feat=False, target_norm=1.0): 46 | B, T, N, _ = pcl.shape 47 | pcl = pcl.view(B * T, -1, 3).transpose(1, 2) # [BT, 3, N] 48 | 49 | centroid = pcl.mean(-1, keepdim=True) # B,3,1 50 | input_pcl = pcl - centroid 51 | 52 | z_scale = input_pcl.norm(dim=1).mean(-1) / target_norm # B 53 | z_center = centroid.permute(0, 2, 1) 54 | 55 | input_pcl = input_pcl / z_scale[:, None, None] 56 | 57 | x, x_perpoint = self.backbone(input_pcl) # B,C,3 58 | 59 | z_so3 = x 60 | z_inv_dual, _ = self.fc_inv(x[..., None]) 61 | z_inv_dual = z_inv_dual.squeeze(-1) 62 | z_inv = (z_inv_dual * z_so3).sum(-1) 63 | 64 | ret = { 65 | "inv": z_inv, 66 | "so3": z_so3, 67 | "scale": z_scale, 68 | "center": z_center, 69 | } 70 | 71 | if ret_perpoint_feat: 72 | ret["per_point_so3"] = x_perpoint # [B, C, 3, N] 73 | 74 | return ret 75 | -------------------------------------------------------------------------------- /equibot/policies/vision/vec_pointnet.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | import numpy as np 4 | import torch 5 | import torch.nn as nn 6 | import torch.nn.functional as F 7 | from pytorch3d.ops.knn import knn_points 8 | import logging 9 | 10 | from equibot.policies.vision.vec_layers import VecLinear 11 | from equibot.policies.vision.vec_layers import VecLinNormAct as VecLNA 12 | 13 | 14 | def meanpool(x, dim=-1, keepdim=False): 15 | out = x.mean(dim=dim, keepdim=keepdim) 16 | return out 17 | 18 | 19 | class VecPointNet(nn.Module): 20 | def __init__( 21 | self, 22 | h_dim=128, 23 | c_dim=128, 24 | num_layers=4, 25 | knn=16, 26 | ): 27 | super().__init__() 28 | 29 | self.h_dim = h_dim 30 | self.c_dim = c_dim 31 | self.num_layers = num_layers 32 | self.knn = knn 33 | 34 | self.pool = meanpool 35 | 36 | act_func = nn.LeakyReLU(negative_slope=0.0, inplace=False) 37 | vnla_cfg = {"mode": "so3", "act_func": act_func} 38 | 39 | self.conv_in = VecLNA(3, h_dim, **vnla_cfg) 40 | self.layers, self.global_layers = nn.ModuleList(), nn.ModuleList() 41 | for i in range(self.num_layers): 42 | self.layers.append(VecLNA(h_dim, h_dim, **vnla_cfg)) 43 | self.global_layers.append(VecLNA(h_dim * 2, h_dim, **vnla_cfg)) 44 | self.conv_out = VecLinear(h_dim * self.num_layers, c_dim, mode="so3") 45 | 46 | self.fc_inv = VecLinear(c_dim, 3, mode="so3") 47 | 48 | def get_graph_feature(self, x: torch.Tensor, k: int, knn_idx=None, cross=False): 49 | # x: B,C,3,N return B,C*2,3,N,K 50 | 51 | 52 | B, C, _, N = x.shape 53 | if knn_idx is None: 54 | # if knn_idx is not none, compute the knn by x distance; ndf use fixed knn as input topo 55 | _x = x.reshape(B, -1, N) 56 | _, knn_idx, neighbors = knn_points( 57 | _x.transpose(2, 1), _x.transpose(2, 1), K=k, return_nn=True 58 | ) # B,N,K; B,N,K; B,N,K,D 59 | neighbors = neighbors.reshape(B, N, k, C, 3).permute(0, -2, -1, 1, 2) 60 | else: # gather from the input knn idx 61 | assert knn_idx.shape[-1] == k, f"input knn gather idx should have k={k}" 62 | neighbors = torch.gather( 63 | x[..., None, :].expand(-1, -1, -1, N, -1), 64 | dim=-1, 65 | index=knn_idx[:, None, None, ...].expand(-1, C, 3, -1, -1), 66 | ) # B,C,3,N,K 67 | x_padded = x[..., None].expand_as(neighbors) 68 | 69 | if cross: 70 | x_dir = F.normalize(x, dim=2) 71 | x_dir_padded = x_dir[..., None].expand_as(neighbors) 72 | cross = torch.cross(x_dir_padded, neighbors, dim=2) 73 | y = torch.cat([cross, neighbors - x_padded, x_padded], 1) 74 | else: 75 | y = torch.cat([neighbors - x_padded, x_padded], 1) 76 | return y, knn_idx # B,C*2,3,N,K 77 | 78 | def forward(self, x): 79 | x = x.unsqueeze(1) # [B, 1, 3, N] 80 | 81 | x, knn_idx = self.get_graph_feature(x, self.knn, cross=True) 82 | x, _ = self.conv_in(x) 83 | x = self.pool(x) 84 | 85 | y = x 86 | feat_list = [] 87 | for i in range(self.num_layers): 88 | y, _ = self.layers[i](y) 89 | y_global = y.mean(-1, keepdim=True) 90 | y = torch.cat([y, y_global.expand_as(y)], dim=1) 91 | y, _ = self.global_layers[i](y) 92 | feat_list.append(y) 93 | x = torch.cat(feat_list, dim=1) 94 | x, _ = self.conv_out(x) 95 | 96 | return x.mean(-1), x 97 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | 3 | 4 | setup(name="equibot", 5 | version="0.1", 6 | description="", 7 | author="", 8 | author_email="", 9 | license="", 10 | packages=[], 11 | install_requires=[ 12 | "click", 13 | "matplotlib", 14 | "scipy", 15 | "tqdm", 16 | "opencv-python==4.9.0.80", 17 | "opencv-python-headless==4.9.0.80", 18 | "opencv-contrib-python==4.7.0.72", 19 | "hydra-core", 20 | "wandb", 21 | "chardet", 22 | "trimesh[all]", 23 | "natsort", 24 | "ffmpeg", 25 | "imageio[ffmpeg]", 26 | "ipykernel", 27 | "robosuite", 28 | "gtimer", 29 | "scikit-video", 30 | "einops", 31 | "diffusers", 32 | "gym==0.26.2", 33 | "pybullet==3.2.6", 34 | "protobuf==3.20.0" 35 | ]) 36 | --------------------------------------------------------------------------------