├── .gitignore ├── .gitmodules ├── LICENSE ├── README.md ├── assets ├── demo_embedding1.gif ├── demo_embedding2.gif ├── demo_sampler1.gif ├── demo_sampler2.gif └── hcg.gif ├── core ├── __init__.py ├── agent.py ├── bc.py ├── common_utils.py ├── ddpg.py ├── dqn_hrl.py ├── load_tensorboard.py ├── loss.py ├── network_arch.py ├── networks.py ├── replay_memory.py ├── test_offline.py ├── test_realworld_ros_clutter.py ├── train_offline.py ├── train_online.py ├── trainer.py ├── traj.py └── utils.py ├── env ├── __init__.py ├── env_planner.py ├── models │ └── panda │ │ ├── meshes │ │ ├── collision │ │ │ ├── camera.stl │ │ │ ├── finger.obj │ │ │ ├── hand.obj │ │ │ ├── link0.obj │ │ │ ├── link1.obj │ │ │ ├── link2.obj │ │ │ ├── link3.obj │ │ │ ├── link4.obj │ │ │ ├── link5.obj │ │ │ ├── link6.mtl │ │ │ ├── link6.obj │ │ │ └── link7.obj │ │ └── visual │ │ │ ├── camera.DAE │ │ │ ├── colors.png │ │ │ ├── finger.mtl │ │ │ ├── finger.obj │ │ │ ├── hand.mtl │ │ │ ├── hand.obj │ │ │ ├── link1.mtl │ │ │ ├── link1.obj │ │ │ ├── link2.mtl │ │ │ ├── link2.obj │ │ │ ├── link3.mtl │ │ │ ├── link3.obj │ │ │ ├── link4.mtl │ │ │ ├── link4.obj │ │ │ ├── link5.mtl │ │ │ ├── link5.obj │ │ │ ├── link6.mtl │ │ │ └── link6.obj │ │ └── panda_gripper_hand_camera.urdf ├── panda_cluttered_scene.py └── panda_gripper_hand_camera.py ├── experiments ├── __init__.py ├── cfgs │ ├── bc_embedding_low_level.yaml │ ├── bc_sampler_restep.yaml │ ├── dqn_critic_hrl.yaml │ └── dqn_save_buffer.yaml ├── config.py ├── model_spec │ ├── rl_pointnet_model_spec.yaml │ └── rl_sampler_large_pointnet_model_spec.yaml ├── object_index │ ├── extra_shape.json │ ├── filter_shapenet.json │ ├── ycb_large.json │ └── ycb_large_scene.json └── scripts │ ├── download_data.sh │ ├── download_model.sh │ ├── download_offline_data.sh │ ├── test_demo.sh │ ├── test_ycb.sh │ ├── test_ycb_gen_12_mpc.sh │ ├── train_embedding_offline.sh │ ├── train_embedding_ray_offline.sh │ ├── train_ray_online_save_data.sh │ └── train_sampler_ray_online.sh └── requirements.txt /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | .idea/* 3 | *.so 4 | *egg-info* 5 | *.avi 6 | *.pdf 7 | *.xyz 8 | *.zip 9 | *.obj 10 | output 11 | experiments/runs 12 | data 13 | output_misc 14 | OMG/data 15 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "OMG"] 2 | path = OMG 3 | url = https://github.com/liruiw/OMG-Planner.git 4 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 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 | # Hierarchical Policies for Cluttered-Scene Grasping with Latent Plans 2 | 3 | 4 | [[Website](https://sites.google.com/view/latent-grasping), [Paper](https://arxiv.org/abs/2107.01518)] 5 | 6 | ![image](assets/hcg.gif) 7 | 8 | 9 | ### Installation 10 | ```bash 11 | git clone https://github.com/liruiw/HCG.git --recursive 12 | ``` 13 | 14 | 0. Setup: Ubuntu 16.04 or above, CUDA 10.0 or above, python 2.7 / 3.6 15 | 16 | 1. * (Training) - Install [OMG](https://github.com/liruiw/OMG-Planner) submodule and reuse conda environment. 17 | * (Demo) - Install HCG inside a new conda environment 18 | ```angular2html 19 | conda create --name hcg python=3.6.9 20 | conda activate hcg 21 | pip install -r requirements.txt 22 | ``` 23 | 2. Install [PointNet++](https://github.com/liruiw/Pointnet2_PyTorch) 24 | 25 | 3. Download environment data with ```bash experiments/scripts/download_data.sh ``` 26 | 27 | ### Pretrained Model Demo and Testing 28 | 0. Download pretrained models with ```bash experiments/scripts/download_model.sh ``` 29 | 1. Demo model test ```bash experiments/scripts/test_demo.sh``` 30 | 31 | 32 | ### Save Data and Offline Training 33 | 0. Download example offline data with ```bash experiments/scripts/download_offline_data.sh ``` The .npz dataset (saved replay buffer) can be found in data/offline_data and can be loaded for training. 34 | 1. To train offline latent plan embedding and low-level policy ```bash ./experiments/scripts/train_embedding_ray_offline.sh bc_embedding_low_level.yaml BC``` or without ray. Add pretrained model after the policy for continual training and finetuning. 35 | 2. To train offline joint embedding and latent plan sampler ```bash ./experiments/scripts/train_embedding_ray_offline.sh bc_sampler_restep.yaml BC``` 36 | 3. To generate and save offline dataset ```bash ./experiments/scripts/train_ray_online_save_data.sh dqn_save_buffer.yaml DQN_HRL```. 37 | 38 | ### Online HRL Training 39 | 0. We use [ray](https://github.com/ray-project/ray) for parallel rollout and training. The training scripts might require adjustments based on the local machine. See ```config.py``` for some notes. 40 | 1. To train the latent critic online ```bash ./experiments/scripts/train_sampler_ray_online.sh dqn_critic_hrl.yaml DQN_HRL MODEL```. Use visdom and tensorboard to monitor. 41 | 42 | ### Testing 43 | 0. To test embedding and low-level policy ```bash experiments/scripts/test_ycb.sh demo_model_clutter ```. 44 | 45 | 46 | Example 1 | Example 2 47 | :-------------------------:|:-------------------------: 48 | | 49 | 50 | 51 | 1. To test high-level sampler and critic ```bash experiments/scripts/test_ycb_gen_12_mpc.sh demo_model_clutter ``` 52 | 53 | 54 | Example 1 | Example 2 55 | :-------------------------:|:-------------------------: 56 | | 57 | 58 | 59 | 2. Testing only works with single GPU. Replace demo_model with trained model name. Logs and videos would be saved to ```output_misc``` 60 | 61 | 62 | ### Note 63 | 0. Checkout ```core/test_realworld_ros_clutter.py``` for an example of real-world usages. 64 | 1. Related Works ([OMG](https://github.com/liruiw/OMG-Planner), [GA-DDPG](https://github.com/liruiw/GA-DDPG), [ACRONYM](https://github.com/NVlabs/acronym), [Unseen-Clustering](https://github.com/NVlabs/UnseenObjectClustering)). 65 | 2. To use the full Acronym dataset with Shapenet meshes, please follow [ACRONYM](https://github.com/NVlabs/acronym#using-the-full-acronym-dataset) to download the meshes and grasps and follow [OMG-Planner](https://github.com/liruiw/OMG-Planner#process-new-shapes) to process and save in ```/data```. ```filter_shapenet.json``` can then be used for training. 66 | 3. Please use Github issue tracker to report bugs. For other questions please contact [Lirui Wang](mailto:wangliruisz@gmail.com). 67 | 68 | ### File Structure 69 | ```angular2html 70 | ├── ... 71 | ├── HCG 72 | | |── data # training data 73 | | | |── grasps # grasps of the objects 74 | | | |── objects # object meshes, sdf, urdf, etc 75 | | | |── robots # robot meshes, urdf, etc 76 | | | |── hcg_scenes # test scenes 77 | | | └── ... 78 | | |── env # environment 79 | | | |── env_planner # interface for the expert planner 80 | | | |── panda_cluttered_scene # environment and task 81 | | | └── panda_gripper_hand_camera # franka panda with gripper and camera 82 | | |── OMG # expert planner submodule 83 | | |── experiments 84 | | | |── config # hyperparameters for training, testing and environment 85 | | | |── scripts # runable scripts for train/test/validate 86 | | | |── model_spec # network architecture spec 87 | | | |── cfgs # experiment config 88 | | | └── object_index # object indexes used for experiments 89 | | |── core # agents and learning 90 | | | |── train_online # online training 91 | | | |── train_offline # offline training 92 | | | |── test_offline # testing and video recording 93 | | | |── network # network architecture 94 | | | |── agent # learner and loss 95 | | | |── replay memory # replay buffer 96 | | | |── trainer # ray-related training 97 | | | |── test_realworld_ros_clutter # example real-world scripts 98 | | | └── ... 99 | | |── output # trained model 100 | | |── output_misc # log and videos 101 | | └── ... 102 | └── ... 103 | ``` 104 | 105 | ### Citation 106 | If you find HCG useful in your research, please consider citing: 107 | ``` 108 | @inproceedings{wang2021hierarchical, 109 | author = {Lirui Wang, Xiangyun Meng, Yu Xiang, and Dieter Fox}, 110 | title = {Hierarchical Policies for Cluttered-Scene Grasping with Latent Plans}, 111 | booktitle = {IEEE Robotics and Automation Letters (RAL)}, 112 | year = {2022} 113 | } 114 | ``` 115 | 116 | ### License 117 | The HCG is licensed under the [MIT License](LICENSE). 118 | -------------------------------------------------------------------------------- /assets/demo_embedding1.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/liruiw/HCG/0059be3091658fc05df25bebb0ff33c02a8df581/assets/demo_embedding1.gif -------------------------------------------------------------------------------- /assets/demo_embedding2.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/liruiw/HCG/0059be3091658fc05df25bebb0ff33c02a8df581/assets/demo_embedding2.gif -------------------------------------------------------------------------------- /assets/demo_sampler1.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/liruiw/HCG/0059be3091658fc05df25bebb0ff33c02a8df581/assets/demo_sampler1.gif -------------------------------------------------------------------------------- /assets/demo_sampler2.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/liruiw/HCG/0059be3091658fc05df25bebb0ff33c02a8df581/assets/demo_sampler2.gif -------------------------------------------------------------------------------- /assets/hcg.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/liruiw/HCG/0059be3091658fc05df25bebb0ff33c02a8df581/assets/hcg.gif -------------------------------------------------------------------------------- /core/__init__.py: -------------------------------------------------------------------------------- 1 | 2 | 3 | -------------------------------------------------------------------------------- /core/bc.py: -------------------------------------------------------------------------------- 1 | # -------------------------------------------------------- 2 | # Licensed under The MIT License [see LICENSE for details] 3 | # -------------------------------------------------------- 4 | import torch 5 | import torch.nn.functional as F 6 | import numpy as np 7 | from core.utils import * 8 | from core.agent import Agent 9 | from core.traj import TrajAgent 10 | from core.loss import * 11 | 12 | class BC(TrajAgent): 13 | def __init__(self, num_inputs, action_space, args): 14 | super(BC, self).__init__(num_inputs, action_space, args, name='BC') 15 | 16 | def load_weight(self, weights): 17 | self.policy.load_state_dict(weights[0]) 18 | self.traj_feature_extractor.load_state_dict(weights[1]) 19 | self.traj_feature_sampler.load_state_dict(weights[2]) 20 | self.state_feature_extractor.load_state_dict(weights[3]) 21 | 22 | def get_weight(self): 23 | return [ self.policy.state_dict(), 24 | self.traj_feature_extractor.state_dict(), 25 | self.traj_feature_sampler.state_dict(), 26 | self.state_feature_extractor.state_dict() ] 27 | 28 | def extract_feature(self, image_batch, point_batch, 29 | action_batch=None, goal_batch=None, time_batch=None, vis=False, 30 | value=False, repeat=False, separate=False, traj_poses=None, 31 | curr_joint=None, goal_point_state=None, traj_point_state=None, 32 | traj_integer_time=None, joint_traj_state=None, train=False, no_action=False ): 33 | """ 34 | extract policy state feature 35 | """ 36 | use_traj = self.full_traj_embedding and traj_integer_time is not None and joint_traj_state is not None 37 | time_input = traj_integer_time if use_traj else time_batch 38 | joint_input = joint_traj_state if use_traj else curr_joint 39 | point_input = traj_point_state if use_traj else point_batch 40 | point_batch = preprocess_points(self, point_input, joint_input, time_input) 41 | 42 | feature = self.unpack_batch(image_batch, point_batch, repeat=repeat ) 43 | if self.use_time: feature[0] = torch.cat((feature[0], time_input[:,None]), dim=1) 44 | self.state_policy_feat_th = feature[0].clone().detach() 45 | self.state_policy_feat = self.state_policy_feat_th.cpu().numpy() 46 | 47 | if self.traj_goal_mutual_conditioned: 48 | feature[0] = torch.cat((feature[0], self.traj_feat), dim=1) 49 | return feature 50 | 51 | 52 | def update_parameters(self, batch_data, updates, k, test=False): 53 | """ train agent """ 54 | self.set_mode(False) 55 | self.prepare_data(batch_data) 56 | self.train_traj_net() 57 | 58 | state_batch, extra_feat_pred = self.extract_feature( 59 | self.image_state_batch, 60 | self.point_state_batch, 61 | goal_batch=self.goal_batch, 62 | traj_point_state=self.traj_point_state_batch, 63 | time_batch=self.time_batch, 64 | curr_joint=self.curr_joint[:, :7], 65 | joint_traj_state=self.traj_joint_batch[:, :7], 66 | traj_integer_time=self.traj_integer_time_batch, 67 | train=True) 68 | self.pi, self.log_pi, _, self.aux_pred = self.policy.sample(state_batch) 69 | loss = self.compute_loss() 70 | 71 | self.optimize(loss, self.update_step) 72 | if self.re_sampler_step: # separately update sampler 73 | self.sampler_update_parameters() 74 | self.update_step += 1 75 | self.log_stat() 76 | return {k: float(getattr(self, k)) for k in self.loss_info } 77 | 78 | def sampler_update_parameters(self): 79 | """ 80 | Second pass for sampler joint training 81 | """ 82 | state_feat = torch.cat((self.state_policy_feat_th, self.sampler_traj_feat), dim=1) 83 | self.pi, self.log_pi, _, self.aux_pred = self.policy.sample(state_feat) 84 | self.sampler_gaussian = (self.sampler_latent_mean, self.sampler_latent_logstd) 85 | 86 | # recons 87 | self.traj_latent_loss = traj_latent_loss(self, self.sampler_traj_feat[self.target_expert_mask], 88 | self.traj_feat_target[self.target_expert_mask].detach()) 89 | 90 | # kl 91 | self.kl_loss = kl_loss(self.sampler_gaussian, self.kl_scale) 92 | 93 | # grasp and bc 94 | self.sampler_grasp_aux_loss = goal_pred_loss(self.aux_pred[self.target_goal_reward_mask, :7], self.target_grasp_batch[self.target_goal_reward_mask, :7] ) 95 | self.sampler_bc_loss = traj_action_loss(self, self.pi, self.traj_expert_action_batch, self.target_expert_mask) 96 | 97 | self.traj_feature_sampler_opt.zero_grad() # only update sampler 98 | self.state_feat_encoder_optim.zero_grad() 99 | self.policy_optim.zero_grad() 100 | total_loss = self.kl_loss + self.traj_latent_loss + self.sampler_bc_loss + self.sampler_grasp_aux_loss 101 | total_loss.backward() 102 | self.traj_feature_sampler_opt.step() -------------------------------------------------------------------------------- /core/ddpg.py: -------------------------------------------------------------------------------- 1 | # -------------------------------------------------------- 2 | # Licensed under The MIT License [see LICENSE for details] 3 | # -------------------------------------------------------- 4 | 5 | import torch 6 | import torch.nn.functional as F 7 | import numpy as np 8 | from core.utils import * 9 | 10 | from core import networks 11 | from core.traj import TrajAgent 12 | from core.loss import * 13 | 14 | class DDPG(TrajAgent): 15 | def __init__(self, num_inputs, action_space, args): 16 | super(DDPG, self).__init__(num_inputs, action_space, args, name='DDPG') 17 | self.action_dim = self.action_space.shape[0] 18 | self.critic_value_dim = 0 if self.sa_channel_concat else self.action_dim 19 | self.value_loss_func = huber_loss 20 | self.critic_num_input = self.num_inputs 21 | self.critic, self.critic_optim, self.critic_scheduler, self.critic_target = get_critic(self) 22 | 23 | def load_weight(self, weights): 24 | self.policy.load_state_dict(weights[0]) 25 | self.critic.load_state_dict(weights[1]) 26 | self.traj_feature_extractor.load_state_dict(weights[2]) 27 | self.state_feature_extractor.load_state_dict(weights[3]) 28 | 29 | def get_weight(self): 30 | return [self.policy.state_dict(), 31 | self.critic.state_dict(), 32 | self.traj_feature_extractor.state_dict(), 33 | self.state_feature_extractor.state_dict() ] 34 | 35 | def get_mix_ratio(self, update_step): 36 | """ 37 | Get a mixed schedule for supervised learning and RL 38 | """ 39 | idx = int((self.update_step > np.array(self.mix_milestones)).sum()) 40 | mix_policy_ratio = get_valid_index(self.mix_policy_ratio_list, idx) 41 | mix_policy_ratio = min(mix_policy_ratio, self.ddpg_coefficients[4]) 42 | mix_value_ratio = get_valid_index(self.mix_value_ratio_list, idx) 43 | mix_value_ratio = min(mix_value_ratio, self.ddpg_coefficients[3]) 44 | return mix_value_ratio, mix_policy_ratio 45 | 46 | def extract_feature(self, image_batch, point_batch, 47 | action_batch=None, goal_batch=None, time_batch=None, 48 | vis=False, value=False, repeat=False, 49 | train=True, separate=False, curr_joint=None, traj_latent=None, 50 | traj_point_state=None, traj_integer_time=None, traj_action=None, 51 | joint_traj_state=None, next_state=False, traj_time_batch=None): 52 | """ 53 | extract features for policy learning 54 | """ 55 | if curr_joint is not None: curr_joint = curr_joint[:, :7] 56 | feature_2 = value and not self.shared_feature 57 | use_traj = False 58 | time_input = traj_integer_time if use_traj else time_batch 59 | joint_input = joint_traj_state if use_traj else curr_joint 60 | point_input = traj_point_state if use_traj else point_batch[:,:,:1030] 61 | action_input = traj_action if use_traj else action_batch 62 | 63 | # clip time 64 | time_input = torch.clamp(time_input, 0., 25.) 65 | 66 | if self.sa_channel_concat and value: 67 | point_input = concat_state_action_channelwise(point_input, action_input) 68 | 69 | feature = self.unpack_batch(image_batch, point_input, gt_goal=goal_batch, val=feature_2, repeat=repeat) 70 | if self.use_time: 71 | feature[0] = torch.cat((feature[0], time_input[:,None]), dim=1) 72 | 73 | if not self.sa_channel_concat and value: 74 | feature[0] = torch.cat((feature[0], action_input), dim=1) 75 | self.state_policy_feat = feature[0].detach().cpu().numpy() 76 | return feature 77 | 78 | def target_value(self): 79 | """ 80 | compute target value 81 | """ 82 | next_time_batch = self.time_batch - 1 83 | next_traj_time_batch = self.traj_integer_time_batch - 1 84 | reward_batch = self.traj_reward_batch if self.full_traj_embedding else self.reward_batch 85 | mask_batch = self.traj_mask_batch if self.full_traj_embedding else self.mask_batch 86 | 87 | with torch.no_grad(): 88 | next_state_batch, _ = self.extract_feature(self.next_image_state_batch, 89 | self.next_point_state_batch, 90 | action_batch=self.next_action_batch, 91 | goal_batch=self.next_goal_batch, 92 | traj_point_state=self.next_traj_point_state_batch, 93 | time_batch=next_time_batch, 94 | curr_joint=self.next_joint_batch, 95 | joint_traj_state=self.traj_joint_batch, 96 | traj_integer_time=next_traj_time_batch, 97 | traj_action=self.next_traj_action_batch, 98 | separate=True, 99 | vis=False, 100 | next_state=True, 101 | value=False) 102 | 103 | next_action_mean, _, _, _ = self.policy_target.sample(next_state_batch) 104 | idx = int((self.update_step > np.array(self.mix_milestones)).sum()) 105 | noise_scale = self.action_noise * get_valid_index(self.noise_ratio_list, idx) 106 | noise_delta = get_noise_delta(next_action_mean, noise_scale, self.noise_type) 107 | noise_delta[:, :3] = torch.clamp(noise_delta[:, :3], -0.01, 0.01) 108 | next_action_mean = next_action_mean + noise_delta 109 | if self.sa_channel_concat or not self.shared_feature: 110 | next_target_state_batch, _ = self.extract_feature( self.next_image_state_batch, self.next_point_state_batch, next_action_mean, 111 | self.next_goal_batch, next_time_batch, value=True, curr_joint=self.next_joint_batch) 112 | else: 113 | next_target_state_batch = torch.cat((next_state_batch, next_action_mean), dim=-1) 114 | 115 | min_qf_next_target = self.state_action_value(next_target_state_batch, next_action_mean, target=True) 116 | next_q_value = reward_batch + (1 - mask_batch) * self.gamma * min_qf_next_target 117 | return next_q_value 118 | 119 | def state_action_value(self, state_batch, action_batch, target=False, return_qf=False): 120 | """ compute state action value """ 121 | critic = self.critic_target if target else self.critic 122 | qf1, qf2, critic_aux = critic(state_batch, None) 123 | min_qf = torch.min(qf1, qf2) 124 | min_qf = min_qf.squeeze(-1) 125 | 126 | if return_qf: 127 | return qf1.squeeze(-1), qf2.squeeze(-1), critic_aux 128 | return min_qf 129 | 130 | def critic_optimize(self): 131 | """ 132 | optimize critic and feature gradient 133 | """ 134 | self.critic_optim.zero_grad() 135 | self.state_feat_val_encoder_optim.zero_grad() 136 | critic_loss = sum([getattr(self, name) for name in get_loss_info_dict().keys() \ 137 | if name.endswith('loss') and name.startswith('critic')]) 138 | if self.sa_channel_concat or not self.shared_feature: 139 | critic_loss.backward() 140 | else: 141 | critic_loss.backward(retain_graph=True) 142 | torch.nn.utils.clip_grad_norm_(self.critic.parameters(), self.clip_grad) 143 | if self.train_value_feature: self.state_feat_val_encoder_optim.step() 144 | self.critic_optim.step() 145 | 146 | def compute_critic_loss(self, value_feat): 147 | """ 148 | compute one step bellman error 149 | """ 150 | self.next_q_value = self.target_value() 151 | self.qf1, self.qf2, self.critic_grasp_aux = self.state_action_value(value_feat, self.action_batch, return_qf=True) 152 | self.critic_loss = F.mse_loss(self.qf1.view(-1), self.next_q_value) + \ 153 | F.mse_loss(self.qf2.view(-1), self.next_q_value) 154 | 155 | if self.critic_aux: 156 | self.critic_grasp_aux_loss += goal_pred_loss(self.critic_grasp_aux[self.goal_reward_mask, :7], self.goal_batch[self.goal_reward_mask]) 157 | 158 | def update_parameters(self, batch_data, updates, k, test=False): 159 | """ update agent parameters """ 160 | self.mix_value_ratio, self.mix_policy_ratio = self.get_mix_ratio(self.update_step) 161 | self.set_mode(test) 162 | self.prepare_data(batch_data) 163 | 164 | if self.train_traj_feature : 165 | self.train_traj_net() 166 | 167 | # value 168 | if self.policy_update_gap > 0: 169 | value_feat, _ = self.extract_feature(self.image_state_batch, 170 | self.point_state_batch, 171 | action_batch=self.action_batch, 172 | goal_batch=self.goal_batch, 173 | traj_point_state=self.traj_point_state_batch, 174 | time_batch=self.time_batch, 175 | curr_joint=self.curr_joint, 176 | joint_traj_state=self.traj_joint_batch, 177 | traj_integer_time=self.traj_integer_time_batch, 178 | traj_time_batch=self.traj_time_batch, 179 | traj_action=self.traj_action_batch, 180 | value=True) 181 | self.compute_critic_loss(value_feat) 182 | self.critic_optimize() 183 | 184 | # policy 185 | if self.sa_channel_concat or not self.shared_feature: 186 | policy_feat, extra_feat_pred = self.extract_feature(self.image_state_batch, 187 | self.point_state_batch, 188 | goal_batch=self.goal_batch, 189 | traj_point_state=self.traj_point_state_batch, 190 | time_batch=self.time_batch, 191 | curr_joint=self.curr_joint, 192 | separate=True, 193 | joint_traj_state=self.traj_joint_batch, 194 | traj_integer_time=self.traj_integer_time_batch, 195 | traj_time_batch=self.traj_time_batch) 196 | else: 197 | policy_feat = value_feat[..., :-self.action_dim] 198 | 199 | self.pi, _, _, self.aux_pred = self.policy.sample(policy_feat) 200 | 201 | # actor critic 202 | if self.has_critic and (self.update_step % self.policy_update_gap == 0) and self.policy_update_gap > 0: 203 | if self.sa_channel_concat or not self.shared_feature: 204 | value_pi_feat, _ = self.extract_feature(self.image_state_batch, self.point_state_batch, 205 | self.pi, self.goal_batch, self.time_batch, 206 | value=True, curr_joint=self.curr_joint) 207 | else: 208 | value_pi_feat = torch.cat((policy_feat, self.pi), dim=-1) 209 | self.qf1_pi, self.qf2_pi, critic_aux = self.state_action_value(value_pi_feat, self.pi, return_qf=True) 210 | self.qf1_pi = self.qf1_pi[~self.expert_reward_mask] 211 | self.qf2_pi = self.qf2_pi[~self.expert_reward_mask] 212 | self.actor_critic_loss = -self.mix_policy_ratio * self.qf1_pi.mean() 213 | 214 | loss = self.compute_loss() 215 | self.optimize(loss, self.update_step) 216 | self.update_step += 1 217 | 218 | # log 219 | self.log_stat() 220 | return {k: float(getattr(self, k)) for k in get_loss_info_dict().keys()} 221 | 222 | -------------------------------------------------------------------------------- /core/dqn_hrl.py: -------------------------------------------------------------------------------- 1 | # -------------------------------------------------------- 2 | # Licensed under The MIT License [see LICENSE for details] 3 | # -------------------------------------------------------- 4 | 5 | import torch 6 | import torch.nn.functional as F 7 | import numpy as np 8 | from core.utils import * 9 | from torch.optim import Adam 10 | 11 | 12 | from core import networks 13 | from core.traj import TrajAgent 14 | from core.loss import * 15 | 16 | class DQN_HRL(TrajAgent): 17 | def __init__(self, num_inputs, action_space, args): 18 | super(DQN_HRL, self).__init__(num_inputs, action_space, args, name='DQN_HRL') 19 | self.action_dim = self.action_space.shape[0] 20 | self.critic_num_input = self.feature_input_dim + 1 21 | self.critic_value_dim = self.policy_traj_latent_size 22 | 23 | self.value_loss_func = F.smooth_l1_loss 24 | self.critic, self.critic_optim, self.critic_scheduler, self.critic_target = get_critic(self) 25 | 26 | def load_weight(self, weights): 27 | self.policy.load_state_dict(weights[0]) 28 | self.critic.load_state_dict(weights[1]) 29 | self.traj_feature_sampler.load_state_dict(weights[2]) 30 | self.traj_feature_extractor.load_state_dict(weights[3]) 31 | self.state_feature_extractor.load_state_dict(weights[4]) 32 | 33 | def get_weight(self): 34 | return [self.policy.state_dict(), 35 | self.critic.state_dict(), 36 | self.traj_feature_sampler.state_dict(), 37 | self.traj_feature_extractor.state_dict(), 38 | self.state_feature_extractor.state_dict()] 39 | 40 | def greedy_action(self): 41 | """ determine if use max score traj or random sample """ 42 | return np.random.uniform() < self.epsilon_greedy_list[0] 43 | 44 | @torch.no_grad() 45 | def compute_critic_value(self, img_state, point_state, timestep, curr_joint_th, goal_state): 46 | """ 47 | compute score on state and traj feature and return argmax 48 | """ 49 | feature = self.extract_feature( 50 | img_state, 51 | point_state, 52 | goal_batch=goal_state, 53 | traj_goal_batch=goal_state, 54 | time_batch=timestep, 55 | value=True, 56 | train=False, 57 | curr_joint=curr_joint_th)[0] 58 | 59 | qf1_pi, qf2_pi, critic_aux = self.state_action_value(feature, None, return_qf=True) 60 | self.q_min = torch.min(qf1_pi, qf2_pi) 61 | self.q_stat = [self.q_min.min(), self.q_min.mean(), self.q_min.max()] 62 | opt_idx = torch.argmax(self.q_min) 63 | self.traj_feat = self.traj_feat[[opt_idx]].view(1, -1) 64 | self.sampler_traj_feat = self.sampler_traj_feat[[opt_idx]].view(1, -1) 65 | self.gaddpg_pred = torch.sigmoid(critic_aux[opt_idx, -1]) 66 | 67 | return opt_idx 68 | 69 | @torch.no_grad() 70 | def critic_select_action( 71 | self, state, remain_timestep, curr_joint, vis=False): 72 | """ 73 | Sample traj and select based on critic 74 | """ 75 | self.set_mode(True) 76 | sample_num = self.test_traj_num if self.greedy_action() or self.test_mode else 1 77 | curr_joint_th = torch.cuda.FloatTensor([curr_joint.flatten()])[:, :7].repeat((sample_num, 1)) 78 | img_state = torch.cuda.FloatTensor(state[0][1])[None] 79 | point_state = torch.cuda.FloatTensor(state[0][0])[None].repeat((sample_num, 1, 1)) 80 | self.timestep = torch.Tensor([remain_timestep]).float().cuda() 81 | traj = self.select_traj(img_state, 82 | point_state, 83 | vis=vis, 84 | remain_timestep=remain_timestep, 85 | curr_joint=curr_joint_th) 86 | timestep = torch.Tensor([remain_timestep]).float().cuda().repeat(sample_num) 87 | 88 | self.timestep = timestep 89 | policy_feature = self.extract_feature( img_state, 90 | point_state, 91 | time_batch=timestep, 92 | value=False, 93 | train=False, 94 | repeat=True, 95 | curr_joint=curr_joint_th)[0] 96 | actions = self.policy.sample(policy_feature) 97 | 98 | # select traj from MPC 99 | opt_idx = self.compute_critic_value( img_state, point_state, timestep, curr_joint_th, None ) 100 | if traj is not None: # visualizing the selected trajectory 101 | traj[0], traj[opt_idx] = traj[opt_idx], traj[0] 102 | 103 | action = actions[0].detach().cpu().numpy()[opt_idx] 104 | extra_pred = actions[1].detach().cpu().numpy()[opt_idx] 105 | action_sample = actions[2].detach().cpu().numpy()[opt_idx] 106 | extra_pred = self.gaddpg_pred.detach().cpu().numpy() 107 | aux_pred = actions[3].detach().cpu().numpy()[opt_idx] 108 | return action, traj, extra_pred, aux_pred 109 | 110 | def extract_feature(self, image_batch, point_batch, 111 | action_batch=None, goal_batch=None, time_batch=None, 112 | vis=False, value=False, repeat=False, traj_goal_batch=None, 113 | train=True, separate=False, curr_joint=None, traj_latent=None, 114 | traj_point_state=None, traj_integer_time=None, traj_action=None, 115 | joint_traj_state=None, next_state=False, traj_time_batch=None, 116 | use_offline_latent=True, no_action=False ): 117 | """ 118 | extract features for policy learning 119 | """ 120 | curr_joint = curr_joint[:, :7] 121 | use_traj = self.full_traj_embedding and joint_traj_state is not None 122 | action_input = traj_latent if traj_latent is not None else self.traj_feat 123 | point_batch = preprocess_points(self, point_batch, curr_joint, time_batch) 124 | 125 | if not value: 126 | feature = self.unpack_batch(image_batch, point_batch, val=False ) 127 | feature[0] = torch.cat((feature[0], time_batch[:,None]), dim=1) 128 | self.state_policy_feat = feature[0].detach().cpu().numpy() 129 | if self.traj_feat is not None: 130 | feature[0] = torch.cat((feature[0], self.traj_feat), dim=1) 131 | return feature 132 | 133 | feature = self.unpack_batch(image_batch, point_batch, val=True, repeat=repeat) 134 | feature[0] = torch.cat((feature[0], time_batch[:,None], action_input), dim=1) 135 | return feature 136 | 137 | def target_value(self): 138 | """ 139 | compute target value 140 | """ 141 | self.dataset_traj_latent_head = None 142 | sampler_state_input_batch = preprocess_points(self, self.next_point_state_batch, self.next_joint_batch, self.next_time_batch, append_pc_time=True) 143 | self.train_traj_idx_batch = torch.arange(len(sampler_state_input_batch)).cuda().long().repeat(self.dqn_sample_num) 144 | self.traj_feature_sampler.eval() 145 | self.traj_feature_extractor.eval() 146 | with torch.no_grad(): 147 | self.autoencode(sampler_state_input_batch, False ) 148 | traj_latent = self.sampler_traj_feat 149 | state_input_batch = preprocess_points(self, self.next_point_state_batch, self.next_joint_batch, self.next_time_batch ) 150 | state_input_batch = state_input_batch.repeat(self.dqn_sample_num, 1, 1) 151 | next_time_batch = self.next_time_batch.repeat(self.dqn_sample_num ) 152 | next_joint_batch = self.next_joint_batch.repeat(self.dqn_sample_num, 1) 153 | next_state_batch, _ = self.extract_feature(self.next_image_state_batch, 154 | state_input_batch, 155 | time_batch=next_time_batch, 156 | curr_joint=next_joint_batch, 157 | traj_latent=traj_latent, 158 | next_state=True, 159 | value=True) 160 | 161 | # use the target network to pick the max action 162 | next_target_state_batch = next_state_batch 163 | min_qf_next_target = self.state_action_value(next_target_state_batch, None, target=True) 164 | min_qf_next_target = min_qf_next_target.view(self.dqn_sample_num, -1).max(0)[0] 165 | next_q_value = self.target_reward_batch + (1 - self.target_mask_batch) * self.gamma * min_qf_next_target 166 | next_q_value = self.mix_value_ratio * next_q_value + (1 - self.mix_value_ratio) * self.target_return 167 | return next_q_value 168 | 169 | def state_action_value(self, state_batch, action_batch, target=False, return_qf=False): 170 | """ compute state action value """ 171 | critic = self.critic_target if target else self.critic 172 | qf1, qf2, critic_aux = critic(state_batch, None) 173 | scale_func = torch.tanh 174 | if self.critic_tanh: 175 | qf1 = scale_func(qf1) 176 | qf2 = scale_func(qf2) 177 | 178 | min_qf = torch.min(qf1, qf2) 179 | min_qf = min_qf.squeeze(-1) 180 | 181 | if return_qf: 182 | return qf1.squeeze(-1), qf2.squeeze(-1), critic_aux 183 | return min_qf 184 | 185 | def critic_optimize(self): 186 | """ 187 | optimize critic and feature gradient 188 | """ 189 | self.critic_optim.zero_grad() 190 | self.state_feat_val_encoder_optim.zero_grad() 191 | 192 | critic_loss = sum([getattr(self, name) for name in get_loss_info_dict().keys() if name.endswith('loss') and name.startswith('critic')]) 193 | critic_loss.backward() 194 | self.state_feat_val_encoder_optim.step() 195 | self.critic_optim.step() 196 | 197 | if hasattr(self, "critic_target"): 198 | half_soft_update(self.critic_target, self.critic, self.tau) 199 | if self.update_step % self.target_update_interval == 0: 200 | half_hard_update(self.critic_target, self.critic, self.tau) 201 | 202 | 203 | def compute_critic_loss(self, value_feat): 204 | """ 205 | compute one step bellman error 206 | """ 207 | self.next_q_value = self.target_value() 208 | self.qf1, self.qf2, self.critic_aux_pred = self.state_action_value(value_feat, self.action_batch, return_qf=True) 209 | next_q_value = self.next_q_value 210 | qf1 = torch.min(self.qf1, self.qf2) 211 | self.critic_loss = self.value_loss_func(self.qf1.view(-1), next_q_value) + self.value_loss_func(self.qf2.view(-1), next_q_value) 212 | 213 | # critic gaddpg auxiliary 214 | gaddpg_batch = self.gaddpg_batch.bool() 215 | gaddpg_pred = self.critic_aux_pred[gaddpg_batch, -1] 216 | target_gaddpg_batch = self.target_return[gaddpg_batch] 217 | 218 | target_gaddpg_batch = (target_gaddpg_batch > 0).float() 219 | self.critic_gaddpg_loss = self.gaddpg_scale * F.binary_cross_entropy_with_logits(gaddpg_pred, target_gaddpg_batch) 220 | 221 | self.gaddpg_pred_mean = gaddpg_pred.mean().item() 222 | self.gaddpg_mask_num = len(target_gaddpg_batch) 223 | self.gaddpg_mask_ratio = target_gaddpg_batch.sum() / len(target_gaddpg_batch) 224 | self.valid_latent_num = len(qf1) 225 | 226 | 227 | def update_parameters(self, batch_data, updates, k, test=False): 228 | """ update agent parameters """ 229 | self.mix_value_ratio, self.mix_policy_ratio = self.get_mix_ratio(self.update_step - self.init_step) 230 | self.set_mode(test) 231 | self.prepare_data(batch_data) 232 | traj_latent = self.traj_dataset_traj_latent if self.full_traj_embedding else self.dataset_traj_latent 233 | 234 | # value 235 | value_feat, _ = self.extract_feature(self.image_state_batch, 236 | self.point_state_batch, 237 | action_batch=self.action_batch, 238 | goal_batch=self.goal_batch, 239 | traj_goal_batch=self.traj_goal_batch, 240 | traj_point_state=self.traj_point_state_batch, 241 | time_batch=self.time_batch, 242 | curr_joint=self.curr_joint, 243 | joint_traj_state=self.traj_joint_batch, 244 | traj_latent=traj_latent, # 245 | traj_integer_time=self.traj_integer_time_batch, 246 | traj_time_batch=self.traj_time_batch, 247 | traj_action=self.traj_action_batch, 248 | value=True) 249 | self.compute_critic_loss(value_feat) 250 | 251 | self.critic_optimize() 252 | 253 | self.update_step += 1 254 | 255 | # log 256 | self.log_stat() 257 | return {k: float(getattr(self, k)) for k in get_loss_info_dict().keys()} 258 | 259 | -------------------------------------------------------------------------------- /core/load_tensorboard.py: -------------------------------------------------------------------------------- 1 | # -------------------------------------------------------- 2 | # Licensed under The MIT License [see LICENSE for details] 3 | # -------------------------------------------------------- 4 | 5 | import os 6 | import time 7 | import sys 8 | import IPython 9 | import argparse 10 | import glob 11 | parser = argparse.ArgumentParser(description="utility script for loading tensor board models") 12 | 13 | parser.add_argument( '--model', 14 | help='model path', 15 | type=str, 16 | default='02_07_2020_12:12:51') 17 | parser.add_argument( '--root', 18 | help='Root dir for data', 19 | type=str, 20 | default='.') 21 | parser.add_argument( '--copy', 22 | help='copy log', 23 | action='store_true') 24 | parser.add_argument( '--sleep', 25 | help='copy log', 26 | type=int, 27 | default=1) 28 | parser.add_argument( '--output', 29 | help='output dir', 30 | default='output') 31 | 32 | args = parser.parse_args() 33 | time.sleep(args.sleep) # 1 h 34 | print('tensorboard model', args.model) 35 | test_models = [item.strip() for item in args.model.split(',')] 36 | model_num = len(test_models) 37 | output_root_dir = os.path.join(args.root, args.output) 38 | run_root_dir = args.root + '/output' 39 | run_models = [] 40 | tensorboard_format_str = [] 41 | 42 | for model in test_models: 43 | root_dir = '{}/{}'.format(output_root_dir, model) 44 | if not os.path.exists(root_dir) : 45 | root_dir = os.path.join('temp_model_output', model) 46 | 47 | config_file = glob.glob(root_dir + '/*.yaml') 48 | config_file = [f for f in config_file if 'td3' in f or 'bc' in f or 'ddpg' in f or 'dqn' in f][0] 49 | tfboard_name = config_file[len(root_dir)+1:] 50 | name_cnt = 1 51 | 52 | while tfboard_name in tensorboard_format_str: 53 | tfboard_name = tfboard_name + '_{}'.format(name_cnt) 54 | name_cnt += 1 55 | 56 | tensorboard_format_str.append(tfboard_name) 57 | print('{}: {}'.format(output_root_dir, model, tfboard_name)) 58 | run_file = glob.glob('{}/*{}*'.format(run_root_dir, model))[0] 59 | run_models.append(run_file) 60 | tensorboard_format_str.append(run_file[len(run_root_dir)+1:]) 61 | 62 | tensorboard_scripts = 'python -m tensorboard.main --bind_all --reload_interval 5 --logdir_spec='+','.join( 63 | ['{}:' + run_root_dir + '/{}'] * model_num) 64 | tensorboard_scripts = tensorboard_scripts.format(*tensorboard_format_str) 65 | print('tensorboard_scripts:', tensorboard_scripts) 66 | os.system(tensorboard_scripts) -------------------------------------------------------------------------------- /core/loss.py: -------------------------------------------------------------------------------- 1 | # -------------------------------------------------------- 2 | # Licensed under The MIT License [see LICENSE for details] 3 | # -------------------------------------------------------- 4 | 5 | import os 6 | import torch 7 | import torch.nn.functional as F 8 | import numpy as np 9 | from core.utils import * 10 | 11 | 12 | 13 | def goal_pred_loss(grasp_pred, goal_batch ): 14 | """ 15 | PM loss for grasp pose detection 16 | """ 17 | grasp_pcs = transform_control_points( grasp_pred, grasp_pred.shape[0], device="cuda", rotz=True ) 18 | grasp_pcs_gt = transform_control_points( goal_batch, goal_batch.shape[0], device="cuda", rotz=True ) 19 | return torch.mean(torch.abs(grasp_pcs - grasp_pcs_gt).sum(-1)) 20 | 21 | def traj_action_loss(agent, pi, traj_action_batch, cont_expert_mask ): 22 | """ 23 | PM loss for traj action 24 | """ 25 | gt_act_pt = control_points_from_rot_and_trans(traj_action_batch[:,3:], traj_action_batch[:,:3], device='cuda') 26 | pi_act_pt = control_points_from_rot_and_trans(pi[:,3:], pi[:,:3], device='cuda') 27 | return torch.mean(torch.abs(gt_act_pt[cont_expert_mask] - pi_act_pt[cont_expert_mask]).sum(-1)) 28 | 29 | def traj_latent_loss(agent, pred_latent, target_latent): 30 | """ 31 | L2 latent reconstruction loss 32 | """ 33 | return F.mse_loss(pred_latent, target_latent) 34 | 35 | def kl_loss(extra_pred, kl_scale): 36 | """ 37 | KL with unit gaussian 38 | """ 39 | mu, log_sigma = extra_pred 40 | return kl_scale * torch.mean( -.5 * torch.sum(1. + log_sigma - mu**2 - torch.exp(log_sigma), dim=-1)) 41 | -------------------------------------------------------------------------------- /core/network_arch.py: -------------------------------------------------------------------------------- 1 | # -------------------------------------------------------- 2 | # Licensed under The MIT License [see LICENSE for details] 3 | # -------------------------------------------------------- 4 | 5 | import torch 6 | from torch import nn 7 | import numpy as np 8 | import pointnet2_ops.pointnet2_modules as pointnet2 9 | from core.utils import * 10 | import torch.nn.functional as F 11 | from torch.distributions import Normal 12 | 13 | LOG_SIG_MAX = 2 14 | LOG_SIG_MIN = -10 15 | epsilon = 1e-6 16 | 17 | 18 | def create_encoder( 19 | model_scale, pointnet_radius, pointnet_nclusters, input_dim=0, 20 | feature_option=0, traj_net=False, group_norm=True ): 21 | global_pc_dim_list = [64, 256, 1024] 22 | local_pc_dim_list = [32, 128, 512] 23 | output_dim = int(global_pc_dim_list[-1] + model_scale * 512) 24 | 25 | # select different pointnet architecture 26 | if feature_option == 0: 27 | model = base_network(pointnet_radius, pointnet_nclusters, model_scale, input_dim) 28 | elif feature_option == 2: 29 | model = base_network_new_large(model_scale, input_dim, group_norm=group_norm) 30 | elif feature_option == 4: 31 | model = base_point_net_large(model_scale, input_dim, group_norm=group_norm) # npoint_2=128 32 | 33 | print('=== net option: {} trainable param: {} === '.format(feature_option, [count_parameters(m) for m in model])) 34 | if traj_net: 35 | model = nn.ModuleList([RobotMLP(input_dim, global_pc_dim_list, gn=group_norm), model]) 36 | return model, output_dim 37 | return model 38 | 39 | def pointnet_encode(encoder, xyz, xyz_features, vis=False): 40 | l_xyz = [xyz] 41 | if type(encoder) is not nn.ModuleList: 42 | return encoder(xyz_features) 43 | 44 | for module in encoder[0]: 45 | xyz, xyz_features = module(xyz, xyz_features) 46 | l_xyz.append(xyz) 47 | 48 | return encoder[1](xyz_features.squeeze(-1)) 49 | 50 | def base_network(pointnet_radius, pointnet_nclusters, scale, in_features, group_norm=False): 51 | sa1_module = pointnet2.PointnetSAModule( 52 | npoint=pointnet_nclusters, 53 | radius=pointnet_radius, 54 | nsample=64, 55 | mlp=[in_features, int(64 * scale), int(64 * scale), int(128 * scale)], 56 | ) 57 | sa2_module = pointnet2.PointnetSAModule( 58 | npoint=32, 59 | radius=0.04, 60 | nsample=128, 61 | mlp=[int(128 * scale), int(128 * scale), int(128 * scale), int(256 * scale)], 62 | ) 63 | 64 | sa3_module = pointnet2.PointnetSAModule( 65 | mlp=[int(256 * scale), int(256 * scale), int(256 * scale), int(512 * scale)] 66 | ) 67 | 68 | sa_modules = nn.ModuleList([sa1_module, sa2_module, sa3_module]) 69 | fc_layer = nn.Sequential( 70 | nn.Linear(int(512 * scale), int(1024 * scale)), 71 | nn.BatchNorm1d(int(1024 * scale)), 72 | nn.ReLU(True), 73 | nn.Linear(int(1024 * scale), int(512 * scale)), 74 | nn.BatchNorm1d(int(512 * scale)), 75 | nn.ReLU(True), 76 | ) 77 | return nn.ModuleList([sa_modules, fc_layer]) 78 | 79 | def base_network_new_large(scale, in_features, 80 | npoints=[512, 256, 128], sample_npoint_list=[128,128,64], radius=[0.1, 0.2, 0.4], layer1=False, 81 | group_norm=True ): 82 | sample_npoint_list = [64] * len(sample_npoint_list) 83 | 84 | sa0_module = pointnet2.PointnetSAModule( 85 | npoint=npoints[0], 86 | radius=radius[0], 87 | nsample=sample_npoint_list[0], 88 | mlp=[in_features, 32, 32, 64], 89 | bn=not group_norm 90 | ) 91 | 92 | sa1_module = pointnet2.PointnetSAModule( 93 | npoint=npoints[1], 94 | radius=radius[1], 95 | nsample=sample_npoint_list[1], 96 | mlp=[64, 64, 64, 128], 97 | bn=not group_norm 98 | ) 99 | 100 | sa2_module = pointnet2.PointnetSAModule( 101 | npoint=npoints[2], # 64 102 | radius=radius[2], # 0.2 103 | nsample=sample_npoint_list[2], # 64 104 | mlp=[128, 128, 128, 256], 105 | bn=not group_norm 106 | ) 107 | 108 | sa3_module = pointnet2.PointnetSAModule( 109 | mlp=[256, 256, 256, 512], 110 | bn=not group_norm 111 | ) 112 | 113 | sa_modules = nn.ModuleList([sa0_module, sa1_module, sa2_module, sa3_module ]) 114 | fc_layer = nn.Sequential( 115 | nn.Linear(512, int(512 * scale)), 116 | nn.GroupNorm(16, int(512 * scale)), 117 | nn.ReLU(True), 118 | ) 119 | return nn.ModuleList([sa_modules, fc_layer]) 120 | 121 | 122 | def base_point_net_large( scale, in_features, 123 | npoints=[512, 128], sample_npoint_list=[128, 128], radius=[0.05, 0.3], 124 | group_norm=True, layer_num=2 ): 125 | 126 | layer_2_input_dim = in_features 127 | sample_npoint_list = [128] * len(sample_npoint_list) 128 | 129 | sa1_module = pointnet2.PointnetSAModule( 130 | npoint=npoints[0], 131 | radius=radius[0], 132 | nsample=sample_npoint_list[0], 133 | mlp=[in_features, 64, 64, 64], 134 | bn=not group_norm 135 | ) 136 | 137 | sa2_module = pointnet2.PointnetSAModule( 138 | npoint=npoints[1], 139 | radius=radius[1], 140 | nsample=sample_npoint_list[1], 141 | mlp=[layer_2_input_dim, 128, 128, 256], 142 | bn=not group_norm 143 | ) 144 | sa3_module = pointnet2.PointnetSAModule( 145 | mlp=[256, 512, 512, 1024], 146 | bn=not group_norm 147 | ) 148 | 149 | sa_modules = nn.ModuleList([sa2_module, sa3_module]) 150 | fc_layer = nn.Sequential( 151 | nn.Linear(1024, int(1024 * scale)), 152 | nn.GroupNorm(16, int(1024 * scale)), 153 | nn.ReLU(True), 154 | nn.Linear(int(1024 * scale), int(512 * scale)), 155 | nn.GroupNorm(16, int(512 * scale)), 156 | nn.ReLU(True), 157 | ) 158 | return nn.ModuleList([sa_modules, fc_layer]) 159 | 160 | 161 | class Identity(nn.Module): 162 | def forward(self, input): 163 | return input 164 | 165 | 166 | def weights_init_(m): 167 | if isinstance(m, nn.Linear): 168 | torch.nn.init.xavier_uniform_(m.weight, gain=1) 169 | torch.nn.init.constant_(m.bias, 0) 170 | 171 | class GaussianPolicy(nn.Module): 172 | def __init__( 173 | self, 174 | num_inputs, 175 | num_actions, 176 | hidden_dim, 177 | action_space=None, 178 | extra_pred_dim=0, 179 | uncertainty=False, 180 | config=None 181 | ): 182 | super(GaussianPolicy, self).__init__() 183 | 184 | self.linear1 = nn.Linear(num_inputs, hidden_dim) 185 | self.linear2 = nn.Linear(hidden_dim, hidden_dim) 186 | self.uncertainty = uncertainty 187 | 188 | self.extra_pred_dim = extra_pred_dim 189 | self.mean = nn.Linear(hidden_dim, num_actions) 190 | self.extra_pred = nn.Linear(hidden_dim, self.extra_pred_dim) 191 | self.log_std_linear = nn.Linear(hidden_dim, num_actions) 192 | self.apply(weights_init_) 193 | self.action_space = action_space 194 | 195 | # action rescaling 196 | if action_space is None: 197 | self.action_scale = torch.tensor(1.0).cuda() 198 | self.action_bias = torch.tensor(0.0).cuda() 199 | else: 200 | self.action_scale = torch.FloatTensor( 201 | (action_space.high - action_space.low) / 2.0 202 | ).cuda() 203 | self.action_bias = torch.FloatTensor( 204 | (action_space.high + action_space.low) / 2.0 205 | ).cuda() 206 | 207 | def forward(self, state): 208 | x = F.relu(self.linear1(state)) 209 | x = F.relu(self.linear2(x)) 210 | mean = self.mean(x) 211 | extra_pred = self.extra_pred(x) 212 | 213 | if self.extra_pred_dim >= 7: 214 | extra_pred = torch.cat( 215 | (F.normalize(extra_pred[:, :4], p=2, dim=-1), extra_pred[:, 4:]), dim=-1) 216 | 217 | log_std = self.log_std_linear(x) 218 | log_std = torch.clamp(log_std, min=LOG_SIG_MIN, max=LOG_SIG_MAX) 219 | 220 | return mean, log_std, extra_pred 221 | 222 | def sample(self, state): 223 | mean, log_std, extra_pred = self.forward(state) 224 | std = log_std.exp() 225 | normal = Normal(mean, std) 226 | x_t = normal.rsample() 227 | if self.action_space is not None: 228 | y_t = torch.tanh(x_t) 229 | action = y_t * self.action_scale + self.action_bias 230 | else: 231 | y_t = x_t 232 | action = x_t 233 | 234 | log_prob = normal.log_prob(x_t) 235 | log_prob -= torch.log(self.action_scale * (1 - y_t.pow(2)) + epsilon) 236 | log_prob = log_prob.sum(1, keepdim=True) 237 | if self.action_space is not None: 238 | mean = torch.tanh(mean) * self.action_scale + self.action_bias 239 | return mean, log_prob, action, extra_pred 240 | 241 | 242 | def to(self, device): 243 | self.action_scale = self.action_scale.to(device) 244 | self.action_bias = self.action_bias.to(device) 245 | return super(GaussianPolicy, self).to(device) 246 | 247 | 248 | class QNetwork(nn.Module): 249 | def __init__( 250 | self, 251 | num_inputs, 252 | num_actions, 253 | hidden_dim, 254 | extra_pred_dim=0, 255 | ): 256 | super(QNetwork, self).__init__() 257 | 258 | self.num_actions = num_actions 259 | self.linear1 = nn.Linear(num_inputs + num_actions, hidden_dim) 260 | self.linear2 = nn.Linear(hidden_dim, hidden_dim) 261 | self.linear3 = nn.Linear(hidden_dim, 1) 262 | self.extra_pred_dim = extra_pred_dim 263 | 264 | self.linear4 = nn.Linear(num_inputs + num_actions, hidden_dim) 265 | self.linear5 = nn.Linear(hidden_dim, hidden_dim) 266 | self.linear6 = nn.Linear(hidden_dim, 1) 267 | if self.extra_pred_dim > 0: 268 | self.linear7 = nn.Linear(num_inputs , hidden_dim) 269 | self.linear8 = nn.Linear(hidden_dim, hidden_dim) 270 | self.extra_pred = nn.Linear(hidden_dim, self.extra_pred_dim) 271 | self.apply(weights_init_) 272 | 273 | def forward(self, state, action=None): 274 | x3 = None 275 | if action is not None: 276 | xu = torch.cat([state, action], 1) 277 | else: 278 | xu = state 279 | x1 = F.relu(self.linear1(xu)) 280 | x1 = F.relu(self.linear2(x1)) 281 | x1 = self.linear3(x1) 282 | 283 | x2 = F.relu(self.linear4(xu)) 284 | x2 = F.relu(self.linear5(x2)) 285 | x2 = self.linear6(x2) 286 | 287 | if self.extra_pred_dim > 0: 288 | if self.num_actions > 0: # state only 289 | state = state[:,:-self.num_actions] 290 | x3 = F.relu(self.linear7(state)) 291 | x3 = F.relu(self.linear8(x3)) 292 | x3 = self.extra_pred(x3) 293 | if self.extra_pred_dim >= 7: # normalize quaternion 294 | x3 = torch.cat((F.normalize(x3[:, :4], p=2, dim=-1), x3[:, 4:]), dim=-1) 295 | return x1, x2, x3 296 | -------------------------------------------------------------------------------- /core/networks.py: -------------------------------------------------------------------------------- 1 | # -------------------------------------------------------- 2 | # Licensed under The MIT License [see LICENSE for details] 3 | # -------------------------------------------------------- 4 | import torch 5 | from torch import nn 6 | import numpy as np 7 | from core.utils import * 8 | import torch.nn.functional as F 9 | from torch.distributions import Normal 10 | from core.network_arch import * 11 | 12 | 13 | class PointTrajLatentNet(nn.Module): 14 | def __init__( 15 | self, 16 | input_dim=3, 17 | pointnet_nclusters=32, 18 | pointnet_radius=0.02, 19 | model_scale=1, 20 | output_model_scale=1, 21 | extra_latent=0, 22 | large_feature=False, 23 | feature_option=0, 24 | extra_pred_dim=2, 25 | group_norm=True, 26 | **kwargs 27 | ): 28 | """ 29 | pointnet++ backbone network 30 | """ 31 | super(PointTrajLatentNet, self).__init__() 32 | self.input_dim = 3 + extra_latent 33 | self.model_scale = model_scale 34 | 35 | self.encoder = create_encoder(model_scale, pointnet_radius, pointnet_nclusters, self.input_dim, feature_option, group_norm=group_norm) 36 | self.feature_option = feature_option 37 | self.fc_embedding = nn.Linear(int(self.model_scale * 512), int(output_model_scale * 512)) 38 | 39 | self.extra_pred_dim = extra_pred_dim 40 | self.fc_extra = get_fc_feat_head(int(self.model_scale * 512), [256, 64], extra_pred_dim) 41 | 42 | def forward( 43 | self, 44 | pc, 45 | grasp=None, 46 | feature_2=False, 47 | train=True, 48 | traj_state=None, 49 | traj_inbatch_index=None, 50 | traj_time_batch=None, 51 | traj_latent=None, 52 | encode=True 53 | ): 54 | 55 | input_features = pc 56 | extra = input_features 57 | if input_features.shape[-1] != 4096: 58 | input_features = input_features[...,6:] 59 | input_features = input_features[:, :self.input_dim].contiguous() 60 | object_grasp_pc = input_features.transpose(1, -1)[..., :3].contiguous() 61 | point_feat = pointnet_encode(self.encoder, object_grasp_pc, input_features) 62 | z, extra = self.fc_embedding(point_feat), self.fc_extra(point_feat) 63 | 64 | extra = point_feat 65 | return z, extra 66 | 67 | 68 | class PointNetFeature(nn.Module): 69 | def __init__( 70 | self, 71 | input_dim=3, 72 | pointnet_nclusters=32, 73 | pointnet_radius=0.02, 74 | model_scale=1, 75 | extra_latent=0, 76 | split_feature=False, 77 | policy_extra_latent=-1, 78 | critic_extra_latent=-1, 79 | action_concat=False, 80 | feature_option=0, 81 | group_norm=True, 82 | **kwargs ): 83 | """ 84 | poinet++ feature network 85 | """ 86 | super(PointNetFeature, self).__init__() 87 | self.input_dim = 3 + extra_latent 88 | input_dim = (3 + policy_extra_latent if policy_extra_latent > 0 else self.input_dim ) 89 | self.policy_input_dim = input_dim 90 | self.model_scale = model_scale 91 | self.encoder = create_encoder( 92 | model_scale, pointnet_radius, pointnet_nclusters, self.policy_input_dim, feature_option, group_norm=group_norm ) 93 | input_dim = 3 + critic_extra_latent if critic_extra_latent > 0 else input_dim 94 | self.critic_input_dim = input_dim 95 | if action_concat: self.critic_input_dim = input_dim + 6 96 | 97 | self.value_encoder = create_encoder( 98 | model_scale, pointnet_radius, pointnet_nclusters, self.critic_input_dim, feature_option, group_norm=group_norm ) 99 | self.feature_option = feature_option 100 | 101 | 102 | def forward( 103 | self, 104 | pc, 105 | grasp=None, 106 | concat_option="channel_wise", 107 | feature_2=False, 108 | train=True, 109 | traj_state=None, 110 | traj_inbatch_index=None, 111 | traj_time_batch=None, 112 | traj_latent=None ): 113 | 114 | input_features = pc 115 | extra = input_features 116 | if input_features.shape[-1] != 4096: 117 | input_features = input_features[...,6:] 118 | input_features = ( 119 | input_features[:, : self.critic_input_dim].contiguous() 120 | if feature_2 121 | else input_features[:, : self.policy_input_dim].contiguous() 122 | ) 123 | object_grasp_pc = input_features.transpose(1, -1)[..., :3].contiguous() 124 | 125 | encoder = self.value_encoder if feature_2 else self.encoder 126 | z = pointnet_encode(encoder, object_grasp_pc, input_features) 127 | return z, extra 128 | 129 | 130 | 131 | class STPointNetFeature(nn.Module): 132 | def __init__( 133 | self, 134 | input_dim=3, 135 | pointnet_nclusters=32, 136 | pointnet_radius=0.02, 137 | model_scale=1, 138 | extra_latent=0, 139 | feature_option=1, 140 | group_norm=True, 141 | **kwargs ): 142 | """ 143 | spatiotemporal point network 144 | """ 145 | super(STPointNetFeature, self).__init__() 146 | self.base_dim = 4 + extra_latent 147 | self.encoder, self.output_dim = create_encoder( 148 | model_scale, pointnet_radius, pointnet_nclusters, 149 | self.base_dim, feature_option, traj_net=True, group_norm=group_norm ) 150 | self.feature_option = feature_option 151 | 152 | 153 | def forward( 154 | self, 155 | pc, 156 | grasp=None, 157 | concat_option="channel_wise", 158 | feature_2=False, 159 | train=True, 160 | traj_state=None, 161 | traj_inbatch_index=None, 162 | traj_time_batch=None, 163 | traj_latent=None 164 | ): 165 | input_features = pc 166 | if input_features.shape[-1] != 4096: 167 | input_features = input_features[...,6:] # ignore hand points 168 | 169 | input_features_vis = input_features 170 | traj_time_batch = traj_time_batch[...,None].expand(-1, -1, input_features.shape[2]) 171 | input_features = torch.cat((input_features, traj_time_batch), dim=1) 172 | pc = input_features.transpose(1, -1)[..., :3].contiguous() 173 | 174 | global_feat = [] 175 | for idx in torch.unique(traj_inbatch_index): # each traj pc separate process no speed up with batch 176 | index = torch.where(traj_inbatch_index == idx) 177 | size = len(index[0]) 178 | global_pc = input_features[index].transpose(1, -1).contiguous().view(-1, self.base_dim).T.contiguous()[None] 179 | global_feat_i = self.encoder[0](global_pc)[0].expand(size, -1) 180 | global_feat.append(global_feat_i) 181 | 182 | global_feat = torch.cat(global_feat, dim=0) 183 | input_features = input_features[:,:self.base_dim].contiguous() 184 | local_feat_1 = pointnet_encode(self.encoder[-1], pc, input_features) # each timestep pc 185 | z = torch.cat((global_feat, local_feat_1), dim=1) 186 | return z, input_features_vis 187 | 188 | 189 | class TrajSamplerNet(nn.Module): 190 | def __init__( 191 | self, 192 | num_inputs, 193 | num_actions, 194 | hidden_dim, 195 | action_space=None, 196 | extra_pred_dim=0, 197 | config=None, 198 | input_dim=3, 199 | **kwargs ): 200 | """ 201 | latent plan sampler network 202 | """ 203 | super(TrajSamplerNet, self).__init__() 204 | self.config = config 205 | self.setup_latent_sampler(**kwargs) 206 | 207 | def setup_latent_sampler(self, **kwargs): 208 | config = self.config 209 | input_dim = config.traj_latent_size 210 | self.curr_state_encoder = eval(config.traj_vae_feature_extractor_class)(**kwargs) 211 | self.sampler_bottleneck = create_bottleneck(config.policy_traj_latent_size, config.normal_vae_dim) 212 | self.cvae_encoder = get_fc_feat_head(input_dim + config.policy_traj_latent_size, [1024, 512, 512, 256, 256, 128], config.policy_traj_latent_size) 213 | self.cvae_decoder = get_fc_feat_head(input_dim + config.normal_vae_dim, [1024, 512, 512, 256, 256, 128], config.policy_traj_latent_size) 214 | self.apply(weights_init_) 215 | 216 | def forward(self, 217 | curr_point_state, 218 | exec_point_state=None, 219 | grasp=None, 220 | train=True, 221 | index_mask=None, 222 | extra_time=None, 223 | traj_latent=None, 224 | traj_time_batch=None, 225 | traj_inbatch_index=None, 226 | encode=True, 227 | vis=False): 228 | 229 | traj_sampler_latent, extra_feat_pred = self.curr_state_encoder(curr_point_state, 230 | traj_latent=traj_latent, 231 | traj_time_batch=traj_time_batch, 232 | traj_inbatch_index=traj_inbatch_index, 233 | encode=encode) 234 | 235 | return traj_sampler_latent, None, None, extra_feat_pred 236 | 237 | def forward_bottleneck(self, traj_feat, traj_inbatch_index=None, prev_latent=None, traj_latent=None): 238 | sampler_mu, sampler_logsigma = self.sampler_bottleneck[0](traj_feat), self.sampler_bottleneck[1](traj_feat) 239 | if traj_inbatch_index is not None: 240 | sampler_mu_, sampler_logsigma_ = sampler_mu[traj_inbatch_index], sampler_logsigma[traj_inbatch_index] 241 | sample = reparameterize(sampler_mu_, sampler_logsigma_) 242 | else: 243 | sample = reparameterize(sampler_mu, sampler_logsigma) 244 | return sample, sampler_mu, sampler_logsigma 245 | 246 | def conditional_sampler_vae_head(self, traj_feat, traj_inbatch_index=None, conditional_latent=None): 247 | """ 248 | conditional vae forward pass 249 | """ 250 | sampler_mu, sampler_logsigma = None, None 251 | if conditional_latent is not None: 252 | encoded_latent = self.cvae_encoder(torch.cat((traj_feat[traj_inbatch_index][:len(conditional_latent)], conditional_latent), dim=-1)) 253 | sampled_encoded_latent, sampler_mu, sampler_logsigma = self.forward_bottleneck(encoded_latent) 254 | else: 255 | sampled_encoded_latent = sample_gaussian((max(traj_feat.shape[0], len(traj_inbatch_index)), self.config.normal_vae_dim), truncate_std=self.config.test_log_sigma_clip).cuda() 256 | 257 | decoded_latent = self.cvae_decoder(torch.cat((traj_feat[traj_inbatch_index], sampled_encoded_latent), dim=-1)) 258 | return decoded_latent, sampler_mu, sampler_logsigma, sampled_encoded_latent 259 | 260 | 261 | class TrajEmbeddingNet(nn.Module): 262 | def __init__( 263 | self, 264 | feature_extractor_class, 265 | num_inputs, 266 | num_actions, 267 | hidden_dim, 268 | action_space=None, 269 | extra_pred_dim=0, 270 | config=None, 271 | input_dim=3, 272 | **kwargs 273 | ): 274 | """ 275 | latent plan embedding network 276 | """ 277 | super(TrajEmbeddingNet, self).__init__() 278 | config.num_inputs = num_inputs 279 | config.action_dim = num_actions 280 | config.action_space = PandaTaskSpace6D() 281 | self.config = config 282 | self.traj_encoder = eval(feature_extractor_class)(**kwargs) 283 | self.fc_embedding = get_fc_feat_head(self.traj_encoder.output_dim, [512], config.traj_latent_size, end_with_act=True) 284 | self.traj_fc_embedding = nn.Linear(config.traj_latent_size, config.traj_latent_size) 285 | self.apply(weights_init_) 286 | 287 | 288 | def forward(self, 289 | traj_point_state=None, 290 | train=True, 291 | traj_state=None, 292 | traj_joint=None, 293 | traj_inbatch_index=None, 294 | traj_time=None, 295 | traj_goal=None, 296 | traj_action=None, 297 | traj_pose=None, 298 | vis=False, 299 | val=False, 300 | **kwargs): 301 | return self.traj_encoder( traj_point_state, 302 | traj_state=traj_state, 303 | traj_inbatch_index=traj_inbatch_index, 304 | traj_time_batch=traj_time) 305 | 306 | 307 | def head(self, feat, traj_inbatch_index, val=False): 308 | """ 309 | summarize local and global point features 310 | """ 311 | feat_embedding = self.fc_embedding(feat) 312 | traj_feat = [] 313 | for idx in torch.unique(traj_inbatch_index): 314 | traj_idx = torch.where(traj_inbatch_index == idx) 315 | global_feat_embedding = feat_embedding[traj_idx].max(0) 316 | traj_feat.append(global_feat_embedding[0]) 317 | 318 | traj_feat = torch.stack(traj_feat, dim=0) 319 | traj_feat = self.traj_fc_embedding(traj_feat) 320 | return traj_feat, None 321 | -------------------------------------------------------------------------------- /core/train_offline.py: -------------------------------------------------------------------------------- 1 | # -------------------------------------------------------- 2 | # Licensed under The MIT License [see LICENSE for details] 3 | # -------------------------------------------------------- 4 | 5 | import argparse 6 | import datetime 7 | 8 | import numpy as np 9 | import itertools 10 | import torch 11 | from core.bc import BC 12 | from core.ddpg import DDPG 13 | from core.dqn_hrl import DQN_HRL 14 | 15 | from tensorboardX import SummaryWriter 16 | from env.panda_cluttered_scene import PandaYCBEnv, PandaTaskSpace6D 17 | from experiments.config import * 18 | from core import networks 19 | from collections import deque 20 | import glob 21 | from core.utils import * 22 | from core.trainer import * 23 | import json 24 | import scipy.io as sio 25 | import IPython 26 | import pprint 27 | import cv2 28 | 29 | import GPUtil 30 | import os 31 | import ray 32 | 33 | def create_parser(): 34 | parser = argparse.ArgumentParser(description= '') 35 | parser.add_argument('--policy', default="BC" ) 36 | parser.add_argument('--seed', type=int, default=123456, metavar='N' ) 37 | 38 | parser.add_argument('--save_model', action="store_true") 39 | parser.add_argument('--pretrained', type=str, default='', help='test one model') 40 | parser.add_argument('--log', action="store_true", help='log') 41 | parser.add_argument('--model_surfix', type=str, default='latest', help='surfix for loaded model') 42 | 43 | parser.add_argument('--config_file', type=str, default=None) 44 | parser.add_argument('--batch_size', type=int, default=-1) 45 | parser.add_argument('--fix_output_time', type=str, default=None) 46 | parser.add_argument('--use_ray', action="store_true") 47 | parser.add_argument('--load_online_buffer', action="store_true") 48 | parser.add_argument('--pretrained_policy_name', type=str, default='BC') 49 | return parser 50 | 51 | 52 | def setup(): 53 | """ 54 | Set up networks with pretrained models and config as well as data migration 55 | """ 56 | 57 | load_from_pretrain = args.pretrained is not None and os.path.exists(os.path.join(args.pretrained, "config.yaml")) 58 | 59 | if load_from_pretrain : 60 | cfg_folder = args.pretrained 61 | 62 | cfg_from_file(os.path.join(cfg_folder, "config.yaml"), reset_model_spec=False) 63 | cfg.RL_MODEL_SPEC = os.path.join(cfg_folder, cfg.RL_MODEL_SPEC.split("/")[-1]) 64 | dt_string = args.pretrained.split("/")[-1] 65 | 66 | if args.fix_output_time is None: 67 | dt_string = datetime.datetime.now().strftime("%d_%m_%Y_%H:%M:%S") 68 | else: 69 | dt_string = args.fix_output_time 70 | 71 | model_output_dir = os.path.join(cfg.OUTPUT_DIR, dt_string) 72 | print("Output will be saved to `{:s}`".format(model_output_dir)) 73 | new_output_dir = not os.path.exists(model_output_dir) 74 | 75 | if new_output_dir: 76 | os.makedirs(model_output_dir) 77 | if args.config_file is not None: 78 | if not args.config_file.endswith('.yaml'): args.config_file = args.config_file + '.yaml' 79 | script_file = os.path.join(cfg.SCRIPT_FOLDER, args.config_file) 80 | cfg_from_file(script_file) 81 | cfg.script_name = args.config_file 82 | os.system( "cp {} {}".format( script_file, os.path.join(model_output_dir, args.config_file))) 83 | os.system( "cp {} {}".format(cfg.RL_MODEL_SPEC, 84 | os.path.join(model_output_dir, cfg.RL_MODEL_SPEC.split("/")[-1]))) 85 | 86 | if load_from_pretrain: 87 | cfg.pretrained_time = args.pretrained.split("/")[-1] 88 | 89 | save_cfg_to_file(os.path.join(model_output_dir, "config.yaml"), cfg) 90 | 91 | if load_from_pretrain: 92 | migrate_model(args.pretrained, model_output_dir, args.model_surfix, args.pretrained_policy_name, POLICY ) 93 | print("migrate policy...") 94 | 95 | print("Using config:") 96 | pprint.pprint(cfg) 97 | net_dict = make_nets_opts_schedulers(cfg.RL_MODEL_SPEC, cfg.RL_TRAIN) 98 | print("Output will be saved to `{:s}`".format(model_output_dir)) 99 | return net_dict, dt_string 100 | 101 | def train_off_policy(): 102 | """ 103 | train the network in inner loop with off-policy saved data 104 | """ 105 | losses = get_loss_info_dict() 106 | update_step = 0 107 | for epoch in itertools.count(1): 108 | start_time = time.time() 109 | data_time, network_time = 0., 0. 110 | 111 | for i in range(CONFIG.updates_per_step): 112 | batch_data = memory.sample(batch_size=CONFIG.batch_size) 113 | if args.load_online_buffer: 114 | online_batch_data = online_memory.sample(batch_size=int(CONFIG.online_buffer_ratio * CONFIG.batch_size)) 115 | batch_data = {k: np.concatenate((batch_data[k], online_batch_data[k]), axis=0) for k in batch_data.keys() \ 116 | if type(batch_data[k]) is np.ndarray and k in online_batch_data.keys()} 117 | data_time = data_time + (time.time() - start_time) 118 | start_time = time.time() 119 | loss = agent.update_parameters(batch_data, agent.update_step, i) 120 | update_step = agent.update_step 121 | agent.step_scheduler(agent.update_step) 122 | lrs = agent.get_lr() 123 | 124 | network_time += (time.time() - start_time) 125 | for k, v in loss.items(): 126 | if k in losses: losses[k].append(v) 127 | 128 | if args.save_model and epoch % 100 == 0 and i == 0: 129 | agent.save_model( update_step, output_dir=model_output_dir) 130 | print('save model path: {} {} step: {}'.format(output_time, logdir, update_step)) 131 | 132 | if args.save_model and update_step in CONFIG.save_epoch: 133 | agent.save_model( update_step, output_dir=model_output_dir, surfix='epoch_{}'.format(update_step)) 134 | print('save model path: {} {} step: {}'.format(model_output_dir, logdir, update_step)) 135 | 136 | if args.log and update_step % LOG_INTERVAL == 0: 137 | loss = merge_two_dicts(loss, lrs) 138 | for k, v in loss.items(): 139 | if v == 0: continue 140 | if k.endswith('loss'): 141 | writer.add_scalar('loss/{}'.format(k), v, update_step) 142 | elif 'ratio' in k or 'gradient' in k or 'lr' in k: 143 | writer.add_scalar('scalar/{}'.format(k), v, update_step) 144 | elif v != 0: 145 | writer.add_scalar('info/{}'.format(k), v, update_step) 146 | 147 | print('==================================== Learn ====================================') 148 | print('model: {} epoch: {} updates: {} lr: {:.6f} network time: {:.2f} data time: {:.2f} batch size: {}'.format( 149 | output_time, epoch, update_step, lrs['policy_lr'], network_time, data_time, CONFIG.batch_size)) 150 | 151 | headers = ['loss name', 'loss val'] 152 | data = [ (name, np.mean(list(loss))) 153 | for name, loss in losses.items() if np.mean(list(loss)) != 0 ] 154 | print(tabulate.tabulate(data, headers, tablefmt='psql')) 155 | print('===================================== {} ====================================='.format(cfg.script_name)) 156 | 157 | if update_step >= CONFIG.max_epoch: 158 | break 159 | 160 | 161 | if __name__ == "__main__": 162 | parser = create_parser() 163 | root_dir = os.path.join(os.path.dirname(os.path.abspath(__file__)), "..") 164 | args, other_args = parser.parse_known_args() 165 | GPUs = torch.cuda.device_count() 166 | CLUSTER = check_ngc() 167 | 168 | if GPUs == 1: 169 | agent_wrapper = AgentWrapperGPU1 170 | elif GPUs == 2: 171 | agent_wrapper = AgentWrapperGPU2 172 | elif GPUs == 3: 173 | agent_wrapper = AgentWrapperGPU3 174 | elif GPUs == 4: 175 | agent_wrapper = AgentWrapperGPU4 176 | 177 | torch.manual_seed(args.seed) 178 | np.random.seed(args.seed) 179 | torch.backends.cudnn.benchmark = True 180 | torch.backends.cudnn.deterministic = True 181 | POLICY = args.policy # 182 | net_dict, output_time = setup() 183 | online_buffer_size = cfg.ONPOLICY_MEMORY_SIZE if cfg.ONPOLICY_MEMORY_SIZE > 0 else cfg.RL_MEMORY_SIZE 184 | 185 | # Args 186 | CONFIG = cfg.RL_TRAIN 187 | TRAIN = True 188 | MAX_STEP = cfg.RL_MAX_STEP 189 | TOTAL_MAX_STEP = MAX_STEP + 20 190 | LOAD_MEMORY = True 191 | LOG_INTERVAL = 4 192 | CONFIG.output_time = output_time 193 | CONFIG.off_policy = True 194 | 195 | # Agent 196 | input_dim = CONFIG.feature_input_dim 197 | model_output_dir = os.path.join(cfg.OUTPUT_DIR, output_time) 198 | pretrained_path = model_output_dir 199 | action_space = PandaTaskSpace6D() 200 | CONFIG.batch_size = cfg.OFFLINE_BATCH_SIZE 201 | 202 | # Tensorboard 203 | logdir = '{}/{}/{}_{}'.format(cfg.OUTPUT_DIR, output_time, CONFIG.env_name, POLICY) 204 | print('output_time: {} logdir: {}'.format(output_time, logdir)) 205 | 206 | CONFIG.model_output_dir = model_output_dir 207 | CONFIG.logdir = logdir 208 | CONFIG.CLUSTER = False 209 | CONFIG.ON_POLICY = CONFIG.onpolicy 210 | 211 | # Main 212 | if not args.use_ray: 213 | from core.replay_memory import BaseMemory as ReplayMemory 214 | agent = globals()[POLICY](input_dim, action_space, CONFIG) # 138 215 | agent.setup_feature_extractor(net_dict, False) 216 | agent.load_model(pretrained_path, surfix=args.model_surfix, set_init_step=True) 217 | memory = ReplayMemory(int(cfg.RL_MEMORY_SIZE) , cfg) 218 | memory.load(cfg.RL_SAVE_DATA_ROOT_DIR, cfg.RL_MEMORY_SIZE) 219 | if args.load_online_buffer: 220 | online_memory = ReplayMemory(online_buffer_size, cfg, 'online') 221 | online_memory.load(cfg.RL_SAVE_DATA_ROOT_DIR, cfg.RL_MEMORY_SIZE) 222 | writer = SummaryWriter(logdir=logdir) 223 | train_off_policy() 224 | else: 225 | object_store_memory = int(8 * 1e9) 226 | ray.init(num_cpus=8, webui_host="0.0.0.0") # 227 | buffer_id = ReplayMemoryWrapper.remote(int(cfg.RL_MEMORY_SIZE), cfg, 'expert') 228 | ray.get(buffer_id.load.remote(cfg.RL_SAVE_DATA_ROOT_DIR, cfg.RL_MEMORY_SIZE)) 229 | 230 | if not args.load_online_buffer: 231 | online_buffer_id = ReplayMemoryWrapper.remote(1, cfg, 'online') 232 | else: 233 | online_buffer_id = ReplayMemoryWrapper.remote(online_buffer_size, cfg, 'online') 234 | ray.get(online_buffer_id.load.remote(cfg.RL_SAVE_DATA_ROOT_DIR, cfg.ONPOLICY_MEMORY_SIZE)) 235 | 236 | learner_id = agent_wrapper.remote(args, cfg, pretrained_path, input_dim, logdir, True, 237 | args.model_surfix, model_output_dir, buffer_id) 238 | 239 | trainer = TrainerRemote.remote(args, cfg, learner_id, buffer_id, online_buffer_id, logdir, model_output_dir) 240 | while ray.get(learner_id.get_agent_update_step.remote()) < CONFIG.max_epoch: 241 | ray.get(trainer.train_iter.remote()) 242 | -------------------------------------------------------------------------------- /core/trainer.py: -------------------------------------------------------------------------------- 1 | # -------------------------------------------------------- 2 | # Licensed under The MIT License [see LICENSE for details] 3 | # -------------------------------------------------------- 4 | 5 | import ray 6 | from core.utils import * 7 | from core.replay_memory import BaseMemory 8 | import datetime 9 | import os 10 | 11 | class ReplayMemoryWrapperBase(object): 12 | """ 13 | Wrapper class for the replay buffer 14 | """ 15 | def __init__(self, buffer_size, args, name): 16 | self.memory = BaseMemory(buffer_size, args, name) 17 | 18 | def sample(self, *args): 19 | return self.memory.sample(*args) 20 | 21 | def sample_episode(self, *args): 22 | return self.memory.sample_episode(*args) 23 | 24 | def load(self, *args): 25 | return self.memory.load(*args) 26 | 27 | def push(self, *args): 28 | return self.memory.push(*args) 29 | 30 | def get_total_lifted(self, *args): 31 | return self.memory.get_total_lifted(*args) 32 | 33 | def is_full(self): 34 | return self.memory.is_full 35 | 36 | def save(self, *args): 37 | return self.memory.save(*args) 38 | 39 | def add_episode(self, *args): 40 | return self.memory.add_episode(*args) 41 | 42 | def upper_idx(self): 43 | return self.memory.upper_idx() 44 | 45 | def reset(self): 46 | return self.memory.reset() 47 | 48 | def get_cur_idx(self): 49 | return self.memory.get_cur_idx() 50 | 51 | def reward_info(self, *args): 52 | return self.memory.reward_info(*args) 53 | 54 | def print_obj_performance(self, *args): 55 | return self.memory.print_obj_performance(*args) 56 | 57 | def get_total_env_step(self): 58 | return self.memory.get_total_env_step() 59 | 60 | def get_info(self): 61 | return (self.memory.upper_idx(), self.memory.get_cur_idx(), self.memory.is_full, 62 | self.memory.print_obj_performance(), self.memory.get_total_env_step(), 63 | self.memory.get_expert_upper_idx(), self.memory.one_sample_timer.avg) 64 | 65 | class AgentWrapper(object): 66 | """ 67 | Wrapper class for agent training and logging 68 | """ 69 | def __init__(self, args_, config_, pretrained_path=None, input_dim=512, 70 | logdir=None, set_init_step=False, model_surfix='latest', model_path=None, buffer_id=None): 71 | from core.bc import BC 72 | from core.ddpg import DDPG 73 | from core.dqn_hrl import DQN_HRL 74 | from core import networks 75 | self.args = args_ 76 | self.config = config_.RL_TRAIN 77 | self.cfg = config_ # the global one 78 | torch.manual_seed(self.args.seed) 79 | POLICY = self.args.policy 80 | 81 | self.agent = eval(POLICY)(input_dim, PandaTaskSpace6D(), self.config) 82 | net_dict = make_nets_opts_schedulers(self.cfg.RL_MODEL_SPEC, self.config) 83 | self.agent.setup_feature_extractor(net_dict) 84 | self.buffer_id = buffer_id 85 | 86 | self.model_path = model_path 87 | self.updates = self.agent.load_model(pretrained_path, set_init_step=set_init_step, surfix=model_surfix) 88 | self.initial_updates = self.updates 89 | self.epoch = 0 90 | 91 | def get_agent_update_step(self): 92 | """ get agent update step """ 93 | return self.agent.update_step 94 | 95 | def get_agent_incr_update_step(self): 96 | """ get agent update step """ 97 | return self.agent.update_step - self.initial_updates 98 | 99 | def load_weight(self, weights): 100 | """ get agent update step """ 101 | self.agent.load_weight(weights) 102 | return [0] 103 | 104 | def get_weight(self): 105 | """ get agent update step """ 106 | return self.agent.get_weight() 107 | 108 | def save_model(self, surfix='latest'): 109 | """ save model """ 110 | self.agent.save_model(self.agent.update_step, output_dir=self.config.model_output_dir, surfix=surfix) 111 | 112 | def get_traj_latent(self): 113 | traj_feat = self.agent.traj_feat.detach().cpu().numpy()[0] if self.agent.traj_feat is not None else np.zeros(self.config.policy_traj_latent_size) 114 | normal_latent_feat = self.agent.latent_sample.detach().cpu().numpy()[0] if self.agent.latent_sample is not None else np.zeros(self.config.normal_vae_dim) 115 | return traj_feat, normal_latent_feat 116 | 117 | def get_agent(self): 118 | return self.agent 119 | 120 | def select_action(self, state, actions=None, goal_state=None, remain_timestep=1, 121 | gt_goal_rollout=True, curr_joint=None, gt_traj=None): 122 | """ on policy action """ 123 | action, traj, extra_pred, aux_pred = self.agent.select_action(state, actions=actions, goal_state=goal_state, repeat=has_check(self.config, 'CLUSTER'), 124 | remain_timestep=remain_timestep, gt_traj=gt_traj, curr_joint=curr_joint) 125 | return action, traj, extra_pred, aux_pred 126 | 127 | def select_traj(self, *args, **kwargs): 128 | """ on policy action """ 129 | return self.agent.select_traj(*args, **kwargs) 130 | 131 | def update_parameter(self, batch_data, updates, i): 132 | return self.agent.update_parameters(batch_data, updates, i) 133 | 134 | def get_agent_lr_info(self): 135 | self.agent.step_scheduler(self.agent.update_step) 136 | return (self.agent.update_step, self.agent.get_lr()) 137 | 138 | 139 | class Trainer(object): 140 | """ 141 | Wrapper class for agent training and logging 142 | """ 143 | def __init__(self, args_, config_, agent_remote_id, buffer_remote_id, online_buffer_remote_id, logdir=None, model_path=None): 144 | self.args = args_ 145 | self.config = config_.RL_TRAIN 146 | self.cfg = config_ # the global one 147 | torch.manual_seed(self.cfg.RNG_SEED) 148 | self.agent_remote_id = agent_remote_id 149 | self.model_path = model_path 150 | self.buffer_remote_id = buffer_remote_id 151 | self.online_buffer_remote_id = online_buffer_remote_id 152 | self.epoch = 0 153 | self.updates = ray.get(agent_remote_id.get_agent_update_step.remote()) 154 | self.agent_update_step = self.updates 155 | if logdir is not None: 156 | self.writer = SummaryWriter(logdir=logdir) 157 | self.file_handle = None 158 | self.losses_info = get_loss_info_dict() 159 | 160 | def save_epoch_model(self, update_step): 161 | ray.get(self.agent_remote_id.save_model.remote(surfix='epoch_{}'.format(update_step))) 162 | print_and_write(self.file_handle, 'save model path: {} {} step: {}'.format(self.config.output_time, self.config.logdir, update_step)) 163 | 164 | def save_latest_model(self, update_step): 165 | ray.get(self.agent_remote_id.save_model.remote()) 166 | print_and_write(self.file_handle, 'save model path: {} {} step: {}'.format(self.config.model_output_dir, self.config.logdir, update_step)) 167 | 168 | def get_agent_update_step(self): 169 | return ray.get(self.agent_remote_id.get_agent_update_step.remote()) 170 | 171 | def write_config(self): 172 | """ write loaded config to tensorboard """ 173 | self.writer.add_text('global args', cfg_repr(self.cfg)) 174 | self.writer.add_text('model spec', cfg_repr(yaml.load(open(self.cfg.RL_MODEL_SPEC).read()))) 175 | self.writer.add_text('output time', self.config.output_time) 176 | self.writer.add_text('script', cfg_repr(yaml.load(open(os.path.join(self.cfg.SCRIPT_FOLDER, self.args.config_file)).read()))) 177 | 178 | def write_buffer_info(self, reward_info): 179 | names = ['reward', 'avg_reward', 'online_reward', 'test_reward', 'avg_collision', 'avg_lifted'] 180 | for i in range(len(names)): # enumerate(names) 181 | eb_info, ob_info = reward_info[i] 182 | if ob_info != 0: self.writer.add_scalar('reward/ob_{}'.format(names[i]), ob_info, self.updates) 183 | 184 | def write_external_info(self, reinit_count=0, env_step=0, online_env_step=0, buffer_curr_idx=0, online_buffer_curr_idx=0, 185 | noise_scale=0, explore_ratio=0, gpu_usage=0, memory_usage=0, buffer_upper_idx=0, sample_time=0, online_buffer_upper_idx=0, 186 | gaddpg_prob=0, greedy_prob=0): 187 | """ 188 | write external information to the tensorboard 189 | """ 190 | if sample_time > 0: self.writer.add_scalar('info/avg_sample_time', sample_time, self.updates) 191 | if gpu_usage > 0: self.writer.add_scalar('info/gpu_usage', gpu_usage, self.updates) 192 | if memory_usage > 0: self.writer.add_scalar('info/memory_usage', memory_usage, self.updates) 193 | if reinit_count > 0: self.writer.add_scalar('info/epoch', reinit_count, self.updates) 194 | if online_env_step > 0: self.writer.add_scalar('info/online_env_step', online_env_step, self.updates) 195 | if env_step > 0: self.writer.add_scalar('info/env_step', env_step, self.updates) 196 | if explore_ratio > 0: self.writer.add_scalar('info/explore_ratio', explore_ratio, self.updates) 197 | if gaddpg_prob > 0: self.writer.add_scalar('info/gaddpg_prob', gaddpg_prob, self.updates) 198 | 199 | if self.agent_update_step > 0: self.writer.add_scalar('info/agent_update_step', self.agent_update_step, self.updates) 200 | if buffer_curr_idx > 0: self.writer.add_scalar('info/buffer_curr_idx', buffer_curr_idx, self.updates) 201 | if buffer_upper_idx > 0: self.writer.add_scalar('info/buffer_upper_idx', buffer_upper_idx, self.updates) 202 | if online_buffer_upper_idx > 0: self.writer.add_scalar('info/online_buffer_upper_idx', online_buffer_upper_idx, self.updates) 203 | if online_buffer_curr_idx > 0: self.writer.add_scalar('info/online_buffer_curr_idx', online_buffer_curr_idx, self.updates) 204 | 205 | def train_iter(self): 206 | """ 207 | Run inner loop training and update parameters 208 | """ 209 | LOG_INTERVAL = 3 210 | self.epoch += 1 211 | if self.epoch < self.config.data_init_epoch: 212 | return [0] # collect data first 213 | start_time = time.time() 214 | batch_size = self.config.batch_size 215 | 216 | onpolicy_batch_size = int(batch_size * self.config.online_buffer_ratio) 217 | batch_data, online_batch_data = ray.get([self.buffer_remote_id.sample.remote(self.config.batch_size), self.online_buffer_remote_id.sample.remote(onpolicy_batch_size)]) 218 | 219 | if self.config.ON_POLICY: 220 | if len(batch_data) == 0 and len(online_batch_data) > 0: # full onpolicy 221 | batch_data = online_batch_data 222 | else: 223 | batch_data = {k: np.concatenate((batch_data[k], online_batch_data[k]), axis=0) for k in batch_data.keys() \ 224 | if type(batch_data[k]) is np.ndarray and k in online_batch_data.keys()} 225 | if len(batch_data) == 0: return [0] 226 | 227 | for i in range(self.config.updates_per_step): 228 | batch_data, online_batch_data, main_loss, (update_step, lrs) = ray.get([ 229 | self.buffer_remote_id.sample.remote(self.config.batch_size), 230 | self.online_buffer_remote_id.sample.remote(onpolicy_batch_size), 231 | self.agent_remote_id.update_parameter.remote(batch_data, self.updates, i), 232 | self.agent_remote_id.get_agent_lr_info.remote() 233 | ]) 234 | 235 | if self.config.ON_POLICY: 236 | batch_data = {k: np.concatenate((batch_data[k], online_batch_data[k]), axis=0) for k in batch_data.keys() \ 237 | if type(batch_data[k]) is np.ndarray and k in online_batch_data.keys()} 238 | 239 | infos = merge_two_dicts(lrs, main_loss) 240 | for k, v in infos.items(): 241 | if k in self.losses_info: self.losses_info[k].append(v) 242 | self.updates += 1 243 | self.agent_update_step = update_step 244 | 245 | if self.args.log and self.updates % LOG_INTERVAL == 0: 246 | for k, v in infos.items(): 247 | if v == 0: 248 | continue 249 | elif k.endswith('loss'): 250 | self.writer.add_scalar('loss/{}'.format(k), v, self.updates) 251 | elif 'ratio' in k or 'gradient' in k or 'lr' in k or 'scale' in k: 252 | self.writer.add_scalar('scalar/{}'.format(k), v, self.updates) 253 | else: 254 | self.writer.add_scalar('info/{}'.format(k), v, self.updates) 255 | 256 | if self.args.save_model and update_step in self.config.save_epoch: 257 | self.save_epoch_model(update_step) 258 | 259 | if self.args.save_model and self.epoch % 50 == 0: 260 | self.save_latest_model(update_step) 261 | 262 | # log 263 | buffer_info, online_buffer_info = ray.get([self.buffer_remote_id.get_info.remote(), self.online_buffer_remote_id.get_info.remote()]) 264 | buffer_upper_idx, buffer_curr_idx, buffer_is_full, obj_performance_str, env_step, buffer_exp_idx, sample_time = buffer_info 265 | online_buffer_upper_idx, online_buffer_curr_idx, online_buffer_is_full, online_obj_performance_str, online_env_step, _, _ = online_buffer_info 266 | gpu_usage, memory_usage = get_usage() 267 | incr_agent_update_step = ray.get(self.agent_remote_id.get_agent_incr_update_step.remote()) 268 | milestone_idx = int((incr_agent_update_step > np.array(self.config.mix_milestones)).sum()) 269 | explore_ratio = min(get_valid_index(self.config.explore_ratio_list, milestone_idx), self.config.explore_cap) 270 | noise_scale = self.config.action_noise * get_valid_index(self.config.noise_ratio_list, milestone_idx) 271 | gaddpg_prob = get_valid_index(self.config.gaddpg_ratio_list, milestone_idx) 272 | greedy_prob = get_valid_index(self.config.epsilon_greedy_list, milestone_idx) 273 | 274 | self.write_external_info( env_step=env_step, 275 | online_env_step=online_env_step, 276 | buffer_curr_idx=buffer_curr_idx, 277 | buffer_upper_idx=buffer_upper_idx, 278 | online_buffer_curr_idx=online_buffer_curr_idx, 279 | online_buffer_upper_idx=online_buffer_upper_idx, 280 | gpu_usage=gpu_usage, 281 | memory_usage=memory_usage, 282 | sample_time=sample_time, 283 | explore_ratio=explore_ratio, 284 | noise_scale=noise_scale, 285 | gaddpg_prob=gaddpg_prob, 286 | greedy_prob=greedy_prob) 287 | 288 | train_iter_time = (time.time() - start_time) 289 | self.writer.add_scalar('info/train_time', train_iter_time, self.updates) 290 | print_and_write(self.file_handle, '==================================== Learn ====================================') 291 | print_and_write(self.file_handle, 'model: {} updates: {} lr: {:.5f} network time: {:.3f} sample time: {:.3f} buffer: {}/{} {}/{}'.format( 292 | self.config.output_time, self.updates, infos['policy_lr'], train_iter_time, sample_time * self.config.updates_per_step, 293 | buffer_curr_idx, buffer_upper_idx, online_buffer_curr_idx, online_buffer_upper_idx)) 294 | 295 | headers = ['loss name', 'loss val'] 296 | data = [(name, np.mean(list(loss))) 297 | for name, loss in self.losses_info.items() if np.mean(list(loss)) != 0 ] 298 | print_and_write(self.file_handle, tabulate.tabulate(data, headers, tablefmt='psql')) 299 | print_and_write(self.file_handle, '== CONFIG: {} == \n== GPU: {} MEMORY: {} TIME: {} PID: {}'.format( 300 | self.cfg.script_name, gpu_usage, memory_usage, 301 | datetime.datetime.now().strftime("%d_%m_%Y_%H:%M:%S"), 302 | os.getppid())) 303 | return [0] 304 | 305 | def get_gaddpg_agent(): 306 | gaddpg_dict = edict(yaml.load(open("output/demo_model/config.yaml", "r"))) 307 | args = edict() 308 | args.policy = 'DDPG' 309 | args.seed = 1 310 | return RolloutAgentWrapperGPU1.remote(args, gaddpg_dict, 'output/demo_model', 512, None, True ) 311 | 312 | 313 | @ray.remote(num_cpus=3) 314 | class ReplayMemoryWrapper(ReplayMemoryWrapperBase): 315 | pass 316 | 317 | @ray.remote(num_cpus=1,num_gpus=0.1) 318 | class AgentWrapperGPU05(AgentWrapper): 319 | pass 320 | 321 | @ray.remote(num_cpus=2, num_gpus=2) 322 | class AgentWrapperGPU2(AgentWrapper): 323 | pass 324 | 325 | @ray.remote(num_cpus=3, num_gpus=3) 326 | class AgentWrapperGPU3(AgentWrapper): 327 | pass 328 | 329 | @ray.remote(num_cpus=4, num_gpus=4) 330 | class AgentWrapperGPU4(AgentWrapper): 331 | pass 332 | 333 | @ray.remote(num_cpus=1, num_gpus=1) 334 | class AgentWrapperGPU1(AgentWrapper): 335 | pass 336 | 337 | @ray.remote(num_cpus=1, num_gpus=0.05) 338 | class RolloutAgentWrapperGPU1(AgentWrapper): 339 | pass 340 | 341 | @ray.remote 342 | class TrainerRemote(Trainer): 343 | pass -------------------------------------------------------------------------------- /core/traj.py: -------------------------------------------------------------------------------- 1 | # -------------------------------------------------------- 2 | # Licensed under The MIT License [see LICENSE for details] 3 | # -------------------------------------------------------- 4 | import torch 5 | import torch.nn.functional as F 6 | import numpy as np 7 | from core.utils import * 8 | from core.agent import Agent 9 | from core.loss import * 10 | 11 | 12 | class TrajAgent(Agent): 13 | """ 14 | Extends Agent to have traj functionality 15 | """ 16 | 17 | def autoencode(self, state_input_batch, train, sampler_extra_time=None): 18 | """ 19 | vae for embedding latent 20 | """ 21 | self.sampler_traj_feat, _, _, self.sampler_extra_feat_pred = self.traj_feature_sampler( state_input_batch, 22 | train=train, 23 | extra_time=sampler_extra_time) 24 | self.sampler_traj_feat, self.sampler_latent_mean, self.sampler_latent_logstd, self.latent_sample = self.traj_feature_sampler.module.conditional_sampler_vae_head( 25 | self.sampler_traj_feat, 26 | self.train_traj_idx_batch, 27 | self.dataset_traj_latent_head ) 28 | 29 | def extract_traj_embedding_feature(self, image_batch, state_input_batch, action_batch=None, goal_batch=None, 30 | time_batch=None, vis=False, goal_point_state=None, 31 | repeat=False, train=True, curr_joint=None, traj_integer_time=None, 32 | traj_state=None, traj_inbatch_index=None, 33 | joint_traj_state=None, traj_goal=None, traj_time_batch=None, sample_num=None, 34 | traj_point_state=None, val=False): 35 | """ 36 | extract trajectory embedding latent 37 | """ 38 | train = not self.test_mode 39 | traj_point_state = preprocess_points(self, traj_point_state, joint_traj_state[:, :7], traj_integer_time, traj=True) 40 | self.traj_feat, self.traj_extra_feat_pred = self.traj_feature_extractor( 41 | traj_point_state=traj_point_state, 42 | traj_state=traj_state, 43 | train=train, 44 | traj_inbatch_index=traj_inbatch_index, 45 | traj_time=traj_time_batch ) 46 | 47 | # including expert and learner 48 | self.traj_feat, self.internal_traj_recons = self.traj_feature_extractor.module.head(self.traj_feat, traj_inbatch_index, val=val) 49 | self.traj_head_feat_mean = self.traj_feat.mean() 50 | self.traj_feat_copy = self.traj_feat.clone() 51 | 52 | if self.train_traj_sampler: 53 | self.dataset_traj_latent = self.traj_feat 54 | self.traj_feat = self.traj_feat[self.train_traj_idx_batch] 55 | self.traj_feat_target = self.traj_feat.clone().detach() 56 | 57 | else: 58 | if not hasattr(self, 'cont_traj_inbatch_index'): 59 | self.cont_traj_inbatch_index = self.train_traj_idx_batch 60 | self.traj_feat = self.traj_feat[self.cont_traj_inbatch_index] 61 | self.traj_feat_target = self.traj_feat.clone().detach() 62 | 63 | def extract_traj_sampling_feature(self, image_batch, state_input_batch, action_batch=None, goal_batch=None, 64 | time_batch=None, vis=False, value=False, repeat=False, 65 | train=True, separate=True, curr_joint=None, traj_integer_time=None, 66 | traj_state=None, traj_inbatch_index=None, index_mask=None, 67 | joint_traj_state=None, traj_time_batch=None, sample_num=None, 68 | traj_pose=None, traj_point_state=None): 69 | """ 70 | extract trajectory sampler latent 71 | """ 72 | train = not self.test_mode 73 | state_input_batch = preprocess_points(self, state_input_batch, curr_joint, time_batch, append_pc_time=True) 74 | self.dataset_traj_latent_head = None if not train else self.dataset_traj_latent[self.train_traj_idx_batch] 75 | self.autoencode(state_input_batch, train) 76 | 77 | if self.use_sampler_latent or not train: 78 | self.traj_feat = self.sampler_traj_feat 79 | 80 | def train_traj_net(self ): 81 | """ 82 | training for embedding and sampler 83 | """ 84 | if not self.train_traj_sampler or not self.use_offline_latent: 85 | self.extract_traj_embedding_feature( self.image_state_batch, 86 | self.point_state_batch, 87 | traj_point_state=self.sparsify_sim_traj_point_state_batch, 88 | goal_batch=self.goal_batch, 89 | time_batch=self.time_batch, 90 | curr_joint=self.curr_joint, 91 | joint_traj_state=self.sparsify_sim_traj_joint_batch, 92 | traj_integer_time=self.sparsify_sim_traj_integer_time_batch, 93 | traj_inbatch_index=self.sparsify_sim_cont_traj_inbatch_index, 94 | traj_time_batch=self.sparsify_sim_traj_time_batch ) 95 | 96 | if self.train_traj_sampler and not self.ignore_traj_sampler: 97 | self.extract_traj_sampling_feature(self.image_state_batch, 98 | self.point_state_batch, 99 | traj_point_state=self.sparsify_sim_traj_point_state_batch, 100 | goal_batch=self.goal_batch, 101 | time_batch=self.time_batch, 102 | curr_joint=self.curr_joint, 103 | joint_traj_state=self.sparsify_sim_traj_joint_batch, 104 | traj_integer_time=self.sparsify_sim_traj_integer_time_batch, 105 | traj_inbatch_index=self.sparsify_sim_cont_traj_inbatch_index, 106 | traj_time_batch=self.sparsify_sim_traj_time_batch) 107 | 108 | 109 | 110 | @torch.no_grad() 111 | def select_traj( 112 | self, 113 | img_state, 114 | point_state, 115 | goal_state=None, 116 | vis=False, 117 | remain_timestep=0, 118 | repeat=False, 119 | curr_joint=None, 120 | gt_traj=None, 121 | sample_num=None, 122 | ): 123 | """ 124 | process trajectory in test time 125 | """ 126 | self.set_mode(True) 127 | query_traj_time = torch.linspace(0, 1, int(remain_timestep) + 1).cuda()[1:].float() 128 | self.train_traj_idx_batch = torch.Tensor([0]).long().cuda() 129 | if len(point_state) > 1: 130 | self.timestep = torch.Tensor([remain_timestep] * len(point_state)).float().cuda() # remain_timestep 131 | remain_timestep = self.timestep 132 | self.train_traj_idx_batch = torch.Tensor([0]).long().cuda().repeat(len(point_state)) 133 | 134 | if gt_traj is not None: 135 | if not hasattr(self, 'traj_feature_extractor'): return 136 | traj_point_state, joint_traj_state, traj_action, traj_poses, traj_goals, traj_integer_time, query_traj_time = gt_traj 137 | self.extract_traj_embedding_feature( img_state, 138 | point_state, 139 | time_batch=remain_timestep, 140 | goal_batch=goal_state, 141 | curr_joint=curr_joint, 142 | repeat=repeat, 143 | train=False, 144 | traj_time_batch=query_traj_time[:, 1][:,None], 145 | traj_point_state=traj_point_state, 146 | joint_traj_state=joint_traj_state, 147 | traj_integer_time=traj_integer_time, 148 | traj_inbatch_index=torch.zeros_like(traj_integer_time).long() ) 149 | 150 | traj = None 151 | 152 | else: 153 | if not hasattr(self, 'traj_feature_sampler'): return 154 | self.extract_traj_sampling_feature(img_state, 155 | point_state, 156 | time_batch=remain_timestep, 157 | goal_batch=goal_state, 158 | curr_joint=curr_joint, 159 | repeat=repeat, 160 | train=False, 161 | traj_time_batch=query_traj_time[:,None], 162 | traj_inbatch_index=torch.zeros(len(point_state), 163 | device=torch.device('cuda')).long()) 164 | print('sample traj latent') 165 | traj = None 166 | 167 | self.traj_feat_target_test = self.traj_feat.clone() 168 | if has_check(self, 'multi_traj_sample') and gt_traj is None: 169 | traj = self.multi_select_traj(img_state, point_state, remain_timestep, curr_joint, vis) 170 | return traj 171 | 172 | @torch.no_grad() 173 | def multi_select_traj( 174 | self, 175 | img_state, 176 | point_state, 177 | remain_timestep=0, 178 | curr_joint=None, 179 | vis=False, 180 | ): 181 | """ 182 | multi-sample trajectory selection in test time 183 | """ 184 | vis_traj = None 185 | if type(curr_joint) is not np.ndarray: 186 | curr_joint = curr_joint.detach().cpu().numpy() 187 | finger_joints = np.ones((len(curr_joint), 2)) * 0.04 188 | curr_joint = np.concatenate((curr_joint, finger_joints), axis=1) 189 | 190 | if type(point_state) is not np.ndarray: 191 | point_state = point_state.detach().cpu().numpy() 192 | 193 | if vis: 194 | vis_traj, traj_time_batch, joint_traj_state, traj_point_state, traj_integer_time = generate_simulated_learner_trajectory( 195 | point_state, curr_joint, self, remain_timestep, self.test_traj_num) 196 | return vis_traj 197 | -------------------------------------------------------------------------------- /env/__init__.py: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /env/env_planner.py: -------------------------------------------------------------------------------- 1 | # -------------------------------------------------------- 2 | # Licensed under The MIT License [see LICENSE for details] 3 | # -------------------------------------------------------- 4 | 5 | import pybullet as p 6 | import numpy as np 7 | import IPython 8 | 9 | try: 10 | from OMG.omg.config import cfg as planner_cfg 11 | from OMG.omg.core import PlanningScene 12 | except: 13 | pass 14 | from core.utils import * 15 | 16 | class EnvPlanner(): 17 | """ 18 | Class for franka panda environment with YCB objects. 19 | """ 20 | 21 | def __init__(self, env=None): 22 | self.env = env 23 | self._planner_setup = False 24 | self.env.planner_reach_grasps = [] 25 | self.env.planner_goal_set = [] 26 | self.planner_change_goal = False 27 | 28 | def plan_to_conf(self, step, start_conf, goal_conf, vis=False): 29 | """ 30 | Plan to a fix configuration 31 | """ 32 | planner_cfg.timesteps = step # 20 33 | planner_cfg.get_global_param(step) 34 | self.planner_scene.traj.start = start_conf 35 | dummy_goal_set = planner_cfg.goal_set_proj 36 | setattr(planner_cfg, 'goal_set_proj', False) 37 | planner_cfg.get_global_param() 38 | self.planner_scene.traj.end = goal_conf 39 | self.planner_scene.update_planner() 40 | info = self.planner_scene.step() 41 | setattr(planner_cfg, 'goal_set_proj', dummy_goal_set) 42 | planner_cfg.get_global_param() 43 | plan = self.planner_scene.planner.history_trajectories[-1] 44 | return plan 45 | 46 | 47 | def setup_expert_scene(self): 48 | """ 49 | Load all meshes once and then update pose 50 | """ 51 | # parameters 52 | print('set up expert scene ...') 53 | for key, val in self.env._omg_config.items(): 54 | setattr(planner_cfg, key, val) 55 | 56 | planner_cfg.get_global_param(planner_cfg.timesteps) 57 | planner_cfg.get_global_path() 58 | 59 | # load obstacles 60 | self.planner_scene = PlanningScene(planner_cfg) 61 | self.planner_scene.traj.start = np.array(self.env._panda.getJointStates()[0]) 62 | self.planner_scene.env.clear() 63 | 64 | obj_names, obj_poses = self.env.get_env_info(self.env._cur_scene_file) 65 | object_lists = [name.split('/')[-1].strip() for name in obj_names] 66 | object_poses = [pack_pose(pose) for pose in obj_poses] 67 | 68 | for i, name in enumerate(self.env.obj_path[:-2]): 69 | name = name.split('/')[-2] 70 | trans, orn = self.env.placed_object_poses[i] 71 | self.planner_scene.env.add_object(name, trans, tf_quat(orn), compute_grasp=True) 72 | 73 | self.planner_scene.env.add_plane(np.array([0.05, 0, -0.17]), np.array([1,0,0,0])) # never moved 74 | self.planner_scene.env.add_table(np.array([0.55, 0, -0.17]), np.array([0.707, 0.707, 0., 0])) 75 | self.planner_scene.env.combine_sdfs() 76 | self._planner_setup = True 77 | 78 | def expert_plan_grasp(self, step=-1, check_scene=False, checked=False, fix_goal=False): 79 | """ 80 | Generate expert to grasp the target object 81 | """ 82 | joint_pos = self.env._panda.getJointStates()[0] 83 | self.planner_scene.traj.start = np.array(joint_pos) 84 | self.planner_scene.env.set_target(self.env.obj_path[self.env.target_idx].split('/')[-2]) #scene.env.names[0]) 85 | info = [] 86 | if step > 0: # continued plan 87 | self.get_planning_scene_target().compute_grasp = False 88 | planner_cfg.timesteps = step # 20 89 | if fix_goal: 90 | planner_cfg.goal_idx = self.planner_scene.traj.goal_idx 91 | planner_cfg.ol_alg = "Baseline" 92 | planner_cfg.get_global_param(planner_cfg.timesteps) 93 | self.planner_scene.reset(lazy=True) 94 | self.prev_planner_goal_idx = self.planner_scene.traj.goal_idx 95 | 96 | if not check_scene: 97 | info = self.planner_scene.step() 98 | 99 | planner_cfg.timesteps = self.env._expert_step # set back 100 | planner_cfg.get_global_param(planner_cfg.timesteps) 101 | if fix_goal: # reset 102 | planner_cfg.goal_idx = -1 103 | planner_cfg.ol_alg = "MD" 104 | self.planner_change_goal = self.prev_planner_goal_idx != self.planner_scene.traj.goal_idx 105 | self.prev_planner_goal_idx = self.planner_scene.traj.goal_idx 106 | 107 | else: 108 | if checked: 109 | self.get_planning_scene_target().compute_grasp = False 110 | 111 | self.planner_scene.reset(lazy=True) 112 | if not check_scene: 113 | info = self.planner_scene.step() 114 | self.planner_change_goal = False 115 | self.prev_planner_goal_idx = -1 116 | 117 | return info 118 | 119 | def expert_plan(self, step=-1, return_success=False, check_scene=False, checked=False, fix_goal=True, 120 | joints=None, ef_pose=None, pts=None, vis=False): 121 | """ 122 | Run OMG planner for the current scene 123 | """ 124 | if not self._planner_setup: 125 | self.setup_expert_scene() 126 | 127 | obj_names, obj_poses = self.env.get_env_info(self.env._cur_scene_file) 128 | object_lists = [name.split('/')[-1].strip() for name in obj_names] 129 | object_poses = [pack_pose(pose) for pose in obj_poses] 130 | exists_ids = [] 131 | placed_poses = [] 132 | placed_obj_names = [] 133 | plan = [] 134 | 135 | if self.env.target_idx == -1 or self.env.target_name == 'noexists': 136 | if not return_success: 137 | return [], np.zeros(0) 138 | return [], np.zeros(0), False 139 | 140 | for i, name in enumerate(object_lists[:-2]): # buffer 141 | self.planner_scene.env.update_pose(name, object_poses[i]) 142 | idx = self.env.obj_path[:-2].index(os.path.join(self.env.root_dir, 'data/objects/' + name + '/')) 143 | exists_ids.append(idx) 144 | trans, orn = self.env.placed_object_poses[idx] 145 | placed_poses.append(np.hstack([trans, ros_quat(orn)])) 146 | placed_obj_names.append(name) 147 | 148 | planner_cfg.disable_collision_set = [name.split('/')[-2] for idx, name in enumerate(self.env.obj_path[:-2]) 149 | if idx not in exists_ids] 150 | 151 | info = self.expert_plan_grasp(step, check_scene, checked, fix_goal) 152 | 153 | if planner_cfg.vis: self.planner_scene.fast_debug_vis(collision_pt=False) 154 | 155 | for i, name in enumerate(placed_obj_names): # reset 156 | self.planner_scene.env.update_pose(name, placed_poses[i]) 157 | 158 | if check_scene: 159 | return len(self.planner_scene.traj.goal_set) >= 15 160 | 161 | if hasattr(self.planner_scene, 'planner') and len(self.planner_scene.planner.history_trajectories) >= 1: # 162 | plan = self.planner_scene.planner.history_trajectories[-1] 163 | if not hasattr(self.env, 'robot'): 164 | self.env.robot = require_robot() 165 | ef_pose = self.env.get_base_pose().dot(self.env.robot.forward_kinematics_parallel( 166 | wrap_value(plan[-1])[None], offset=False)[0][-3]) # world coordinate 167 | 168 | pos, orn = p.getBasePositionAndOrientation(self.env._objectUids[self.env.target_idx]) # to target 169 | obj_pose = list(pos) + [orn[3], orn[0], orn[1], orn[2]] 170 | self.env.cur_goal = se3_inverse(unpack_pose(obj_pose)).dot(ef_pose) 171 | 172 | 173 | success = info[-1]['terminate'] if len(info) > 1 else False 174 | failure = info[-1]['failure_terminate'] if len(info) > 1 else True 175 | self.env.planner_reach_grasps = self.get_planning_scene_target().reach_grasps 176 | self.env.planner_goal_set = self.planner_scene.traj.goal_set 177 | 178 | if not return_success: 179 | return plan, failure 180 | return plan, failure, success 181 | 182 | def set_planner_scene_file(self, scene_path, scene_file): 183 | planner_cfg.traj_init = "scene" 184 | planner_cfg.scene_path = scene_path 185 | planner_cfg.scene_file = scene_file 186 | 187 | def get_planning_scene_target(self): 188 | return self.planner_scene.env.objects[self.planner_scene.env.target_idx] 189 | 190 | def _get_init_info(self): 191 | """ 192 | Get environment information 193 | """ 194 | return [self.obj_names, self.obj_poses, self.placed_object_target_idx, 195 | np.array(self._panda.getJointStates()[0]), 196 | self.planner_scene.traj.goal_set, 197 | self.get_planning_scene_target().reach_grasps] 198 | 199 | def goal_num_full(self): 200 | """ 201 | Check if the scene has enough goals 202 | """ 203 | return not self._planner_setup or len(self.planner_scene.traj.goal_set) >= 6 204 | -------------------------------------------------------------------------------- /env/models/panda/meshes/collision/camera.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/liruiw/HCG/0059be3091658fc05df25bebb0ff33c02a8df581/env/models/panda/meshes/collision/camera.stl -------------------------------------------------------------------------------- /env/models/panda/meshes/collision/finger.obj: -------------------------------------------------------------------------------- 1 | # File produced by Open Asset Import Library (http://www.assimp.sf.net) 2 | # (assimp v3.0.1262) 3 | 4 | mtllib finger.stl.obj.mtl 5 | 6 | # 96 vertex positions 7 | v 0.01036 0.0264034 0.000154629 8 | v 0.0104486 0.0025833 0.000146801 9 | v -0.0103872 0.00253418 0.000131696 10 | v -0.0103872 0.00253418 0.000131696 11 | v -0.0104795 0.0161016 0.0189882 12 | v -0.0104013 0.0263094 0.00016651 13 | v -0.0104013 0.0263094 0.00016651 14 | v -0.0104795 0.0161016 0.0189882 15 | v -0.0103889 0.0252203 0.0191876 16 | v 0.0104486 0.0025833 0.000146801 17 | v -0.0087304 -2.35252e-05 0.0361648 18 | v -0.0103872 0.00253418 0.000131696 19 | v 0.0104005 0.0252534 0.0190366 20 | v -0.0103889 0.0252203 0.0191876 21 | v 0.00583983 0.0142743 0.0538035 22 | v -0.0103872 0.00253418 0.000131696 23 | v -0.0104013 0.0263094 0.00016651 24 | v 0.01036 0.0264034 0.000154629 25 | v 0.00861608 0.0139887 0.0513279 26 | v 0.0104948 0.0151026 0.0184356 27 | v 0.0104005 0.0252534 0.0190366 28 | v 0.0104948 0.0151026 0.0184356 29 | v 0.00869277 -0.000132643 0.0501662 30 | v 0.0104486 0.0025833 0.000146801 31 | v 0.00861608 0.0139887 0.0513279 32 | v 0.00869277 -0.000132643 0.0501662 33 | v 0.0104948 0.0151026 0.0184356 34 | v -0.00862294 -5.68019e-05 0.0509528 35 | v -0.0103872 0.00253418 0.000131696 36 | v -0.0087304 -2.35252e-05 0.0361648 37 | v 0.0104486 0.0025833 0.000146801 38 | v 0.00869277 -0.000132643 0.0501662 39 | v -0.0087304 -2.35252e-05 0.0361648 40 | v 0.00869277 -0.000132643 0.0501662 41 | v -0.00548142 -9.11208e-05 0.0537247 42 | v -0.0087304 -2.35252e-05 0.0361648 43 | v 0.0104005 0.0252534 0.0190366 44 | v 0.0104948 0.0151026 0.0184356 45 | v 0.0104486 0.0025833 0.000146801 46 | v 0.0104005 0.0252534 0.0190366 47 | v 0.0104486 0.0025833 0.000146801 48 | v 0.01036 0.0264034 0.000154629 49 | v 0.0104005 0.0252534 0.0190366 50 | v 0.01036 0.0264034 0.000154629 51 | v -0.0103889 0.0252203 0.0191876 52 | v -0.0103889 0.0252203 0.0191876 53 | v -0.00527792 0.0142931 0.053849 54 | v 0.00583983 0.0142743 0.0538035 55 | v -0.00777838 0.0142182 0.0523659 56 | v -0.00527792 0.0142931 0.053849 57 | v -0.0103889 0.0252203 0.0191876 58 | v -0.00862294 -5.68019e-05 0.0509528 59 | v -0.00548142 -9.11208e-05 0.0537247 60 | v -0.00777838 0.0142182 0.0523659 61 | v -0.0103872 0.00253418 0.000131696 62 | v -0.00862294 -5.68019e-05 0.0509528 63 | v -0.0104795 0.0161016 0.0189882 64 | v 0.0104005 0.0252534 0.0190366 65 | v 0.00583983 0.0142743 0.0538035 66 | v 0.00861608 0.0139887 0.0513279 67 | v 0.01036 0.0264034 0.000154629 68 | v -0.0104013 0.0263094 0.00016651 69 | v -0.0103889 0.0252203 0.0191876 70 | v -0.0104795 0.0161016 0.0189882 71 | v -0.00884117 0.0139176 0.0505894 72 | v -0.0103889 0.0252203 0.0191876 73 | v -0.00884117 0.0139176 0.0505894 74 | v -0.00777838 0.0142182 0.0523659 75 | v -0.0103889 0.0252203 0.0191876 76 | v 0.00583983 0.0142743 0.0538035 77 | v -0.00548142 -9.11208e-05 0.0537247 78 | v 0.00613802 -2.06026e-05 0.0535776 79 | v -0.00527792 0.0142931 0.053849 80 | v -0.00548142 -9.11208e-05 0.0537247 81 | v 0.00583983 0.0142743 0.0538035 82 | v 0.00869277 -0.000132643 0.0501662 83 | v 0.00613802 -2.06026e-05 0.0535776 84 | v -0.00548142 -9.11208e-05 0.0537247 85 | v 0.00613802 -2.06026e-05 0.0535776 86 | v 0.00869277 -0.000132643 0.0501662 87 | v 0.00861608 0.0139887 0.0513279 88 | v -0.00548142 -9.11208e-05 0.0537247 89 | v -0.00862294 -5.68019e-05 0.0509528 90 | v -0.0087304 -2.35252e-05 0.0361648 91 | v -0.00777838 0.0142182 0.0523659 92 | v -0.00548142 -9.11208e-05 0.0537247 93 | v -0.00527792 0.0142931 0.053849 94 | v -0.00862294 -5.68019e-05 0.0509528 95 | v -0.00884117 0.0139176 0.0505894 96 | v -0.0104795 0.0161016 0.0189882 97 | v 0.00613802 -2.06026e-05 0.0535776 98 | v 0.00861608 0.0139887 0.0513279 99 | v 0.00583983 0.0142743 0.0538035 100 | v -0.00777838 0.0142182 0.0523659 101 | v -0.00884117 0.0139176 0.0505894 102 | v -0.00862294 -5.68019e-05 0.0509528 103 | 104 | # 0 UV coordinates 105 | 106 | # 96 vertex normals 107 | vn 0.000724164 0.000331308 -1 108 | vn 0.000724164 0.000331308 -1 109 | vn 0.000724164 0.000331308 -1 110 | vn -0.99999 -0.000585782 -0.00447174 111 | vn -0.99999 -0.000585782 -0.00447174 112 | vn -0.99999 -0.000585782 -0.00447174 113 | vn -0.99995 0.00990875 0.00122006 114 | vn -0.99995 0.00990875 0.00122006 115 | vn -0.99995 0.00990875 0.00122006 116 | vn 0.00240304 -0.997479 -0.0709137 117 | vn 0.00240304 -0.997479 -0.0709137 118 | vn 0.00240304 -0.997479 -0.0709137 119 | vn 0.000668274 0.953556 0.301214 120 | vn 0.000668274 0.953556 0.301214 121 | vn 0.000668274 0.953556 0.301214 122 | vn -0.0005789 0.00146393 -0.999999 123 | vn -0.0005789 0.00146393 -0.999999 124 | vn -0.0005789 0.00146393 -0.999999 125 | vn 0.998344 0.00589157 0.0572229 126 | vn 0.998344 0.00589157 0.0572229 127 | vn 0.998344 0.00589157 0.0572229 128 | vn 0.998185 -0.050838 0.0322792 129 | vn 0.998185 -0.050838 0.0322792 130 | vn 0.998185 -0.050838 0.0322792 131 | vn 0.998371 0.000729252 0.0570496 132 | vn 0.998371 0.000729252 0.0570496 133 | vn 0.998371 0.000729252 0.0570496 134 | vn -0.871287 -0.490747 0.00522707 135 | vn -0.871287 -0.490747 0.00522707 136 | vn -0.871287 -0.490747 0.00522707 137 | vn 0.0362712 -0.99794 -0.0529128 138 | vn 0.0362712 -0.99794 -0.0529128 139 | vn 0.0362712 -0.99794 -0.0529128 140 | vn -0.00372285 -0.999988 -0.00316058 141 | vn -0.00372285 -0.999988 -0.00316058 142 | vn -0.00372285 -0.999988 -0.00316058 143 | vn 0.999909 0.00984192 -0.00926313 144 | vn 0.999909 0.00984192 -0.00926313 145 | vn 0.999909 0.00984192 -0.00926313 146 | vn 0.999991 0.00372285 -0.00191858 147 | vn 0.999991 0.00372285 -0.00191858 148 | vn 0.999991 0.00372285 -0.00191858 149 | vn -0.0011495 0.99815 0.0607926 150 | vn -0.0011495 0.99815 0.0607926 151 | vn -0.0011495 0.99815 0.0607926 152 | vn 0.00284562 0.953846 0.300284 153 | vn 0.00284562 0.953846 0.300284 154 | vn 0.00284562 0.953846 0.300284 155 | vn -0.218924 0.920873 0.32259 156 | vn -0.218924 0.920873 0.32259 157 | vn -0.218924 0.920873 0.32259 158 | vn -0.661425 -0.0350314 0.749193 159 | vn -0.661425 -0.0350314 0.749193 160 | vn -0.661425 -0.0350314 0.749193 161 | vn -0.998169 -0.0513123 0.0320353 162 | vn -0.998169 -0.0513123 0.0320353 163 | vn -0.998169 -0.0513123 0.0320353 164 | vn 0.377717 0.867563 0.323518 165 | vn 0.377717 0.867563 0.323518 166 | vn 0.377717 0.867563 0.323518 167 | vn -0.00448618 0.998355 0.0571685 168 | vn -0.00448618 0.998355 0.0571685 169 | vn -0.00448618 0.998355 0.0571685 170 | vn -0.998589 0.0087761 0.0523757 171 | vn -0.998589 0.0087761 0.0523757 172 | vn -0.998589 0.0087761 0.0523757 173 | vn -0.665951 0.690851 0.281485 174 | vn -0.665951 0.690851 0.281485 175 | vn -0.665951 0.690851 0.281485 176 | vn 0.0127505 -0.0155288 0.999798 177 | vn 0.0127505 -0.0155288 0.999798 178 | vn 0.0127505 -0.0155288 0.999798 179 | vn 0.00408502 -0.00870048 0.999954 180 | vn 0.00408502 -0.00870048 0.999954 181 | vn 0.00408502 -0.00870048 0.999954 182 | vn 0.006542 -0.999267 0.0377181 183 | vn 0.006542 -0.999267 0.0377181 184 | vn 0.006542 -0.999267 0.0377181 185 | vn 0.798906 -0.0450007 0.59977 186 | vn 0.798906 -0.0450007 0.59977 187 | vn 0.798906 -0.0450007 0.59977 188 | vn -0.00899609 -0.999957 -0.00218479 189 | vn -0.00899609 -0.999957 -0.00218479 190 | vn -0.00899609 -0.999957 -0.00218479 191 | vn -0.510144 -0.000216662 0.860089 192 | vn -0.510144 -0.000216662 0.860089 193 | vn -0.510144 -0.000216662 0.860089 194 | vn -0.998608 -0.0142745 0.0507836 195 | vn -0.998608 -0.0142745 0.0507836 196 | vn -0.998608 -0.0142745 0.0507836 197 | vn 0.665648 0.00209596 0.746263 198 | vn 0.665648 0.00209596 0.746263 199 | vn 0.665648 0.00209596 0.746263 200 | vn -0.858159 -4.90786e-05 0.513384 201 | vn -0.858159 -4.90786e-05 0.513384 202 | vn -0.858159 -4.90786e-05 0.513384 203 | 204 | # Mesh '' with 32 faces 205 | g 206 | usemtl DefaultMaterial 207 | f 1//1 2//2 3//3 208 | f 4//4 5//5 6//6 209 | f 7//7 8//8 9//9 210 | f 10//10 11//11 12//12 211 | f 13//13 14//14 15//15 212 | f 16//16 17//17 18//18 213 | f 19//19 20//20 21//21 214 | f 22//22 23//23 24//24 215 | f 25//25 26//26 27//27 216 | f 28//28 29//29 30//30 217 | f 31//31 32//32 33//33 218 | f 34//34 35//35 36//36 219 | f 37//37 38//38 39//39 220 | f 40//40 41//41 42//42 221 | f 43//43 44//44 45//45 222 | f 46//46 47//47 48//48 223 | f 49//49 50//50 51//51 224 | f 52//52 53//53 54//54 225 | f 55//55 56//56 57//57 226 | f 58//58 59//59 60//60 227 | f 61//61 62//62 63//63 228 | f 64//64 65//65 66//66 229 | f 67//67 68//68 69//69 230 | f 70//70 71//71 72//72 231 | f 73//73 74//74 75//75 232 | f 76//76 77//77 78//78 233 | f 79//79 80//80 81//81 234 | f 82//82 83//83 84//84 235 | f 85//85 86//86 87//87 236 | f 88//88 89//89 90//90 237 | f 91//91 92//92 93//93 238 | f 94//94 95//95 96//96 239 | 240 | -------------------------------------------------------------------------------- /env/models/panda/meshes/collision/hand.obj: -------------------------------------------------------------------------------- 1 | # File produced by Open Asset Import Library (http://www.assimp.sf.net) 2 | # (assimp v3.1.187496374) 3 | 4 | mtllib hand.obj.mtl 5 | 6 | # 102 vertex positions 7 | v 0.0167349 0.0901289 0.0644319 8 | v 0.0171343 -0.0892712 0.0636144 9 | v 0.0315898 -0.000558518 0.00621423 10 | v -0.0170537 -0.0955248 0.0221212 11 | v -0.0175104 -0.0979076 0.0516662 12 | v -0.0315975 -0.0019865 0.00115293 13 | v -0.025981 0.0757697 -0.00508223 14 | v -0.0316359 -0.000578733 0.00597307 15 | v 0.0170332 -0.0968023 0.030322 16 | v 0.0316158 -0.00186086 0.00117971 17 | v 0.0175851 -0.0974071 0.0519846 18 | v 0.0185559 0.0946019 0.0566173 19 | v -0.0168042 -0.0919061 0.00629548 20 | v -0.0168122 -0.0875329 -0.00120645 21 | v 0.0162301 -0.0995375 0.0515841 22 | v -4.10844e-05 -0.10399 0.00569088 23 | v -1.72733e-05 -0.103743 -0.0091993 24 | v -0.00361677 -0.101158 -0.0126103 25 | v -0.00342956 -0.102598 -0.0089643 26 | v -0.0252704 0.0820651 0.00519704 27 | v -0.0280109 0.0417843 -0.0063964 28 | v -0.000112569 0.0928085 0.0659622 29 | v -0.0125667 0.0937125 0.0651685 30 | v -0.014422 0.0912765 0.0656897 31 | v -0.0172547 -0.0907003 0.063082 32 | v -0.0154717 -0.0907063 0.0651901 33 | v -0.0167672 0.0902522 0.0643322 34 | v 0.0169555 -0.0916029 0.00556283 35 | v 0.0176072 -0.0938476 0.0584485 36 | v 0.0125057 -0.100188 0.0568941 37 | v 0.0155509 0.1002 0.0546726 38 | v 0.0192688 0.0984936 0.0449417 39 | v 0.0162184 0.100285 0.0458698 40 | v 0.00191744 -0.0455199 -0.0256594 41 | v 0.0280151 0.0420423 -0.0063496 42 | v 0.00381034 -0.0829662 -0.0244493 43 | v 0.0206709 0.0936918 0.0433083 44 | v 0.0254479 0.0811805 0.0035006 45 | v -0.00363682 -0.0835859 -0.0246063 46 | v -0.0165327 -0.0822363 -0.00511811 47 | v -0.0140171 -0.100091 0.0560269 48 | v -0.0160131 -0.0996286 0.0493014 49 | v -0.00321509 -0.087766 -0.0238844 50 | v -0.0129879 -0.0924958 0.0657119 51 | v -0.000505639 -0.0929453 0.0659531 52 | v 0.0135396 0.0926721 0.0656879 53 | v 0.0134248 -0.091927 0.0657313 54 | v -0.0192292 0.0972606 0.0505884 55 | v 0.0165582 -0.0851322 -0.00387274 56 | v 0.00349477 -0.102716 -0.00802934 57 | v 0.0238692 0.0904014 0.00436824 58 | v 0.0227078 0.0937356 0.01434 59 | v 0.0260343 0.0752976 -0.00503085 60 | v 0.0248167 0.0743775 -0.0071557 61 | v -0.00150015 -0.0458698 -0.0259248 62 | v -0.000833771 0.08026 -0.00732172 63 | v 0.0229591 0.0779014 -0.00739904 64 | v -0.0227611 0.0801744 -0.00690954 65 | v 3.18097e-05 -0.0994514 -0.0171612 66 | v -0.0166321 -0.098554 0.0547339 67 | v 0.00360213 -0.101195 -0.0126276 68 | v -0.00161673 0.0974711 0.0623073 69 | v 0.0157184 -0.0917796 0.0649125 70 | v -0.0184487 0.0920155 0.0583932 71 | v -0.000626994 -0.0879747 -0.0249616 72 | v 0.003076 -0.0879729 -0.0239787 73 | v 0.019802 0.0970111 0.020914 74 | v 0.00994155 0.100035 0.0583408 75 | v 0.0171295 0.0990672 0.0543052 76 | v -0.000314131 0.0946153 0.00757466 77 | v -1.59471e-05 0.0984961 0.0270818 78 | v -0.000337323 0.0865616 -0.00416328 79 | v 0.0223143 0.08825 -0.0019332 80 | v 0.022377 0.0833552 -0.00552611 81 | v -0.021325 0.0909269 0.000979802 82 | v -0.0227328 0.0854345 -0.00412033 83 | v -0.0244953 0.0858923 -0.00183273 84 | v 0.00184279 -0.0840023 -0.0255249 85 | v -0.0246502 0.076668 -0.00693213 86 | v 0.0131387 -0.0940629 0.0646832 87 | v -0.00932387 -0.0999826 0.0584892 88 | v -0.0190668 0.0976971 0.0246275 89 | v -0.00449439 0.100426 0.0581191 90 | v -0.0146231 0.100168 0.0557349 91 | v 0.0158049 -0.0994163 0.0549703 92 | v 0.0213426 0.0943331 0.00894201 93 | v -0.000173406 0.090995 0.000576784 94 | v 0.0214829 0.0917947 0.00268413 95 | v 0.0245152 0.0817026 -0.00505142 96 | v 0.0247294 0.0857401 -0.00123169 97 | v -0.0206576 0.0946679 0.00953798 98 | v 0.00119059 -0.100301 0.0586231 99 | v 0.000469602 -0.0966559 0.0630784 100 | v 0.0153969 0.0929056 0.0645563 101 | v -0.000867829 -0.0825485 -0.0258702 102 | v -0.0235558 0.0911663 0.00460792 103 | v -0.014617 -0.0934417 0.0645 104 | v -0.0230261 0.0928969 0.00987686 105 | v -0.0218221 0.0954417 0.0217564 106 | v -0.0181926 0.0995179 0.0438142 107 | v -0.0160455 0.0932093 0.0636169 108 | v -0.0169948 0.0993629 0.0541148 109 | 110 | # 0 UV coordinates 111 | 112 | # 200 vertex normals 113 | vn 0.96934 0.00103808 0.245721 114 | vn -0.986759 -0.159734 -0.0281361 115 | vn -0.997162 0.0697563 -0.0283202 116 | vn 0.986571 -0.160626 -0.0296212 117 | vn 0.99137 -0.125585 0.0376046 118 | vn 0.972565 0.0101093 0.232413 119 | vn -0.981257 -0.166939 -0.096261 120 | vn -0.991092 -0.129756 0.0299978 121 | vn -0.984842 -0.16507 -0.0532708 122 | vn 0.303856 -0.952595 -0.0153173 123 | vn -0.745916 -0.631674 -0.211181 124 | vn -0.996795 0.0769815 0.0217643 125 | vn -0.992516 0.0633187 -0.104418 126 | vn -0.044276 0.241169 0.969473 127 | vn -0.763533 -0.00240486 0.645764 128 | vn 0.98594 -0.162885 -0.0372967 129 | vn 0.986849 -0.0650596 0.147976 130 | vn 0.991549 -0.115042 0.0599665 131 | vn 0.203398 -0.97883 0.0228453 132 | vn 0.516573 0.854926 0.0474658 133 | vn 0.587131 0.00352045 -0.809484 134 | vn 0.99247 0.0777169 0.094674 135 | vn -0.832137 -0.113798 -0.542768 136 | vn -0.990867 -0.10289 0.0871544 137 | vn -0.24667 -0.969077 0.00656585 138 | vn -0.796752 -0.375025 -0.473859 139 | vn -0.0190316 -2.76701e-05 0.999819 140 | vn -0.0193168 -7.80837e-06 0.999813 141 | vn 0.0200855 -9.11851e-05 0.999798 142 | vn 0.0159035 0.000225346 0.999874 143 | vn -0.991348 0.0773534 0.106043 144 | vn 0.846324 -0.531423 -0.0364002 145 | vn 0.838336 -0.119317 -0.531936 146 | vn 0.713391 -0.685355 -0.146159 147 | vn 0.752915 -0.0507794 -0.656156 148 | vn 0.962439 0.234445 0.136915 149 | vn 0.98673 0.0540529 0.153111 150 | vn 0.984464 0.162241 0.0671482 151 | vn 0.963684 0.258201 0.0681518 152 | vn 0.988283 0.13579 0.0696969 153 | vn 0.992401 0.0632947 -0.105521 154 | vn 0.997132 0.0735636 0.0178061 155 | vn 0.997125 0.0744552 -0.0141125 156 | vn 0.525671 0.0308018 -0.85013 157 | vn 0.0112429 0.145846 -0.989244 158 | vn -0.0191625 0.145985 -0.989101 159 | vn 0.295801 0.090268 -0.950975 160 | vn -0.753521 -0.0512984 -0.655419 161 | vn -0.557064 -0.525003 -0.643468 162 | vn -0.826525 -0.175737 -0.534765 163 | vn -0.815256 -0.315571 -0.485564 164 | vn -0.947514 -0.156066 -0.279037 165 | vn -0.93384 -0.292644 0.205677 166 | vn 0.168928 -0.868066 -0.466825 167 | vn -0.713161 -0.698683 0.0569532 168 | vn -0.304509 -0.95237 -0.0162868 169 | vn 0.0219418 0.621147 0.783387 170 | vn -0.00551132 0.61581 0.787876 171 | vn 0.36612 0.000413018 0.930568 172 | vn 0.336199 1.25396e-05 0.941791 173 | vn -0.969753 -7.34049e-05 0.244089 174 | vn -0.962126 0.000708185 0.272604 175 | vn -0.988229 0.0614231 0.14011 176 | vn -0.360462 -0.538401 -0.761703 177 | vn 0.829279 -0.171621 -0.53183 178 | vn 0.677571 -0.00184298 0.735455 179 | vn 0.496348 0.867083 -0.0424839 180 | vn 0.0755333 0.722595 0.687133 181 | vn 0.904977 0.383975 0.183246 182 | vn 0.0127058 0.980664 -0.195286 183 | vn 0.851811 0.0713165 -0.518972 184 | vn 0.0351954 0.591653 -0.805424 185 | vn -0.538898 0.643215 -0.543933 186 | vn 0.0777842 -0.00363624 -0.996964 187 | vn -0.19873 0.11334 -0.973479 188 | vn -0.793118 -0.605993 -0.0611343 189 | vn 0.283571 -0.958905 -0.00945907 190 | vn -0.178475 -0.865689 -0.467685 191 | vn -0.987868 0.153186 -0.0255232 192 | vn -0.206645 -0.00149425 0.978415 193 | vn -0.501132 0.000514844 0.865371 194 | vn 0.0467749 -0.445101 0.894258 195 | vn -0.0152497 -0.997215 0.0730019 196 | vn -0.0160242 0.980702 -0.194854 197 | vn -0.10472 0.777309 0.620342 198 | vn -0.0162484 0.999109 -0.0389505 199 | vn 0.812943 -0.46373 -0.352249 200 | vn 0.734943 -0.648754 -0.197427 201 | vn 0.355505 -0.889833 -0.28603 202 | vn 0.77975 -0.493229 0.385637 203 | vn 0.264343 -0.962054 0.0676396 204 | vn 0.542077 -0.664141 0.514848 205 | vn 0.470493 -0.711825 0.52148 206 | vn 0.584351 -0.807368 -0.0817933 207 | vn 0.826668 -0.548978 0.123462 208 | vn 0.873315 -0.427986 0.232701 209 | vn 0.0134981 0.821385 0.570215 210 | vn 0.481279 -0.00399798 -0.876558 211 | vn 0.0262961 0.976262 -0.21499 212 | vn 0.035393 0.991525 -0.125004 213 | vn -0.0252692 0.468427 -0.883141 214 | vn 0.029533 0.795427 -0.605329 215 | vn 0.0095104 0.447459 -0.894254 216 | vn 0.0293922 0.327461 -0.944407 217 | vn 0.0628955 0.135567 -0.98877 218 | vn -0.512379 0.0361849 -0.857997 219 | vn -0.82575 0.0709576 -0.559555 220 | vn 0.671124 0.490386 -0.555981 221 | vn -0.583104 0.00463288 -0.812385 222 | vn -0.0162994 0.980639 -0.195146 223 | vn -0.157039 0.79873 0.580834 224 | vn 0.973711 -0.16665 -0.155286 225 | vn 0.0757908 -0.76565 0.638776 226 | vn -0.0242742 0.999674 0.00785851 227 | vn 0.0265568 0.999088 0.0334344 228 | vn 0.763663 0.633795 -0.122975 229 | vn 0.59401 0.799769 0.086723 230 | vn 0.277533 0.743174 0.608825 231 | vn 0.344643 0.752738 0.560898 232 | vn 0.784743 0.463667 0.411327 233 | vn -0.0358679 0.684935 -0.72772 234 | vn 0.932801 0.220325 -0.285201 235 | vn 0.624229 0.728724 -0.281602 236 | vn -0.0116186 0.888023 -0.459652 237 | vn 0.0118944 0.888215 -0.459274 238 | vn 0.12674 0.000708284 -0.991936 239 | vn -0.412606 -0.00846907 -0.91087 240 | vn -0.798626 -0.520306 -0.302452 241 | vn -0.541514 -0.834316 0.103337 242 | vn -0.319295 -0.94743 -0.0206758 243 | vn -0.723656 -0.67023 -0.164661 244 | vn 0.024012 -0.608318 0.79333 245 | vn -0.0315557 -0.776033 0.629902 246 | vn 0.0205919 -0.997403 0.0690183 247 | vn -0.0311286 -0.997047 0.0701972 248 | vn -0.982775 0.184638 -0.00783846 249 | vn 0.216124 -0.540324 -0.813229 250 | vn 0.246085 -0.284389 -0.926588 251 | vn 0.556314 -0.528433 -0.641306 252 | vn 0.792539 -0.37877 -0.477928 253 | vn 0.880372 -0.312129 0.3571 254 | vn 0.325605 -0.451322 0.830837 255 | vn 0.101105 -0.786037 0.609856 256 | vn 0.53596 -0.156588 -0.829594 257 | vn 0.580736 0.650676 0.489251 258 | vn 0.491562 0.198884 0.847828 259 | vn 0.828341 0.380744 0.410957 260 | vn 0.035746 0.926351 -0.374962 261 | vn 0.656777 0.749488 -0.083134 262 | vn 0.720531 0.692906 -0.0267625 263 | vn 0.572813 0.248018 -0.781263 264 | vn 0.990951 0.132224 -0.0230681 265 | vn 0.985773 0.167665 0.011866 266 | vn -0.0153618 0.730516 -0.682723 267 | vn 0.0128537 0.730055 -0.683268 268 | vn -0.0185446 0.449169 -0.893255 269 | vn -0.619205 -0.779582 -0.0940023 270 | vn -0.320427 -0.88655 -0.333701 271 | vn -0.0989435 -0.715546 0.691523 272 | vn -0.0841099 -0.712852 0.696252 273 | vn -0.60803 -0.644894 0.463045 274 | vn -0.423761 -0.343804 0.837989 275 | vn -0.605208 0.788757 -0.107633 276 | vn -0.0174541 0.996656 -0.0798282 277 | vn -0.0193589 0.998627 -0.0486783 278 | vn -0.634294 0.640925 -0.432304 279 | vn 0.0379495 -0.163387 -0.985832 280 | vn 0.538782 0.502971 -0.675821 281 | vn 0.813644 0.191216 -0.549017 282 | vn 0.652479 0.653287 -0.384041 283 | vn 0.717815 0.58535 -0.37697 284 | vn -0.286085 -0.763532 0.578942 285 | vn -0.0374327 -0.618147 0.785171 286 | vn -0.709079 -0.37258 0.598657 287 | vn -0.878489 0.351219 0.323886 288 | vn -0.890816 0.299973 0.341268 289 | vn -0.868133 0.372523 0.327981 290 | vn -0.605163 0.772391 -0.192849 291 | vn -0.984555 0.156402 0.0786764 292 | vn -0.982298 0.176886 0.06166 293 | vn -0.981595 0.187328 0.0371542 294 | vn -0.44589 0.832394 -0.329094 295 | vn -0.363723 0.729882 0.578773 296 | vn -0.575721 0.322294 0.751446 297 | vn 0.442358 0.333164 -0.832659 298 | vn -0.70625 0.351573 -0.614497 299 | vn -0.654528 0.356931 -0.666478 300 | vn -0.838844 0.274101 -0.47033 301 | vn -0.354702 0.933343 0.0552966 302 | vn -0.561743 0.820557 -0.105505 303 | vn -0.851741 0.52214 0.0436597 304 | vn -0.759618 0.642942 0.0980105 305 | vn -0.0360333 0.916716 -0.39791 306 | vn -0.560808 0.66826 0.488798 307 | vn -0.414234 0.480837 0.772791 308 | vn -0.390321 -0.168871 -0.905059 309 | vn -0.752648 -0.506645 0.420514 310 | vn -0.029743 0.995213 -0.0930902 311 | vn -0.346496 -0.193469 -0.917884 312 | vn -0.602128 0.796943 -0.0481963 313 | 314 | # Mesh '' with 200 faces 315 | g 316 | usemtl DefaultMaterial 317 | f 1//1 2//1 3//1 318 | f 4//2 5//2 6//2 319 | f 7//3 6//3 8//3 320 | f 9//4 10//4 11//4 321 | f 10//5 3//5 11//5 322 | f 12//6 1//6 3//6 323 | f 13//7 6//7 14//7 324 | f 8//8 6//8 5//8 325 | f 4//9 6//9 13//9 326 | f 15//10 16//10 17//10 327 | f 18//11 19//11 13//11 328 | f 20//12 7//12 8//12 329 | f 21//13 6//13 7//13 330 | f 22//14 23//14 24//14 331 | f 25//15 26//15 27//15 332 | f 28//16 10//16 9//16 333 | f 2//17 29//17 3//17 334 | f 29//18 11//18 3//18 335 | f 30//19 16//19 15//19 336 | f 31//20 32//20 33//20 337 | f 34//21 35//21 36//21 338 | f 37//22 3//22 38//22 339 | f 6//23 39//23 40//23 340 | f 25//24 8//24 5//24 341 | f 41//25 42//25 16//25 342 | f 18//26 14//26 43//26 343 | f 22//27 24//27 44//27 344 | f 44//28 45//28 22//28 345 | f 46//29 22//29 45//29 346 | f 45//30 47//30 46//30 347 | f 20//31 8//31 48//31 348 | f 9//32 11//32 15//32 349 | f 36//33 10//33 49//33 350 | f 50//34 28//34 9//34 351 | f 10//35 36//35 35//35 352 | f 12//36 37//36 32//36 353 | f 37//37 12//37 3//37 354 | f 37//38 38//38 51//38 355 | f 37//39 52//39 32//39 356 | f 52//40 37//40 51//40 357 | f 10//41 35//41 53//41 358 | f 10//42 53//42 38//42 359 | f 10//43 38//43 3//43 360 | f 54//44 35//44 34//44 361 | f 55//45 56//45 57//45 362 | f 56//46 55//46 58//46 363 | f 57//47 54//47 34//47 364 | f 39//48 6//48 21//48 365 | f 59//49 18//49 43//49 366 | f 40//50 39//50 43//50 367 | f 14//51 40//51 43//51 368 | f 14//52 6//52 40//52 369 | f 60//53 25//53 5//53 370 | f 59//54 61//54 17//54 371 | f 42//55 60//55 5//55 372 | f 16//56 42//56 17//56 373 | f 62//57 22//57 46//57 374 | f 22//58 62//58 23//58 375 | f 1//59 46//59 63//59 376 | f 47//60 63//60 46//60 377 | f 25//61 64//61 8//61 378 | f 64//62 25//62 27//62 379 | f 64//63 48//63 8//63 380 | f 65//64 59//64 43//64 381 | f 66//65 36//65 49//65 382 | f 63//66 2//66 1//66 383 | f 33//67 32//67 67//67 384 | f 68//68 62//68 46//68 385 | f 32//69 69//69 12//69 386 | f 70//70 71//70 67//70 387 | f 53//71 35//71 54//71 388 | f 72//72 73//72 74//72 389 | f 75//73 76//73 77//73 390 | f 78//74 55//74 34//74 391 | f 55//75 79//75 58//75 392 | f 4//76 42//76 5//76 393 | f 50//77 15//77 17//77 394 | f 59//78 17//78 18//78 395 | f 7//79 20//79 77//79 396 | f 24//80 26//80 44//80 397 | f 27//81 26//81 24//81 398 | f 45//82 80//82 47//82 399 | f 41//83 16//83 81//83 400 | f 82//84 71//84 70//84 401 | f 62//85 83//85 23//85 402 | f 33//86 84//86 83//86 403 | f 49//87 28//87 61//87 404 | f 61//88 28//88 50//88 405 | f 61//89 50//89 17//89 406 | f 63//90 85//90 29//90 407 | f 85//91 30//91 15//91 408 | f 80//92 85//92 63//92 409 | f 85//93 80//93 30//93 410 | f 9//94 15//94 50//94 411 | f 85//95 15//95 11//95 412 | f 11//96 29//96 85//96 413 | f 83//97 62//97 68//97 414 | f 34//98 36//98 78//98 415 | f 70//99 67//99 86//99 416 | f 71//100 33//100 67//100 417 | f 58//101 76//101 72//101 418 | f 73//102 87//102 88//102 419 | f 72//103 74//103 56//103 420 | f 56//104 74//104 57//104 421 | f 34//105 55//105 57//105 422 | f 21//106 79//106 55//106 423 | f 21//107 7//107 79//107 424 | f 89//108 73//108 90//108 425 | f 39//109 21//109 55//109 426 | f 91//110 82//110 70//110 427 | f 84//111 23//111 83//111 428 | f 28//112 49//112 10//112 429 | f 92//113 80//113 93//113 430 | f 31//114 33//114 68//114 431 | f 68//115 33//115 83//115 432 | f 51//116 86//116 52//116 433 | f 69//117 32//117 31//117 434 | f 46//118 94//118 68//118 435 | f 94//119 31//119 68//119 436 | f 94//120 12//120 69//120 437 | f 76//121 75//121 72//121 438 | f 90//122 53//122 89//122 439 | f 86//123 51//123 88//123 440 | f 75//124 70//124 87//124 441 | f 87//125 70//125 88//125 442 | f 95//126 55//126 78//126 443 | f 39//127 55//127 95//127 444 | f 14//128 18//128 13//128 445 | f 42//129 41//129 60//129 446 | f 19//130 17//130 42//130 447 | f 19//131 4//131 13//131 448 | f 45//132 93//132 80//132 449 | f 92//133 93//133 81//133 450 | f 16//134 30//134 92//134 451 | f 92//135 81//135 16//135 452 | f 96//136 77//136 20//136 453 | f 66//137 59//137 65//137 454 | f 65//138 78//138 66//138 455 | f 59//139 66//139 61//139 456 | f 49//140 61//140 66//140 457 | f 2//141 63//141 29//141 458 | f 80//142 63//142 47//142 459 | f 80//143 92//143 30//143 460 | f 78//144 36//144 66//144 461 | f 94//145 69//145 31//145 462 | f 94//146 46//146 1//146 463 | f 12//147 94//147 1//147 464 | f 70//148 86//148 88//148 465 | f 67//149 52//149 86//149 466 | f 52//150 67//150 32//150 467 | f 57//151 89//151 54//151 468 | f 53//152 90//152 38//152 469 | f 90//153 51//153 38//153 470 | f 75//154 87//154 72//154 471 | f 72//155 87//155 73//155 472 | f 56//156 58//156 72//156 473 | f 42//157 4//157 19//157 474 | f 19//158 18//158 17//158 475 | f 97//159 81//159 44//159 476 | f 81//160 93//160 44//160 477 | f 41//161 97//161 60//161 478 | f 26//162 97//162 44//162 479 | f 98//163 99//163 91//163 480 | f 100//164 33//164 71//164 481 | f 84//165 33//165 100//165 482 | f 96//166 75//166 77//166 483 | f 95//167 78//167 65//167 484 | f 73//168 89//168 74//168 485 | f 53//169 54//169 89//169 486 | f 73//170 88//170 51//170 487 | f 90//171 73//171 51//171 488 | f 41//172 81//172 97//172 489 | f 44//173 93//173 45//173 490 | f 25//174 97//174 26//174 491 | f 101//175 48//175 64//175 492 | f 64//176 27//176 101//176 493 | f 102//177 48//177 101//177 494 | f 96//178 98//178 91//178 495 | f 48//179 99//179 20//179 496 | f 99//180 98//180 20//180 497 | f 98//181 96//181 20//181 498 | f 96//182 91//182 75//182 499 | f 84//183 101//183 23//183 500 | f 27//184 24//184 101//184 501 | f 89//185 57//185 74//185 502 | f 77//186 76//186 79//186 503 | f 79//187 76//187 58//187 504 | f 79//188 7//188 77//188 505 | f 102//189 84//189 100//189 506 | f 91//190 99//190 82//190 507 | f 48//191 100//191 99//191 508 | f 100//192 48//192 102//192 509 | f 75//193 91//193 70//193 510 | f 84//194 102//194 101//194 511 | f 24//195 23//195 101//195 512 | f 43//196 95//196 65//196 513 | f 25//197 60//197 97//197 514 | f 82//198 100//198 71//198 515 | f 43//199 39//199 95//199 516 | f 100//200 82//200 99//200 517 | 518 | -------------------------------------------------------------------------------- /env/models/panda/meshes/collision/link0.obj: -------------------------------------------------------------------------------- 1 | # File produced by Open Asset Import Library (http://www.assimp.sf.net) 2 | # (assimp v3.1.187496374) 3 | 4 | mtllib link0.obj.mtl 5 | 6 | # 102 vertex positions 7 | v -0.00122674 -0.0946137 -3.24928e-05 8 | v 0.0710809 0.00844602 0.00133993 9 | v 0.071567 -0.00127396 0.0013494 10 | v -0.00051723 0.0946704 -6.42576e-06 11 | v 0.0706646 -0.0115626 0.00133336 12 | v -0.135131 -0.0658707 0.00751985 13 | v -0.136305 -0.0659457 3.26395e-07 14 | v -0.131625 -0.0687992 -5.27309e-07 15 | v 0.0543353 -0.0469489 0.00104792 16 | v 0.0619449 -0.0359785 0.00118116 17 | v 0.0673593 -0.0245271 0.00127621 18 | v 0.0526517 -0.0163561 0.14 19 | v 0.0548547 0.00447165 0.14 20 | v -0.0549666 0.00345713 0.14 21 | v 0.0391015 0.0387489 0.14 22 | v 0.00777804 -0.0910717 0.00846191 23 | v 0.0395665 -0.0384696 0.140003 24 | v -0.029337 -0.0466451 0.140001 25 | v 0.000349224 -0.0550819 0.14 26 | v 0.0686186 0.0208178 0.00129774 27 | v 0.0635871 0.0329931 0.00120988 28 | v 0.0525783 0.048771 0.00101755 29 | v 0.05859 0.0412413 0.00112268 30 | v 0.0509475 0.0207886 0.14 31 | v -0.0218528 0.0506849 0.14 32 | v -0.0317059 0.0450099 0.14 33 | v 0.0317161 0.0450507 0.14 34 | v -0.0400853 0.0378337 0.14 35 | v -0.0477037 0.0275963 0.14 36 | v 0.00485769 0.0549582 0.14 37 | v 0.0474762 -0.0279259 0.14 38 | v -0.00886536 0.0543586 0.140002 39 | v 0.0276805 -0.0476799 0.14 40 | v -0.054766 -0.0054659 0.14 41 | v -0.0528838 -0.015585 0.14 42 | v -0.122615 0.00563594 0.0611487 43 | v -0.122711 -0.00614431 0.060957 44 | v 0.0143701 -0.0532489 0.139999 45 | v -0.071675 -0.0756205 0.0575204 46 | v -0.111235 -0.0474773 0.0590551 47 | v -0.137824 -0.0624363 0.00755269 48 | v -0.133699 0.0669321 0.00741502 49 | v -0.13583 0.0663151 8.33834e-07 50 | v -0.136293 0.0646171 0.00756638 51 | v -0.131612 0.0688472 7.4968e-07 52 | v 0.0031716 0.0933497 0.00863144 53 | v 0.00408648 0.0939818 0.000170711 54 | v 0.0462474 0.0300326 0.14 55 | v 0.0548768 -0.00456585 0.14 56 | v -0.129159 0.0686836 0.00744486 57 | v -0.0528609 0.0157105 0.14 58 | v 0.0200202 0.0514462 0.139999 59 | v 0.00648893 0.0735192 0.08108 60 | v 0.00826788 -0.0918247 0.00024289 61 | v -0.0464371 -0.0299037 0.14 62 | v -0.140043 0.0607227 -1.6486e-07 63 | v 0.00769447 0.0922715 0.000234291 64 | v -0.00159938 0.0936322 0.00849181 65 | v 0.00734398 0.0914466 0.00831775 66 | v -0.148808 -0.0420547 -7.21858e-08 67 | v -0.149244 -0.0371861 0.00781712 68 | v -0.152578 -0.0248176 1.68373e-07 69 | v 0.00907648 0.0737151 0.0784549 70 | v -0.121673 -0.0147367 0.0614178 71 | v 0.00678171 -0.0735316 0.0809757 72 | v -0.130951 -0.0681401 0.00766805 73 | v -0.0189172 -0.051755 0.14 74 | v -0.00908423 -0.0542971 0.14 75 | v -0.0658985 -0.0765477 0.0589793 76 | v -0.0376956 -0.0401377 0.14 77 | v -0.143472 -0.0548617 -4.38612e-08 78 | v -0.145452 -0.0485961 0.00768247 79 | v -0.1203 -0.0235449 0.0615099 80 | v -0.118609 -0.03105 0.06134 81 | v -0.114761 -0.0419663 0.0601784 82 | v -0.154079 -0.00554168 -3.75503e-07 83 | v -0.152725 -0.0137992 0.00819143 84 | v -0.153166 -0.00203576 0.00835078 85 | v -0.142117 0.0555469 0.00762839 86 | v 0.0535544 0.0128185 0.14 87 | v -0.109983 0.0486112 0.0588796 88 | v 0.00366946 -0.0932492 0.00847566 89 | v 0.00349566 -0.0942157 0.000177738 90 | v 0.00906588 -0.073708 0.0785962 91 | v -0.117004 -0.0366378 0.0608364 92 | v -0.151587 -0.0248333 0.00804774 93 | v -0.15241 0.0258275 -1.09161e-08 94 | v -0.153841 0.0105757 0 95 | v -0.15202 0.0217804 0.00810954 96 | v -0.121149 0.0181378 0.0615703 97 | v -0.113238 0.0447149 0.0596848 98 | v -0.000866581 -0.0937369 0.00817823 99 | v -0.141842 -0.0559592 0.00761703 100 | v -0.149634 0.0355816 0.00786878 101 | v -0.14929 0.0402499 3.01802e-07 102 | v -0.152949 0.0100689 0.00821985 103 | v -0.145377 0.0508924 -1.27878e-07 104 | v -0.11571 0.0395945 0.0607111 105 | v -0.0231112 -0.0759725 0.0689683 106 | v -0.117233 0.035881 0.060957 107 | v -0.146172 0.0467529 0.00770437 108 | v -0.119414 0.0287903 0.0611397 109 | 110 | # 0 UV coordinates 111 | 112 | # 200 vertex normals 113 | vn 0.0190098 -2.30831e-05 -0.999819 114 | vn 0.0188816 6.69136e-05 -0.999822 115 | vn 0.0191344 -0.000120273 -0.999817 116 | vn -0.518479 -0.850405 0.0893954 117 | vn 0.0222843 -0.00331509 -0.999746 118 | vn 0.0205047 -0.00139704 -0.999789 119 | vn 7.25974e-07 -5.81537e-06 1 120 | vn 9.85433e-07 -5.68784e-06 1 121 | vn 0.991829 0.0497189 0.11749 122 | vn 0.692904 -0.711464 0.11706 123 | vn 4.45103e-05 3.61862e-05 1 124 | vn 0.0193277 0.000437566 -0.999813 125 | vn 0.0199677 0.0010368 -0.9998 126 | vn 0.0232843 0.0046319 -0.999718 127 | vn 0.0210964 0.0022106 -0.999775 128 | vn 0.0197075 -0.000616531 -0.999806 129 | vn 6.42989e-05 2.18738e-05 1 130 | vn 0.91755 0.380023 0.116978 131 | vn 3.35708e-05 -1.87847e-05 1 132 | vn -7.63878e-06 1.53928e-05 1 133 | vn -8.50133e-06 1.24836e-05 1 134 | vn 8.62742e-06 2.71648e-06 1 135 | vn 4.31916e-06 1.10982e-05 1 136 | vn 0.000248513 -0.00151077 0.999999 137 | vn -0.000100683 -0.000160507 1 138 | vn 0.000441559 2.26903e-05 1 139 | vn -0.759017 -0.00436697 0.651056 140 | vn -0.758153 -0.0170388 0.651854 141 | vn -4.08147e-05 -6.51499e-05 1 142 | vn 3.63574e-05 2.96612e-05 1 143 | vn -0.488489 -0.714003 0.501575 144 | vn -0.779928 -0.612675 0.127831 145 | vn -0.6563 0.74377 0.126791 146 | vn -0.51319 0.854867 0.0764032 147 | vn 0.143998 0.98555 0.0891981 148 | vn 6.6605e-05 0.00021454 1 149 | vn 0.794096 -0.595678 0.120749 150 | vn 0.815277 -0.566949 0.117862 151 | vn 0.897326 -0.425249 0.118194 152 | vn 0.906239 -0.40539 0.119958 153 | vn 0.848855 0.515505 0.117047 154 | vn 0.992823 0.00242477 0.11957 155 | vn 0.000137166 -1.78568e-05 1 156 | vn -0.256878 0.90791 0.331229 157 | vn -4.88056e-05 2.05051e-05 1 158 | vn 1.50367e-05 -7.30032e-06 1 159 | vn -3.22116e-05 6.85885e-05 1 160 | vn -0.0415945 0.953296 0.29916 161 | vn 0.0339572 -0.0169327 -0.99928 162 | vn -9.01903e-05 -3.23136e-05 1 163 | vn -0.795774 0.599474 0.0858742 164 | vn 0.423964 0.898605 0.112979 165 | vn 0.047941 0.0640793 -0.996793 166 | vn 0.0548988 0.990257 0.12797 167 | vn 0.0494285 0.963916 0.261577 168 | vn 0.0344071 0.0175091 -0.999255 169 | vn 0.691328 0.715168 0.102958 170 | vn 0.691451 0.712649 0.118438 171 | vn -0.973898 -0.213048 0.0783147 172 | vn 7.42399e-05 3.32698e-05 1 173 | vn 0.884937 0.449942 0.120157 174 | vn 0.989315 -0.0869511 0.117026 175 | vn 0.962281 -0.245852 0.116503 176 | vn -0.192564 0.977601 0.0849446 177 | vn -0.191831 0.976828 0.0949088 178 | vn -0.123193 0.952732 0.277713 179 | vn -0.441485 0.766549 0.466362 180 | vn 0.467327 0.85467 0.22615 181 | vn 0.692847 -0.713157 0.106629 182 | vn -0.756883 -0.0565346 0.651101 183 | vn 0.123333 -0.943265 0.308285 184 | vn -0.493923 -0.720807 0.48629 185 | vn -0.477017 -0.8711 0.116787 186 | vn -0.44611 -0.794944 0.411157 187 | vn -6.11448e-05 -0.000331755 1 188 | vn -1.43156e-05 -8.03482e-05 1 189 | vn -0.228986 -0.885719 0.403816 190 | vn -0.608389 -0.519659 0.599848 191 | vn -0.920452 -0.383492 0.0755124 192 | vn -0.736949 -0.180808 0.651318 193 | vn -0.611742 -0.514007 0.601306 194 | vn 0.419302 0.901141 0.110141 195 | vn -0.991437 -0.0387918 0.124694 196 | vn -0.993276 -0.0773118 0.0861707 197 | vn -0.833082 0.535892 0.137092 198 | vn 8.17296e-05 2.2162e-05 1 199 | vn 0.943552 0.308615 0.120276 200 | vn 0.981039 0.152834 0.119183 201 | vn 0.973953 0.194242 0.116982 202 | vn 0.767761 0.629431 0.119836 203 | vn 0.775121 0.620511 0.118969 204 | vn 0.975511 -0.184098 0.120356 205 | vn -0.334118 0.859344 0.38716 206 | vn -0.186878 0.944088 0.271613 207 | vn -0.461627 0.746886 0.478605 208 | vn 0.418676 0.882839 0.21285 209 | vn 0.516842 0.832916 0.197804 210 | vn 0.465813 -0.878241 0.10822 211 | vn 0.444917 -0.890581 0.0944139 212 | vn 0.0505572 -0.0737673 -0.995993 213 | vn 0.458814 -0.864421 0.205587 214 | vn 0.516064 -0.83361 0.196908 215 | vn -0.387155 -0.789577 0.476109 216 | vn -0.244693 -0.878338 0.410666 217 | vn -0.208468 -0.972704 0.101918 218 | vn -0.193464 -0.977355 0.0857284 219 | vn -0.182203 -0.978184 0.0997879 220 | vn -0.519874 -0.667682 0.532852 221 | vn -0.000155222 -6.31553e-05 1 222 | vn -4.55781e-05 -2.14887e-05 1 223 | vn -0.773654 -0.387569 0.501247 224 | vn -0.836842 -0.541097 0.0831258 225 | vn -0.938711 -0.313668 0.142944 226 | vn 0.642112 0.752523 0.146293 227 | vn -0.975003 -0.187197 0.119691 228 | vn -0.98721 -0.103388 0.121358 229 | vn -0.991212 0.0930191 0.0940533 230 | vn -0.745994 0.128207 0.653495 231 | vn -0.357321 0.923722 0.138052 232 | vn -2.44058e-05 1.82076e-05 1 233 | vn -0.562279 0.66248 0.494937 234 | vn -0.609088 0.612822 0.50345 235 | vn 0.183429 0.950262 0.251704 236 | vn 0.21534 0.929691 0.298837 237 | vn 0.229004 0.927667 0.294942 238 | vn 0.604019 -0.779541 0.165763 239 | vn 0.230026 -0.929068 0.289689 240 | vn 0.372231 -0.889715 0.264293 241 | vn -0.171199 -0.978844 0.112047 242 | vn 0.0790207 -0.991596 0.102437 243 | vn -0.774998 -0.384377 0.501629 244 | vn -0.890148 -0.437507 0.127373 245 | vn -0.67661 -0.535887 0.505 246 | vn -0.844574 -0.525041 0.105012 247 | vn -0.695542 -0.313148 0.646653 248 | vn -0.714122 -0.263551 0.648514 249 | vn -0.974069 0.210719 0.082385 250 | vn -0.866667 0.0208868 0.498449 251 | vn -0.993956 0.0146296 0.108802 252 | vn -0.994723 0.0188777 0.100843 253 | vn -0.990466 0.0795982 0.112429 254 | vn -0.875827 0.475205 0.0843008 255 | vn -0.648594 0.437886 0.622561 256 | vn -0.631999 0.470323 0.615934 257 | vn -0.542549 0.633495 0.551656 258 | vn -0.569254 0.593159 0.569309 259 | vn -0.854631 -0.127937 0.503228 260 | vn 0.187376 -0.949866 0.250288 261 | vn -0.0299751 -0.962324 0.270247 262 | vn -0.733474 -0.46004 0.500378 263 | vn -0.819413 -0.278244 0.501142 264 | vn -0.818885 -0.280329 0.500842 265 | vn -0.840131 -0.200724 0.503876 266 | vn -0.865477 -0.0010483 0.500948 267 | vn -0.861767 -0.077237 0.501391 268 | vn -0.734892 0.455474 0.502471 269 | vn -0.789187 0.35685 0.499842 270 | vn -0.899886 0.416094 0.130652 271 | vn -0.934904 0.343772 0.0881805 272 | vn -0.750321 -0.110129 0.651836 273 | vn -0.744296 -0.138475 0.653336 274 | vn -0.0486186 -0.96337 0.263731 275 | vn -0.0569915 -0.952558 0.298973 276 | vn -0.0786935 -0.945844 0.314939 277 | vn 0.0865387 -0.962264 0.257989 278 | vn 0.0988769 -0.988653 0.113084 279 | vn -0.040165 -0.954927 0.294112 280 | vn -0.679455 -0.364641 0.636692 281 | vn -0.676925 -0.535527 0.504959 282 | vn -0.860556 -0.0952615 0.500368 283 | vn -0.866101 -0.0391639 0.498333 284 | vn -0.848661 -0.170347 0.500757 285 | vn -0.976331 0.171097 0.132301 286 | vn -0.784297 0.365988 0.500931 287 | vn -0.743675 0.147478 0.652072 288 | vn -0.75636 0.0667582 0.650741 289 | vn -0.726694 0.470061 0.500957 290 | vn -0.946042 0.295169 0.133715 291 | vn -0.699993 0.303725 0.646344 292 | vn -0.690574 0.326044 0.645603 293 | vn -0.722223 0.238827 0.64912 294 | vn -0.862635 0.0731211 0.500513 295 | vn -0.860436 0.0839585 0.502594 296 | vn -0.823144 0.266065 0.501641 297 | vn -0.850508 0.155797 0.502359 298 | vn -0.850275 0.158766 0.501823 299 | vn -0.824425 0.262885 0.501214 300 | vn -0.000104827 0.000138107 -1 301 | vn -6.4345e-05 4.8788e-05 -1 302 | vn -0.000255153 -5.05889e-05 -1 303 | vn 8.56579e-06 1.35717e-06 -1 304 | vn 3.45225e-06 -5.63502e-06 -1 305 | vn 0.000304041 -4.54974e-06 -1 306 | vn 0.0004587 -0.000166978 -1 307 | vn -4.30629e-05 3.84989e-05 -1 308 | vn -0.000206776 -3.99625e-05 -1 309 | vn -7.87823e-05 -3.27836e-06 -1 310 | vn -0.000409956 0.000110369 -1 311 | vn 6.31049e-05 7.3988e-06 -1 312 | vn -0.0003035 2.77655e-05 -1 313 | 314 | # Mesh '' with 200 faces 315 | g 316 | usemtl DefaultMaterial 317 | f 1//1 2//1 3//1 318 | f 4//2 2//2 1//2 319 | f 3//3 5//3 1//3 320 | f 6//4 7//4 8//4 321 | f 9//5 1//5 10//5 322 | f 1//6 11//6 10//6 323 | f 12//7 13//7 14//7 324 | f 13//7 15//8 14//7 325 | f 2//9 13//9 3//9 326 | f 16//10 9//10 17//10 327 | f 14//11 18//11 19//11 328 | f 20//12 2//12 4//12 329 | f 21//13 20//13 4//13 330 | f 22//14 23//14 4//14 331 | f 23//15 21//15 4//15 332 | f 1//16 5//16 11//16 333 | f 13//17 24//17 15//17 334 | f 20//18 21//18 24//18 335 | f 25//19 26//19 14//19 336 | f 15//20 27//20 14//20 337 | f 28//21 29//21 14//20 338 | f 14//22 30//22 25//22 339 | f 14//22 31//23 12//23 340 | f 32//24 25//24 30//24 341 | f 33//25 17//25 14//25 342 | f 34//26 35//26 14//26 343 | f 36//27 37//27 14//27 344 | f 14//28 37//28 34//28 345 | f 38//29 33//29 14//29 346 | f 19//30 38//30 14//30 347 | f 39//31 18//31 40//31 348 | f 7//32 6//32 41//32 349 | f 42//33 43//33 44//33 350 | f 45//34 43//34 42//34 351 | f 4//35 46//35 47//35 352 | f 17//36 31//36 14//36 353 | f 9//37 31//37 17//37 354 | f 10//38 31//38 9//38 355 | f 11//39 31//39 10//39 356 | f 12//40 31//40 11//40 357 | f 21//41 23//41 48//41 358 | f 13//42 49//42 3//42 359 | f 12//43 49//43 13//43 360 | f 25//44 32//44 50//44 361 | f 14//45 29//45 51//45 362 | f 52//46 30//46 14//46 363 | f 27//47 52//47 14//47 364 | f 53//48 32//48 30//48 365 | f 9//49 54//49 1//49 366 | f 35//50 55//50 14//50 367 | f 56//51 44//51 43//51 368 | f 57//52 47//52 46//52 369 | f 4//53 47//53 57//53 370 | f 58//54 46//54 4//54 371 | f 53//55 46//55 58//55 372 | f 57//56 22//56 4//56 373 | f 57//57 59//57 22//57 374 | f 59//58 15//58 22//58 375 | f 60//59 61//59 62//59 376 | f 24//60 48//60 15//60 377 | f 24//61 21//61 48//61 378 | f 49//62 5//62 3//62 379 | f 5//63 12//63 11//63 380 | f 50//64 4//64 45//64 381 | f 4//65 50//65 58//65 382 | f 32//66 53//66 58//66 383 | f 42//67 26//67 25//67 384 | f 52//68 27//68 63//68 385 | f 9//69 16//69 54//69 386 | f 37//70 64//70 34//70 387 | f 38//71 19//71 65//71 388 | f 40//72 6//72 39//72 389 | f 8//73 66//73 6//73 390 | f 6//74 66//74 39//74 391 | f 18//75 67//75 19//75 392 | f 68//76 19//76 67//76 393 | f 67//77 69//77 68//77 394 | f 40//78 70//78 55//78 395 | f 71//79 72//79 60//79 396 | f 73//80 74//80 35//80 397 | f 55//81 75//81 40//81 398 | f 57//82 46//82 59//82 399 | f 76//83 77//83 78//83 400 | f 62//84 77//84 76//84 401 | f 79//85 44//85 56//85 402 | f 80//86 24//86 13//86 403 | f 24//87 80//87 20//87 404 | f 2//88 80//88 13//88 405 | f 20//89 80//89 2//89 406 | f 15//90 48//90 22//90 407 | f 22//91 48//91 23//91 408 | f 49//92 12//92 5//92 409 | f 25//93 50//93 42//93 410 | f 58//94 50//94 32//94 411 | f 42//95 81//95 26//95 412 | f 59//96 46//96 63//96 413 | f 59//97 63//97 27//97 414 | f 82//98 54//98 16//98 415 | f 82//99 83//99 54//99 416 | f 54//100 83//100 1//100 417 | f 16//101 84//101 82//101 418 | f 33//102 84//102 16//102 419 | f 39//103 67//103 18//103 420 | f 69//104 67//104 39//104 421 | f 8//105 39//105 66//105 422 | f 1//106 39//106 8//106 423 | f 69//107 39//107 1//107 424 | f 40//108 18//108 70//108 425 | f 70//109 18//109 14//109 426 | f 55//110 70//110 14//110 427 | f 72//111 75//111 85//111 428 | f 7//112 41//112 71//112 429 | f 60//113 72//113 61//113 430 | f 59//114 27//114 15//114 431 | f 62//115 61//115 86//115 432 | f 86//116 77//116 62//116 433 | f 87//117 88//117 89//117 434 | f 51//118 90//118 14//118 435 | f 42//119 50//119 45//119 436 | f 14//120 26//120 28//120 437 | f 42//121 44//121 81//121 438 | f 44//122 91//122 81//122 439 | f 53//123 63//123 46//123 440 | f 52//124 53//124 30//124 441 | f 63//125 53//125 52//125 442 | f 33//126 16//126 17//126 443 | f 65//127 84//127 38//127 444 | f 33//128 38//128 84//128 445 | f 69//129 1//129 92//129 446 | f 83//130 92//130 1//130 447 | f 75//131 72//131 93//131 448 | f 93//132 72//132 71//132 449 | f 6//133 40//133 75//133 450 | f 41//134 93//134 71//134 451 | f 85//135 55//135 35//135 452 | f 74//136 85//136 35//136 453 | f 87//137 94//137 95//137 454 | f 36//138 96//138 78//138 455 | f 76//139 96//139 88//139 456 | f 78//140 96//140 76//140 457 | f 88//141 96//141 89//141 458 | f 97//142 79//142 56//142 459 | f 29//143 91//143 98//143 460 | f 28//144 91//144 29//144 461 | f 26//145 81//145 28//145 462 | f 91//146 28//146 81//146 463 | f 73//147 64//147 86//147 464 | f 65//148 82//148 84//148 465 | f 92//149 65//149 99//149 466 | f 93//150 41//150 75//150 467 | f 72//151 85//151 61//151 468 | f 61//152 85//152 74//152 469 | f 61//153 74//153 73//153 470 | f 36//154 78//154 37//154 471 | f 37//155 77//155 64//155 472 | f 98//156 91//156 79//156 473 | f 79//157 100//157 98//157 474 | f 97//158 101//158 79//158 475 | f 95//159 101//159 97//159 476 | f 64//160 73//160 34//160 477 | f 73//161 35//161 34//161 478 | f 69//162 92//162 99//162 479 | f 19//163 69//163 99//163 480 | f 68//164 69//164 19//164 481 | f 92//165 82//165 65//165 482 | f 92//166 83//166 82//166 483 | f 99//167 65//167 19//167 484 | f 85//168 75//168 55//168 485 | f 41//169 6//169 75//169 486 | f 64//170 77//170 86//170 487 | f 77//171 37//171 78//171 488 | f 86//172 61//172 73//172 489 | f 87//173 89//173 94//173 490 | f 79//174 101//174 100//174 491 | f 102//175 90//175 51//175 492 | f 36//176 14//176 90//176 493 | f 91//177 44//177 79//177 494 | f 95//178 94//178 101//178 495 | f 29//179 98//179 51//179 496 | f 100//180 51//180 98//180 497 | f 102//181 51//181 100//181 498 | f 89//182 96//182 36//182 499 | f 36//183 90//183 89//183 500 | f 101//184 102//184 100//184 501 | f 94//185 89//185 102//185 502 | f 90//186 102//186 89//186 503 | f 101//187 94//187 102//187 504 | f 4//188 1//188 87//188 505 | f 4//189 87//189 45//189 506 | f 1//190 8//190 87//190 507 | f 60//191 87//191 71//191 508 | f 56//192 87//191 97//192 509 | f 76//193 87//193 62//193 510 | f 56//194 43//194 87//194 511 | f 45//195 87//195 43//195 512 | f 8//196 7//196 87//196 513 | f 62//197 87//197 60//197 514 | f 97//198 87//198 95//198 515 | f 71//199 87//199 7//199 516 | f 88//200 87//200 76//200 517 | 518 | -------------------------------------------------------------------------------- /env/models/panda/meshes/collision/link6.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'link2.blend' 2 | # Material Count: 17 3 | 4 | newmtl Face064_002_001_002_001.001 5 | Ns -1.960784 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 1.000000 0.000000 0.000000 8 | Ks 0.003906 0.003906 0.003906 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | 14 | newmtl Face065_002_001_002_001.001 15 | Ns -1.960784 16 | Ka 1.000000 1.000000 1.000000 17 | Kd 0.000000 1.000000 0.000000 18 | Ks 0.003906 0.003906 0.003906 19 | Ke 0.000000 0.000000 0.000000 20 | Ni 1.000000 21 | d 1.000000 22 | illum 2 23 | 24 | newmtl Face374_002_001_002_001.001 25 | Ns -1.960784 26 | Ka 1.000000 1.000000 1.000000 27 | Kd 1.000000 1.000000 1.000000 28 | Ks 0.003906 0.003906 0.003906 29 | Ke 0.000000 0.000000 0.000000 30 | Ni 1.000000 31 | d 1.000000 32 | illum 2 33 | 34 | newmtl Face539_002_001_002_001.001 35 | Ns -1.960784 36 | Ka 1.000000 1.000000 1.000000 37 | Kd 0.250980 0.250980 0.250980 38 | Ks 0.003906 0.003906 0.003906 39 | Ke 0.000000 0.000000 0.000000 40 | Ni 1.000000 41 | d 1.000000 42 | illum 2 43 | map_Kd D:\dev\pybullet_robots\data\franka_panda\meshes\visual\colors.png 44 | 45 | newmtl Part__Feature001_009_001_002_001.001 46 | Ns -1.960784 47 | Ka 1.000000 1.000000 1.000000 48 | Kd 0.250980 0.250980 0.250980 49 | Ks 0.003906 0.003906 0.003906 50 | Ke 0.000000 0.000000 0.000000 51 | Ni 1.000000 52 | d 1.000000 53 | illum 2 54 | map_Kd D:\dev\pybullet_robots\data\franka_panda\meshes\visual\colors.png 55 | 56 | newmtl Part__Feature002_006_001_002_001.001 57 | Ns -1.960784 58 | Ka 1.000000 1.000000 1.000000 59 | Kd 0.250980 0.250980 0.250980 60 | Ks 0.003906 0.003906 0.003906 61 | Ke 0.000000 0.000000 0.000000 62 | Ni 1.000000 63 | d 1.000000 64 | illum 2 65 | map_Kd D:\dev\pybullet_robots\data\franka_panda\meshes\visual\colors.png 66 | 67 | newmtl Shell002_002_001_002_001.001 68 | Ns -1.960784 69 | Ka 1.000000 1.000000 1.000000 70 | Kd 1.000000 1.000000 1.000000 71 | Ks 0.003906 0.003906 0.003906 72 | Ke 0.000000 0.000000 0.000000 73 | Ni 1.000000 74 | d 1.000000 75 | illum 2 76 | 77 | newmtl Shell003_002_001_002_001.001 78 | Ns -1.960784 79 | Ka 1.000000 1.000000 1.000000 80 | Kd 1.000000 1.000000 1.000000 81 | Ks 0.003906 0.003906 0.003906 82 | Ke 0.000000 0.000000 0.000000 83 | Ni 1.000000 84 | d 1.000000 85 | illum 2 86 | 87 | newmtl Shell004_001_001_002_001.001 88 | Ns -1.960784 89 | Ka 1.000000 1.000000 1.000000 90 | Kd 1.000000 1.000000 1.000000 91 | Ks 0.003906 0.003906 0.003906 92 | Ke 0.000000 0.000000 0.000000 93 | Ni 1.000000 94 | d 1.000000 95 | illum 2 96 | 97 | newmtl Shell005_001_001_002_001.001 98 | Ns -1.960784 99 | Ka 1.000000 1.000000 1.000000 100 | Kd 1.000000 1.000000 1.000000 101 | Ks 0.003906 0.003906 0.003906 102 | Ke 0.000000 0.000000 0.000000 103 | Ni 1.000000 104 | d 1.000000 105 | illum 2 106 | 107 | newmtl Shell006_003_002_001.001 108 | Ns -1.960784 109 | Ka 1.000000 1.000000 1.000000 110 | Kd 0.901961 0.921569 0.929412 111 | Ks 0.015625 0.015625 0.015625 112 | Ke 0.000000 0.000000 0.000000 113 | Ni 1.000000 114 | d 1.000000 115 | illum 2 116 | 117 | newmtl Shell007_002_002_001.001 118 | Ns -1.960784 119 | Ka 1.000000 1.000000 1.000000 120 | Kd 0.250000 0.250000 0.250000 121 | Ks 0.003906 0.003906 0.003906 122 | Ke 0.000000 0.000000 0.000000 123 | Ni 1.000000 124 | d 1.000000 125 | illum 2 126 | 127 | newmtl Shell011_002_002_001.001 128 | Ns -1.960784 129 | Ka 1.000000 1.000000 1.000000 130 | Kd 1.000000 1.000000 1.000000 131 | Ks 0.003906 0.003906 0.003906 132 | Ke 0.000000 0.000000 0.000000 133 | Ni 1.000000 134 | d 1.000000 135 | illum 2 136 | 137 | newmtl Shell012_002_002_001.001 138 | Ns -1.960784 139 | Ka 1.000000 1.000000 1.000000 140 | Kd 1.000000 1.000000 1.000000 141 | Ks 0.003906 0.003906 0.003906 142 | Ke 0.000000 0.000000 0.000000 143 | Ni 1.000000 144 | d 1.000000 145 | illum 2 146 | map_Kd D:\dev\pybullet_robots\data\franka_panda\meshes\visual\colors.png 147 | 148 | newmtl Shell_003_001_002_001.001 149 | Ns -1.960784 150 | Ka 1.000000 1.000000 1.000000 151 | Kd 0.250980 0.250980 0.250980 152 | Ks 0.003906 0.003906 0.003906 153 | Ke 0.000000 0.000000 0.000000 154 | Ni 1.000000 155 | d 1.000000 156 | illum 2 157 | map_Kd D:\dev\pybullet_robots\data\franka_panda\meshes\visual\colors.png 158 | 159 | newmtl Union001_001_001_002_001.001 160 | Ns -1.960784 161 | Ka 1.000000 1.000000 1.000000 162 | Kd 0.039216 0.541176 0.780392 163 | Ks 0.003906 0.003906 0.003906 164 | Ke 0.000000 0.000000 0.000000 165 | Ni 1.000000 166 | d 1.000000 167 | illum 2 168 | 169 | newmtl Union_001_001_002_001.001 170 | Ns -1.960784 171 | Ka 1.000000 1.000000 1.000000 172 | Kd 0.039216 0.541176 0.780392 173 | Ks 0.003906 0.003906 0.003906 174 | Ke 0.000000 0.000000 0.000000 175 | Ni 1.000000 176 | d 1.000000 177 | illum 2 178 | -------------------------------------------------------------------------------- /env/models/panda/meshes/visual/colors.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/liruiw/HCG/0059be3091658fc05df25bebb0ff33c02a8df581/env/models/panda/meshes/visual/colors.png -------------------------------------------------------------------------------- /env/models/panda/meshes/visual/finger.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'link2.blend' 2 | # Material Count: 2 3 | 4 | newmtl Part__Feature001_006.001 5 | Ns -1.960784 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 0.901961 0.921569 0.929412 8 | Ks 0.250000 0.250000 0.250000 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | map_Kd colors.png 14 | 15 | newmtl Part__Feature_007.001 16 | Ns -1.960784 17 | Ka 1.000000 1.000000 1.000000 18 | Kd 0.250980 0.250980 0.250980 19 | Ks 0.250000 0.250000 0.250000 20 | Ke 0.000000 0.000000 0.000000 21 | Ni 1.000000 22 | d 1.000000 23 | illum 2 24 | map_Kd colors.png 25 | -------------------------------------------------------------------------------- /env/models/panda/meshes/visual/hand.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'link2.blend' 2 | # Material Count: 5 3 | 4 | newmtl Part__Feature001_008_005.001 5 | Ns -1.960784 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 0.250980 0.250980 0.250980 8 | Ks 0.007812 0.007812 0.007812 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | map_Kd colors.png 14 | 15 | newmtl Part__Feature002_005_005.001 16 | Ns -1.960784 17 | Ka 1.000000 1.000000 1.000000 18 | Kd 0.901961 0.921569 0.929412 19 | Ks 0.015625 0.015625 0.015625 20 | Ke 0.000000 0.000000 0.000000 21 | Ni 1.000000 22 | d 1.000000 23 | illum 2 24 | 25 | newmtl Part__Feature005_001_005.001 26 | Ns -1.960784 27 | Ka 1.000000 1.000000 1.000000 28 | Kd 1.000000 1.000000 1.000000 29 | Ks 0.015625 0.015625 0.015625 30 | Ke 0.000000 0.000000 0.000000 31 | Ni 1.000000 32 | d 1.000000 33 | illum 2 34 | map_Kd colors.png 35 | 36 | newmtl Part__Feature005_001_005_001.001 37 | Ns -1.960784 38 | Ka 1.000000 1.000000 1.000000 39 | Kd 0.901961 0.921569 0.929412 40 | Ks 0.015625 0.015625 0.015625 41 | Ke 0.000000 0.000000 0.000000 42 | Ni 1.000000 43 | d 1.000000 44 | illum 2 45 | map_Kd colors.png 46 | 47 | newmtl Part__Feature_009_005.001 48 | Ns -1.960784 49 | Ka 1.000000 1.000000 1.000000 50 | Kd 0.250980 0.250980 0.250980 51 | Ks 0.015625 0.015625 0.015625 52 | Ke 0.000000 0.000000 0.000000 53 | Ni 1.000000 54 | d 1.000000 55 | illum 2 56 | map_Kd colors.png 57 | -------------------------------------------------------------------------------- /env/models/panda/meshes/visual/link1.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'link2.blend' 2 | # Material Count: 1 3 | 4 | newmtl Part__Feature_001 5 | Ns -1.960784 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 1.000000 1.000000 1.000000 8 | Ks 0.062500 0.062500 0.062500 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | map_Kd colors.png 14 | -------------------------------------------------------------------------------- /env/models/panda/meshes/visual/link2.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'link3.blend' 2 | # Material Count: 1 3 | 4 | newmtl Part__Feature024 5 | Ns -1.960784 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 1.000000 1.000000 1.000000 8 | Ks 0.125000 0.125000 0.125000 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | map_Kd colors.png 14 | -------------------------------------------------------------------------------- /env/models/panda/meshes/visual/link3.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'link4.blend' 2 | # Material Count: 4 3 | 4 | newmtl Part__Feature001_010_001_002 5 | Ns -1.960784 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 1.000000 1.000000 1.000000 8 | Ks 0.007812 0.007812 0.007812 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | map_Kd colors.png 14 | 15 | newmtl Part__Feature002_007_001_002 16 | Ns -1.960784 17 | Ka 1.000000 1.000000 1.000000 18 | Kd 1.000000 1.000000 1.000000 19 | Ks 0.007812 0.007812 0.007812 20 | Ke 0.000000 0.000000 0.000000 21 | Ni 1.000000 22 | d 1.000000 23 | illum 2 24 | map_Kd colors.png 25 | 26 | newmtl Part__Feature003_004_001_002 27 | Ns -1.960784 28 | Ka 1.000000 1.000000 1.000000 29 | Kd 1.000000 1.000000 1.000000 30 | Ks 0.007812 0.007812 0.007812 31 | Ke 0.000000 0.000000 0.000000 32 | Ni 1.000000 33 | d 1.000000 34 | illum 2 35 | map_Kd colors.png 36 | 37 | newmtl Part__Feature_001_001_001_002 38 | Ns -1.960784 39 | Ka 1.000000 1.000000 1.000000 40 | Kd 0.250980 0.250980 0.250980 41 | Ks 0.007812 0.007812 0.007812 42 | Ke 0.000000 0.000000 0.000000 43 | Ni 1.000000 44 | d 1.000000 45 | illum 2 46 | map_Kd colors.png 47 | -------------------------------------------------------------------------------- /env/models/panda/meshes/visual/link4.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'link4.blend' 2 | # Material Count: 4 3 | 4 | newmtl Part__Feature001_001_003_001.001 5 | Ns -1.960784 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 1.000000 1.000000 1.000000 8 | Ks 0.007812 0.007812 0.007812 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | 14 | newmtl Part__Feature002_001_003_001.001 15 | Ns -1.960784 16 | Ka 1.000000 1.000000 1.000000 17 | Kd 0.250980 0.250980 0.250980 18 | Ks 0.007812 0.007812 0.007812 19 | Ke 0.000000 0.000000 0.000000 20 | Ni 1.000000 21 | d 1.000000 22 | illum 2 23 | map_Kd colors.png 24 | 25 | newmtl Part__Feature003_001_003_001.001 26 | Ns -1.960784 27 | Ka 1.000000 1.000000 1.000000 28 | Kd 1.000000 1.000000 1.000000 29 | Ks 0.007812 0.007812 0.007812 30 | Ke 0.000000 0.000000 0.000000 31 | Ni 1.000000 32 | d 1.000000 33 | illum 2 34 | map_Kd colors.png 35 | 36 | newmtl Part__Feature_002_003_001.001 37 | Ns -1.960784 38 | Ka 1.000000 1.000000 1.000000 39 | Kd 1.000000 1.000000 1.000000 40 | Ks 0.007812 0.007812 0.007812 41 | Ke 0.000000 0.000000 0.000000 42 | Ni 1.000000 43 | d 1.000000 44 | illum 2 45 | map_Kd colors.png 46 | -------------------------------------------------------------------------------- /env/models/panda/meshes/visual/link5.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 3 3 | 4 | newmtl Part__Feature_002_004_003.002 5 | Ns -1.960784 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 1.000000 1.000000 1.000000 8 | Ks 0.015625 0.015625 0.015625 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | map_Kd colors.png 14 | 15 | newmtl Shell001_001_001_003.002 16 | Ns -1.960784 17 | Ka 1.000000 1.000000 1.000000 18 | Kd 0.250000 0.250000 0.250000 19 | Ks 0.015625 0.015625 0.015625 20 | Ke 0.000000 0.000000 0.000000 21 | Ni 1.000000 22 | d 1.000000 23 | illum 2 24 | map_Kd colors.png 25 | 26 | newmtl Shell_001_001_003.002 27 | Ns -1.960784 28 | Ka 1.000000 1.000000 1.000000 29 | Kd 1.000000 1.000000 1.000000 30 | Ks 0.015625 0.015625 0.015625 31 | Ke 0.000000 0.000000 0.000000 32 | Ni 1.000000 33 | d 1.000000 34 | illum 2 35 | -------------------------------------------------------------------------------- /env/models/panda/meshes/visual/link6.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'link2.blend' 2 | # Material Count: 17 3 | 4 | newmtl Face064_002_001_002_001.001 5 | Ns -1.960784 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 1.000000 0.000000 0.000000 8 | Ks 0.003906 0.003906 0.003906 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | 14 | newmtl Face065_002_001_002_001.001 15 | Ns -1.960784 16 | Ka 1.000000 1.000000 1.000000 17 | Kd 0.000000 1.000000 0.000000 18 | Ks 0.003906 0.003906 0.003906 19 | Ke 0.000000 0.000000 0.000000 20 | Ni 1.000000 21 | d 1.000000 22 | illum 2 23 | 24 | newmtl Face374_002_001_002_001.001 25 | Ns -1.960784 26 | Ka 1.000000 1.000000 1.000000 27 | Kd 1.000000 1.000000 1.000000 28 | Ks 0.003906 0.003906 0.003906 29 | Ke 0.000000 0.000000 0.000000 30 | Ni 1.000000 31 | d 1.000000 32 | illum 2 33 | 34 | newmtl Face539_002_001_002_001.001 35 | Ns -1.960784 36 | Ka 1.000000 1.000000 1.000000 37 | Kd 0.250980 0.250980 0.250980 38 | Ks 0.003906 0.003906 0.003906 39 | Ke 0.000000 0.000000 0.000000 40 | Ni 1.000000 41 | d 1.000000 42 | illum 2 43 | map_Kd colors.png 44 | 45 | newmtl Part__Feature001_009_001_002_001.001 46 | Ns -1.960784 47 | Ka 1.000000 1.000000 1.000000 48 | Kd 0.250980 0.250980 0.250980 49 | Ks 0.003906 0.003906 0.003906 50 | Ke 0.000000 0.000000 0.000000 51 | Ni 1.000000 52 | d 1.000000 53 | illum 2 54 | map_Kd colors.png 55 | 56 | newmtl Part__Feature002_006_001_002_001.001 57 | Ns -1.960784 58 | Ka 1.000000 1.000000 1.000000 59 | Kd 0.250980 0.250980 0.250980 60 | Ks 0.003906 0.003906 0.003906 61 | Ke 0.000000 0.000000 0.000000 62 | Ni 1.000000 63 | d 1.000000 64 | illum 2 65 | map_Kd colors.png 66 | 67 | newmtl Shell002_002_001_002_001.001 68 | Ns -1.960784 69 | Ka 1.000000 1.000000 1.000000 70 | Kd 1.000000 1.000000 1.000000 71 | Ks 0.003906 0.003906 0.003906 72 | Ke 0.000000 0.000000 0.000000 73 | Ni 1.000000 74 | d 1.000000 75 | illum 2 76 | 77 | newmtl Shell003_002_001_002_001.001 78 | Ns -1.960784 79 | Ka 1.000000 1.000000 1.000000 80 | Kd 1.000000 1.000000 1.000000 81 | Ks 0.003906 0.003906 0.003906 82 | Ke 0.000000 0.000000 0.000000 83 | Ni 1.000000 84 | d 1.000000 85 | illum 2 86 | 87 | newmtl Shell004_001_001_002_001.001 88 | Ns -1.960784 89 | Ka 1.000000 1.000000 1.000000 90 | Kd 1.000000 1.000000 1.000000 91 | Ks 0.003906 0.003906 0.003906 92 | Ke 0.000000 0.000000 0.000000 93 | Ni 1.000000 94 | d 1.000000 95 | illum 2 96 | 97 | newmtl Shell005_001_001_002_001.001 98 | Ns -1.960784 99 | Ka 1.000000 1.000000 1.000000 100 | Kd 1.000000 1.000000 1.000000 101 | Ks 0.003906 0.003906 0.003906 102 | Ke 0.000000 0.000000 0.000000 103 | Ni 1.000000 104 | d 1.000000 105 | illum 2 106 | 107 | newmtl Shell006_003_002_001.001 108 | Ns -1.960784 109 | Ka 1.000000 1.000000 1.000000 110 | Kd 0.901961 0.921569 0.929412 111 | Ks 0.015625 0.015625 0.015625 112 | Ke 0.000000 0.000000 0.000000 113 | Ni 1.000000 114 | d 1.000000 115 | illum 2 116 | 117 | newmtl Shell007_002_002_001.001 118 | Ns -1.960784 119 | Ka 1.000000 1.000000 1.000000 120 | Kd 0.250000 0.250000 0.250000 121 | Ks 0.003906 0.003906 0.003906 122 | Ke 0.000000 0.000000 0.000000 123 | Ni 1.000000 124 | d 1.000000 125 | illum 2 126 | 127 | newmtl Shell011_002_002_001.001 128 | Ns -1.960784 129 | Ka 1.000000 1.000000 1.000000 130 | Kd 1.000000 1.000000 1.000000 131 | Ks 0.003906 0.003906 0.003906 132 | Ke 0.000000 0.000000 0.000000 133 | Ni 1.000000 134 | d 1.000000 135 | illum 2 136 | 137 | newmtl Shell012_002_002_001.001 138 | Ns -1.960784 139 | Ka 1.000000 1.000000 1.000000 140 | Kd 1.000000 1.000000 1.000000 141 | Ks 0.003906 0.003906 0.003906 142 | Ke 0.000000 0.000000 0.000000 143 | Ni 1.000000 144 | d 1.000000 145 | illum 2 146 | map_Kd colors.png 147 | 148 | newmtl Shell_003_001_002_001.001 149 | Ns -1.960784 150 | Ka 1.000000 1.000000 1.000000 151 | Kd 0.250980 0.250980 0.250980 152 | Ks 0.003906 0.003906 0.003906 153 | Ke 0.000000 0.000000 0.000000 154 | Ni 1.000000 155 | d 1.000000 156 | illum 2 157 | map_Kd colors.png 158 | 159 | newmtl Union001_001_001_002_001.001 160 | Ns -1.960784 161 | Ka 1.000000 1.000000 1.000000 162 | Kd 0.039216 0.541176 0.780392 163 | Ks 0.003906 0.003906 0.003906 164 | Ke 0.000000 0.000000 0.000000 165 | Ni 1.000000 166 | d 1.000000 167 | illum 2 168 | 169 | newmtl Union_001_001_002_001.001 170 | Ns -1.960784 171 | Ka 1.000000 1.000000 1.000000 172 | Kd 0.039216 0.541176 0.780392 173 | Ks 0.003906 0.003906 0.003906 174 | Ke 0.000000 0.000000 0.000000 175 | Ni 1.000000 176 | d 1.000000 177 | illum 2 178 | -------------------------------------------------------------------------------- /env/models/panda/panda_gripper_hand_camera.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 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 279 | 280 | 281 | 282 | 283 | 284 | 285 | 286 | 287 | 288 | 289 | 290 | 291 | 292 | 293 | 294 | 295 | 296 | 297 | 298 | 299 | 300 | 301 | 302 | 303 | 304 | 305 | 306 | 307 | 308 | 309 | 310 | 311 | 312 | 313 | 314 | 315 | 316 | 317 | 318 | 319 | 320 | 321 | 322 | 323 | 324 | 325 | 326 | 327 | 328 | 329 | 330 | 331 | 332 | 333 | 334 | 335 | 336 | 337 | 338 | 339 | 340 | 341 | 342 | 343 | 344 | 345 | 346 | 347 | 348 | 349 | 350 | 351 | 352 | 353 | 354 | 355 | 356 | 357 | 358 | 359 | 360 | 361 | 362 | 363 | 364 | 365 | 366 | 367 | -------------------------------------------------------------------------------- /env/panda_gripper_hand_camera.py: -------------------------------------------------------------------------------- 1 | # -------------------------------------------------------- 2 | # Licensed under The MIT License [see LICENSE for details] 3 | # -------------------------------------------------------- 4 | # Edited from https://github.com/bryandlee/franka-pybullet/blob/master/src/panda.py 5 | 6 | import pybullet as p 7 | import numpy as np 8 | import IPython 9 | import os 10 | 11 | class Panda: 12 | def __init__(self, stepsize=1e-3, realtime=0, init_joints=None, base_shift=[0,0,0], bullet_client=None): 13 | self.t = 0.0 14 | self.stepsize = stepsize 15 | self.realtime = realtime 16 | self.control_mode = "position" 17 | self.client = p if bullet_client is None else bullet_client 18 | self.position_control_gain_p = [0.01,0.01,0.01,0.01,0.01,0.01,0.01,0.01,0.01,0.01,0.01] 19 | self.position_control_gain_d = [1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0] 20 | f_max = 250 21 | self.max_torque = [f_max,f_max,f_max,f_max,f_max,f_max,f_max,150,150,150,150] 22 | 23 | # connect pybullet 24 | self.client.setRealTimeSimulation(self.realtime) 25 | 26 | # load models 27 | current_dir = os.path.dirname(os.path.abspath(__file__)) 28 | self.client.setAdditionalSearchPath(current_dir + "/models") 29 | self.robot = self.client.loadURDF("panda/panda_gripper_hand_camera.urdf", 30 | useFixedBase=True, 31 | flags=self.client.URDF_USE_SELF_COLLISION) 32 | self._base_position = [-0.05 - base_shift[0], 0.0 - base_shift[1], -0.65 - base_shift[2]] 33 | self.pandaUid = self.robot 34 | 35 | # robot parameters 36 | self.dof = self.client.getNumJoints(self.robot) 37 | c = self.client.createConstraint(self.robot, 38 | 8, 39 | self.robot, 40 | 9, 41 | jointType=self.client.JOINT_GEAR, 42 | jointAxis=[1, 0, 0], 43 | parentFramePosition=[0, 0, 0], 44 | childFramePosition=[0, 0, 0]) 45 | self.client.changeConstraint(c, gearRatio=-1, erp=0.1, maxForce=50) 46 | 47 | self.joints = [] 48 | self.q_min = [] 49 | self.q_max = [] 50 | self.target_pos = [] 51 | self.target_torque = [] 52 | self.pandaEndEffectorIndex = 7 53 | self._joint_min_limit = np.array([-2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973, 0, 0, 0, 0]) 54 | self._joint_max_limit = np.array([2.8973, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973, 0, 0.04, 0.04, 0]) 55 | 56 | for j in range(self.dof): 57 | self.client.changeDynamics(self.robot, j, linearDamping=0, angularDamping=0) 58 | joint_info = self.client.getJointInfo(self.robot, j) 59 | self.joints.append(j) 60 | self.q_min.append(joint_info[8]) 61 | self.q_max.append(joint_info[9]) 62 | self.target_pos.append((self.q_min[j] + self.q_max[j])/2.0) 63 | self.target_torque.append(0.) 64 | self.reset(init_joints) 65 | 66 | 67 | def reset(self, joints=None): 68 | self.t = 0.0 69 | self.control_mode = "position" 70 | self.client.resetBasePositionAndOrientation(self.pandaUid, self._base_position, 71 | [0.000000, 0.000000, 0.000000, 1.000000]) 72 | if joints is None: 73 | self.target_pos = [ 74 | 0.0, -1.285, 0, -2.356, 0.0, 1.571, 0.785, 0, 0.04, 0.04] 75 | 76 | self.target_pos = self.standardize(self.target_pos) 77 | for j in range(self.dof): 78 | self.target_torque[j] = 0. 79 | self.client.resetJointState(self.robot,j,targetValue=self.target_pos[j]) 80 | 81 | else: 82 | joints = self.standardize(joints) 83 | for j in range(self.dof): 84 | self.target_pos[j] = joints[j] 85 | self.target_torque[j] = 0. 86 | self.client.resetJointState(self.robot,j,targetValue=self.target_pos[j]) 87 | self.resetController() 88 | self.setTargetPositions(self.target_pos) 89 | 90 | def step(self): 91 | self.t += self.stepsize 92 | self.client.stepSimulation() 93 | 94 | def resetController(self): 95 | self.client.setJointMotorControlArray(bodyUniqueId=self.robot, 96 | jointIndices=self.joints, 97 | controlMode=self.client.VELOCITY_CONTROL, 98 | forces=[0. for i in range(self.dof)]) 99 | 100 | def standardize(self, target_pos): 101 | if type(target_pos) is np.ndarray and len(target_pos.shape) == 2: 102 | target_pos = target_pos[0] 103 | 104 | if len(target_pos) == 9: 105 | if type(target_pos) == list: 106 | target_pos.insert(7, 0) 107 | else: 108 | target_pos = np.insert(target_pos, 7, 0) 109 | target_pos = np.array(target_pos) 110 | if len(target_pos) == 10: 111 | target_pos = np.append(target_pos, 0) 112 | 113 | target_pos = np.minimum(np.maximum(target_pos, self._joint_min_limit), self._joint_max_limit) 114 | return target_pos 115 | 116 | def setTargetPositions(self, target_pos): 117 | self.target_pos = self.standardize(target_pos) 118 | self.client.setJointMotorControlArray(bodyUniqueId=self.robot, 119 | jointIndices=self.joints, 120 | controlMode=self.client.POSITION_CONTROL, 121 | targetPositions=self.target_pos, 122 | forces=self.max_torque, 123 | positionGains=self.position_control_gain_p, 124 | velocityGains=self.position_control_gain_d) 125 | 126 | def getJointStates(self): 127 | joint_states = self.client.getJointStates(self.robot, self.joints) 128 | joint_pos = [x[0] for x in joint_states] 129 | joint_vel = [x[1] for x in joint_states] 130 | 131 | if len(joint_pos) == 11: 132 | del joint_pos[7], joint_pos[-1] 133 | del joint_vel[7], joint_vel[-1] 134 | return joint_pos, joint_vel 135 | 136 | if __name__ == "__main__": 137 | robot = Panda(realtime=1) 138 | while True: 139 | pass 140 | -------------------------------------------------------------------------------- /experiments/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/liruiw/HCG/0059be3091658fc05df25bebb0ff33c02a8df581/experiments/__init__.py -------------------------------------------------------------------------------- /experiments/cfgs/bc_embedding_low_level.yaml: -------------------------------------------------------------------------------- 1 | RL_MEMORY_SIZE: 3000000 2 | RL_SAVE_DATA_NAME: 'data_5w_clutter.npz' 3 | OFFLINE_RL_MEMORY_SIZE: 50000 4 | RL_MEMORY_SIZE: 50000 5 | OFFLINE_BATCH_SIZE: 51 6 | RL_TRAIN: 7 | RL: False 8 | policy_aux: True 9 | batch_size: 64 10 | updates_per_step: 50 11 | train_traj_feature: True 12 | full_traj_embedding: True 13 | balanced_batch_length: True 14 | batch_sequence_size: 2 15 | batch_sequence_ratio: 9 16 | use_time: True 17 | feature_input_dim: 1024 18 | max_epoch: 120000 19 | -------------------------------------------------------------------------------- /experiments/cfgs/bc_sampler_restep.yaml: -------------------------------------------------------------------------------- 1 | RL_SAVE_DATA_NAME: 'data_5w_clutter.npz' 2 | OFFLINE_RL_MEMORY_SIZE: 50000 3 | RL_MEMORY_SIZE: 50000 4 | OFFLINE_BATCH_SIZE: 51 5 | RL_TRAIN: 6 | RL: False 7 | policy_aux: True 8 | updates_per_step: 50 9 | train_traj_feature: True 10 | full_traj_embedding: True 11 | balanced_batch_length: True 12 | batch_sequence_size: 2 13 | batch_sequence_ratio: 4 14 | use_time: True 15 | feature_input_dim: 1024 16 | max_epoch: 120000 17 | joint_sampler_embed: True 18 | train_traj_sampler: True 19 | traj_normal_vae: True 20 | use_latent_reg_loss: True 21 | normal_vae_dim: 2 22 | traj_sampler_latent_size: 1024 23 | vae_huge_model: True 24 | sampler_extra_abs_time: True 25 | re_sampler_step: True 26 | kl_scale: 0.02 27 | 28 | -------------------------------------------------------------------------------- /experiments/cfgs/dqn_critic_hrl.yaml: -------------------------------------------------------------------------------- 1 | RL_SAVE_DATA_NAME: 'data_5w_clutter.npz' 2 | OFFLINE_RL_MEMORY_SIZE: 150000 3 | RL_MEMORY_SIZE: 150000 4 | ONPOLICY_MEMORY_SIZE: 150000 5 | OFFLINE_BATCH_SIZE: 48 6 | RL_TRAIN: 7 | RL: True 8 | dqn: True 9 | ONPOLICY_BUFFER_NAME: 'data_5w_clutter_online.npz' 10 | explore_cap: 1.0 11 | dart: False 12 | explore_ratio_list: [1.0] 13 | epsilon_greedy_list: [0.7] 14 | gaddpg_ratio_list: [0.5, 0.7, 1.0] 15 | mix_value_ratio_list: [0.5] 16 | onpolicy: True 17 | online_buffer_ratio: 1. 18 | num_remotes: 17 19 | updates_per_step: 50 20 | index_file: 'ycb_large.json' 21 | full_traj_embedding: False 22 | balanced_batch_length: False 23 | max_epoch: 170000 24 | use_dataset_policy_feat: True 25 | use_offline_latent: True 26 | multi_traj_sample: True 27 | critic_mpc: True 28 | critic_gaddpg: True 29 | ignore_traj: True 30 | ignore_traj_sampler: True 31 | train_traj_sampler: True 32 | reinit_feat_opt: True 33 | critic_tanh: True 34 | critic_aux: False 35 | gaddpg_scale: 0.1 36 | test_log_sigma_clip: 1. 37 | 38 | -------------------------------------------------------------------------------- /experiments/cfgs/dqn_save_buffer.yaml: -------------------------------------------------------------------------------- 1 | RL_SAVE_DATA_NAME: 'data_5w_clutter.npz' 2 | OFFLINE_RL_MEMORY_SIZE: 50000 3 | RL_MEMORY_SIZE: 50000 4 | ONPOLICY_MEMORY_SIZE: 50000 5 | OFFLINE_BATCH_SIZE: 32 6 | RL_TRAIN: 7 | RL: True 8 | dqn: True 9 | ONPOLICY_BUFFER_NAME: 'data_5w_clutter_online.npz' 10 | explore_cap: 0.0 11 | dart: False 12 | onpolicy: True 13 | online_buffer_ratio: 1. 14 | num_remotes: 8 15 | updates_per_step: 10 16 | full_traj_embedding: False 17 | balanced_batch_length: False 18 | max_epoch: 180000 19 | use_dataset_policy_feat: True 20 | use_offline_latent: True 21 | multi_traj_sample: True 22 | critic_mpc: True 23 | critic_gaddpg: True 24 | ignore_traj: True 25 | ignore_traj_sampler: True 26 | train_traj_sampler: True 27 | sa_channel_concat: False 28 | reinit_feat_opt: True 29 | critic_tanh: True 30 | critic_aux: False 31 | 32 | -------------------------------------------------------------------------------- /experiments/config.py: -------------------------------------------------------------------------------- 1 | # -------------------------------------------------------- 2 | # Licensed under The MIT License [see LICENSE for details] 3 | # -------------------------------------------------------- 4 | 5 | """ 6 | Tuning online training scale based on local GPU and memory limits. The code is test on 4 7 | V100 GPU, and 100 GB CPU memory. 2 GPUs are used for actor rollout and the other two for training. 8 | 9 | The configs that can be adjusted: 10 | num_remotes, batch_size, RL_MEMORY_SIZE, @ray.remote(num_cpus=*, num_gpus=*) 11 | """ 12 | 13 | 14 | import os 15 | import os.path as osp 16 | import numpy as np 17 | import math 18 | import tabulate 19 | from easydict import EasyDict as edict 20 | import IPython 21 | import yaml 22 | import torch 23 | import random 24 | 25 | torch.manual_seed(0) 26 | np.random.seed(0) 27 | random.seed(0) 28 | 29 | root_dir = os.path.join(os.path.dirname(os.path.abspath(__file__)), '..') 30 | # create output folders 31 | 32 | if not os.path.exists(os.path.join(root_dir, 'output')): 33 | os.makedirs(os.path.join(root_dir, 'output')) 34 | if not os.path.exists(os.path.join(root_dir, 'output_misc')): 35 | os.makedirs(os.path.join(root_dir, 'output_misc/rl_output_stat')) 36 | 37 | __C = edict() 38 | cfg = __C 39 | 40 | # Global options 41 | # 42 | __C.script_name = '' 43 | __C.pretrained_time = '' 44 | 45 | __C.RNG_SEED = 3 46 | __C.EPOCHS = 200 47 | __C.ROOT_DIR = root_dir + '/' 48 | __C.DATA_ROOT_DIR = 'data/scenes' 49 | __C.ROBOT_DATA_DIR = 'data/robots' 50 | __C.OBJECT_DATA_DIR = 'data/objects' 51 | __C.OUTPUT_DIR = 'output' # temp_model_output 52 | __C.OUTPUT_MISC_DIR = 'output_misc' 53 | __C.MODEL_SPEC_DIR = "experiments/model_spec" 54 | __C.EXPERIMENT_OBJ_INDEX_DIR = "experiments/object_index" 55 | __C.LOG = True 56 | __C.MAX_SCENE_INDEX = 18000 57 | __C.IMG_SIZE = (112, 112) 58 | 59 | __C.RL_IMG_SIZE = (112, 112) 60 | __C.RL_MAX_STEP = 20 61 | __C.RL_DATA_ROOT_DIR = __C.DATA_ROOT_DIR 62 | __C.ONPOLICY_MEMORY_SIZE = -1 63 | __C.RL_MEMORY_SIZE = 100000 64 | __C.OFFLINE_RL_MEMORY_SIZE = 100000 65 | __C.OFFLINE_BATCH_SIZE = 180 66 | 67 | __C.RL_SAVE_DATA_ROOT_DIR = 'data/offline_data' 68 | __C.RL_SAVE_SCENE_ROOT_DIR = 'data/online_scenes' 69 | __C.SCRIPT_FOLDER = 'experiments/cfgs' 70 | __C.RL_SAVE_DATA_NAME = 'data_100k_exp.npz' 71 | __C.RL_MODEL_SPEC = os.path.join(__C.MODEL_SPEC_DIR, 'rl_pointnet_model_spec.yaml') 72 | __C.RL_TEST_SCENE = 'data/hcg_scenes' 73 | 74 | 75 | # detailed options 76 | # 77 | __C.RL_TRAIN = edict() 78 | 79 | # architecture and network hyperparameter 80 | __C.RL_TRAIN.ONPOLICY_BUFFER_NAME = '' 81 | __C.RL_TRAIN.clip_grad = 1.0 82 | __C.RL_TRAIN.sampler_clip_grad = -1.0 83 | __C.RL_TRAIN.gamma = 0.99 84 | __C.RL_TRAIN.batch_size = 256 85 | __C.RL_TRAIN.updates_per_step = 4 86 | __C.RL_TRAIN.hidden_size = 256 87 | 88 | __C.RL_TRAIN.tau = 0.0001 89 | __C.RL_TRAIN.lr = 3e-4 90 | __C.RL_TRAIN.reinit_lr = 1e-4 91 | __C.RL_TRAIN.value_lr = 3e-4 92 | __C.RL_TRAIN.lr_gamma = 0.5 93 | __C.RL_TRAIN.value_lr_gamma = 0.5 94 | __C.RL_TRAIN.head_lr = 3e-4 95 | __C.RL_TRAIN.feature_input_dim = 512 96 | __C.RL_TRAIN.ddpg_coefficients = [0.5, 0.001, 1.0003, 1., 0.2] 97 | __C.RL_TRAIN.value_milestones = [20000, 40000, 60000, 80000] 98 | __C.RL_TRAIN.policy_milestones = [20000, 40000, 60000, 80000] 99 | __C.RL_TRAIN.mix_milestones = [20000, 40000, 60000, 80000] 100 | __C.RL_TRAIN.mix_policy_ratio_list = [0.1, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2] 101 | __C.RL_TRAIN.mix_value_ratio_list = [1., 1., 1., 1., 1., 1., 1.] 102 | __C.RL_TRAIN.policy_extra_latent = -1 103 | __C.RL_TRAIN.critic_extra_latent = -1 104 | __C.RL_TRAIN.save_epoch = [3000, 10000, 20000, 40000, 80000, 100000, 140000, 170000, 200000, 230000] 105 | __C.RL_TRAIN.overwrite_feat_milestone = [] 106 | __C.RL_TRAIN.load_buffer = False 107 | __C.RL_TRAIN.object_dir = 'objects' 108 | 109 | # algorithm hyperparameter 110 | __C.RL_TRAIN.train_value_feature = True 111 | __C.RL_TRAIN.train_feature = True 112 | __C.RL_TRAIN.reinit_optim = False 113 | __C.RL_TRAIN.off_policy = True 114 | __C.RL_TRAIN.use_action_limit = True 115 | __C.RL_TRAIN.sa_channel_concat = False 116 | __C.RL_TRAIN.use_image = False 117 | __C.RL_TRAIN.dagger = False 118 | __C.RL_TRAIN.use_time = True 119 | __C.RL_TRAIN.RL = True 120 | __C.RL_TRAIN.value_model = False 121 | __C.RL_TRAIN.shared_feature = False 122 | __C.RL_TRAIN.policy_update_gap = 2 123 | __C.RL_TRAIN.self_supervision = False 124 | __C.RL_TRAIN.critic_goal = False 125 | __C.RL_TRAIN.policy_aux = True 126 | __C.RL_TRAIN.train_goal_feature = False 127 | __C.RL_TRAIN.train_traj_feature = False 128 | __C.RL_TRAIN.fix_traj_feature = False 129 | 130 | __C.RL_TRAIN.critic_aux = True 131 | __C.RL_TRAIN.policy_goal = False 132 | __C.RL_TRAIN.goal_reward_flag = False 133 | __C.RL_TRAIN.online_buffer_ratio = 0. 134 | __C.RL_TRAIN.onpolicy = False 135 | __C.RL_TRAIN.channel_num = 5 136 | __C.RL_TRAIN.change_dynamics = False 137 | __C.RL_TRAIN.pt_accumulate_ratio = 0.95 138 | __C.RL_TRAIN.dart = False 139 | __C.RL_TRAIN.accumulate_points = True 140 | __C.RL_TRAIN.max_epoch = 120000 141 | __C.RL_TRAIN.action_noise = 0.01 142 | __C.RL_TRAIN.expert_fix_goal = True 143 | 144 | # environment hyperparameter 145 | __C.RL_TRAIN.load_obj_num = 25 146 | __C.RL_TRAIN.reinit_factor = 2. 147 | __C.RL_TRAIN.shared_objects_across_worker = False 148 | __C.RL_TRAIN.target_update_interval = 3000 149 | __C.RL_TRAIN.index_split = 'train' 150 | __C.RL_TRAIN.env_name = 'PandaYCBEnv' 151 | __C.RL_TRAIN.index_file = os.path.join(__C.EXPERIMENT_OBJ_INDEX_DIR, 'extra_shape.json') 152 | __C.RL_TRAIN.max_num_pts = 20000 153 | __C.RL_TRAIN.uniform_num_pts = 1024 154 | __C.RL_TRAIN.initial_perturb = True 155 | 156 | # exploration worker hyperparameter 157 | __C.RL_TRAIN.num_remotes = 8 158 | __C.RL_TRAIN.init_distance_low = 0.2 159 | __C.RL_TRAIN.init_distance_high = 0.65 160 | __C.RL_TRAIN.explore_ratio = 0.0 161 | __C.RL_TRAIN.explore_cap = 1.0 162 | __C.RL_TRAIN.expert_perturbation_ratio = 1 163 | __C.RL_TRAIN.explore_ratio_list = [0.7] 164 | __C.RL_TRAIN.gaddpg_ratio_list = [1.0] 165 | __C.RL_TRAIN.epsilon_greedy_list = [0.8] 166 | __C.RL_TRAIN.noise_ratio_list = [1.] 167 | __C.RL_TRAIN.noise_type = 'uniform' 168 | __C.RL_TRAIN.expert_initial_state = False 169 | __C.RL_TRAIN.DAGGER_RATIO = 0.6 170 | __C.RL_TRAIN.DART_MIN_STEP = 5 171 | __C.RL_TRAIN.DART_MAX_STEP = 42 172 | __C.RL_TRAIN.DART_RATIO = 0.35 173 | __C.RL_TRAIN.SAVE_EPISODE_INTERVAL = 50 174 | __C.RL_TRAIN.ENV_NEAR = 0.6 175 | __C.RL_TRAIN.ENV_FAR = 0.8 176 | 177 | # misc hyperparameter 178 | __C.RL_TRAIN.log = True 179 | __C.RL_TRAIN.visdom = True 180 | __C.RL_TRAIN.domain_randomization = False 181 | __C.RL_TRAIN.buffer_full_size = -1 182 | __C.RL_TRAIN.buffer_start_idx = 0 183 | __C.RL_TRAIN.scene_sample_check_ik_cnt = 2 184 | __C.RL_TRAIN.scene_sample_inner_cnt = 5 185 | __C.RL_TRAIN.load_scene_recompute_ik = True 186 | __C.RL_TRAIN.max_test_per_obj = 10 187 | 188 | __C.RL_TRAIN.joint_point_state_input = True 189 | __C.RL_TRAIN.traj_joint_point_state_input = True 190 | __C.RL_TRAIN.feature_option = 2 191 | __C.RL_TRAIN.st_feature_option = 4 192 | __C.RL_TRAIN.traj_feature_option = 2 193 | __C.RL_TRAIN.traj_feature_extractor_class = "STPointNetFeature" 194 | __C.RL_TRAIN.traj_vae_feature_extractor_class = "PointTrajLatentNet" 195 | __C.RL_TRAIN.state_feature_extractor = "PointNetFeature" 196 | __C.RL_TRAIN.traj_goal_mutual_conditioned = True 197 | __C.RL_TRAIN.traj_latent_size = 64 198 | __C.RL_TRAIN.traj_sampler_latent_size = 1024 199 | __C.RL_TRAIN.full_traj_embedding = False 200 | __C.RL_TRAIN.batch_sequence_size = 4 201 | __C.RL_TRAIN.sparsify_traj_ratio = 5 202 | __C.RL_TRAIN.sparsify_bc_ratio = 6 203 | __C.RL_TRAIN.batch_sequence_ratio = 1 204 | __C.RL_TRAIN.use_simulated_plan = True 205 | 206 | __C.RL_TRAIN.traj_net_lr = 1e-3 207 | __C.RL_TRAIN.traj_sampler_net_lr = 3e-4 208 | __C.RL_TRAIN.traj_loss_scale = 1. 209 | __C.RL_TRAIN.latent_loss_scale = 1. 210 | __C.RL_TRAIN.kl_scale = 1. 211 | __C.RL_TRAIN.gaddpg_scale = 1. 212 | __C.RL_TRAIN.traj_feat_recons_loss = True 213 | __C.RL_TRAIN.policy_traj_latent_size = 64 214 | __C.RL_TRAIN.gaddpg_mean_remaining_step = 8 215 | __C.RL_TRAIN.gaddpg_std_remaining_step = 2. 216 | __C.RL_TRAIN.gaddpg_min_step = 5 217 | __C.RL_TRAIN.gaddpg_max_step = 15 218 | __C.RL_TRAIN.train_traj_sampler = False 219 | __C.RL_TRAIN.use_sampler_latent = False 220 | __C.RL_TRAIN.use_latent_reg_loss = False 221 | __C.RL_TRAIN.sampler_extra_abs_time = False 222 | 223 | __C.RL_TRAIN.critic_tanh = False 224 | __C.RL_TRAIN.hrl_offline_latent = False 225 | __C.RL_TRAIN.hrl_latent_action = False 226 | __C.RL_TRAIN.regress_saved_latent_path = '' 227 | __C.RL_TRAIN.use_dataset_policy_feat = False 228 | __C.RL_TRAIN.traj_normal_vae = False 229 | __C.RL_TRAIN.normal_vae_dim = 2 230 | __C.RL_TRAIN.sample_sim_traj = True 231 | 232 | __C.RL_TRAIN.save_expert_plan_latent_online = False 233 | __C.RL_TRAIN.debug_latent_type = 'zero' 234 | __C.RL_TRAIN.clip_tail_idx = 2 235 | __C.RL_TRAIN.use_offline_latent = False 236 | __C.RL_TRAIN.test_traj_num = 8 237 | __C.RL_TRAIN.numObjects = 23 238 | __C.RL_TRAIN.dqn = False 239 | __C.RL_TRAIN.critic_mpc = False 240 | __C.RL_TRAIN.critic_gaddpg = True 241 | __C.RL_TRAIN.multi_traj_sample = False 242 | __C.RL_TRAIN.ignore_traj = False 243 | __C.RL_TRAIN.ignore_traj_sampler = False 244 | __C.RL_TRAIN.re_sampler_step = False 245 | 246 | __C.RL_TRAIN.gaddpg_prob = 1. 247 | __C.RL_TRAIN.online_traj_sample_prob = 0.05 248 | __C.RL_TRAIN.reinit_feat_opt = True 249 | __C.RL_TRAIN.test_log_sigma_clip = 0.7 250 | __C.RL_TRAIN.value_overwrite_lr = 1.0e-3 251 | __C.RL_TRAIN.data_init_epoch = 1 252 | __C.RL_TRAIN.fix_batch_gaddpg_latent = True 253 | __C.RL_TRAIN.gaddpg_batch_ratio = 0.3 254 | __C.RL_TRAIN.dqn_sample_num = 4 255 | 256 | 257 | def process_cfg(reset_model_spec=True): 258 | """ 259 | adapt configs 260 | """ 261 | 262 | if reset_model_spec: 263 | __C.RL_MODEL_SPEC = os.path.join(__C.MODEL_SPEC_DIR , "rl_pointnet_model_spec.yaml" ) 264 | 265 | __C.RL_MODEL_SPEC = os.path.join(__C.MODEL_SPEC_DIR , "rl_sampler_large_pointnet_model_spec.yaml") 266 | 267 | # overwrite for cluttered scene 268 | __C.RL_TRAIN.V = [ [-0.9351, 0.3518, 0.0428, 0.3037], 269 | [0.2065, 0.639, -0.741, 0.132], 270 | [-0.2881, -0.684, -0.6702, 1.8803], 271 | [0.0, 0.0, 0.0, 1.0]] 272 | __C.RL_TRAIN.max_step = 50 273 | __C.RL_TRAIN.uniform_num_pts = 4096 274 | __C.RL_IMG_SIZE = (224, 224) 275 | __C.RL_MAX_STEP = 50 276 | __C.RL_TRAIN.value_model = True 277 | 278 | # expert planner parameters 279 | __C.omg_config = { 280 | 'traj_init':'grasp', 281 | 'scene_file': '' , 282 | 'vis': False, 283 | 'increment_iks': True , 284 | 'top_k_collision': 1200, 285 | 'collision_point_num': 15, 286 | 'uncheck_finger_collision': -1, 287 | 'optim_steps': 10, 288 | 'extra_smooth_steps': 15, 289 | 'ol_alg' :'MD', 290 | 'target_size': 1.0, 291 | 'goal_idx' :-2, 292 | 'traj_delta': 0.05, 293 | 'standoff_dist': 0.08, 294 | 'ik_clearance': 0.06, 295 | 'clearance': 0.04, 296 | 'goal_set_max_num': 50, 297 | 'dist_eps': 2.0, 298 | 'reach_tail_length': 7, 299 | 'ik_parallel': False, 300 | 'traj_max_step': int(__C.RL_MAX_STEP) + 6, 301 | 'root_dir': root_dir + "/", 302 | 'traj_min_step' :int(__C.RL_MAX_STEP) - 5, 303 | 'timesteps': int(__C.RL_MAX_STEP), 304 | 'dynamic_timestep': False, 305 | 'silent': True, 306 | 'timeout': 1.2, 307 | 'cam_V': __C.RL_TRAIN.V, 308 | 'force_standoff': True, 309 | 'z_upsample': True 310 | } 311 | 312 | # environment parameters 313 | __C.env_config = { 314 | "action_space": 'task6d', 315 | "data_type": 'RGBDM', 316 | "expert_step": int(__C.RL_MAX_STEP), 317 | "numObjects": __C.RL_TRAIN.numObjects, 318 | "width": __C.RL_IMG_SIZE[0], 319 | "height": __C.RL_IMG_SIZE[1], 320 | "img_resize": __C.RL_IMG_SIZE, 321 | "random_target": False, 322 | "use_hand_finger_point": True, 323 | "accumulate_points": __C.RL_TRAIN.accumulate_points, 324 | "uniform_num_pts": __C.RL_TRAIN.uniform_num_pts, 325 | "regularize_pc_point_count": True, 326 | "pt_accumulate_ratio": __C.RL_TRAIN.pt_accumulate_ratio, 327 | "omg_config": __C.omg_config, 328 | 'use_normal': False, 329 | 'env_remove_table_pt_input': True, 330 | } 331 | compute_input_dim() 332 | 333 | def compute_input_dim(): 334 | def has_check(x, prop): 335 | return hasattr(x, prop) and getattr(x, prop) 336 | 337 | extra_pred_dim = 1 338 | critic_extra_pred_dim = 0 339 | num_inputs = 0 340 | 341 | if __C.RL_TRAIN.policy_aux: 342 | extra_pred_dim = 7 343 | 344 | if __C.RL_TRAIN.critic_aux: 345 | critic_extra_pred_dim = 7 346 | 347 | if __C.RL_TRAIN.use_time: 348 | num_inputs += 1 349 | 350 | if has_check(__C.RL_TRAIN, 'critic_gaddpg'): 351 | critic_extra_pred_dim += 1 352 | 353 | 354 | __C.RL_TRAIN.num_input_extra = num_inputs 355 | __C.RL_TRAIN.extra_pred_dim = extra_pred_dim 356 | __C.RL_TRAIN.critic_extra_pred_dim = critic_extra_pred_dim 357 | __C.RL_TRAIN.num_inputs = __C.RL_TRAIN.num_input_extra + __C.RL_TRAIN.hidden_size 358 | 359 | 360 | def _merge_a_into_b(a, b): 361 | """Merge config dictionary a into config dictionary b, clobbering the 362 | options in b whenever they are also specified in a. 363 | """ 364 | if type(a) is not edict: 365 | return 366 | 367 | for k, v in a.items(): 368 | if not k in b.keys(): 369 | continue 370 | 371 | # the types must match, too 372 | if type(b[k]) is not type(v): 373 | continue 374 | 375 | # recursively merge dicts 376 | if type(v) is edict: 377 | try: 378 | _merge_a_into_b(a[k], b[k]) 379 | except: 380 | print("Error under config key: {}".format(k)) 381 | raise 382 | else: 383 | b[k] = v 384 | 385 | 386 | def cfg_from_file(filename=None, dict=None, reset_model_spec=True): 387 | """Load a config file and merge it into the default options.""" 388 | 389 | with open(filename, "r") as f: 390 | yaml_cfg = edict(yaml.load(f)) 391 | 392 | if not reset_model_spec: 393 | output_dir = "/".join(filename.split("/")[:-1]) 394 | __C.RL_MODEL_SPEC = os.path.join( 395 | output_dir, yaml_cfg["RL_MODEL_SPEC"].split("/")[-1] 396 | ) 397 | if dict is None: 398 | _merge_a_into_b(yaml_cfg, __C) 399 | else: 400 | _merge_a_into_b(yaml_cfg, dict) 401 | process_cfg(reset_model_spec=reset_model_spec) 402 | 403 | def save_cfg_to_file(filename, cfg): 404 | """Load a config file and merge it into the default options.""" 405 | with open(filename, 'w+') as f: 406 | yaml.dump(cfg, f, default_flow_style=False) 407 | 408 | def cfg_repr(cfg, fmt='plain'): 409 | def helper(d): 410 | ret = {} 411 | for k, v in d.items(): 412 | if isinstance(v, dict): 413 | ret[k] = helper(v) 414 | else: 415 | ret[k] = v 416 | return tabulate.tabulate(ret.items(), tablefmt=fmt) 417 | return helper(cfg) 418 | -------------------------------------------------------------------------------- /experiments/model_spec/rl_pointnet_model_spec.yaml: -------------------------------------------------------------------------------- 1 | 2 | traj_feature_extractor: 3 | class: GoalFeature 4 | opt: Adam 5 | opt_kwargs: 6 | lr: 1.0e-3 7 | scheduler: MultiStepLR 8 | scheduler_kwargs: 9 | milestones: [8000, 16000, 30000, 50000, 70000, 90000] 10 | gamma: 0.3 11 | 12 | 13 | state_feature_extractor: 14 | class: PointNetFeature 15 | net_kwargs: 16 | input_dim: 40 17 | extra_latent: 1 18 | opt: Adam 19 | opt_kwargs: 20 | lr: 1.0e-3 21 | scheduler: MultiStepLR 22 | scheduler_kwargs: 23 | milestones: [8000, 16000, 30000, 50000, 70000, 90000] 24 | gamma: 0.3 25 | 26 | 27 | -------------------------------------------------------------------------------- /experiments/model_spec/rl_sampler_large_pointnet_model_spec.yaml: -------------------------------------------------------------------------------- 1 | 2 | traj_feature_extractor: 3 | class: TrajEmbeddingNet 4 | net_kwargs: 5 | input_dim: 40 6 | extra_latent: 1 7 | large_feature: True 8 | num_actions: 6 9 | opt: Adam 10 | opt_kwargs: 11 | lr: 1.0e-3 12 | scheduler: MultiStepLR 13 | scheduler_kwargs: 14 | milestones: [20000, 40000, 60000, 80000, 100000] 15 | gamma: 0.3 16 | 17 | traj_feature_sampler: 18 | class: TrajSamplerNet 19 | net_kwargs: 20 | input_dim: 40 21 | extra_latent: 1 22 | large_feature: True 23 | num_actions: 6 24 | opt: Adam 25 | opt_kwargs: 26 | lr: 3.0e-4 27 | scheduler: MultiStepLR 28 | scheduler_kwargs: 29 | milestones: [20000, 40000, 60000, 80000, 100000] 30 | gamma: 0.3 31 | 32 | 33 | state_feature_extractor: 34 | class: PointNetTrajFeature 35 | net_kwargs: 36 | input_dim: 40 37 | extra_latent: 1 38 | large_feature: True 39 | opt: Adam 40 | opt_kwargs: 41 | lr: 1.0e-3 42 | scheduler: MultiStepLR 43 | scheduler_kwargs: 44 | milestones: [20000, 40000, 60000, 80000, 100000] 45 | gamma: 0.3 46 | 47 | 48 | -------------------------------------------------------------------------------- /experiments/object_index/extra_shape.json: -------------------------------------------------------------------------------- 1 | { 2 | "train": [ 3 | "box_box011_1.0.json", "box_box000_1.0.json", "box_box001_1.0.json", "box_box002_1.0.json", "box_box004_1.0.json", "box_box006_1.0.json", "box_box008_1.0.json", "box_box009_1.0.json", "box_box010_1.0.json", "box_box012_1.0.json", "box_box013_1.0.json", "box_box014_1.0.json", "box_box015_1.0.json", "box_box017_1.0.json", "box_box018_1.0.json", "box_box019_1.0.json", 4 | "cylinder_cylinder017_1.0.json", "cylinder_cylinder014_1.0.json", "cylinder_cylinder011_1.0.json", "cylinder_cylinder008_1.0.json", "cylinder_cylinder019_1.0.json", "cylinder_cylinder010_1.0.json", "cylinder_cylinder006_1.0.json", "cylinder_cylinder007_1.0.json", "cylinder_cylinder013_1.0.json", "cylinder_cylinder004_1.0.json", "cylinder_cylinder003_1.0.json", "cylinder_cylinder000_1.0.json", "cylinder_cylinder015_1.0.json", "cylinder_cylinder009_1.0.json", "cylinder_cylinder001_1.0.json", 5 | "Bowl_8eab5598b81afd7bab5b523beb03efcd_0.01534163.json", "Bowl_8e840ba109f252534c6955b188e290e0_0.0278882275.json", "Bowl_960c5c5bff2d3a4bbced73c51e99f8b2_0.020977083.json", "Bowl_4be4184845972fba5ea36d5a57a5a3bb_0.0006201346.json", "Bowl_594b22f21daf33ce6aea2f18ee404fd5_0.0013631187.json", "Bowl_68582543c4c6d0bccfdfe3f21f42a111_0.017394351.json", "Bowl_be3c2533130dd3da55f46d55537192b6_0.016554613.json", "Bowl_4b93a81c3534e56d19620b61f6587b3e_0.0107422066.json", 6 | "Bottle_244894af3ba967ccd957eaf7f4edb205_0.0472991972.json", "Bottle_15787789482f045d8add95bf56d3d2fa_0.0106996944.json", "Bottle_a86d587f38569fdf394a7890920ef7fd_0.0406087106.json", "Bottle_d7305324e9dd49eccee5e41d780064a2_0.0332494603.json", "Bottle_3108a736282eec1bc58e834f0b160845_0.0316568134.json", "Bottle_83fd5e88eca47f1dab298e30fc6f45ba_0.0011760663.json", "Bottle_dc0926ce09d6ce78eb8e919b102c6c08_0.0224658542.json", "Bottle_d74bc917899133e080c257afea181fa2_0.033104028.json" 7 | ], 8 | "test": [ 9 | "box_box011_1.0.json", "box_box000_1.0.json", "box_box001_1.0.json", "box_box002_1.0.json", "box_box004_1.0.json", "box_box006_1.0.json", "box_box008_1.0.json", "box_box009_1.0.json", "box_box010_1.0.json", "box_box012_1.0.json", "box_box013_1.0.json", "box_box014_1.0.json", "box_box015_1.0.json", "box_box017_1.0.json", "box_box018_1.0.json", "box_box019_1.0.json", 10 | "cylinder_cylinder017_1.0.json", "cylinder_cylinder014_1.0.json", "cylinder_cylinder011_1.0.json", "cylinder_cylinder008_1.0.json", "cylinder_cylinder019_1.0.json", "cylinder_cylinder010_1.0.json", "cylinder_cylinder006_1.0.json", "cylinder_cylinder007_1.0.json", "cylinder_cylinder013_1.0.json", "cylinder_cylinder004_1.0.json", "cylinder_cylinder003_1.0.json", "cylinder_cylinder000_1.0.json", "cylinder_cylinder015_1.0.json", "cylinder_cylinder009_1.0.json", "cylinder_cylinder001_1.0.json", 11 | "Bowl_8eab5598b81afd7bab5b523beb03efcd_0.01534163.json", "Bowl_8e840ba109f252534c6955b188e290e0_0.0278882275.json", "Bowl_960c5c5bff2d3a4bbced73c51e99f8b2_0.020977083.json", "Bowl_4be4184845972fba5ea36d5a57a5a3bb_0.0006201346.json", "Bowl_594b22f21daf33ce6aea2f18ee404fd5_0.0013631187.json", "Bowl_68582543c4c6d0bccfdfe3f21f42a111_0.017394351.json", "Bowl_be3c2533130dd3da55f46d55537192b6_0.016554613.json", "Bowl_4b93a81c3534e56d19620b61f6587b3e_0.0107422066.json", 12 | "Bottle_244894af3ba967ccd957eaf7f4edb205_0.0472991972.json", "Bottle_15787789482f045d8add95bf56d3d2fa_0.0106996944.json", "Bottle_a86d587f38569fdf394a7890920ef7fd_0.0406087106.json", "Bottle_d7305324e9dd49eccee5e41d780064a2_0.0332494603.json", "Bottle_3108a736282eec1bc58e834f0b160845_0.0316568134.json", "Bottle_83fd5e88eca47f1dab298e30fc6f45ba_0.0011760663.json", "Bottle_dc0926ce09d6ce78eb8e919b102c6c08_0.0224658542.json", "Bottle_d74bc917899133e080c257afea181fa2_0.033104028.json" 13 | ] 14 | } -------------------------------------------------------------------------------- /experiments/object_index/ycb_large.json: -------------------------------------------------------------------------------- 1 | {"test": ["003_cracker_box_1.0.json","004_sugar_box_1.0.json","005_tomato_soup_can_1.0.json","006_mustard_bottle_1.0.json","021_bleach_cleanser_1.0.json","010_potted_meat_can_1.0.json","024_bowl_1.0.json","025_mug_1.0.json","061_foam_brick_1.0.json"], "train": ["003_cracker_box_1.0.json","004_sugar_box_1.0.json","005_tomato_soup_can_1.0.json","006_mustard_bottle_1.0.json","021_bleach_cleanser_1.0.json","019_pitcher_base_1.0.json","010_potted_meat_can_1.0.json","024_bowl_1.0.json","025_mug_1.0.json","061_foam_brick_1.0.json"]} 2 | -------------------------------------------------------------------------------- /experiments/object_index/ycb_large_scene.json: -------------------------------------------------------------------------------- 1 | [ 2 | "025_mug", 3 | "006_mustard_bottle", 4 | "004_sugar_box", 5 | "021_bleach_cleanser", 6 | "025_mug", 7 | "019_pitcher_base", 8 | "025_mug", 9 | "024_bowl", 10 | "010_potted_meat_can", 11 | "024_bowl", 12 | "005_tomato_soup_can", 13 | "010_potted_meat_can", 14 | "024_bowl", 15 | "024_bowl", 16 | "010_potted_meat_can", 17 | "010_potted_meat_can", 18 | "061_foam_brick", 19 | "025_mug", 20 | "005_tomato_soup_can", 21 | "006_mustard_bottle", 22 | "005_tomato_soup_can", 23 | "025_mug", 24 | "025_mug", 25 | "005_tomato_soup_can", 26 | "010_potted_meat_can", 27 | "003_cracker_box", 28 | "010_potted_meat_can", 29 | "061_foam_brick", 30 | "061_foam_brick", 31 | "061_foam_brick", 32 | "003_cracker_box", 33 | "024_bowl", 34 | "010_potted_meat_can", 35 | "061_foam_brick", 36 | "004_sugar_box", 37 | "006_mustard_bottle" 38 | ] -------------------------------------------------------------------------------- /experiments/scripts/download_data.sh: -------------------------------------------------------------------------------- 1 | # Download the data 2 | wget --no-check-certificate -r 'https://drive.google.com/uc?export=download&id=136rLjyjFFRMyVxUZT6txB5XR2Ct_LNWC' -O data.zip 3 | echo "Data downloaded. Starting to unzip" 4 | unzip data.zip -d data 5 | rm data.zip 6 | -------------------------------------------------------------------------------- /experiments/scripts/download_model.sh: -------------------------------------------------------------------------------- 1 | # Download the pretrained model 2 | wget --no-check-certificate -r 'https://drive.google.com/uc?export=download&id=1oBfO4Xx0sSLyFMel6YDMQ5iQla6J8lIM' -O model_clutter.zip 3 | echo "HCG Model downloaded. Starting to unzip" 4 | mkdir output 5 | unzip -q model_clutter.zip -d ./ 6 | rm model_clutter.zip 7 | 8 | 9 | -------------------------------------------------------------------------------- /experiments/scripts/download_offline_data.sh: -------------------------------------------------------------------------------- 1 | # Download the example replay buffer 2 | wget --no-check-certificate -r 'https://drive.google.com/uc?export=download&id=1vdkgXMOjEo_CJDqAO3dlJAneb0KzbK26' -O data.zip 3 | echo "Data downloaded. Starting to unzip" 4 | unzip data.zip -d data/offline_data/ 5 | rm data.zip 6 | -------------------------------------------------------------------------------- /experiments/scripts/test_demo.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -x 3 | set -e 4 | 5 | DIR="$( cd "$( dirname -- "$0" )" && pwd )" 6 | export PYTHONUNBUFFERED="True" 7 | LOG_NAME="agent" 8 | 9 | MODEL_NAME=${1-"03_07_2020_16:10:32"} 10 | RUN_NUM=${2-1} 11 | EPI_NUM=${3-10} 12 | EPOCH=${4-latest} 13 | LOG="outputs/${MODEL_NAME}/test_log.txt.`date +'%Y-%m-%d_%H-%M-%S'`" 14 | exec &> >(tee -a "$LOG") 15 | echo Logging output to "$LOG" 16 | 17 | time python -m core.test_offline --pretrained output/demo_model_clutter --test --record --log \ 18 | --load_test_scene --render --load_goal --use_sample_latent --sample_latent_gap 12 --test_script_name $(basename $BASH_SOURCE) \ 19 | --test_episode_num ${EPI_NUM} --num_runs ${RUN_NUM} --model_surfix ${EPOCH} --critic_mpc --multi_traj_sample; 20 | -------------------------------------------------------------------------------- /experiments/scripts/test_ycb.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | KMP_SETTING="KMP_AFFINITY=granularity=fine,compact,1,0" 4 | KMP_BLOCKTIME=1 5 | 6 | export OMP_NUM_THREADS=1 7 | export $KMP_SETTING 8 | export KMP_BLOCKTIME=$KMP_BLOCKTIME 9 | 10 | set -x 11 | set -e 12 | 13 | DIR="$( cd "$( dirname -- "$0" )" && pwd )" 14 | export PYTHONPATH="${DIR}/../":"$PYTHONPATH" 15 | export PYTHONUNBUFFERED="True" 16 | 17 | MODEL_NAME=${1-"03_07_2020_16:10:32"} 18 | RUN_NUM=${2-2} 19 | EPI_NUM=${3-300} 20 | EPOCH=${4-latest} 21 | LOG="output/${MODEL_NAME}/test_log.txt.`date +'%Y-%m-%d_%H-%M-%S'`" 22 | exec &> >(tee -a "$LOG") 23 | echo Logging output to "$LOG" 24 | 25 | time python -m core.test_offline --pretrained output/${MODEL_NAME} --test --record --log \ 26 | --load_test_scene --egl --load_goal --test_script_name $(basename $BASH_SOURCE) \ 27 | --test_episode_num ${EPI_NUM} --num_runs ${RUN_NUM} --model_surfix ${EPOCH} ; 28 | -------------------------------------------------------------------------------- /experiments/scripts/test_ycb_gen_12_mpc.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | KMP_SETTING="KMP_AFFINITY=granularity=fine,compact,1,0" 4 | set -x 5 | set -e 6 | 7 | DIR="$( cd "$( dirname -- "$0" )" && pwd )" 8 | export PYTHONUNBUFFERED="True" 9 | LOG_NAME="agent" 10 | 11 | MODEL_NAME=${1-"03_07_2020_16:10:32"} 12 | RUN_NUM=${2-2} 13 | EPI_NUM=${3-300} 14 | EPOCH=${4-latest} 15 | LOG="outputs/${MODEL_NAME}/test_log.txt.`date +'%Y-%m-%d_%H-%M-%S'`" 16 | exec &> >(tee -a "$LOG") 17 | echo Logging output to "$LOG" 18 | 19 | time python -m core.test_offline --pretrained output/${MODEL_NAME} --test --record --log \ 20 | --load_test_scene --egl --load_goal --use_sample_latent --sample_latent_gap 12 --test_script_name $(basename $BASH_SOURCE) \ 21 | --test_episode_num ${EPI_NUM} --num_runs ${RUN_NUM} --model_surfix ${EPOCH} --critic_mpc --multi_traj_sample; 22 | -------------------------------------------------------------------------------- /experiments/scripts/train_embedding_offline.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -x 3 | set -e 4 | 5 | DIR="$( cd "$( dirname -- "$0" )" && pwd )" 6 | export PYTHONUNBUFFERED="True" 7 | LOG_NAME="agent" 8 | LOG="output_misc/logs/$LOG_NAME.txt.`date +'%Y-%m-%d_%H-%M-%S'`" 9 | 10 | exec &> >(tee -a "$LOG") 11 | echo Logging output to "$LOG" 12 | SCRIPT_NAME=${1-"ddpg_finetune.yaml"} 13 | POLICY_NAME=${2-"BC"} 14 | PRETRAINED=${3-"" } 15 | MODEL_NAME=${4-"`date +'%d_%m_%Y_%H:%M:%S'`"} 16 | SURFIX=${5-"latest"} 17 | 18 | time python -m core.train_offline --save_model \ 19 | --policy ${POLICY_NAME} --log --fix_output_time ${MODEL_NAME} \ 20 | --pretrained output/${PRETRAINED} \ 21 | --config_file ${SCRIPT_NAME} 22 | 23 | bash ./experiments/scripts/test_ycb.sh ${MODEL_NAME} -------------------------------------------------------------------------------- /experiments/scripts/train_embedding_ray_offline.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -x 3 | set -e 4 | 5 | DIR="$( cd "$( dirname -- "$0" )" && pwd )" 6 | export PYTHONUNBUFFERED="True" 7 | LOG_NAME="agent" 8 | LOG="output_misc/logs/$LOG_NAME.txt.`date +'%Y-%m-%d_%H-%M-%S'`" 9 | 10 | exec &> >(tee -a "$LOG") 11 | echo Logging output to "$LOG" 12 | SCRIPT_NAME=${1-"ddpg_finetune.yaml"} 13 | POLICY_NAME=${2-"BC"} 14 | PRETRAINED=${3-"" } 15 | MODEL_NAME=${4-"`date +'%d_%m_%Y_%H:%M:%S'`"} 16 | SURFIX=${5-"latest"} 17 | 18 | 19 | time python -m core.train_offline --save_model \ 20 | --policy ${POLICY_NAME} --log --fix_output_time ${MODEL_NAME} \ 21 | --use_ray --pretrained output/${PRETRAINED} \ 22 | --config_file ${SCRIPT_NAME} 23 | 24 | bash ./experiments/scripts/test_ycb.sh ${MODEL_NAME} -------------------------------------------------------------------------------- /experiments/scripts/train_ray_online_save_data.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -x 3 | set -e 4 | 5 | DIR="$( cd "$( dirname -- "$0" )" && pwd )" 6 | export PYTHONUNBUFFERED="True" 7 | LOG_NAME="agent" 8 | LOG="output_misc/logs/$LOG_NAME.txt.`date +'%Y-%m-%d_%H-%M-%S'`" 9 | exec &> >(tee -a "$LOG") 10 | echo Logging output to "$LOG" 11 | SCRIPT_NAME=${1-"ddpg_finetune.yaml"} 12 | POLICY_NAME=${2-"BC"} 13 | PRETRAINED=${3-"" } # 14 | MODEL_NAME=${4-"`date +'%d_%m_%Y_%H:%M:%S'`" } 15 | 16 | time python -m core.train_online --save_model \ 17 | --policy ${POLICY_NAME} --log --fix_output_time ${MODEL_NAME} \ 18 | --save_buffer --finetune \ 19 | --config_file ${SCRIPT_NAME} --load_buffer --load_online_buffer --visdom 20 | 21 | -------------------------------------------------------------------------------- /experiments/scripts/train_sampler_ray_online.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -x 3 | set -e 4 | 5 | DIR="$( cd "$( dirname -- "$0" )" && pwd )" 6 | export PYTHONUNBUFFERED="True" 7 | LOG_NAME="agent" 8 | LOG="output_misc/logs/$LOG_NAME.txt.`date +'%Y-%m-%d_%H-%M-%S'`" 9 | exec &> >(tee -a "$LOG") 10 | echo Logging output to "$LOG" 11 | SCRIPT_NAME=${1-"ddpg_finetune.yaml"} 12 | POLICY_NAME=${2-"BC"} 13 | PRETRAINED=${3-"" } # 14 | MODEL_NAME=${4-"`date +'%d_%m_%Y_%H:%M:%S'`" } 15 | SURFIX=${5-"latest"} 16 | 17 | time python -m core.train_online --save_model \ 18 | --policy ${POLICY_NAME} --log --fix_output_time ${MODEL_NAME} \ 19 | --finetune --pretrained output/${PRETRAINED} \ 20 | --config_file ${SCRIPT_NAME} --load_buffer --load_online_buffer --visdom 21 | 22 | bash ./experiments/scripts/test_ycb_gen_12_mpc.sh ${MODEL_NAME} -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | GPUtil==1.4.0 2 | ray==0.8.4 3 | tabulate==0.8.6 4 | tensorboardX==1.7 5 | visdom==0.1.8.8 6 | psutil 7 | pybullet 8 | gym 9 | numpy 10 | torchvision==0.5.0 11 | torch==1.4.0 12 | transforms3d 13 | opencv-python==3.4.3.18 14 | IPython 15 | matplotlib 16 | easydict 17 | pyyaml 18 | cmake 19 | future 20 | transforms3d 21 | scipy 22 | IPython 23 | --------------------------------------------------------------------------------