├── .gitignore ├── LICENSE ├── README.md ├── evaluate.py ├── main.py ├── models ├── __init__.py ├── denseunet.py ├── grconvnet.py ├── grconvnet3.py ├── grconvnet4.py └── network.py ├── requirements.txt ├── robot.py ├── simulation ├── __init__.py ├── objects │ ├── blocks │ │ ├── 0.obj │ │ ├── 1.obj │ │ ├── 2.obj │ │ ├── 3.obj │ │ ├── 4.obj │ │ ├── 5.obj │ │ ├── 6.obj │ │ ├── 7.obj │ │ └── 8.obj │ └── mixed_shapes │ │ ├── 0.obj │ │ ├── 1.obj │ │ ├── 2.obj │ │ ├── 3.obj │ │ ├── 4.obj │ │ ├── 6.obj │ │ ├── 7.obj │ │ └── 8.obj ├── remoteApi.dylib ├── sim.py ├── simConst.py ├── simulation.ttt └── test-cases │ ├── test-10-obj-00.txt │ ├── test-10-obj-01.txt │ ├── test-10-obj-02.txt │ ├── test-10-obj-03.txt │ ├── test-10-obj-04.txt │ ├── test-10-obj-05.txt │ ├── test-10-obj-06.txt │ ├── test-10-obj-07.txt │ ├── test-10-obj-08.txt │ ├── test-10-obj-09.txt │ └── test-10-obj-10.txt ├── trainer.py └── utils ├── __init__.py ├── logger.py ├── utils.py └── viz.py /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | pip-wheel-metadata/ 24 | share/python-wheels/ 25 | *.egg-info/ 26 | .installed.cfg 27 | *.egg 28 | MANIFEST 29 | 30 | # PyInstaller 31 | # Usually these files are written by a python script from a template 32 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 33 | *.manifest 34 | *.spec 35 | 36 | # Installer logs 37 | pip-log.txt 38 | pip-delete-this-directory.txt 39 | 40 | # Unit test / coverage reports 41 | htmlcov/ 42 | .tox/ 43 | .nox/ 44 | .coverage 45 | .coverage.* 46 | .cache 47 | nosetests.xml 48 | coverage.xml 49 | *.cover 50 | *.py,cover 51 | .hypothesis/ 52 | .pytest_cache/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | target/ 76 | 77 | # Jupyter Notebook 78 | .ipynb_checkpoints 79 | 80 | # IPython 81 | profile_default/ 82 | ipython_config.py 83 | 84 | # pyenv 85 | .python-version 86 | 87 | # pipenv 88 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 89 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 90 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 91 | # install all needed dependencies. 92 | #Pipfile.lock 93 | 94 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 95 | __pypackages__/ 96 | 97 | # Celery stuff 98 | celerybeat-schedule 99 | celerybeat.pid 100 | 101 | # SageMath parsed files 102 | *.sage.py 103 | 104 | # Environments 105 | .env 106 | .venv 107 | env/ 108 | venv/ 109 | ENV/ 110 | env.bak/ 111 | venv.bak/ 112 | 113 | # Spyder project settings 114 | .spyderproject 115 | .spyproject 116 | 117 | # Rope project settings 118 | .ropeproject 119 | 120 | # mkdocs documentation 121 | /site 122 | 123 | # mypy 124 | .mypy_cache/ 125 | .dmypy.json 126 | dmypy.json 127 | 128 | # Pyre type checker 129 | .pyre/ 130 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 2-Clause License 2 | 3 | Copyright (c) 2021, Sulabh Kumra 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # RoManNet 2 | 3 | This repository contains the implementation of the Robotic Manipulation Network (RoManNet) which is a vision-based model architecture to learn the action-value functions and predict manipulation action candidates. More details can be found in the following paper: 4 | 5 | #### Learning Robotic Manipulation Tasks via Task Progress Based Gaussian Reward and Loss Adjusted Exploration 6 | 7 | Sulabh Kumra, Shirin Joshi, Ferat Sahin 8 | 9 | [Xplore](https://ieeexplore.ieee.org/abstract/document/9625869) | [arxiv](https://arxiv.org/abs/2103.01434) 10 | 11 | If you use this project in your research or wish to refer to the baseline results published in the paper, please use the following BibTeX entry: 12 | ``` 13 | @article{kumra2022learning, 14 | author={Kumra, Sulabh and Joshi, Shirin and Sahin, Ferat}, 15 | journal={IEEE Robotics and Automation Letters}, 16 | title={Learning Robotic Manipulation Tasks via Task Progress Based Gaussian Reward and Loss Adjusted Exploration}, 17 | year={2022}, 18 | volume={7}, 19 | number={1}, 20 | pages={534-541}, 21 | doi={10.1109/LRA.2021.3129833} 22 | } 23 | ``` 24 | 25 | ## Installation 26 | - Checkout the repository 27 | ```bash 28 | $ git clone https://github.com/skumra/romannet.git 29 | ``` 30 | 31 | - Create a virtual environment 32 | ```bash 33 | $ python3.6 -m venv --system-site-packages venv 34 | ``` 35 | 36 | - Source the virtual environment 37 | ```bash 38 | $ source venv/bin/activate 39 | ``` 40 | 41 | - Install the requirements 42 | ```bash 43 | $ cd romannet 44 | $ pip install -r requirements.txt 45 | ``` 46 | 47 | - Install the [CoppeliaSim](http://www.coppeliarobotics.com/) simulation environment 48 | 49 | 50 | ## Usage 51 | - Run CoppeliaSim (navigate to your CoppeliaSim directory and run `./sim.sh`). From the main menu, select `File` > `Open scene`..., and open the file `romannet/simulation/simulation.ttt` from this repository. 52 | 53 | - In another terminal window, run the following: 54 | ```bash 55 | python main.py 56 | ``` 57 | 58 | Note: Various training/testing options can be modified or toggled on/off with different flags (run `python main.py -h` to see all options) 59 | 60 | ### Acknowledgement 61 | Some parts of the code and simulation environment has been borrowed from [andyzeng/visual-pushing-grasping](https://github.com/andyzeng/visual-pushing-grasping) for fair comparison of our work in simulation. 62 | -------------------------------------------------------------------------------- /evaluate.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import argparse 4 | import logging 5 | import os 6 | import re 7 | 8 | import numpy as np 9 | 10 | 11 | def evaluate(session_directory, num_obj_complete): 12 | # Parse data from session (action executed, reward values) 13 | transitions_directory = os.path.join(session_directory, 'transitions') 14 | executed_action_log = np.loadtxt(os.path.join(transitions_directory, 'executed-action.log.txt'), delimiter=' ') 15 | max_iteration = executed_action_log.shape[0] 16 | executed_action_log = executed_action_log[0:max_iteration, :] 17 | reward_value_log = np.loadtxt(os.path.join(transitions_directory, 'reward-value.log.txt'), delimiter=' ') 18 | reward_value_log = reward_value_log[0:max_iteration] 19 | clearance_log = np.loadtxt(os.path.join(transitions_directory, 'task_complete.log.txt'), delimiter=' ') 20 | clearance_log = np.unique(clearance_log) 21 | max_trials = len(clearance_log) 22 | clearance_log = np.concatenate((np.asarray([0]), clearance_log), axis=0).astype(int) 23 | 24 | # Count number of pushing/grasping actions before completion 25 | num_actions_before_completion = clearance_log[1:(max_trials + 1)] - clearance_log[0:(max_trials)] 26 | 27 | grasp_success_rate = np.zeros(max_trials) 28 | grasp_num_success = np.zeros(max_trials) 29 | grasp_to_push_ratio = np.zeros(max_trials) 30 | for trial_idx in range(1, len(clearance_log)): 31 | # Get actions and reward values for current trial 32 | tmp_executed_action_log = executed_action_log[clearance_log[trial_idx - 1]:clearance_log[trial_idx], 0] 33 | tmp_reward_value_log = reward_value_log[clearance_log[trial_idx - 1]:clearance_log[trial_idx]] 34 | 35 | # Get indices of pushing and grasping actions for current trial 36 | tmp_grasp_attempt_ind = np.argwhere(tmp_executed_action_log == 1) 37 | tmp_push_attempt_ind = np.argwhere(tmp_executed_action_log == 0) 38 | 39 | grasp_to_push_ratio[trial_idx - 1] = float(len(tmp_grasp_attempt_ind)) / float(len(tmp_executed_action_log)) 40 | 41 | # Count number of times grasp attempts were successful 42 | # Reward value for successful grasping is anything larger than 0.5 43 | tmp_num_grasp_success = np.sum(tmp_reward_value_log[ 44 | tmp_grasp_attempt_ind] >= 0.5) 45 | 46 | grasp_num_success[trial_idx - 1] = tmp_num_grasp_success 47 | grasp_success_rate[trial_idx - 1] = float(tmp_num_grasp_success) / float(len(tmp_grasp_attempt_ind)) 48 | 49 | # Which trials reached task completion? 50 | valid_clearance = grasp_num_success >= num_obj_complete 51 | 52 | # Display results 53 | clearance = float(np.sum(valid_clearance)) / float(max_trials) * 100 54 | logging.info('Average %% clearance: %2.1f' % clearance) 55 | 56 | grasp_success = np.mean(grasp_success_rate[valid_clearance]) * 100 57 | logging.info('Average %% grasp success per clearance: %2.1f' % grasp_success) 58 | 59 | action_efficiency = 100 * np.mean( 60 | np.divide(float(num_obj_complete), num_actions_before_completion[valid_clearance])) 61 | logging.info('Average %% action efficiency: %2.1f' % action_efficiency) 62 | 63 | grasp_to_push_ratio = np.mean(grasp_to_push_ratio[valid_clearance]) * 100 64 | logging.info('Average grasp to push ratio: %2.1f' % grasp_to_push_ratio) 65 | 66 | return clearance, grasp_success, action_efficiency, grasp_to_push_ratio 67 | 68 | 69 | if __name__ == '__main__': 70 | parser = argparse.ArgumentParser(description='Evaluate the performance all test sessions.') 71 | parser.add_argument('--session_directory', dest='session_directory', action='store', type=str, 72 | help='path to session directory for which to measure performance') 73 | parser.add_argument('--test_type', dest='test_type', action='store', type=str, default='preset', 74 | help='type of test to evaluate. (random/preset)') 75 | parser.add_argument('--num_obj_complete', dest='num_obj_complete', action='store', type=int, default=10, 76 | help='number of objects picked before considering task complete') 77 | args = parser.parse_args() 78 | 79 | # Initialize logging 80 | logging.root.handlers = [] 81 | logging.basicConfig( 82 | level=logging.INFO, 83 | filename="{0}/{1}.log".format(args.session_directory, 'evaluate'), 84 | format='[%(asctime)s] {%(pathname)s:%(lineno)d} %(levelname)s - %(message)s', 85 | datefmt='%H:%M:%S' 86 | ) 87 | # set up logging to console 88 | console = logging.StreamHandler() 89 | console.setLevel(logging.DEBUG) 90 | # set a format which is simpler for console use 91 | formatter = logging.Formatter('%(name)-12s: %(levelname)-8s %(message)s') 92 | console.setFormatter(formatter) 93 | # add the handler to the root logger 94 | logging.getLogger('').addHandler(console) 95 | 96 | if args.test_type == 'random': 97 | session_directory = args.session_directory 98 | num_obj_complete = args.num_obj_complete 99 | evaluate(session_directory, num_obj_complete) 100 | elif args.test_type == 'preset': 101 | num_obj_presets = [4, 5, 3, 5, 5, 6, 3, 6, 6, 5, 4] 102 | preset_files = os.listdir(args.session_directory) 103 | preset_files = [os.path.abspath(os.path.join(args.session_directory, filename)) for filename in preset_files if 104 | os.path.isdir(os.path.join(args.session_directory, filename))] 105 | preset_files = sorted(preset_files) 106 | 107 | avg_clearance = 0 108 | avg_grasp_success = 0 109 | avg_action_efficiency = 0 110 | avg_grasp_to_push_ratio = 0 111 | valid_clearance_count = 0 112 | complete_clearance_count = 0 113 | 114 | for idx, preset_file in enumerate(preset_files): 115 | logging.info('Preset {}: {}'.format(idx, preset_file)) 116 | session_directory = preset_file 117 | m = re.search(r'\d+$', session_directory) 118 | num_obj_complete = num_obj_presets[int(m.group())] 119 | clearance, grasp_success, action_efficiency, grasp_to_push_ratio = evaluate(session_directory, 120 | num_obj_complete) 121 | avg_clearance += clearance 122 | if clearance: 123 | avg_grasp_success += grasp_success 124 | avg_action_efficiency += action_efficiency 125 | avg_grasp_to_push_ratio += grasp_to_push_ratio 126 | valid_clearance_count += 1 127 | if clearance == 100: 128 | complete_clearance_count += 1 129 | 130 | logging.info('Summary') 131 | logging.info('Scenarios 100 %% complete: %d' % complete_clearance_count) 132 | logging.info('Overall average %% clearance: %2.1f' % (avg_clearance / (idx + 1))) 133 | logging.info('Overall average %% grasp success per clearance: %2.1f' % (avg_grasp_success / valid_clearance_count)) 134 | logging.info('Overall average %% action efficiency: %2.1f' % (avg_action_efficiency / valid_clearance_count)) 135 | logging.info('Overall average grasp to push ratio: %2.1f' % (avg_grasp_to_push_ratio / valid_clearance_count)) 136 | else: 137 | raise NotImplementedError('Test type {} is not implemented'.format(args.test_type)) 138 | -------------------------------------------------------------------------------- /main.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import argparse 4 | import concurrent.futures 5 | import logging 6 | import os 7 | import re 8 | import threading 9 | import time 10 | 11 | import cv2 12 | import numpy as np 13 | import tensorboardX 14 | import torch 15 | from scipy import ndimage 16 | 17 | from robot import SimRobot 18 | from trainer import Trainer 19 | from utils import utils, viz 20 | from utils.logger import Logger 21 | 22 | 23 | class LearnManipulation: 24 | def __init__(self, args): 25 | # --------------- Setup options --------------- 26 | self.is_sim = args.is_sim 27 | sim_port = args.sim_port 28 | obj_mesh_dir = os.path.abspath(args.obj_mesh_dir) if self.is_sim else None 29 | num_obj = args.num_obj if self.is_sim else None 30 | # Cols: min max, Rows: x y z (define workspace limits in robot coordinates) 31 | if self.is_sim: 32 | self.workspace_limits = np.asarray([[-0.724, -0.276], [-0.224, 0.224], [-0.0001, 0.8]]) 33 | else: 34 | self.workspace_limits = np.asarray([[0.3, 0.748], [-0.224, 0.224], [-0.255, -0.1]]) 35 | self.heightmap_resolution = args.heightmap_resolution 36 | random_seed = args.random_seed 37 | force_cpu = args.force_cpu 38 | 39 | # ------------- Algorithm options ------------- 40 | network = args.network 41 | num_rotations = args.num_rotations 42 | self.future_reward_discount = args.future_reward_discount 43 | self.explore_actions = args.explore_actions 44 | self.explore_type = args.explore_type 45 | self.explore_rate_decay = args.explore_rate_decay 46 | self.LAE_sigma = 0.33 47 | self.LAE_beta = 0.25 48 | self.experience_replay_disabled = args.experience_replay_disabled 49 | self.push_enabled = args.push_enabled 50 | self.place_enabled = args.place_enabled 51 | self.max_iter = args.max_iter 52 | self.reward_type = args.reward_type 53 | self.filter_type = args.filter_type 54 | self.place_reward_scale = args.place_reward_scale 55 | self.goal_stack_height = args.goal_stack_height 56 | 57 | # -------------- Testing options -------------- 58 | self.is_testing = args.is_testing 59 | self.max_test_trials = args.max_test_trials 60 | test_preset_cases = args.test_preset_cases 61 | test_preset_file = os.path.abspath(args.test_preset_file) if test_preset_cases else None 62 | 63 | # ------ Pre-loading and logging options ------ 64 | if args.logging_directory and not args.snapshot_file: 65 | logging_directory = os.path.abspath(args.logging_directory) 66 | self.snapshot_file = os.path.join(logging_directory, 'models/snapshot-backup.pth') 67 | elif args.snapshot_file: 68 | logging_directory = os.path.abspath(args.logging_directory) 69 | self.snapshot_file = os.path.abspath(args.snapshot_file) 70 | else: 71 | logging_directory = None 72 | self.snapshot_file = None 73 | 74 | self.save_visualizations = args.save_visualizations 75 | 76 | # Initialize pick-and-place system (camera and robot) 77 | if self.is_sim: 78 | self.robot = SimRobot(sim_port, obj_mesh_dir, num_obj, self.workspace_limits, self.is_testing, 79 | test_preset_cases, test_preset_file, self.place_enabled) 80 | else: 81 | raise NotImplementedError 82 | 83 | # Initialize data logger 84 | self.logger = Logger(logging_directory, args) 85 | self.logger.save_camera_info(self.robot.cam_intrinsics, self.robot.cam_pose, self.robot.cam_depth_scale) 86 | self.logger.save_heightmap_info(self.workspace_limits, self.heightmap_resolution) 87 | 88 | # Tensorboard 89 | self.tb = tensorboardX.SummaryWriter(logging_directory) 90 | 91 | # Initialize trainer 92 | self.trainer = Trainer(network, force_cpu, self.push_enabled, self.place_enabled, num_rotations) 93 | 94 | # Find last executed iteration of pre-loaded log, and load execution info and RL variables 95 | if self.logger.logging_directory_exists and not self.is_testing: 96 | self.trainer.preload(self.logger.transitions_directory) 97 | self.trainer.load_snapshot(self.snapshot_file) 98 | elif args.snapshot_file: 99 | self.trainer.load_snapshot(self.snapshot_file) 100 | 101 | # Set random seed 102 | np.random.seed(random_seed) 103 | 104 | # Initialize variables for heuristic bootstrapping and exploration probability 105 | self.no_change_count = [2, 2] if not self.is_testing else [0, 0] 106 | self.explore_prob = 0.5 if not self.is_testing else 0.0 107 | 108 | self.mission_complete = False 109 | self.execute_action = False 110 | self.shutdown_called = False 111 | 112 | self.prev_primitive_action = None 113 | self.prev_grasp_success = None 114 | self.prev_push_success = None 115 | self.prev_place_success = None 116 | self.prev_color_heightmap = None 117 | self.prev_depth_heightmap = None 118 | self.prev_best_pix_ind = None 119 | self.prev_stack_height = 0 120 | self.last_task_complete = 0 121 | 122 | self.push_predictions = None 123 | self.grasp_predictions = None 124 | self.place_predictions = None 125 | self.color_heightmap = None 126 | self.depth_heightmap = None 127 | self.primitive_action = None 128 | self.best_pix_ind = None 129 | self.predicted_value = None 130 | 131 | def policy(self): 132 | """ 133 | Determine whether grasping or pushing or placing should be executed based on network predictions 134 | """ 135 | best_push_conf = np.max(self.push_predictions) 136 | best_grasp_conf = np.max(self.grasp_predictions) 137 | best_place_conf = np.max(self.place_predictions) 138 | logging.info('Primitive confidence scores: %f (push), %f (grasp), %f (place)' % ( 139 | best_push_conf, best_grasp_conf, best_place_conf)) 140 | 141 | # Exploitation (do best action) vs exploration (do other action) 142 | if self.explore_actions and not self.is_testing: 143 | explore_actions = np.random.uniform() < self.explore_prob 144 | logging.info('Strategy: explore (exploration probability: %f)' % self.explore_prob) 145 | else: 146 | explore_actions = False 147 | 148 | self.trainer.is_exploit_log.append([0 if explore_actions else 1]) 149 | self.logger.write_to_log('is-exploit', self.trainer.is_exploit_log) 150 | 151 | # Select action type 152 | self.primitive_action = 'grasp' 153 | if self.place_enabled and self.prev_primitive_action == 'grasp' and self.prev_grasp_success: 154 | self.primitive_action = 'place' 155 | elif self.push_enabled: 156 | if best_push_conf > best_grasp_conf: 157 | self.primitive_action = 'push' 158 | if explore_actions: 159 | self.primitive_action = 'push' if np.random.randint(0, 2) == 0 else 'grasp' 160 | 161 | # Get pixel location and rotation with highest affordance prediction (rotation, y, x) 162 | if self.primitive_action == 'push': 163 | self.compute_action(explore_actions, self.push_predictions) 164 | elif self.primitive_action == 'grasp': 165 | self.compute_action(explore_actions, self.grasp_predictions) 166 | elif self.primitive_action == 'place': 167 | self.compute_action(explore_actions, self.place_predictions) 168 | else: 169 | raise NotImplementedError('Primitive action type {} is not implemented'.format(self.primitive_action)) 170 | 171 | # Save predicted confidence value 172 | self.trainer.predicted_value_log.append([self.predicted_value]) 173 | self.logger.write_to_log('predicted-value', self.trainer.predicted_value_log) 174 | 175 | def compute_action(self, explore_actions, predictions): 176 | if explore_actions: 177 | maximas = utils.k_largest_index_argpartition(predictions, k=10) 178 | self.best_pix_ind = maximas[np.random.choice(maximas.shape[0])] 179 | else: 180 | self.best_pix_ind = np.unravel_index(np.argmax(predictions), predictions.shape) 181 | 182 | self.predicted_value = predictions[self.best_pix_ind[0], self.best_pix_ind[1], self.best_pix_ind[2]] 183 | 184 | def agent(self): 185 | """ 186 | Parallel thread to process network output and execute actions 187 | """ 188 | while not self.shutdown_called and self.trainer.iteration <= self.max_iter: 189 | if self.execute_action: 190 | # Select action based on policy 191 | self.policy() 192 | 193 | # Compute 3D position of pixel 194 | logging.info( 195 | 'Action: %s at (%d, %d, %d)' % ( 196 | self.primitive_action, self.best_pix_ind[0], self.best_pix_ind[1], self.best_pix_ind[2])) 197 | best_rotation_angle = np.deg2rad(self.best_pix_ind[0] * (360.0 / self.trainer.model.num_rotations)) 198 | best_pix_x = self.best_pix_ind[2] 199 | best_pix_y = self.best_pix_ind[1] 200 | primitive_position = [best_pix_x * self.heightmap_resolution + self.workspace_limits[0][0], 201 | best_pix_y * self.heightmap_resolution + self.workspace_limits[1][0], 202 | self.depth_heightmap[best_pix_y][best_pix_x] + self.workspace_limits[2][0]] 203 | 204 | # If pushing, adjust start position, and make sure z value is safe and not too low 205 | if self.primitive_action == 'push' or self.primitive_action == 'place': 206 | finger_width = 0.02 207 | safe_kernel_width = int(np.round((finger_width / 2) / self.heightmap_resolution)) 208 | local_region = self.depth_heightmap[ 209 | max(best_pix_y - safe_kernel_width, 0):min(best_pix_y + safe_kernel_width + 1, 210 | self.depth_heightmap.shape[0]), 211 | max(best_pix_x - safe_kernel_width, 0):min(best_pix_x + safe_kernel_width + 1, 212 | self.depth_heightmap.shape[1])] 213 | if local_region.size == 0: 214 | safe_z_position = self.workspace_limits[2][0] 215 | else: 216 | safe_z_position = np.max(local_region) + self.workspace_limits[2][0] 217 | primitive_position[2] = safe_z_position 218 | 219 | # Save executed primitive 220 | if self.primitive_action == 'push': 221 | self.trainer.executed_action_log.append( 222 | [0, self.best_pix_ind[0], self.best_pix_ind[1], self.best_pix_ind[2]]) # 0 - push 223 | elif self.primitive_action == 'grasp': 224 | self.trainer.executed_action_log.append( 225 | [1, self.best_pix_ind[0], self.best_pix_ind[1], self.best_pix_ind[2]]) # 1 - grasp 226 | elif self.primitive_action == 'place': 227 | self.trainer.executed_action_log.append( 228 | [2, self.best_pix_ind[0], self.best_pix_ind[1], self.best_pix_ind[2]]) # 2 - place 229 | self.logger.write_to_log('executed-action', self.trainer.executed_action_log) 230 | 231 | # Visualize executed primitive, and affordances 232 | grasp_pred_vis = viz.get_prediction_vis(self.grasp_predictions, self.color_heightmap, 233 | self.best_pix_ind, 'grasp') 234 | imgs = torch.from_numpy(grasp_pred_vis).permute(2, 0, 1) 235 | self.tb.add_image('grasp_pred', imgs, self.trainer.iteration) 236 | 237 | # grasp_pred_vis = viz.get_prediction_full_vis(self.grasp_predictions, self.color_heightmap, self.best_pix_ind) 238 | # imgs = torch.from_numpy(grasp_pred_vis).permute(2, 0, 1) 239 | # self.tb.add_image('grasp_pred_full', imgs, self.trainer.iteration) 240 | 241 | if self.push_enabled: 242 | push_pred_vis = viz.get_prediction_vis(self.push_predictions, self.color_heightmap, 243 | self.best_pix_ind, 'push') 244 | imgs = torch.from_numpy(push_pred_vis).permute(2, 0, 1) 245 | self.tb.add_image('push_pred', imgs, self.trainer.iteration) 246 | 247 | if self.place_enabled: 248 | place_pred_vis = viz.get_prediction_vis(self.place_predictions, self.color_heightmap, 249 | self.best_pix_ind, 'place') 250 | imgs = torch.from_numpy(place_pred_vis).permute(2, 0, 1) 251 | self.tb.add_image('place_pred', imgs, self.trainer.iteration) 252 | 253 | if self.save_visualizations: 254 | if self.primitive_action == 'push': 255 | self.logger.save_visualizations(self.trainer.iteration, push_pred_vis, 'push') 256 | elif self.primitive_action == 'grasp': 257 | self.logger.save_visualizations(self.trainer.iteration, grasp_pred_vis, 'grasp') 258 | elif self.primitive_action == 'place': 259 | self.logger.save_visualizations(self.trainer.iteration, place_pred_vis, 'place') 260 | 261 | # Initialize variables that influence reward 262 | push_success = False 263 | grasp_success = False 264 | place_success = False 265 | 266 | # Execute primitive 267 | pool = concurrent.futures.ThreadPoolExecutor() 268 | try: 269 | if self.primitive_action == 'push': 270 | future = pool.submit(self.robot.push, primitive_position, best_rotation_angle) 271 | push_success = future.result(timeout=60) 272 | logging.info('Push successful: %r' % push_success) 273 | elif self.primitive_action == 'grasp': 274 | future = pool.submit(self.robot.grasp, primitive_position, best_rotation_angle) 275 | grasp_success = future.result(timeout=60) 276 | logging.info('Grasp successful: %r' % grasp_success) 277 | elif self.primitive_action == 'place': 278 | future = pool.submit(self.robot.place, primitive_position, best_rotation_angle) 279 | place_success = future.result(timeout=60) 280 | logging.info('Place successful: %r' % place_success) 281 | except concurrent.futures.TimeoutError: 282 | logging.error('Robot execution timeout!') 283 | self.mission_complete = False 284 | else: 285 | self.mission_complete = True 286 | 287 | # Save information for next training step 288 | self.prev_color_heightmap = self.color_heightmap.copy() 289 | self.prev_depth_heightmap = self.depth_heightmap.copy() 290 | self.prev_grasp_success = grasp_success 291 | self.prev_push_success = push_success 292 | self.prev_place_success = place_success 293 | self.prev_primitive_action = self.primitive_action 294 | self.prev_best_pix_ind = self.best_pix_ind 295 | 296 | self.execute_action = False 297 | else: 298 | time.sleep(0.1) 299 | 300 | def compute_reward(self, change_detected, stack_height): 301 | # Compute current reward 302 | current_reward = 0 303 | if self.prev_primitive_action == 'push' and self.prev_push_success: 304 | if change_detected: 305 | if self.reward_type == 3: 306 | current_reward = 0.75 307 | else: 308 | current_reward = 0.5 309 | else: 310 | self.prev_push_success = False 311 | elif self.prev_primitive_action == 'grasp' and self.prev_grasp_success: 312 | if self.reward_type < 4: 313 | if (self.place_enabled and stack_height >= self.prev_stack_height) or (not self.place_enabled): 314 | current_reward = 1.0 315 | else: 316 | self.prev_grasp_success = False 317 | elif self.reward_type == 4: 318 | if self.place_enabled: 319 | if stack_height >= self.prev_stack_height: 320 | current_reward = 1.0 321 | else: 322 | self.prev_grasp_success = False 323 | current_reward = -0.5 324 | else: 325 | current_reward = 1.0 326 | elif self.prev_primitive_action == 'place' and self.prev_place_success: 327 | if stack_height > self.prev_stack_height: 328 | current_reward = self.place_reward_scale * stack_height 329 | else: 330 | self.prev_place_success = False 331 | 332 | # Compute future reward 333 | if self.place_enabled and not change_detected and not self.prev_grasp_success and not self.prev_place_success: 334 | future_reward = 0 335 | elif not self.place_enabled and not change_detected and not self.prev_grasp_success: 336 | future_reward = 0 337 | elif self.reward_type > 1 and current_reward == 0: 338 | future_reward = 0 339 | else: 340 | future_reward = self.predicted_value 341 | expected_reward = current_reward + self.future_reward_discount * future_reward 342 | 343 | return expected_reward, current_reward, future_reward 344 | 345 | def reward_function(self): 346 | # Detect changes 347 | depth_diff = abs(self.depth_heightmap - self.prev_depth_heightmap) 348 | depth_diff[np.isnan(depth_diff)] = 0 349 | depth_diff[depth_diff > 0.3] = 0 350 | depth_diff[depth_diff < 0.01] = 0 351 | depth_diff[depth_diff > 0] = 1 352 | change_threshold = 300 353 | change_value = np.sum(depth_diff) 354 | change_detected = change_value > change_threshold or self.prev_grasp_success 355 | logging.info('Change detected: %r (value: %d)' % (change_detected, change_value)) 356 | 357 | if change_detected: 358 | if self.prev_primitive_action == 'push': 359 | self.no_change_count[0] = 0 360 | elif self.prev_primitive_action == 'grasp' or self.prev_primitive_action == 'place': 361 | self.no_change_count[1] = 0 362 | else: 363 | if self.prev_primitive_action == 'push': 364 | self.no_change_count[0] += 1 365 | elif self.prev_primitive_action == 'grasp': 366 | self.no_change_count[1] += 1 367 | 368 | # Check stack height 369 | img_median = ndimage.median_filter(self.depth_heightmap, size=5) 370 | max_z = np.max(img_median) 371 | if max_z <= 0.069: 372 | stack_height = 1 373 | elif (max_z > 0.069) and (max_z <= 0.11): 374 | stack_height = 2 375 | elif (max_z > 0.11) and (max_z <= 0.156): 376 | stack_height = 3 377 | elif (max_z > 0.156) and (max_z <= 0.21): 378 | stack_height = 4 379 | else: 380 | stack_height = 0 381 | 382 | if self.place_enabled: 383 | logging.info('Current stack height is {}'.format(stack_height)) 384 | self.tb.add_scalar('stack_height', stack_height, self.trainer.iteration) 385 | 386 | # Compute reward 387 | expected_reward, current_reward, future_reward = self.compute_reward(change_detected, stack_height) 388 | 389 | logging.info('Current reward: %f' % current_reward) 390 | logging.info('Future reward: %f' % future_reward) 391 | logging.info('Expected reward: %f + %f x %f = %f' % ( 392 | current_reward, self.future_reward_discount, future_reward, expected_reward)) 393 | 394 | self.prev_stack_height = stack_height 395 | 396 | return expected_reward, current_reward 397 | 398 | def experience_replay(self, prev_reward_value): 399 | """ 400 | Sample a reward value from the same action as the current one which differs from the most recent reward value 401 | to reduce the chance of catastrophic forgetting 402 | """ 403 | sample_primitive_action = self.prev_primitive_action 404 | if sample_primitive_action == 'push': 405 | sample_primitive_action_id = 0 406 | sample_reward_value = 0 if prev_reward_value == 0.5 else 0.5 407 | elif sample_primitive_action == 'grasp': 408 | sample_primitive_action_id = 1 409 | sample_reward_value = 0 if prev_reward_value == 1 else 1 410 | elif sample_primitive_action == 'place': 411 | sample_primitive_action_id = 2 412 | sample_reward_value = 0 if prev_reward_value >= 1 else 1 413 | else: 414 | raise NotImplementedError( 415 | 'ERROR: {} action is not yet supported in experience replay'.format(sample_primitive_action)) 416 | 417 | # Get samples of the same primitive but with different results 418 | sample_ind = np.argwhere(np.logical_and( 419 | np.asarray(self.trainer.reward_value_log)[1:self.trainer.iteration, 0] == sample_reward_value, 420 | np.asarray(self.trainer.executed_action_log)[1:self.trainer.iteration, 0] == sample_primitive_action_id)) 421 | 422 | if sample_ind.size > 0: 423 | 424 | # Find sample with highest surprise value 425 | sample_surprise_values = np.abs(np.asarray(self.trainer.predicted_value_log)[sample_ind[:, 0]] - 426 | np.asarray(self.trainer.label_value_log)[sample_ind[:, 0]]) 427 | sorted_surprise_ind = np.argsort(sample_surprise_values[:, 0]) 428 | sorted_sample_ind = sample_ind[sorted_surprise_ind, 0] 429 | pow_law_exp = 2 430 | rand_sample_ind = int(np.round(np.random.power(pow_law_exp, 1) * (sample_ind.size - 1))) 431 | sample_iteration = sorted_sample_ind[rand_sample_ind] 432 | logging.info('Experience replay: iteration %d (surprise value: %f)' % ( 433 | sample_iteration, sample_surprise_values[sorted_surprise_ind[rand_sample_ind]])) 434 | 435 | # Load sample RGB-D heightmap 436 | sample_color_heightmap = cv2.imread( 437 | os.path.join(self.logger.color_heightmaps_directory, '%06d.0.color.png' % sample_iteration)) 438 | sample_color_heightmap = cv2.cvtColor(sample_color_heightmap, cv2.COLOR_BGR2RGB) 439 | sample_depth_heightmap = cv2.imread( 440 | os.path.join(self.logger.depth_heightmaps_directory, '%06d.0.depth.png' % sample_iteration), -1) 441 | sample_depth_heightmap = sample_depth_heightmap.astype(np.float32) / 100000 442 | 443 | # Compute forward pass with sample 444 | with torch.no_grad(): 445 | sample_push_predictions, sample_grasp_predictions, sample_place_predictions = self.trainer.forward( 446 | sample_color_heightmap, sample_depth_heightmap, is_volatile=True) 447 | 448 | # Get labels for sample and backpropagate 449 | sample_best_pix_ind = (np.asarray(self.trainer.executed_action_log)[sample_iteration, 1:4]).astype(int) 450 | self.trainer.backprop(sample_color_heightmap, sample_depth_heightmap, sample_primitive_action, 451 | sample_best_pix_ind, self.trainer.label_value_log[sample_iteration], self.filter_type) 452 | 453 | # Recompute prediction value and label for replay buffer 454 | if sample_primitive_action == 'push': 455 | self.trainer.predicted_value_log[sample_iteration] = [np.max(sample_push_predictions)] 456 | elif sample_primitive_action == 'grasp': 457 | self.trainer.predicted_value_log[sample_iteration] = [np.max(sample_grasp_predictions)] 458 | elif sample_primitive_action == 'place': 459 | self.trainer.predicted_value_log[sample_iteration] = [np.max(sample_place_predictions)] 460 | 461 | else: 462 | logging.info('Not enough prior training samples. Skipping experience replay.') 463 | 464 | def loop(self): 465 | """ 466 | Main training/testing loop 467 | """ 468 | # Init current mission 469 | self.mission_complete = False 470 | reset_trial = False 471 | 472 | # Make sure simulation is still stable (if not, reset simulation) 473 | if self.is_sim: 474 | self.robot.check_sim() 475 | 476 | # Get latest RGB-D image 477 | color_img, depth_img = self.robot.get_camera_data() 478 | depth_img = depth_img * self.robot.cam_depth_scale # Apply depth scale from calibration 479 | 480 | # Get heightmap from RGB-D image (by re-projecting 3D point cloud) 481 | self.color_heightmap, self.depth_heightmap = utils.get_heightmap(color_img, depth_img, 482 | self.robot.cam_intrinsics, 483 | self.robot.cam_pose, self.workspace_limits, 484 | self.heightmap_resolution) 485 | # Remove NaNs from the depth heightmap 486 | self.depth_heightmap[np.isnan(self.depth_heightmap)] = 0 487 | 488 | # Reset simulation or pause real-world training if table is empty 489 | stuff_count = np.zeros(self.depth_heightmap.shape) 490 | stuff_count[self.depth_heightmap > 0.02] = 1 491 | empty_threshold = 300 492 | if self.is_sim and self.is_testing: 493 | empty_threshold = 10 494 | if np.sum(stuff_count) < empty_threshold: 495 | logging.info('Not enough objects in view (value: %d)! Repositioning objects.' % (np.sum(stuff_count))) 496 | reset_trial = True 497 | 498 | # Reset simulation or pause real-world training if no change is detected for last 10 iterations 499 | if self.is_sim and self.no_change_count[0] + self.no_change_count[1] > 15: 500 | logging.info('No change is detected for last 15 iterations. Resetting simulation.') 501 | reset_trial = True 502 | 503 | if self.prev_stack_height >= self.goal_stack_height and self.place_enabled: 504 | logging.info('Stack completed. Repositioning objects.') 505 | reset_trial = True 506 | 507 | if not reset_trial: 508 | # Run forward pass with network to get affordances 509 | self.push_predictions, self.grasp_predictions, self.place_predictions = self.trainer.forward( 510 | self.color_heightmap, self.depth_heightmap, is_volatile=True) 511 | 512 | # Execute best primitive action on robot in another thread 513 | self.execute_action = True 514 | 515 | # Save RGB-D images and RGB-D heightmaps 516 | self.logger.save_images(self.trainer.iteration, color_img, depth_img, '0') 517 | self.logger.save_heightmaps(self.trainer.iteration, self.color_heightmap, self.depth_heightmap, '0') 518 | 519 | # Run training iteration in current thread (aka training thread) 520 | if self.prev_primitive_action is not None: 521 | 522 | # Compute training labels 523 | label_value, prev_reward_value = self.reward_function() 524 | 525 | # Backpropagate 526 | self.trainer.backprop(self.prev_color_heightmap, self.prev_depth_heightmap, 527 | self.prev_primitive_action, self.prev_best_pix_ind, label_value, self.filter_type) 528 | 529 | # Save training labels and reward 530 | self.trainer.label_value_log.append([label_value]) 531 | self.trainer.reward_value_log.append([prev_reward_value]) 532 | self.trainer.grasp_success_log.append([int(self.prev_grasp_success)]) 533 | self.logger.write_to_log('label-value', self.trainer.label_value_log) 534 | self.logger.write_to_log('reward-value', self.trainer.reward_value_log) 535 | self.logger.write_to_log('grasp-success', self.trainer.grasp_success_log) 536 | if self.push_enabled: 537 | self.trainer.push_success_log.append([int(self.prev_push_success)]) 538 | self.logger.write_to_log('push-success', self.trainer.push_success_log) 539 | if self.place_enabled: 540 | self.trainer.place_success_log.append([int(self.prev_place_success)]) 541 | self.logger.write_to_log('place-success', self.trainer.place_success_log) 542 | 543 | # Save to tensorboard 544 | self.tb.add_scalar('loss', self.trainer.running_loss.mean(), self.trainer.iteration) 545 | if self.prev_primitive_action == 'grasp': 546 | self.tb.add_scalar('success-rate/grasp', self.prev_grasp_success, self.trainer.iteration) 547 | elif self.prev_primitive_action == 'push': 548 | self.tb.add_scalar('success-rate/push', self.prev_push_success, self.trainer.iteration) 549 | elif self.prev_primitive_action == 'place': 550 | self.tb.add_scalar('success-rate/place', self.prev_place_success, self.trainer.iteration) 551 | 552 | if not self.is_testing: 553 | # Adjust exploration probability 554 | if self.explore_type == 1: 555 | self.explore_prob = max(0.5 * np.power(0.99994, self.trainer.iteration), 556 | 0.1) if self.explore_rate_decay else 0.5 557 | elif self.explore_type == 2: 558 | f = (1.0 - np.exp((-self.trainer.running_loss.mean()) / self.LAE_sigma)) \ 559 | / (1.0 + np.exp((-self.trainer.running_loss.mean()) / self.LAE_sigma)) 560 | self.explore_prob = self.LAE_beta * f + (1 - self.LAE_beta) * self.explore_prob 561 | 562 | # Check for progress counting inconsistencies 563 | if len(self.trainer.reward_value_log) < self.trainer.iteration - 2: 564 | logging.warning( 565 | 'WARNING POSSIBLE CRITICAL ERROR DETECTED: log data index and trainer.iteration out of sync!!! ' 566 | 'Experience Replay may break! ' 567 | 'Check code for errors in indexes, continue statements etc.') 568 | 569 | if not self.experience_replay_disabled: 570 | # Do sampling for experience replay 571 | self.experience_replay(prev_reward_value) 572 | 573 | # Save model snapshot 574 | self.logger.save_backup_model(self.trainer.model) 575 | if self.trainer.iteration % 1000 == 0: 576 | self.logger.save_model(self.trainer.iteration, self.trainer.model) 577 | self.trainer.model.to(self.trainer.device) 578 | 579 | if not reset_trial: 580 | # Sync both action thread and training thread 581 | while self.execute_action: 582 | time.sleep(0.1) 583 | 584 | if self.mission_complete: 585 | logging.info('Mission complete') 586 | else: 587 | logging.warning('Robot execution failed. Restarting simulation..') 588 | self.robot.restart_sim() 589 | 590 | if reset_trial: 591 | if self.is_sim: 592 | self.robot.restart_sim() 593 | self.robot.add_objects() 594 | else: 595 | time.sleep(30) 596 | 597 | if self.is_testing: # If at end of test run, re-load original weights (before test run) 598 | self.trainer.model.load_state_dict(torch.load(self.snapshot_file)) 599 | 600 | self.trainer.task_complete_log.append([self.trainer.iteration]) 601 | self.logger.write_to_log('task_complete', self.trainer.task_complete_log) 602 | self.tb.add_scalar('task_complete', self.trainer.iteration - self.last_task_complete, len(self.trainer.task_complete_log)) 603 | 604 | self.last_task_complete = self.trainer.iteration 605 | self.no_change_count = [2, 2] if not self.is_testing else [0, 0] 606 | self.prev_stack_height = 0 607 | self.prev_primitive_action = None 608 | 609 | def run(self): 610 | agent_thread = threading.Thread(target=self.agent) 611 | agent_thread.daemon = True 612 | agent_thread.start() 613 | while self.trainer.iteration <= self.max_iter: 614 | logging.info('\n%s iteration: %d' % ('Testing' if self.is_testing else 'Training', self.trainer.iteration)) 615 | 616 | # Main loop 617 | iteration_time_0 = time.time() 618 | self.loop() 619 | if self.mission_complete: 620 | self.trainer.iteration += 1 621 | iteration_time_1 = time.time() 622 | logging.info('Time elapsed: %f' % (iteration_time_1 - iteration_time_0)) 623 | 624 | # Check for number of test trails completed 625 | if self.is_testing and len(self.trainer.task_complete_log) >= self.max_test_trials: 626 | break 627 | self.shutdown_called = True 628 | agent_thread.join() 629 | 630 | def teardown(self): 631 | self.robot.shutdown() 632 | del self.trainer, self.robot 633 | torch.cuda.empty_cache() 634 | 635 | 636 | if __name__ == '__main__': 637 | 638 | # Parse arguments 639 | parser = argparse.ArgumentParser( 640 | description='Train robotic agents to learn manipulation actions with deep reinforcement learning in PyTorch.') 641 | 642 | # --------------- Setup options --------------- 643 | parser.add_argument('--is_sim', dest='is_sim', action='store_true', default=True, 644 | help='run in simulation?') 645 | parser.add_argument('--sim_port', dest='sim_port', type=int, action='store', default=19997, 646 | help='port for simulation') 647 | parser.add_argument('--obj_mesh_dir', dest='obj_mesh_dir', action='store', default='simulation/objects/mixed_shapes', 648 | help='directory containing 3D mesh files (.obj) of objects to be added to simulation') 649 | parser.add_argument('--num_obj', dest='num_obj', type=int, action='store', default=10, 650 | help='number of objects to add to simulation') 651 | parser.add_argument('--heightmap_resolution', dest='heightmap_resolution', type=float, action='store', 652 | default=0.002, help='meters per pixel of heightmap') 653 | parser.add_argument('--random_seed', dest='random_seed', type=int, action='store', default=123, 654 | help='random seed for simulation and neural net initialization') 655 | parser.add_argument('--cpu', dest='force_cpu', action='store_true', default=False, 656 | help='force code to run in CPU mode') 657 | 658 | # ------------- Algorithm options ------------- 659 | parser.add_argument('--network', dest='network', action='store', default='grconvnet4', 660 | help='Neural network architecture choice, options are grconvnet, efficientnet, denseunet') 661 | parser.add_argument('--num_rotations', dest='num_rotations', type=int, action='store', default=16) 662 | parser.add_argument('--push_enabled', dest='push_enabled', action='store_true', default=False) 663 | parser.add_argument('--place_enabled', dest='place_enabled', action='store_true', default=False) 664 | parser.add_argument('--reward_type', dest='reward_type', type=int, action='store', default=2) 665 | parser.add_argument('--filter_type', dest='filter_type', type=int, action='store', default=4) 666 | parser.add_argument('--experience_replay_disabled', dest='experience_replay_disabled', action='store_true', 667 | default=False, help='disable prioritized experience replay') 668 | parser.add_argument('--future_reward_discount', dest='future_reward_discount', type=float, action='store', 669 | default=0.5) 670 | parser.add_argument('--place_reward_scale', dest='place_reward_scale', type=float, action='store', default=1.0) 671 | parser.add_argument('--goal_stack_height', dest='goal_stack_height', type=int, action='store', default=4) 672 | parser.add_argument('--explore_actions', dest='explore_actions', type=int, action='store', default=1) 673 | parser.add_argument('--explore_type', dest='explore_type', type=int, action='store', default=1) 674 | parser.add_argument('--explore_rate_decay', dest='explore_rate_decay', action='store_true', default=True) 675 | parser.add_argument('--max_iter', dest='max_iter', action='store', type=int, default=50000, 676 | help='max iter for training') 677 | 678 | # -------------- Testing options -------------- 679 | parser.add_argument('--is_testing', dest='is_testing', action='store_true', default=False) 680 | parser.add_argument('--max_test_trials', dest='max_test_trials', type=int, action='store', default=30, 681 | help='maximum number of test runs per case/scenario') 682 | parser.add_argument('--test_preset_cases', dest='test_preset_cases', action='store_true', default=False) 683 | parser.add_argument('--test_preset_file', dest='test_preset_file', action='store', default='') 684 | parser.add_argument('--test_preset_dir', dest='test_preset_dir', action='store', default='simulation/test-cases/') 685 | 686 | # ------ Pre-loading and logging options ------ 687 | parser.add_argument('--snapshot_file', dest='snapshot_file', action='store') 688 | parser.add_argument('--logging_directory', dest='logging_directory', action='store') 689 | parser.add_argument('--save_visualizations', dest='save_visualizations', action='store_true', default=False, 690 | help='save visualizations of model predictions?') 691 | 692 | # Run main program with specified arguments 693 | args = parser.parse_args() 694 | 695 | if args.is_testing and args.test_preset_cases: 696 | preset_files = os.listdir(args.test_preset_dir) 697 | preset_files = [os.path.abspath(os.path.join(args.test_preset_dir, filename)) for filename in preset_files] 698 | preset_files = sorted(preset_files) 699 | args.continue_logging = True 700 | for idx, preset_file in enumerate(preset_files): 701 | logging.info('Running test {}'.format(preset_file)) 702 | args.test_preset_file = preset_file 703 | args.num_obj = 10 704 | args.logging_directory = args.snapshot_file.split('/')[0] + '/' + args.snapshot_file.split('/')[ 705 | 1] + '/preset-test/' + re.findall("\d+", args.snapshot_file.split('/')[3])[0] + '/' + str(idx) 706 | 707 | task = LearnManipulation(args) 708 | task.run() 709 | task.teardown() 710 | else: 711 | task = LearnManipulation(args) 712 | task.run() 713 | task.teardown() 714 | -------------------------------------------------------------------------------- /models/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skumra/romannet/0af092a1be26ac5f213f1e5c21f81f89699a4c92/models/__init__.py -------------------------------------------------------------------------------- /models/denseunet.py: -------------------------------------------------------------------------------- 1 | import torch 2 | import torch.nn as nn 3 | 4 | 5 | class DownBlock(nn.Module): 6 | def __init__(self, input_channels, output_channels, down_size, dropout=False, prob=0.0): 7 | super(DownBlock, self).__init__() 8 | self.conv1 = nn.Conv2d(input_channels, output_channels, kernel_size=3, padding=1) 9 | self.conv21 = nn.Conv2d(input_channels + output_channels, output_channels, kernel_size=1, padding=0) 10 | self.conv22 = nn.Conv2d(output_channels, output_channels, kernel_size=3, padding=1) 11 | self.conv31 = nn.Conv2d(input_channels + 2 * output_channels, output_channels, kernel_size=1, padding=0) 12 | self.conv32 = nn.Conv2d(output_channels, output_channels, kernel_size=3, padding=1) 13 | self.max_pool = nn.MaxPool2d(kernel_size=down_size) 14 | self.relu = nn.LeakyReLU() 15 | self.down_size = down_size 16 | self.dropout = dropout 17 | self.dropout1 = nn.Dropout(p=prob) 18 | self.dropout2 = nn.Dropout(p=prob) 19 | self.dropout3 = nn.Dropout(p=prob) 20 | 21 | def forward(self, x): 22 | if self.down_size is not None: 23 | x = self.max_pool(x) 24 | 25 | if self.dropout: 26 | x1 = self.relu(self.dropout1(self.conv1(x))) 27 | x21 = torch.cat((x, x1), dim=1) 28 | x22 = self.relu(self.dropout2(self.conv22(self.conv21(x21)))) 29 | x31 = torch.cat((x21, x22), dim=1) 30 | out = self.relu(self.dropout3(self.conv32(self.conv31(x31)))) 31 | else: 32 | x1 = self.relu(self.conv1(x)) 33 | x21 = torch.cat((x, x1), dim=1) 34 | x22 = self.relu(self.conv22(self.conv21(x21))) 35 | x31 = torch.cat((x21, x22), dim=1) 36 | out = self.relu(self.conv32(self.conv31(x31))) 37 | 38 | return out 39 | 40 | 41 | class UpBlock(nn.Module): 42 | def __init__(self, skip_channels, input_channels, output_channels, up_stride, dropout=False, prob=0.0): 43 | super(UpBlock, self).__init__() 44 | self.conv11 = nn.Conv2d(skip_channels + input_channels, output_channels, kernel_size=1, padding=0) 45 | self.conv12 = nn.Conv2d(output_channels, output_channels, kernel_size=3, padding=1) 46 | self.conv21 = nn.Conv2d(skip_channels + input_channels + output_channels, output_channels, kernel_size=1, padding=0) 47 | self.conv22 = nn.Conv2d(output_channels, output_channels, kernel_size=3, padding=1) 48 | self.relu = nn.LeakyReLU() 49 | self.up_stride = up_stride 50 | self.dropout = dropout 51 | self.dropout1 = nn.Dropout(p=prob) 52 | self.dropout2 = nn.Dropout(p=prob) 53 | 54 | def forward(self, prev_feature_map, x): 55 | x = nn.functional.interpolate(x, scale_factor=self.up_stride, mode='nearest') 56 | x = torch.cat((x, prev_feature_map), dim=1) 57 | 58 | if self.dropout: 59 | x1 = self.relu(self.dropout1(self.conv12(self.conv11(x)))) 60 | x21 = torch.cat((x, x1), dim=1) 61 | out = self.relu(self.dropout2(self.conv22(self.conv21(x21)))) 62 | else: 63 | x1 = self.relu(self.conv12(self.conv11(x))) 64 | x21 = torch.cat((x, x1), dim=1) 65 | out = self.relu(self.conv22(self.conv21(x21))) 66 | 67 | return out 68 | 69 | 70 | class DenseUNet(nn.Module): 71 | def __init__(self, input_channels=4, output_channels=1, channel_size=16, dropout=False, prob=0.0): 72 | super(DenseUNet, self).__init__() 73 | 74 | self.down_block1 = DownBlock(input_channels=input_channels, output_channels=channel_size, 75 | down_size=None, dropout=dropout, prob=prob) 76 | self.down_block2 = DownBlock(input_channels=channel_size, output_channels=channel_size, 77 | down_size=2, dropout=dropout, prob=prob) 78 | self.down_block3 = DownBlock(input_channels=channel_size, output_channels=channel_size, 79 | down_size=2, dropout=dropout, prob=prob) 80 | self.down_block4 = DownBlock(input_channels=channel_size, output_channels=channel_size, 81 | down_size=2, dropout=dropout, prob=prob) 82 | self.down_block5 = DownBlock(input_channels=channel_size, output_channels=channel_size, 83 | down_size=2, dropout=dropout, prob=prob) 84 | 85 | self.up_block1 = UpBlock(skip_channels=channel_size, input_channels=channel_size, 86 | output_channels=channel_size, up_stride=2, dropout=dropout, 87 | prob=prob) 88 | self.up_block2 = UpBlock(skip_channels=channel_size, input_channels=channel_size, 89 | output_channels=channel_size, up_stride=2, dropout=dropout, 90 | prob=prob) 91 | self.up_block3 = UpBlock(skip_channels=channel_size, input_channels=channel_size, 92 | output_channels=channel_size, up_stride=2, dropout=dropout, 93 | prob=prob) 94 | self.up_block4 = UpBlock(skip_channels=channel_size, input_channels=channel_size, 95 | output_channels=channel_size, up_stride=2, dropout=dropout, 96 | prob=prob) 97 | 98 | self.push_output = nn.Conv2d(in_channels=channel_size, out_channels=output_channels, 99 | kernel_size=1, padding=0) 100 | self.grasp_output = nn.Conv2d(in_channels=channel_size, out_channels=output_channels, 101 | kernel_size=1, padding=0) 102 | 103 | self.dropout = dropout 104 | self.dropout_pos = nn.Dropout(p=prob) 105 | self.dropout_cos = nn.Dropout(p=prob) 106 | 107 | for m in self.modules(): 108 | if isinstance(m, nn.Conv2d): 109 | nn.init.xavier_uniform_(m.weight, gain=1) 110 | 111 | def forward(self, x): 112 | x1 = self.down_block1(x) 113 | x2 = self.down_block2(x1) 114 | x3 = self.down_block3(x2) 115 | x4 = self.down_block4(x3) 116 | x5 = self.down_block5(x4) 117 | x6 = self.up_block1(x4, x5) 118 | x7 = self.up_block2(x3, x6) 119 | x8 = self.up_block3(x2, x7) 120 | x9 = self.up_block4(x1, x8) 121 | 122 | if self.dropout: 123 | push_output = self.push_output(self.dropout_pos(x9)) 124 | grasp_output = self.grasp_output(self.dropout_cos(x9)) 125 | else: 126 | push_output = self.push_output(x9) 127 | grasp_output = self.grasp_output(x9) 128 | 129 | return push_output, grasp_output 130 | -------------------------------------------------------------------------------- /models/grconvnet.py: -------------------------------------------------------------------------------- 1 | import torch.nn as nn 2 | import torch.nn.functional as F 3 | 4 | 5 | class ResidualBlock(nn.Module): 6 | 7 | def __init__(self, in_channels, out_channels, kernel_size=3): 8 | super(ResidualBlock, self).__init__() 9 | self.conv1 = nn.Conv2d(in_channels, out_channels, kernel_size, padding=1) 10 | self.bn1 = nn.BatchNorm2d(in_channels) 11 | self.conv2 = nn.Conv2d(in_channels, out_channels, kernel_size, padding=1) 12 | self.bn2 = nn.BatchNorm2d(in_channels) 13 | 14 | def forward(self, x_in): 15 | x = self.bn1(self.conv1(x_in)) 16 | x = F.relu(x) 17 | x = self.bn2(self.conv2(x)) 18 | return x + x_in 19 | 20 | 21 | class GenerativeResnet(nn.Module): 22 | 23 | def __init__(self, input_channels=4, output_channels=1, channel_size=32, dropout=False, prob=0.0): 24 | super(GenerativeResnet, self).__init__() 25 | self.conv1 = nn.Conv2d(input_channels, channel_size, kernel_size=9, stride=1, padding=4) 26 | self.bn1 = nn.BatchNorm2d(channel_size) 27 | 28 | self.conv2 = nn.Conv2d(channel_size, channel_size * 2, kernel_size=4, stride=2, padding=1) 29 | self.bn2 = nn.BatchNorm2d(channel_size * 2) 30 | 31 | self.conv3 = nn.Conv2d(channel_size * 2, channel_size * 4, kernel_size=4, stride=2, padding=1) 32 | self.bn3 = nn.BatchNorm2d(channel_size * 4) 33 | 34 | self.res1 = ResidualBlock(channel_size * 4, channel_size * 4) 35 | self.res2 = ResidualBlock(channel_size * 4, channel_size * 4) 36 | self.res3 = ResidualBlock(channel_size * 4, channel_size * 4) 37 | self.res4 = ResidualBlock(channel_size * 4, channel_size * 4) 38 | self.res5 = ResidualBlock(channel_size * 4, channel_size * 4) 39 | 40 | self.conv4 = nn.ConvTranspose2d(channel_size * 4, channel_size * 2, kernel_size=4, stride=2, padding=1, output_padding=1) 41 | self.bn4 = nn.BatchNorm2d(channel_size * 2) 42 | 43 | self.conv5 = nn.ConvTranspose2d(channel_size * 2, channel_size, kernel_size=4, stride=2, padding=2, output_padding=1) 44 | self.bn5 = nn.BatchNorm2d(channel_size) 45 | 46 | self.conv6 = nn.ConvTranspose2d(channel_size, channel_size, kernel_size=9, stride=1, padding=4) 47 | 48 | self.push_output = nn.Conv2d(in_channels=channel_size, out_channels=output_channels, kernel_size=2) 49 | self.grasp_output = nn.Conv2d(in_channels=channel_size, out_channels=output_channels, kernel_size=2) 50 | self.place_output = nn.Conv2d(in_channels=channel_size, out_channels=output_channels, kernel_size=2) 51 | 52 | self.dropout = dropout 53 | self.dropout_push = nn.Dropout(p=prob) 54 | self.dropout_grasp = nn.Dropout(p=prob) 55 | self.dropout_place = nn.Dropout(p=prob) 56 | 57 | for m in self.modules(): 58 | if isinstance(m, (nn.Conv2d, nn.ConvTranspose2d)): 59 | nn.init.xavier_uniform_(m.weight, gain=1) 60 | 61 | def forward(self, x_in): 62 | x = F.relu(self.bn1(self.conv1(x_in))) 63 | x = F.relu(self.bn2(self.conv2(x))) 64 | x = F.relu(self.bn3(self.conv3(x))) 65 | x = self.res1(x) 66 | x = self.res2(x) 67 | x = self.res3(x) 68 | x = self.res4(x) 69 | x = self.res5(x) 70 | x = F.relu(self.bn4(self.conv4(x))) 71 | x = F.relu(self.bn5(self.conv5(x))) 72 | x = self.conv6(x) 73 | 74 | if self.dropout: 75 | push_output = self.push_output(self.dropout_push(x)) 76 | grasp_output = self.grasp_output(self.dropout_grasp(x)) 77 | place_output = self.place_output(self.dropout_place(x)) 78 | else: 79 | push_output = self.push_output(x) 80 | grasp_output = self.grasp_output(x) 81 | place_output = self.place_output(x) 82 | 83 | return push_output, grasp_output, place_output 84 | -------------------------------------------------------------------------------- /models/grconvnet3.py: -------------------------------------------------------------------------------- 1 | import torch.nn as nn 2 | import torch.nn.functional as F 3 | 4 | 5 | class ResidualBlock(nn.Module): 6 | 7 | def __init__(self, in_channels, out_channels, kernel_size=3): 8 | super(ResidualBlock, self).__init__() 9 | self.conv1 = nn.Conv2d(in_channels, out_channels, kernel_size, padding=1) 10 | self.bn1 = nn.BatchNorm2d(in_channels) 11 | self.conv2 = nn.Conv2d(in_channels, out_channels, kernel_size, padding=1) 12 | self.bn2 = nn.BatchNorm2d(in_channels) 13 | 14 | def forward(self, x_in): 15 | x = self.bn1(self.conv1(x_in)) 16 | x = F.relu(x) 17 | x = self.bn2(self.conv2(x)) 18 | return x + x_in 19 | 20 | 21 | class GenerativeResnet3(nn.Module): 22 | 23 | def __init__(self, input_channels=4, output_channels=1, channel_size=32, dropout=False, prob=0.1): 24 | super(GenerativeResnet3, self).__init__() 25 | self.conv1 = nn.Conv2d(input_channels, channel_size, kernel_size=9, stride=1, padding=4) 26 | self.bn1 = nn.BatchNorm2d(channel_size) 27 | 28 | self.conv2 = nn.Conv2d(channel_size, channel_size * 2, kernel_size=4, stride=2, padding=1) 29 | self.bn2 = nn.BatchNorm2d(channel_size * 2) 30 | 31 | self.conv3 = nn.Conv2d(channel_size * 2, channel_size * 4, kernel_size=4, stride=2, padding=1) 32 | self.bn3 = nn.BatchNorm2d(channel_size * 4) 33 | 34 | self.res1 = ResidualBlock(channel_size * 4, channel_size * 4) 35 | self.res2 = ResidualBlock(channel_size * 4, channel_size * 4) 36 | self.res3 = ResidualBlock(channel_size * 4, channel_size * 4) 37 | self.res4 = ResidualBlock(channel_size * 4, channel_size * 4) 38 | self.res5 = ResidualBlock(channel_size * 4, channel_size * 4) 39 | 40 | self.conv4 = nn.ConvTranspose2d(channel_size * 4, channel_size * 2, kernel_size=4, stride=2, padding=1, output_padding=1) 41 | self.bn4 = nn.BatchNorm2d(channel_size * 2) 42 | 43 | self.conv5 = nn.ConvTranspose2d(channel_size * 2, channel_size, kernel_size=4, stride=2, padding=2, output_padding=1) 44 | self.bn5 = nn.BatchNorm2d(channel_size) 45 | 46 | self.conv6 = nn.ConvTranspose2d(channel_size, channel_size, kernel_size=9, stride=1, padding=4) 47 | 48 | self.output = nn.Conv2d(in_channels=channel_size, out_channels=output_channels, kernel_size=2) 49 | 50 | self.dropout = dropout 51 | self.dropout_layer = nn.Dropout(p=prob) 52 | 53 | for m in self.modules(): 54 | if isinstance(m, (nn.Conv2d, nn.ConvTranspose2d)): 55 | nn.init.xavier_uniform_(m.weight, gain=1) 56 | 57 | def forward(self, x_in): 58 | x = F.relu(self.bn1(self.conv1(x_in))) 59 | x = F.relu(self.bn2(self.conv2(x))) 60 | x = F.relu(self.bn3(self.conv3(x))) 61 | x = self.res1(x) 62 | x = self.res2(x) 63 | x = self.res3(x) 64 | x = self.res4(x) 65 | x = self.res5(x) 66 | x = F.relu(self.bn4(self.conv4(x))) 67 | x = F.relu(self.bn5(self.conv5(x))) 68 | x = self.conv6(x) 69 | 70 | if self.dropout: 71 | output = self.output(self.dropout_layer(x)) 72 | else: 73 | output = self.output(x) 74 | 75 | return output 76 | -------------------------------------------------------------------------------- /models/grconvnet4.py: -------------------------------------------------------------------------------- 1 | import torch.nn as nn 2 | import torch.nn.functional as F 3 | 4 | 5 | class ResidualBlock(nn.Module): 6 | 7 | def __init__(self, in_channels, out_channels, kernel_size=3): 8 | super(ResidualBlock, self).__init__() 9 | self.conv1 = nn.Conv2d(in_channels, out_channels, kernel_size, padding=1) 10 | self.bn1 = nn.BatchNorm2d(in_channels) 11 | self.conv2 = nn.Conv2d(in_channels, out_channels, kernel_size, padding=1) 12 | self.bn2 = nn.BatchNorm2d(in_channels) 13 | 14 | def forward(self, x_in): 15 | x = self.bn1(self.conv1(x_in)) 16 | x = F.relu(x) 17 | x = self.bn2(self.conv2(x)) 18 | return x + x_in 19 | 20 | 21 | class GenerativeResnet4(nn.Module): 22 | 23 | def __init__(self, input_channels=4, output_channels=1, channel_size=32, dropout=False, prob=0.1): 24 | super(GenerativeResnet4, self).__init__() 25 | self.conv1 = nn.Conv2d(input_channels, channel_size, kernel_size=9, stride=1, padding=4) 26 | self.bn1 = nn.BatchNorm2d(channel_size) 27 | 28 | self.conv2 = nn.Conv2d(channel_size, channel_size * 2, kernel_size=4, stride=2, padding=1) 29 | self.bn2 = nn.BatchNorm2d(channel_size * 2) 30 | 31 | self.conv3 = nn.Conv2d(channel_size * 2, channel_size * 4, kernel_size=4, stride=2, padding=1) 32 | self.bn3 = nn.BatchNorm2d(channel_size * 4) 33 | 34 | self.res1 = ResidualBlock(channel_size * 4, channel_size * 4) 35 | self.res2 = ResidualBlock(channel_size * 4, channel_size * 4) 36 | self.res3 = ResidualBlock(channel_size * 4, channel_size * 4) 37 | self.res4 = ResidualBlock(channel_size * 4, channel_size * 4) 38 | self.res5 = ResidualBlock(channel_size * 4, channel_size * 4) 39 | 40 | self.conv4 = nn.ConvTranspose2d(channel_size * 4, channel_size * 2, kernel_size=4, stride=2, padding=1, output_padding=1) 41 | self.bn4 = nn.BatchNorm2d(channel_size * 2) 42 | 43 | self.conv5 = nn.ConvTranspose2d(channel_size * 2, channel_size, kernel_size=4, stride=2, padding=2, output_padding=1) 44 | self.bn5 = nn.BatchNorm2d(channel_size) 45 | 46 | self.conv6 = nn.ConvTranspose2d(channel_size, channel_size, kernel_size=9, stride=1, padding=4) 47 | 48 | self.output = nn.Conv2d(in_channels=channel_size, out_channels=output_channels, kernel_size=2) 49 | 50 | self.dropout = dropout 51 | self.dropout_layer = nn.Dropout(p=prob) 52 | 53 | # for m in self.modules(): 54 | # if isinstance(m, (nn.Conv2d, nn.ConvTranspose2d)): 55 | # nn.init.xavier_uniform_(m.weight, gain=1) 56 | 57 | def forward(self, x_in): 58 | x = F.relu(self.bn1(self.conv1(x_in))) 59 | x = F.relu(self.bn2(self.conv2(x))) 60 | x = F.relu(self.bn3(self.conv3(x))) 61 | x = self.res1(x) 62 | x = self.res2(x) 63 | x = self.res3(x) 64 | x = self.res4(x) 65 | x = self.res5(x) 66 | x = F.relu(self.bn4(self.conv4(x))) 67 | x = F.relu(self.bn5(self.conv5(x))) 68 | x = self.conv6(x) 69 | 70 | if self.dropout: 71 | output = self.output(self.dropout_layer(x)) 72 | else: 73 | output = self.output(x) 74 | 75 | return output 76 | -------------------------------------------------------------------------------- /models/network.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import numpy as np 4 | import segmentation_models_pytorch as smp 5 | import torch 6 | import torch.nn as nn 7 | import torch.nn.functional as F 8 | from segmentation_models_pytorch.encoders import get_preprocessing_fn 9 | from torch.autograd import Variable 10 | 11 | from models.denseunet import DenseUNet 12 | from models.grconvnet import GenerativeResnet 13 | from models.grconvnet3 import GenerativeResnet3 14 | from models.grconvnet4 import GenerativeResnet4 15 | 16 | 17 | class ManipulationNet(nn.Module): 18 | 19 | def __init__(self, network, device, push_enabled, place_enabled, num_rotations): 20 | super(ManipulationNet, self).__init__() 21 | self.device = device 22 | self.push_enabled = push_enabled 23 | self.place_enabled = place_enabled 24 | self.net = None 25 | self.preprocess_input = None 26 | 27 | # Initialize network 28 | if network == 'grconvnet': 29 | self.net = GenerativeResnet() 30 | elif network == 'grconvnet3': 31 | # if self.push_enabled: 32 | self.push_net = GenerativeResnet3() 33 | self.grasp_net = GenerativeResnet3() 34 | self.place_net = GenerativeResnet3() 35 | elif network == 'grconvnet4': 36 | # if self.push_enabled: 37 | self.push_net = GenerativeResnet4() 38 | self.grasp_net = GenerativeResnet4() 39 | self.place_net = GenerativeResnet4() 40 | elif network == 'denseunet': 41 | self.net = DenseUNet() 42 | elif network == 'efficientunet': 43 | encoder = 'efficientnet-b4' 44 | encoder_weights = 'imagenet' 45 | self.preprocess_input = get_preprocessing_fn(encoder, pretrained=encoder_weights) 46 | self.push_net = smp.Unet(encoder, encoder_weights=encoder_weights, in_channels=4) 47 | self.grasp_net = smp.Unet(encoder, encoder_weights=encoder_weights, in_channels=4) 48 | self.place_net = smp.Unet(encoder, encoder_weights=encoder_weights, in_channels=4) 49 | else: 50 | raise NotImplementedError('Network type {} is not implemented'.format(network)) 51 | 52 | self.num_rotations = num_rotations 53 | 54 | # Initialize variables 55 | self.padding_width = 0 56 | self.output_prob = [] 57 | 58 | def pre_process(self, color_heightmap, depth_heightmap): 59 | 60 | # Add extra padding (to handle rotations inside network) 61 | diag_length = float(color_heightmap.shape[0]) * np.sqrt(2) 62 | diag_length = np.ceil(diag_length / 32) * 32 63 | self.padding_width = int((diag_length - color_heightmap.shape[0]) / 2) 64 | color_heightmap_r = np.pad(color_heightmap[:, :, 0], self.padding_width, 'constant', constant_values=0) 65 | color_heightmap_r.shape = (color_heightmap_r.shape[0], color_heightmap_r.shape[1], 1) 66 | color_heightmap_g = np.pad(color_heightmap[:, :, 1], self.padding_width, 'constant', constant_values=0) 67 | color_heightmap_g.shape = (color_heightmap_g.shape[0], color_heightmap_g.shape[1], 1) 68 | color_heightmap_b = np.pad(color_heightmap[:, :, 2], self.padding_width, 'constant', constant_values=0) 69 | color_heightmap_b.shape = (color_heightmap_b.shape[0], color_heightmap_b.shape[1], 1) 70 | input_color_image = np.concatenate((color_heightmap_r, color_heightmap_g, color_heightmap_b), axis=2) 71 | input_depth_image = np.pad(depth_heightmap, self.padding_width, 'constant', constant_values=0) 72 | 73 | # Pre-process color image (scale and normalize) 74 | image_mean = [0.485, 0.456, 0.406] 75 | image_std = [0.229, 0.224, 0.225] 76 | input_color_image = input_color_image.astype(float) / 255 77 | for c in range(3): 78 | input_color_image[:, :, c] = (input_color_image[:, :, c] - image_mean[c]) / image_std[c] 79 | 80 | if self.preprocess_input is not None: 81 | input_color_image = self.preprocess_input(input_color_image) 82 | 83 | # Pre-process depth image (normalize) 84 | image_mean = 0.01 85 | image_std = 0.03 86 | input_depth_image.shape = (input_depth_image.shape[0], input_depth_image.shape[1], 1) 87 | input_depth_image = (input_depth_image - image_mean) / image_std 88 | 89 | # Construct minibatch of size 1 (b,c,h,w) 90 | input_color_image.shape = ( 91 | input_color_image.shape[0], input_color_image.shape[1], input_color_image.shape[2], 1) 92 | input_depth_image.shape = ( 93 | input_depth_image.shape[0], input_depth_image.shape[1], input_depth_image.shape[2], 1) 94 | input_color_data = torch.from_numpy(input_color_image.astype(np.float32)).permute(3, 2, 0, 1) 95 | input_depth_data = torch.from_numpy(input_depth_image.astype(np.float32)).permute(3, 2, 0, 1) 96 | 97 | data = torch.cat((input_color_data, input_depth_data), dim=1) 98 | 99 | return data 100 | 101 | def post_process(self, data, output_prob): 102 | 103 | # Return Q values (and remove extra padding) 104 | for rotate_idx in range(len(output_prob)): 105 | if rotate_idx == 0: 106 | push_predictions = output_prob[rotate_idx][0].cpu().data.numpy()[:, 0, 107 | int(self.padding_width):int(data.shape[2] - self.padding_width), 108 | int(self.padding_width):int(data.shape[2] - self.padding_width)] 109 | grasp_predictions = output_prob[rotate_idx][1].cpu().data.numpy()[:, 0, 110 | int(self.padding_width):int(data.shape[2] - self.padding_width), 111 | int(self.padding_width):int(data.shape[2] - self.padding_width)] 112 | place_predictions = output_prob[rotate_idx][2].cpu().data.numpy()[:, 0, 113 | int(self.padding_width):int(data.shape[2] - self.padding_width), 114 | int(self.padding_width):int(data.shape[2] - self.padding_width)] 115 | else: 116 | push_predictions = np.concatenate((push_predictions, output_prob[rotate_idx][0].cpu().data.numpy()[:, 0, 117 | int(self.padding_width):int( 118 | data.shape[2] - self.padding_width), 119 | int(self.padding_width):int( 120 | data.shape[2] - self.padding_width)]), axis=0) 121 | grasp_predictions = np.concatenate((grasp_predictions, 122 | output_prob[rotate_idx][1].cpu().data.numpy()[:, 0, 123 | int(self.padding_width):int(data.shape[2] - self.padding_width), 124 | int(self.padding_width):int(data.shape[2] - self.padding_width)]), 125 | axis=0) 126 | place_predictions = np.concatenate((place_predictions, 127 | output_prob[rotate_idx][2].cpu().data.numpy()[:, 0, 128 | int(self.padding_width):int(data.shape[2] - self.padding_width), 129 | int(self.padding_width):int(data.shape[2] - self.padding_width)]), 130 | axis=0) 131 | 132 | return push_predictions, grasp_predictions, place_predictions 133 | 134 | def forward(self, data, is_volatile=False, specific_rotation=-1): 135 | if is_volatile: 136 | torch.set_grad_enabled(False) 137 | output_prob = [] 138 | 139 | # Apply rotations to images 140 | for rotate_idx in range(self.num_rotations): 141 | rotate_theta = np.radians(rotate_idx * (360 / self.num_rotations)) 142 | 143 | # Compute sample grid for rotation BEFORE neural network 144 | affine_mat_before = np.asarray([[np.cos(-rotate_theta), np.sin(-rotate_theta), 0], 145 | [-np.sin(-rotate_theta), np.cos(-rotate_theta), 0]]) 146 | affine_mat_before.shape = (2, 3, 1) 147 | affine_mat_before = torch.from_numpy(affine_mat_before).permute(2, 0, 1).float() 148 | flow_grid_before = F.affine_grid(Variable(affine_mat_before, requires_grad=False).to(self.device), data.size()) 149 | 150 | # Rotate images clockwise 151 | rotate_data = F.grid_sample(Variable(data, volatile=True).to(self.device), flow_grid_before, mode='nearest') 152 | 153 | # Compute features 154 | if self.net is not None: 155 | push_feat, grasp_feat, place_feat = self.net.forward(rotate_data) 156 | else: 157 | push_feat = self.push_net.forward(rotate_data) 158 | grasp_feat = self.grasp_net.forward(rotate_data) 159 | place_feat = self.place_net.forward(rotate_data) 160 | 161 | # Compute sample grid for rotation AFTER branches 162 | affine_mat_after = np.asarray([[np.cos(rotate_theta), np.sin(rotate_theta), 0], 163 | [-np.sin(rotate_theta), np.cos(rotate_theta), 0]]) 164 | affine_mat_after.shape = (2, 3, 1) 165 | affine_mat_after = torch.from_numpy(affine_mat_after).permute(2, 0, 1).float() 166 | flow_grid_after = F.affine_grid(Variable(affine_mat_after, requires_grad=False).to(self.device), push_feat.data.size()) 167 | 168 | # Forward pass through branches, undo rotation on output predictions, upsample results 169 | output_prob.append([F.grid_sample(push_feat, flow_grid_after, mode='nearest'), 170 | F.grid_sample(grasp_feat, flow_grid_after, mode='nearest'), 171 | F.grid_sample(place_feat, flow_grid_after, mode='nearest')]) 172 | torch.set_grad_enabled(True) 173 | 174 | return output_prob 175 | 176 | else: 177 | self.output_prob = [] 178 | 179 | rotate_idx = specific_rotation 180 | rotate_theta = np.radians(rotate_idx * (360 / self.num_rotations)) 181 | 182 | # Compute sample grid for rotation BEFORE branches 183 | affine_mat_before = np.asarray( 184 | [[np.cos(-rotate_theta), np.sin(-rotate_theta), 0], [-np.sin(-rotate_theta), np.cos(-rotate_theta), 0]]) 185 | affine_mat_before.shape = (2, 3, 1) 186 | affine_mat_before = torch.from_numpy(affine_mat_before).permute(2, 0, 1).float() 187 | flow_grid_before = F.affine_grid(Variable(affine_mat_before, requires_grad=False).to(self.device), data.size()) 188 | 189 | # Rotate images clockwise 190 | rotate_data = F.grid_sample(Variable(data, requires_grad=False).to(self.device), flow_grid_before, mode='nearest') 191 | 192 | # Compute features 193 | if self.net is not None: 194 | push_feat, grasp_feat, place_feat = self.net.forward(rotate_data) 195 | else: 196 | push_feat = self.push_net.forward(rotate_data) 197 | grasp_feat = self.grasp_net.forward(rotate_data) 198 | place_feat = self.place_net.forward(rotate_data) 199 | 200 | # Compute sample grid for rotation AFTER branches 201 | affine_mat_after = np.asarray([[np.cos(rotate_theta), np.sin(rotate_theta), 0], [-np.sin(rotate_theta), np.cos(rotate_theta), 0]]) 202 | affine_mat_after.shape = (2, 3, 1) 203 | affine_mat_after = torch.from_numpy(affine_mat_after).permute(2, 0, 1).float() 204 | flow_grid_after = F.affine_grid(Variable(affine_mat_after, requires_grad=False).to(self.device), push_feat.data.size()) 205 | 206 | # Forward pass through branches, undo rotation on output predictions, upsample results 207 | self.output_prob.append([F.grid_sample(push_feat, flow_grid_after, mode='nearest'), 208 | F.grid_sample(grasp_feat, flow_grid_after, mode='nearest'), 209 | F.grid_sample(place_feat, flow_grid_after, mode='nearest')]) 210 | 211 | return self.output_prob 212 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | numpy 2 | opencv-python 3 | matplotlib 4 | scipy 5 | torch 6 | torchvision 7 | tensorboardX -------------------------------------------------------------------------------- /robot.py: -------------------------------------------------------------------------------- 1 | import logging 2 | import os 3 | import time 4 | 5 | import numpy as np 6 | 7 | from simulation import sim as vrep 8 | from utils import utils 9 | 10 | 11 | class SimRobot: 12 | def __init__(self, sim_port, obj_mesh_dir, num_obj, workspace_limits, 13 | is_testing, test_preset_cases, test_preset_file, place_enabled): 14 | 15 | self.sim_port = sim_port 16 | self.workspace_limits = workspace_limits 17 | self.place_enabled = place_enabled 18 | 19 | # Define home position 20 | self.home_position = [-0.3, 0.0, 0.45] 21 | 22 | # Define colors for object meshes (Tableau palette) 23 | self.color_space = np.asarray([[78.0, 121.0, 167.0], # blue 24 | [89.0, 161.0, 79.0], # green 25 | [156, 117, 95], # brown 26 | [242, 142, 43], # orange 27 | [237.0, 201.0, 72.0], # yellow 28 | [186, 176, 172], # gray 29 | [255.0, 87.0, 89.0], # red 30 | [176, 122, 161], # purple 31 | [118, 183, 178], # cyan 32 | [255, 157, 167]]) / 255.0 # pink 33 | 34 | # Read files in object mesh directory 35 | self.obj_mesh_dir = obj_mesh_dir 36 | self.num_obj = num_obj 37 | self.num_obj_clear = 0 38 | self.mesh_list = os.listdir(self.obj_mesh_dir) 39 | 40 | # Randomly choose objects to add to scene 41 | self.obj_mesh_ind = np.random.randint(0, len(self.mesh_list), size=self.num_obj) 42 | self.obj_mesh_color = self.color_space[np.asarray(range(self.num_obj)) % 10, :] 43 | 44 | self.is_testing = is_testing 45 | self.test_preset_cases = test_preset_cases 46 | self.test_preset_file = test_preset_file 47 | 48 | # Setup simulation 49 | self.setup_sim() 50 | 51 | # If testing, read object meshes and poses from test case file 52 | if self.test_preset_cases: 53 | file = open(self.test_preset_file, 'r') 54 | file_content = file.readlines() 55 | self.test_obj_mesh_files = [] 56 | self.test_obj_mesh_colors = [] 57 | self.test_obj_positions = [] 58 | self.test_obj_orientations = [] 59 | for object_idx in range(self.num_obj): 60 | file_content_curr_object = file_content[object_idx].split() 61 | self.test_obj_mesh_files.append(os.path.join(self.obj_mesh_dir, file_content_curr_object[0])) 62 | self.test_obj_mesh_colors.append( 63 | [float(file_content_curr_object[1]), float(file_content_curr_object[2]), 64 | float(file_content_curr_object[3])]) 65 | self.test_obj_positions.append([float(file_content_curr_object[4]), float(file_content_curr_object[5]), 66 | float(file_content_curr_object[6])]) 67 | self.test_obj_orientations.append( 68 | [float(file_content_curr_object[7]), float(file_content_curr_object[8]), 69 | float(file_content_curr_object[9])]) 70 | file.close() 71 | self.obj_mesh_color = np.asarray(self.test_obj_mesh_colors) 72 | 73 | # Add objects to simulation environment 74 | self.add_objects() 75 | 76 | def setup_sim(self): 77 | # Connect to simulator 78 | self.sim_client = -1 79 | vrep.simxFinish(-1) # Just in case, close all opened connections 80 | logging.info('Connecting to simulation...') 81 | while self.sim_client == -1: 82 | self.sim_client = vrep.simxStart('127.0.0.1', self.sim_port, True, True, 5000, 5) 83 | if self.sim_client == -1: 84 | logging.error('Failed to connect to simulation. Trying again..') 85 | time.sleep(5) 86 | else: 87 | logging.info('Connected to simulation.') 88 | self.restart_sim() 89 | break 90 | 91 | # Get handle to camera 92 | sim_ret, self.cam_handle = vrep.simxGetObjectHandle(self.sim_client, 'Vision_sensor_persp', 93 | vrep.simx_opmode_blocking) 94 | 95 | # Get camera pose and intrinsics in simulation 96 | sim_ret, cam_position = vrep.simxGetObjectPosition(self.sim_client, self.cam_handle, -1, 97 | vrep.simx_opmode_blocking) 98 | sim_ret, cam_orientation = vrep.simxGetObjectOrientation(self.sim_client, self.cam_handle, -1, 99 | vrep.simx_opmode_blocking) 100 | cam_trans = np.eye(4, 4) 101 | cam_trans[0:3, 3] = np.asarray(cam_position) 102 | cam_orientation = [-cam_orientation[0], -cam_orientation[1], -cam_orientation[2]] 103 | cam_rotm = np.eye(4, 4) 104 | cam_rotm[0:3, 0:3] = np.linalg.inv(utils.euler2rotm(cam_orientation)) 105 | self.cam_pose = np.dot(cam_trans, cam_rotm) # Compute rigid transformation representating camera pose 106 | self.cam_intrinsics = np.asarray([[618.62, 0, 320], [0, 618.62, 240], [0, 0, 1]]) 107 | self.cam_depth_scale = 1 108 | 109 | # Get background image 110 | self.bg_color_img, self.bg_depth_img = self.get_camera_data() 111 | self.bg_depth_img = self.bg_depth_img * self.cam_depth_scale 112 | 113 | def add_objects(self): 114 | # Add each object to robot workspace at x,y location and orientation (random or pre-loaded) 115 | logging.info('Adding objects to the scene..') 116 | self.object_handles = [] 117 | self.num_obj_clear = 0 118 | for object_idx in range(len(self.obj_mesh_ind)): 119 | curr_mesh_file = os.path.join(self.obj_mesh_dir, self.mesh_list[self.obj_mesh_ind[object_idx]]) 120 | if self.test_preset_cases: 121 | curr_mesh_file = self.test_obj_mesh_files[object_idx] 122 | curr_shape_name = 'shape_%02d' % object_idx 123 | drop_x = (self.workspace_limits[0][1] - self.workspace_limits[0][0] - 0.2) * np.random.random_sample() + \ 124 | self.workspace_limits[0][0] + 0.1 125 | drop_y = (self.workspace_limits[1][1] - self.workspace_limits[1][0] - 0.2) * np.random.random_sample() + \ 126 | self.workspace_limits[1][0] + 0.1 127 | object_position = [drop_x, drop_y, 0.15] 128 | object_orientation = [2 * np.pi * np.random.random_sample(), 2 * np.pi * np.random.random_sample(), 129 | 2 * np.pi * np.random.random_sample()] 130 | if self.test_preset_cases: 131 | object_position = [self.test_obj_positions[object_idx][0], self.test_obj_positions[object_idx][1], 132 | self.test_obj_positions[object_idx][2]] 133 | object_orientation = [self.test_obj_orientations[object_idx][0], 134 | self.test_obj_orientations[object_idx][1], 135 | self.test_obj_orientations[object_idx][2]] 136 | object_color = [self.obj_mesh_color[object_idx][0], self.obj_mesh_color[object_idx][1], 137 | self.obj_mesh_color[object_idx][2]] 138 | ret_resp, ret_ints, ret_floats, ret_strings, ret_buffer = vrep.simxCallScriptFunction(self.sim_client, 139 | 'remoteApiCommandServer', 140 | vrep.sim_scripttype_childscript, 141 | 'importShape', 142 | [0, 0, 255, 0], 143 | object_position + object_orientation + object_color, 144 | [curr_mesh_file, 145 | curr_shape_name], 146 | bytearray(), 147 | vrep.simx_opmode_blocking) 148 | if ret_resp == 8: 149 | logging.error('Failed to add new objects to simulation. Restarting..') 150 | self.setup_sim() 151 | else: 152 | curr_shape_handle = ret_ints[0] 153 | self.object_handles.append(curr_shape_handle) 154 | if not self.test_preset_cases: 155 | time.sleep(0.5) 156 | self.prev_obj_positions = [] 157 | self.obj_positions = [] 158 | 159 | def restart_sim(self): 160 | sim_ret, self.UR5_target_handle = vrep.simxGetObjectHandle(self.sim_client, 'UR5_target', 161 | vrep.simx_opmode_blocking) 162 | vrep.simxSetObjectPosition(self.sim_client, self.UR5_target_handle, -1, (-0.5, 0, 0.3), 163 | vrep.simx_opmode_blocking) 164 | vrep.simxStopSimulation(self.sim_client, vrep.simx_opmode_blocking) 165 | vrep.simxStartSimulation(self.sim_client, vrep.simx_opmode_blocking) 166 | time.sleep(1) 167 | sim_ret, self.RG2_tip_handle = vrep.simxGetObjectHandle(self.sim_client, 'UR5_tip', vrep.simx_opmode_blocking) 168 | sim_ret, gripper_position = vrep.simxGetObjectPosition(self.sim_client, self.RG2_tip_handle, -1, 169 | vrep.simx_opmode_blocking) 170 | while gripper_position[2] > 0.4: # V-REP bug requiring multiple starts and stops to restart 171 | vrep.simxStopSimulation(self.sim_client, vrep.simx_opmode_blocking) 172 | vrep.simxStartSimulation(self.sim_client, vrep.simx_opmode_blocking) 173 | time.sleep(1) 174 | sim_ret, gripper_position = vrep.simxGetObjectPosition(self.sim_client, self.RG2_tip_handle, -1, 175 | vrep.simx_opmode_blocking) 176 | 177 | def check_sim(self): 178 | # Check if simulation is stable by checking if gripper is within workspace 179 | sim_ret, gripper_position = vrep.simxGetObjectPosition(self.sim_client, self.RG2_tip_handle, -1, 180 | vrep.simx_opmode_blocking) 181 | sim_ok = self.workspace_limits[0][0] - 0.1 < gripper_position[0] < self.workspace_limits[0][1] + 0.1 and \ 182 | self.workspace_limits[1][0] - 0.1 < gripper_position[1] < self.workspace_limits[1][1] + 0.1 and \ 183 | self.workspace_limits[2][0] < gripper_position[2] < self.workspace_limits[2][1] 184 | if not sim_ok: 185 | logging.info('Simulation unstable. Restarting environment.') 186 | self.restart_sim() 187 | self.add_objects() 188 | 189 | def get_task_score(self): 190 | key_positions = np.asarray([[-0.625, 0.125, 0.0], # red 191 | [-0.625, -0.125, 0.0], # blue 192 | [-0.375, 0.125, 0.0], # green 193 | [-0.375, -0.125, 0.0]]) # yellow 194 | 195 | obj_positions = np.asarray(self.get_obj_positions()) 196 | obj_positions.shape = (1, obj_positions.shape[0], obj_positions.shape[1]) 197 | obj_positions = np.tile(obj_positions, (key_positions.shape[0], 1, 1)) 198 | 199 | key_positions.shape = (key_positions.shape[0], 1, key_positions.shape[1]) 200 | key_positions = np.tile(key_positions, (1, obj_positions.shape[1], 1)) 201 | 202 | key_dist = np.sqrt(np.sum(np.power(obj_positions - key_positions, 2), axis=2)) 203 | key_nn_idx = np.argmin(key_dist, axis=0) 204 | 205 | return np.sum(key_nn_idx == np.asarray(range(self.num_obj)) % 4) 206 | 207 | def check_goal_reached(self): 208 | goal_reached = self.get_task_score() == self.num_obj 209 | return goal_reached 210 | 211 | def get_obj_positions(self): 212 | obj_positions = [] 213 | for object_handle in self.object_handles: 214 | sim_ret, object_position = vrep.simxGetObjectPosition(self.sim_client, object_handle, -1, 215 | vrep.simx_opmode_blocking) 216 | obj_positions.append(object_position) 217 | return obj_positions 218 | 219 | def get_obj_positions_and_orientations(self): 220 | obj_positions = [] 221 | obj_orientations = [] 222 | for object_handle in self.object_handles: 223 | sim_ret, object_position = vrep.simxGetObjectPosition(self.sim_client, object_handle, -1, 224 | vrep.simx_opmode_blocking) 225 | sim_ret, object_orientation = vrep.simxGetObjectOrientation(self.sim_client, object_handle, -1, 226 | vrep.simx_opmode_blocking) 227 | obj_positions.append(object_position) 228 | obj_orientations.append(object_orientation) 229 | return obj_positions, obj_orientations 230 | 231 | def reposition_objects(self, workspace_limits): 232 | # Move gripper out of the way 233 | self.move_to([-0.1, 0, 0.3], None) 234 | 235 | for object_handle in self.object_handles: 236 | # Drop object at random x,y location and random orientation in robot workspace 237 | drop_x = (workspace_limits[0][1] - workspace_limits[0][0] - 0.2) * np.random.random_sample() + \ 238 | workspace_limits[0][0] + 0.1 239 | drop_y = (workspace_limits[1][1] - workspace_limits[1][0] - 0.2) * np.random.random_sample() + \ 240 | workspace_limits[1][0] + 0.1 241 | object_position = [drop_x, drop_y, 0.15] 242 | object_orientation = [2 * np.pi * np.random.random_sample(), 2 * np.pi * np.random.random_sample(), 243 | 2 * np.pi * np.random.random_sample()] 244 | vrep.simxSetObjectPosition(self.sim_client, object_handle, -1, object_position, vrep.simx_opmode_blocking) 245 | vrep.simxSetObjectOrientation(self.sim_client, object_handle, -1, object_orientation, 246 | vrep.simx_opmode_blocking) 247 | time.sleep(2) 248 | 249 | def get_camera_data(self): 250 | # Get color image from simulation 251 | sim_ret, resolution, raw_image = vrep.simxGetVisionSensorImage(self.sim_client, self.cam_handle, 0, 252 | vrep.simx_opmode_blocking) 253 | color_img = np.asarray(raw_image) 254 | color_img.shape = (resolution[1], resolution[0], 3) 255 | color_img = color_img.astype(np.float) / 255 256 | color_img[color_img < 0] += 1 257 | color_img *= 255 258 | color_img = np.fliplr(color_img) 259 | color_img = color_img.astype(np.uint8) 260 | 261 | # Get depth image from simulation 262 | sim_ret, resolution, depth_buffer = vrep.simxGetVisionSensorDepthBuffer(self.sim_client, self.cam_handle, 263 | vrep.simx_opmode_blocking) 264 | depth_img = np.asarray(depth_buffer) 265 | depth_img.shape = (resolution[1], resolution[0]) 266 | depth_img = np.fliplr(depth_img) 267 | zNear = 0.01 268 | zFar = 10 269 | depth_img = depth_img * (zFar - zNear) + zNear 270 | 271 | return color_img, depth_img 272 | 273 | def close_gripper(self): 274 | gripper_motor_velocity = -0.5 275 | gripper_motor_force = 100 276 | sim_ret, RG2_gripper_handle = vrep.simxGetObjectHandle(self.sim_client, 'RG2_openCloseJoint', 277 | vrep.simx_opmode_blocking) 278 | sim_ret, gripper_joint_position = vrep.simxGetJointPosition(self.sim_client, RG2_gripper_handle, 279 | vrep.simx_opmode_blocking) 280 | vrep.simxSetJointForce(self.sim_client, RG2_gripper_handle, gripper_motor_force, vrep.simx_opmode_blocking) 281 | vrep.simxSetJointTargetVelocity(self.sim_client, RG2_gripper_handle, gripper_motor_velocity, 282 | vrep.simx_opmode_blocking) 283 | gripper_fully_closed = False 284 | while gripper_joint_position > -0.045: # Block until gripper is fully closed 285 | sim_ret, new_gripper_joint_position = vrep.simxGetJointPosition(self.sim_client, RG2_gripper_handle, 286 | vrep.simx_opmode_blocking) 287 | # logging.info(gripper_joint_position) 288 | if new_gripper_joint_position >= gripper_joint_position: 289 | return gripper_fully_closed 290 | gripper_joint_position = new_gripper_joint_position 291 | gripper_fully_closed = True 292 | 293 | return gripper_fully_closed 294 | 295 | def open_gripper(self): 296 | gripper_motor_velocity = 0.5 297 | gripper_motor_force = 20 298 | sim_ret, RG2_gripper_handle = vrep.simxGetObjectHandle(self.sim_client, 'RG2_openCloseJoint', 299 | vrep.simx_opmode_blocking) 300 | sim_ret, gripper_joint_position = vrep.simxGetJointPosition(self.sim_client, RG2_gripper_handle, 301 | vrep.simx_opmode_blocking) 302 | vrep.simxSetJointForce(self.sim_client, RG2_gripper_handle, gripper_motor_force, vrep.simx_opmode_blocking) 303 | vrep.simxSetJointTargetVelocity(self.sim_client, RG2_gripper_handle, gripper_motor_velocity, 304 | vrep.simx_opmode_blocking) 305 | while gripper_joint_position < 0.03: # Block until gripper is fully open 306 | sim_ret, gripper_joint_position = vrep.simxGetJointPosition(self.sim_client, RG2_gripper_handle, 307 | vrep.simx_opmode_blocking) 308 | 309 | def move_to(self, tool_position, tool_orientation): 310 | sim_ret, UR5_target_position = vrep.simxGetObjectPosition(self.sim_client, self.UR5_target_handle, -1, 311 | vrep.simx_opmode_blocking) 312 | 313 | move_direction = np.asarray( 314 | [tool_position[0] - UR5_target_position[0], tool_position[1] - UR5_target_position[1], 315 | tool_position[2] - UR5_target_position[2]]) 316 | move_magnitude = np.linalg.norm(move_direction) 317 | move_step = 0.02 * move_direction / move_magnitude 318 | try: 319 | num_move_steps = int(np.floor(move_magnitude / 0.02)) 320 | except ValueError: 321 | return False 322 | 323 | for step_iter in range(num_move_steps): 324 | vrep.simxSetObjectPosition(self.sim_client, self.UR5_target_handle, -1, ( 325 | UR5_target_position[0] + move_step[0], UR5_target_position[1] + move_step[1], 326 | UR5_target_position[2] + move_step[2]), vrep.simx_opmode_blocking) 327 | sim_ret, UR5_target_position = vrep.simxGetObjectPosition(self.sim_client, self.UR5_target_handle, -1, 328 | vrep.simx_opmode_blocking) 329 | vrep.simxSetObjectPosition(self.sim_client, self.UR5_target_handle, -1, 330 | (tool_position[0], tool_position[1], tool_position[2]), vrep.simx_opmode_blocking) 331 | 332 | def go_home(self): 333 | self.move_to(self.home_position, None) 334 | 335 | # Primitives ---------------------------------------------------------- 336 | def grasp(self, position, heightmap_rotation_angle, grasp_vertical_offset=-0.04, grasp_location_margin=0.15, ): 337 | logging.info('Executing: grasp at (%f, %f, %f)' % (position[0], position[1], position[2])) 338 | 339 | # Compute tool orientation from heightmap rotation angle 340 | tool_rotation_angle = (heightmap_rotation_angle % np.pi) - np.pi / 2 341 | 342 | # Avoid collision with floor 343 | position = np.asarray(position).copy() 344 | position[2] = max(position[2] + grasp_vertical_offset, self.workspace_limits[2][0] + 0.02) 345 | 346 | # Move gripper to location above grasp target 347 | location_above_grasp_target = (position[0], position[1], position[2] + grasp_location_margin) 348 | 349 | # Compute gripper position and linear movement increments 350 | tool_position = location_above_grasp_target 351 | sim_ret, UR5_target_position = vrep.simxGetObjectPosition(self.sim_client, self.UR5_target_handle, -1, 352 | vrep.simx_opmode_blocking) 353 | move_direction = np.asarray( 354 | [tool_position[0] - UR5_target_position[0], tool_position[1] - UR5_target_position[1], 355 | tool_position[2] - UR5_target_position[2]]) 356 | move_magnitude = np.linalg.norm(move_direction) 357 | move_step = 0.05 * move_direction / move_magnitude 358 | try: 359 | num_move_steps = int(np.floor(move_direction[0] / move_step[0])) 360 | except ValueError: 361 | return False 362 | 363 | # Compute gripper orientation and rotation increments 364 | sim_ret, gripper_orientation = vrep.simxGetObjectOrientation(self.sim_client, self.UR5_target_handle, -1, 365 | vrep.simx_opmode_blocking) 366 | rotation_step = 0.3 if (tool_rotation_angle - gripper_orientation[1] > 0) else -0.3 367 | num_rotation_steps = int(np.floor((tool_rotation_angle - gripper_orientation[1]) / rotation_step)) 368 | 369 | # Simultaneously move and rotate gripper 370 | for step_iter in range(max(num_move_steps, num_rotation_steps)): 371 | vrep.simxSetObjectPosition(self.sim_client, self.UR5_target_handle, -1, ( 372 | UR5_target_position[0] + move_step[0] * min(step_iter, num_move_steps), 373 | UR5_target_position[1] + move_step[1] * min(step_iter, num_move_steps), 374 | UR5_target_position[2] + move_step[2] * min(step_iter, num_move_steps)), vrep.simx_opmode_blocking) 375 | vrep.simxSetObjectOrientation(self.sim_client, self.UR5_target_handle, -1, ( 376 | np.pi / 2, gripper_orientation[1] + rotation_step * min(step_iter, num_rotation_steps), np.pi / 2), 377 | vrep.simx_opmode_blocking) 378 | vrep.simxSetObjectPosition(self.sim_client, self.UR5_target_handle, -1, 379 | (tool_position[0], tool_position[1], tool_position[2]), vrep.simx_opmode_blocking) 380 | vrep.simxSetObjectOrientation(self.sim_client, self.UR5_target_handle, -1, 381 | (np.pi / 2, tool_rotation_angle, np.pi / 2), vrep.simx_opmode_blocking) 382 | 383 | # Ensure gripper is open 384 | self.open_gripper() 385 | 386 | # Approach grasp target 387 | self.move_to(position, None) 388 | 389 | # Close gripper to grasp target 390 | gripper_full_closed = self.close_gripper() 391 | 392 | # Move gripper to location above grasp target 393 | self.move_to(location_above_grasp_target, None) 394 | 395 | # Check if grasp is successful 396 | gripper_full_closed = self.close_gripper() 397 | grasp_success = not gripper_full_closed 398 | 399 | # Move the grasped object elsewhere 400 | if grasp_success: 401 | if self.place_enabled: 402 | self.go_home() 403 | else: 404 | self.num_obj_clear += 1 405 | object_positions = np.asarray(self.get_obj_positions()) 406 | object_positions = object_positions[:, 2] 407 | grasped_object_ind = np.argmax(object_positions) 408 | grasped_object_handle = self.object_handles[grasped_object_ind] 409 | vrep.simxSetObjectPosition(self.sim_client, grasped_object_handle, -1, 410 | (-0.5, 0.5 + 0.05 * float(grasped_object_ind), 0.1), vrep.simx_opmode_blocking) 411 | 412 | return grasp_success 413 | 414 | def push(self, position, heightmap_rotation_angle, push_vertical_offset=0.01, pushing_point_margin=0.1): 415 | logging.info('Executing: push at (%f, %f, %f)' % (position[0], position[1], position[2])) 416 | 417 | # Compute tool orientation from heightmap rotation angle 418 | tool_rotation_angle = (heightmap_rotation_angle % np.pi) - np.pi / 2 419 | 420 | # Adjust pushing point to be on tip of finger 421 | position[2] = position[2] + push_vertical_offset 422 | 423 | # Compute pushing direction 424 | push_orientation = [1.0, 0.0] 425 | push_direction = np.asarray([push_orientation[0] * np.cos(heightmap_rotation_angle) - push_orientation[ 426 | 1] * np.sin(heightmap_rotation_angle), 427 | push_orientation[0] * np.sin(heightmap_rotation_angle) + push_orientation[ 428 | 1] * np.cos(heightmap_rotation_angle)]) 429 | 430 | # Move gripper to location above pushing point 431 | location_above_pushing_point = (position[0], position[1], position[2] + pushing_point_margin) 432 | 433 | # Compute gripper position and linear movement increments 434 | tool_position = location_above_pushing_point 435 | sim_ret, UR5_target_position = vrep.simxGetObjectPosition(self.sim_client, self.UR5_target_handle, -1, 436 | vrep.simx_opmode_blocking) 437 | move_direction = np.asarray( 438 | [tool_position[0] - UR5_target_position[0], tool_position[1] - UR5_target_position[1], 439 | tool_position[2] - UR5_target_position[2]]) 440 | move_magnitude = np.linalg.norm(move_direction) 441 | move_step = 0.05 * move_direction / move_magnitude 442 | try: 443 | num_move_steps = int(np.floor(move_direction[0] / move_step[0])) 444 | except ValueError: 445 | return False 446 | 447 | # Compute gripper orientation and rotation increments 448 | sim_ret, gripper_orientation = vrep.simxGetObjectOrientation(self.sim_client, self.UR5_target_handle, -1, 449 | vrep.simx_opmode_blocking) 450 | rotation_step = 0.3 if (tool_rotation_angle - gripper_orientation[1] > 0) else -0.3 451 | num_rotation_steps = int(np.floor((tool_rotation_angle - gripper_orientation[1]) / rotation_step)) 452 | 453 | # Simultaneously move and rotate gripper 454 | for step_iter in range(max(num_move_steps, num_rotation_steps)): 455 | vrep.simxSetObjectPosition(self.sim_client, self.UR5_target_handle, -1, ( 456 | UR5_target_position[0] + move_step[0] * min(step_iter, num_move_steps), 457 | UR5_target_position[1] + move_step[1] * min(step_iter, num_move_steps), 458 | UR5_target_position[2] + move_step[2] * min(step_iter, num_move_steps)), vrep.simx_opmode_blocking) 459 | vrep.simxSetObjectOrientation(self.sim_client, self.UR5_target_handle, -1, ( 460 | np.pi / 2, gripper_orientation[1] + rotation_step * min(step_iter, num_rotation_steps), np.pi / 2), 461 | vrep.simx_opmode_blocking) 462 | vrep.simxSetObjectPosition(self.sim_client, self.UR5_target_handle, -1, 463 | (tool_position[0], tool_position[1], tool_position[2]), vrep.simx_opmode_blocking) 464 | vrep.simxSetObjectOrientation(self.sim_client, self.UR5_target_handle, -1, 465 | (np.pi / 2, tool_rotation_angle, np.pi / 2), vrep.simx_opmode_blocking) 466 | 467 | # Ensure gripper is closed 468 | self.close_gripper() 469 | 470 | # Approach pushing point 471 | self.move_to(position, None) 472 | 473 | # Compute target location (push to the right) 474 | push_length = 0.1 475 | target_x = min(max(position[0] + push_direction[0] * push_length, self.workspace_limits[0][0]), 476 | self.workspace_limits[0][1]) 477 | target_y = min(max(position[1] + push_direction[1] * push_length, self.workspace_limits[1][0]), 478 | self.workspace_limits[1][1]) 479 | push_length = np.sqrt(np.power(target_x - position[0], 2) + np.power(target_y - position[1], 2)) 480 | 481 | # Move in pushing direction towards target location 482 | self.move_to([target_x, target_y, position[2]], None) 483 | 484 | # Move gripper to location above grasp target 485 | self.move_to([target_x, target_y, location_above_pushing_point[2]], None) 486 | 487 | push_success = True 488 | 489 | return push_success 490 | 491 | def place(self, position, heightmap_rotation_angle, place_vertical_offset=0.04): 492 | logging.info('Executing: place at (%f, %f, %f)' % (position[0], position[1], position[2])) 493 | 494 | # Ensure gripper is closed 495 | gripper_fully_closed = self.close_gripper() 496 | if gripper_fully_closed: 497 | # There is no object present, so we cannot possibly place! 498 | return False 499 | 500 | # Compute tool orientation from heightmap rotation angle 501 | tool_rotation_angle = (heightmap_rotation_angle % np.pi) - np.pi / 2 502 | 503 | # Avoid collision with floor 504 | position[2] = max(position[2] + place_vertical_offset, self.workspace_limits[2][0] + 0.02) 505 | 506 | # Move gripper to location above place target 507 | place_location_margin = 0.1 508 | sim_ret, UR5_target_handle = vrep.simxGetObjectHandle(self.sim_client, 'UR5_target', vrep.simx_opmode_blocking) 509 | location_above_place_target = (position[0], position[1], position[2] + place_location_margin) 510 | self.move_to(location_above_place_target, None) 511 | 512 | sim_ret, gripper_orientation = vrep.simxGetObjectOrientation(self.sim_client, UR5_target_handle, -1, 513 | vrep.simx_opmode_blocking) 514 | if tool_rotation_angle - gripper_orientation[1] > 0: 515 | increment = 0.2 516 | else: 517 | increment = -0.2 518 | while abs(tool_rotation_angle - gripper_orientation[1]) >= 0.2: 519 | vrep.simxSetObjectOrientation(self.sim_client, UR5_target_handle, -1, 520 | (np.pi / 2, gripper_orientation[1] + increment, np.pi / 2), 521 | vrep.simx_opmode_blocking) 522 | time.sleep(0.01) 523 | sim_ret, gripper_orientation = vrep.simxGetObjectOrientation(self.sim_client, UR5_target_handle, -1, 524 | vrep.simx_opmode_blocking) 525 | vrep.simxSetObjectOrientation(self.sim_client, UR5_target_handle, -1, 526 | (np.pi / 2, tool_rotation_angle, np.pi / 2), vrep.simx_opmode_blocking) 527 | 528 | # Approach place target 529 | self.move_to(position, None) 530 | 531 | # Ensure gripper is open 532 | self.open_gripper() 533 | 534 | # Move gripper to location above place target 535 | self.move_to(location_above_place_target, None) 536 | 537 | return True 538 | 539 | def shutdown(self): 540 | logging.info('Shutting down simulation..') 541 | vrep.simxStopSimulation(self.sim_client, vrep.simx_opmode_oneshot) 542 | vrep.simxSynchronousTrigger(self.sim_client) 543 | time.sleep(1) 544 | vrep.simxFinish(self.sim_client) 545 | vrep.simxFinish(-1) 546 | logging.info("Disconnected from simulation.") 547 | -------------------------------------------------------------------------------- /simulation/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skumra/romannet/0af092a1be26ac5f213f1e5c21f81f89699a4c92/simulation/__init__.py -------------------------------------------------------------------------------- /simulation/objects/blocks/0.obj: -------------------------------------------------------------------------------- 1 | #### 2 | # 3 | # OBJ File Generated by Meshlab 4 | # 5 | #### 6 | # Object 4.obj 7 | # 8 | # Vertices: 37 9 | # Faces: 72 10 | # 11 | #### 12 | v 0.025000 0.000074 0.023000 13 | v 0.025000 0.000074 -0.023000 14 | v 0.022932 0.023068 0.025000 15 | v 0.023000 0.000068 0.025000 16 | v 0.023290 0.025036 0.023000 17 | v 0.025068 -0.022926 0.023000 18 | v 0.023668 0.023070 -0.024860 19 | v -0.023074 0.024898 -0.023364 20 | v -0.000070 0.023736 -0.024860 21 | v -0.000074 0.025000 -0.023000 22 | v -0.023074 0.024932 0.000000 23 | v -0.023074 0.024932 0.023000 24 | v -0.000068 0.023000 0.025000 25 | v -0.000074 0.025000 0.023000 26 | v -0.025000 -0.000074 -0.023000 27 | v -0.023000 -0.000068 -0.025000 28 | v -0.025000 -0.000074 0.023000 29 | v -0.024932 -0.023074 0.023000 30 | v 0.023438 -0.024897 -0.023000 31 | v -0.023296 -0.023069 -0.024967 32 | v 0.000074 -0.025000 0.023000 33 | v 0.000068 -0.023000 0.025000 34 | v 0.023068 -0.022932 -0.025000 35 | v 0.025068 -0.022926 -0.023000 36 | v 0.023290 0.025036 -0.023000 37 | v 0.024932 0.023074 -0.023000 38 | v 0.024932 0.023074 0.023000 39 | v 0.023068 -0.022932 0.025000 40 | v 0.023074 -0.024932 0.023000 41 | v -0.023586 0.022930 -0.024932 42 | v -0.025068 0.022926 -0.023000 43 | v -0.024742 0.022927 0.024094 44 | v -0.023070 0.023668 0.024860 45 | v -0.022926 -0.025068 -0.023000 46 | v -0.024791 -0.023073 -0.023736 47 | v -0.022929 -0.024162 0.024674 48 | v -0.023290 -0.025036 0.023000 49 | # 37 vertices, 0 vertices normals 50 | 51 | f 1 6 24 52 | f 1 24 2 53 | f 2 26 27 54 | f 2 27 1 55 | f 14 5 25 56 | f 14 25 10 57 | f 11 10 8 58 | f 11 12 14 59 | f 10 11 14 60 | f 17 32 31 61 | f 17 31 15 62 | f 15 35 18 63 | f 15 18 17 64 | f 21 37 34 65 | f 29 21 19 66 | f 34 19 21 67 | f 16 23 20 68 | f 30 7 16 69 | f 23 16 7 70 | f 4 22 28 71 | f 36 22 13 72 | f 36 13 33 73 | f 13 4 3 74 | f 4 13 22 75 | f 23 7 26 76 | f 23 26 24 77 | f 2 24 26 78 | f 5 27 26 79 | f 5 26 25 80 | f 4 28 1 81 | f 1 28 6 82 | f 27 3 1 83 | f 3 4 1 84 | f 19 24 29 85 | f 24 6 29 86 | f 10 25 7 87 | f 25 8 10 88 | f 25 10 8 89 | f 9 30 8 90 | f 9 8 10 91 | f 7 9 10 92 | f 7 30 9 93 | f 32 12 8 94 | f 32 8 31 95 | f 11 8 12 96 | f 13 3 14 97 | f 14 3 5 98 | f 12 33 14 99 | f 33 13 14 100 | f 15 31 30 101 | f 15 30 16 102 | f 16 35 15 103 | f 16 20 35 104 | f 37 18 34 105 | f 18 35 34 106 | f 33 18 36 107 | f 33 32 18 108 | f 17 18 32 109 | f 20 23 34 110 | f 34 23 19 111 | f 21 22 37 112 | f 22 36 37 113 | f 22 21 29 114 | f 29 28 22 115 | f 24 19 23 116 | f 26 7 25 117 | f 3 27 5 118 | f 29 6 28 119 | f 30 31 8 120 | f 32 33 12 121 | f 20 34 35 122 | f 18 37 36 123 | # 72 faces, 0 coords texture 124 | 125 | # End of File -------------------------------------------------------------------------------- /simulation/objects/blocks/1.obj: -------------------------------------------------------------------------------- 1 | #### 2 | # 3 | # OBJ File Generated by Meshlab 4 | # 5 | #### 6 | # Object 4.obj 7 | # 8 | # Vertices: 37 9 | # Faces: 72 10 | # 11 | #### 12 | v 0.025000 0.000074 0.023000 13 | v 0.025000 0.000074 -0.023000 14 | v 0.022932 0.023068 0.025000 15 | v 0.023000 0.000068 0.025000 16 | v 0.023290 0.025036 0.023000 17 | v 0.025068 -0.022926 0.023000 18 | v 0.023668 0.023070 -0.024860 19 | v -0.023074 0.024898 -0.023364 20 | v -0.000070 0.023736 -0.024860 21 | v -0.000074 0.025000 -0.023000 22 | v -0.023074 0.024932 0.000000 23 | v -0.023074 0.024932 0.023000 24 | v -0.000068 0.023000 0.025000 25 | v -0.000074 0.025000 0.023000 26 | v -0.025000 -0.000074 -0.023000 27 | v -0.023000 -0.000068 -0.025000 28 | v -0.025000 -0.000074 0.023000 29 | v -0.024932 -0.023074 0.023000 30 | v 0.023438 -0.024897 -0.023000 31 | v -0.023296 -0.023069 -0.024967 32 | v 0.000074 -0.025000 0.023000 33 | v 0.000068 -0.023000 0.025000 34 | v 0.023068 -0.022932 -0.025000 35 | v 0.025068 -0.022926 -0.023000 36 | v 0.023290 0.025036 -0.023000 37 | v 0.024932 0.023074 -0.023000 38 | v 0.024932 0.023074 0.023000 39 | v 0.023068 -0.022932 0.025000 40 | v 0.023074 -0.024932 0.023000 41 | v -0.023586 0.022930 -0.024932 42 | v -0.025068 0.022926 -0.023000 43 | v -0.024742 0.022927 0.024094 44 | v -0.023070 0.023668 0.024860 45 | v -0.022926 -0.025068 -0.023000 46 | v -0.024791 -0.023073 -0.023736 47 | v -0.022929 -0.024162 0.024674 48 | v -0.023290 -0.025036 0.023000 49 | # 37 vertices, 0 vertices normals 50 | 51 | f 1 6 24 52 | f 1 24 2 53 | f 2 26 27 54 | f 2 27 1 55 | f 14 5 25 56 | f 14 25 10 57 | f 11 10 8 58 | f 11 12 14 59 | f 10 11 14 60 | f 17 32 31 61 | f 17 31 15 62 | f 15 35 18 63 | f 15 18 17 64 | f 21 37 34 65 | f 29 21 19 66 | f 34 19 21 67 | f 16 23 20 68 | f 30 7 16 69 | f 23 16 7 70 | f 4 22 28 71 | f 36 22 13 72 | f 36 13 33 73 | f 13 4 3 74 | f 4 13 22 75 | f 23 7 26 76 | f 23 26 24 77 | f 2 24 26 78 | f 5 27 26 79 | f 5 26 25 80 | f 4 28 1 81 | f 1 28 6 82 | f 27 3 1 83 | f 3 4 1 84 | f 19 24 29 85 | f 24 6 29 86 | f 10 25 7 87 | f 25 8 10 88 | f 25 10 8 89 | f 9 30 8 90 | f 9 8 10 91 | f 7 9 10 92 | f 7 30 9 93 | f 32 12 8 94 | f 32 8 31 95 | f 11 8 12 96 | f 13 3 14 97 | f 14 3 5 98 | f 12 33 14 99 | f 33 13 14 100 | f 15 31 30 101 | f 15 30 16 102 | f 16 35 15 103 | f 16 20 35 104 | f 37 18 34 105 | f 18 35 34 106 | f 33 18 36 107 | f 33 32 18 108 | f 17 18 32 109 | f 20 23 34 110 | f 34 23 19 111 | f 21 22 37 112 | f 22 36 37 113 | f 22 21 29 114 | f 29 28 22 115 | f 24 19 23 116 | f 26 7 25 117 | f 3 27 5 118 | f 29 6 28 119 | f 30 31 8 120 | f 32 33 12 121 | f 20 34 35 122 | f 18 37 36 123 | # 72 faces, 0 coords texture 124 | 125 | # End of File -------------------------------------------------------------------------------- /simulation/objects/blocks/2.obj: -------------------------------------------------------------------------------- 1 | #### 2 | # 3 | # OBJ File Generated by Meshlab 4 | # 5 | #### 6 | # Object 4.obj 7 | # 8 | # Vertices: 37 9 | # Faces: 72 10 | # 11 | #### 12 | v 0.025000 0.000074 0.023000 13 | v 0.025000 0.000074 -0.023000 14 | v 0.022932 0.023068 0.025000 15 | v 0.023000 0.000068 0.025000 16 | v 0.023290 0.025036 0.023000 17 | v 0.025068 -0.022926 0.023000 18 | v 0.023668 0.023070 -0.024860 19 | v -0.023074 0.024898 -0.023364 20 | v -0.000070 0.023736 -0.024860 21 | v -0.000074 0.025000 -0.023000 22 | v -0.023074 0.024932 0.000000 23 | v -0.023074 0.024932 0.023000 24 | v -0.000068 0.023000 0.025000 25 | v -0.000074 0.025000 0.023000 26 | v -0.025000 -0.000074 -0.023000 27 | v -0.023000 -0.000068 -0.025000 28 | v -0.025000 -0.000074 0.023000 29 | v -0.024932 -0.023074 0.023000 30 | v 0.023438 -0.024897 -0.023000 31 | v -0.023296 -0.023069 -0.024967 32 | v 0.000074 -0.025000 0.023000 33 | v 0.000068 -0.023000 0.025000 34 | v 0.023068 -0.022932 -0.025000 35 | v 0.025068 -0.022926 -0.023000 36 | v 0.023290 0.025036 -0.023000 37 | v 0.024932 0.023074 -0.023000 38 | v 0.024932 0.023074 0.023000 39 | v 0.023068 -0.022932 0.025000 40 | v 0.023074 -0.024932 0.023000 41 | v -0.023586 0.022930 -0.024932 42 | v -0.025068 0.022926 -0.023000 43 | v -0.024742 0.022927 0.024094 44 | v -0.023070 0.023668 0.024860 45 | v -0.022926 -0.025068 -0.023000 46 | v -0.024791 -0.023073 -0.023736 47 | v -0.022929 -0.024162 0.024674 48 | v -0.023290 -0.025036 0.023000 49 | # 37 vertices, 0 vertices normals 50 | 51 | f 1 6 24 52 | f 1 24 2 53 | f 2 26 27 54 | f 2 27 1 55 | f 14 5 25 56 | f 14 25 10 57 | f 11 10 8 58 | f 11 12 14 59 | f 10 11 14 60 | f 17 32 31 61 | f 17 31 15 62 | f 15 35 18 63 | f 15 18 17 64 | f 21 37 34 65 | f 29 21 19 66 | f 34 19 21 67 | f 16 23 20 68 | f 30 7 16 69 | f 23 16 7 70 | f 4 22 28 71 | f 36 22 13 72 | f 36 13 33 73 | f 13 4 3 74 | f 4 13 22 75 | f 23 7 26 76 | f 23 26 24 77 | f 2 24 26 78 | f 5 27 26 79 | f 5 26 25 80 | f 4 28 1 81 | f 1 28 6 82 | f 27 3 1 83 | f 3 4 1 84 | f 19 24 29 85 | f 24 6 29 86 | f 10 25 7 87 | f 25 8 10 88 | f 25 10 8 89 | f 9 30 8 90 | f 9 8 10 91 | f 7 9 10 92 | f 7 30 9 93 | f 32 12 8 94 | f 32 8 31 95 | f 11 8 12 96 | f 13 3 14 97 | f 14 3 5 98 | f 12 33 14 99 | f 33 13 14 100 | f 15 31 30 101 | f 15 30 16 102 | f 16 35 15 103 | f 16 20 35 104 | f 37 18 34 105 | f 18 35 34 106 | f 33 18 36 107 | f 33 32 18 108 | f 17 18 32 109 | f 20 23 34 110 | f 34 23 19 111 | f 21 22 37 112 | f 22 36 37 113 | f 22 21 29 114 | f 29 28 22 115 | f 24 19 23 116 | f 26 7 25 117 | f 3 27 5 118 | f 29 6 28 119 | f 30 31 8 120 | f 32 33 12 121 | f 20 34 35 122 | f 18 37 36 123 | # 72 faces, 0 coords texture 124 | 125 | # End of File -------------------------------------------------------------------------------- /simulation/objects/blocks/3.obj: -------------------------------------------------------------------------------- 1 | #### 2 | # 3 | # OBJ File Generated by Meshlab 4 | # 5 | #### 6 | # Object 4.obj 7 | # 8 | # Vertices: 37 9 | # Faces: 72 10 | # 11 | #### 12 | v 0.025000 0.000074 0.023000 13 | v 0.025000 0.000074 -0.023000 14 | v 0.022932 0.023068 0.025000 15 | v 0.023000 0.000068 0.025000 16 | v 0.023290 0.025036 0.023000 17 | v 0.025068 -0.022926 0.023000 18 | v 0.023668 0.023070 -0.024860 19 | v -0.023074 0.024898 -0.023364 20 | v -0.000070 0.023736 -0.024860 21 | v -0.000074 0.025000 -0.023000 22 | v -0.023074 0.024932 0.000000 23 | v -0.023074 0.024932 0.023000 24 | v -0.000068 0.023000 0.025000 25 | v -0.000074 0.025000 0.023000 26 | v -0.025000 -0.000074 -0.023000 27 | v -0.023000 -0.000068 -0.025000 28 | v -0.025000 -0.000074 0.023000 29 | v -0.024932 -0.023074 0.023000 30 | v 0.023438 -0.024897 -0.023000 31 | v -0.023296 -0.023069 -0.024967 32 | v 0.000074 -0.025000 0.023000 33 | v 0.000068 -0.023000 0.025000 34 | v 0.023068 -0.022932 -0.025000 35 | v 0.025068 -0.022926 -0.023000 36 | v 0.023290 0.025036 -0.023000 37 | v 0.024932 0.023074 -0.023000 38 | v 0.024932 0.023074 0.023000 39 | v 0.023068 -0.022932 0.025000 40 | v 0.023074 -0.024932 0.023000 41 | v -0.023586 0.022930 -0.024932 42 | v -0.025068 0.022926 -0.023000 43 | v -0.024742 0.022927 0.024094 44 | v -0.023070 0.023668 0.024860 45 | v -0.022926 -0.025068 -0.023000 46 | v -0.024791 -0.023073 -0.023736 47 | v -0.022929 -0.024162 0.024674 48 | v -0.023290 -0.025036 0.023000 49 | # 37 vertices, 0 vertices normals 50 | 51 | f 1 6 24 52 | f 1 24 2 53 | f 2 26 27 54 | f 2 27 1 55 | f 14 5 25 56 | f 14 25 10 57 | f 11 10 8 58 | f 11 12 14 59 | f 10 11 14 60 | f 17 32 31 61 | f 17 31 15 62 | f 15 35 18 63 | f 15 18 17 64 | f 21 37 34 65 | f 29 21 19 66 | f 34 19 21 67 | f 16 23 20 68 | f 30 7 16 69 | f 23 16 7 70 | f 4 22 28 71 | f 36 22 13 72 | f 36 13 33 73 | f 13 4 3 74 | f 4 13 22 75 | f 23 7 26 76 | f 23 26 24 77 | f 2 24 26 78 | f 5 27 26 79 | f 5 26 25 80 | f 4 28 1 81 | f 1 28 6 82 | f 27 3 1 83 | f 3 4 1 84 | f 19 24 29 85 | f 24 6 29 86 | f 10 25 7 87 | f 25 8 10 88 | f 25 10 8 89 | f 9 30 8 90 | f 9 8 10 91 | f 7 9 10 92 | f 7 30 9 93 | f 32 12 8 94 | f 32 8 31 95 | f 11 8 12 96 | f 13 3 14 97 | f 14 3 5 98 | f 12 33 14 99 | f 33 13 14 100 | f 15 31 30 101 | f 15 30 16 102 | f 16 35 15 103 | f 16 20 35 104 | f 37 18 34 105 | f 18 35 34 106 | f 33 18 36 107 | f 33 32 18 108 | f 17 18 32 109 | f 20 23 34 110 | f 34 23 19 111 | f 21 22 37 112 | f 22 36 37 113 | f 22 21 29 114 | f 29 28 22 115 | f 24 19 23 116 | f 26 7 25 117 | f 3 27 5 118 | f 29 6 28 119 | f 30 31 8 120 | f 32 33 12 121 | f 20 34 35 122 | f 18 37 36 123 | # 72 faces, 0 coords texture 124 | 125 | # End of File -------------------------------------------------------------------------------- /simulation/objects/blocks/4.obj: -------------------------------------------------------------------------------- 1 | #### 2 | # 3 | # OBJ File Generated by Meshlab 4 | # 5 | #### 6 | # Object 4.obj 7 | # 8 | # Vertices: 37 9 | # Faces: 72 10 | # 11 | #### 12 | v 0.025000 0.000074 0.023000 13 | v 0.025000 0.000074 -0.023000 14 | v 0.022932 0.023068 0.025000 15 | v 0.023000 0.000068 0.025000 16 | v 0.023290 0.025036 0.023000 17 | v 0.025068 -0.022926 0.023000 18 | v 0.023668 0.023070 -0.024860 19 | v -0.023074 0.024898 -0.023364 20 | v -0.000070 0.023736 -0.024860 21 | v -0.000074 0.025000 -0.023000 22 | v -0.023074 0.024932 0.000000 23 | v -0.023074 0.024932 0.023000 24 | v -0.000068 0.023000 0.025000 25 | v -0.000074 0.025000 0.023000 26 | v -0.025000 -0.000074 -0.023000 27 | v -0.023000 -0.000068 -0.025000 28 | v -0.025000 -0.000074 0.023000 29 | v -0.024932 -0.023074 0.023000 30 | v 0.023438 -0.024897 -0.023000 31 | v -0.023296 -0.023069 -0.024967 32 | v 0.000074 -0.025000 0.023000 33 | v 0.000068 -0.023000 0.025000 34 | v 0.023068 -0.022932 -0.025000 35 | v 0.025068 -0.022926 -0.023000 36 | v 0.023290 0.025036 -0.023000 37 | v 0.024932 0.023074 -0.023000 38 | v 0.024932 0.023074 0.023000 39 | v 0.023068 -0.022932 0.025000 40 | v 0.023074 -0.024932 0.023000 41 | v -0.023586 0.022930 -0.024932 42 | v -0.025068 0.022926 -0.023000 43 | v -0.024742 0.022927 0.024094 44 | v -0.023070 0.023668 0.024860 45 | v -0.022926 -0.025068 -0.023000 46 | v -0.024791 -0.023073 -0.023736 47 | v -0.022929 -0.024162 0.024674 48 | v -0.023290 -0.025036 0.023000 49 | # 37 vertices, 0 vertices normals 50 | 51 | f 1 6 24 52 | f 1 24 2 53 | f 2 26 27 54 | f 2 27 1 55 | f 14 5 25 56 | f 14 25 10 57 | f 11 10 8 58 | f 11 12 14 59 | f 10 11 14 60 | f 17 32 31 61 | f 17 31 15 62 | f 15 35 18 63 | f 15 18 17 64 | f 21 37 34 65 | f 29 21 19 66 | f 34 19 21 67 | f 16 23 20 68 | f 30 7 16 69 | f 23 16 7 70 | f 4 22 28 71 | f 36 22 13 72 | f 36 13 33 73 | f 13 4 3 74 | f 4 13 22 75 | f 23 7 26 76 | f 23 26 24 77 | f 2 24 26 78 | f 5 27 26 79 | f 5 26 25 80 | f 4 28 1 81 | f 1 28 6 82 | f 27 3 1 83 | f 3 4 1 84 | f 19 24 29 85 | f 24 6 29 86 | f 10 25 7 87 | f 25 8 10 88 | f 25 10 8 89 | f 9 30 8 90 | f 9 8 10 91 | f 7 9 10 92 | f 7 30 9 93 | f 32 12 8 94 | f 32 8 31 95 | f 11 8 12 96 | f 13 3 14 97 | f 14 3 5 98 | f 12 33 14 99 | f 33 13 14 100 | f 15 31 30 101 | f 15 30 16 102 | f 16 35 15 103 | f 16 20 35 104 | f 37 18 34 105 | f 18 35 34 106 | f 33 18 36 107 | f 33 32 18 108 | f 17 18 32 109 | f 20 23 34 110 | f 34 23 19 111 | f 21 22 37 112 | f 22 36 37 113 | f 22 21 29 114 | f 29 28 22 115 | f 24 19 23 116 | f 26 7 25 117 | f 3 27 5 118 | f 29 6 28 119 | f 30 31 8 120 | f 32 33 12 121 | f 20 34 35 122 | f 18 37 36 123 | # 72 faces, 0 coords texture 124 | 125 | # End of File -------------------------------------------------------------------------------- /simulation/objects/blocks/5.obj: -------------------------------------------------------------------------------- 1 | #### 2 | # 3 | # OBJ File Generated by Meshlab 4 | # 5 | #### 6 | # Object 4.obj 7 | # 8 | # Vertices: 37 9 | # Faces: 72 10 | # 11 | #### 12 | v 0.025000 0.000074 0.023000 13 | v 0.025000 0.000074 -0.023000 14 | v 0.022932 0.023068 0.025000 15 | v 0.023000 0.000068 0.025000 16 | v 0.023290 0.025036 0.023000 17 | v 0.025068 -0.022926 0.023000 18 | v 0.023668 0.023070 -0.024860 19 | v -0.023074 0.024898 -0.023364 20 | v -0.000070 0.023736 -0.024860 21 | v -0.000074 0.025000 -0.023000 22 | v -0.023074 0.024932 0.000000 23 | v -0.023074 0.024932 0.023000 24 | v -0.000068 0.023000 0.025000 25 | v -0.000074 0.025000 0.023000 26 | v -0.025000 -0.000074 -0.023000 27 | v -0.023000 -0.000068 -0.025000 28 | v -0.025000 -0.000074 0.023000 29 | v -0.024932 -0.023074 0.023000 30 | v 0.023438 -0.024897 -0.023000 31 | v -0.023296 -0.023069 -0.024967 32 | v 0.000074 -0.025000 0.023000 33 | v 0.000068 -0.023000 0.025000 34 | v 0.023068 -0.022932 -0.025000 35 | v 0.025068 -0.022926 -0.023000 36 | v 0.023290 0.025036 -0.023000 37 | v 0.024932 0.023074 -0.023000 38 | v 0.024932 0.023074 0.023000 39 | v 0.023068 -0.022932 0.025000 40 | v 0.023074 -0.024932 0.023000 41 | v -0.023586 0.022930 -0.024932 42 | v -0.025068 0.022926 -0.023000 43 | v -0.024742 0.022927 0.024094 44 | v -0.023070 0.023668 0.024860 45 | v -0.022926 -0.025068 -0.023000 46 | v -0.024791 -0.023073 -0.023736 47 | v -0.022929 -0.024162 0.024674 48 | v -0.023290 -0.025036 0.023000 49 | # 37 vertices, 0 vertices normals 50 | 51 | f 1 6 24 52 | f 1 24 2 53 | f 2 26 27 54 | f 2 27 1 55 | f 14 5 25 56 | f 14 25 10 57 | f 11 10 8 58 | f 11 12 14 59 | f 10 11 14 60 | f 17 32 31 61 | f 17 31 15 62 | f 15 35 18 63 | f 15 18 17 64 | f 21 37 34 65 | f 29 21 19 66 | f 34 19 21 67 | f 16 23 20 68 | f 30 7 16 69 | f 23 16 7 70 | f 4 22 28 71 | f 36 22 13 72 | f 36 13 33 73 | f 13 4 3 74 | f 4 13 22 75 | f 23 7 26 76 | f 23 26 24 77 | f 2 24 26 78 | f 5 27 26 79 | f 5 26 25 80 | f 4 28 1 81 | f 1 28 6 82 | f 27 3 1 83 | f 3 4 1 84 | f 19 24 29 85 | f 24 6 29 86 | f 10 25 7 87 | f 25 8 10 88 | f 25 10 8 89 | f 9 30 8 90 | f 9 8 10 91 | f 7 9 10 92 | f 7 30 9 93 | f 32 12 8 94 | f 32 8 31 95 | f 11 8 12 96 | f 13 3 14 97 | f 14 3 5 98 | f 12 33 14 99 | f 33 13 14 100 | f 15 31 30 101 | f 15 30 16 102 | f 16 35 15 103 | f 16 20 35 104 | f 37 18 34 105 | f 18 35 34 106 | f 33 18 36 107 | f 33 32 18 108 | f 17 18 32 109 | f 20 23 34 110 | f 34 23 19 111 | f 21 22 37 112 | f 22 36 37 113 | f 22 21 29 114 | f 29 28 22 115 | f 24 19 23 116 | f 26 7 25 117 | f 3 27 5 118 | f 29 6 28 119 | f 30 31 8 120 | f 32 33 12 121 | f 20 34 35 122 | f 18 37 36 123 | # 72 faces, 0 coords texture 124 | 125 | # End of File -------------------------------------------------------------------------------- /simulation/objects/blocks/6.obj: -------------------------------------------------------------------------------- 1 | #### 2 | # 3 | # OBJ File Generated by Meshlab 4 | # 5 | #### 6 | # Object 4.obj 7 | # 8 | # Vertices: 37 9 | # Faces: 72 10 | # 11 | #### 12 | v 0.025000 0.000074 0.023000 13 | v 0.025000 0.000074 -0.023000 14 | v 0.022932 0.023068 0.025000 15 | v 0.023000 0.000068 0.025000 16 | v 0.023290 0.025036 0.023000 17 | v 0.025068 -0.022926 0.023000 18 | v 0.023668 0.023070 -0.024860 19 | v -0.023074 0.024898 -0.023364 20 | v -0.000070 0.023736 -0.024860 21 | v -0.000074 0.025000 -0.023000 22 | v -0.023074 0.024932 0.000000 23 | v -0.023074 0.024932 0.023000 24 | v -0.000068 0.023000 0.025000 25 | v -0.000074 0.025000 0.023000 26 | v -0.025000 -0.000074 -0.023000 27 | v -0.023000 -0.000068 -0.025000 28 | v -0.025000 -0.000074 0.023000 29 | v -0.024932 -0.023074 0.023000 30 | v 0.023438 -0.024897 -0.023000 31 | v -0.023296 -0.023069 -0.024967 32 | v 0.000074 -0.025000 0.023000 33 | v 0.000068 -0.023000 0.025000 34 | v 0.023068 -0.022932 -0.025000 35 | v 0.025068 -0.022926 -0.023000 36 | v 0.023290 0.025036 -0.023000 37 | v 0.024932 0.023074 -0.023000 38 | v 0.024932 0.023074 0.023000 39 | v 0.023068 -0.022932 0.025000 40 | v 0.023074 -0.024932 0.023000 41 | v -0.023586 0.022930 -0.024932 42 | v -0.025068 0.022926 -0.023000 43 | v -0.024742 0.022927 0.024094 44 | v -0.023070 0.023668 0.024860 45 | v -0.022926 -0.025068 -0.023000 46 | v -0.024791 -0.023073 -0.023736 47 | v -0.022929 -0.024162 0.024674 48 | v -0.023290 -0.025036 0.023000 49 | # 37 vertices, 0 vertices normals 50 | 51 | f 1 6 24 52 | f 1 24 2 53 | f 2 26 27 54 | f 2 27 1 55 | f 14 5 25 56 | f 14 25 10 57 | f 11 10 8 58 | f 11 12 14 59 | f 10 11 14 60 | f 17 32 31 61 | f 17 31 15 62 | f 15 35 18 63 | f 15 18 17 64 | f 21 37 34 65 | f 29 21 19 66 | f 34 19 21 67 | f 16 23 20 68 | f 30 7 16 69 | f 23 16 7 70 | f 4 22 28 71 | f 36 22 13 72 | f 36 13 33 73 | f 13 4 3 74 | f 4 13 22 75 | f 23 7 26 76 | f 23 26 24 77 | f 2 24 26 78 | f 5 27 26 79 | f 5 26 25 80 | f 4 28 1 81 | f 1 28 6 82 | f 27 3 1 83 | f 3 4 1 84 | f 19 24 29 85 | f 24 6 29 86 | f 10 25 7 87 | f 25 8 10 88 | f 25 10 8 89 | f 9 30 8 90 | f 9 8 10 91 | f 7 9 10 92 | f 7 30 9 93 | f 32 12 8 94 | f 32 8 31 95 | f 11 8 12 96 | f 13 3 14 97 | f 14 3 5 98 | f 12 33 14 99 | f 33 13 14 100 | f 15 31 30 101 | f 15 30 16 102 | f 16 35 15 103 | f 16 20 35 104 | f 37 18 34 105 | f 18 35 34 106 | f 33 18 36 107 | f 33 32 18 108 | f 17 18 32 109 | f 20 23 34 110 | f 34 23 19 111 | f 21 22 37 112 | f 22 36 37 113 | f 22 21 29 114 | f 29 28 22 115 | f 24 19 23 116 | f 26 7 25 117 | f 3 27 5 118 | f 29 6 28 119 | f 30 31 8 120 | f 32 33 12 121 | f 20 34 35 122 | f 18 37 36 123 | # 72 faces, 0 coords texture 124 | 125 | # End of File -------------------------------------------------------------------------------- /simulation/objects/blocks/7.obj: -------------------------------------------------------------------------------- 1 | #### 2 | # 3 | # OBJ File Generated by Meshlab 4 | # 5 | #### 6 | # Object 4.obj 7 | # 8 | # Vertices: 37 9 | # Faces: 72 10 | # 11 | #### 12 | v 0.025000 0.000074 0.023000 13 | v 0.025000 0.000074 -0.023000 14 | v 0.022932 0.023068 0.025000 15 | v 0.023000 0.000068 0.025000 16 | v 0.023290 0.025036 0.023000 17 | v 0.025068 -0.022926 0.023000 18 | v 0.023668 0.023070 -0.024860 19 | v -0.023074 0.024898 -0.023364 20 | v -0.000070 0.023736 -0.024860 21 | v -0.000074 0.025000 -0.023000 22 | v -0.023074 0.024932 0.000000 23 | v -0.023074 0.024932 0.023000 24 | v -0.000068 0.023000 0.025000 25 | v -0.000074 0.025000 0.023000 26 | v -0.025000 -0.000074 -0.023000 27 | v -0.023000 -0.000068 -0.025000 28 | v -0.025000 -0.000074 0.023000 29 | v -0.024932 -0.023074 0.023000 30 | v 0.023438 -0.024897 -0.023000 31 | v -0.023296 -0.023069 -0.024967 32 | v 0.000074 -0.025000 0.023000 33 | v 0.000068 -0.023000 0.025000 34 | v 0.023068 -0.022932 -0.025000 35 | v 0.025068 -0.022926 -0.023000 36 | v 0.023290 0.025036 -0.023000 37 | v 0.024932 0.023074 -0.023000 38 | v 0.024932 0.023074 0.023000 39 | v 0.023068 -0.022932 0.025000 40 | v 0.023074 -0.024932 0.023000 41 | v -0.023586 0.022930 -0.024932 42 | v -0.025068 0.022926 -0.023000 43 | v -0.024742 0.022927 0.024094 44 | v -0.023070 0.023668 0.024860 45 | v -0.022926 -0.025068 -0.023000 46 | v -0.024791 -0.023073 -0.023736 47 | v -0.022929 -0.024162 0.024674 48 | v -0.023290 -0.025036 0.023000 49 | # 37 vertices, 0 vertices normals 50 | 51 | f 1 6 24 52 | f 1 24 2 53 | f 2 26 27 54 | f 2 27 1 55 | f 14 5 25 56 | f 14 25 10 57 | f 11 10 8 58 | f 11 12 14 59 | f 10 11 14 60 | f 17 32 31 61 | f 17 31 15 62 | f 15 35 18 63 | f 15 18 17 64 | f 21 37 34 65 | f 29 21 19 66 | f 34 19 21 67 | f 16 23 20 68 | f 30 7 16 69 | f 23 16 7 70 | f 4 22 28 71 | f 36 22 13 72 | f 36 13 33 73 | f 13 4 3 74 | f 4 13 22 75 | f 23 7 26 76 | f 23 26 24 77 | f 2 24 26 78 | f 5 27 26 79 | f 5 26 25 80 | f 4 28 1 81 | f 1 28 6 82 | f 27 3 1 83 | f 3 4 1 84 | f 19 24 29 85 | f 24 6 29 86 | f 10 25 7 87 | f 25 8 10 88 | f 25 10 8 89 | f 9 30 8 90 | f 9 8 10 91 | f 7 9 10 92 | f 7 30 9 93 | f 32 12 8 94 | f 32 8 31 95 | f 11 8 12 96 | f 13 3 14 97 | f 14 3 5 98 | f 12 33 14 99 | f 33 13 14 100 | f 15 31 30 101 | f 15 30 16 102 | f 16 35 15 103 | f 16 20 35 104 | f 37 18 34 105 | f 18 35 34 106 | f 33 18 36 107 | f 33 32 18 108 | f 17 18 32 109 | f 20 23 34 110 | f 34 23 19 111 | f 21 22 37 112 | f 22 36 37 113 | f 22 21 29 114 | f 29 28 22 115 | f 24 19 23 116 | f 26 7 25 117 | f 3 27 5 118 | f 29 6 28 119 | f 30 31 8 120 | f 32 33 12 121 | f 20 34 35 122 | f 18 37 36 123 | # 72 faces, 0 coords texture 124 | 125 | # End of File -------------------------------------------------------------------------------- /simulation/objects/blocks/8.obj: -------------------------------------------------------------------------------- 1 | #### 2 | # 3 | # OBJ File Generated by Meshlab 4 | # 5 | #### 6 | # Object 4.obj 7 | # 8 | # Vertices: 37 9 | # Faces: 72 10 | # 11 | #### 12 | v 0.025000 0.000074 0.023000 13 | v 0.025000 0.000074 -0.023000 14 | v 0.022932 0.023068 0.025000 15 | v 0.023000 0.000068 0.025000 16 | v 0.023290 0.025036 0.023000 17 | v 0.025068 -0.022926 0.023000 18 | v 0.023668 0.023070 -0.024860 19 | v -0.023074 0.024898 -0.023364 20 | v -0.000070 0.023736 -0.024860 21 | v -0.000074 0.025000 -0.023000 22 | v -0.023074 0.024932 0.000000 23 | v -0.023074 0.024932 0.023000 24 | v -0.000068 0.023000 0.025000 25 | v -0.000074 0.025000 0.023000 26 | v -0.025000 -0.000074 -0.023000 27 | v -0.023000 -0.000068 -0.025000 28 | v -0.025000 -0.000074 0.023000 29 | v -0.024932 -0.023074 0.023000 30 | v 0.023438 -0.024897 -0.023000 31 | v -0.023296 -0.023069 -0.024967 32 | v 0.000074 -0.025000 0.023000 33 | v 0.000068 -0.023000 0.025000 34 | v 0.023068 -0.022932 -0.025000 35 | v 0.025068 -0.022926 -0.023000 36 | v 0.023290 0.025036 -0.023000 37 | v 0.024932 0.023074 -0.023000 38 | v 0.024932 0.023074 0.023000 39 | v 0.023068 -0.022932 0.025000 40 | v 0.023074 -0.024932 0.023000 41 | v -0.023586 0.022930 -0.024932 42 | v -0.025068 0.022926 -0.023000 43 | v -0.024742 0.022927 0.024094 44 | v -0.023070 0.023668 0.024860 45 | v -0.022926 -0.025068 -0.023000 46 | v -0.024791 -0.023073 -0.023736 47 | v -0.022929 -0.024162 0.024674 48 | v -0.023290 -0.025036 0.023000 49 | # 37 vertices, 0 vertices normals 50 | 51 | f 1 6 24 52 | f 1 24 2 53 | f 2 26 27 54 | f 2 27 1 55 | f 14 5 25 56 | f 14 25 10 57 | f 11 10 8 58 | f 11 12 14 59 | f 10 11 14 60 | f 17 32 31 61 | f 17 31 15 62 | f 15 35 18 63 | f 15 18 17 64 | f 21 37 34 65 | f 29 21 19 66 | f 34 19 21 67 | f 16 23 20 68 | f 30 7 16 69 | f 23 16 7 70 | f 4 22 28 71 | f 36 22 13 72 | f 36 13 33 73 | f 13 4 3 74 | f 4 13 22 75 | f 23 7 26 76 | f 23 26 24 77 | f 2 24 26 78 | f 5 27 26 79 | f 5 26 25 80 | f 4 28 1 81 | f 1 28 6 82 | f 27 3 1 83 | f 3 4 1 84 | f 19 24 29 85 | f 24 6 29 86 | f 10 25 7 87 | f 25 8 10 88 | f 25 10 8 89 | f 9 30 8 90 | f 9 8 10 91 | f 7 9 10 92 | f 7 30 9 93 | f 32 12 8 94 | f 32 8 31 95 | f 11 8 12 96 | f 13 3 14 97 | f 14 3 5 98 | f 12 33 14 99 | f 33 13 14 100 | f 15 31 30 101 | f 15 30 16 102 | f 16 35 15 103 | f 16 20 35 104 | f 37 18 34 105 | f 18 35 34 106 | f 33 18 36 107 | f 33 32 18 108 | f 17 18 32 109 | f 20 23 34 110 | f 34 23 19 111 | f 21 22 37 112 | f 22 36 37 113 | f 22 21 29 114 | f 29 28 22 115 | f 24 19 23 116 | f 26 7 25 117 | f 3 27 5 118 | f 29 6 28 119 | f 30 31 8 120 | f 32 33 12 121 | f 20 34 35 122 | f 18 37 36 123 | # 72 faces, 0 coords texture 124 | 125 | # End of File -------------------------------------------------------------------------------- /simulation/objects/mixed_shapes/0.obj: -------------------------------------------------------------------------------- 1 | #### 2 | # 3 | # OBJ File Generated by Meshlab 4 | # 5 | #### 6 | # Object 0.obj 7 | # 8 | # Vertices: 26 9 | # Faces: 50 10 | # 11 | #### 12 | v 0.024002 -0.022998 0.001414 13 | v -0.000002 0.025000 -0.022586 14 | v -0.023998 -0.023002 0.001414 15 | v 0.001417 -0.023000 0.024000 16 | v 0.001414 0.000000 0.024000 17 | v -0.001412 -0.023000 0.024000 18 | v -0.000002 0.025000 0.022586 19 | v -0.045662 -0.023005 -0.024524 20 | v -0.046583 -0.023005 -0.021171 21 | v -0.046673 0.022995 -0.023910 22 | v 0.046588 -0.022996 -0.021171 23 | v 0.022588 -0.024998 0.000000 24 | v 0.023998 0.023002 0.001414 25 | v 0.022583 0.025002 -0.000000 26 | v 0.046673 -0.022996 -0.023910 27 | v -0.000002 0.023000 -0.024586 28 | v -0.045169 -0.025005 -0.022586 29 | v 0.000003 -0.025000 0.022586 30 | v -0.001416 0.023000 0.024000 31 | v 0.001412 0.023000 0.024000 32 | v -0.046588 0.022995 -0.021172 33 | v -0.045174 0.024995 -0.022586 34 | v 0.045174 -0.024996 -0.022586 35 | v 0.046584 0.023004 -0.021172 36 | v 0.045427 0.024971 -0.022329 37 | v 0.045662 0.023004 -0.024524 38 | # 26 vertices, 0 vertices normals 39 | 40 | f 3 21 9 41 | f 19 3 6 42 | f 21 3 19 43 | f 1 5 4 44 | f 13 20 5 45 | f 5 11 24 46 | f 24 13 5 47 | f 11 5 1 48 | f 16 26 15 49 | f 10 16 8 50 | f 15 8 16 51 | f 17 23 12 52 | f 12 18 17 53 | f 2 14 25 54 | f 22 7 2 55 | f 14 2 7 56 | f 9 17 3 57 | f 9 18 3 58 | f 3 18 6 59 | f 9 3 18 60 | f 17 18 3 61 | f 20 19 6 62 | f 20 6 4 63 | f 20 4 5 64 | f 22 21 19 65 | f 22 19 7 66 | f 8 9 21 67 | f 8 21 10 68 | f 18 23 11 69 | f 18 11 4 70 | f 11 1 4 71 | f 18 12 23 72 | f 26 24 11 73 | f 26 11 15 74 | f 20 13 7 75 | f 7 24 14 76 | f 24 7 13 77 | f 14 24 25 78 | f 23 17 8 79 | f 23 8 15 80 | f 26 10 22 81 | f 25 26 22 82 | f 25 22 2 83 | f 16 10 26 84 | f 8 17 9 85 | f 4 6 18 86 | f 19 20 7 87 | f 22 10 21 88 | f 23 15 11 89 | f 26 25 24 90 | # 50 faces, 0 coords texture 91 | 92 | # End of File -------------------------------------------------------------------------------- /simulation/objects/mixed_shapes/1.obj: -------------------------------------------------------------------------------- 1 | #### 2 | # 3 | # OBJ File Generated by Meshlab 4 | # 5 | #### 6 | # Object 1.obj 7 | # 8 | # Vertices: 100 9 | # Faces: 200 10 | # 11 | #### 12 | v -0.050000 0.022993 0.023006 13 | v -0.050000 -0.023000 -0.000006 14 | v -0.019103 -0.024999 -0.005905 15 | v 0.024538 -0.024996 -0.013758 16 | v -0.048000 -0.024994 -0.023007 17 | v -0.024000 -0.025003 0.011493 18 | v 0.024000 -0.025003 0.011493 19 | v -0.016000 0.024994 0.023007 20 | v 0.016000 0.024994 0.023007 21 | v 0.032000 0.024994 0.023007 22 | v 0.024537 0.025004 -0.013744 23 | v 0.019103 0.025002 -0.005891 24 | v -0.000000 0.024999 0.002024 25 | v -0.024000 0.024997 0.011507 26 | v 0.024000 0.024997 0.011507 27 | v -0.024211 -0.022995 -0.018773 28 | v -0.022632 -0.022996 -0.014383 29 | v -0.013011 -0.022999 -0.003658 30 | v 0.022632 -0.022996 -0.014383 31 | v 0.013011 0.023001 -0.003646 32 | v 0.008725 0.023000 -0.001565 33 | v 0.004310 0.023000 -0.000368 34 | v -0.004310 0.023000 -0.000368 35 | v -0.008725 0.023000 -0.001565 36 | v -0.013011 0.023001 -0.003646 37 | v -0.016906 0.023002 -0.006576 38 | v -0.022632 0.023004 -0.014370 39 | v -0.024933 0.007673 -0.023146 40 | v 0.024933 -0.007660 -0.023150 41 | v 0.016906 -0.022998 -0.006589 42 | v 0.016906 0.023002 -0.006576 43 | v -0.020171 -0.022997 -0.010236 44 | v -0.020171 0.023003 -0.010223 45 | v -0.048000 -0.022993 -0.025006 46 | v -0.050000 0.000006 -0.023000 47 | v -0.049859 -0.000007 0.023736 48 | v -0.049966 -0.023371 0.022994 49 | v 0.050000 -0.023000 -0.000006 50 | v 0.048000 -0.025002 0.007660 51 | v 0.048000 -0.024998 -0.007674 52 | v 0.048000 -0.022993 -0.025006 53 | v 0.048000 0.024994 0.023007 54 | v 0.050000 0.022994 0.023006 55 | v 0.048000 0.022993 0.025006 56 | v 0.048000 -0.000007 0.025000 57 | v 0.048364 -0.023007 0.024960 58 | v -0.048000 -0.025007 0.022993 59 | v -0.000000 0.024994 0.023007 60 | v -0.032000 0.024993 0.023007 61 | v -0.024932 -0.007660 -0.023150 62 | v -0.024989 0.023006 -0.023488 63 | v 0.024932 0.023006 -0.023142 64 | v 0.024933 0.007673 -0.023146 65 | v 0.025112 0.000007 -0.023842 66 | v 0.037464 -0.024993 -0.023007 67 | v 0.048000 0.024739 -0.023993 68 | v -0.010324 0.025000 -0.000063 69 | v 0.010324 0.025000 -0.000063 70 | v -0.024212 0.023005 -0.018760 71 | v -0.000000 0.023000 0.000006 72 | v 0.020171 0.023003 -0.010223 73 | v 0.022632 0.023004 -0.014370 74 | v 0.024212 0.023005 -0.018761 75 | v 0.026930 0.025006 -0.022993 76 | v -0.019103 0.025001 -0.005891 77 | v -0.026930 0.024866 -0.023729 78 | v 0.019104 -0.024998 -0.005905 79 | v 0.000000 -0.023000 -0.000006 80 | v 0.000000 -0.025001 0.002011 81 | v -0.024537 -0.024996 -0.013758 82 | v 0.024212 -0.022995 -0.018773 83 | v 0.020171 -0.022997 -0.010236 84 | v 0.013011 -0.022999 -0.003658 85 | v 0.008725 -0.023000 -0.001578 86 | v 0.004310 -0.023000 -0.000380 87 | v -0.004309 -0.023000 -0.000380 88 | v -0.016906 -0.022998 -0.006589 89 | v -0.026927 -0.023730 -0.024866 90 | v 0.010324 -0.025000 -0.000077 91 | v -0.009783 -0.024414 -0.001382 92 | v -0.048364 -0.023007 0.024960 93 | v -0.050000 -0.022994 -0.023006 94 | v -0.048000 0.023007 -0.024994 95 | v -0.048000 0.025006 -0.022993 96 | v -0.050000 0.023006 -0.022994 97 | v -0.048364 0.024960 0.023007 98 | v -0.048364 0.022993 0.024973 99 | v 0.049095 -0.024667 -0.023007 100 | v 0.050000 -0.023006 0.022994 101 | v 0.048000 -0.025006 0.022993 102 | v 0.050000 0.023007 -0.022994 103 | v -0.024932 -0.022994 -0.023154 104 | v 0.026577 -0.022993 -0.024976 105 | v 0.026930 -0.024960 -0.023371 106 | v 0.024933 -0.022994 -0.023155 107 | v 0.026926 0.023007 -0.024994 108 | v -0.037464 0.024866 -0.023729 109 | v -0.037463 -0.023730 -0.024866 110 | v 0.037464 -0.022993 -0.025006 111 | v -0.024538 0.025004 -0.013744 112 | # 100 vertices, 0 vertices normals 113 | 114 | f 1 2 37 115 | f 35 2 1 116 | f 2 35 82 117 | f 1 85 35 118 | f 38 43 89 119 | f 91 38 88 120 | f 43 38 91 121 | f 90 47 6 122 | f 90 6 7 123 | f 5 3 6 124 | f 6 3 80 125 | f 5 6 47 126 | f 69 6 80 127 | f 39 90 7 128 | f 67 39 7 129 | f 69 79 7 130 | f 79 67 7 131 | f 7 6 69 132 | f 70 5 78 133 | f 5 70 3 134 | f 55 40 4 135 | f 67 4 40 136 | f 40 55 88 137 | f 39 67 40 138 | f 41 99 56 139 | f 96 56 99 140 | f 100 97 84 141 | f 84 65 100 142 | f 65 84 14 143 | f 11 56 64 144 | f 15 56 12 145 | f 56 11 12 146 | f 58 15 12 147 | f 14 13 57 148 | f 14 57 65 149 | f 14 86 49 150 | f 14 49 8 151 | f 14 84 86 152 | f 8 48 14 153 | f 13 14 15 154 | f 15 14 48 155 | f 15 48 9 156 | f 58 13 15 157 | f 15 9 10 158 | f 15 10 42 159 | f 56 15 42 160 | f 45 44 87 161 | f 46 45 81 162 | f 87 81 45 163 | f 53 63 52 164 | f 63 71 19 165 | f 63 19 62 166 | f 53 71 63 167 | f 62 19 72 168 | f 62 72 61 169 | f 31 30 73 170 | f 31 73 20 171 | f 20 73 74 172 | f 20 74 21 173 | f 61 72 30 174 | f 61 30 31 175 | f 21 74 75 176 | f 21 75 22 177 | f 71 29 95 178 | f 53 29 71 179 | f 68 60 22 180 | f 68 22 75 181 | f 60 68 76 182 | f 60 76 23 183 | f 24 80 18 184 | f 24 18 25 185 | f 23 76 80 186 | f 23 80 24 187 | f 26 77 32 188 | f 26 32 33 189 | f 27 17 16 190 | f 27 16 59 191 | f 28 51 59 192 | f 33 32 17 193 | f 33 17 27 194 | f 25 18 77 195 | f 25 77 26 196 | f 59 16 50 197 | f 50 16 92 198 | f 50 28 59 199 | f 98 34 83 200 | f 98 83 97 201 | f 5 37 82 202 | f 5 47 37 203 | f 2 82 37 204 | f 83 34 82 205 | f 83 82 85 206 | f 35 85 82 207 | f 86 84 85 208 | f 86 85 1 209 | f 36 1 37 210 | f 37 81 36 211 | f 36 87 1 212 | f 81 87 36 213 | f 88 89 90 214 | f 88 39 40 215 | f 88 90 39 216 | f 38 89 88 217 | f 56 91 41 218 | f 91 88 41 219 | f 42 43 91 220 | f 42 91 56 221 | f 45 46 89 222 | f 45 89 43 223 | f 45 43 44 224 | f 98 5 34 225 | f 66 83 97 226 | f 83 66 97 227 | f 97 83 84 228 | f 81 47 90 229 | f 81 90 46 230 | f 44 42 86 231 | f 10 9 42 232 | f 44 86 87 233 | f 48 42 9 234 | f 86 48 8 235 | f 86 8 49 236 | f 48 86 42 237 | f 51 78 66 238 | f 78 51 28 239 | f 50 78 28 240 | f 50 92 78 241 | f 93 95 54 242 | f 54 96 93 243 | f 95 29 54 244 | f 52 54 53 245 | f 29 53 54 246 | f 54 52 96 247 | f 88 94 55 248 | f 41 88 55 249 | f 94 88 55 250 | f 94 99 55 251 | f 41 55 99 252 | f 99 94 93 253 | f 96 64 56 254 | f 27 51 66 255 | f 65 33 27 256 | f 59 51 27 257 | f 65 25 26 258 | f 57 24 25 259 | f 57 25 65 260 | f 13 23 24 261 | f 13 24 57 262 | f 60 23 13 263 | f 33 65 26 264 | f 65 27 100 265 | f 66 100 27 266 | f 58 21 22 267 | f 58 22 13 268 | f 12 31 20 269 | f 12 20 58 270 | f 21 58 20 271 | f 61 31 12 272 | f 11 62 61 273 | f 11 63 62 274 | f 11 52 63 275 | f 11 64 52 276 | f 12 11 61 277 | f 22 60 13 278 | f 4 19 71 279 | f 67 72 19 280 | f 67 19 4 281 | f 71 95 4 282 | f 67 73 30 283 | f 79 74 73 284 | f 79 73 67 285 | f 68 75 74 286 | f 68 74 79 287 | f 72 67 30 288 | f 4 95 94 289 | f 79 69 68 290 | f 3 77 18 291 | f 3 18 80 292 | f 70 17 32 293 | f 70 32 3 294 | f 77 3 32 295 | f 70 16 17 296 | f 16 70 92 297 | f 70 78 92 298 | f 80 68 69 299 | f 76 68 80 300 | f 37 47 81 301 | f 34 5 82 302 | f 84 83 85 303 | f 1 87 86 304 | f 46 90 89 305 | f 43 42 44 306 | f 94 95 93 307 | f 52 64 96 308 | f 98 66 78 309 | f 98 97 66 310 | f 4 94 55 311 | f 96 99 93 312 | f 100 66 97 313 | f 98 78 5 314 | # 200 faces, 0 coords texture 315 | 316 | # End of File -------------------------------------------------------------------------------- /simulation/objects/mixed_shapes/2.obj: -------------------------------------------------------------------------------- 1 | #### 2 | # 3 | # OBJ File Generated by Meshlab 4 | # 5 | #### 6 | # Object 2.obj 7 | # 8 | # Vertices: 38 9 | # Faces: 72 10 | # 11 | #### 12 | v 0.000008 0.015544 0.030209 13 | v -0.000007 -0.017994 -0.028809 14 | v 0.012502 -0.006103 0.022175 15 | v 0.012507 0.022177 0.006105 16 | v -0.012498 -0.006097 0.022178 17 | v -0.012500 0.006112 -0.022172 18 | v -0.010506 -0.015892 -0.030270 19 | v 0.010493 -0.017996 -0.028810 20 | v 0.010494 -0.015537 -0.030208 21 | v -0.000006 -0.015535 -0.030207 22 | v 0.012473 0.028378 -0.016421 23 | v -0.010493 0.030191 -0.016679 24 | v 0.000006 0.029488 -0.017666 25 | v 0.000007 0.030213 -0.015538 26 | v -0.010494 0.028818 -0.017996 27 | v -0.000004 -0.028806 0.018000 28 | v 0.010495 -0.030206 0.015540 29 | v 0.010495 -0.029777 0.017394 30 | v -0.010505 -0.030261 0.016289 31 | v -0.000005 -0.030203 0.015541 32 | v 0.010502 -0.006634 0.024104 33 | v -0.012436 -0.028409 0.016572 34 | v -0.010506 -0.024096 -0.006633 35 | v -0.012505 -0.022167 -0.006102 36 | v -0.010507 -0.017991 -0.028808 37 | v -0.012426 0.028425 -0.016567 38 | v -0.012506 -0.016063 -0.028277 39 | v 0.012494 -0.016069 -0.028280 40 | v 0.010507 0.030210 -0.015539 41 | v 0.010506 0.028813 -0.017998 42 | v 0.011910 -0.028653 0.017434 43 | v -0.010504 -0.028803 0.018001 44 | v 0.010508 0.015542 0.030208 45 | v 0.012508 0.016072 0.028280 46 | v 0.010508 0.018001 0.028811 47 | v -0.010492 0.016688 0.030187 48 | v -0.012492 0.016078 0.028282 49 | v -0.010492 0.018006 0.028813 50 | # 38 vertices, 0 vertices normals 51 | 52 | f 10 7 15 53 | f 30 9 10 54 | f 15 30 10 55 | f 32 16 1 56 | f 32 1 36 57 | f 16 21 1 58 | f 21 33 1 59 | f 18 21 16 60 | f 2 23 25 61 | f 20 19 23 62 | f 8 17 20 63 | f 8 20 2 64 | f 23 2 20 65 | f 31 28 3 66 | f 4 3 28 67 | f 28 11 4 68 | f 3 4 34 69 | f 14 35 29 70 | f 38 14 12 71 | f 35 14 38 72 | f 26 6 5 73 | f 26 5 37 74 | f 6 24 5 75 | f 24 22 5 76 | f 27 24 6 77 | f 10 9 8 78 | f 2 10 8 79 | f 7 10 25 80 | f 2 25 10 81 | f 11 28 9 82 | f 11 9 30 83 | f 13 30 15 84 | f 15 12 14 85 | f 14 13 15 86 | f 30 13 29 87 | f 14 29 13 88 | f 27 26 15 89 | f 27 15 7 90 | f 26 27 6 91 | f 16 20 18 92 | f 20 17 18 93 | f 20 16 32 94 | f 32 19 20 95 | f 33 18 3 96 | f 3 34 33 97 | f 18 33 21 98 | f 18 31 3 99 | f 38 36 33 100 | f 38 33 35 101 | f 1 33 36 102 | f 22 32 37 103 | f 32 36 37 104 | f 37 5 22 105 | f 31 17 8 106 | f 31 8 28 107 | f 35 34 29 108 | f 34 11 29 109 | f 11 34 4 110 | f 25 24 27 111 | f 19 25 23 112 | f 25 19 24 113 | f 19 22 24 114 | f 38 12 26 115 | f 37 38 26 116 | f 27 7 25 117 | f 8 9 28 118 | f 30 29 11 119 | f 12 15 26 120 | f 17 31 18 121 | f 32 22 19 122 | f 34 35 33 123 | f 38 37 36 124 | # 72 faces, 0 coords texture 125 | 126 | # End of File -------------------------------------------------------------------------------- /simulation/objects/mixed_shapes/3.obj: -------------------------------------------------------------------------------- 1 | #### 2 | # 3 | # OBJ File Generated by Meshlab 4 | # 5 | #### 6 | # Object 3.obj 7 | # 8 | # Vertices: 30 9 | # Faces: 58 10 | # 11 | #### 12 | v -0.000000 -0.023000 -0.012501 13 | v 0.000000 0.023000 0.012501 14 | v 0.048000 -0.000000 0.012500 15 | v -0.048000 -0.025000 -0.010501 16 | v -0.050000 0.023000 0.000000 17 | v -0.049674 0.024094 0.010501 18 | v -0.048000 -0.000000 0.012500 19 | v 0.048000 -0.025000 0.010499 20 | v 0.048000 -0.023000 -0.012501 21 | v 0.050000 0.023000 -0.010499 22 | v 0.048000 0.025000 -0.010499 23 | v 0.000000 -0.025000 -0.010501 24 | v 0.000000 0.025000 -0.010499 25 | v -0.000000 0.025000 0.010501 26 | v -0.050000 -0.023000 0.010499 27 | v -0.048000 -0.023000 0.012499 28 | v -0.048000 -0.025000 0.010499 29 | v -0.049967 -0.023364 -0.010501 30 | v -0.048000 -0.023000 -0.012501 31 | v -0.048000 0.023000 -0.012500 32 | v -0.048000 0.025000 -0.010499 33 | v -0.049932 0.023000 -0.011017 34 | v -0.048000 0.023000 0.012501 35 | v 0.048000 -0.025000 -0.010501 36 | v 0.050000 -0.023000 -0.010500 37 | v 0.048000 -0.023000 0.012500 38 | v 0.050000 -0.023000 0.010500 39 | v 0.048000 0.023000 -0.012499 40 | v 0.048364 0.023000 0.012467 41 | v 0.049414 0.024414 0.010501 42 | # 30 vertices, 0 vertices normals 43 | 44 | f 5 15 6 45 | f 22 18 5 46 | f 15 5 18 47 | f 30 27 25 48 | f 30 25 10 49 | f 8 17 4 50 | f 8 4 12 51 | f 8 12 24 52 | f 20 1 19 53 | f 9 1 28 54 | f 20 28 1 55 | f 13 21 6 56 | f 13 6 14 57 | f 11 13 14 58 | f 11 14 30 59 | f 7 2 23 60 | f 3 29 2 61 | f 16 26 2 62 | f 26 3 2 63 | f 7 16 2 64 | f 4 15 18 65 | f 4 17 15 66 | f 20 18 22 67 | f 20 19 18 68 | f 6 21 22 69 | f 5 6 22 70 | f 23 15 16 71 | f 23 16 7 72 | f 15 23 6 73 | f 24 25 8 74 | f 25 27 8 75 | f 28 10 25 76 | f 28 25 9 77 | f 30 10 11 78 | f 26 27 30 79 | f 26 30 29 80 | f 26 29 3 81 | f 24 4 12 82 | f 9 24 12 83 | f 4 24 12 84 | f 1 12 19 85 | f 9 12 1 86 | f 12 4 19 87 | f 13 28 20 88 | f 28 13 11 89 | f 13 20 21 90 | f 26 16 17 91 | f 26 17 8 92 | f 30 14 2 93 | f 2 29 30 94 | f 2 6 23 95 | f 2 14 6 96 | f 17 16 15 97 | f 19 4 18 98 | f 22 21 20 99 | f 9 25 24 100 | f 27 26 8 101 | f 10 28 11 102 | # 58 faces, 0 coords texture 103 | 104 | # End of File -------------------------------------------------------------------------------- /simulation/objects/mixed_shapes/4.obj: -------------------------------------------------------------------------------- 1 | #### 2 | # 3 | # OBJ File Generated by Meshlab 4 | # 5 | #### 6 | # Object 4.obj 7 | # 8 | # Vertices: 37 9 | # Faces: 72 10 | # 11 | #### 12 | v 0.025000 0.000074 0.023000 13 | v 0.025000 0.000074 -0.023000 14 | v 0.022932 0.023068 0.025000 15 | v 0.023000 0.000068 0.025000 16 | v 0.023290 0.025036 0.023000 17 | v 0.025068 -0.022926 0.023000 18 | v 0.023668 0.023070 -0.024860 19 | v -0.023074 0.024898 -0.023364 20 | v -0.000070 0.023736 -0.024860 21 | v -0.000074 0.025000 -0.023000 22 | v -0.023074 0.024932 0.000000 23 | v -0.023074 0.024932 0.023000 24 | v -0.000068 0.023000 0.025000 25 | v -0.000074 0.025000 0.023000 26 | v -0.025000 -0.000074 -0.023000 27 | v -0.023000 -0.000068 -0.025000 28 | v -0.025000 -0.000074 0.023000 29 | v -0.024932 -0.023074 0.023000 30 | v 0.023438 -0.024897 -0.023000 31 | v -0.023296 -0.023069 -0.024967 32 | v 0.000074 -0.025000 0.023000 33 | v 0.000068 -0.023000 0.025000 34 | v 0.023068 -0.022932 -0.025000 35 | v 0.025068 -0.022926 -0.023000 36 | v 0.023290 0.025036 -0.023000 37 | v 0.024932 0.023074 -0.023000 38 | v 0.024932 0.023074 0.023000 39 | v 0.023068 -0.022932 0.025000 40 | v 0.023074 -0.024932 0.023000 41 | v -0.023586 0.022930 -0.024932 42 | v -0.025068 0.022926 -0.023000 43 | v -0.024742 0.022927 0.024094 44 | v -0.023070 0.023668 0.024860 45 | v -0.022926 -0.025068 -0.023000 46 | v -0.024791 -0.023073 -0.023736 47 | v -0.022929 -0.024162 0.024674 48 | v -0.023290 -0.025036 0.023000 49 | # 37 vertices, 0 vertices normals 50 | 51 | f 1 6 24 52 | f 1 24 2 53 | f 2 26 27 54 | f 2 27 1 55 | f 14 5 25 56 | f 14 25 10 57 | f 11 10 8 58 | f 11 12 14 59 | f 10 11 14 60 | f 17 32 31 61 | f 17 31 15 62 | f 15 35 18 63 | f 15 18 17 64 | f 21 37 34 65 | f 29 21 19 66 | f 34 19 21 67 | f 16 23 20 68 | f 30 7 16 69 | f 23 16 7 70 | f 4 22 28 71 | f 36 22 13 72 | f 36 13 33 73 | f 13 4 3 74 | f 4 13 22 75 | f 23 7 26 76 | f 23 26 24 77 | f 2 24 26 78 | f 5 27 26 79 | f 5 26 25 80 | f 4 28 1 81 | f 1 28 6 82 | f 27 3 1 83 | f 3 4 1 84 | f 19 24 29 85 | f 24 6 29 86 | f 10 25 7 87 | f 25 8 10 88 | f 25 10 8 89 | f 9 30 8 90 | f 9 8 10 91 | f 7 9 10 92 | f 7 30 9 93 | f 32 12 8 94 | f 32 8 31 95 | f 11 8 12 96 | f 13 3 14 97 | f 14 3 5 98 | f 12 33 14 99 | f 33 13 14 100 | f 15 31 30 101 | f 15 30 16 102 | f 16 35 15 103 | f 16 20 35 104 | f 37 18 34 105 | f 18 35 34 106 | f 33 18 36 107 | f 33 32 18 108 | f 17 18 32 109 | f 20 23 34 110 | f 34 23 19 111 | f 21 22 37 112 | f 22 36 37 113 | f 22 21 29 114 | f 29 28 22 115 | f 24 19 23 116 | f 26 7 25 117 | f 3 27 5 118 | f 29 6 28 119 | f 30 31 8 120 | f 32 33 12 121 | f 20 34 35 122 | f 18 37 36 123 | # 72 faces, 0 coords texture 124 | 125 | # End of File -------------------------------------------------------------------------------- /simulation/objects/mixed_shapes/6.obj: -------------------------------------------------------------------------------- 1 | #### 2 | # 3 | # OBJ File Generated by Meshlab 4 | # 5 | #### 6 | # Object 6.obj 7 | # 8 | # Vertices: 38 9 | # Faces: 76 10 | # 11 | #### 12 | v -0.023016 -0.024986 0.047999 13 | v -0.000014 -0.022999 -0.050000 14 | v 0.000015 0.023001 -0.050000 15 | v -0.023001 0.000014 0.050000 16 | v 0.000014 0.022999 0.050000 17 | v -0.000016 -0.025001 0.048000 18 | v -0.025015 -0.022985 0.047999 19 | v 0.025001 -0.000015 -0.048000 20 | v 0.023379 0.022986 -0.049966 21 | v 0.023001 -0.000014 -0.050000 22 | v 0.022999 -0.000015 0.050000 23 | v 0.023014 0.022985 0.050001 24 | v 0.024999 -0.000016 0.048000 25 | v 0.023380 0.024953 -0.047999 26 | v 0.000016 0.025001 -0.048000 27 | v 0.023015 0.024985 0.048001 28 | v 0.000015 0.024999 0.048000 29 | v -0.025000 0.000016 -0.048000 30 | v -0.025014 -0.022984 -0.048001 31 | v -0.022999 0.000015 -0.050000 32 | v -0.025001 0.000015 0.048000 33 | v -0.023015 -0.024985 -0.048001 34 | v -0.023014 -0.022985 -0.050001 35 | v 0.024986 -0.023015 -0.048000 36 | v 0.022986 -0.023378 -0.049967 37 | v 0.022985 -0.025014 -0.048000 38 | v 0.022984 -0.025015 0.048000 39 | v 0.024985 -0.023016 0.048000 40 | v 0.022985 -0.023015 0.050000 41 | v -0.023015 -0.022986 0.049999 42 | v 0.025015 0.022985 -0.047999 43 | v 0.025014 0.022984 0.048001 44 | v -0.022984 0.025015 -0.048000 45 | v -0.022985 0.023015 -0.050000 46 | v -0.024985 0.023016 -0.048000 47 | v -0.024986 0.023015 0.048000 48 | v -0.022986 0.023378 0.049967 49 | v -0.022985 0.025014 0.048000 50 | # 38 vertices, 0 vertices normals 51 | 52 | f 6 1 22 53 | f 27 6 26 54 | f 22 26 6 55 | f 28 24 8 56 | f 28 8 13 57 | f 31 32 13 58 | f 31 13 8 59 | f 16 14 15 60 | f 16 15 17 61 | f 33 38 17 62 | f 33 17 15 63 | f 36 35 18 64 | f 36 18 21 65 | f 19 7 21 66 | f 19 21 18 67 | f 20 2 23 68 | f 20 34 3 69 | f 3 2 20 70 | f 2 3 10 71 | f 10 3 9 72 | f 25 2 10 73 | f 5 4 30 74 | f 5 37 4 75 | f 5 29 11 76 | f 11 12 5 77 | f 30 29 5 78 | f 23 25 26 79 | f 23 26 22 80 | f 23 2 25 81 | f 28 27 26 82 | f 28 26 24 83 | f 1 6 30 84 | f 1 6 27 85 | f 1 27 6 86 | f 6 27 29 87 | f 30 6 29 88 | f 19 22 1 89 | f 19 1 7 90 | f 24 25 8 91 | f 8 25 10 92 | f 10 31 8 93 | f 10 9 31 94 | f 16 32 14 95 | f 32 31 14 96 | f 28 13 29 97 | f 29 32 13 98 | f 32 29 13 99 | f 12 11 13 100 | f 29 13 11 101 | f 13 32 12 102 | f 14 9 15 103 | f 15 9 3 104 | f 3 33 15 105 | f 3 34 33 106 | f 36 38 33 107 | f 36 33 35 108 | f 12 16 17 109 | f 5 17 37 110 | f 12 17 5 111 | f 17 38 37 112 | f 18 35 20 113 | f 34 20 35 114 | f 20 23 18 115 | f 19 18 23 116 | f 36 21 37 117 | f 21 4 37 118 | f 4 21 30 119 | f 7 30 21 120 | f 22 19 23 121 | f 25 24 26 122 | f 29 27 28 123 | f 1 30 7 124 | f 9 14 31 125 | f 12 32 16 126 | f 33 34 35 127 | f 36 37 38 128 | # 76 faces, 0 coords texture 129 | 130 | # End of File -------------------------------------------------------------------------------- /simulation/objects/mixed_shapes/7.obj: -------------------------------------------------------------------------------- 1 | #### 2 | # 3 | # OBJ File Generated by Meshlab 4 | # 5 | #### 6 | # Object 7.obj 7 | # 8 | # Vertices: 99 9 | # Faces: 194 10 | # 11 | #### 12 | v 0.022913 -0.023000 -0.012500 13 | v -0.022913 -0.000000 -0.012500 14 | v -0.022516 -0.023000 -0.001636 15 | v -0.020035 -0.023000 0.002454 16 | v -0.016770 -0.023000 0.006041 17 | v -0.008643 -0.023000 0.010958 18 | v -0.004269 -0.023000 0.012133 19 | v -0.000000 -0.023000 0.012500 20 | v 0.008642 -0.023000 0.010959 21 | v 0.012894 -0.023000 0.008918 22 | v 0.024135 -0.023000 -0.005977 23 | v 0.024906 0.000000 -0.010326 24 | v 0.024135 0.023000 -0.005977 25 | v 0.022516 0.023000 -0.001635 26 | v 0.016770 0.023000 0.006041 27 | v 0.012894 0.023000 0.008918 28 | v 0.004268 0.023000 0.012133 29 | v -0.000000 0.023000 0.012500 30 | v -0.024134 0.023000 -0.005978 31 | v 0.021787 0.025000 -0.005096 32 | v 0.007817 0.025000 0.009134 33 | v -0.021260 0.025000 -0.003735 34 | v -0.022344 0.025000 -0.007057 35 | v 0.017708 -0.025000 0.002160 36 | v 0.014765 -0.025000 0.005132 37 | v 0.007817 -0.025000 0.009134 38 | v 0.000208 -0.025000 0.010500 39 | v -0.007636 -0.025000 0.009192 40 | v -0.014761 -0.025000 0.005135 41 | v 0.024906 0.023000 -0.010326 42 | v 0.024388 0.023000 -0.011851 43 | v -0.022913 -0.023364 -0.012467 44 | v -0.024905 -0.000000 -0.010327 45 | v -0.024387 -0.023000 -0.011852 46 | v -0.024387 0.023000 -0.011852 47 | v 0.023548 0.023736 -0.004497 48 | v 0.018275 0.024860 0.002630 49 | v 0.012036 0.024414 0.008753 50 | v 0.004320 0.024414 0.011525 51 | v -0.004269 0.023000 0.012133 52 | v -0.003880 0.024414 0.011612 53 | v -0.012895 0.023000 0.008918 54 | v -0.012077 0.024414 0.008705 55 | v -0.011377 0.025000 0.007477 56 | v -0.019102 0.023736 0.003424 57 | v -0.018238 0.024860 0.002705 58 | v -0.022979 0.023736 -0.003026 59 | v 0.019140 0.023736 0.003346 60 | v 0.008642 0.023000 0.010959 61 | v -0.008643 0.023000 0.010958 62 | v -0.016770 0.023000 0.006041 63 | v -0.020035 0.023000 0.002454 64 | v -0.022516 0.023000 -0.001636 65 | v -0.019693 0.025000 -0.000612 66 | v -0.014761 0.025000 0.005135 67 | v -0.007636 0.025000 0.009192 68 | v -0.003655 0.025000 0.010215 69 | v 0.000208 0.025000 0.010500 70 | v 0.004070 0.025000 0.010133 71 | v 0.011339 0.025000 0.007522 72 | v 0.014765 0.025000 0.005132 73 | v 0.020078 0.025000 -0.001285 74 | v -0.022914 0.024674 -0.011595 75 | v 0.023548 -0.023736 -0.004497 76 | v 0.021787 -0.025000 -0.005096 77 | v 0.019140 -0.023736 0.003346 78 | v 0.012036 -0.024414 0.008753 79 | v 0.004070 -0.025000 0.010133 80 | v -0.003880 -0.024414 0.011612 81 | v -0.003655 -0.025000 0.010215 82 | v -0.012895 -0.023000 0.008918 83 | v -0.012077 -0.024414 0.008705 84 | v -0.019102 -0.023736 0.003424 85 | v -0.022979 -0.023736 -0.003026 86 | v -0.021260 -0.025000 -0.003735 87 | v 0.022516 -0.023000 -0.001635 88 | v 0.016770 -0.023000 0.006041 89 | v 0.004398 -0.023736 0.011963 90 | v -0.024134 -0.023000 -0.005978 91 | v -0.022344 -0.025000 -0.007057 92 | v -0.019693 -0.025000 -0.000612 93 | v -0.018238 -0.024860 0.002705 94 | v -0.011377 -0.025000 0.007477 95 | v 0.011339 -0.025000 0.007522 96 | v 0.020078 -0.025000 -0.001285 97 | v 0.024906 -0.023000 -0.010326 98 | v -0.022915 -0.024967 -0.010864 99 | v 0.022913 0.025000 -0.010500 100 | v 0.022913 0.024414 -0.011914 101 | v 0.022913 0.023000 -0.012500 102 | v 0.024267 0.024354 -0.011077 103 | v 0.024388 -0.023000 -0.011851 104 | v 0.022913 -0.024860 -0.011236 105 | v 0.024322 -0.024414 -0.010377 106 | v -0.024267 -0.024353 -0.011078 107 | v -0.024905 -0.023000 -0.010327 108 | v -0.024872 0.023364 -0.010329 109 | v -0.022913 0.023364 -0.012467 110 | v -0.024004 0.024674 -0.010405 111 | # 99 vertices, 0 vertices normals 112 | 113 | f 98 90 2 114 | f 1 2 90 115 | f 12 30 13 116 | f 14 48 66 117 | f 14 66 76 118 | f 13 14 76 119 | f 13 76 11 120 | f 15 16 10 121 | f 15 10 77 122 | f 49 17 78 123 | f 49 78 9 124 | f 17 18 8 125 | f 17 8 78 126 | f 16 49 9 127 | f 16 9 10 128 | f 48 15 77 129 | f 48 77 66 130 | f 11 86 12 131 | f 13 11 12 132 | f 40 50 6 133 | f 40 6 7 134 | f 42 51 5 135 | f 42 5 71 136 | f 50 42 71 137 | f 50 71 6 138 | f 52 53 3 139 | f 52 3 4 140 | f 33 19 97 141 | f 53 19 79 142 | f 53 79 3 143 | f 51 52 4 144 | f 51 4 5 145 | f 79 19 33 146 | f 7 8 18 147 | f 7 18 40 148 | f 33 96 79 149 | f 21 60 44 150 | f 56 59 21 151 | f 44 60 61 152 | f 22 62 20 153 | f 61 37 55 154 | f 62 54 37 155 | f 23 20 88 156 | f 57 58 59 157 | f 21 44 56 158 | f 44 61 55 159 | f 37 46 55 160 | f 54 62 22 161 | f 20 23 22 162 | f 46 37 54 163 | f 57 59 56 164 | f 26 83 84 165 | f 28 26 68 166 | f 83 25 84 167 | f 75 65 85 168 | f 25 29 24 169 | f 85 24 81 170 | f 80 93 65 171 | f 70 68 27 172 | f 26 28 83 173 | f 83 29 25 174 | f 24 29 82 175 | f 81 75 85 176 | f 65 75 80 177 | f 82 81 24 178 | f 70 28 68 179 | f 89 98 88 180 | f 89 90 98 181 | f 63 88 98 182 | f 31 1 90 183 | f 30 92 31 184 | f 31 92 1 185 | f 30 12 92 186 | f 12 86 92 187 | f 1 93 32 188 | f 93 87 32 189 | f 34 35 98 190 | f 34 98 32 191 | f 96 35 34 192 | f 96 33 35 193 | f 35 33 97 194 | f 2 32 98 195 | f 37 48 36 196 | f 37 36 20 197 | f 38 16 48 198 | f 38 48 37 199 | f 39 17 16 200 | f 39 16 38 201 | f 41 40 17 202 | f 41 17 39 203 | f 43 42 40 204 | f 43 40 41 205 | f 44 43 41 206 | f 45 42 43 207 | f 46 45 43 208 | f 46 43 44 209 | f 22 47 45 210 | f 22 45 46 211 | f 36 13 30 212 | f 14 13 36 213 | f 91 36 30 214 | f 48 14 36 215 | f 16 15 48 216 | f 17 49 16 217 | f 88 20 36 218 | f 20 62 37 219 | f 36 91 88 220 | f 37 61 60 221 | f 37 60 38 222 | f 38 21 59 223 | f 38 59 39 224 | f 39 59 58 225 | f 21 38 60 226 | f 39 58 57 227 | f 39 57 41 228 | f 42 50 40 229 | f 45 51 42 230 | f 52 51 45 231 | f 47 53 52 232 | f 47 52 45 233 | f 47 19 53 234 | f 97 19 47 235 | f 41 57 56 236 | f 41 56 44 237 | f 44 55 46 238 | f 22 99 47 239 | f 22 63 99 240 | f 22 23 63 241 | f 46 54 22 242 | f 99 97 47 243 | f 40 18 17 244 | f 24 65 64 245 | f 24 64 66 246 | f 67 66 10 247 | f 67 24 66 248 | f 78 67 10 249 | f 68 67 78 250 | f 69 78 7 251 | f 70 68 78 252 | f 70 78 69 253 | f 72 69 7 254 | f 72 7 71 255 | f 72 70 69 256 | f 73 72 71 257 | f 82 72 73 258 | f 75 82 73 259 | f 75 73 74 260 | f 64 86 11 261 | f 76 64 11 262 | f 94 86 64 263 | f 66 64 76 264 | f 10 66 77 265 | f 78 10 9 266 | f 93 64 65 267 | f 65 24 85 268 | f 64 93 94 269 | f 24 67 84 270 | f 24 84 25 271 | f 67 68 26 272 | f 26 84 67 273 | f 68 70 27 274 | f 71 7 6 275 | f 73 71 5 276 | f 4 73 5 277 | f 74 73 4 278 | f 74 4 3 279 | f 74 3 79 280 | f 96 74 79 281 | f 95 74 96 282 | f 70 72 83 283 | f 70 83 28 284 | f 72 82 29 285 | f 29 83 72 286 | f 75 74 87 287 | f 75 87 80 288 | f 82 75 81 289 | f 87 74 95 290 | f 7 78 8 291 | f 31 88 91 292 | f 31 91 30 293 | f 31 89 88 294 | f 31 90 89 295 | f 93 92 86 296 | f 93 86 94 297 | f 1 92 93 298 | f 32 87 95 299 | f 34 32 95 300 | f 34 95 96 301 | f 98 35 63 302 | f 63 35 97 303 | f 63 97 99 304 | f 2 1 32 305 | f 23 88 63 306 | f 80 87 93 307 | # 194 faces, 0 coords texture 308 | 309 | # End of File -------------------------------------------------------------------------------- /simulation/objects/mixed_shapes/8.obj: -------------------------------------------------------------------------------- 1 | v 0.0223623 -0.002484 0.023 2 | v 0.0214092 -0.006921 0.023 3 | v 0.0196731 -0.0109188 0.023 4 | v -0.002484 -0.0223623 0.023 5 | v -0.0109188 -0.0196731 0.023 6 | v -0.017235 -0.014463 0.023 7 | v -0.0197001 -0.0108693 0.023 8 | v -0.0214812 -0.0066951 0.023 9 | v -0.0223623 0.002484 0.023 10 | v -0.0108702 0.0197001 0.023 11 | v -0.002151 0.0223965 0.023 12 | v 0.002484 0.0223623 0.023 13 | v -0.0108702 0.0197001 -0.023 14 | v -0.014463 0.0172359 -0.023 15 | v -0.0173583 0.0143163 -0.023 16 | v -0.0196731 0.0109188 -0.023 17 | v -0.017235 -0.014463 -0.023 18 | v 0.014463 -0.0172359 -0.023 19 | v 0.0173583 -0.0143163 -0.023 20 | v 0.0223623 -0.002484 -0.023 21 | v 0.0172359 0.014463 -0.023 22 | v -0.01458 -0.0014022 0.025 23 | v -0.0065907 -0.0079911 0.025 24 | v 0.0013986 -0.0145809 0.025 25 | v -0.0079929 0.0065853 0.025 26 | v -3.6e-06 -4.5e-06 0.025 27 | v 0.0079857 -0.0065934 0.025 28 | v -0.0014058 0.0145719 0.025 29 | v 0.0065835 0.007983 0.025 30 | v 0.0145728 0.0013932 0.025 31 | v 0.0182745 0.009711 0.025 32 | v -0.0097092 0.0182673 0.025 33 | v -0.00198 0.0206163 0.025 34 | v -0.01458 -0.0014022 -0.025 35 | v -0.0065907 -0.0079911 -0.025 36 | v 0.0013986 -0.0145809 -0.025 37 | v -0.0079929 0.0065853 -0.025 38 | v -3.6e-06 -4.5e-06 -0.025 39 | v 0.0079857 -0.0065934 -0.025 40 | v -0.0014058 0.0145719 -0.025 41 | v 0.0065835 0.007983 -0.025 42 | v 0.0145728 0.0013932 -0.025 43 | v 0.0206163 0.00198 -0.025 44 | v 0.0159795 -0.0131778 -0.025 45 | v 0.0097092 -0.0182691 -0.025 46 | v -0.0206181 -0.0019809 -0.025 47 | v 0.0060552 0.0197883 -0.025 48 | v 0.0143163 0.0173583 0.023 49 | v 0.0206163 0.00198 0.025 50 | v 0.0204147 -0.0062478 0.02486 51 | v 0.0172719 -0.0142434 0.023736 52 | v 0.0159795 -0.0131778 0.025 53 | v 0.0108702 -0.0197001 0.023 54 | v 0.0100197 -0.0188541 0.02486 55 | v 0.00198 -0.0206181 0.025 56 | v -0.006921 -0.0214092 0.023 57 | v -0.0062478 -0.0204165 0.02486 58 | v -0.0143163 -0.0173583 0.023 59 | v -0.0135999 -0.0164898 0.02486 60 | v -0.0188541 -0.0100197 0.02486 61 | v -0.0223965 -0.002151 0.023 62 | v -0.0206181 -0.0019809 0.025 63 | v -0.0213822 0.0065439 0.023736 64 | v -0.0197829 0.0060543 0.025 65 | v -0.0172701 0.0142452 0.023736 66 | v -0.0159786 0.0131796 0.025 67 | v 0.0065448 0.0213885 0.023736 68 | v 0.0060552 0.0197883 0.025 69 | v 0.0135927 0.0164808 0.02486 70 | v 0.0172359 0.014463 0.023 71 | v 0.0197514 0.0104967 0.023736 72 | v 0.0214812 0.0066951 0.023 73 | v 0.0222831 0.0021402 0.023736 74 | v 0.014463 -0.0172359 0.023 75 | v 0.0066951 -0.0214812 0.023 76 | v 0.0021393 -0.0222858 0.023736 77 | v -0.0196731 0.0109188 0.023 78 | v -0.014463 0.0172359 0.023 79 | v -0.0066951 0.0214812 0.023 80 | v 0.0109188 0.0196731 0.023 81 | v 0.0143163 0.0173583 -0.023 82 | v 0.0197001 0.0108693 -0.023 83 | v 0.0182745 0.009711 -0.025 84 | v 0.0223965 0.002151 -0.023 85 | v 0.0214092 -0.006921 -0.023 86 | v 0.0204147 -0.0062478 -0.02486 87 | v 0.010494 -0.019746 -0.023736 88 | v 0.0021393 -0.0222858 -0.023736 89 | v 0.00198 -0.0206181 -0.025 90 | v -0.006543 -0.0213831 -0.023736 91 | v -0.0060534 -0.0197829 -0.025 92 | v -0.0143163 -0.0173583 -0.023 93 | v -0.0135999 -0.0164898 -0.02486 94 | v -0.0197469 -0.0104931 -0.023736 95 | v -0.0182691 -0.0097083 -0.025 96 | v -0.0214092 0.006921 -0.023 97 | v -0.0197829 0.0060543 -0.025 98 | v -0.0164898 0.0136017 -0.02486 99 | v -0.0100197 0.0188523 -0.02486 100 | v -0.0021402 0.0222831 -0.023736 101 | v -0.00198 0.0206163 -0.025 102 | v 0.0065448 0.0213885 -0.023736 103 | v 0.0131706 0.0159696 -0.025 104 | v 0.0214812 0.0066951 -0.023 105 | v 0.0196731 -0.0109188 -0.023 106 | v 0.0066951 -0.0214812 -0.023 107 | v -0.002484 -0.0223623 -0.023 108 | v -0.0109188 -0.0196731 -0.023 109 | v -0.0214812 -0.0066951 -0.023 110 | v -0.0222858 -0.0021411 -0.023736 111 | v -0.0223623 0.002484 -0.023 112 | v -0.0066951 0.0214812 -0.023 113 | v 0.002484 0.0223623 -0.023 114 | v 0.0109188 0.0196731 -0.023 115 | f 48 70 21 116 | f 48 21 81 117 | f 71 72 104 118 | f 71 104 82 119 | f 70 71 82 120 | f 70 82 21 121 | f 73 1 20 122 | f 73 20 84 123 | f 2 3 105 124 | f 2 105 85 125 | f 1 2 85 126 | f 1 85 20 127 | f 72 73 84 128 | f 72 84 104 129 | f 51 74 18 130 | f 51 18 19 131 | f 53 75 106 132 | f 53 106 87 133 | f 74 53 87 134 | f 74 87 18 135 | f 76 4 107 136 | f 76 107 88 137 | f 56 5 108 138 | f 56 108 90 139 | f 4 56 90 140 | f 4 90 107 141 | f 75 76 88 142 | f 75 88 106 143 | f 3 51 19 144 | f 3 19 105 145 | f 92 108 5 146 | f 92 5 58 147 | f 58 6 17 148 | f 58 17 92 149 | f 7 8 109 150 | f 7 109 94 151 | f 6 7 94 152 | f 6 94 17 153 | f 61 9 111 154 | f 61 111 110 155 | f 63 77 16 156 | f 63 16 96 157 | f 9 63 96 158 | f 9 96 111 159 | f 8 61 110 160 | f 8 110 109 161 | f 65 78 14 162 | f 65 14 15 163 | f 10 79 112 164 | f 10 112 13 165 | f 78 10 13 166 | f 78 13 14 167 | f 11 12 113 168 | f 11 113 100 169 | f 67 80 114 170 | f 67 114 102 171 | f 12 67 102 172 | f 12 102 113 173 | f 79 11 100 174 | f 79 100 112 175 | f 77 65 15 176 | f 77 15 16 177 | f 81 114 80 178 | f 81 80 48 179 | f 26 25 22 180 | f 26 22 23 181 | f 27 26 23 182 | f 27 23 24 183 | f 29 28 25 184 | f 29 25 26 185 | f 30 29 26 186 | f 30 26 27 187 | f 22 62 60 188 | f 64 62 22 189 | f 23 22 60 190 | f 64 22 25 191 | f 66 64 25 192 | f 59 23 60 193 | f 55 24 57 194 | f 23 59 57 195 | f 54 27 24 196 | f 24 23 57 197 | f 52 27 54 198 | f 55 54 24 199 | f 25 28 32 200 | f 33 32 28 201 | f 29 68 28 202 | f 33 28 68 203 | f 69 68 29 204 | f 52 50 27 205 | f 50 30 27 206 | f 49 30 50 207 | f 49 31 30 208 | f 31 29 30 209 | f 31 69 29 210 | f 25 32 66 211 | f 38 35 34 212 | f 38 34 37 213 | f 39 36 35 214 | f 39 35 38 215 | f 41 38 37 216 | f 41 37 40 217 | f 42 39 38 218 | f 42 38 41 219 | f 34 95 46 220 | f 97 34 46 221 | f 35 95 34 222 | f 97 37 34 223 | f 98 37 97 224 | f 93 95 35 225 | f 89 91 36 226 | f 35 91 93 227 | f 45 36 39 228 | f 36 91 35 229 | f 44 45 39 230 | f 89 36 45 231 | f 37 99 40 232 | f 101 40 99 233 | f 41 40 47 234 | f 101 47 40 235 | f 103 41 47 236 | f 44 39 86 237 | f 86 39 42 238 | f 43 86 42 239 | f 43 42 83 240 | f 83 42 41 241 | f 83 41 103 242 | f 37 98 99 243 | f 31 71 48 244 | f 31 48 69 245 | f 49 73 71 246 | f 49 71 31 247 | f 2 73 49 248 | f 50 2 49 249 | f 52 51 2 250 | f 52 2 50 251 | f 53 51 52 252 | f 54 53 52 253 | f 55 76 53 254 | f 55 53 54 255 | f 56 76 55 256 | f 57 56 55 257 | f 59 58 56 258 | f 59 56 57 259 | f 60 7 58 260 | f 60 58 59 261 | f 62 61 7 262 | f 62 7 60 263 | f 63 61 62 264 | f 32 10 65 265 | f 33 11 10 266 | f 33 10 32 267 | f 68 67 11 268 | f 68 11 33 269 | f 48 67 68 270 | f 69 48 68 271 | f 71 70 48 272 | f 2 1 73 273 | f 72 71 73 274 | f 51 3 2 275 | f 4 76 56 276 | f 5 56 58 277 | f 76 75 53 278 | f 53 74 51 279 | f 61 8 7 280 | f 9 61 63 281 | f 65 77 63 282 | f 11 79 10 283 | f 65 10 78 284 | f 63 62 64 285 | f 65 63 64 286 | f 65 64 66 287 | f 66 32 65 288 | f 80 67 48 289 | f 67 12 11 290 | f 6 58 7 291 | f 82 103 81 292 | f 83 103 82 293 | f 43 83 82 294 | f 43 82 84 295 | f 86 43 84 296 | f 86 84 85 297 | f 44 86 85 298 | f 44 85 19 299 | f 45 19 87 300 | f 45 44 19 301 | f 88 45 87 302 | f 91 88 90 303 | f 92 91 90 304 | f 93 91 92 305 | f 94 93 92 306 | f 46 94 110 307 | f 96 46 110 308 | f 97 46 96 309 | f 98 97 96 310 | f 98 96 15 311 | f 99 98 15 312 | f 99 15 13 313 | f 100 99 13 314 | f 47 100 102 315 | f 103 47 102 316 | f 103 102 81 317 | f 82 81 21 318 | f 85 84 20 319 | f 104 84 82 320 | f 19 85 105 321 | f 107 90 88 322 | f 108 92 90 323 | f 88 87 106 324 | f 45 88 89 325 | f 91 89 88 326 | f 87 19 18 327 | f 110 94 109 328 | f 111 96 110 329 | f 15 96 16 330 | f 100 13 112 331 | f 15 14 13 332 | f 94 95 93 333 | f 46 95 94 334 | f 114 81 102 335 | f 102 100 113 336 | f 100 101 99 337 | f 101 100 47 338 | f 17 94 92 339 | -------------------------------------------------------------------------------- /simulation/remoteApi.dylib: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skumra/romannet/0af092a1be26ac5f213f1e5c21f81f89699a4c92/simulation/remoteApi.dylib -------------------------------------------------------------------------------- /simulation/simulation.ttt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skumra/romannet/0af092a1be26ac5f213f1e5c21f81f89699a4c92/simulation/simulation.ttt -------------------------------------------------------------------------------- /simulation/test-cases/test-10-obj-00.txt: -------------------------------------------------------------------------------- 1 | 0.obj 3.058823529411764941e-01 4.745098039215686070e-01 6.549019607843137303e-01 -8.639966249465942383e-01 -1.993816792964935303e-01 2.525954507291316986e-02 -1.575720429420471191e+00 -4.672823846340179443e-02 -1.562571525573730469e+00 2 | 4.obj 3.490196078431372362e-01 6.313725490196078205e-01 3.098039215686274606e-01 -6.397874355316162109e-01 1.227664127945899963e-01 2.598815225064754486e-02 -1.574589610099792480e+00 -1.027426600456237793e+00 3.137130022048950195e+00 3 | 8.obj 6.117647058823529882e-01 4.588235294117646856e-01 3.725490196078431460e-01 -5.038855671882629395e-01 -1.674339547753334045e-02 2.599896490573883057e-02 1.569804906845092773e+00 -5.015395879745483398e-01 -1.571069240570068359e+00 4 | 7.obj 9.490196078431372140e-01 5.568627450980392357e-01 1.686274509803921684e-01 -8.961212038993835449e-01 2.019450217485427856e-01 1.333863474428653717e-02 1.572719454765319824e+00 1.299324989318847656e+00 -1.670424699783325195e+00 5 | 1.obj 9.294117647058823817e-01 7.882352941176470340e-01 2.823529411764705843e-01 -5.159068107604980469e-01 7.698099128901958466e-03 2.598643302917480469e-02 -1.570837140083312988e+00 1.084242105484008789e+00 -3.141483068466186523e+00 6 | 1.obj 7.294117647058823151e-01 6.901960784313725394e-01 6.745098039215686736e-01 -4.925853312015533447e-01 -3.888086974620819092e-02 2.599379792809486389e-02 -1.570276498794555664e+00 1.084977507591247559e+00 -6.821056012995541096e-04 7 | 2.obj 1.000000000000000000e+00 3.411764705882353033e-01 3.490196078431372362e-01 -5.995965600013732910e-01 -4.007218480110168457e-01 2.599959447979927063e-02 3.141305208206176758e+00 1.639837137190625072e-05 3.087614297866821289e+00 8 | 6.obj 6.901960784313725394e-01 4.784313725490196290e-01 6.313725490196078205e-01 -6.937489509582519531e-01 -3.193295001983642578e-01 -7.265822143554687500e+02 -2.545783996582031250e+00 -7.159572839736938477e-01 1.355211436748504639e-01 9 | 0.obj 4.627450980392157076e-01 7.176470588235294157e-01 6.980392156862744724e-01 -6.987582445144653320e-01 4.438695013523101807e-01 1.915624924004077911e-02 -1.407869309186935425e-01 7.753546833992004395e-01 1.983767598867416382e-01 10 | 6.obj 1.000000000000000000e+00 6.156862745098039547e-01 6.549019607843137303e-01 -8.913003206253051758e-01 -5.488097667694091797e-02 2.599896863102912903e-02 -1.570545911788940430e+00 6.251817941665649414e-01 3.141496181488037109e+00 11 | -------------------------------------------------------------------------------- /simulation/test-cases/test-10-obj-01.txt: -------------------------------------------------------------------------------- 1 | 0.obj 3.058823529411764941e-01 4.745098039215686070e-01 6.549019607843137303e-01 -8.938759565353393555e-01 3.463501930236816406e-01 2.525835856795310974e-02 -1.577488422393798828e+00 -1.118952929973602295e-01 -1.559650897979736328e+00 2 | 4.obj 3.490196078431372362e-01 6.313725490196078205e-01 3.098039215686274606e-01 -4.348892569541931152e-01 1.262731254100799561e-01 2.596998773515224457e-02 1.578630328178405762e+00 -1.069632411003112793e+00 4.975980147719383240e-03 3 | 8.obj 6.117647058823529882e-01 4.588235294117646856e-01 3.725490196078431460e-01 -8.410242199897766113e-01 -5.341342836618423462e-02 2.332024835050106049e-02 5.199093818664550781e-01 -1.544387698173522949e+00 -1.050708055496215820e+00 4 | 7.obj 9.490196078431372140e-01 5.568627450980392357e-01 1.686274509803921684e-01 -8.836488723754882812e-01 1.438548117876052856e-01 1.337853074073791504e-02 -1.574074864387512207e+00 3.841824829578399658e-01 -1.569372653961181641e+00 5 | 1.obj 9.294117647058823817e-01 7.882352941176470340e-01 2.823529411764705843e-01 -4.894813895225524902e-01 -9.994027018547058105e-02 2.598705515265464783e-02 -1.570391893386840820e+00 8.459852933883666992e-01 -1.571142315864562988e+00 6 | 1.obj 7.294117647058823151e-01 6.901960784313725394e-01 6.745098039215686736e-01 -5.238978266716003418e-01 -6.070769950747489929e-02 2.598705887794494629e-02 -1.570391893386840820e+00 8.459926843643188477e-01 -1.571142077445983887e+00 7 | 2.obj 1.000000000000000000e+00 3.411764705882353033e-01 3.490196078431372362e-01 -8.753516077995300293e-01 -1.954853981733322144e-01 1.349995285272598267e-02 -1.570344805717468262e+00 -1.417716622352600098e+00 -1.570291638374328613e+00 8 | 6.obj 6.901960784313725394e-01 4.784313725490196290e-01 6.313725490196078205e-01 -5.594569444656372070e-01 -2.272572368383407593e-02 2.599943056702613831e-02 1.570817708969116211e+00 -8.460341691970825195e-01 -1.570719599723815918e+00 9 | 0.obj 4.627450980392157076e-01 7.176470588235294157e-01 6.980392156862744724e-01 -9.067844152450561523e-01 1.149458717554807663e-02 1.915624924004077911e-02 -2.821606993675231934e-01 7.433290481567382812e-01 4.033253490924835205e-01 10 | 6.obj 1.000000000000000000e+00 6.156862745098039547e-01 6.549019607843137303e-01 -5.939925909042358398e-01 1.616008952260017395e-02 2.599743939936161041e-02 -1.570723176002502441e+00 8.460550308227539062e-01 -3.141587018966674805e+00 11 | -------------------------------------------------------------------------------- /simulation/test-cases/test-10-obj-02.txt: -------------------------------------------------------------------------------- 1 | 0.obj 3.058823529411764941e-01 4.745098039215686070e-01 6.549019607843137303e-01 -4.768479764461517334e-01 -4.529477283358573914e-02 2.599991299211978912e-02 -1.570903778076171875e+00 -4.091446101665496826e-01 -1.339488313533365726e-03 2 | 4.obj 3.490196078431372362e-01 6.313725490196078205e-01 3.098039215686274606e-01 -4.499697089195251465e-01 7.510072737932205200e-02 2.587917074561119080e-02 -1.570220589637756348e+00 -5.433267354965209961e-01 1.575231313705444336e+00 3 | 8.obj 6.117647058823529882e-01 4.588235294117646856e-01 3.725490196078431460e-01 -8.716513514518737793e-01 8.258035778999328613e-02 2.337466552853584290e-02 -2.167853116989135742e-01 5.332167148590087891e-01 -1.155802726745605469e+00 4 | 7.obj 9.490196078431372140e-01 5.568627450980392357e-01 1.686274509803921684e-01 -8.716565370559692383e-01 -1.214288920164108276e-01 1.345349848270416260e-02 -1.570830106735229492e+00 1.069701075553894043e+00 1.656172513961791992e+00 5 | 1.obj 9.294117647058823817e-01 7.882352941176470340e-01 2.823529411764705843e-01 -9.276854395866394043e-01 2.376570999622344971e-01 2.599691785871982574e-02 1.565860390663146973e+00 1.222539782524108887e+00 4.007950890809297562e-03 6 | 1.obj 7.294117647058823151e-01 6.901960784313725394e-01 6.745098039215686736e-01 -7.635034322738647461e-01 4.070004522800445557e-01 2.599609829485416412e-02 -1.570596814155578613e+00 -1.162186503410339355e+00 1.570914506912231445e+00 7 | 2.obj 1.000000000000000000e+00 3.411764705882353033e-01 3.490196078431372362e-01 -7.229076027870178223e-01 -3.935649096965789795e-01 2.599995769560337067e-02 3.141355276107788086e+00 1.588740560691803694e-04 2.546513795852661133e+00 8 | 6.obj 6.901960784313725394e-01 4.784313725490196290e-01 6.313725490196078205e-01 -1.037640929222106934e+00 -2.488626241683959961e-01 -3.814564514160156250e+02 -1.407393366098403931e-01 9.718171358108520508e-01 -3.128857851028442383e+00 9 | 0.obj 4.627450980392157076e-01 7.176470588235294157e-01 6.980392156862744724e-01 -5.241798758506774902e-01 -6.404431164264678955e-02 2.599991485476493835e-02 1.570903658866882324e+00 4.046312570571899414e-01 3.140253543853759766e+00 10 | 6.obj 1.000000000000000000e+00 6.156862745098039547e-01 6.549019607843137303e-01 -9.104987382888793945e-01 2.107938192784786224e-02 2.599777281284332275e-02 1.572488427162170410e+00 1.563327074050903320e+00 -1.572424530982971191e+00 11 | -------------------------------------------------------------------------------- /simulation/test-cases/test-10-obj-03.txt: -------------------------------------------------------------------------------- 1 | 0.obj 3.058823529411764941e-01 4.745098039215686070e-01 6.549019607843137303e-01 -4.505302309989929199e-01 2.177611179649829865e-02 2.599991299211978912e-02 1.570925474166870117e+00 7.020306587219238281e-01 3.140212535858154297e+00 2 | 4.obj 3.490196078431372362e-01 6.313725490196078205e-01 3.098039215686274606e-01 -3.965125977993011475e-01 -1.142756119370460510e-01 2.598538994789123535e-02 1.586319684982299805e+00 -1.519705295562744141e+00 1.519017480313777924e-02 3 | 8.obj 6.117647058823529882e-01 4.588235294117646856e-01 3.725490196078431460e-01 -6.642135381698608398e-01 4.090449213981628418e-01 2.330158650875091553e-02 4.259851574897766113e-01 -6.658453941345214844e-01 -9.377915263175964355e-01 4 | 7.obj 9.490196078431372140e-01 5.568627450980392357e-01 1.686274509803921684e-01 -7.246276140213012695e-01 3.261557519435882568e-01 1.329104881733655930e-02 -1.573226094245910645e+00 -3.266469836235046387e-01 -1.569639682769775391e+00 5 | 1.obj 9.294117647058823817e-01 7.882352941176470340e-01 2.823529411764705843e-01 -9.332630038261413574e-01 1.760754385031759739e-03 2.598705515265464783e-02 1.570137023925781250e+00 1.152019739151000977e+00 1.571355700492858887e+00 6 | 1.obj 7.294117647058823151e-01 6.901960784313725394e-01 6.745098039215686736e-01 -8.838861584663391113e-01 1.670285314321517944e-01 2.599610388278961182e-02 -1.570712447166442871e+00 -3.358049392700195312e-01 1.570759296417236328e+00 7 | 2.obj 1.000000000000000000e+00 3.411764705882353033e-01 3.490196078431372362e-01 -9.107548594474792480e-01 3.257294595241546631e-01 1.348974741995334625e-02 1.570782661437988281e+00 -6.862876415252685547e-01 -1.570765614509582520e+00 8 | 6.obj 6.901960784313725394e-01 4.784313725490196290e-01 6.313725490196078205e-01 -4.975685179233551025e-01 1.189244096167385578e-03 2.599979005753993988e-02 1.570808887481689453e+00 -8.336962759494781494e-02 -1.570742011070251465e+00 9 | 0.obj 4.627450980392157076e-01 7.176470588235294157e-01 6.980392156862744724e-01 -4.537530541419982910e-01 -1.724851131439208984e-02 2.599433623254299164e-02 -1.570674896240234375e+00 8.671962022781372070e-01 3.140277624130249023e+00 10 | 6.obj 1.000000000000000000e+00 6.156862745098039547e-01 6.549019607843137303e-01 -5.494566559791564941e-01 5.543432664126157761e-03 2.599892020225524902e-02 -1.570745825767517090e+00 8.304226398468017578e-02 -3.141547918319702148e+00 11 | -------------------------------------------------------------------------------- /simulation/test-cases/test-10-obj-04.txt: -------------------------------------------------------------------------------- 1 | 0.obj 3.058823529411764941e-01 4.745098039215686070e-01 6.549019607843137303e-01 -9.034169912338256836e-01 -7.592255622148513794e-02 2.525835856795310974e-02 -1.577625751495361328e+00 -2.294905185699462891e-01 -1.560457110404968262e+00 2 | 4.obj 3.490196078431372362e-01 6.313725490196078205e-01 3.098039215686274606e-01 -6.097059249877929688e-01 -1.278720796108245850e-01 2.595863863825798035e-02 1.570919275283813477e+00 -1.004956364631652832e+00 3.666052652988582850e-04 3 | 8.obj 6.117647058823529882e-01 4.588235294117646856e-01 3.725490196078431460e-01 -9.103258848190307617e-01 2.332479953765869141e-01 2.337326295673847198e-02 2.774595499038696289e+00 -3.218136727809906006e-03 -3.123552799224853516e+00 4 | 7.obj 9.490196078431372140e-01 5.568627450980392357e-01 1.686274509803921684e-01 -8.813908696174621582e-01 3.423114866018295288e-02 1.323722489178180695e-02 -1.579007029533386230e+00 1.277554750442504883e+00 -1.563908696174621582e+00 5 | 1.obj 9.294117647058823817e-01 7.882352941176470340e-01 2.823529411764705843e-01 -5.658037662506103516e-01 6.242804601788520813e-02 2.596948668360710144e-02 1.570075511932373047e+00 -7.883070707321166992e-01 3.141292333602905273e+00 6 | 1.obj 7.294117647058823151e-01 6.901960784313725394e-01 6.745098039215686736e-01 -4.451852440834045410e-01 -1.075510755181312561e-01 2.599938772618770599e-02 -1.570748805999755859e+00 -1.783737093210220337e-01 -3.141427278518676758e+00 7 | 2.obj 1.000000000000000000e+00 3.411764705882353033e-01 3.490196078431372362e-01 -8.657690286636352539e-01 1.461720019578933716e-01 1.349013112485408783e-02 1.570896983146667480e+00 -9.893895387649536133e-01 -1.570406794548034668e+00 8 | 6.obj 6.901960784313725394e-01 4.784313725490196290e-01 6.313725490196078205e-01 -6.031733751296997070e-01 9.899705648422241211e-02 2.599980868399143219e-02 -1.570815443992614746e+00 7.886335849761962891e-01 1.570963263511657715e+00 9 | 0.obj 4.627450980392157076e-01 7.176470588235294157e-01 6.980392156862744724e-01 -8.994771838188171387e-01 -1.740136593580245972e-01 1.915624924004077911e-02 -5.142924189567565918e-01 6.229379177093505859e-01 7.679688334465026855e-01 10 | 6.obj 1.000000000000000000e+00 6.156862745098039547e-01 6.549019607843137303e-01 -3.939029574394226074e-01 -9.854458272457122803e-02 2.599555812776088715e-02 1.570745944976806641e+00 1.779725253582000732e-01 7.677650137338787317e-05 11 | -------------------------------------------------------------------------------- /simulation/test-cases/test-10-obj-05.txt: -------------------------------------------------------------------------------- 1 | 0.obj 3.058823529411764941e-01 4.745098039215686070e-01 6.549019607843137303e-01 -6.297667026519775391e-01 -3.741825222969055176e-01 2.525835856795310974e-02 -1.577620744705200195e+00 -2.264302074909210205e-01 -1.560435533523559570e+00 2 | 4.obj 3.490196078431372362e-01 6.313725490196078205e-01 3.098039215686274606e-01 -5.284264087677001953e-01 -7.464861124753952026e-02 2.597979642450809479e-02 1.571214556694030762e+00 -1.060347437858581543e+00 2.804381540045142174e-03 3 | 8.obj 6.117647058823529882e-01 4.588235294117646856e-01 3.725490196078431460e-01 -4.807353615760803223e-01 -5.282332375645637512e-02 2.598507329821586609e-02 1.570655822753906250e+00 1.508692651987075806e-01 1.570368528366088867e+00 4 | 7.obj 9.490196078431372140e-01 5.568627450980392357e-01 1.686274509803921684e-01 -5.486415624618530273e-01 -4.070396721363067627e-02 2.597284317016601562e-02 -3.141010522842407227e+00 9.141881600953638554e-04 -2.083969354629516602e+00 5 | 1.obj 9.294117647058823817e-01 7.882352941176470340e-01 2.823529411764705843e-01 -8.036068081855773926e-01 4.176226556301116943e-01 2.598705515265464783e-02 1.570055723190307617e+00 1.201008439064025879e+00 1.571443796157836914e+00 6 | 1.obj 7.294117647058823151e-01 6.901960784313725394e-01 6.745098039215686736e-01 -9.577372670173645020e-01 1.871969997882843018e-01 2.598704956471920013e-02 -1.569863796234130859e+00 1.278969764709472656e+00 -1.571732759475708008e+00 7 | 2.obj 1.000000000000000000e+00 3.411764705882353033e-01 3.490196078431372362e-01 -4.969281554222106934e-01 -1.956742070615291595e-02 2.599923871457576752e-02 1.570602893829345703e+00 -1.117180705070495605e+00 -3.346668672747910023e-04 8 | 6.obj 6.901960784313725394e-01 4.784313725490196290e-01 6.313725490196078205e-01 -9.036800265312194824e-01 -1.503273099660873413e-01 -3.786637268066406250e+02 3.103663682937622070e+00 5.985128283500671387e-01 -7.057658582925796509e-02 9 | 0.obj 4.627450980392157076e-01 7.176470588235294157e-01 6.980392156862744724e-01 -5.611776709556579590e-01 -1.026061363518238068e-02 2.599991299211978912e-02 1.571043968200683594e+00 1.160931110382080078e+00 3.140068769454956055e+00 10 | 6.obj 1.000000000000000000e+00 6.156862745098039547e-01 6.549019607843137303e-01 -3.920490741729736328e-01 -1.275125443935394287e-01 2.598267421126365662e-02 -1.570720553398132324e+00 8.252868652343750000e-01 -3.141516447067260742e+00 11 | -------------------------------------------------------------------------------- /simulation/test-cases/test-10-obj-06.txt: -------------------------------------------------------------------------------- 1 | 0.obj 3.058823529411764941e-01 4.745098039215686070e-01 6.549019607843137303e-01 -5.980781316757202148e-01 1.577074825763702393e-02 2.525835856795310974e-02 -1.622630953788757324e+00 1.442087292671203613e+00 -1.507497191429138184e+00 2 | 4.obj 3.490196078431372362e-01 6.313725490196078205e-01 3.098039215686274606e-01 -4.535477161407470703e-01 1.077248603105545044e-01 2.598884142935276031e-02 1.570246577262878418e+00 -8.976268768310546875e-01 1.433072466170415282e-04 3 | 8.obj 6.117647058823529882e-01 4.588235294117646856e-01 3.725490196078431460e-01 -7.285601496696472168e-01 4.361740052700042725e-01 2.338188514113426208e-02 -2.197392940521240234e+00 8.248519897460937500e-01 4.886553883552551270e-01 4 | 7.obj 9.490196078431372140e-01 5.568627450980392357e-01 1.686274509803921684e-01 -8.993567228317260742e-01 2.597263753414154053e-01 1.322755496948957443e-02 -1.569637179374694824e+00 1.619989126920700073e-01 1.648199319839477539e+00 5 | 1.obj 9.294117647058823817e-01 7.882352941176470340e-01 2.823529411764705843e-01 -9.072597622871398926e-01 -9.942449629306793213e-02 2.599836885929107666e-02 1.568363904953002930e+00 2.930172085762023926e-01 1.255968003533780575e-03 6 | 1.obj 7.294117647058823151e-01 6.901960784313725394e-01 6.745098039215686736e-01 -7.587447762489318848e-01 -4.127331376075744629e-01 2.599610388278961182e-02 -1.570555090904235840e+00 1.235516548156738281e+00 1.570503592491149902e+00 7 | 2.obj 1.000000000000000000e+00 3.411764705882353033e-01 3.490196078431372362e-01 -6.425569653511047363e-01 3.856639564037322998e-01 1.349995099008083344e-02 1.570798873901367188e+00 -9.814043641090393066e-01 1.570801496505737305e+00 8 | 6.obj 6.901960784313725394e-01 4.784313725490196290e-01 6.313725490196078205e-01 -1.296027302742004395e+00 2.806423902511596680e-01 -1.000144531250000000e+03 1.074975728988647461e+00 -9.543343186378479004e-01 2.388572454452514648e+00 9 | 0.obj 4.627450980392157076e-01 7.176470588235294157e-01 6.980392156862744724e-01 -5.914387106895446777e-01 -3.659099712967872620e-02 2.525836229324340820e-02 1.618276476860046387e+00 -1.430217981338500977e+00 1.629701852798461914e+00 10 | 6.obj 1.000000000000000000e+00 6.156862745098039547e-01 6.549019607843137303e-01 -8.814523816108703613e-01 7.142892479896545410e-02 2.599963545799255371e-02 -1.570745944976806641e+00 -6.045399699360132217e-03 -3.141546010971069336e+00 11 | -------------------------------------------------------------------------------- /simulation/test-cases/test-10-obj-07.txt: -------------------------------------------------------------------------------- 1 | 0.obj 3.058823529411764941e-01 4.745098039215686070e-01 6.549019607843137303e-01 -8.885226249694824219e-01 -7.545685768127441406e-02 2.525835856795310974e-02 -1.577636003494262695e+00 -2.358794361352920532e-01 -1.560502052307128906e+00 2 | 4.obj 3.490196078431372362e-01 6.313725490196078205e-01 3.098039215686274606e-01 -5.141063928604125977e-01 -5.489115789532661438e-02 2.598390541970729828e-02 1.572011947631835938e+00 -9.765665531158447266e-01 3.324743127450346947e-03 3 | 8.obj 6.117647058823529882e-01 4.588235294117646856e-01 3.725490196078431460e-01 -3.704296946525573730e-01 -3.902738392353057861e-01 2.339143864810466766e-02 -2.409207105636596680e+00 6.948611736297607422e-01 6.187498569488525391e-01 4 | 7.obj 9.490196078431372140e-01 5.568627450980392357e-01 1.686274509803921684e-01 -4.959417283535003662e-01 3.592922985553741455e-01 1.327874045819044113e-02 -1.567450165748596191e+00 9.572667479515075684e-01 -1.573325395584106445e+00 5 | 1.obj 9.294117647058823817e-01 7.882352941176470340e-01 2.823529411764705843e-01 -4.575408399105072021e-01 -4.552512615919113159e-02 2.594062685966491699e-02 1.569360256195068359e+00 5.950005054473876953e-01 -1.569864273071289062e+00 6 | 1.obj 7.294117647058823151e-01 6.901960784313725394e-01 6.745098039215686736e-01 -5.059142112731933594e-01 -1.147382259368896484e-01 2.598874643445014954e-02 -1.570412993431091309e+00 9.524186253547668457e-01 -1.705225789919495583e-03 7 | 2.obj 1.000000000000000000e+00 3.411764705882353033e-01 3.490196078431372362e-01 -6.620352268218994141e-01 -1.293891668319702148e-01 2.599425055086612701e-02 -3.141563415527343750e+00 2.628090151119977236e-04 1.483776450157165527e+00 8 | 6.obj 6.901960784313725394e-01 4.784313725490196290e-01 6.313725490196078205e-01 -5.710005164146423340e-01 -6.554044783115386963e-02 2.599991858005523682e-02 1.570811152458190918e+00 6.089183092117309570e-01 -1.570754289627075195e+00 9 | 0.obj 4.627450980392157076e-01 7.176470588235294157e-01 6.980392156862744724e-01 -8.832956552505493164e-01 1.424720436334609985e-01 1.915624924004077911e-02 -6.515119224786758423e-02 7.832698225975036621e-01 9.080813825130462646e-02 10 | 6.obj 1.000000000000000000e+00 6.156862745098039547e-01 6.549019607843137303e-01 -5.238535404205322266e-01 2.549948636442422867e-03 2.599965035915374756e-02 -1.570821046829223633e+00 9.545059800148010254e-01 1.570864796638488770e+00 11 | -------------------------------------------------------------------------------- /simulation/test-cases/test-10-obj-08.txt: -------------------------------------------------------------------------------- 1 | 0.obj 3.058823529411764941e-01 4.745098039215686070e-01 6.549019607843137303e-01 -5.458834767341613770e-01 -2.008101902902126312e-02 2.599961124360561371e-02 -1.572645783424377441e+00 -1.511719822883605957e+00 -3.105830866843461990e-03 2 | 4.obj 3.490196078431372362e-01 6.313725490196078205e-01 3.098039215686274606e-01 -6.166151762008666992e-01 1.416724026203155518e-01 2.598096802830696106e-02 1.578990578651428223e+00 -1.321351528167724609e+00 6.800119765102863312e-03 3 | 8.obj 6.117647058823529882e-01 4.588235294117646856e-01 3.725490196078431460e-01 -8.992892503738403320e-01 2.466165125370025635e-01 2.337156236171722412e-02 3.129465579986572266e+00 -1.859941147267818451e-02 1.100259900093078613e+00 4 | 7.obj 9.490196078431372140e-01 5.568627450980392357e-01 1.686274509803921684e-01 -9.293566942214965820e-01 1.137998327612876892e-01 1.345340628176927567e-02 -1.570747971534729004e+00 1.197049617767333984e+00 1.656706809997558594e+00 5 | 1.obj 9.294117647058823817e-01 7.882352941176470340e-01 2.823529411764705843e-01 -5.430006980895996094e-01 7.671429309993982315e-03 2.598775550723075867e-02 1.570847392082214355e+00 1.505378723144531250e+00 -3.510403621476143599e-05 6 | 1.obj 7.294117647058823151e-01 6.901960784313725394e-01 6.745098039215686736e-01 -5.461698174476623535e-01 -1.500567942857742310e-01 2.598229423165321350e-02 1.561604261398315430e+00 1.547805070877075195e+00 -3.133453130722045898e+00 7 | 2.obj 1.000000000000000000e+00 3.411764705882353033e-01 3.490196078431372362e-01 -8.892617225646972656e-01 -9.761676192283630371e-03 1.349988020956516266e-02 -1.570362925529479980e+00 7.726754546165466309e-01 1.570963263511657715e+00 8 | 6.obj 6.901960784313725394e-01 4.784313725490196290e-01 6.313725490196078205e-01 -5.461826920509338379e-01 -7.147582620382308960e-02 2.599875256419181824e-02 -1.571012854576110840e+00 -1.510764479637145996e+00 1.570640325546264648e+00 9 | 0.obj 4.627450980392157076e-01 7.176470588235294157e-01 6.980392156862744724e-01 -5.464501380920410156e-01 -1.228710412979125977e-01 2.599972859025001526e-02 1.572375893592834473e+00 1.510838031768798828e+00 3.138759374618530273e+00 10 | 6.obj 1.000000000000000000e+00 6.156862745098039547e-01 6.549019607843137303e-01 -9.692828059196472168e-01 1.825933158397674561e-02 2.599950693547725677e-02 -1.570807933807373047e+00 3.076411783695220947e-01 1.570878863334655762e+00 11 | -------------------------------------------------------------------------------- /simulation/test-cases/test-10-obj-09.txt: -------------------------------------------------------------------------------- 1 | 0.obj 3.058823529411764941e-01 4.745098039215686070e-01 6.549019607843137303e-01 -4.484587609767913818e-01 -7.399411499500274658e-02 2.599991299211978912e-02 -1.571039438247680664e+00 1.153759479522705078e+00 -1.074656262062489986e-03 2 | 4.obj 3.490196078431372362e-01 6.313725490196078205e-01 3.098039215686274606e-01 -6.899880170822143555e-01 -4.371502399444580078e-01 2.586308680474758148e-02 -1.559924840927124023e+00 -1.476561784744262695e+00 1.587531566619873047e+00 3 | 8.obj 6.117647058823529882e-01 4.588235294117646856e-01 3.725490196078431460e-01 -4.700918197631835938e-01 -1.934173889458179474e-02 5.053236335515975952e-02 -2.142965793609619141e-02 4.696149751543998718e-02 1.988645076751708984e+00 4 | 7.obj 9.490196078431372140e-01 5.568627450980392357e-01 1.686274509803921684e-01 -9.033929705619812012e-01 2.986058294773101807e-01 1.339414156973361969e-02 1.568349838256835938e+00 -6.658577919006347656e-01 -1.536758184432983398e+00 5 | 1.obj 9.294117647058823817e-01 7.882352941176470340e-01 2.823529411764705843e-01 -4.991133809089660645e-01 2.359391935169696808e-02 2.595471218228340149e-02 -1.570811629295349121e+00 1.047576665878295898e+00 -3.141265153884887695e+00 6 | 1.obj 7.294117647058823151e-01 6.901960784313725394e-01 6.745098039215686736e-01 -4.669134020805358887e-01 -2.605012618005275726e-02 2.598910592496395111e-02 -1.570627570152282715e+00 1.154365777969360352e+00 1.570565581321716309e+00 7 | 2.obj 1.000000000000000000e+00 3.411764705882353033e-01 3.490196078431372362e-01 -3.774843811988830566e-01 7.194006443023681641e-02 2.599971368908882141e-02 3.141317367553710938e+00 8.460550452582538128e-05 2.826282501220703125e+00 8 | 6.obj 6.901960784313725394e-01 4.784313725490196290e-01 6.313725490196078205e-01 -6.260623335838317871e-01 -1.656028181314468384e-01 -6.863348388671875000e+02 -7.580479383468627930e-01 1.374383121728897095e-01 -2.365541458129882812e+00 9 | 0.obj 4.627450980392157076e-01 7.176470588235294157e-01 6.980392156862744724e-01 -8.918865919113159180e-01 -1.935733407735824585e-01 1.915624924004077911e-02 2.239872068166732788e-01 7.594376802444458008e-01 -3.209284842014312744e-01 10 | 6.obj 1.000000000000000000e+00 6.156862745098039547e-01 6.549019607843137303e-01 -8.962839841842651367e-01 1.182995215058326721e-01 2.599956467747688293e-02 -1.570122003555297852e+00 4.625549018383026123e-01 3.140546321868896484e+00 11 | -------------------------------------------------------------------------------- /simulation/test-cases/test-10-obj-10.txt: -------------------------------------------------------------------------------- 1 | 0.obj 3.058823529411764941e-01 4.745098039215686070e-01 6.549019607843137303e-01 -4.850563108921051025e-01 -2.247652038931846619e-03 2.599991299211978912e-02 -1.570897698402404785e+00 -2.410651147365570068e-01 -1.321009476669132710e-03 2 | 4.obj 3.490196078431372362e-01 6.313725490196078205e-01 3.098039215686274606e-01 -8.822568655014038086e-01 1.264191120862960815e-01 2.597530744969844818e-02 1.573765277862548828e+00 -6.823571920394897461e-01 8.587929769419133663e-04 3 | 8.obj 6.117647058823529882e-01 4.588235294117646856e-01 3.725490196078431460e-01 -8.861376643180847168e-01 -1.900835931301116943e-01 2.329775691032409668e-02 -6.189142465591430664e-01 1.116745114326477051e+00 -9.005227088928222656e-01 4 | 7.obj 9.490196078431372140e-01 5.568627450980392357e-01 1.686274509803921684e-01 -8.720415234565734863e-01 2.891220152378082275e-03 1.345393527299165726e-02 -1.570900797843933105e+00 1.105768680572509766e+00 1.655882358551025391e+00 5 | 1.obj 9.294117647058823817e-01 7.882352941176470340e-01 2.823529411764705843e-01 -9.133684039115905762e-01 3.698933422565460205e-01 2.597486414015293121e-02 1.571752190589904785e+00 1.447395801544189453e+00 -6.454970571212470531e-04 6 | 1.obj 7.294117647058823151e-01 6.901960784313725394e-01 6.745098039215686736e-01 -6.965701580047607422e-01 -3.790239691734313965e-01 2.599610388278961182e-02 1.570436000823974609e+00 -1.348667263984680176e+00 -1.571212410926818848e+00 7 | 2.obj 1.000000000000000000e+00 3.411764705882353033e-01 3.490196078431372362e-01 -6.122485399246215820e-01 -1.585589647293090820e-01 2.599985897541046143e-02 -3.140546560287475586e+00 5.894388887099921703e-04 2.486470937728881836e+00 8 | 6.obj 6.901960784313725394e-01 4.784313725490196290e-01 6.313725490196078205e-01 -9.170722961425781250e-01 -1.203523501753807068e-01 2.597122453153133392e-02 -1.571383118629455566e+00 -1.226401686668395996e+00 -1.401903340592980385e-03 9 | 0.obj 4.627450980392157076e-01 7.176470588235294157e-01 6.980392156862744724e-01 -5.852774381637573242e-01 -2.469941601157188416e-02 2.599992789328098297e-02 -1.570702910423278809e+00 -2.377281934022903442e-01 3.140332221984863281e+00 10 | 6.obj 1.000000000000000000e+00 6.156862745098039547e-01 6.549019607843137303e-01 -5.346742272377014160e-01 -1.557980291545391083e-02 2.599953673779964447e-02 1.570808649063110352e+00 2.388687729835510254e-01 -1.570748329162597656e+00 11 | -------------------------------------------------------------------------------- /trainer.py: -------------------------------------------------------------------------------- 1 | import logging 2 | import os 3 | 4 | import cv2 5 | import numpy as np 6 | import torch 7 | from torch.autograd import Variable 8 | 9 | from models.network import ManipulationNet 10 | 11 | 12 | class Trainer: 13 | def __init__(self, network, force_cpu, push_enabled, place_enabled, num_rotations): 14 | 15 | # Check if CUDA can be used 16 | if torch.cuda.is_available() and not force_cpu: 17 | logging.info("CUDA detected. Running with GPU acceleration.") 18 | use_cuda = True 19 | elif force_cpu: 20 | logging.info("CUDA detected, but overriding with option '--cpu'. Running with only CPU.") 21 | use_cuda = False 22 | else: 23 | logging.info("CUDA is *NOT* detected. Running with only CPU.") 24 | use_cuda = False 25 | 26 | if use_cuda: 27 | self.device = torch.device("cuda") 28 | else: 29 | self.device = torch.device("cpu") 30 | 31 | # ManipulationNet for deep reinforcement learning 32 | self.push_enabled = push_enabled 33 | self.place_enabled = place_enabled 34 | self.model = ManipulationNet(network, self.device, self.push_enabled, self.place_enabled, num_rotations) 35 | self.model = self.model.to(self.device) 36 | 37 | # Initialize Huber loss 38 | self.criterion = torch.nn.SmoothL1Loss(reduce=False) 39 | self.criterion = self.criterion.to(self.device) 40 | 41 | # Set model to training mode 42 | self.model.train() 43 | 44 | # Initialize optimizer 45 | self.optimizer = torch.optim.SGD(self.model.parameters(), lr=1e-4, momentum=0.9, weight_decay=2e-5) 46 | self.iteration = 0 47 | self.loss_value = 0 48 | self.running_loss = np.ones(10) 49 | 50 | # Initialize lists to save execution info and RL variables 51 | self.executed_action_log = [] 52 | self.label_value_log = [] 53 | self.reward_value_log = [] 54 | self.grasp_success_log = [] 55 | self.predicted_value_log = [] 56 | self.is_exploit_log = [] 57 | self.task_complete_log = [] 58 | if self.push_enabled: 59 | self.push_success_log = [] 60 | if self.place_enabled: 61 | self.place_success_log = [] 62 | 63 | def load_snapshot(self, snapshot_file): 64 | """ 65 | Load pre-trained model 66 | """ 67 | self.model.load_state_dict(torch.load(snapshot_file)) 68 | logging.info('Pre-trained model snapshot loaded from: %s' % snapshot_file) 69 | 70 | def preload(self, transitions_directory): 71 | """ 72 | Pre-load execution info and RL variables 73 | """ 74 | self.executed_action_log = np.loadtxt(os.path.join(transitions_directory, 'executed-action.log.txt'), 75 | delimiter=' ') 76 | self.iteration = self.executed_action_log.shape[0] - 2 77 | self.executed_action_log = self.executed_action_log[0:self.iteration, :] 78 | self.executed_action_log = self.executed_action_log.tolist() 79 | self.label_value_log = np.loadtxt(os.path.join(transitions_directory, 'label-value.log.txt'), delimiter=' ') 80 | self.label_value_log = self.label_value_log[0:self.iteration] 81 | self.label_value_log.shape = (self.iteration, 1) 82 | self.label_value_log = self.label_value_log.tolist() 83 | self.predicted_value_log = np.loadtxt(os.path.join(transitions_directory, 'predicted-value.log.txt'), 84 | delimiter=' ') 85 | self.predicted_value_log = self.predicted_value_log[0:self.iteration] 86 | self.predicted_value_log.shape = (self.iteration, 1) 87 | self.predicted_value_log = self.predicted_value_log.tolist() 88 | self.reward_value_log = np.loadtxt(os.path.join(transitions_directory, 'reward-value.log.txt'), delimiter=' ') 89 | self.reward_value_log = self.reward_value_log[0:self.iteration] 90 | self.reward_value_log.shape = (self.iteration, 1) 91 | self.reward_value_log = self.reward_value_log.tolist() 92 | self.grasp_success_log = np.loadtxt(os.path.join(transitions_directory, 'grasp-success.log.txt'), delimiter=' ') 93 | self.grasp_success_log = self.grasp_success_log[0:self.iteration] 94 | self.grasp_success_log.shape = (self.iteration, 1) 95 | self.grasp_success_log = self.grasp_success_log.tolist() 96 | self.is_exploit_log = np.loadtxt(os.path.join(transitions_directory, 'is-exploit.log.txt'), delimiter=' ') 97 | self.is_exploit_log = self.is_exploit_log[0:self.iteration] 98 | self.is_exploit_log.shape = (self.iteration, 1) 99 | self.is_exploit_log = self.is_exploit_log.tolist() 100 | self.task_complete_log = np.loadtxt(os.path.join(transitions_directory, 'task_complete.log.txt'), delimiter=' ') 101 | self.task_complete_log.shape = (self.task_complete_log.shape[0], 1) 102 | self.task_complete_log = self.task_complete_log.tolist() 103 | if self.push_enabled: 104 | self.push_success_log = np.loadtxt(os.path.join(transitions_directory, 'push-success.log.txt'), 105 | delimiter=' ') 106 | self.push_success_log = self.push_success_log[0:self.iteration] 107 | self.push_success_log.shape = (self.iteration, 1) 108 | self.push_success_log = self.push_success_log.tolist() 109 | if self.place_enabled: 110 | self.place_success_log = np.loadtxt(os.path.join(transitions_directory, 'place-success.log.txt'), 111 | delimiter=' ') 112 | self.place_success_log = self.place_success_log[0:self.iteration] 113 | self.place_success_log.shape = (self.iteration, 1) 114 | self.place_success_log = self.place_success_log.tolist() 115 | 116 | def forward(self, color_heightmap, depth_heightmap, is_volatile=False, specific_rotation=None): 117 | """ 118 | Compute forward pass through model to compute affordances/Q 119 | """ 120 | # Pre-process 121 | data = self.model.pre_process(color_heightmap, depth_heightmap) 122 | 123 | # Pass input data through model 124 | output_prob = self.model.forward(data, is_volatile, specific_rotation) 125 | 126 | # Post process 127 | push_predictions, grasp_predictions, place_predictions = self.model.post_process(data, output_prob) 128 | 129 | return push_predictions, grasp_predictions, place_predictions 130 | 131 | def backprop(self, color_heightmap, depth_heightmap, primitive_action, best_pix_ind, label_value, filter_type): 132 | """ 133 | Compute labels and backpropagate 134 | """ 135 | # Compute labels 136 | label = np.zeros((1, 320, 320)) 137 | action_area = np.zeros((224, 224)) 138 | action_area[best_pix_ind[1]][best_pix_ind[2]] = 1 139 | 140 | if filter_type == 1: 141 | blur_kernel = np.ones((5, 5), np.float32)/25 142 | action_area = cv2.filter2D(action_area, -1, blur_kernel) 143 | elif filter_type == 2: 144 | action_area = cv2.GaussianBlur(action_area, (3, 3), 0) 145 | elif filter_type == 3: 146 | action_area = cv2.GaussianBlur(action_area, (5, 5), 0) 147 | 148 | tmp_label = np.zeros((224, 224)) 149 | tmp_label[action_area > 0] = label_value 150 | label[0, 48:(320 - 48), 48:(320 - 48)] = tmp_label 151 | 152 | # Compute label mask 153 | label_weights = np.zeros(label.shape) 154 | tmp_label_weights = np.zeros((224, 224)) 155 | tmp_label_weights[action_area > 0] = 1 156 | label_weights[0, 48:(320 - 48), 48:(320 - 48)] = tmp_label_weights 157 | 158 | _label = Variable(torch.from_numpy(label).float().to(self.device)) 159 | _label_weights = Variable(torch.from_numpy(label_weights).float().to(self.device), requires_grad=False) 160 | 161 | # Compute loss and backward pass 162 | self.optimizer.zero_grad() 163 | if primitive_action == 'push': 164 | 165 | # Do forward pass with specified rotation (to save gradients) 166 | self.forward(color_heightmap, depth_heightmap, is_volatile=False, specific_rotation=best_pix_ind[0]) 167 | loss = self.criterion(self.model.output_prob[0][0].view(1, 320, 320), _label) * _label_weights 168 | loss = loss.sum() 169 | loss.backward() 170 | self.loss_value = loss.cpu().data.numpy() 171 | 172 | elif primitive_action == 'grasp': 173 | 174 | # Do forward pass with specified rotation (to save gradients) 175 | self.forward(color_heightmap, depth_heightmap, is_volatile=False, specific_rotation=best_pix_ind[0]) 176 | loss = self.criterion(self.model.output_prob[0][1].view(1, 320, 320), _label) * _label_weights 177 | loss = loss.sum() 178 | loss.backward() 179 | self.loss_value = loss.cpu().data.numpy() 180 | 181 | opposite_rotate_idx = (best_pix_ind[0] + self.model.num_rotations / 2) % self.model.num_rotations 182 | 183 | self.forward(color_heightmap, depth_heightmap, is_volatile=False, specific_rotation=opposite_rotate_idx) 184 | 185 | loss = self.criterion(self.model.output_prob[0][1].view(1, 320, 320), _label) * _label_weights 186 | 187 | loss = loss.sum() 188 | loss.backward() 189 | self.loss_value += loss.cpu().data.numpy() 190 | 191 | self.loss_value = self.loss_value / 2 192 | 193 | elif primitive_action == 'place': 194 | 195 | # Do forward pass with specified rotation (to save gradients) 196 | self.forward(color_heightmap, depth_heightmap, is_volatile=False, specific_rotation=best_pix_ind[0]) 197 | loss = self.criterion(self.model.output_prob[0][2].view(1, 320, 320), _label) * _label_weights 198 | loss = loss.sum() 199 | loss.backward() 200 | self.loss_value = loss.cpu().data.numpy() 201 | 202 | opposite_rotate_idx = (best_pix_ind[0] + self.model.num_rotations / 2) % self.model.num_rotations 203 | 204 | self.forward(color_heightmap, depth_heightmap, is_volatile=False, specific_rotation=opposite_rotate_idx) 205 | loss = self.criterion(self.model.output_prob[0][2].view(1, 320, 320), _label) * _label_weights 206 | loss = loss.sum() 207 | loss.backward() 208 | self.loss_value += loss.cpu().data.numpy() 209 | 210 | self.loss_value = self.loss_value / 2 211 | 212 | logging.info('Training loss: %f' % self.loss_value) 213 | self.optimizer.step() 214 | self.running_loss[:-1] = self.running_loss[1:] 215 | self.running_loss[-1] = self.loss_value 216 | -------------------------------------------------------------------------------- /utils/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skumra/romannet/0af092a1be26ac5f213f1e5c21f81f89699a4c92/utils/__init__.py -------------------------------------------------------------------------------- /utils/logger.py: -------------------------------------------------------------------------------- 1 | import datetime 2 | import json 3 | import logging 4 | import os 5 | import time 6 | 7 | import cv2 8 | import numpy as np 9 | import torch 10 | 11 | 12 | class Logger: 13 | 14 | def __init__(self, logging_directory, args=None): 15 | 16 | # Create directory to save data 17 | if logging_directory: 18 | self.base_directory = logging_directory 19 | else: 20 | timestamp = time.time() 21 | timestamp_value = datetime.datetime.fromtimestamp(timestamp) 22 | self.base_directory = os.path.join(os.path.abspath('logs'), timestamp_value.strftime('%Y-%m-%d.%H:%M:%S')) 23 | 24 | self.info_directory = os.path.join(self.base_directory, 'info') 25 | self.color_images_directory = os.path.join(self.base_directory, 'data', 'color-images') 26 | self.depth_images_directory = os.path.join(self.base_directory, 'data', 'depth-images') 27 | self.color_heightmaps_directory = os.path.join(self.base_directory, 'data', 'color-heightmaps') 28 | self.depth_heightmaps_directory = os.path.join(self.base_directory, 'data', 'depth-heightmaps') 29 | self.models_directory = os.path.join(self.base_directory, 'models') 30 | self.visualizations_directory = os.path.join(self.base_directory, 'visualizations') 31 | self.recordings_directory = os.path.join(self.base_directory, 'recordings') 32 | self.transitions_directory = os.path.join(self.base_directory, 'transitions') 33 | 34 | if os.path.exists(self.base_directory): 35 | self.logging_directory_exists = True 36 | else: 37 | self.logging_directory_exists = False 38 | if not os.path.exists(self.info_directory): 39 | os.makedirs(self.info_directory) 40 | if not os.path.exists(self.color_images_directory): 41 | os.makedirs(self.color_images_directory) 42 | if not os.path.exists(self.depth_images_directory): 43 | os.makedirs(self.depth_images_directory) 44 | if not os.path.exists(self.color_heightmaps_directory): 45 | os.makedirs(self.color_heightmaps_directory) 46 | if not os.path.exists(self.depth_heightmaps_directory): 47 | os.makedirs(self.depth_heightmaps_directory) 48 | if not os.path.exists(self.models_directory): 49 | os.makedirs(self.models_directory) 50 | if not os.path.exists(self.visualizations_directory): 51 | os.makedirs(self.visualizations_directory) 52 | if not os.path.exists(self.recordings_directory): 53 | os.makedirs(self.recordings_directory) 54 | if not os.path.exists(self.transitions_directory): 55 | os.makedirs(os.path.join(self.transitions_directory, 'data')) 56 | 57 | # Save commandline args 58 | if args is not None: 59 | params_path = os.path.join(self.base_directory, 'commandline_args.json') 60 | with open(params_path, 'w') as f: 61 | json.dump(vars(args), f) 62 | 63 | # Initialize logging 64 | logging.root.handlers = [] 65 | logging.basicConfig( 66 | level=logging.INFO, 67 | filename="{0}/{1}.log".format(self.base_directory, 'log'), 68 | format='[%(asctime)s] {%(pathname)s:%(lineno)d} %(levelname)s - %(message)s', 69 | datefmt='%H:%M:%S' 70 | ) 71 | # set up logging to console 72 | console = logging.StreamHandler() 73 | console.setLevel(logging.DEBUG) 74 | # set a format which is simpler for console use 75 | formatter = logging.Formatter('%(name)-12s: %(levelname)-8s %(message)s') 76 | console.setFormatter(formatter) 77 | # add the handler to the root logger 78 | logging.getLogger('').addHandler(console) 79 | 80 | if self.logging_directory_exists: 81 | logging.info('Pre-loaded data logging session: %s' % self.base_directory) 82 | else: 83 | logging.info('Created data logging session: %s' % self.base_directory) 84 | 85 | def save_camera_info(self, intrinsics, pose, depth_scale): 86 | np.savetxt(os.path.join(self.info_directory, 'camera-intrinsics.txt'), intrinsics, delimiter=' ') 87 | np.savetxt(os.path.join(self.info_directory, 'camera-pose.txt'), pose, delimiter=' ') 88 | np.savetxt(os.path.join(self.info_directory, 'camera-depth-scale.txt'), [depth_scale], delimiter=' ') 89 | 90 | def save_heightmap_info(self, boundaries, resolution): 91 | np.savetxt(os.path.join(self.info_directory, 'heightmap-boundaries.txt'), boundaries, delimiter=' ') 92 | np.savetxt(os.path.join(self.info_directory, 'heightmap-resolution.txt'), [resolution], delimiter=' ') 93 | 94 | def save_images(self, iteration, color_image, depth_image, mode): 95 | color_image = cv2.cvtColor(color_image, cv2.COLOR_RGB2BGR) 96 | cv2.imwrite(os.path.join(self.color_images_directory, '%06d.%s.color.png' % (iteration, mode)), color_image) 97 | depth_image = np.round(depth_image * 10000).astype(np.uint16) # Save depth in 1e-4 meters 98 | cv2.imwrite(os.path.join(self.depth_images_directory, '%06d.%s.depth.png' % (iteration, mode)), depth_image) 99 | 100 | def save_heightmaps(self, iteration, color_heightmap, depth_heightmap, mode): 101 | color_heightmap = cv2.cvtColor(color_heightmap, cv2.COLOR_RGB2BGR) 102 | cv2.imwrite(os.path.join(self.color_heightmaps_directory, '%06d.%s.color.png' % (iteration, mode)), 103 | color_heightmap) 104 | depth_heightmap = np.round(depth_heightmap * 100000).astype(np.uint16) # Save depth in 1e-5 meters 105 | cv2.imwrite(os.path.join(self.depth_heightmaps_directory, '%06d.%s.depth.png' % (iteration, mode)), 106 | depth_heightmap) 107 | 108 | def write_to_log(self, log_name, log): 109 | np.savetxt(os.path.join(self.transitions_directory, '%s.log.txt' % log_name), log, delimiter=' ') 110 | 111 | def save_model(self, iteration, model): 112 | torch.save(model.cpu().state_dict(), os.path.join(self.models_directory, 'snapshot-%06d.pth' % iteration)) 113 | 114 | def save_backup_model(self, model): 115 | torch.save(model.state_dict(), os.path.join(self.models_directory, 'snapshot-backup.pth')) 116 | 117 | def save_visualizations(self, iteration, affordance_vis, name): 118 | # cv2.imwrite(os.path.join(self.visualizations_directory, '%06d.%s.png' % (iteration, name)), affordance_vis) 119 | cv2.imwrite(os.path.join(self.visualizations_directory, '%s.png' % name), affordance_vis) 120 | 121 | def make_new_recording_directory(self, iteration): 122 | recording_directory = os.path.join(self.recordings_directory, '%06d' % (iteration)) 123 | if not os.path.exists(recording_directory): 124 | os.makedirs(recording_directory) 125 | return recording_directory 126 | 127 | def save_transition(self, iteration, transition): 128 | depth_heightmap = np.round(transition.state * 100000).astype(np.uint16) # Save depth in 1e-5 meters 129 | cv2.imwrite(os.path.join(self.transitions_directory, 'data', '%06d.0.depth.png' % (iteration)), depth_heightmap) 130 | next_depth_heightmap = np.round(transition.next_state * 100000).astype(np.uint16) # Save depth in 1e-5 meters 131 | cv2.imwrite(os.path.join(self.transitions_directory, 'data', '%06d.1.depth.png' % (iteration)), 132 | next_depth_heightmap) 133 | # np.savetxt(os.path.join(self.transitions_directory, '%06d.action.txt' % (iteration)), [1 if (transition.action == 'grasp') else 0], delimiter=' ') 134 | # np.savetxt(os.path.join(self.transitions_directory, '%06d.reward.txt' % (iteration)), [reward_value], delimiter=' ') 135 | -------------------------------------------------------------------------------- /utils/utils.py: -------------------------------------------------------------------------------- 1 | import math 2 | 3 | import numpy as np 4 | 5 | 6 | def get_pointcloud(color_img, depth_img, camera_intrinsics): 7 | # Get depth image size 8 | im_h = depth_img.shape[0] 9 | im_w = depth_img.shape[1] 10 | 11 | # Project depth into 3D point cloud in camera coordinates 12 | pix_x, pix_y = np.meshgrid(np.linspace(0, im_w - 1, im_w), np.linspace(0, im_h - 1, im_h)) 13 | cam_pts_x = np.multiply(pix_x - camera_intrinsics[0][2], depth_img / camera_intrinsics[0][0]) 14 | cam_pts_y = np.multiply(pix_y - camera_intrinsics[1][2], depth_img / camera_intrinsics[1][1]) 15 | cam_pts_z = depth_img.copy() 16 | cam_pts_x.shape = (im_h * im_w, 1) 17 | cam_pts_y.shape = (im_h * im_w, 1) 18 | cam_pts_z.shape = (im_h * im_w, 1) 19 | 20 | # Reshape image into colors for 3D point cloud 21 | rgb_pts_r = color_img[:, :, 0] 22 | rgb_pts_g = color_img[:, :, 1] 23 | rgb_pts_b = color_img[:, :, 2] 24 | rgb_pts_r.shape = (im_h * im_w, 1) 25 | rgb_pts_g.shape = (im_h * im_w, 1) 26 | rgb_pts_b.shape = (im_h * im_w, 1) 27 | 28 | cam_pts = np.concatenate((cam_pts_x, cam_pts_y, cam_pts_z), axis=1) 29 | rgb_pts = np.concatenate((rgb_pts_r, rgb_pts_g, rgb_pts_b), axis=1) 30 | 31 | return cam_pts, rgb_pts 32 | 33 | 34 | def get_heightmap(color_img, depth_img, cam_intrinsics, cam_pose, workspace_limits, heightmap_resolution): 35 | # Compute heightmap size 36 | heightmap_size = np.round(((workspace_limits[1][1] - workspace_limits[1][0]) / heightmap_resolution, 37 | (workspace_limits[0][1] - workspace_limits[0][0]) / heightmap_resolution)).astype(int) 38 | 39 | # Get 3D point cloud from RGB-D images 40 | surface_pts, color_pts = get_pointcloud(color_img, depth_img, cam_intrinsics) 41 | 42 | # Transform 3D point cloud from camera coordinates to robot coordinates 43 | surface_pts = np.transpose( 44 | np.dot(cam_pose[0:3, 0:3], np.transpose(surface_pts)) + np.tile(cam_pose[0:3, 3:], (1, surface_pts.shape[0]))) 45 | 46 | # Sort surface points by z value 47 | sort_z_ind = np.argsort(surface_pts[:, 2]) 48 | surface_pts = surface_pts[sort_z_ind] 49 | color_pts = color_pts[sort_z_ind] 50 | 51 | # Filter out surface points outside heightmap boundaries 52 | heightmap_valid_ind = np.logical_and(np.logical_and(np.logical_and( 53 | np.logical_and(surface_pts[:, 0] >= workspace_limits[0][0], surface_pts[:, 0] < workspace_limits[0][1]), 54 | surface_pts[:, 1] >= workspace_limits[1][0]), surface_pts[:, 1] < workspace_limits[1][1]), 55 | surface_pts[:, 2] < workspace_limits[2][1]) 56 | surface_pts = surface_pts[heightmap_valid_ind] 57 | color_pts = color_pts[heightmap_valid_ind] 58 | 59 | # Create orthographic top-down-view RGB-D heightmaps 60 | color_heightmap_r = np.zeros((heightmap_size[0], heightmap_size[1], 1), dtype=np.uint8) 61 | color_heightmap_g = np.zeros((heightmap_size[0], heightmap_size[1], 1), dtype=np.uint8) 62 | color_heightmap_b = np.zeros((heightmap_size[0], heightmap_size[1], 1), dtype=np.uint8) 63 | depth_heightmap = np.zeros(heightmap_size) 64 | heightmap_pix_x = np.floor((surface_pts[:, 0] - workspace_limits[0][0]) / heightmap_resolution).astype(int) 65 | heightmap_pix_y = np.floor((surface_pts[:, 1] - workspace_limits[1][0]) / heightmap_resolution).astype(int) 66 | color_heightmap_r[heightmap_pix_y, heightmap_pix_x] = color_pts[:, [0]] 67 | color_heightmap_g[heightmap_pix_y, heightmap_pix_x] = color_pts[:, [1]] 68 | color_heightmap_b[heightmap_pix_y, heightmap_pix_x] = color_pts[:, [2]] 69 | color_heightmap = np.concatenate((color_heightmap_r, color_heightmap_g, color_heightmap_b), axis=2) 70 | depth_heightmap[heightmap_pix_y, heightmap_pix_x] = surface_pts[:, 2] 71 | z_bottom = workspace_limits[2][0] 72 | depth_heightmap = depth_heightmap - z_bottom 73 | depth_heightmap[depth_heightmap < 0] = 0 74 | depth_heightmap[depth_heightmap == -z_bottom] = np.nan 75 | 76 | return color_heightmap, depth_heightmap 77 | 78 | 79 | # Get rotation matrix from euler angles 80 | def euler2rotm(theta): 81 | R_x = np.array([[1, 0, 0], 82 | [0, math.cos(theta[0]), -math.sin(theta[0])], 83 | [0, math.sin(theta[0]), math.cos(theta[0])] 84 | ]) 85 | R_y = np.array([[math.cos(theta[1]), 0, math.sin(theta[1])], 86 | [0, 1, 0], 87 | [-math.sin(theta[1]), 0, math.cos(theta[1])] 88 | ]) 89 | R_z = np.array([[math.cos(theta[2]), -math.sin(theta[2]), 0], 90 | [math.sin(theta[2]), math.cos(theta[2]), 0], 91 | [0, 0, 1] 92 | ]) 93 | R = np.dot(R_z, np.dot(R_y, R_x)) 94 | return R 95 | 96 | 97 | # Checks if a matrix is a valid rotation matrix. 98 | def isRotm(R): 99 | Rt = np.transpose(R) 100 | shouldBeIdentity = np.dot(Rt, R) 101 | I = np.identity(3, dtype=R.dtype) 102 | n = np.linalg.norm(I - shouldBeIdentity) 103 | return n < 1e-6 104 | 105 | 106 | # Calculates rotation matrix to euler angles 107 | def rotm2euler(R): 108 | assert (isRotm(R)) 109 | 110 | sy = math.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0]) 111 | singular = sy < 1e-6 112 | 113 | if not singular: 114 | x = math.atan2(R[2, 1], R[2, 2]) 115 | y = math.atan2(-R[2, 0], sy) 116 | z = math.atan2(R[1, 0], R[0, 0]) 117 | else: 118 | x = math.atan2(-R[1, 2], R[1, 1]) 119 | y = math.atan2(-R[2, 0], sy) 120 | z = 0 121 | 122 | return np.array([x, y, z]) 123 | 124 | 125 | def angle2rotm(angle, axis, point=None): 126 | # Copyright (c) 2006-2018, Christoph Gohlke 127 | 128 | sina = math.sin(angle) 129 | cosa = math.cos(angle) 130 | axis = axis / np.linalg.norm(axis) 131 | 132 | # Rotation matrix around unit vector 133 | R = np.diag([cosa, cosa, cosa]) 134 | R += np.outer(axis, axis) * (1.0 - cosa) 135 | axis *= sina 136 | R += np.array([[0.0, -axis[2], axis[1]], 137 | [axis[2], 0.0, -axis[0]], 138 | [-axis[1], axis[0], 0.0]]) 139 | M = np.identity(4) 140 | M[:3, :3] = R 141 | if point is not None: 142 | # Rotation not around origin 143 | point = np.array(point[:3], dtype=np.float64, copy=False) 144 | M[:3, 3] = point - np.dot(R, point) 145 | return M 146 | 147 | 148 | def rotm2angle(R): 149 | # From: euclideanspace.com 150 | 151 | epsilon = 0.01 # Margin to allow for rounding errors 152 | epsilon2 = 0.1 # Margin to distinguish between 0 and 180 degrees 153 | 154 | assert (isRotm(R)) 155 | 156 | if ((abs(R[0][1] - R[1][0]) < epsilon) and (abs(R[0][2] - R[2][0]) < epsilon) and ( 157 | abs(R[1][2] - R[2][1]) < epsilon)): 158 | # Singularity found 159 | # First check for identity matrix which must have +1 for all terms in leading diagonaland zero in other terms 160 | if ((abs(R[0][1] + R[1][0]) < epsilon2) and (abs(R[0][2] + R[2][0]) < epsilon2) and ( 161 | abs(R[1][2] + R[2][1]) < epsilon2) and (abs(R[0][0] + R[1][1] + R[2][2] - 3) < epsilon2)): 162 | # this singularity is identity matrix so angle = 0 163 | return [0, 1, 0, 0] # zero angle, arbitrary axis 164 | 165 | # Otherwise this singularity is angle = 180 166 | angle = np.pi 167 | xx = (R[0][0] + 1) / 2 168 | yy = (R[1][1] + 1) / 2 169 | zz = (R[2][2] + 1) / 2 170 | xy = (R[0][1] + R[1][0]) / 4 171 | xz = (R[0][2] + R[2][0]) / 4 172 | yz = (R[1][2] + R[2][1]) / 4 173 | if ((xx > yy) and (xx > zz)): # R[0][0] is the largest diagonal term 174 | if (xx < epsilon): 175 | x = 0 176 | y = 0.7071 177 | z = 0.7071 178 | else: 179 | x = np.sqrt(xx) 180 | y = xy / x 181 | z = xz / x 182 | elif (yy > zz): # R[1][1] is the largest diagonal term 183 | if (yy < epsilon): 184 | x = 0.7071 185 | y = 0 186 | z = 0.7071 187 | else: 188 | y = np.sqrt(yy) 189 | x = xy / y 190 | z = yz / y 191 | else: # R[2][2] is the largest diagonal term so base result on this 192 | if (zz < epsilon): 193 | x = 0.7071 194 | y = 0.7071 195 | z = 0 196 | else: 197 | z = np.sqrt(zz) 198 | x = xz / z 199 | y = yz / z 200 | return [angle, x, y, z] # Return 180 deg rotation 201 | 202 | # As we have reached here there are no singularities so we can handle normally 203 | s = np.sqrt( 204 | (R[2][1] - R[1][2]) * (R[2][1] - R[1][2]) + (R[0][2] - R[2][0]) * (R[0][2] - R[2][0]) + (R[1][0] - R[0][1]) * ( 205 | R[1][0] - R[0][1])) # used to normalise 206 | if (abs(s) < 0.001): 207 | s = 1 208 | 209 | # Prevent divide by zero, should not happen if matrix is orthogonal and should be 210 | # Caught by singularity test above, but I've left it in just in case 211 | angle = np.arccos((R[0][0] + R[1][1] + R[2][2] - 1) / 2) 212 | x = (R[2][1] - R[1][2]) / s 213 | y = (R[0][2] - R[2][0]) / s 214 | z = (R[1][0] - R[0][1]) / s 215 | return [angle, x, y, z] 216 | -------------------------------------------------------------------------------- /utils/viz.py: -------------------------------------------------------------------------------- 1 | import math 2 | 3 | import cv2 4 | import numpy as np 5 | 6 | 7 | def draw_angled_rec(x0, y0, angle, img, width=50, height=25): 8 | b = math.cos(angle) * 0.5 9 | a = math.sin(angle) * 0.5 10 | pt0 = (int(x0 - a * height - b * width), 11 | int(y0 + b * height - a * width)) 12 | pt1 = (int(x0 + a * height - b * width), 13 | int(y0 - b * height - a * width)) 14 | pt2 = (int(2 * x0 - pt0[0]), int(2 * y0 - pt0[1])) 15 | pt3 = (int(2 * x0 - pt1[0]), int(2 * y0 - pt1[1])) 16 | 17 | cv2.line(img, pt0, pt1, (255, 255, 255), 3) 18 | cv2.line(img, pt1, pt2, (255, 255, 255), 3) 19 | cv2.line(img, pt2, pt3, (255, 255, 255), 3) 20 | cv2.line(img, pt3, pt0, (255, 255, 255), 3) 21 | 22 | return img 23 | 24 | 25 | def draw_arrow(x, y, angle, img, length=50): 26 | _x = int(x + length * math.cos(angle)) 27 | _y = int(y + length * math.sin(angle)) 28 | img = cv2.arrowedLine(img, (x, y), (_x, _y), (255, 255, 255), 3) 29 | 30 | return img 31 | 32 | 33 | def draw_circle(x, y, img): 34 | img = cv2.circle(img, (x, y), 10, (255, 255, 255), 3) 35 | return img 36 | 37 | 38 | def get_prediction_vis(predictions, color_heightmap, best_pix_ind, action): 39 | num_rotations = predictions.shape[0] 40 | angle = math.radians(best_pix_ind[0] * (360.0 / num_rotations)) 41 | prediction_vis = predictions[best_pix_ind[0], :, :].copy() 42 | cv2.normalize(prediction_vis, prediction_vis, 0, 255, norm_type=cv2.NORM_MINMAX) 43 | prediction_vis = cv2.applyColorMap(prediction_vis.astype(np.uint8), cv2.COLORMAP_HOT) 44 | prediction_vis = (0.5 * cv2.cvtColor(color_heightmap, cv2.COLOR_RGB2BGR) + 0.5 * prediction_vis).astype(np.uint8) 45 | if action == 'grasp': 46 | prediction_vis = draw_angled_rec(int(best_pix_ind[2]), int(best_pix_ind[1]), angle, prediction_vis) 47 | elif action == 'place': 48 | prediction_vis = draw_circle(int(best_pix_ind[2]), int(best_pix_ind[1]), prediction_vis) 49 | elif action == 'push': 50 | prediction_vis = draw_arrow(int(best_pix_ind[2]), int(best_pix_ind[1]), angle, prediction_vis) 51 | return prediction_vis 52 | --------------------------------------------------------------------------------