├── .gitignore ├── LICENSE ├── MANIFEST.in ├── README.md ├── requirements.txt ├── safemotions ├── __init__.py ├── description │ ├── meshes │ │ ├── iiwa7 │ │ │ ├── collision │ │ │ │ ├── base.stl │ │ │ │ ├── link1.stl │ │ │ │ ├── link2.stl │ │ │ │ ├── link3.stl │ │ │ │ ├── link4.stl │ │ │ │ ├── link5.stl │ │ │ │ ├── link6.stl │ │ │ │ └── link7.stl │ │ │ └── visual │ │ │ │ ├── base.dae │ │ │ │ ├── link1.dae │ │ │ │ ├── link2.dae │ │ │ │ ├── link3.dae │ │ │ │ ├── link4.dae │ │ │ │ ├── link5.dae │ │ │ │ ├── link6.dae │ │ │ │ └── link7.dae │ │ ├── misc │ │ │ └── visual │ │ │ │ ├── base_adapter.dae │ │ │ │ └── table.dae │ │ └── obstacles │ │ │ ├── collision │ │ │ ├── monitor_no_pivot.obj │ │ │ └── monitor_pivot.obj │ │ │ └── visual │ │ │ ├── monitor_no_pivot.obj │ │ │ └── monitor_pivot.obj │ └── urdf │ │ ├── obstacles │ │ ├── monitor_no_pivot.urdf │ │ └── monitor_pivot.urdf │ │ ├── one_robot.urdf │ │ ├── three_robots.urdf │ │ └── two_robots.urdf ├── envs │ ├── __init__.py │ ├── decorators │ │ ├── __init__.py │ │ ├── actions.py │ │ ├── observations.py │ │ ├── rewards.py │ │ └── video_recording.py │ ├── safe_motions_base.py │ └── safe_motions_env.py ├── evaluate.py ├── model │ ├── __init__.py │ ├── custom_action_dist.py │ ├── fcnet_v2_last_layer_activation.py │ └── keras_fcnet_last_layer_activation.py ├── random_agent.py ├── robot_scene │ ├── __init__.py │ ├── collision_torque_limit_prevention.py │ ├── real_robot_scene.py │ ├── robot_scene_base.py │ └── simulated_robot_scene.py ├── train.py ├── trained_networks │ ├── humanoid │ │ ├── armar6 │ │ │ └── collision │ │ │ │ ├── alternating │ │ │ │ ├── checkpoint │ │ │ │ │ ├── .is_checkpoint │ │ │ │ │ ├── checkpoint │ │ │ │ │ └── checkpoint.tune_metadata │ │ │ │ └── params.json │ │ │ │ ├── simultaneous │ │ │ │ ├── checkpoint │ │ │ │ │ ├── .is_checkpoint │ │ │ │ │ ├── checkpoint │ │ │ │ │ └── checkpoint.tune_metadata │ │ │ │ └── params.json │ │ │ │ └── single │ │ │ │ ├── checkpoint │ │ │ │ ├── .is_checkpoint │ │ │ │ ├── checkpoint │ │ │ │ └── checkpoint.tune_metadata │ │ │ │ └── params.json │ │ └── armar6_x4 │ │ │ └── collision │ │ │ ├── alternating │ │ │ ├── checkpoint │ │ │ │ ├── .is_checkpoint │ │ │ │ ├── checkpoint │ │ │ │ └── checkpoint.tune_metadata │ │ │ └── params.json │ │ │ ├── simultaneous │ │ │ ├── checkpoint │ │ │ │ ├── .is_checkpoint │ │ │ │ ├── checkpoint │ │ │ │ └── checkpoint.tune_metadata │ │ │ └── params.json │ │ │ └── single │ │ │ ├── checkpoint │ │ │ ├── .is_checkpoint │ │ │ ├── checkpoint │ │ │ └── checkpoint.tune_metadata │ │ │ └── params.json │ └── industrial │ │ ├── one_robot │ │ └── collision │ │ │ ├── checkpoint │ │ │ ├── .is_checkpoint │ │ │ ├── checkpoint │ │ │ └── checkpoint.tune_metadata │ │ │ └── params.json │ │ ├── three_robots │ │ └── collision │ │ │ └── alternating │ │ │ ├── checkpoint │ │ │ ├── .is_checkpoint │ │ │ ├── checkpoint │ │ │ └── checkpoint.tune_metadata │ │ │ └── params.json │ │ └── two_robots │ │ └── collision │ │ ├── alternating │ │ ├── checkpoint │ │ │ ├── .is_checkpoint │ │ │ ├── checkpoint │ │ │ └── checkpoint.tune_metadata │ │ └── params.json │ │ └── simultaneous │ │ ├── checkpoint │ │ ├── .is_checkpoint │ │ ├── checkpoint │ │ └── checkpoint.tune_metadata │ │ └── params.json └── utils │ ├── __init__.py │ ├── braking_trajectory_generator.py │ ├── trajectory_manager.py │ └── trajectory_plotter.py └── setup.py /.gitignore: -------------------------------------------------------------------------------- 1 | __pycache__/ 2 | .idea/ 3 | dist/ 4 | build/ 5 | safemotions.egg-info/ 6 | 7 | *.pyc 8 | *.wrl 9 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Jonas C. Kiemel 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | 23 | 24 | -------------------------------------------------------------------------------- 25 | 26 | 27 | Code in safemotions/model is adapted from 28 | https://github.com/ray-project/ray/ 29 | 30 | # Copyright 2021 Ray Team 31 | # Licensed under the Apache License, Version 2.0 (the "License"); 32 | # you may not use this file except in compliance with the License. 33 | # You may obtain a copy of the License at 34 | 35 | # https://www.apache.org/licenses/LICENSE-2.0 36 | 37 | # Unless required by applicable law or agreed to in writing, software 38 | # distributed under the License is distributed on an "AS IS" BASIS, 39 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 40 | # See the License for the specific language governing permissions and 41 | # limitations under the License. 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- 46 | 47 | 48 | The robots of the ARMAR family are designed and developed by the 49 | Chair of High Performance Humanoid Technologies (H²T) at the Karlsruhe 50 | Institute of Technology (KIT) and licensed under the Creative 51 | Commons Attribution-NonCommercial 4.0 International (CC BY-NC 4.0) license. 52 | 53 | # https://creativecommons.org/licenses/by-nc/4.0 54 | -------------------------------------------------------------------------------- /MANIFEST.in: -------------------------------------------------------------------------------- 1 | graft safemotions/description 2 | graft safemotions/trained_networks 3 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Learning Collision-free and Torque-limited Robot Trajectories based on Alternative Safe Behaviors 2 | [![IEEE Humanoids 2022](https://img.shields.io/badge/Humanoids-2022-%3C%3E)](https://www.humanoids2022.org/) 3 | [![arXiv](https://img.shields.io/badge/arXiv-2103.03793-B31B1B)](https://arxiv.org/abs/2103.03793) 4 | [![PyPI version](https://img.shields.io/pypi/v/safemotions)](https://pypi.python.org/pypi/safemotions) 5 | [![PyPI license](https://img.shields.io/pypi/l/safemotions)](https://pypi.python.org/pypi/safemotions) 6 | [![GitHub issues](https://img.shields.io/github/issues/translearn/safemotions)](https://github.com/translearn/safemotions/issues/)
7 | This repository provides the code to learn torque-limited and collision-free robot trajectories without exceeding limits on the position, velocity, acceleration and jerk of each robot joint. 8 | 9 | ![safemotions_picture](https://user-images.githubusercontent.com/51738372/116555683-f32d7680-a8fc-11eb-8cce-b01931c6ba58.png) 10 | 11 | ## Installation 12 | 13 | To use the code, clone the repository with: 14 | 15 | git clone https://github.com/translearn/safeMotions.git 16 | 17 | The required dependencies can be installed by running: 18 | 19 | pip install -r requirements.txt 20 | 21 | ## Trajectory generation   [![Open In Colab](https://colab.research.google.com/assets/colab-badge.svg)](https://colab.research.google.com/github/translearn/notebooks/blob/main/safemotions_random_agent_demo.ipynb) 22 | 23 | To generate a random trajectory with a single robot run 24 | 25 | python safemotions/random_agent.py --use_gui --check_braking_trajectory_collisions --check_braking_trajectory_torque_limits --torque_limit_factor=0.6 --plot_trajectory 26 | 27 | For a demonstration scenario with two robots run 28 | 29 | python safemotions/random_agent.py --use_gui --check_braking_trajectory_collisions --robot_scene=1 30 | 31 | Collision-free trajectories for three robots can be generated by running 32 | 33 | python safemotions/random_agent.py --use_gui --check_braking_trajectory_collisions --robot_scene=2 34 | 35 | 36 | ## Pretrained networks   [![Open In Colab](https://colab.research.google.com/assets/colab-badge.svg)](https://colab.research.google.com/github/translearn/notebooks/blob/main/safemotions_trained_networks_demo.ipynb) 37 | 38 | Various pretrained networks for reaching randomly sampled target points are provided. \ 39 | Make sure you use ray==1.4.1 to open the pretrained networks. 40 | 41 | ### Industrial robots 42 | To generate and plot trajectories for a reaching task with a single industrial robot run 43 | 44 | ```bash 45 | python safemotions/evaluate.py --use_gui --plot_trajectory --plot_actual_torques --checkpoint=industrial/one_robot/collision 46 | ``` 47 | Trajectories for two and three industrial robots with alternating target points can be generated by running 48 | 49 | ```bash 50 | python safemotions/evaluate.py --use_gui --checkpoint=industrial/two_robots/collision/alternating 51 | ``` 52 | and 53 | ```bash 54 | python safemotions/evaluate.py --use_gui --checkpoint=industrial/three_robots/collision/alternating 55 | ``` 56 | 57 | ### Humanoid robots 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 77 | 79 | 80 | 81 | 82 | 84 | 86 | 87 | 88 | 89 | 91 | 93 | 94 | 95 |
ARMAR 6ARMAR 6x4
Alternating target points --checkpoint=humanoid/armar6/collision/alternating 76 | --checkpoint=humanoid/armar6_x4/collision/alternating 78 |
Simultaneous target points --checkpoint=humanoid/armar6/collision/simultaneous 83 | --checkpoint=humanoid/armar6_x4/collision/simultaneous 85 |
Single target point --checkpoint=humanoid/armar6/collision/single 90 | --checkpoint=humanoid/armar6_x4/collision/single 92 |
96 | 97 | 98 | 99 | ## Training 100 | 101 | Networks can also be trained from scratch. For instance, a reaching task with a single robot can be learned by running 102 | ```bash 103 | python safemotions/train.py --logdir=safemotions_training --name=industrial_one_robot_collision --robot_scene=0 --online_trajectory_time_step=0.1 --hidden_layer_activation=swish --online_trajectory_duration=8.0 --obstacle_scene=3 --use_target_points --target_point_sequence=0 --target_point_cartesian_range_scene=0 --target_link_offset="[0, 0, 0.126]" --target_point_radius=0.065 --obs_add_target_point_pos --obs_add_target_point_relative_pos --check_braking_trajectory_collisions --closest_point_safety_distance=0.01 --acc_limit_factor_braking=1.0 --jerk_limit_factor_braking=1.0 --punish_action --action_punishment_min_threshold=0.95 --action_max_punishment=0.4 --target_point_reached_reward_bonus=5 --pos_limit_factor=1.0 --vel_limit_factor=1.0 --acc_limit_factor=1.0 --jerk_limit_factor=1.0 --torque_limit_factor=1.0 --punish_braking_trajectory_min_distance --braking_trajectory_min_distance_max_threshold=0.05 --braking_trajectory_max_punishment=0.5 --last_layer_activation=tanh --solver_iterations=50 --normalize_reward_to_initial_target_point_distance --collision_check_time=0.033 --iterations_per_checkpoint=50 --time=200 104 | ``` 105 | 106 | ## Publication 107 | The corresponding publication is available at [https://arxiv.org/abs/2103.03793](https://arxiv.org/abs/2103.03793). 108 | 109 | [![Video](https://user-images.githubusercontent.com/51738372/196687365-0adb05c7-88ec-423f-86bd-f0600e62ea82.png)](https://youtu.be/U2OWsQrt-40) 110 | 111 | 112 | ## Disclaimer 113 | 114 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 115 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | aiohttp==3.7.4.post0 2 | aiohttp-cors==0.7.0 3 | aioredis==1.3.1 4 | redis==3.5.3 5 | prometheus-client==0.11.0 6 | armar==1.1.0 7 | gym==0.18.3 8 | klimits==1.1.1 9 | matplotlib==3.3.4 10 | numpy==1.19.5 11 | pybullet==3.2.2 12 | ray[default]==1.4.1 13 | ray[rllib]==1.4.1 14 | tensorflow==2.5.0 15 | -------------------------------------------------------------------------------- /safemotions/__init__.py: -------------------------------------------------------------------------------- 1 | __version__ = '1.0.1' 2 | VERSION = __version__ 3 | __version_info__ = tuple([int(sub_version) for sub_version in __version__.split('.')]) -------------------------------------------------------------------------------- /safemotions/description/meshes/iiwa7/collision/base.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/translearn/safeMotions/36a97fb0f71fa7f81543dd94cdb58578b6a1022d/safemotions/description/meshes/iiwa7/collision/base.stl -------------------------------------------------------------------------------- /safemotions/description/meshes/iiwa7/collision/link1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/translearn/safeMotions/36a97fb0f71fa7f81543dd94cdb58578b6a1022d/safemotions/description/meshes/iiwa7/collision/link1.stl -------------------------------------------------------------------------------- /safemotions/description/meshes/iiwa7/collision/link2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/translearn/safeMotions/36a97fb0f71fa7f81543dd94cdb58578b6a1022d/safemotions/description/meshes/iiwa7/collision/link2.stl -------------------------------------------------------------------------------- /safemotions/description/meshes/iiwa7/collision/link3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/translearn/safeMotions/36a97fb0f71fa7f81543dd94cdb58578b6a1022d/safemotions/description/meshes/iiwa7/collision/link3.stl -------------------------------------------------------------------------------- /safemotions/description/meshes/iiwa7/collision/link4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/translearn/safeMotions/36a97fb0f71fa7f81543dd94cdb58578b6a1022d/safemotions/description/meshes/iiwa7/collision/link4.stl -------------------------------------------------------------------------------- /safemotions/description/meshes/iiwa7/collision/link5.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/translearn/safeMotions/36a97fb0f71fa7f81543dd94cdb58578b6a1022d/safemotions/description/meshes/iiwa7/collision/link5.stl -------------------------------------------------------------------------------- /safemotions/description/meshes/iiwa7/collision/link6.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/translearn/safeMotions/36a97fb0f71fa7f81543dd94cdb58578b6a1022d/safemotions/description/meshes/iiwa7/collision/link6.stl -------------------------------------------------------------------------------- /safemotions/description/meshes/iiwa7/collision/link7.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/translearn/safeMotions/36a97fb0f71fa7f81543dd94cdb58578b6a1022d/safemotions/description/meshes/iiwa7/collision/link7.stl -------------------------------------------------------------------------------- /safemotions/description/meshes/obstacles/collision/monitor_no_pivot.obj: -------------------------------------------------------------------------------- 1 | o convex_0 2 | v -0.138734 -0.087308 -0.333349 3 | v 0.138734 0.130150 -0.325852 4 | v 0.141250 0.127638 -0.325852 5 | v 0.136245 0.125148 -0.338349 6 | v -0.138734 0.130150 -0.333349 7 | v 0.138734 -0.087308 -0.325852 8 | v -0.141250 0.127638 -0.325850 9 | v 0.136245 -0.082306 -0.338349 10 | v -0.136217 0.125148 -0.338350 11 | v -0.138734 -0.087308 -0.325850 12 | v 0.041230 0.110141 -0.323350 13 | v -0.136217 -0.082306 -0.338350 14 | v -0.041230 0.067654 -0.323350 15 | v 0.141250 -0.084796 -0.333349 16 | v -0.141250 -0.084796 -0.333349 17 | v 0.138734 0.130150 -0.333349 18 | v -0.036253 0.112653 -0.323350 19 | v 0.041230 0.067654 -0.323350 20 | v 0.138734 -0.087308 -0.333349 21 | v -0.138734 0.130150 -0.325850 22 | v -0.141250 0.127638 -0.333349 23 | v 0.141250 0.127638 -0.333349 24 | v -0.141250 -0.084796 -0.325850 25 | v 0.141250 -0.084796 -0.325852 26 | f 18 6 24 27 | f 4 8 9 28 | f 1 6 10 29 | f 3 2 11 30 | f 9 8 12 31 | f 8 4 14 32 | f 1 10 15 33 | f 12 1 15 34 | f 9 12 15 35 | f 2 3 16 36 | f 5 2 16 37 | f 9 5 16 38 | f 4 9 16 39 | f 11 2 17 40 | f 7 13 17 41 | f 13 11 17 42 | f 10 6 18 43 | f 3 11 18 44 | f 13 10 18 45 | f 11 13 18 46 | f 6 1 19 47 | f 1 12 19 48 | f 12 8 19 49 | f 14 6 19 50 | f 8 14 19 51 | f 2 5 20 52 | f 5 7 20 53 | f 17 2 20 54 | f 7 17 20 55 | f 7 5 21 56 | f 5 9 21 57 | f 15 7 21 58 | f 9 15 21 59 | f 3 14 22 60 | f 14 4 22 61 | f 16 3 22 62 | f 4 16 22 63 | f 10 13 23 64 | f 13 7 23 65 | f 7 15 23 66 | f 15 10 23 67 | f 14 3 24 68 | f 6 14 24 69 | f 3 18 24 70 | o convex_1 71 | v -0.041250 0.110165 -0.323350 72 | v 0.041250 0.110165 0.044150 73 | v 0.036227 0.112667 0.044150 74 | v -0.041250 0.067675 0.044150 75 | v 0.041250 0.067675 -0.323350 76 | v 0.041250 0.110165 -0.323350 77 | v 0.041250 0.067675 0.044150 78 | v -0.041250 0.067675 -0.323350 79 | v -0.041250 0.110165 0.044150 80 | v -0.036227 0.112667 -0.323350 81 | v -0.036227 0.112667 0.044150 82 | v 0.036227 0.112667 -0.323350 83 | f 34 27 36 84 | f 26 27 28 85 | f 26 29 30 86 | f 27 26 30 87 | f 29 25 30 88 | f 26 28 31 89 | f 28 29 31 90 | f 29 26 31 91 | f 28 25 32 92 | f 25 29 32 93 | f 29 28 32 94 | f 28 27 33 95 | f 25 28 33 96 | f 30 25 34 97 | f 25 33 34 98 | f 34 33 35 99 | f 33 27 35 100 | f 27 34 35 101 | f 27 30 36 102 | f 30 34 36 103 | o convex_2 104 | v 0.296231 -0.002332 -0.158345 105 | v -0.331250 -0.029826 -0.170850 106 | v -0.331250 -0.009831 -0.170850 107 | v -0.331250 -0.009831 0.211650 108 | v 0.328721 -0.032329 0.209141 109 | v 0.258684 0.005171 0.154140 110 | v 0.328721 -0.032329 -0.170850 111 | v -0.328721 -0.032329 0.209141 112 | v -0.258684 0.005171 -0.145839 113 | v -0.258684 0.005171 0.154140 114 | v 0.331250 -0.009831 0.211650 115 | v 0.258684 0.005171 -0.145839 116 | v -0.328721 -0.032329 -0.170850 117 | v 0.331250 -0.009831 -0.170850 118 | v 0.331250 -0.029826 0.211650 119 | v -0.321198 -0.007331 0.204124 120 | v -0.331250 -0.029826 0.211650 121 | v -0.321198 -0.007331 -0.168341 122 | v 0.321198 -0.007331 0.204124 123 | v 0.321198 -0.007331 -0.168341 124 | v 0.331250 -0.029826 -0.170850 125 | f 50 51 57 126 | f 39 38 40 127 | f 38 39 43 128 | f 43 41 44 129 | f 42 45 46 130 | f 42 37 48 131 | f 45 42 48 132 | f 38 43 49 133 | f 43 44 49 134 | f 44 38 49 135 | f 43 39 50 136 | f 41 43 51 137 | f 44 41 51 138 | f 47 40 51 139 | f 50 47 51 140 | f 39 40 52 141 | f 42 46 52 142 | f 46 45 52 143 | f 40 47 52 144 | f 40 38 53 145 | f 38 44 53 146 | f 44 51 53 147 | f 51 40 53 148 | f 45 48 54 149 | f 50 39 54 150 | f 39 52 54 151 | f 52 45 54 152 | f 37 42 55 153 | f 47 50 55 154 | f 42 52 55 155 | f 52 47 55 156 | f 48 37 56 157 | f 54 48 56 158 | f 50 54 56 159 | f 37 55 56 160 | f 55 50 56 161 | f 43 50 57 162 | f 51 43 57 163 | o convex_3 164 | v 0.058720 0.032671 0.079121 165 | v -0.246246 0.005174 -0.140850 166 | v -0.056209 0.005171 -0.118337 167 | v -0.246246 0.005174 0.144150 168 | v 0.246250 0.005174 0.144150 169 | v 0.226244 0.022668 -0.128324 170 | v -0.226239 0.027667 -0.078332 171 | v -0.228746 0.027667 0.134135 172 | v 0.246250 0.005174 -0.140850 173 | v 0.228702 0.027667 0.134135 174 | v 0.058720 0.032671 -0.038299 175 | v -0.226239 0.022668 -0.128324 176 | v 0.228702 0.027667 -0.065834 177 | v -0.058715 0.032671 -0.038299 178 | v -0.058715 0.032671 0.079121 179 | v -0.246246 0.007674 -0.140850 180 | v 0.246250 0.007674 0.144150 181 | v 0.246250 0.007674 -0.140850 182 | v -0.246246 0.007674 0.144150 183 | v -0.228746 0.027667 -0.065834 184 | v 0.231209 0.017672 -0.133346 185 | v 0.226244 0.027667 -0.078332 186 | v -0.231205 0.017672 -0.133346 187 | f 78 73 80 188 | f 59 60 61 189 | f 61 60 62 190 | f 60 59 66 191 | f 62 60 66 192 | f 58 65 67 193 | f 58 67 68 194 | f 68 63 69 195 | f 68 67 70 196 | f 58 68 71 197 | f 69 64 71 198 | f 68 69 71 199 | f 65 58 72 200 | f 58 71 72 201 | f 71 65 72 202 | f 59 61 73 203 | f 66 59 73 204 | f 61 62 74 205 | f 62 66 74 206 | f 67 65 74 207 | f 70 67 74 208 | f 70 74 75 209 | f 63 70 75 210 | f 66 73 75 211 | f 74 66 75 212 | f 73 61 76 213 | f 65 73 76 214 | f 61 74 76 215 | f 74 65 76 216 | f 64 69 77 217 | f 65 71 77 218 | f 71 64 77 219 | f 73 65 77 220 | f 69 73 77 221 | f 69 63 78 222 | f 63 75 78 223 | f 75 73 78 224 | f 63 68 79 225 | f 70 63 79 226 | f 68 70 79 227 | f 73 69 80 228 | f 69 78 80 229 | o convex_4 230 | v 0.041250 0.065170 0.031649 231 | v -0.038747 0.060165 -0.005850 232 | v -0.041250 0.032671 -0.000847 233 | v -0.041250 0.052669 0.041650 234 | v 0.041250 0.032671 -0.000847 235 | v 0.041250 0.032671 0.041650 236 | v 0.038747 0.067671 -0.005850 237 | v -0.038747 0.067671 0.036642 238 | v -0.041250 0.032671 0.041650 239 | v 0.038747 0.067671 0.036642 240 | v -0.038747 0.067671 -0.005850 241 | v 0.041250 0.052669 0.041650 242 | v -0.041250 0.067671 0.014152 243 | f 88 91 93 244 | f 83 82 85 245 | f 83 85 86 246 | f 85 81 86 247 | f 81 85 87 248 | f 85 82 87 249 | f 84 83 89 250 | f 83 86 89 251 | f 86 84 89 252 | f 81 87 90 253 | f 88 84 90 254 | f 87 88 90 255 | f 82 83 91 256 | f 87 82 91 257 | f 88 87 91 258 | f 84 86 92 259 | f 86 81 92 260 | f 90 84 92 261 | f 81 90 92 262 | f 83 84 93 263 | f 84 88 93 264 | f 91 83 93 265 | -------------------------------------------------------------------------------- /safemotions/description/meshes/obstacles/collision/monitor_pivot.obj: -------------------------------------------------------------------------------- 1 | o convex_0 2 | v -0.139141 -0.087298 -0.325940 3 | v 0.138643 0.130944 -0.325938 4 | v 0.141134 0.128444 -0.325938 5 | v 0.136152 0.123488 -0.338340 6 | v -0.139141 0.130944 -0.333378 7 | v 0.136152 -0.082320 -0.338340 8 | v -0.134187 -0.082320 -0.338340 9 | v 0.138643 -0.087298 -0.325938 10 | v -0.042404 0.111098 -0.323457 11 | v -0.134187 0.123488 -0.338340 12 | v -0.139141 0.130944 -0.325940 13 | v 0.041906 0.068928 -0.323457 14 | v -0.139141 -0.087298 -0.333378 15 | v 0.141134 -0.084798 -0.333378 16 | v 0.138643 0.130944 -0.333378 17 | v -0.141632 0.128444 -0.333378 18 | v -0.141632 -0.084798 -0.325940 19 | v -0.042404 0.068928 -0.323457 20 | v 0.041906 0.111098 -0.323457 21 | v 0.138643 -0.087298 -0.333378 22 | v 0.141134 0.128444 -0.333378 23 | v -0.141632 -0.084798 -0.333378 24 | v 0.141134 -0.084798 -0.325938 25 | v -0.141632 0.128444 -0.325940 26 | f 17 9 24 27 | f 4 6 7 28 | f 5 4 10 29 | f 4 7 10 30 | f 2 5 11 31 | f 9 2 11 32 | f 7 6 13 33 | f 8 1 13 34 | f 6 4 14 35 | f 2 3 15 36 | f 4 5 15 37 | f 5 2 15 38 | f 5 10 16 39 | f 10 7 16 40 | f 11 5 16 41 | f 13 1 17 42 | f 1 8 18 43 | f 8 12 18 44 | f 12 9 18 45 | f 17 1 18 46 | f 9 17 18 47 | f 3 2 19 48 | f 2 9 19 49 | f 12 3 19 50 | f 9 12 19 51 | f 13 6 20 52 | f 8 13 20 53 | f 14 8 20 54 | f 6 14 20 55 | f 3 14 21 56 | f 14 4 21 57 | f 15 3 21 58 | f 4 15 21 59 | f 7 13 22 60 | f 16 7 22 61 | f 13 17 22 62 | f 17 16 22 63 | f 3 12 23 64 | f 12 8 23 65 | f 8 14 23 66 | f 14 3 23 67 | f 9 11 24 68 | f 11 16 24 69 | f 16 17 24 70 | o convex_1 71 | v -0.166425 0.004456 -0.241558 72 | v 0.190707 -0.010426 0.351240 73 | v 0.185771 -0.007945 0.346248 74 | v -0.191216 -0.030268 0.351240 75 | v 0.190707 -0.030268 -0.311055 76 | v -0.191216 -0.030268 -0.311055 77 | v -0.161452 0.006940 0.269361 78 | v 0.126243 0.006940 -0.229111 79 | v 0.190707 -0.030268 0.351240 80 | v 0.126243 0.006940 0.269361 81 | v 0.190707 -0.010426 -0.311055 82 | v -0.191216 -0.010429 0.351240 83 | v -0.188748 -0.007945 -0.306063 84 | v -0.161452 0.006940 -0.229111 85 | v 0.185771 -0.007945 -0.306063 86 | v -0.191216 -0.010429 -0.311055 87 | v -0.188748 -0.007945 0.346248 88 | v -0.173866 -0.000504 -0.268786 89 | v -0.171398 0.001976 0.294126 90 | v 0.165916 -0.002985 0.321419 91 | v 0.156007 -0.000504 -0.268786 92 | v -0.178839 -0.002985 0.321419 93 | v -0.171398 0.001976 -0.253940 94 | f 43 38 47 95 | f 29 28 30 96 | f 26 28 33 97 | f 28 29 33 98 | f 29 26 33 99 | f 27 32 34 100 | f 32 31 34 101 | f 27 26 35 102 | f 26 29 35 103 | f 29 30 35 104 | f 28 26 36 105 | f 30 28 36 106 | f 31 32 38 107 | f 32 27 39 108 | f 27 35 39 109 | f 35 37 39 110 | f 35 30 40 111 | f 30 36 40 112 | f 36 37 40 113 | f 37 35 40 114 | f 26 27 41 115 | f 36 26 41 116 | f 37 36 41 117 | f 37 25 42 118 | f 25 38 42 119 | f 38 32 42 120 | f 39 37 42 121 | f 31 38 43 122 | f 41 31 43 123 | f 37 41 43 124 | f 27 34 44 125 | f 34 31 44 126 | f 41 27 44 127 | f 32 39 45 128 | f 42 32 45 129 | f 39 42 45 130 | f 31 41 46 131 | f 44 31 46 132 | f 41 44 46 133 | f 25 37 47 134 | f 38 25 47 135 | f 37 43 47 136 | o convex_2 137 | v -0.059774 0.031745 -0.038168 138 | v 0.123771 0.009424 0.264423 139 | v 0.116325 0.024301 0.251986 140 | v 0.123771 0.006940 -0.224237 141 | v -0.159007 0.006940 0.264423 142 | v -0.159007 0.006940 -0.224237 143 | v 0.113834 0.029264 -0.206826 144 | v -0.131714 0.024303 0.246964 145 | v -0.149070 0.021822 -0.206826 146 | v 0.113834 0.029264 0.246964 147 | v 0.123771 0.006940 0.264423 148 | v -0.082112 0.029264 0.246964 149 | v -0.082112 0.029264 -0.206826 150 | v 0.059277 0.031745 0.078305 151 | v -0.149070 0.021822 0.246964 152 | v -0.159007 0.009424 -0.224237 153 | v -0.059774 0.031745 0.078305 154 | v 0.059277 0.031745 -0.038168 155 | v -0.159007 0.009424 0.264423 156 | v 0.123771 0.009424 -0.224237 157 | v -0.131714 0.024303 -0.206826 158 | v -0.154024 0.016861 -0.214288 159 | v -0.099467 0.024301 -0.211800 160 | v -0.154024 0.016861 0.254426 161 | f 69 66 71 162 | f 51 52 53 163 | f 50 54 57 164 | f 51 49 58 165 | f 52 51 58 166 | f 49 52 58 167 | f 50 57 59 168 | f 48 54 60 169 | f 57 54 61 170 | f 59 57 61 171 | f 55 56 62 172 | f 51 53 63 173 | f 53 52 63 174 | f 48 55 64 175 | f 55 59 64 176 | f 61 48 64 177 | f 59 61 64 178 | f 54 48 65 179 | f 48 61 65 180 | f 61 54 65 181 | f 49 50 66 182 | f 52 49 66 183 | f 50 59 66 184 | f 59 55 66 185 | f 63 52 66 186 | f 50 49 67 187 | f 49 51 67 188 | f 54 50 67 189 | f 51 63 67 190 | f 55 48 68 191 | f 56 55 68 192 | f 48 60 68 193 | f 62 56 69 194 | f 63 66 69 195 | f 56 68 69 196 | f 60 54 70 197 | f 54 67 70 198 | f 67 63 70 199 | f 68 60 70 200 | f 63 69 70 201 | f 69 68 70 202 | f 55 62 71 203 | f 66 55 71 204 | f 62 69 71 205 | o convex_3 206 | v -0.017612 0.061506 -0.020836 207 | v 0.039434 0.051583 0.041174 208 | v 0.039434 0.036711 0.041174 209 | v -0.039942 0.036711 0.041174 210 | v 0.022068 0.031745 -0.020836 211 | v -0.010169 0.066472 0.061021 212 | v 0.039434 0.066472 -0.005948 213 | v 0.022068 0.031745 0.061021 214 | v -0.039942 0.031745 0.001496 215 | v -0.039942 0.066472 -0.005948 216 | v -0.020091 0.031745 0.061021 217 | v -0.039942 0.063987 0.038690 218 | v 0.039434 0.066472 0.036214 219 | v -0.020091 0.031745 -0.020836 220 | v 0.039434 0.031745 0.001496 221 | v 0.017103 0.061506 0.061021 222 | v 0.009668 0.066472 -0.020836 223 | v -0.020091 0.056550 0.061021 224 | v 0.022068 0.051583 -0.020836 225 | f 88 78 90 226 | f 73 74 78 227 | f 74 73 79 228 | f 76 79 80 229 | f 77 78 81 230 | f 80 75 81 231 | f 79 77 82 232 | f 75 80 82 233 | f 80 79 82 234 | f 81 75 83 235 | f 77 81 83 236 | f 73 78 84 237 | f 78 77 84 238 | f 72 76 85 239 | f 76 80 85 240 | f 81 72 85 241 | f 80 81 85 242 | f 78 74 86 243 | f 76 78 86 244 | f 74 79 86 245 | f 79 76 86 246 | f 79 73 87 247 | f 77 79 87 248 | f 73 84 87 249 | f 84 77 87 250 | f 76 72 88 251 | f 72 81 88 252 | f 81 78 88 253 | f 75 82 89 254 | f 82 77 89 255 | f 83 75 89 256 | f 77 83 89 257 | f 78 76 90 258 | f 76 88 90 259 | o convex_4 260 | v -0.039938 0.066472 -0.005958 261 | v 0.041914 0.111120 0.046138 262 | v 0.012145 0.113602 0.043642 263 | v 0.041914 0.111120 -0.323457 264 | v -0.042423 0.111120 -0.323457 265 | v 0.041914 0.068954 -0.323457 266 | v 0.041914 0.068954 0.046138 267 | v -0.042423 0.068954 -0.323457 268 | v -0.042423 0.111120 0.043642 269 | v -0.042423 0.068954 0.043642 270 | v 0.014630 0.066472 -0.323457 271 | v -0.012654 0.113602 -0.323457 272 | v 0.039429 0.066472 0.036189 273 | v -0.010177 0.113602 0.046138 274 | v -0.015139 0.066472 -0.323457 275 | v 0.012145 0.113602 -0.323457 276 | v -0.012654 0.066472 0.046138 277 | v -0.039938 0.111120 0.046138 278 | f 100 107 108 279 | f 93 92 94 280 | f 94 92 96 281 | f 95 94 96 282 | f 96 92 97 283 | f 95 96 98 284 | f 95 98 99 285 | f 99 98 100 286 | f 98 91 100 287 | f 98 96 101 288 | f 94 95 102 289 | f 95 99 102 290 | f 96 97 103 291 | f 91 101 103 292 | f 101 96 103 293 | f 92 93 104 294 | f 97 92 104 295 | f 93 102 104 296 | f 102 99 104 297 | f 91 98 105 298 | f 101 91 105 299 | f 98 101 105 300 | f 93 94 106 301 | f 102 93 106 302 | f 94 102 106 303 | f 100 91 107 304 | f 103 97 107 305 | f 91 103 107 306 | f 97 104 107 307 | f 107 104 108 308 | f 99 100 108 309 | f 104 99 108 310 | -------------------------------------------------------------------------------- /safemotions/description/meshes/obstacles/visual/monitor_no_pivot.obj: -------------------------------------------------------------------------------- 1 | o convex_0 2 | v -0.138734 -0.087308 -0.333349 3 | v 0.138734 0.130150 -0.325852 4 | v 0.141250 0.127638 -0.325852 5 | v 0.136245 0.125148 -0.338349 6 | v -0.138734 0.130150 -0.333349 7 | v 0.138734 -0.087308 -0.325852 8 | v -0.141250 0.127638 -0.325850 9 | v 0.136245 -0.082306 -0.338349 10 | v -0.136217 0.125148 -0.338350 11 | v -0.138734 -0.087308 -0.325850 12 | v 0.041230 0.110141 -0.323350 13 | v -0.136217 -0.082306 -0.338350 14 | v -0.041230 0.067654 -0.323350 15 | v 0.141250 -0.084796 -0.333349 16 | v -0.141250 -0.084796 -0.333349 17 | v 0.138734 0.130150 -0.333349 18 | v -0.036253 0.112653 -0.323350 19 | v 0.041230 0.067654 -0.323350 20 | v 0.138734 -0.087308 -0.333349 21 | v -0.138734 0.130150 -0.325850 22 | v -0.141250 0.127638 -0.333349 23 | v 0.141250 0.127638 -0.333349 24 | v -0.141250 -0.084796 -0.325850 25 | v 0.141250 -0.084796 -0.325852 26 | f 18 6 24 27 | f 4 8 9 28 | f 1 6 10 29 | f 3 2 11 30 | f 9 8 12 31 | f 8 4 14 32 | f 1 10 15 33 | f 12 1 15 34 | f 9 12 15 35 | f 2 3 16 36 | f 5 2 16 37 | f 9 5 16 38 | f 4 9 16 39 | f 11 2 17 40 | f 7 13 17 41 | f 13 11 17 42 | f 10 6 18 43 | f 3 11 18 44 | f 13 10 18 45 | f 11 13 18 46 | f 6 1 19 47 | f 1 12 19 48 | f 12 8 19 49 | f 14 6 19 50 | f 8 14 19 51 | f 2 5 20 52 | f 5 7 20 53 | f 17 2 20 54 | f 7 17 20 55 | f 7 5 21 56 | f 5 9 21 57 | f 15 7 21 58 | f 9 15 21 59 | f 3 14 22 60 | f 14 4 22 61 | f 16 3 22 62 | f 4 16 22 63 | f 10 13 23 64 | f 13 7 23 65 | f 7 15 23 66 | f 15 10 23 67 | f 14 3 24 68 | f 6 14 24 69 | f 3 18 24 70 | o convex_1 71 | v -0.041250 0.110165 -0.323350 72 | v 0.041250 0.110165 0.044150 73 | v 0.036227 0.112667 0.044150 74 | v -0.041250 0.067675 0.044150 75 | v 0.041250 0.067675 -0.323350 76 | v 0.041250 0.110165 -0.323350 77 | v 0.041250 0.067675 0.044150 78 | v -0.041250 0.067675 -0.323350 79 | v -0.041250 0.110165 0.044150 80 | v -0.036227 0.112667 -0.323350 81 | v -0.036227 0.112667 0.044150 82 | v 0.036227 0.112667 -0.323350 83 | f 34 27 36 84 | f 26 27 28 85 | f 26 29 30 86 | f 27 26 30 87 | f 29 25 30 88 | f 26 28 31 89 | f 28 29 31 90 | f 29 26 31 91 | f 28 25 32 92 | f 25 29 32 93 | f 29 28 32 94 | f 28 27 33 95 | f 25 28 33 96 | f 30 25 34 97 | f 25 33 34 98 | f 34 33 35 99 | f 33 27 35 100 | f 27 34 35 101 | f 27 30 36 102 | f 30 34 36 103 | o convex_2 104 | v 0.296231 -0.002332 -0.158345 105 | v -0.331250 -0.029826 -0.170850 106 | v -0.331250 -0.009831 -0.170850 107 | v -0.331250 -0.009831 0.211650 108 | v 0.328721 -0.032329 0.209141 109 | v 0.258684 0.005171 0.154140 110 | v 0.328721 -0.032329 -0.170850 111 | v -0.328721 -0.032329 0.209141 112 | v -0.258684 0.005171 -0.145839 113 | v -0.258684 0.005171 0.154140 114 | v 0.331250 -0.009831 0.211650 115 | v 0.258684 0.005171 -0.145839 116 | v -0.328721 -0.032329 -0.170850 117 | v 0.331250 -0.009831 -0.170850 118 | v 0.331250 -0.029826 0.211650 119 | v -0.321198 -0.007331 0.204124 120 | v -0.331250 -0.029826 0.211650 121 | v -0.321198 -0.007331 -0.168341 122 | v 0.321198 -0.007331 0.204124 123 | v 0.321198 -0.007331 -0.168341 124 | v 0.331250 -0.029826 -0.170850 125 | f 50 51 57 126 | f 39 38 40 127 | f 38 39 43 128 | f 43 41 44 129 | f 42 45 46 130 | f 42 37 48 131 | f 45 42 48 132 | f 38 43 49 133 | f 43 44 49 134 | f 44 38 49 135 | f 43 39 50 136 | f 41 43 51 137 | f 44 41 51 138 | f 47 40 51 139 | f 50 47 51 140 | f 39 40 52 141 | f 42 46 52 142 | f 46 45 52 143 | f 40 47 52 144 | f 40 38 53 145 | f 38 44 53 146 | f 44 51 53 147 | f 51 40 53 148 | f 45 48 54 149 | f 50 39 54 150 | f 39 52 54 151 | f 52 45 54 152 | f 37 42 55 153 | f 47 50 55 154 | f 42 52 55 155 | f 52 47 55 156 | f 48 37 56 157 | f 54 48 56 158 | f 50 54 56 159 | f 37 55 56 160 | f 55 50 56 161 | f 43 50 57 162 | f 51 43 57 163 | o convex_3 164 | v 0.058720 0.032671 0.079121 165 | v -0.246246 0.005174 -0.140850 166 | v -0.056209 0.005171 -0.118337 167 | v -0.246246 0.005174 0.144150 168 | v 0.246250 0.005174 0.144150 169 | v 0.226244 0.022668 -0.128324 170 | v -0.226239 0.027667 -0.078332 171 | v -0.228746 0.027667 0.134135 172 | v 0.246250 0.005174 -0.140850 173 | v 0.228702 0.027667 0.134135 174 | v 0.058720 0.032671 -0.038299 175 | v -0.226239 0.022668 -0.128324 176 | v 0.228702 0.027667 -0.065834 177 | v -0.058715 0.032671 -0.038299 178 | v -0.058715 0.032671 0.079121 179 | v -0.246246 0.007674 -0.140850 180 | v 0.246250 0.007674 0.144150 181 | v 0.246250 0.007674 -0.140850 182 | v -0.246246 0.007674 0.144150 183 | v -0.228746 0.027667 -0.065834 184 | v 0.231209 0.017672 -0.133346 185 | v 0.226244 0.027667 -0.078332 186 | v -0.231205 0.017672 -0.133346 187 | f 78 73 80 188 | f 59 60 61 189 | f 61 60 62 190 | f 60 59 66 191 | f 62 60 66 192 | f 58 65 67 193 | f 58 67 68 194 | f 68 63 69 195 | f 68 67 70 196 | f 58 68 71 197 | f 69 64 71 198 | f 68 69 71 199 | f 65 58 72 200 | f 58 71 72 201 | f 71 65 72 202 | f 59 61 73 203 | f 66 59 73 204 | f 61 62 74 205 | f 62 66 74 206 | f 67 65 74 207 | f 70 67 74 208 | f 70 74 75 209 | f 63 70 75 210 | f 66 73 75 211 | f 74 66 75 212 | f 73 61 76 213 | f 65 73 76 214 | f 61 74 76 215 | f 74 65 76 216 | f 64 69 77 217 | f 65 71 77 218 | f 71 64 77 219 | f 73 65 77 220 | f 69 73 77 221 | f 69 63 78 222 | f 63 75 78 223 | f 75 73 78 224 | f 63 68 79 225 | f 70 63 79 226 | f 68 70 79 227 | f 73 69 80 228 | f 69 78 80 229 | o convex_4 230 | v 0.041250 0.065170 0.031649 231 | v -0.038747 0.060165 -0.005850 232 | v -0.041250 0.032671 -0.000847 233 | v -0.041250 0.052669 0.041650 234 | v 0.041250 0.032671 -0.000847 235 | v 0.041250 0.032671 0.041650 236 | v 0.038747 0.067671 -0.005850 237 | v -0.038747 0.067671 0.036642 238 | v -0.041250 0.032671 0.041650 239 | v 0.038747 0.067671 0.036642 240 | v -0.038747 0.067671 -0.005850 241 | v 0.041250 0.052669 0.041650 242 | v -0.041250 0.067671 0.014152 243 | f 88 91 93 244 | f 83 82 85 245 | f 83 85 86 246 | f 85 81 86 247 | f 81 85 87 248 | f 85 82 87 249 | f 84 83 89 250 | f 83 86 89 251 | f 86 84 89 252 | f 81 87 90 253 | f 88 84 90 254 | f 87 88 90 255 | f 82 83 91 256 | f 87 82 91 257 | f 88 87 91 258 | f 84 86 92 259 | f 86 81 92 260 | f 90 84 92 261 | f 81 90 92 262 | f 83 84 93 263 | f 84 88 93 264 | f 91 83 93 265 | -------------------------------------------------------------------------------- /safemotions/description/meshes/obstacles/visual/monitor_pivot.obj: -------------------------------------------------------------------------------- 1 | o convex_0 2 | v -0.139141 -0.087298 -0.325940 3 | v 0.138643 0.130944 -0.325938 4 | v 0.141134 0.128444 -0.325938 5 | v 0.136152 0.123488 -0.338340 6 | v -0.139141 0.130944 -0.333378 7 | v 0.136152 -0.082320 -0.338340 8 | v -0.134187 -0.082320 -0.338340 9 | v 0.138643 -0.087298 -0.325938 10 | v -0.042404 0.111098 -0.323457 11 | v -0.134187 0.123488 -0.338340 12 | v -0.139141 0.130944 -0.325940 13 | v 0.041906 0.068928 -0.323457 14 | v -0.139141 -0.087298 -0.333378 15 | v 0.141134 -0.084798 -0.333378 16 | v 0.138643 0.130944 -0.333378 17 | v -0.141632 0.128444 -0.333378 18 | v -0.141632 -0.084798 -0.325940 19 | v -0.042404 0.068928 -0.323457 20 | v 0.041906 0.111098 -0.323457 21 | v 0.138643 -0.087298 -0.333378 22 | v 0.141134 0.128444 -0.333378 23 | v -0.141632 -0.084798 -0.333378 24 | v 0.141134 -0.084798 -0.325938 25 | v -0.141632 0.128444 -0.325940 26 | f 17 9 24 27 | f 4 6 7 28 | f 5 4 10 29 | f 4 7 10 30 | f 2 5 11 31 | f 9 2 11 32 | f 7 6 13 33 | f 8 1 13 34 | f 6 4 14 35 | f 2 3 15 36 | f 4 5 15 37 | f 5 2 15 38 | f 5 10 16 39 | f 10 7 16 40 | f 11 5 16 41 | f 13 1 17 42 | f 1 8 18 43 | f 8 12 18 44 | f 12 9 18 45 | f 17 1 18 46 | f 9 17 18 47 | f 3 2 19 48 | f 2 9 19 49 | f 12 3 19 50 | f 9 12 19 51 | f 13 6 20 52 | f 8 13 20 53 | f 14 8 20 54 | f 6 14 20 55 | f 3 14 21 56 | f 14 4 21 57 | f 15 3 21 58 | f 4 15 21 59 | f 7 13 22 60 | f 16 7 22 61 | f 13 17 22 62 | f 17 16 22 63 | f 3 12 23 64 | f 12 8 23 65 | f 8 14 23 66 | f 14 3 23 67 | f 9 11 24 68 | f 11 16 24 69 | f 16 17 24 70 | o convex_1 71 | v -0.166425 0.004456 -0.241558 72 | v 0.190707 -0.010426 0.351240 73 | v 0.185771 -0.007945 0.346248 74 | v -0.191216 -0.030268 0.351240 75 | v 0.190707 -0.030268 -0.311055 76 | v -0.191216 -0.030268 -0.311055 77 | v -0.161452 0.006940 0.269361 78 | v 0.126243 0.006940 -0.229111 79 | v 0.190707 -0.030268 0.351240 80 | v 0.126243 0.006940 0.269361 81 | v 0.190707 -0.010426 -0.311055 82 | v -0.191216 -0.010429 0.351240 83 | v -0.188748 -0.007945 -0.306063 84 | v -0.161452 0.006940 -0.229111 85 | v 0.185771 -0.007945 -0.306063 86 | v -0.191216 -0.010429 -0.311055 87 | v -0.188748 -0.007945 0.346248 88 | v -0.173866 -0.000504 -0.268786 89 | v -0.171398 0.001976 0.294126 90 | v 0.165916 -0.002985 0.321419 91 | v 0.156007 -0.000504 -0.268786 92 | v -0.178839 -0.002985 0.321419 93 | v -0.171398 0.001976 -0.253940 94 | f 43 38 47 95 | f 29 28 30 96 | f 26 28 33 97 | f 28 29 33 98 | f 29 26 33 99 | f 27 32 34 100 | f 32 31 34 101 | f 27 26 35 102 | f 26 29 35 103 | f 29 30 35 104 | f 28 26 36 105 | f 30 28 36 106 | f 31 32 38 107 | f 32 27 39 108 | f 27 35 39 109 | f 35 37 39 110 | f 35 30 40 111 | f 30 36 40 112 | f 36 37 40 113 | f 37 35 40 114 | f 26 27 41 115 | f 36 26 41 116 | f 37 36 41 117 | f 37 25 42 118 | f 25 38 42 119 | f 38 32 42 120 | f 39 37 42 121 | f 31 38 43 122 | f 41 31 43 123 | f 37 41 43 124 | f 27 34 44 125 | f 34 31 44 126 | f 41 27 44 127 | f 32 39 45 128 | f 42 32 45 129 | f 39 42 45 130 | f 31 41 46 131 | f 44 31 46 132 | f 41 44 46 133 | f 25 37 47 134 | f 38 25 47 135 | f 37 43 47 136 | o convex_2 137 | v -0.059774 0.031745 -0.038168 138 | v 0.123771 0.009424 0.264423 139 | v 0.116325 0.024301 0.251986 140 | v 0.123771 0.006940 -0.224237 141 | v -0.159007 0.006940 0.264423 142 | v -0.159007 0.006940 -0.224237 143 | v 0.113834 0.029264 -0.206826 144 | v -0.131714 0.024303 0.246964 145 | v -0.149070 0.021822 -0.206826 146 | v 0.113834 0.029264 0.246964 147 | v 0.123771 0.006940 0.264423 148 | v -0.082112 0.029264 0.246964 149 | v -0.082112 0.029264 -0.206826 150 | v 0.059277 0.031745 0.078305 151 | v -0.149070 0.021822 0.246964 152 | v -0.159007 0.009424 -0.224237 153 | v -0.059774 0.031745 0.078305 154 | v 0.059277 0.031745 -0.038168 155 | v -0.159007 0.009424 0.264423 156 | v 0.123771 0.009424 -0.224237 157 | v -0.131714 0.024303 -0.206826 158 | v -0.154024 0.016861 -0.214288 159 | v -0.099467 0.024301 -0.211800 160 | v -0.154024 0.016861 0.254426 161 | f 69 66 71 162 | f 51 52 53 163 | f 50 54 57 164 | f 51 49 58 165 | f 52 51 58 166 | f 49 52 58 167 | f 50 57 59 168 | f 48 54 60 169 | f 57 54 61 170 | f 59 57 61 171 | f 55 56 62 172 | f 51 53 63 173 | f 53 52 63 174 | f 48 55 64 175 | f 55 59 64 176 | f 61 48 64 177 | f 59 61 64 178 | f 54 48 65 179 | f 48 61 65 180 | f 61 54 65 181 | f 49 50 66 182 | f 52 49 66 183 | f 50 59 66 184 | f 59 55 66 185 | f 63 52 66 186 | f 50 49 67 187 | f 49 51 67 188 | f 54 50 67 189 | f 51 63 67 190 | f 55 48 68 191 | f 56 55 68 192 | f 48 60 68 193 | f 62 56 69 194 | f 63 66 69 195 | f 56 68 69 196 | f 60 54 70 197 | f 54 67 70 198 | f 67 63 70 199 | f 68 60 70 200 | f 63 69 70 201 | f 69 68 70 202 | f 55 62 71 203 | f 66 55 71 204 | f 62 69 71 205 | o convex_3 206 | v -0.017612 0.061506 -0.020836 207 | v 0.039434 0.051583 0.041174 208 | v 0.039434 0.036711 0.041174 209 | v -0.039942 0.036711 0.041174 210 | v 0.022068 0.031745 -0.020836 211 | v -0.010169 0.066472 0.061021 212 | v 0.039434 0.066472 -0.005948 213 | v 0.022068 0.031745 0.061021 214 | v -0.039942 0.031745 0.001496 215 | v -0.039942 0.066472 -0.005948 216 | v -0.020091 0.031745 0.061021 217 | v -0.039942 0.063987 0.038690 218 | v 0.039434 0.066472 0.036214 219 | v -0.020091 0.031745 -0.020836 220 | v 0.039434 0.031745 0.001496 221 | v 0.017103 0.061506 0.061021 222 | v 0.009668 0.066472 -0.020836 223 | v -0.020091 0.056550 0.061021 224 | v 0.022068 0.051583 -0.020836 225 | f 88 78 90 226 | f 73 74 78 227 | f 74 73 79 228 | f 76 79 80 229 | f 77 78 81 230 | f 80 75 81 231 | f 79 77 82 232 | f 75 80 82 233 | f 80 79 82 234 | f 81 75 83 235 | f 77 81 83 236 | f 73 78 84 237 | f 78 77 84 238 | f 72 76 85 239 | f 76 80 85 240 | f 81 72 85 241 | f 80 81 85 242 | f 78 74 86 243 | f 76 78 86 244 | f 74 79 86 245 | f 79 76 86 246 | f 79 73 87 247 | f 77 79 87 248 | f 73 84 87 249 | f 84 77 87 250 | f 76 72 88 251 | f 72 81 88 252 | f 81 78 88 253 | f 75 82 89 254 | f 82 77 89 255 | f 83 75 89 256 | f 77 83 89 257 | f 78 76 90 258 | f 76 88 90 259 | o convex_4 260 | v -0.039938 0.066472 -0.005958 261 | v 0.041914 0.111120 0.046138 262 | v 0.012145 0.113602 0.043642 263 | v 0.041914 0.111120 -0.323457 264 | v -0.042423 0.111120 -0.323457 265 | v 0.041914 0.068954 -0.323457 266 | v 0.041914 0.068954 0.046138 267 | v -0.042423 0.068954 -0.323457 268 | v -0.042423 0.111120 0.043642 269 | v -0.042423 0.068954 0.043642 270 | v 0.014630 0.066472 -0.323457 271 | v -0.012654 0.113602 -0.323457 272 | v 0.039429 0.066472 0.036189 273 | v -0.010177 0.113602 0.046138 274 | v -0.015139 0.066472 -0.323457 275 | v 0.012145 0.113602 -0.323457 276 | v -0.012654 0.066472 0.046138 277 | v -0.039938 0.111120 0.046138 278 | f 100 107 108 279 | f 93 92 94 280 | f 94 92 96 281 | f 95 94 96 282 | f 96 92 97 283 | f 95 96 98 284 | f 95 98 99 285 | f 99 98 100 286 | f 98 91 100 287 | f 98 96 101 288 | f 94 95 102 289 | f 95 99 102 290 | f 96 97 103 291 | f 91 101 103 292 | f 101 96 103 293 | f 92 93 104 294 | f 97 92 104 295 | f 93 102 104 296 | f 102 99 104 297 | f 91 98 105 298 | f 101 91 105 299 | f 98 101 105 300 | f 93 94 106 301 | f 102 93 106 302 | f 94 102 106 303 | f 100 91 107 304 | f 103 97 107 305 | f 91 103 107 306 | f 97 104 107 307 | f 107 104 108 308 | f 99 100 108 309 | f 104 99 108 310 | -------------------------------------------------------------------------------- /safemotions/description/urdf/obstacles/monitor_no_pivot.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /safemotions/description/urdf/obstacles/monitor_pivot.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /safemotions/description/urdf/one_robot.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 279 | 280 | -------------------------------------------------------------------------------- /safemotions/description/urdf/two_robots.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 279 | 280 | 281 | 282 | 283 | 284 | 285 | 286 | 287 | 288 | 289 | 290 | 291 | 292 | 293 | 294 | 295 | 296 | 297 | 298 | 299 | 300 | 301 | 302 | 303 | 304 | 305 | 306 | 307 | 308 | 309 | 310 | 311 | 312 | 313 | 314 | 315 | 316 | 317 | 318 | 319 | 320 | 321 | 322 | 323 | 324 | 325 | 326 | 327 | 328 | 329 | 330 | 331 | 332 | 333 | 334 | 335 | 336 | 337 | 338 | 339 | 340 | 341 | 342 | 343 | 344 | 345 | 346 | 347 | 348 | 349 | 350 | 351 | 352 | 353 | 354 | 355 | 356 | 357 | 358 | 359 | 360 | 361 | 362 | 363 | 364 | 365 | 366 | 367 | 368 | 369 | 370 | 371 | 372 | 373 | 374 | 375 | 376 | 377 | 378 | 379 | 380 | 381 | 382 | 383 | 384 | 385 | 386 | 387 | 388 | 389 | 390 | 391 | 392 | 393 | 394 | 395 | 396 | 397 | 398 | 399 | 400 | 401 | 402 | 403 | 404 | 405 | 406 | 407 | 408 | 409 | 410 | 411 | 412 | 413 | 414 | 415 | 416 | 417 | 418 | 419 | 420 | 421 | 422 | 423 | 424 | 425 | 426 | 427 | 428 | 429 | 430 | 431 | 432 | 433 | 434 | 435 | 436 | 437 | 438 | 439 | 440 | 441 | 442 | 443 | 444 | 445 | 446 | 447 | 448 | 449 | 450 | 451 | 452 | 453 | 454 | 455 | 456 | 457 | 458 | 459 | 460 | 461 | 462 | 463 | 464 | 465 | 466 | 467 | 468 | 469 | 470 | 471 | 472 | 473 | 474 | 475 | 476 | 477 | 478 | 479 | 480 | 481 | 482 | 483 | 484 | 485 | 486 | 487 | 488 | 489 | 490 | 491 | 492 | 493 | 494 | 495 | 496 | 497 | 498 | 499 | 500 | 501 | 502 | 503 | 504 | 505 | 506 | 507 | 508 | 509 | 510 | 511 | 512 | 513 | 514 | 515 | 516 | 517 | 518 | 519 | 520 | 521 | 522 | 523 | 524 | 525 | 526 | 527 | 528 | 529 | -------------------------------------------------------------------------------- /safemotions/envs/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/translearn/safeMotions/36a97fb0f71fa7f81543dd94cdb58578b6a1022d/safemotions/envs/__init__.py -------------------------------------------------------------------------------- /safemotions/envs/decorators/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/translearn/safeMotions/36a97fb0f71fa7f81543dd94cdb58578b6a1022d/safemotions/envs/decorators/__init__.py -------------------------------------------------------------------------------- /safemotions/envs/decorators/actions.py: -------------------------------------------------------------------------------- 1 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE 2 | # WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 3 | # COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR 4 | # OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 5 | 6 | import logging 7 | from abc import ABC 8 | 9 | import numpy as np 10 | from gym.spaces import Box 11 | from klimits import PosVelJerkLimitation 12 | from klimits import denormalize as denormalize 13 | from klimits import get_num_threads 14 | from klimits import interpolate_acceleration_batch as interpolate_acceleration_batch 15 | from klimits import interpolate_position_batch as interpolate_position_batch 16 | from klimits import interpolate_velocity_batch as interpolate_velocity_batch 17 | from klimits import normalize_batch as normalize_batch 18 | 19 | from safemotions.envs.safe_motions_base import SafeMotionsBase 20 | from safemotions.utils import trajectory_plotter 21 | from safemotions.utils.braking_trajectory_generator import BrakingTrajectoryGenerator 22 | 23 | TERMINATION_JOINT_LIMITS = 1 24 | 25 | 26 | class AccelerationPredictionBoundedJerkAccVelPos(ABC, SafeMotionsBase): 27 | 28 | def __init__(self, 29 | *vargs, 30 | limit_velocity=True, 31 | limit_position=True, 32 | set_velocity_after_max_pos_to_zero=True, 33 | acc_limit_factor_braking=1.0, 34 | jerk_limit_factor_braking=1.0, 35 | plot_trajectory=False, 36 | save_trajectory_plot=False, 37 | plot_joint=None, 38 | plot_acc_limits=False, 39 | plot_actual_values=False, 40 | plot_time_limits=None, 41 | **kwargs): 42 | super().__init__(*vargs, **kwargs) 43 | 44 | self.action_space = Box(low=np.float32(-1), high=np.float32(1), shape=(self._num_manip_joints,), 45 | dtype=np.float32) 46 | 47 | self._plot_trajectory = plot_trajectory 48 | self._save_trajectory_plot = save_trajectory_plot 49 | self._limit_velocity = limit_velocity 50 | self._limit_position = limit_position 51 | self._safe_acc_range = None 52 | 53 | self._pos_limits_min_max = np.array([self._robot_scene.joint_lower_limits, 54 | self._robot_scene.joint_upper_limits]) # [min_max][joint] 55 | pos_limits_joint = np.swapaxes(self._pos_limits_min_max, 0, 1) # [joint][min_max] 56 | self._vel_limits_min_max = np.array([-1 * self._robot_scene.max_velocities, self._robot_scene.max_velocities]) 57 | vel_limits_joint = np.swapaxes(self._vel_limits_min_max, 0, 1) 58 | self._acc_limits_min_max = np.array([-1 * self._robot_scene.max_accelerations, 59 | self._robot_scene.max_accelerations]) 60 | acc_limits_joint = np.swapaxes(self._acc_limits_min_max, 0, 1) 61 | jerk_limits_joint = np.swapaxes(np.array([-1 * self._robot_scene.max_jerk_linear_interpolation, 62 | self._robot_scene.max_jerk_linear_interpolation]), 0, 1) 63 | torque_limits_joint = np.swapaxes(np.array([-1 * self._robot_scene.max_torques, 64 | self._robot_scene.max_torques]), 0, 1) 65 | 66 | max_accelerations_braking = acc_limit_factor_braking * self._robot_scene.max_accelerations 67 | acc_limits_braking = np.swapaxes(np.array([(-1) * max_accelerations_braking, max_accelerations_braking]), 0, 1) 68 | max_jerk_braking = np.array([min(2 * jerk_limit_factor_braking * max_accelerations_braking[i] 69 | / self._trajectory_time_step, 70 | self._robot_scene.max_jerk_linear_interpolation[i]) 71 | for i in range(len(max_accelerations_braking))]) 72 | jerk_limits_braking = np.swapaxes(np.array([-1 * max_jerk_braking, max_jerk_braking]), 0, 1) 73 | 74 | self._plot_acc_limits = plot_acc_limits 75 | self._plot_actual_values = plot_actual_values 76 | 77 | if self._plot_trajectory and self._plot_actual_values and self._use_real_robot: 78 | raise NotImplementedError("Simultaneous plotting of actual values not implemented for real robots") 79 | 80 | num_threads = None 81 | if num_threads is None: 82 | logging.warning("Using %s thread(s) per worker to compute the range of safe accelerations.", 83 | get_num_threads()) 84 | else: 85 | logging.warning("Using %s thread(s) per worker to compute the range of safe accelerations.", num_threads) 86 | 87 | self._acc_limitation = PosVelJerkLimitation(time_step=self._trajectory_time_step, 88 | pos_limits=pos_limits_joint, vel_limits=vel_limits_joint, 89 | acc_limits=acc_limits_joint, jerk_limits=jerk_limits_joint, 90 | acceleration_after_max_vel_limit_factor= 91 | self._acceleration_after_max_vel_limit_factor, 92 | set_velocity_after_max_pos_to_zero= 93 | set_velocity_after_max_pos_to_zero, 94 | limit_velocity=limit_velocity, limit_position=limit_position, 95 | normalize_acc_range=False, 96 | num_threads=num_threads) 97 | 98 | self._braking_trajectory_generator = BrakingTrajectoryGenerator(trajectory_time_step= 99 | self._trajectory_time_step, 100 | acc_limits_braking=acc_limits_braking, 101 | jerk_limits_braking=jerk_limits_braking) 102 | 103 | if self._plot_trajectory or self._save_trajectory_plot: 104 | self._trajectory_plotter = \ 105 | trajectory_plotter.TrajectoryPlotter(time_step=self._trajectory_time_step, 106 | control_time_step=self._control_time_step, 107 | computed_actual_values_time_step=self._simulation_time_step, 108 | pos_limits=pos_limits_joint, vel_limits=vel_limits_joint, 109 | acc_limits=acc_limits_joint, 110 | jerk_limits=jerk_limits_joint, 111 | torque_limits=torque_limits_joint, 112 | plot_joint=plot_joint, 113 | plot_acc_limits=self._plot_acc_limits, 114 | plot_time_limits=plot_time_limits, 115 | plot_actual_values=self._plot_actual_values, 116 | plot_computed_actual_values=self._plot_computed_actual_values, 117 | plot_actual_torques=self._plot_actual_torques) 118 | 119 | def _get_safe_acc_range(self): 120 | return self._safe_acc_range 121 | 122 | @property 123 | def pos_limits_min_max(self): 124 | return self._pos_limits_min_max 125 | 126 | def _reset_plotter(self, initial_joint_position): 127 | if self._plot_trajectory or self._save_trajectory_plot: 128 | self._trajectory_plotter.reset_plotter(initial_joint_position) 129 | 130 | def _display_plot(self): 131 | if self._plot_trajectory: 132 | self._trajectory_plotter.display_plot(obstacle_wrapper=self._robot_scene.obstacle_wrapper) 133 | 134 | def _add_actual_position_to_plot(self, actual_position=None): 135 | if (self._plot_trajectory and self._plot_actual_values) or self._save_trajectory_plot: 136 | if actual_position is None: 137 | actual_position = self._robot_scene.get_actual_joint_positions() 138 | self._trajectory_plotter.add_actual_position(actual_position) 139 | 140 | def _add_computed_actual_position_to_plot(self, computed_position_is, computed_velocity_is, 141 | computed_acceleration_is): 142 | if self._plot_trajectory and self._plot_computed_actual_values: 143 | self._trajectory_plotter.add_computed_actual_value(computed_position_is, computed_velocity_is, 144 | computed_acceleration_is) 145 | 146 | def _add_actual_torques_to_plot(self, actual_torques): 147 | if self._plot_trajectory and self._plot_actual_torques: 148 | self._trajectory_plotter.add_actual_torque(actual_torques) 149 | 150 | def _save_plot(self, class_name, experiment_name): 151 | if self._save_trajectory_plot or (self._log_obstacle_data and self._save_obstacle_data): 152 | self._trajectory_plotter.save_trajectory(class_name, experiment_name) 153 | if self._log_obstacle_data and self._save_obstacle_data: 154 | self._trajectory_plotter.save_obstacle_data(class_name, experiment_name) 155 | 156 | def _calculate_safe_acc_range(self, start_position, start_velocity, start_acceleration, trajectory_point_index): 157 | # the acc range is required to compute the corresponding mapping to meet the next reference acceleration 158 | # which can be included into the state. -> Acc range for observation 0 required; called in base reset() 159 | self._safe_acc_range, _ = self._acc_limitation.calculate_valid_acceleration_range(start_position, 160 | start_velocity, 161 | start_acceleration) 162 | 163 | def compute_next_acc_min_and_next_acc_max(self, start_position, start_velocity, start_acceleration): 164 | safe_acc_range_joint, _ = self._acc_limitation.calculate_valid_acceleration_range(start_position, 165 | start_velocity, 166 | start_acceleration) 167 | 168 | safe_acc_range_min_max = safe_acc_range_joint.T 169 | # computes the denormalized minimum and maximum acceleration that can be reached at the following time step 170 | next_acc_min = safe_acc_range_min_max[0] 171 | next_acc_max = safe_acc_range_min_max[1] 172 | 173 | return next_acc_min, next_acc_max 174 | 175 | def acc_braking_function(self, *vargs, **kwargs): 176 | return self._braking_trajectory_generator.get_clipped_braking_acceleration(*vargs, **kwargs) 177 | 178 | def _compute_controller_setpoints_from_action(self, action): 179 | info = {'average': {}, 180 | 'max': {}} 181 | 182 | robot_stopped = False 183 | 184 | safe_acc_range_min_max = self._safe_acc_range.T 185 | self._end_acceleration = denormalize(action, safe_acc_range_min_max) 186 | 187 | execute_braking_trajectory = False 188 | self._adaptation_punishment = 0 189 | 190 | if not self._brake: 191 | self._end_acceleration, execute_braking_trajectory, self._adaptation_punishment, \ 192 | min_distance, max_torque = \ 193 | self._robot_scene.obstacle_wrapper.adapt_action( 194 | current_acc=self._start_acceleration, 195 | current_vel=self._start_velocity, 196 | current_pos=self._start_position, 197 | target_acc=self._end_acceleration, 198 | time_step_counter=self._current_trajectory_point_index) 199 | 200 | if execute_braking_trajectory: 201 | self._end_acceleration, robot_stopped, min_distance, max_torque = \ 202 | self._robot_scene.obstacle_wrapper.get_braking_acceleration() 203 | if min_distance is not None: 204 | self._end_min_distance = min_distance 205 | if max_torque is not None: 206 | self._end_max_torque = max_torque 207 | else: 208 | self._end_min_distance = min_distance 209 | self._end_max_torque = max_torque 210 | else: 211 | self._end_acceleration, robot_stopped, min_distance, max_torque = \ 212 | self._robot_scene.obstacle_wrapper.get_braking_acceleration() 213 | if min_distance is not None: 214 | self._end_min_distance = min_distance 215 | if max_torque is not None: 216 | self._end_max_torque = max_torque 217 | 218 | # compute setpoints 219 | controller_setpoints, joint_limit_violation = self._compute_interpolated_setpoints() 220 | 221 | if joint_limit_violation and not self._brake: 222 | self._network_prediction_part_done = True 223 | self._termination_reason = TERMINATION_JOINT_LIMITS 224 | self._trajectory_successful = False 225 | self._brake = True 226 | 227 | if not execute_braking_trajectory: 228 | self._end_acceleration, robot_stopped, min_distance, max_torque = \ 229 | self._robot_scene.obstacle_wrapper.get_braking_acceleration(last=True) 230 | if min_distance is not None: 231 | self._end_min_distance = min_distance 232 | if max_torque is not None: 233 | self._end_max_torque = max_torque 234 | controller_setpoints, _ = self._compute_interpolated_setpoints() 235 | 236 | if self._control_time_step != self._simulation_time_step: 237 | obstacle_client_update_setpoints, _ = self._compute_interpolated_setpoints( 238 | use_obstacle_client_update_time_step=True) 239 | else: 240 | obstacle_client_update_setpoints = controller_setpoints 241 | 242 | if self._plot_trajectory or self._save_trajectory_plot: 243 | self._trajectory_plotter.add_data_point(self._end_acceleration, self._safe_acc_range) 244 | 245 | return self._end_acceleration, controller_setpoints, obstacle_client_update_setpoints, info, robot_stopped 246 | 247 | def _compute_interpolated_setpoints(self, use_obstacle_client_update_time_step=False): 248 | joint_limit_violation = False 249 | 250 | if not use_obstacle_client_update_time_step: 251 | steps_per_action = self._control_steps_per_action 252 | else: 253 | steps_per_action = self._obstacle_client_update_steps_per_action 254 | 255 | time_since_start = np.linspace(self._trajectory_time_step / steps_per_action, 256 | self._trajectory_time_step, steps_per_action) 257 | 258 | interpolated_position_setpoints = interpolate_position_batch(self._start_acceleration, 259 | self._end_acceleration, 260 | self._start_velocity, 261 | self._start_position, time_since_start, 262 | self._trajectory_time_step) 263 | 264 | interpolated_velocity_setpoints = interpolate_velocity_batch(self._start_acceleration, 265 | self._end_acceleration, 266 | self._start_velocity, 267 | time_since_start, 268 | self._trajectory_time_step) 269 | 270 | interpolated_acceleration_setpoints = interpolate_acceleration_batch(self._start_acceleration, 271 | self._end_acceleration, 272 | time_since_start, 273 | self._trajectory_time_step) 274 | 275 | interpolated_setpoints = {'positions': interpolated_position_setpoints, 276 | 'velocities': interpolated_velocity_setpoints, 277 | 'accelerations': interpolated_acceleration_setpoints} 278 | 279 | if self._use_real_robot and not self._brake and not use_obstacle_client_update_time_step: 280 | 281 | # Note: It might be more efficient to calculate the min / max absolute value for each joint individually 282 | # rather than normalizing all values 283 | max_normalized_position = np.max(np.abs(normalize_batch(interpolated_position_setpoints, 284 | self._pos_limits_min_max))) 285 | max_normalized_velocity = np.max(np.abs(normalize_batch(interpolated_velocity_setpoints, 286 | self._vel_limits_min_max))) 287 | max_normalized_acceleration = np.max(np.abs(normalize_batch(interpolated_acceleration_setpoints, 288 | self._acc_limits_min_max))) 289 | 290 | if max_normalized_position > 1.002 or max_normalized_velocity > 1.002 \ 291 | or max_normalized_acceleration > 1.002: 292 | joint_limit_violation = True 293 | 294 | if max_normalized_position > 1.002: 295 | logging.warning("Position limit exceeded: %s", max_normalized_position) 296 | if max_normalized_velocity > 1.002: 297 | logging.warning("Velocity limit exceeded: %s", max_normalized_velocity) 298 | if max_normalized_acceleration > 1.002: 299 | logging.warning("Acceleration limit exceeded: %s", max_normalized_acceleration) 300 | 301 | return interpolated_setpoints, joint_limit_violation 302 | 303 | def _interpolate_position(self, step): 304 | interpolated_position = self._start_position + self._start_velocity * step + \ 305 | 0.5 * self._start_acceleration * step ** 2 + \ 306 | 1 / 6 * ((self._end_acceleration - self._start_acceleration) 307 | / self._trajectory_time_step) * step ** 3 308 | return list(interpolated_position) 309 | 310 | def _interpolate_velocity(self, step): 311 | interpolated_velocity = self._start_velocity + self._start_acceleration * step + \ 312 | 0.5 * ((self._end_acceleration - self._start_acceleration) / 313 | self._trajectory_time_step) * step ** 2 314 | 315 | return list(interpolated_velocity) 316 | 317 | def _interpolate_acceleration(self, step): 318 | interpolated_acceleration = self._start_acceleration + \ 319 | ((self._end_acceleration - self._start_acceleration) / 320 | self._trajectory_time_step) * step 321 | 322 | return list(interpolated_acceleration) 323 | 324 | def _integrate_linear(self, start_value, end_value): 325 | return (end_value + start_value) * self._trajectory_time_step / 2 326 | 327 | def _normalize(self, value, value_range): 328 | normalized_value = -1 + 2 * (value - value_range[0]) / (value_range[1] - value_range[0]) 329 | return normalized_value 330 | 331 | def _denormalize(self, norm_value, value_range): 332 | actual_value = value_range[0] + 0.5 * (norm_value + 1) * (value_range[1] - value_range[0]) 333 | return actual_value 334 | -------------------------------------------------------------------------------- /safemotions/envs/decorators/observations.py: -------------------------------------------------------------------------------- 1 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE 2 | # WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 3 | # COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR 4 | # OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 5 | 6 | import logging 7 | from abc import ABC 8 | 9 | import numpy as np 10 | from gym.spaces import Box 11 | from safemotions.envs.safe_motions_base import SafeMotionsBase 12 | 13 | 14 | def normalize_joint_values(values, joint_limits): 15 | return list(np.array(values) / np.array(joint_limits)) 16 | 17 | 18 | def _normalize_joint_values_min_max(values, joint_limit_ranges): 19 | normalized_values = -1 + 2 * (values - joint_limit_ranges[0]) / (joint_limit_ranges[1] - joint_limit_ranges[0]) 20 | continuous_joint_indices = np.isnan(joint_limit_ranges[0]) | np.isnan(joint_limit_ranges[1]) 21 | if np.any(continuous_joint_indices): 22 | # continuous joint -> map [-np.pi, np.pi] and all values shifted by 2 * np.pi to [-1, 1] 23 | normalized_values[continuous_joint_indices] = \ 24 | -1 + 2 * (((values[continuous_joint_indices] + np.pi)/(2 * np.pi)) % 1) 25 | return list(normalized_values) 26 | 27 | 28 | TARGET_POINT_SIMULTANEOUS = 0 29 | TARGET_POINT_ALTERNATING = 1 30 | TARGET_POINT_SINGLE = 2 31 | 32 | 33 | class SafeObservation(ABC, SafeMotionsBase): 34 | 35 | def __init__(self, 36 | *vargs, 37 | m_prev=0, 38 | obs_add_target_point_pos=False, 39 | obs_add_target_point_relative_pos=False, 40 | **kwargs): 41 | super().__init__(*vargs, **kwargs) 42 | 43 | self._m_prev = m_prev 44 | self._next_joint_acceleration_mapping = None 45 | 46 | self._obs_add_target_point_pos = obs_add_target_point_pos 47 | self._obs_add_target_point_relative_pos = obs_add_target_point_relative_pos 48 | 49 | obs_current_size = 3 # pos, vel, acc 50 | 51 | obs_target_point_size = 0 52 | 53 | if self._robot_scene.obstacle_wrapper.use_target_points: 54 | if obs_add_target_point_pos: 55 | if self._robot_scene.obstacle_wrapper.target_point_sequence == TARGET_POINT_SINGLE: 56 | obs_target_point_size += 3 # one target point only 57 | else: 58 | obs_target_point_size += 3 * self._robot_scene.num_robots # one target point per robot 59 | 60 | if obs_add_target_point_relative_pos: 61 | obs_target_point_size += 3 * self._robot_scene.num_robots 62 | 63 | if self._robot_scene.obstacle_wrapper.target_point_sequence == TARGET_POINT_ALTERNATING: 64 | obs_target_point_size += self._robot_scene.num_robots # target point active signal for each robot 65 | 66 | self._observation_size = self._m_prev * self._num_manip_joints + obs_current_size * self._num_manip_joints \ 67 | + obs_target_point_size 68 | 69 | self.observation_space = Box(low=np.float32(-1), high=np.float32(1), shape=(self._observation_size,), 70 | dtype=np.float32) 71 | 72 | logging.info("Observation size: " + str(self._observation_size)) 73 | 74 | def reset(self): 75 | super().reset() 76 | self._robot_scene.prepare_for_start_of_episode() 77 | self._init_observation_attributes() 78 | 79 | observation, observation_info = self._get_observation() 80 | 81 | return observation 82 | 83 | def _init_observation_attributes(self): 84 | pass 85 | 86 | def _prepare_for_next_action(self): 87 | super()._prepare_for_next_action() 88 | 89 | def _get_observation(self): 90 | prev_joint_accelerations = self._get_m_prev_joint_values(self._m_prev, key='accelerations') 91 | curr_joint_position = self._get_generated_trajectory_point(-1) 92 | curr_joint_velocity = self._get_generated_trajectory_point(-1, key='velocities') 93 | curr_joint_acceleration = self._get_generated_trajectory_point(-1, key='accelerations') 94 | prev_joint_accelerations_rel = [normalize_joint_values(p, self._robot_scene.max_accelerations) 95 | for p in prev_joint_accelerations] 96 | curr_joint_positions_rel_obs = _normalize_joint_values_min_max(curr_joint_position, 97 | self.pos_limits_min_max) 98 | curr_joint_velocity_rel_obs = normalize_joint_values(curr_joint_velocity, self._robot_scene.max_velocities) 99 | curr_joint_acceleration_rel_obs = normalize_joint_values(curr_joint_acceleration, 100 | self._robot_scene.max_accelerations) 101 | 102 | # target point for reaching tasks 103 | target_point_rel_obs = [] 104 | if self._robot_scene.obstacle_wrapper.use_target_points: 105 | # the function needs to be called even if the return value is not used. 106 | # Otherwise new target points are not generated 107 | target_point_pos, target_point_relative_pos, _, target_point_active_obs = \ 108 | self._robot_scene.obstacle_wrapper.get_target_point_observation( 109 | compute_relative_pos_norm=self._obs_add_target_point_relative_pos, 110 | compute_target_point_joint_pos_norm=False) 111 | if self._obs_add_target_point_pos: 112 | target_point_rel_obs = target_point_rel_obs + list(target_point_pos) 113 | 114 | if self._obs_add_target_point_relative_pos: 115 | target_point_rel_obs = target_point_rel_obs + list(target_point_relative_pos) 116 | 117 | target_point_rel_obs = target_point_rel_obs + list(target_point_active_obs) 118 | # to indicate if the target point is active (1.0) or inactive (0.0); the list is empty if not required 119 | 120 | observation = np.array((np.core.umath.clip( 121 | [item for sublist in prev_joint_accelerations_rel for item in sublist] 122 | + curr_joint_positions_rel_obs + curr_joint_velocity_rel_obs 123 | + curr_joint_acceleration_rel_obs + target_point_rel_obs, -1, 1)), dtype=np.float32) 124 | 125 | info = {'average': {}, 126 | 'max': {}, 127 | 'min': {}} 128 | 129 | pos_violation = 0.0 130 | vel_violation = 0.0 131 | acc_violation = 0.0 132 | 133 | for j in range(self._num_manip_joints): 134 | 135 | info['average']['joint_{}_pos'.format(j)] = curr_joint_positions_rel_obs[j] 136 | info['average']['joint_{}_pos_abs'.format(j)] = abs(curr_joint_positions_rel_obs[j]) 137 | info['max']['joint_{}_pos'.format(j)] = curr_joint_positions_rel_obs[j] 138 | info['min']['joint_{}_pos'.format(j)] = curr_joint_positions_rel_obs[j] 139 | if abs(curr_joint_positions_rel_obs[j]) > 1.001: 140 | logging.warning("Position violation: t = %s Joint: %s Rel position %s", 141 | (self._episode_length - 1) * self._trajectory_time_step, j, 142 | curr_joint_positions_rel_obs[j]) 143 | pos_violation = 1.0 144 | 145 | info['average']['joint_{}_vel'.format(j)] = curr_joint_velocity_rel_obs[j] 146 | info['average']['joint_{}_vel_abs'.format(j)] = abs(curr_joint_velocity_rel_obs[j]) 147 | info['max']['joint_{}_vel'.format(j)] = curr_joint_velocity_rel_obs[j] 148 | info['min']['joint_{}_vel'.format(j)] = curr_joint_velocity_rel_obs[j] 149 | if abs(curr_joint_velocity_rel_obs[j]) > 1.001: 150 | logging.warning("Velocity violation: t = %s Joint: %s Rel velocity %s", 151 | (self._episode_length - 1) * self._trajectory_time_step, j, 152 | curr_joint_velocity_rel_obs[j]) 153 | vel_violation = 1.0 154 | 155 | info['average']['joint_{}_acc'.format(j)] = curr_joint_acceleration_rel_obs[j] 156 | info['average']['joint_{}_acc_abs'.format(j)] = abs(curr_joint_acceleration_rel_obs[j]) 157 | info['max']['joint_{}_acc'.format(j)] = curr_joint_acceleration_rel_obs[j] 158 | info['min']['joint_{}_acc'.format(j)] = curr_joint_acceleration_rel_obs[j] 159 | if abs(curr_joint_acceleration_rel_obs[j]) > 1.001: 160 | logging.warning("Acceleration violation: t = %s Joint: %s Rel acceleration %s", 161 | (self._episode_length - 1) * self._trajectory_time_step, j, 162 | curr_joint_acceleration_rel_obs[j]) 163 | acc_violation = 1.0 164 | 165 | info['max']['joint_pos_violation'] = pos_violation 166 | info['max']['joint_vel_violation'] = vel_violation 167 | info['max']['joint_acc_violation'] = acc_violation 168 | 169 | logging.debug("Observation %s: %s", self._episode_length, np.asarray(observation)) 170 | 171 | return observation, info 172 | 173 | def _get_m_prev_joint_values(self, m, key): 174 | 175 | m_prev_joint_values = [] 176 | 177 | for i in range(m+1, 1, -1): 178 | m_prev_joint_values.append(self._get_generated_trajectory_point(-i, key)) 179 | 180 | return m_prev_joint_values 181 | -------------------------------------------------------------------------------- /safemotions/envs/decorators/rewards.py: -------------------------------------------------------------------------------- 1 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE 2 | # WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 3 | # COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR 4 | # OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 5 | import numpy as np 6 | import logging 7 | from abc import ABC 8 | from safemotions.envs.safe_motions_base import SafeMotionsBase 9 | 10 | 11 | def normalize_joint_values(values, joint_limits): 12 | return list(np.array(values) / np.array(joint_limits)) 13 | 14 | 15 | class TargetPointReachingReward(ABC, SafeMotionsBase): 16 | # optional action penalty 17 | ACTION_THRESHOLD = 0.9 18 | ACTION_MAX_PUNISHMENT = 1.0 19 | ADAPTATION_MAX_PUNISHMENT = 1.0 20 | END_MIN_DISTANCE_MAX_PUNISHMENT = 1.0 21 | END_MAX_TORQUE_MAX_PUNISHMENT = 1.0 22 | END_MAX_TORQUE_MIN_THRESHOLD = 0.9 23 | 24 | # braking trajectory max punishment (either collision or torque -> max) 25 | BRAKING_TRAJECTORY_MAX_PUNISHMENT = 1.0 26 | # braking trajectory torque penalty 27 | BRAKING_TRAJECTORY_MAX_TORQUE_MIN_THRESHOLD = 0.9 # rel. abs. torque threshold 28 | 29 | def __init__(self, 30 | *vargs, 31 | normalize_reward_to_frequency=False, 32 | normalize_reward_to_initial_target_point_distance=False, 33 | punish_action=False, 34 | action_punishment_min_threshold=ACTION_THRESHOLD, 35 | action_max_punishment=ACTION_MAX_PUNISHMENT, 36 | punish_adaptation=False, 37 | adaptation_max_punishment=ADAPTATION_MAX_PUNISHMENT, 38 | punish_end_min_distance=False, 39 | end_min_distance_max_punishment=END_MIN_DISTANCE_MAX_PUNISHMENT, 40 | end_min_distance_max_threshold=None, 41 | punish_end_max_torque=False, 42 | end_max_torque_max_punishment=END_MAX_TORQUE_MAX_PUNISHMENT, 43 | end_max_torque_min_threshold=END_MAX_TORQUE_MIN_THRESHOLD, 44 | braking_trajectory_max_punishment=BRAKING_TRAJECTORY_MAX_PUNISHMENT, 45 | punish_braking_trajectory_min_distance=False, 46 | braking_trajectory_min_distance_max_threshold=None, 47 | punish_braking_trajectory_max_torque=False, 48 | braking_trajectory_max_torque_min_threshold=BRAKING_TRAJECTORY_MAX_TORQUE_MIN_THRESHOLD, 49 | target_point_reward_factor=1.0, 50 | **kwargs): 51 | 52 | # reward settings 53 | self.reward_range = [0, 1] 54 | self._normalize_reward_to_frequency = normalize_reward_to_frequency 55 | self._normalize_reward_to_initial_target_point_distance = normalize_reward_to_initial_target_point_distance 56 | 57 | self._punish_action = punish_action 58 | self._action_punishment_min_threshold = action_punishment_min_threshold 59 | self._action_max_punishment = action_max_punishment 60 | 61 | self._punish_adaptation = punish_adaptation 62 | self._adaptation_max_punishment = adaptation_max_punishment 63 | 64 | self._punish_end_min_distance = punish_end_min_distance 65 | self._end_min_distance_max_punishment = end_min_distance_max_punishment 66 | self._end_min_distance_max_threshold = end_min_distance_max_threshold 67 | self._punish_end_max_torque = punish_end_max_torque 68 | self._end_max_torque_max_punishment = end_max_torque_max_punishment 69 | self._end_max_torque_min_threshold = end_max_torque_min_threshold 70 | 71 | self._punish_braking_trajectory_min_distance = punish_braking_trajectory_min_distance 72 | self._braking_trajectory_min_distance_max_threshold = braking_trajectory_min_distance_max_threshold 73 | self._punish_braking_trajectory_max_torque = punish_braking_trajectory_max_torque 74 | self._braking_trajectory_max_punishment = braking_trajectory_max_punishment 75 | self._max_torque_min_threshold = braking_trajectory_max_torque_min_threshold 76 | 77 | self._target_point_reward_factor = target_point_reward_factor 78 | self._reward_maximum_relevant_distance = None 79 | 80 | if self._punish_braking_trajectory_min_distance or self._punish_end_min_distance: 81 | if self._punish_braking_trajectory_min_distance and \ 82 | self._braking_trajectory_min_distance_max_threshold is None: 83 | raise ValueError("punish_braking_trajectory_min_distance requires " 84 | "braking_trajectory_min_distance_max_threshold to be specified") 85 | if self._punish_end_min_distance and \ 86 | self._end_min_distance_max_threshold is None: 87 | raise ValueError("punish_end_min_distance requires " 88 | "end_min_distance_max_threshold to be specified") 89 | 90 | if self._punish_braking_trajectory_min_distance and self._punish_end_min_distance: 91 | self._reward_maximum_relevant_distance = max(self._braking_trajectory_min_distance_max_threshold, 92 | self._end_min_distance_max_threshold) 93 | elif self._punish_braking_trajectory_min_distance: 94 | self._reward_maximum_relevant_distance = self._braking_trajectory_min_distance_max_threshold 95 | else: 96 | self._reward_maximum_relevant_distance = self._end_min_distance_max_threshold 97 | 98 | super().__init__(*vargs, **kwargs) 99 | 100 | def _get_reward(self): 101 | info = {'average': {'reward': 0, 'action_punishment': 0, 'target_point_reward': 0, 102 | 'braking_trajectory_min_distance_punishment': 0, 103 | 'braking_trajectory_max_torque_punishment': 0}, 104 | 'max': {'reward': 0, 'action_punishment': 0, 'target_point_reward': 0, 105 | 'braking_trajectory_min_distance_punishment': 0, 106 | 'braking_trajectory_max_torque_punishment': 0}, 107 | 'min': {'reward': 0, 'action_punishment': 0, 'target_point_reward': 0, 108 | 'braking_trajectory_min_distance_punishment': 0, 109 | 'braking_trajectory_max_torque_punishment': 0 110 | }} 111 | 112 | reward = 0 113 | target_point_reward = 0 114 | action_punishment = 0 115 | adaptation_punishment = 0 116 | end_min_distance_punishment = 0 117 | end_max_torque_punishment = 0 118 | 119 | braking_trajectory_min_distance_punishment = 0 120 | braking_trajectory_max_torque_punishment = 0 121 | 122 | if self._robot_scene.obstacle_wrapper.use_target_points: 123 | if self._punish_action: 124 | action_punishment = self._compute_action_punishment() # action punishment 125 | 126 | if self._punish_adaptation: 127 | adaptation_punishment = self._adaptation_punishment 128 | 129 | if self._punish_end_min_distance: 130 | if self._end_min_distance is None: 131 | self._end_min_distance, _, _, _ = self._robot_scene.obstacle_wrapper.get_minimum_distance( 132 | manip_joint_indices=self._robot_scene.manip_joint_indices, 133 | target_position=self._start_position) 134 | 135 | end_min_distance_punishment = self._compute_quadratic_punishment( 136 | a=self._end_min_distance_max_threshold, 137 | b=self._end_min_distance, 138 | c=self._end_min_distance_max_threshold, 139 | d=self._robot_scene.obstacle_wrapper.closest_point_safety_distance) 140 | 141 | if self._punish_end_max_torque: 142 | if self._end_max_torque is not None: 143 | # None if check_braking_trajectory is False and asynchronous movement execution is active 144 | # in this case, no penalty is computed, but the penalty is not required anyways 145 | end_max_torque_punishment = self._compute_quadratic_punishment( 146 | a=self._end_max_torque, 147 | b=self._end_max_torque_min_threshold, 148 | c=1, 149 | d=self._end_max_torque_min_threshold) 150 | 151 | target_point_reward = self._robot_scene.obstacle_wrapper.get_target_point_reward( 152 | normalize_distance_reward_to_initial_target_point_distance= 153 | self._normalize_reward_to_initial_target_point_distance) 154 | 155 | reward = target_point_reward * self._target_point_reward_factor \ 156 | - action_punishment * self._action_max_punishment \ 157 | - adaptation_punishment * self._adaptation_max_punishment \ 158 | - end_min_distance_punishment * self._end_min_distance_max_punishment \ 159 | - end_max_torque_punishment * self._end_max_torque_max_punishment 160 | 161 | if self._punish_braking_trajectory_min_distance or self._punish_braking_trajectory_max_torque: 162 | braking_trajectory_min_distance_punishment, braking_trajectory_max_torque_punishment = \ 163 | self._robot_scene.obstacle_wrapper.get_braking_trajectory_punishment( 164 | minimum_distance_max_threshold=self._braking_trajectory_min_distance_max_threshold, 165 | maximum_torque_min_threshold=self._max_torque_min_threshold) 166 | 167 | if self._punish_braking_trajectory_min_distance and self._punish_braking_trajectory_max_torque: 168 | braking_trajectory_punishment = self._braking_trajectory_max_punishment * \ 169 | max(braking_trajectory_min_distance_punishment, 170 | braking_trajectory_max_torque_punishment) 171 | elif self._punish_braking_trajectory_min_distance: 172 | braking_trajectory_punishment = self._braking_trajectory_max_punishment * \ 173 | braking_trajectory_min_distance_punishment 174 | else: 175 | braking_trajectory_punishment = self._braking_trajectory_max_punishment * \ 176 | braking_trajectory_max_torque_punishment 177 | 178 | reward = reward - braking_trajectory_punishment 179 | 180 | if self._normalize_reward_to_frequency: 181 | # Baseline: 10 Hz 182 | reward = reward * self._trajectory_time_step / 0.1 183 | 184 | info['average'].update(reward=reward, 185 | action_punishment=action_punishment, 186 | adaptation_punishment=adaptation_punishment, 187 | end_min_distance_punishment=end_min_distance_punishment, 188 | end_max_torque_punishment=end_max_torque_punishment, 189 | target_point_reward=target_point_reward, 190 | braking_trajectory_min_distance_punishment=braking_trajectory_min_distance_punishment, 191 | braking_trajectory_max_torque_punishment=braking_trajectory_max_torque_punishment) 192 | 193 | info['max'].update(reward=reward, 194 | action_punishment=action_punishment, 195 | adaptation_punishment=adaptation_punishment, 196 | end_min_distance_punishment=end_min_distance_punishment, 197 | end_max_torque_punishment=end_max_torque_punishment, 198 | target_point_reward=target_point_reward, 199 | braking_trajectory_min_distance_punishment=braking_trajectory_min_distance_punishment, 200 | braking_trajectory_max_torque_punishment=braking_trajectory_max_torque_punishment 201 | ) 202 | 203 | info['min'].update(reward=reward, 204 | action_punishment=action_punishment, 205 | adaptation_punishment=adaptation_punishment, 206 | end_min_distance_punishment=end_min_distance_punishment, 207 | end_max_torque_punishment=end_max_torque_punishment, 208 | target_point_reward=target_point_reward, 209 | braking_trajectory_min_distance_punishment=braking_trajectory_min_distance_punishment, 210 | braking_trajectory_max_torque_punishment=braking_trajectory_max_torque_punishment 211 | ) 212 | 213 | # add information about the jerk as custom metric 214 | curr_joint_jerk = (np.array(self._get_generated_trajectory_point(-1, key='accelerations')) 215 | - np.array(self._get_generated_trajectory_point(-2, key='accelerations'))) \ 216 | / self._trajectory_time_step 217 | 218 | curr_joint_jerk_rel = normalize_joint_values(curr_joint_jerk, self._robot_scene.max_jerk_linear_interpolation) 219 | jerk_violation = 0.0 220 | 221 | for j in range(self._num_manip_joints): 222 | info['average']['joint_{}_jerk'.format(j)] = curr_joint_jerk_rel[j] 223 | info['average']['joint_{}_jerk_abs'.format(j)] = abs(curr_joint_jerk_rel[j]) 224 | info['max']['joint_{}_jerk'.format(j)] = curr_joint_jerk_rel[j] 225 | info['min']['joint_{}_jerk'.format(j)] = curr_joint_jerk_rel[j] 226 | 227 | max_normalized_jerk = np.max(np.abs(curr_joint_jerk_rel)) 228 | if max_normalized_jerk > 1.002: 229 | jerk_violation = 1.0 230 | logging.warning("Jerk violation: t = %s Joint: %s Rel jerk %s", 231 | (self._episode_length - 1) * self._trajectory_time_step, 232 | np.argmax(np.abs(curr_joint_jerk_rel)), 233 | max_normalized_jerk) 234 | 235 | info['max']['joint_jerk_violation'] = jerk_violation 236 | 237 | logging.debug("Reward %s: %s", self._episode_length - 1, reward) 238 | 239 | return reward, info 240 | 241 | def _compute_quadratic_punishment(self, a, b, c, d): 242 | # returns max(min((a - b) / (c - d), 1), 0) ** 2 243 | punishment = (a - b) / (c - d) 244 | return max(min(punishment, 1), 0) ** 2 245 | 246 | def _compute_action_punishment(self): 247 | # The aim of the action punishment is to avoid the action being too close to -1 or 1. 248 | action_abs = np.abs(self._last_action) 249 | max_action_abs = max(action_abs) 250 | return self._compute_quadratic_punishment(max_action_abs, self._action_punishment_min_threshold, 251 | 1, self._action_punishment_min_threshold) 252 | 253 | @property 254 | def reward_maximum_relevant_distance(self): 255 | return self._reward_maximum_relevant_distance 256 | 257 | -------------------------------------------------------------------------------- /safemotions/envs/decorators/video_recording.py: -------------------------------------------------------------------------------- 1 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE 2 | # WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 3 | # COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR 4 | # OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 5 | 6 | import os 7 | import pybullet as p 8 | import json 9 | import numpy as np 10 | from glob import glob 11 | from abc import ABC 12 | from PIL import Image, ImageDraw, ImageFont 13 | from gym.wrappers.monitoring.video_recorder import VideoRecorder 14 | from safemotions.envs.safe_motions_base import SafeMotionsBase 15 | 16 | RENDER_MODES = ["human", "rgb_array"] 17 | VIDEO_FRAME_RATE = 60 18 | ASPECT_RATIO = 16/9 19 | VIDEO_HEIGHT = 1080 20 | # Renderer 21 | OPENGL_GUI_RENDERER = 0 22 | OPENGL_EGL_RENDERER = 1 23 | CPU_TINY_RENDERER = 2 24 | 25 | 26 | def compute_projection_matrix(): 27 | fov = 90 28 | near_distance = 0.1 29 | far_distance = 100 30 | 31 | return p.computeProjectionMatrixFOV(fov, ASPECT_RATIO, near_distance, far_distance) 32 | 33 | 34 | class VideoRecordingManager(ABC, SafeMotionsBase): 35 | def __init__(self, 36 | *vargs, 37 | extra_render_modes=None, 38 | video_frame_rate=None, 39 | video_height=VIDEO_HEIGHT, 40 | video_dir=None, 41 | video_add_text=False, 42 | fixed_video_filename=False, 43 | camera_angle=0, 44 | render_video=False, 45 | renderer=OPENGL_GUI_RENDERER, 46 | **kwargs): 47 | 48 | # video recording settings 49 | if video_frame_rate is None: 50 | video_frame_rate = VIDEO_FRAME_RATE 51 | if video_height is None: 52 | video_height = VIDEO_HEIGHT 53 | 54 | self._video_height = video_height 55 | self._video_width = int(ASPECT_RATIO * self._video_height) 56 | self._render_video = render_video 57 | self._renderer = renderer 58 | 59 | super().__init__(*vargs, **kwargs) 60 | 61 | self._video_recorder = None 62 | if video_dir is None: 63 | self._video_dir = self._evaluation_dir 64 | else: 65 | self._video_dir = video_dir 66 | self._video_add_text = video_add_text 67 | self._fixed_video_filename = fixed_video_filename 68 | self._video_base_path = None 69 | self._render_modes = RENDER_MODES.copy() 70 | if extra_render_modes: 71 | self._render_modes += extra_render_modes 72 | self._sim_steps_per_frame = int(1 / (video_frame_rate * self._control_time_step)) 73 | self._video_frame_rate = 1 / (self._sim_steps_per_frame * self._control_time_step) 74 | 75 | self._camera_angle = camera_angle 76 | self._view_matrix = self.compute_view_matrix(self._camera_angle) 77 | self._projection_matrix = compute_projection_matrix() 78 | self._sim_step_counter = None 79 | 80 | if self._gui_client_id is not None: 81 | gpu_support = 'CUDA' in os.environ['PATH'].upper() if 'PATH' in os.environ else False 82 | p.configureDebugVisualizer(p.COV_ENABLE_SHADOWS, 1, lightPosition=[-15, 0, 28], 83 | shadowMapResolution=16384 if gpu_support else 4096, 84 | physicsClientId=self._gui_client_id) 85 | # set shadowMapResolution to 4096 when running the code without a dedicated GPU and to 16384 otherwise 86 | 87 | def compute_view_matrix(self, camera_angle=0): 88 | if camera_angle == 0: 89 | cam_target_pos = (0, 0, 0) 90 | cam_dist = 1.75 91 | yaw = 90 92 | pitch = -70 93 | roll = 0 94 | 95 | elif camera_angle == 1: 96 | cam_target_pos = (-0.25, 0, 0) 97 | cam_dist = 1.95 98 | yaw = 90 99 | pitch = -40 100 | roll = 0 101 | 102 | elif camera_angle == 2: 103 | yaw = 59.59992599487305 104 | pitch = -49.400054931640625 105 | cam_dist = 2.000002861022949 106 | cam_target_pos = (0.0, 0.0, 0.0) 107 | roll = 0 108 | 109 | elif camera_angle == 3: 110 | yaw = 64.39994049072266 111 | pitch = -37.000003814697266 112 | cam_dist = 2.000002861022949 113 | cam_target_pos = (0.0, 0.0, 0.0) 114 | roll = 0 115 | 116 | elif camera_angle == 4: 117 | yaw = 69.59991455078125 118 | pitch = -33.8000602722168 119 | cam_dist = 1.8000028133392334 120 | cam_target_pos = (0.0, 0.0, 0.0) 121 | roll = 0 122 | 123 | elif camera_angle == 5: 124 | yaw = 90.800048828125 125 | pitch = -59.800079345703125 126 | cam_dist = 1.8000028133392334 127 | cam_target_pos = (0.0, 0.0, 0.0) 128 | roll = 0 129 | 130 | elif camera_angle == 6: 131 | yaw = 90.4000473022461 132 | pitch = -65.40008544921875 133 | cam_dist = 2.000002861022949 134 | cam_target_pos = (0.0, 0.0, 0.0) 135 | roll = 0 136 | 137 | elif camera_angle == 7: 138 | yaw = 90.00004577636719 139 | pitch = -45.4000358581543 140 | cam_dist = 2.000002861022949 141 | cam_target_pos = (0.0, 0.0, 0.0) 142 | roll = 0 143 | 144 | elif camera_angle == 8: 145 | yaw = 89.60002899169922 146 | pitch = -17.400007247924805 147 | cam_dist = 1.4000000953674316 148 | cam_target_pos = (-0.07712450623512268, 0.05323473736643791, 0.45070940256118774) 149 | roll = 0 150 | 151 | elif camera_angle == 9: 152 | yaw = 90 # 91.20002746582031 153 | pitch = -29.0 154 | cam_dist = 2.0000 155 | cam_target_pos = (0.0, -0.04, 0.58) 156 | roll = 0 157 | 158 | elif camera_angle == 10: 159 | yaw = 90 160 | pitch = -28.999988555908203 161 | cam_dist = 2.3000 162 | cam_target_pos = (0.000, 0, 0.5800000429153442) 163 | roll = 0 164 | 165 | else: 166 | raise ValueError("camera_angle {} is not defined".format(camera_angle)) 167 | 168 | return p.computeViewMatrixFromYawPitchRoll(cam_target_pos, cam_dist, yaw, pitch, roll, 2) 169 | 170 | def reset(self): 171 | observation = super().reset() 172 | 173 | self._sim_step_counter = 0 174 | 175 | if self._render_video: 176 | self._reset_video_recorder() 177 | 178 | return observation 179 | 180 | def close(self): 181 | if self._video_recorder: 182 | self._close_video_recorder() 183 | 184 | super().close() 185 | 186 | def _sim_step(self): 187 | super()._sim_step() 188 | self._sim_step_counter += 1 189 | 190 | if self._render_video: 191 | if self._sim_step_counter == self._sim_steps_per_frame: 192 | self._capture_frame_with_video_recorder() 193 | 194 | def _prepare_for_end_of_episode(self): 195 | super()._prepare_for_end_of_episode() 196 | 197 | if self._render_video: 198 | self._capture_frame_with_video_recorder(frames=int(self._video_frame_rate)) 199 | if self._video_recorder: 200 | self._close_video_recorder() 201 | 202 | def _capture_frame_with_video_recorder(self, frames=1): 203 | self._sim_step_counter = 0 204 | self._video_recorder.capture_frame() 205 | for _ in range(frames - 1): 206 | self._video_recorder._encode_image_frame(self._video_recorder.last_frame) 207 | 208 | @property 209 | def metadata(self): 210 | metadata = { 211 | 'render.modes': self._render_modes, 212 | 'video.frames_per_second': self._video_frame_rate 213 | } 214 | 215 | return metadata 216 | 217 | def _reset_video_recorder(self): 218 | if self._video_recorder: 219 | self._close_video_recorder() 220 | 221 | os.makedirs(self._video_dir, exist_ok=True) 222 | 223 | episode_id = self._episode_counter 224 | if self._fixed_video_filename: 225 | self._video_base_path = os.path.join(self._video_dir, "episode_{}".format(episode_id)) 226 | else: 227 | self._video_base_path = os.path.join(self._video_dir, "episode_{}_{}".format(episode_id, self.pid)) 228 | 229 | metadata = { 230 | 'video.frames_per_second': self._video_frame_rate, 231 | 'video.height': self._video_height, 232 | 'video.width': self._video_width, 233 | 'video.camera_angle': self._camera_angle, 234 | 'episode_id': episode_id, 235 | 'seed': self._fixed_seed 236 | } 237 | 238 | self._video_recorder = VideoRecorder(self, base_path=self._video_base_path, metadata=metadata, enabled=True) 239 | 240 | self._capture_frame_with_video_recorder(frames=int(self._video_frame_rate)) 241 | 242 | def render(self, mode="human"): 243 | if mode == "human": 244 | return np.array([]) 245 | else: 246 | physics_client_id = self._simulation_client_id if not self._switch_gui else self._obstacle_client_id 247 | (_, _, image, _, _) = p.getCameraImage(width=self._video_width, height=self._video_height, 248 | renderer=self._pybullet_renderer, 249 | viewMatrix=self._view_matrix, 250 | projectionMatrix=self._projection_matrix, 251 | shadow=0, lightDirection=[-20, -0.5, 150], 252 | flags=p.ER_NO_SEGMENTATION_MASK, 253 | physicsClientId=physics_client_id) 254 | image = np.reshape(image, (self._video_height, self._video_width, 4)) 255 | image = np.uint8(image[:, :, :3]) 256 | 257 | if self._use_target_points and self._video_height == 1440 and self._video_add_text: 258 | image = Image.fromarray(image) 259 | draw = ImageDraw.Draw(image) 260 | font = ImageFont.truetype("/usr/share/fonts/truetype/freefont/FreeSans", 72) 261 | total_number_of_target_points_origin = (400, 1380) 262 | x_diff = 135 263 | draw.text(total_number_of_target_points_origin, 264 | str(self._robot_scene.obstacle_wrapper.get_num_target_points_reached()), 265 | anchor='ms', fill=(0, 0, 0), font=font) 266 | for i in range(self._robot_scene.num_robots): 267 | sign = (-1) if i % 2 else 1 268 | multiplier = int(i / 2) + 1 269 | draw.text((total_number_of_target_points_origin[0] + sign * multiplier * x_diff, 270 | total_number_of_target_points_origin[1]), 271 | str(self._robot_scene.obstacle_wrapper.get_num_target_points_reached(robot=i)), 272 | anchor='ms', 273 | fill=tuple( 274 | (np.array(self._robot_scene.obstacle_wrapper.get_target_point_color(robot=i))[0:3] 275 | * 255).astype(int)), 276 | font=font) 277 | 278 | return np.array(image) 279 | 280 | def _close_video_recorder(self): 281 | self._video_recorder.close() 282 | self._video_recorder = [] 283 | if not self._fixed_video_filename: 284 | self._adapt_rendering_metadata() 285 | self._rename_output_files() 286 | 287 | def _adapt_rendering_metadata(self): 288 | metadata_ext = ".meta.json" 289 | metadata_file = self._video_base_path + metadata_ext 290 | 291 | with open(metadata_file, 'r') as f: 292 | metadata_json = json.load(f) 293 | 294 | encoder_metadata = metadata_json.pop('encoder_version', None) 295 | if encoder_metadata: 296 | metadata_json.update(encoder=encoder_metadata['backend']) 297 | 298 | metadata_json.update(trajectory_length=self._trajectory_manager.trajectory_length) 299 | metadata_json.update(episode_length=self._episode_length) 300 | metadata_json.update(total_reward=round(self._total_reward, 3)) 301 | 302 | with open(metadata_file, 'w') as f: 303 | f.write(json.dumps(metadata_json, indent=4)) 304 | f.close() 305 | 306 | def _rename_output_files(self): 307 | output_file = glob(self._video_base_path + ".*") 308 | 309 | for file in output_file: 310 | dir_path, file_name = os.path.split(file) 311 | name, extension = os.path.splitext(file_name) 312 | new_name = "_".join(map(str, [name, self._episode_length] + 313 | [self._robot_scene.obstacle_wrapper.get_num_target_points_reached(robot=robot) 314 | for robot in range(self._robot_scene.num_robots)] 315 | + [self._robot_scene.obstacle_wrapper.get_num_target_points_reached(), 316 | round(self._total_reward, 3)])) 317 | new_file_name = new_name + extension 318 | os.rename(file, os.path.join(dir_path, new_file_name)) 319 | -------------------------------------------------------------------------------- /safemotions/envs/safe_motions_env.py: -------------------------------------------------------------------------------- 1 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE 2 | # WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 3 | # COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR 4 | # OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 5 | 6 | import json 7 | import numpy as np 8 | import os 9 | import inspect 10 | import errno 11 | from collections import defaultdict 12 | from itertools import chain 13 | from safemotions.envs.decorators import actions, observations, rewards, video_recording 14 | 15 | project_dir = os.path.dirname(os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))) 16 | 17 | 18 | class SafeMotionsEnv(actions.AccelerationPredictionBoundedJerkAccVelPos, 19 | observations.SafeObservation, 20 | rewards.TargetPointReachingReward, 21 | video_recording.VideoRecordingManager): 22 | def __init__(self, 23 | *vargs, 24 | **kwargs): 25 | super().__init__(*vargs, **kwargs) 26 | 27 | def _process_action_outcome(self, base_info, action_info): 28 | reward, reward_info = self._get_reward() 29 | observation, observation_info = self._get_observation() 30 | done = self._check_termination() 31 | info = defaultdict(dict) 32 | 33 | for k, v in chain(base_info.items(), action_info.items(), observation_info.items(), reward_info.items()): 34 | info[k] = {**info[k], **v} 35 | 36 | return observation, reward, done, info 37 | 38 | def _process_end_of_episode(self, observation, reward, done, info): 39 | info.update(trajectory_length=self._trajectory_manager.trajectory_length) 40 | info.update(episode_length=self._episode_length) 41 | info['termination_reason'] = self._termination_reason 42 | 43 | # get info from obstacle wrapper 44 | obstacle_info = self._robot_scene.obstacle_wrapper.get_info_and_print_stats() 45 | info = dict(info, **obstacle_info) # concatenate dicts 46 | self._display_plot() 47 | self._save_plot(self.__class__.__name__, self._experiment_name) 48 | 49 | return observation, reward, done, info 50 | 51 | def _store_action_list(self): 52 | action_dict = {'actions': np.asarray(self._action_list).tolist()} 53 | eval_dir = os.path.join(self._evaluation_dir, "action_logs") 54 | 55 | if not os.path.exists(eval_dir): 56 | try: 57 | os.makedirs(eval_dir) 58 | except OSError as exc: 59 | if exc.errno != errno.EEXIST: 60 | raise 61 | 62 | with open(os.path.join(eval_dir, "episode_{}_{}.json".format(self._episode_counter, self.pid)), 'w') as f: 63 | f.write(json.dumps(action_dict)) 64 | f.flush() 65 | 66 | def _store_trajectory_data(self): 67 | trajectory_dict = {'actions': np.asarray(self._action_list).tolist(), 68 | 'trajectory_setpoints': self._to_list( 69 | self._trajectory_manager.generated_trajectory_control_points), 70 | 'trajectory_measured_actual_values': self._to_list( 71 | self._trajectory_manager.measured_actual_trajectory_control_points), 72 | 'trajectory_computed_actual_values': self._to_list( 73 | self._trajectory_manager.computed_actual_trajectory_control_points), 74 | } 75 | eval_dir = os.path.join(self._evaluation_dir, "trajectory_data") 76 | 77 | if not os.path.exists(eval_dir): 78 | try: 79 | os.makedirs(eval_dir) 80 | except OSError as exc: 81 | if exc.errno != errno.EEXIST: 82 | raise 83 | 84 | with open(os.path.join(eval_dir, "episode_{}_{}.json".format(self._episode_counter, self.pid)), 'w') as f: 85 | f.write(json.dumps(trajectory_dict)) 86 | f.flush() 87 | 88 | @staticmethod 89 | def _to_list(dictionary): 90 | for key, value in dictionary.items(): 91 | dictionary[key] = np.asarray(value).tolist() 92 | return dictionary 93 | -------------------------------------------------------------------------------- /safemotions/model/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/translearn/safeMotions/36a97fb0f71fa7f81543dd94cdb58578b6a1022d/safemotions/model/__init__.py -------------------------------------------------------------------------------- /safemotions/model/custom_action_dist.py: -------------------------------------------------------------------------------- 1 | # Code adapted from https://github.com/ray-project/ray/blob/master/rllib/models/tf/tf_action_dist.py 2 | # Copyright 2021 Ray Team 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | 7 | # https://www.apache.org/licenses/LICENSE-2.0 8 | 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import numpy as np 16 | import gym 17 | from math import log 18 | from ray.rllib.models.action_dist import ActionDistribution 19 | from ray.rllib.models.tf.tf_action_dist import TFActionDistribution 20 | from ray.rllib.models.modelv2 import ModelV2 21 | from ray.rllib.utils.annotations import override 22 | from ray.rllib.utils.framework import try_import_tf, try_import_tfp 23 | from ray.rllib.utils.typing import TensorType, List, Union, ModelConfigDict 24 | from ray.rllib.utils import SMALL_NUMBER 25 | 26 | tf1, tf, tfv = try_import_tf() 27 | tfp = try_import_tfp() 28 | 29 | 30 | class TruncatedNormal(TFActionDistribution): 31 | """Normal distribution that is truncated such that all values are within [low, high]. 32 | The distribution is defined by the mean and the std deviation of a normal distribution that is not truncated. 33 | (loc=mean -> first half of input, scale=exp(log_std) -> log_std = second half of input) 34 | KL corresponds to the KL of the underlying normal distributions 35 | """ 36 | 37 | def __init__(self, inputs: List[TensorType], model: ModelV2, low: float = -1.0, 38 | high: float = 1.0): 39 | mean_normal, log_std_normal = tf.split(inputs, 2, axis=1) 40 | self.mean_normal = mean_normal 41 | self.log_std_normal = log_std_normal 42 | self.std_normal = tf.exp(log_std_normal) 43 | self.low = low 44 | self.high = high 45 | self.zeros = tf.reduce_sum(tf.zeros_like(self.mean_normal), axis=1) 46 | self.dist = tfp.distributions.TruncatedNormal( 47 | loc=self.mean_normal, scale=self.std_normal, 48 | low=self.low, high=self.high) 49 | super().__init__(inputs, model) 50 | 51 | @override(ActionDistribution) 52 | def deterministic_sample(self) -> TensorType: 53 | return self.dist.mean() 54 | 55 | @override(ActionDistribution) 56 | def logp(self, x: TensorType) -> TensorType: 57 | return tf.math.reduce_sum(self.dist.log_prob(x), axis=-1) 58 | 59 | @override(ActionDistribution) 60 | def kl(self, other: ActionDistribution) -> TensorType: 61 | assert isinstance(other, TruncatedNormal) 62 | # return self.dist.cross_entropy(other.dist) - self.dist.entropy() -> kl_divergence not implemented 63 | # return tf.reduce_sum(self.dist.kl_divergence(other.dist), axis=1) -> kl_divergence not implemented 64 | # Return the kl_divergence of the underlying normal distributions since the correct kl_divergence is not 65 | # implemented 66 | return tf.reduce_sum( 67 | other.log_std_normal - self.log_std_normal + 68 | (tf.math.square(self.std_normal) + tf.math.square(self.mean_normal - other.mean_normal)) 69 | / (2.0 * tf.math.square(other.std_normal)) - 0.5, 70 | axis=1) 71 | 72 | @override(ActionDistribution) 73 | def entropy(self) -> TensorType: 74 | return tf.reduce_sum(self.dist.entropy(), axis=1) 75 | 76 | @override(TFActionDistribution) 77 | def _build_sample_op(self) -> TensorType: 78 | return self.dist.sample() 79 | 80 | @staticmethod 81 | @override(ActionDistribution) 82 | def required_model_output_shape( 83 | action_space: gym.Space, 84 | model_config: ModelConfigDict) -> Union[int, np.ndarray]: 85 | return np.prod(action_space.shape) * 2 86 | 87 | 88 | class TruncatedNormalZeroKL(TruncatedNormal): 89 | """Normal distribution that is truncated such that all values are within [low, high]. 90 | The distribution is defined by the mean and the std deviation of a normal distribution that is not truncated. 91 | (loc=mean -> first half of input, scale=exp(log_std) -> log_std = second half of input). 92 | KL is always set to zero. 93 | """ 94 | 95 | def __init__(self, inputs: List[TensorType], model: ModelV2, low: float = -1.0, 96 | high: float = 1.0): 97 | super().__init__(inputs, model, low, high) 98 | self.zeros = tf.reduce_sum(tf.zeros_like(self.mean_normal), axis=1) 99 | 100 | @override(TruncatedNormal) 101 | def kl(self, other: ActionDistribution) -> TensorType: 102 | assert isinstance(other, TruncatedNormal) 103 | return self.zeros 104 | 105 | 106 | class BetaBase(TFActionDistribution): 107 | """ 108 | A Beta distribution is defined on the interval [0, 1] and parameterized by 109 | shape parameters alpha and beta (also called concentration parameters). 110 | PDF(x; alpha, beta) = x**(alpha - 1) (1 - x)**(beta - 1) / Z 111 | with Z = Gamma(alpha) Gamma(beta) / Gamma(alpha + beta) 112 | and Gamma(n) = (n - 1)! 113 | """ 114 | 115 | def __init__(self, 116 | inputs: List[TensorType], 117 | model: ModelV2, 118 | low: float = -1.0, 119 | high: float = 1.0): 120 | 121 | self.dist = None 122 | self.low = low 123 | self.high = high 124 | 125 | @override(ActionDistribution) 126 | def deterministic_sample(self) -> TensorType: 127 | mean = self.dist.mean() 128 | return self._squash(mean) 129 | 130 | @override(TFActionDistribution) 131 | def _build_sample_op(self) -> TensorType: 132 | return self._squash(self.dist.sample()) 133 | 134 | @override(ActionDistribution) 135 | def entropy(self) -> TensorType: 136 | return tf.reduce_sum(self.dist.entropy(), axis=1) 137 | 138 | @override(ActionDistribution) 139 | def kl(self, other: ActionDistribution) -> TensorType: 140 | assert isinstance(other, BetaBase) 141 | return tf.reduce_sum(self.dist.kl_divergence(other.dist), axis=1) 142 | 143 | @override(ActionDistribution) 144 | def logp(self, x: TensorType) -> TensorType: 145 | unsquashed_values = self._unsquash(x) 146 | return tf.math.reduce_sum( 147 | self.dist.log_prob(unsquashed_values), axis=-1) 148 | 149 | def _squash(self, raw_values: TensorType) -> TensorType: 150 | return raw_values * (self.high - self.low) + self.low 151 | 152 | def _unsquash(self, values: TensorType) -> TensorType: 153 | return (values - self.low) / (self.high - self.low) 154 | 155 | @staticmethod 156 | @override(ActionDistribution) 157 | def required_model_output_shape( 158 | action_space: gym.Space, 159 | model_config: ModelConfigDict) -> Union[int, np.ndarray]: 160 | return np.prod(action_space.shape) * 2 161 | 162 | 163 | class BetaMeanTotal(BetaBase): 164 | """ 165 | A Beta distribution defined by the mean (squashed) and the log total concentration 166 | """ 167 | 168 | def __init__(self, 169 | inputs: List[TensorType], 170 | model: ModelV2, 171 | low: float = -1.0, 172 | high: float = 1.0): 173 | super().__init__(inputs, model, low, high) 174 | mean_squashed, log_total_concentration = tf.split(self.inputs, 2, axis=-1) 175 | log_total_concentration = tf.clip_by_value(log_total_concentration, log(SMALL_NUMBER), 176 | -log(SMALL_NUMBER)) 177 | # total_concentration > 0 178 | mean = self._unsquash(mean_squashed) 179 | total_concentration = tf.exp(log_total_concentration) 180 | alpha = mean * total_concentration 181 | beta = (1.0 - mean) * total_concentration 182 | self.dist = tfp.distributions.Beta( 183 | concentration1=alpha, concentration0=beta) 184 | super(BetaBase, self).__init__(inputs, model) 185 | 186 | 187 | class BetaAlphaBeta(BetaBase): 188 | """ 189 | A Beta distribution defined by alpha and beta with alpha, beta > 1 190 | """ 191 | 192 | def __init__(self, 193 | inputs: List[TensorType], 194 | model: ModelV2, 195 | low: float = -1.0, 196 | high: float = 1.0): 197 | inputs = tf.clip_by_value(inputs, log(SMALL_NUMBER), 198 | -log(SMALL_NUMBER)) 199 | inputs = tf.math.log(tf.math.exp(inputs) + 1.0) + 1.0 # ensures alpha > 1, beta > 1 200 | super().__init__(inputs, model, low, high) 201 | alpha, beta = tf.split(inputs, 2, axis=-1) 202 | 203 | self.dist = tfp.distributions.Beta( 204 | concentration1=alpha, concentration0=beta) 205 | super(BetaBase, self).__init__(inputs, model) 206 | 207 | -------------------------------------------------------------------------------- /safemotions/model/fcnet_v2_last_layer_activation.py: -------------------------------------------------------------------------------- 1 | # Code adapted from https://github.com/ray-project/ray/blob/master/rllib/models/tf/fcnet.py 2 | # Copyright 2021 Ray Team 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | 7 | # https://www.apache.org/licenses/LICENSE-2.0 8 | 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import numpy as np 16 | 17 | from ray.rllib.models.tf.tf_modelv2 import TFModelV2 18 | from ray.rllib.models.tf.misc import normc_initializer 19 | from ray.rllib.models.utils import get_activation_fn 20 | from ray.rllib.utils.framework import try_import_tf 21 | 22 | tf1, tf, tfv = try_import_tf() 23 | 24 | 25 | class FullyConnectedNetworkLastLayerActivation(TFModelV2): 26 | """Generic fully connected network implemented in ModelV2 API.""" 27 | 28 | def __init__(self, obs_space, action_space, num_outputs, model_config, 29 | name, last_layer_activation): 30 | super(FullyConnectedNetworkLastLayerActivation, self).__init__( 31 | obs_space, action_space, num_outputs, model_config, name) 32 | 33 | activation = get_activation_fn(model_config.get("fcnet_activation")) 34 | if last_layer_activation is not None: 35 | last_layer_activation = get_activation_fn(last_layer_activation) 36 | hiddens = model_config.get("fcnet_hiddens") 37 | no_final_linear = model_config.get("no_final_linear") 38 | vf_share_layers = model_config.get("vf_share_layers") 39 | 40 | # we are using obs_flat, so take the flattened shape as input 41 | inputs = tf.keras.layers.Input( 42 | shape=(np.product(obs_space.shape), ), name="observations") 43 | last_layer = inputs 44 | i = 1 45 | 46 | if no_final_linear: 47 | # the last layer is adjusted to be of size num_outputs 48 | for size in hiddens[:-1]: 49 | last_layer = tf.keras.layers.Dense( 50 | size, 51 | name="fc_{}".format(i), 52 | activation=activation, 53 | kernel_initializer=normc_initializer(1.0))(last_layer) 54 | i += 1 55 | layer_out = tf.keras.layers.Dense( 56 | num_outputs, 57 | name="fc_out", 58 | activation=activation, 59 | kernel_initializer=normc_initializer(1.0))(last_layer) 60 | else: 61 | # the last layer is a linear if last_layer_activation is None, else last_layer_activation 62 | for size in hiddens: 63 | last_layer = tf.keras.layers.Dense( 64 | size, 65 | name="fc_{}".format(i), 66 | activation=activation, 67 | kernel_initializer=normc_initializer(1.0))(last_layer) 68 | i += 1 69 | layer_out = tf.keras.layers.Dense( 70 | num_outputs, 71 | name="fc_out", 72 | activation=last_layer_activation, 73 | kernel_initializer=normc_initializer(0.01))(last_layer) 74 | 75 | if not vf_share_layers: 76 | # build a parallel set of hidden layers for the value net 77 | last_layer = inputs 78 | i = 1 79 | for size in hiddens: 80 | last_layer = tf.keras.layers.Dense( 81 | size, 82 | name="fc_value_{}".format(i), 83 | activation=activation, 84 | kernel_initializer=normc_initializer(1.0))(last_layer) 85 | i += 1 86 | 87 | value_out = tf.keras.layers.Dense( 88 | 1, 89 | name="value_out", 90 | activation=None, 91 | kernel_initializer=normc_initializer(0.01))(last_layer) 92 | 93 | self.base_model = tf.keras.Model(inputs, [layer_out, value_out]) 94 | # self.register_variables(self.base_model.variables) 95 | 96 | def forward(self, input_dict, state, seq_lens): 97 | model_out, self._value_out = self.base_model(input_dict["obs_flat"]) 98 | return model_out, state 99 | 100 | def value_function(self): 101 | return tf.reshape(self._value_out, [-1]) 102 | -------------------------------------------------------------------------------- /safemotions/model/keras_fcnet_last_layer_activation.py: -------------------------------------------------------------------------------- 1 | # Code adapted from https://github.com/ray-project/ray/blob/master/rllib/models/tf/fcnet.py 2 | # Copyright 2021 Ray Team 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | 7 | # https://www.apache.org/licenses/LICENSE-2.0 8 | 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import numpy as np 16 | import gym 17 | from typing import Dict, Optional, Sequence 18 | 19 | from ray.rllib.models.tf.misc import normc_initializer 20 | from ray.rllib.models.utils import get_activation_fn 21 | from ray.rllib.policy.sample_batch import SampleBatch 22 | from ray.rllib.utils.framework import try_import_tf 23 | from ray.rllib.utils.typing import TensorType, List 24 | 25 | tf1, tf, tfv = try_import_tf() 26 | 27 | 28 | class FullyConnectedNetworkLastLayerActivation(tf.keras.Model if tf else object): 29 | """Generic fully connected network implemented in tf Keras.""" 30 | 31 | def __init__( 32 | self, 33 | input_space: gym.spaces.Space, 34 | action_space: gym.spaces.Space, 35 | num_outputs: Optional[int] = None, 36 | *, 37 | name: str = "", 38 | fcnet_hiddens: Optional[Sequence[int]] = (), 39 | fcnet_activation: Optional[str] = None, 40 | post_fcnet_hiddens: Optional[Sequence[int]] = (), 41 | post_fcnet_activation: Optional[str] = None, 42 | no_final_linear: bool = False, 43 | vf_share_layers: bool = False, 44 | free_log_std: bool = False, 45 | last_layer_activation: str = None, 46 | no_log_std_activation: bool = False, 47 | log_std_range: Optional[Sequence[float]] = (), 48 | output_intermediate_layers: bool = False, 49 | **kwargs, 50 | ): 51 | super().__init__(name=name) 52 | 53 | hiddens = list(fcnet_hiddens or ()) + \ 54 | list(post_fcnet_hiddens or ()) 55 | activation = fcnet_activation 56 | if not fcnet_hiddens: 57 | activation = post_fcnet_activation 58 | activation = get_activation_fn(activation) 59 | 60 | if last_layer_activation is not None: 61 | last_layer_activation = get_activation_fn(last_layer_activation) 62 | 63 | # Generate free-floating bias variables for the second half of 64 | # the outputs. 65 | if free_log_std: 66 | assert num_outputs % 2 == 0, ( 67 | "num_outputs must be divisible by two", num_outputs) 68 | num_outputs = num_outputs // 2 69 | self.log_std_var = tf.Variable( 70 | [0.0] * num_outputs, dtype=tf.float32, name="log_std") 71 | 72 | # We are using obs_flat, so take the flattened shape as input. 73 | inputs = tf.keras.layers.Input( 74 | shape=(int(np.product(input_space.shape)), ), name="observations") 75 | # Last hidden layer output (before logits outputs). 76 | last_layer = inputs 77 | # The action distribution outputs. 78 | logits_out = None 79 | logits_intermediate_layers = [] 80 | self._log_std_range = log_std_range 81 | self._output_intermediate_layers = output_intermediate_layers 82 | self._intermediate_layer_names = [] 83 | i = 1 84 | 85 | # Create layers 0 to second-last. 86 | for size in hiddens[:-1]: 87 | name = "fc_{}".format(i) 88 | last_layer = tf.keras.layers.Dense( 89 | size, 90 | name=name, 91 | activation=activation, 92 | kernel_initializer=normc_initializer(1.0))(last_layer) 93 | self._intermediate_layer_names.append(name) 94 | logits_intermediate_layers.append(last_layer) 95 | i += 1 96 | 97 | # The last layer is adjusted to be of size num_outputs, but it's a 98 | # layer with activation. 99 | if no_final_linear and num_outputs: 100 | logits_out = tf.keras.layers.Dense( 101 | num_outputs, 102 | name="fc_out", 103 | activation=activation, 104 | kernel_initializer=normc_initializer(1.0))(last_layer) 105 | # Finish the layers with the provided sizes (`hiddens`), plus - 106 | # iff num_outputs > 0 - a last linear layer of size num_outputs. 107 | else: 108 | if len(hiddens) > 0: 109 | name = "fc_{}".format(i) 110 | last_layer = tf.keras.layers.Dense( 111 | hiddens[-1], 112 | name=name, 113 | activation=activation, 114 | kernel_initializer=normc_initializer(1.0))(last_layer) 115 | logits_intermediate_layers.append(last_layer) 116 | self._intermediate_layer_names.append(name) 117 | if num_outputs: 118 | if no_log_std_activation and not vf_share_layers: 119 | actions_out = tf.keras.layers.Dense( 120 | num_outputs // 2, 121 | name="fc_out_actions", 122 | activation=last_layer_activation, 123 | kernel_initializer=normc_initializer(0.01))(last_layer) 124 | log_std_out = tf.keras.layers.Dense( 125 | num_outputs // 2, 126 | name="fc_out_log_std", 127 | activation=None, 128 | kernel_initializer=normc_initializer(0.01))(last_layer) 129 | logits_out = tf.keras.layers.Concatenate(axis=1)([actions_out, log_std_out]) 130 | else: 131 | logits_out = tf.keras.layers.Dense( 132 | num_outputs, 133 | name="fc_out", 134 | activation=last_layer_activation, 135 | kernel_initializer=normc_initializer(0.01))(last_layer) 136 | 137 | # Concat the log std vars to the end of the state-dependent means. 138 | if free_log_std and logits_out is not None: 139 | 140 | def tiled_log_std(x): 141 | return tf.tile( 142 | tf.expand_dims(self.log_std_var, 0), [tf.shape(x)[0], 1]) 143 | 144 | log_std_out = tf.keras.layers.Lambda(tiled_log_std)(inputs) 145 | logits_out = tf.keras.layers.Concatenate(axis=1)( 146 | [logits_out, log_std_out]) 147 | 148 | last_vf_layer = None 149 | vf_intermediate_layers = [] 150 | if not vf_share_layers: 151 | # Build a parallel set of hidden layers for the value net. 152 | last_vf_layer = inputs 153 | i = 1 154 | for size in hiddens: 155 | name = "fc_value_{}".format(i) 156 | last_vf_layer = tf.keras.layers.Dense( 157 | size, 158 | name=name, 159 | activation=activation, 160 | kernel_initializer=normc_initializer(1.0))(last_vf_layer) 161 | i += 1 162 | vf_intermediate_layers.append(last_vf_layer) 163 | self._intermediate_layer_names.append(name) 164 | 165 | value_out = tf.keras.layers.Dense( 166 | 1, 167 | name="value_out", 168 | activation=None, 169 | kernel_initializer=normc_initializer(0.01))( 170 | last_vf_layer if last_vf_layer is not None else last_layer) 171 | 172 | if not self._output_intermediate_layers: 173 | self.base_model = tf.keras.Model( 174 | inputs, [(logits_out 175 | if logits_out is not None else last_layer), value_out]) 176 | else: 177 | self.base_model = tf.keras.Model( 178 | inputs, [logits_layer for logits_layer in logits_intermediate_layers] + 179 | [vf_layer for vf_layer in vf_intermediate_layers] + 180 | [(logits_out if logits_out is not None else last_layer), value_out]) 181 | 182 | def call(self, input_dict: SampleBatch) -> \ 183 | (TensorType, List[TensorType], Dict[str, TensorType]): 184 | model_out = self.base_model(input_dict[SampleBatch.OBS]) 185 | logits_out = model_out[-2] 186 | if self._log_std_range: 187 | mean, log_std = tf.split(logits_out, 2, axis=1) 188 | log_std = self._log_std_range[0] + 0.5 * (log_std + 1) * (self._log_std_range[1] - 189 | self._log_std_range[0]) 190 | logits_out = tf.concat([mean, log_std], axis=1) 191 | value_out = model_out[-1] 192 | extra_outs = {SampleBatch.VF_PREDS: tf.reshape(value_out, [-1])} 193 | if self._output_intermediate_layers: 194 | for i in range(len(model_out) - 2): 195 | extra_outs[self._intermediate_layer_names[i]] = model_out[i] 196 | extra_outs['logits'] = logits_out 197 | return logits_out, [], extra_outs 198 | -------------------------------------------------------------------------------- /safemotions/robot_scene/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/translearn/safeMotions/36a97fb0f71fa7f81543dd94cdb58578b6a1022d/safemotions/robot_scene/__init__.py -------------------------------------------------------------------------------- /safemotions/robot_scene/real_robot_scene.py: -------------------------------------------------------------------------------- 1 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE 2 | # WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 3 | # COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR 4 | # OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 5 | 6 | from safemotions.robot_scene.robot_scene_base import RobotSceneBase 7 | import pybullet as p 8 | from abc import abstractmethod 9 | 10 | 11 | class RealRobotScene(RobotSceneBase): 12 | 13 | def __init__(self, 14 | *vargs, 15 | real_robot_debug_mode=False, 16 | **kwargs): 17 | 18 | super().__init__(**kwargs) 19 | 20 | self._real_robot_debug_mode = real_robot_debug_mode 21 | 22 | if not self._real_robot_debug_mode: 23 | print("Waiting for real robot ...") 24 | self._connect_to_real_robot() 25 | print("Connected to real robot") 26 | else: 27 | print("Real robot debug mode: Commands are not send to the real robot") 28 | 29 | @abstractmethod 30 | def _connect_to_real_robot(self): 31 | raise NotImplementedError() 32 | 33 | def pose_manipulator(self, joint_positions, **kwargs): 34 | 35 | if self._simulation_client_id is not None: 36 | # set the simulation client to the desired starting position 37 | for i in range(len(self._manip_joint_indices)): 38 | p.resetJointState(bodyUniqueId=self._robot_id, 39 | jointIndex=self._manip_joint_indices[i], 40 | targetValue=joint_positions[i], 41 | targetVelocity=0, 42 | physicsClientId=self._simulation_client_id) 43 | 44 | go_on = "" 45 | while go_on != "YES": 46 | go_on = input("Type in 'yes' to move to the starting position").upper() 47 | 48 | if not self._real_robot_debug_mode: 49 | success = self._move_to_joint_position(joint_positions) 50 | else: 51 | success = True 52 | 53 | return success 54 | 55 | @abstractmethod 56 | def _move_to_joint_position(self, joint_positions): 57 | raise NotImplementedError() 58 | 59 | def set_motor_control(self, target_positions, physics_client_id=None, computed_position_is=None, 60 | computed_velocity_is=None, **kwargs): 61 | if physics_client_id is None: 62 | if not self._real_robot_debug_mode: 63 | self.send_command_to_trajectory_controller(target_positions, **kwargs) 64 | if self._simulation_client_id is not None and \ 65 | computed_position_is is not None \ 66 | and computed_velocity_is is not None: 67 | for i in range(len(self._manip_joint_indices)): 68 | p.resetJointState(bodyUniqueId=self._robot_id, 69 | jointIndex=self._manip_joint_indices[i], 70 | targetValue=computed_position_is[i], 71 | targetVelocity=computed_velocity_is[i], 72 | physicsClientId=self._simulation_client_id) 73 | else: 74 | super().set_motor_control(target_positions, physics_client_id=physics_client_id, **kwargs) 75 | 76 | @staticmethod 77 | def send_command_to_trajectory_controller(target_positions, **kwargs): 78 | raise NotImplementedError() 79 | 80 | @abstractmethod 81 | def _read_actual_torques(self): 82 | raise NotImplementedError() 83 | 84 | def get_actual_joint_torques(self, physics_client_id=None, **kwargs): 85 | if physics_client_id is None: 86 | return self._read_actual_torques() 87 | else: 88 | return super().get_actual_joint_torques(physics_client_id=physics_client_id, **kwargs) 89 | 90 | @abstractmethod 91 | def get_actual_joint_position_and_velocity(self, manip_joint_indices): 92 | raise NotImplementedError() 93 | 94 | def disconnect(self): 95 | super().disconnect() 96 | 97 | def prepare_for_end_of_episode(self): 98 | pass 99 | 100 | def prepare_for_start_of_episode(self): 101 | input("Press key to start the online trajectory generation") 102 | 103 | 104 | -------------------------------------------------------------------------------- /safemotions/robot_scene/simulated_robot_scene.py: -------------------------------------------------------------------------------- 1 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE 2 | # WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 3 | # COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR 4 | # OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 5 | 6 | import numpy as np 7 | import pybullet as p 8 | from safemotions.robot_scene.robot_scene_base import RobotSceneBase 9 | 10 | 11 | class SimRobotScene(RobotSceneBase): 12 | def __init__(self, 13 | **kwargs): 14 | 15 | super().__init__(**kwargs) 16 | 17 | def pose_manipulator(self, joint_positions): 18 | for i in range(self._num_manip_joints): 19 | p.resetJointState(self._robot_id, 20 | jointIndex=self._manip_joint_indices[i], 21 | targetValue=joint_positions[i], 22 | targetVelocity=0, 23 | physicsClientId=self._simulation_client_id) 24 | 25 | def get_actual_joint_position_and_velocity(self, manip_joint_indices=None, physics_client_id=0): 26 | if manip_joint_indices is None: 27 | manip_joint_indices = self._manip_joint_indices 28 | # return the actual joint position and velocity for the specified joint indices from the physicsClient 29 | joint_states = p.getJointStates(self._robot_id, manip_joint_indices, 30 | physicsClientId=physics_client_id) 31 | 32 | joint_states_swap = np.swapaxes(np.array(joint_states, dtype=object), 0, 1) 33 | 34 | return joint_states_swap[0], joint_states_swap[1] 35 | 36 | @staticmethod 37 | def send_command_to_trajectory_controller(target_positions, **kwargs): 38 | pass 39 | -------------------------------------------------------------------------------- /safemotions/trained_networks/humanoid/armar6/collision/alternating/checkpoint/.is_checkpoint: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/translearn/safeMotions/36a97fb0f71fa7f81543dd94cdb58578b6a1022d/safemotions/trained_networks/humanoid/armar6/collision/alternating/checkpoint/.is_checkpoint -------------------------------------------------------------------------------- /safemotions/trained_networks/humanoid/armar6/collision/alternating/checkpoint/checkpoint: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/translearn/safeMotions/36a97fb0f71fa7f81543dd94cdb58578b6a1022d/safemotions/trained_networks/humanoid/armar6/collision/alternating/checkpoint/checkpoint -------------------------------------------------------------------------------- /safemotions/trained_networks/humanoid/armar6/collision/alternating/checkpoint/checkpoint.tune_metadata: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/translearn/safeMotions/36a97fb0f71fa7f81543dd94cdb58578b6a1022d/safemotions/trained_networks/humanoid/armar6/collision/alternating/checkpoint/checkpoint.tune_metadata -------------------------------------------------------------------------------- /safemotions/trained_networks/humanoid/armar6/collision/alternating/params.json: -------------------------------------------------------------------------------- 1 | { 2 | "batch_mode": "complete_episodes", 3 | "callbacks": "", 4 | "clip_param": 0.3, 5 | "entropy_coeff": 0.0, 6 | "env": "SafeMotionsEnv", 7 | "env_config": { 8 | "acc_limit_factor": 1.0, 9 | "acc_limit_factor_braking": 1.0, 10 | "action_max_punishment": 0.4, 11 | "action_punishment_min_threshold": 0.95, 12 | "braking_trajectory_max_punishment": 0.5, 13 | "braking_trajectory_max_torque_min_threshold": 0.8, 14 | "braking_trajectory_min_distance_max_threshold": 0.05, 15 | "check_braking_trajectory_collisions": true, 16 | "check_braking_trajectory_torque_limits": false, 17 | "closest_point_safety_distance": 0.01, 18 | "collision_check_time": 0.033, 19 | "experiment_name": "humanoid/armar6/collision/alternating", 20 | "jerk_limit_factor": 1.0, 21 | "klimits_version": "1.1.1", 22 | "jerk_limit_factor_braking": 1.0, 23 | "log_obstacle_data": false, 24 | "logging_level": "WARNING", 25 | "m_prev": 0, 26 | "normalize_reward_to_frequency": false, 27 | "normalize_reward_to_initial_target_point_distance": true, 28 | "obs_add_target_point_pos": true, 29 | "obs_add_target_point_relative_pos": true, 30 | "obstacle_scene": 5, 31 | "obstacle_use_computed_actual_values": false, 32 | "online_trajectory_duration": 8.0, 33 | "online_trajectory_time_step": 0.1, 34 | "pos_limit_factor": 1.0, 35 | "punish_action": true, 36 | "punish_braking_trajectory_max_torque": false, 37 | "punish_braking_trajectory_min_distance": true, 38 | "ray_version": "1.4.1", 39 | "robot_scene": 3, 40 | "solver_iterations": 50, 41 | "target_link_offset": [ 42 | 0.03, 43 | 0, 44 | 0.135 45 | ], 46 | "target_point_cartesian_range_scene": 5, 47 | "target_point_radius": 0.065, 48 | "target_point_reached_reward_bonus": 5.0, 49 | "target_point_relative_pos_scene": 1, 50 | "target_point_reward_factor": 1.0, 51 | "target_point_sequence": 1, 52 | "target_point_use_actual_position": false, 53 | "torque_limit_factor": 1.0, 54 | "use_target_points": true, 55 | "vel_limit_factor": 1.0 56 | }, 57 | "evaluation_config": { 58 | "explore": false, 59 | "rollout_fragment_length": 1 60 | }, 61 | "evaluation_interval": 50, 62 | "evaluation_num_episodes": 624, 63 | "evaluation_num_workers": 0, 64 | "evaluation_parallel_to_training": false, 65 | "gamma": 0.99, 66 | "kl_coeff": 0.2, 67 | "kl_target": 0.01, 68 | "lambda": 1.0, 69 | "lr": 5e-05, 70 | "lr_schedule": null, 71 | "model": { 72 | "conv_filters": null, 73 | "custom_model": "keras_fcnet_last_layer_activation", 74 | "custom_model_config": { 75 | "fcnet_activation": "swish", 76 | "fcnet_hiddens": [ 77 | 256, 78 | 128 79 | ], 80 | "last_layer_activation": "tanh", 81 | "no_log_std_activation": false 82 | }, 83 | "fcnet_activation": "swish", 84 | "fcnet_hiddens": [ 85 | 256, 86 | 128 87 | ], 88 | "use_lstm": false 89 | }, 90 | "normalize_actions": true, 91 | "num_gpus": 1, 92 | "num_sgd_iter": 16, 93 | "num_workers": 12, 94 | "rollout_fragment_length": 4160, 95 | "sgd_minibatch_size": 1024, 96 | "train_batch_size": 49920, 97 | "use_gae": true, 98 | "vf_clip_param": 10.0, 99 | "vf_loss_coeff": 1.0 100 | } 101 | -------------------------------------------------------------------------------- /safemotions/trained_networks/humanoid/armar6/collision/simultaneous/checkpoint/.is_checkpoint: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/translearn/safeMotions/36a97fb0f71fa7f81543dd94cdb58578b6a1022d/safemotions/trained_networks/humanoid/armar6/collision/simultaneous/checkpoint/.is_checkpoint -------------------------------------------------------------------------------- /safemotions/trained_networks/humanoid/armar6/collision/simultaneous/checkpoint/checkpoint: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/translearn/safeMotions/36a97fb0f71fa7f81543dd94cdb58578b6a1022d/safemotions/trained_networks/humanoid/armar6/collision/simultaneous/checkpoint/checkpoint -------------------------------------------------------------------------------- /safemotions/trained_networks/humanoid/armar6/collision/simultaneous/checkpoint/checkpoint.tune_metadata: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/translearn/safeMotions/36a97fb0f71fa7f81543dd94cdb58578b6a1022d/safemotions/trained_networks/humanoid/armar6/collision/simultaneous/checkpoint/checkpoint.tune_metadata -------------------------------------------------------------------------------- /safemotions/trained_networks/humanoid/armar6/collision/simultaneous/params.json: -------------------------------------------------------------------------------- 1 | { 2 | "batch_mode": "complete_episodes", 3 | "callbacks": "", 4 | "clip_param": 0.3, 5 | "entropy_coeff": 0.0, 6 | "env": "SafeMotionsEnv", 7 | "env_config": { 8 | "acc_limit_factor": 1.0, 9 | "acc_limit_factor_braking": 1.0, 10 | "action_max_punishment": 0.4, 11 | "action_punishment_min_threshold": 0.95, 12 | "activate_obstacle_collisions": false, 13 | "adaptation_max_punishment": 1.0, 14 | "braking_trajectory_max_punishment": 0.5, 15 | "braking_trajectory_max_torque_min_threshold": 0.8, 16 | "braking_trajectory_min_distance_max_threshold": 0.05, 17 | "check_braking_trajectory_collisions": true, 18 | "check_braking_trajectory_torque_limits": false, 19 | "closest_point_safety_distance": 0.01, 20 | "collision_check_time": 0.033, 21 | "end_max_torque_max_punishment": 1.0, 22 | "end_max_torque_min_threshold": 0.9, 23 | "end_min_distance_max_punishment": 0.5, 24 | "end_min_distance_max_threshold": 0.05, 25 | "experiment_name": "humanoid/armar6/collision/simultaneous", 26 | "jerk_limit_factor": 1.0, 27 | "jerk_limit_factor_braking": 1.0, 28 | "klimits_version": "1.1.1", 29 | "log_obstacle_data": false, 30 | "logging_level": "WARNING", 31 | "m_prev": 0, 32 | "no_self_collision": false, 33 | "normalize_reward_to_frequency": false, 34 | "normalize_reward_to_initial_target_point_distance": true, 35 | "obs_add_target_point_pos": true, 36 | "obs_add_target_point_relative_pos": true, 37 | "obstacle_scene": 5, 38 | "obstacle_use_computed_actual_values": false, 39 | "online_trajectory_duration": 8.0, 40 | "online_trajectory_time_step": 0.1, 41 | "pos_limit_factor": 1.0, 42 | "punish_action": true, 43 | "punish_adaptation": false, 44 | "punish_braking_trajectory_max_torque": false, 45 | "punish_braking_trajectory_min_distance": false, 46 | "punish_end_max_torque": false, 47 | "punish_end_min_distance": true, 48 | "ray_version": "1.4.1", 49 | "robot_scene": 3, 50 | "solver_iterations": 50, 51 | "target_link_offset": [ 52 | 0.03, 53 | 0, 54 | 0.135 55 | ], 56 | "target_point_cartesian_range_scene": 5, 57 | "target_point_radius": 0.065, 58 | "target_point_reached_reward_bonus": 5.0, 59 | "target_point_relative_pos_scene": 1, 60 | "target_point_reward_factor": 1.0, 61 | "target_point_sequence": 0, 62 | "target_point_use_actual_position": false, 63 | "torque_limit_factor": 1.0, 64 | "use_target_points": true, 65 | "vel_limit_factor": 1.0 66 | }, 67 | "evaluation_config": { 68 | "explore": false, 69 | "rollout_fragment_length": 1 70 | }, 71 | "evaluation_interval": 50, 72 | "evaluation_num_episodes": 624, 73 | "evaluation_num_workers": 0, 74 | "evaluation_parallel_to_training": false, 75 | "gamma": 0.99, 76 | "kl_coeff": 0.2, 77 | "kl_target": 0.01, 78 | "lambda": 1.0, 79 | "lr": 5e-05, 80 | "lr_schedule": null, 81 | "model": { 82 | "conv_filters": null, 83 | "custom_model": "keras_fcnet_last_layer_activation", 84 | "custom_model_config": { 85 | "fcnet_activation": "swish", 86 | "fcnet_hiddens": [ 87 | 256, 88 | 128 89 | ], 90 | "last_layer_activation": "tanh", 91 | "no_log_std_activation": false 92 | }, 93 | "fcnet_activation": "swish", 94 | "fcnet_hiddens": [ 95 | 256, 96 | 128 97 | ], 98 | "use_lstm": false 99 | }, 100 | "normalize_actions": true, 101 | "num_gpus": 1, 102 | "num_sgd_iter": 16, 103 | "num_workers": 12, 104 | "rollout_fragment_length": 4160, 105 | "sgd_minibatch_size": 1024, 106 | "train_batch_size": 49920, 107 | "use_gae": true, 108 | "vf_clip_param": 10.0, 109 | "vf_loss_coeff": 1.0 110 | } -------------------------------------------------------------------------------- /safemotions/trained_networks/humanoid/armar6/collision/single/checkpoint/.is_checkpoint: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/translearn/safeMotions/36a97fb0f71fa7f81543dd94cdb58578b6a1022d/safemotions/trained_networks/humanoid/armar6/collision/single/checkpoint/.is_checkpoint -------------------------------------------------------------------------------- /safemotions/trained_networks/humanoid/armar6/collision/single/checkpoint/checkpoint: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/translearn/safeMotions/36a97fb0f71fa7f81543dd94cdb58578b6a1022d/safemotions/trained_networks/humanoid/armar6/collision/single/checkpoint/checkpoint -------------------------------------------------------------------------------- /safemotions/trained_networks/humanoid/armar6/collision/single/checkpoint/checkpoint.tune_metadata: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/translearn/safeMotions/36a97fb0f71fa7f81543dd94cdb58578b6a1022d/safemotions/trained_networks/humanoid/armar6/collision/single/checkpoint/checkpoint.tune_metadata -------------------------------------------------------------------------------- /safemotions/trained_networks/humanoid/armar6/collision/single/params.json: -------------------------------------------------------------------------------- 1 | { 2 | "batch_mode": "complete_episodes", 3 | "callbacks": "", 4 | "clip_param": 0.3, 5 | "entropy_coeff": 0.0, 6 | "env": "SafeMotionsEnv", 7 | "env_config": { 8 | "acc_limit_factor": 1.0, 9 | "acc_limit_factor_braking": 1.0, 10 | "action_max_punishment": 0.4, 11 | "action_punishment_min_threshold": 0.95, 12 | "braking_trajectory_max_punishment": 0.5, 13 | "braking_trajectory_max_torque_min_threshold": 0.8, 14 | "braking_trajectory_min_distance_max_threshold": 0.05, 15 | "check_braking_trajectory_collisions": true, 16 | "check_braking_trajectory_torque_limits": false, 17 | "closest_point_safety_distance": 0.01, 18 | "collision_check_time": 0.033, 19 | "experiment_name": "humanoid/armar6/collision/single", 20 | "jerk_limit_factor": 1.0, 21 | "klimits_version": "1.1.1", 22 | "jerk_limit_factor_braking": 1.0, 23 | "log_obstacle_data": false, 24 | "logging_level": "WARNING", 25 | "m_prev": 0, 26 | "normalize_reward_to_frequency": false, 27 | "normalize_reward_to_initial_target_point_distance": true, 28 | "obs_add_target_point_pos": true, 29 | "obs_add_target_point_relative_pos": true, 30 | "obstacle_scene": 5, 31 | "obstacle_use_computed_actual_values": false, 32 | "online_trajectory_duration": 8.0, 33 | "online_trajectory_time_step": 0.1, 34 | "pos_limit_factor": 1.0, 35 | "punish_action": true, 36 | "punish_braking_trajectory_max_torque": false, 37 | "punish_braking_trajectory_min_distance": true, 38 | "ray_version": "1.4.1", 39 | "robot_scene": 3, 40 | "solver_iterations": 50, 41 | "target_link_offset": [ 42 | 0.03, 43 | 0, 44 | 0.135 45 | ], 46 | "target_point_cartesian_range_scene": 5, 47 | "target_point_radius": 0.065, 48 | "target_point_reached_reward_bonus": 5.0, 49 | "target_point_relative_pos_scene": 1, 50 | "target_point_reward_factor": 1.0, 51 | "target_point_sequence": 2, 52 | "target_point_use_actual_position": false, 53 | "torque_limit_factor": 1.0, 54 | "use_target_points": true, 55 | "vel_limit_factor": 1.0 56 | }, 57 | "evaluation_config": { 58 | "explore": false, 59 | "rollout_fragment_length": 1 60 | }, 61 | "evaluation_interval": 50, 62 | "evaluation_num_episodes": 624, 63 | "evaluation_num_workers": 0, 64 | "evaluation_parallel_to_training": false, 65 | "gamma": 0.99, 66 | "kl_coeff": 0.2, 67 | "kl_target": 0.01, 68 | "lambda": 1.0, 69 | "lr": 5e-05, 70 | "lr_schedule": null, 71 | "model": { 72 | "conv_filters": null, 73 | "custom_model": "keras_fcnet_last_layer_activation", 74 | "custom_model_config": { 75 | "fcnet_activation": "swish", 76 | "fcnet_hiddens": [ 77 | 256, 78 | 128 79 | ], 80 | "last_layer_activation": "tanh", 81 | "no_log_std_activation": false 82 | }, 83 | "fcnet_activation": "swish", 84 | "fcnet_hiddens": [ 85 | 256, 86 | 128 87 | ], 88 | "use_lstm": false 89 | }, 90 | "normalize_actions": true, 91 | "num_gpus": 1, 92 | "num_sgd_iter": 16, 93 | "num_workers": 12, 94 | "rollout_fragment_length": 4160, 95 | "sgd_minibatch_size": 1024, 96 | "train_batch_size": 49920, 97 | "use_gae": true, 98 | "vf_clip_param": 10.0, 99 | "vf_loss_coeff": 1.0 100 | } 101 | -------------------------------------------------------------------------------- /safemotions/trained_networks/humanoid/armar6_x4/collision/alternating/checkpoint/.is_checkpoint: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/translearn/safeMotions/36a97fb0f71fa7f81543dd94cdb58578b6a1022d/safemotions/trained_networks/humanoid/armar6_x4/collision/alternating/checkpoint/.is_checkpoint -------------------------------------------------------------------------------- /safemotions/trained_networks/humanoid/armar6_x4/collision/alternating/checkpoint/checkpoint: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/translearn/safeMotions/36a97fb0f71fa7f81543dd94cdb58578b6a1022d/safemotions/trained_networks/humanoid/armar6_x4/collision/alternating/checkpoint/checkpoint -------------------------------------------------------------------------------- /safemotions/trained_networks/humanoid/armar6_x4/collision/alternating/checkpoint/checkpoint.tune_metadata: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/translearn/safeMotions/36a97fb0f71fa7f81543dd94cdb58578b6a1022d/safemotions/trained_networks/humanoid/armar6_x4/collision/alternating/checkpoint/checkpoint.tune_metadata -------------------------------------------------------------------------------- /safemotions/trained_networks/humanoid/armar6_x4/collision/alternating/params.json: -------------------------------------------------------------------------------- 1 | { 2 | "batch_mode": "complete_episodes", 3 | "callbacks": "", 4 | "clip_param": 0.3, 5 | "entropy_coeff": 0.0, 6 | "env": "SafeMotionsEnv", 7 | "env_config": { 8 | "acc_limit_factor": 1.0, 9 | "acc_limit_factor_braking": 1.0, 10 | "action_max_punishment": 0.4, 11 | "action_punishment_min_threshold": 0.95, 12 | "braking_trajectory_max_punishment": 0.5, 13 | "braking_trajectory_max_torque_min_threshold": 0.8, 14 | "braking_trajectory_min_distance_max_threshold": 0.05, 15 | "check_braking_trajectory_collisions": true, 16 | "check_braking_trajectory_torque_limits": false, 17 | "closest_point_safety_distance": 0.01, 18 | "collision_check_time": 0.05, 19 | "experiment_name": "humanoid/armar6_x4/collision/alternating", 20 | "jerk_limit_factor": 1.0, 21 | "jerk_limit_factor_braking": 1.0, 22 | "klimits_version": "1.1.1", 23 | "log_obstacle_data": false, 24 | "m_prev": 0, 25 | "normalize_reward_to_frequency": false, 26 | "normalize_reward_to_initial_target_point_distance": true, 27 | "obs_add_target_point_pos": true, 28 | "obs_add_target_point_relative_pos": true, 29 | "obstacle_scene": 5, 30 | "obstacle_use_computed_actual_values": false, 31 | "online_trajectory_duration": 8.0, 32 | "online_trajectory_time_step": 0.1, 33 | "pos_limit_factor": 1.0, 34 | "punish_action": true, 35 | "punish_braking_trajectory_max_torque": false, 36 | "punish_braking_trajectory_min_distance": true, 37 | "ray_version": "1.4.1", 38 | "robot_scene": 5, 39 | "solver_iterations": 50, 40 | "target_link_offset": [ 41 | 0.03, 42 | 0, 43 | 0.135 44 | ], 45 | "target_point_cartesian_range_scene": 6, 46 | "target_point_radius": 0.065, 47 | "target_point_reached_reward_bonus": 5.0, 48 | "target_point_reward_factor": 1.0, 49 | "target_point_sequence": 1, 50 | "target_point_use_actual_position": false, 51 | "torque_limit_factor": 1.0, 52 | "use_target_points": true, 53 | "vel_limit_factor": 1.0 54 | }, 55 | "evaluation_config": { 56 | "explore": false, 57 | "rollout_fragment_length": 1 58 | }, 59 | "evaluation_interval": null, 60 | "evaluation_num_episodes": 624, 61 | "evaluation_num_workers": 0, 62 | "evaluation_parallel_to_training": false, 63 | "gamma": 0.99, 64 | "kl_coeff": 0.2, 65 | "kl_target": 0.01, 66 | "lambda": 1.0, 67 | "log_level": "WARNING", 68 | "lr": 5e-05, 69 | "lr_schedule": null, 70 | "model": { 71 | "conv_filters": null, 72 | "custom_model": "keras_fcnet_last_layer_activation", 73 | "custom_model_config": { 74 | "fcnet_activation": "swish", 75 | "fcnet_hiddens": [ 76 | 256, 77 | 128 78 | ], 79 | "last_layer_activation": "tanh", 80 | "no_log_std_activation": false 81 | }, 82 | "fcnet_activation": "swish", 83 | "fcnet_hiddens": [ 84 | 256, 85 | 128 86 | ], 87 | "use_lstm": false 88 | }, 89 | "normalize_actions": true, 90 | "num_gpus": 1, 91 | "num_sgd_iter": 16, 92 | "num_workers": 12, 93 | "rollout_fragment_length": 4160, 94 | "sgd_minibatch_size": 1024, 95 | "train_batch_size": 49920, 96 | "use_gae": true, 97 | "vf_clip_param": 10.0, 98 | "vf_loss_coeff": 1.0 99 | } 100 | -------------------------------------------------------------------------------- /safemotions/trained_networks/humanoid/armar6_x4/collision/simultaneous/checkpoint/.is_checkpoint: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/translearn/safeMotions/36a97fb0f71fa7f81543dd94cdb58578b6a1022d/safemotions/trained_networks/humanoid/armar6_x4/collision/simultaneous/checkpoint/.is_checkpoint -------------------------------------------------------------------------------- /safemotions/trained_networks/humanoid/armar6_x4/collision/simultaneous/checkpoint/checkpoint: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/translearn/safeMotions/36a97fb0f71fa7f81543dd94cdb58578b6a1022d/safemotions/trained_networks/humanoid/armar6_x4/collision/simultaneous/checkpoint/checkpoint -------------------------------------------------------------------------------- /safemotions/trained_networks/humanoid/armar6_x4/collision/simultaneous/checkpoint/checkpoint.tune_metadata: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/translearn/safeMotions/36a97fb0f71fa7f81543dd94cdb58578b6a1022d/safemotions/trained_networks/humanoid/armar6_x4/collision/simultaneous/checkpoint/checkpoint.tune_metadata -------------------------------------------------------------------------------- /safemotions/trained_networks/humanoid/armar6_x4/collision/simultaneous/params.json: -------------------------------------------------------------------------------- 1 | { 2 | "batch_mode": "complete_episodes", 3 | "callbacks": "", 4 | "clip_param": 0.3, 5 | "entropy_coeff": 0.0, 6 | "env": "SafeMotionsEnv", 7 | "env_config": { 8 | "acc_limit_factor": 1.0, 9 | "acc_limit_factor_braking": 1.0, 10 | "action_max_punishment": 0.4, 11 | "action_punishment_min_threshold": 0.95, 12 | "braking_trajectory_max_punishment": 0.5, 13 | "braking_trajectory_max_torque_min_threshold": 0.8, 14 | "braking_trajectory_min_distance_max_threshold": 0.05, 15 | "check_braking_trajectory_collisions": true, 16 | "check_braking_trajectory_torque_limits": false, 17 | "closest_point_safety_distance": 0.01, 18 | "collision_check_time": 0.05, 19 | "experiment_name": "humanoid/armar6_x4/collision/simultaneous", 20 | "jerk_limit_factor": 1.0, 21 | "jerk_limit_factor_braking": 1.0, 22 | "klimits_version": "1.1.1", 23 | "log_obstacle_data": false, 24 | "m_prev": 0, 25 | "normalize_reward_to_frequency": false, 26 | "normalize_reward_to_initial_target_point_distance": true, 27 | "obs_add_target_point_pos": true, 28 | "obs_add_target_point_relative_pos": true, 29 | "obstacle_scene": 5, 30 | "obstacle_use_computed_actual_values": false, 31 | "online_trajectory_duration": 8.0, 32 | "online_trajectory_time_step": 0.1, 33 | "pos_limit_factor": 1.0, 34 | "punish_action": true, 35 | "punish_braking_trajectory_max_torque": false, 36 | "punish_braking_trajectory_min_distance": true, 37 | "ray_version": "1.4.1", 38 | "robot_scene": 5, 39 | "solver_iterations": 50, 40 | "target_link_offset": [ 41 | 0.03, 42 | 0, 43 | 0.135 44 | ], 45 | "target_point_cartesian_range_scene": 6, 46 | "target_point_radius": 0.065, 47 | "target_point_reached_reward_bonus": 5.0, 48 | "target_point_reward_factor": 1.0, 49 | "target_point_sequence": 0, 50 | "target_point_use_actual_position": false, 51 | "torque_limit_factor": 1.0, 52 | "use_target_points": true, 53 | "vel_limit_factor": 1.0 54 | }, 55 | "evaluation_config": { 56 | "explore": false, 57 | "rollout_fragment_length": 1 58 | }, 59 | "evaluation_interval": null, 60 | "evaluation_num_episodes": 624, 61 | "evaluation_num_workers": 0, 62 | "evaluation_parallel_to_training": false, 63 | "gamma": 0.99, 64 | "kl_coeff": 0.2, 65 | "kl_target": 0.01, 66 | "lambda": 1.0, 67 | "log_level": "WARNING", 68 | "lr": 5e-05, 69 | "lr_schedule": null, 70 | "model": { 71 | "conv_filters": null, 72 | "custom_model": "keras_fcnet_last_layer_activation", 73 | "custom_model_config": { 74 | "fcnet_activation": "swish", 75 | "fcnet_hiddens": [ 76 | 256, 77 | 128 78 | ], 79 | "last_layer_activation": "tanh", 80 | "no_log_std_activation": false 81 | }, 82 | "fcnet_activation": "swish", 83 | "fcnet_hiddens": [ 84 | 256, 85 | 128 86 | ], 87 | "use_lstm": false 88 | }, 89 | "normalize_actions": true, 90 | "num_gpus": 1, 91 | "num_sgd_iter": 16, 92 | "num_workers": 12, 93 | "rollout_fragment_length": 4160, 94 | "sgd_minibatch_size": 1024, 95 | "train_batch_size": 49920, 96 | "use_gae": true, 97 | "vf_clip_param": 10.0, 98 | "vf_loss_coeff": 1.0 99 | } 100 | -------------------------------------------------------------------------------- /safemotions/trained_networks/humanoid/armar6_x4/collision/single/checkpoint/.is_checkpoint: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/translearn/safeMotions/36a97fb0f71fa7f81543dd94cdb58578b6a1022d/safemotions/trained_networks/humanoid/armar6_x4/collision/single/checkpoint/.is_checkpoint -------------------------------------------------------------------------------- /safemotions/trained_networks/humanoid/armar6_x4/collision/single/checkpoint/checkpoint: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/translearn/safeMotions/36a97fb0f71fa7f81543dd94cdb58578b6a1022d/safemotions/trained_networks/humanoid/armar6_x4/collision/single/checkpoint/checkpoint -------------------------------------------------------------------------------- /safemotions/trained_networks/humanoid/armar6_x4/collision/single/checkpoint/checkpoint.tune_metadata: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/translearn/safeMotions/36a97fb0f71fa7f81543dd94cdb58578b6a1022d/safemotions/trained_networks/humanoid/armar6_x4/collision/single/checkpoint/checkpoint.tune_metadata -------------------------------------------------------------------------------- /safemotions/trained_networks/humanoid/armar6_x4/collision/single/params.json: -------------------------------------------------------------------------------- 1 | { 2 | "batch_mode": "complete_episodes", 3 | "callbacks": "", 4 | "clip_param": 0.3, 5 | "entropy_coeff": 0.0, 6 | "env": "SafeMotionsEnv", 7 | "env_config": { 8 | "acc_limit_factor": 1.0, 9 | "acc_limit_factor_braking": 1.0, 10 | "action_max_punishment": 0.4, 11 | "action_punishment_min_threshold": 0.95, 12 | "braking_trajectory_max_punishment": 0.5, 13 | "braking_trajectory_max_torque_min_threshold": 0.8, 14 | "braking_trajectory_min_distance_max_threshold": 0.05, 15 | "check_braking_trajectory_collisions": true, 16 | "check_braking_trajectory_torque_limits": false, 17 | "closest_point_safety_distance": 0.01, 18 | "collision_check_time": 0.05, 19 | "experiment_name": "humanoid/armar6_x4/collision/single", 20 | "jerk_limit_factor": 1.0, 21 | "jerk_limit_factor_braking": 1.0, 22 | "klimits_version": "1.1.1", 23 | "log_obstacle_data": false, 24 | "logging_level": "WARNING", 25 | "m_prev": 0, 26 | "normalize_reward_to_frequency": false, 27 | "normalize_reward_to_initial_target_point_distance": true, 28 | "obs_add_target_point_pos": true, 29 | "obs_add_target_point_relative_pos": true, 30 | "obstacle_scene": 5, 31 | "obstacle_use_computed_actual_values": false, 32 | "online_trajectory_duration": 8.0, 33 | "online_trajectory_time_step": 0.1, 34 | "pos_limit_factor": 1.0, 35 | "punish_action": true, 36 | "punish_braking_trajectory_max_torque": false, 37 | "punish_braking_trajectory_min_distance": true, 38 | "ray_version": "1.4.1", 39 | "robot_scene": 5, 40 | "solver_iterations": 50, 41 | "target_link_offset": [ 42 | 0.03, 43 | 0, 44 | 0.135 45 | ], 46 | "target_point_cartesian_range_scene": 6, 47 | "target_point_radius": 0.065, 48 | "target_point_reached_reward_bonus": 5.0, 49 | "target_point_relative_pos_scene": 1, 50 | "target_point_reward_factor": 1.0, 51 | "target_point_sequence": 2, 52 | "target_point_use_actual_position": false, 53 | "torque_limit_factor": 1.0, 54 | "use_target_points": true, 55 | "vel_limit_factor": 1.0 56 | }, 57 | "evaluation_config": { 58 | "explore": false, 59 | "rollout_fragment_length": 1 60 | }, 61 | "evaluation_interval": 50, 62 | "evaluation_num_episodes": 624, 63 | "evaluation_num_workers": 0, 64 | "evaluation_parallel_to_training": false, 65 | "gamma": 0.99, 66 | "kl_coeff": 0.2, 67 | "kl_target": 0.01, 68 | "lambda": 1.0, 69 | "lr": 5e-05, 70 | "lr_schedule": null, 71 | "model": { 72 | "conv_filters": null, 73 | "custom_model": "keras_fcnet_last_layer_activation", 74 | "custom_model_config": { 75 | "fcnet_activation": "swish", 76 | "fcnet_hiddens": [ 77 | 256, 78 | 128 79 | ], 80 | "last_layer_activation": "tanh", 81 | "no_log_std_activation": false 82 | }, 83 | "fcnet_activation": "swish", 84 | "fcnet_hiddens": [ 85 | 256, 86 | 128 87 | ], 88 | "use_lstm": false 89 | }, 90 | "normalize_actions": true, 91 | "num_gpus": 1, 92 | "num_sgd_iter": 16, 93 | "num_workers": 12, 94 | "rollout_fragment_length": 4160, 95 | "sgd_minibatch_size": 1024, 96 | "train_batch_size": 49920, 97 | "use_gae": true, 98 | "vf_clip_param": 10.0, 99 | "vf_loss_coeff": 1.0 100 | } 101 | -------------------------------------------------------------------------------- /safemotions/trained_networks/industrial/one_robot/collision/checkpoint/.is_checkpoint: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/translearn/safeMotions/36a97fb0f71fa7f81543dd94cdb58578b6a1022d/safemotions/trained_networks/industrial/one_robot/collision/checkpoint/.is_checkpoint -------------------------------------------------------------------------------- /safemotions/trained_networks/industrial/one_robot/collision/checkpoint/checkpoint: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/translearn/safeMotions/36a97fb0f71fa7f81543dd94cdb58578b6a1022d/safemotions/trained_networks/industrial/one_robot/collision/checkpoint/checkpoint -------------------------------------------------------------------------------- /safemotions/trained_networks/industrial/one_robot/collision/checkpoint/checkpoint.tune_metadata: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/translearn/safeMotions/36a97fb0f71fa7f81543dd94cdb58578b6a1022d/safemotions/trained_networks/industrial/one_robot/collision/checkpoint/checkpoint.tune_metadata -------------------------------------------------------------------------------- /safemotions/trained_networks/industrial/one_robot/collision/params.json: -------------------------------------------------------------------------------- 1 | { 2 | "batch_mode": "complete_episodes", 3 | "callbacks": "", 4 | "clip_param": 0.3, 5 | "entropy_coeff": 0.0, 6 | "env": "SafeMotionsEnv", 7 | "env_config": { 8 | "acc_limit_factor": 1.0, 9 | "acc_limit_factor_braking": 1.0, 10 | "action_max_punishment": 0.4, 11 | "action_punishment_min_threshold": 0.95, 12 | "braking_trajectory_max_punishment": 0.5, 13 | "braking_trajectory_max_torque_min_threshold": 0.8, 14 | "braking_trajectory_min_distance_max_threshold": 0.05, 15 | "check_braking_trajectory_collisions": true, 16 | "check_braking_trajectory_torque_limits": false, 17 | "closest_point_safety_distance": 0.01, 18 | "collision_check_time": 0.033, 19 | "experiment_name": "industrial/one_robot/collision", 20 | "jerk_limit_factor": 1.0, 21 | "jerk_limit_factor_braking": 1.0, 22 | "klimits_version": "1.1.1", 23 | "log_obstacle_data": false, 24 | "logging_level": "WARNING", 25 | "m_prev": 0, 26 | "normalize_reward_to_frequency": false, 27 | "normalize_reward_to_initial_target_point_distance": true, 28 | "obs_add_target_point_pos": true, 29 | "obs_add_target_point_relative_pos": true, 30 | "obstacle_scene": 3, 31 | "obstacle_use_computed_actual_values": false, 32 | "online_trajectory_duration": 8.0, 33 | "online_trajectory_time_step": 0.1, 34 | "pos_limit_factor": 1.0, 35 | "punish_action": true, 36 | "punish_braking_trajectory_max_torque": false, 37 | "punish_braking_trajectory_min_distance": true, 38 | "ray_version": "1.4.1", 39 | "robot_scene": 0, 40 | "solver_iterations": 50, 41 | "target_link_offset": [ 42 | 0, 43 | 0, 44 | 0.126 45 | ], 46 | "target_point_cartesian_range_scene": 0, 47 | "target_point_radius": 0.065, 48 | "target_point_reached_reward_bonus": 5.0, 49 | "target_point_relative_pos_scene": 0, 50 | "target_point_reward_factor": 1.0, 51 | "target_point_sequence": 0, 52 | "target_point_use_actual_position": false, 53 | "torque_limit_factor": 1.0, 54 | "use_target_points": true, 55 | "vel_limit_factor": 1.0 56 | }, 57 | "evaluation_config": { 58 | "explore": false, 59 | "rollout_fragment_length": 1 60 | }, 61 | "evaluation_interval": 50, 62 | "evaluation_num_episodes": 624, 63 | "evaluation_num_workers": 0, 64 | "evaluation_parallel_to_training": false, 65 | "gamma": 0.99, 66 | "kl_coeff": 0.2, 67 | "kl_target": 0.01, 68 | "lambda": 1.0, 69 | "lr": 5e-05, 70 | "lr_schedule": null, 71 | "model": { 72 | "conv_filters": null, 73 | "custom_model": "keras_fcnet_last_layer_activation", 74 | "custom_model_config": { 75 | "fcnet_activation": "swish", 76 | "fcnet_hiddens": [ 77 | 256, 78 | 128 79 | ], 80 | "last_layer_activation": "tanh", 81 | "no_log_std_activation": false 82 | }, 83 | "fcnet_activation": "swish", 84 | "fcnet_hiddens": [ 85 | 256, 86 | 128 87 | ], 88 | "use_lstm": false 89 | }, 90 | "normalize_actions": true, 91 | "num_gpus": 1, 92 | "num_sgd_iter": 16, 93 | "num_workers": 12, 94 | "rollout_fragment_length": 4160, 95 | "sgd_minibatch_size": 1024, 96 | "train_batch_size": 49920, 97 | "use_gae": true, 98 | "vf_clip_param": 10.0, 99 | "vf_loss_coeff": 1.0 100 | } 101 | -------------------------------------------------------------------------------- /safemotions/trained_networks/industrial/three_robots/collision/alternating/checkpoint/.is_checkpoint: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/translearn/safeMotions/36a97fb0f71fa7f81543dd94cdb58578b6a1022d/safemotions/trained_networks/industrial/three_robots/collision/alternating/checkpoint/.is_checkpoint -------------------------------------------------------------------------------- /safemotions/trained_networks/industrial/three_robots/collision/alternating/checkpoint/checkpoint: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/translearn/safeMotions/36a97fb0f71fa7f81543dd94cdb58578b6a1022d/safemotions/trained_networks/industrial/three_robots/collision/alternating/checkpoint/checkpoint -------------------------------------------------------------------------------- /safemotions/trained_networks/industrial/three_robots/collision/alternating/checkpoint/checkpoint.tune_metadata: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/translearn/safeMotions/36a97fb0f71fa7f81543dd94cdb58578b6a1022d/safemotions/trained_networks/industrial/three_robots/collision/alternating/checkpoint/checkpoint.tune_metadata -------------------------------------------------------------------------------- /safemotions/trained_networks/industrial/three_robots/collision/alternating/params.json: -------------------------------------------------------------------------------- 1 | { 2 | "batch_mode": "complete_episodes", 3 | "callbacks": "", 4 | "clip_param": 0.3, 5 | "entropy_coeff": 0.0, 6 | "env": "SafeMotionsEnv", 7 | "env_config": { 8 | "acc_limit_factor": 1.0, 9 | "acc_limit_factor_braking": 1.0, 10 | "action_max_punishment": 0.4, 11 | "action_punishment_min_threshold": 0.95, 12 | "braking_trajectory_max_punishment": 0.5, 13 | "braking_trajectory_max_torque_min_threshold": 0.8, 14 | "braking_trajectory_min_distance_max_threshold": 0.05, 15 | "check_braking_trajectory_collisions": true, 16 | "check_braking_trajectory_torque_limits": false, 17 | "closest_point_safety_distance": 0.01, 18 | "collision_check_time": 0.033, 19 | "experiment_name": "industrial/three_robots/collision/alternating", 20 | "jerk_limit_factor": 1.0, 21 | "jerk_limit_factor_braking": 1.0, 22 | "klimits_version": "1.1.1", 23 | "log_obstacle_data": false, 24 | "logging_level": "WARNING", 25 | "m_prev": 0, 26 | "normalize_reward_to_frequency": false, 27 | "normalize_reward_to_initial_target_point_distance": true, 28 | "obs_add_target_point_pos": true, 29 | "obs_add_target_point_relative_pos": true, 30 | "obstacle_scene": 1, 31 | "obstacle_use_computed_actual_values": false, 32 | "online_trajectory_duration": 8.0, 33 | "online_trajectory_time_step": 0.1, 34 | "pos_limit_factor": 1.0, 35 | "punish_action": true, 36 | "punish_braking_trajectory_max_torque": false, 37 | "punish_braking_trajectory_min_distance": true, 38 | "ray_version": "1.4.1", 39 | "robot_scene": 2, 40 | "solver_iterations": 50, 41 | "target_link_offset": [ 42 | 0, 43 | 0, 44 | 0.126 45 | ], 46 | "target_point_cartesian_range_scene": 2, 47 | "target_point_radius": 0.065, 48 | "target_point_reached_reward_bonus": 5.0, 49 | "target_point_relative_pos_scene": 0, 50 | "target_point_reward_factor": 1.0, 51 | "target_point_sequence": 1, 52 | "target_point_use_actual_position": false, 53 | "torque_limit_factor": 1.0, 54 | "use_target_points": true, 55 | "vel_limit_factor": 1.0 56 | }, 57 | "evaluation_config": { 58 | "explore": false, 59 | "rollout_fragment_length": 1 60 | }, 61 | "evaluation_interval": 50, 62 | "evaluation_num_episodes": 624, 63 | "evaluation_num_workers": 0, 64 | "evaluation_parallel_to_training": false, 65 | "gamma": 0.99, 66 | "kl_coeff": 0.2, 67 | "kl_target": 0.01, 68 | "lambda": 1.0, 69 | "lr": 5e-05, 70 | "lr_schedule": null, 71 | "model": { 72 | "conv_filters": null, 73 | "custom_model": "keras_fcnet_last_layer_activation", 74 | "custom_model_config": { 75 | "fcnet_activation": "swish", 76 | "fcnet_hiddens": [ 77 | 256, 78 | 128 79 | ], 80 | "last_layer_activation": "tanh", 81 | "no_log_std_activation": false 82 | }, 83 | "fcnet_activation": "swish", 84 | "fcnet_hiddens": [ 85 | 256, 86 | 128 87 | ], 88 | "use_lstm": false 89 | }, 90 | "normalize_actions": true, 91 | "num_gpus": 1, 92 | "num_sgd_iter": 16, 93 | "num_workers": 12, 94 | "rollout_fragment_length": 4160, 95 | "sgd_minibatch_size": 1024, 96 | "train_batch_size": 49920, 97 | "use_gae": true, 98 | "vf_clip_param": 10.0, 99 | "vf_loss_coeff": 1.0 100 | } 101 | -------------------------------------------------------------------------------- /safemotions/trained_networks/industrial/two_robots/collision/alternating/checkpoint/.is_checkpoint: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/translearn/safeMotions/36a97fb0f71fa7f81543dd94cdb58578b6a1022d/safemotions/trained_networks/industrial/two_robots/collision/alternating/checkpoint/.is_checkpoint -------------------------------------------------------------------------------- /safemotions/trained_networks/industrial/two_robots/collision/alternating/checkpoint/checkpoint: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/translearn/safeMotions/36a97fb0f71fa7f81543dd94cdb58578b6a1022d/safemotions/trained_networks/industrial/two_robots/collision/alternating/checkpoint/checkpoint -------------------------------------------------------------------------------- /safemotions/trained_networks/industrial/two_robots/collision/alternating/checkpoint/checkpoint.tune_metadata: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/translearn/safeMotions/36a97fb0f71fa7f81543dd94cdb58578b6a1022d/safemotions/trained_networks/industrial/two_robots/collision/alternating/checkpoint/checkpoint.tune_metadata -------------------------------------------------------------------------------- /safemotions/trained_networks/industrial/two_robots/collision/alternating/params.json: -------------------------------------------------------------------------------- 1 | { 2 | "batch_mode": "complete_episodes", 3 | "callbacks": "", 4 | "clip_param": 0.3, 5 | "entropy_coeff": 0.0, 6 | "env": "SafeMotionsEnv", 7 | "env_config": { 8 | "acc_limit_factor": 1.0, 9 | "acc_limit_factor_braking": 1.0, 10 | "action_max_punishment": 0.4, 11 | "action_punishment_min_threshold": 0.95, 12 | "braking_trajectory_max_punishment": 0.5, 13 | "braking_trajectory_max_torque_min_threshold": 0.8, 14 | "braking_trajectory_min_distance_max_threshold": 0.05, 15 | "check_braking_trajectory_collisions": true, 16 | "check_braking_trajectory_torque_limits": false, 17 | "closest_point_safety_distance": 0.01, 18 | "collision_check_time": 0.033, 19 | "experiment_name": "industrial/two_robots/collision/alternating", 20 | "jerk_limit_factor": 1.0, 21 | "jerk_limit_factor_braking": 1.0, 22 | "klimits_version": "1.1.1", 23 | "log_obstacle_data": false, 24 | "logging_level": "WARNING", 25 | "m_prev": 0, 26 | "normalize_reward_to_frequency": false, 27 | "normalize_reward_to_initial_target_point_distance": true, 28 | "obs_add_target_point_pos": true, 29 | "obs_add_target_point_relative_pos": true, 30 | "obstacle_scene": 1, 31 | "obstacle_use_computed_actual_values": false, 32 | "online_trajectory_duration": 8.0, 33 | "online_trajectory_time_step": 0.1, 34 | "pos_limit_factor": 1.0, 35 | "punish_action": true, 36 | "punish_braking_trajectory_max_torque": false, 37 | "punish_braking_trajectory_min_distance": true, 38 | "ray_version": "1.4.1", 39 | "robot_scene": 1, 40 | "solver_iterations": 50, 41 | "target_link_offset": [ 42 | 0, 43 | 0, 44 | 0.126 45 | ], 46 | "target_point_cartesian_range_scene": 1, 47 | "target_point_radius": 0.065, 48 | "target_point_reached_reward_bonus": 5.0, 49 | "target_point_relative_pos_scene": 0, 50 | "target_point_reward_factor": 1.0, 51 | "target_point_sequence": 1, 52 | "target_point_use_actual_position": false, 53 | "torque_limit_factor": 1.0, 54 | "use_target_points": true, 55 | "vel_limit_factor": 1.0 56 | }, 57 | "evaluation_config": { 58 | "explore": false, 59 | "rollout_fragment_length": 1 60 | }, 61 | "evaluation_interval": 50, 62 | "evaluation_num_episodes": 624, 63 | "evaluation_num_workers": 0, 64 | "evaluation_parallel_to_training": false, 65 | "gamma": 0.99, 66 | "kl_coeff": 0.2, 67 | "kl_target": 0.01, 68 | "lambda": 1.0, 69 | "lr": 5e-05, 70 | "lr_schedule": null, 71 | "model": { 72 | "conv_filters": null, 73 | "custom_model": "keras_fcnet_last_layer_activation", 74 | "custom_model_config": { 75 | "fcnet_activation": "swish", 76 | "fcnet_hiddens": [ 77 | 256, 78 | 128 79 | ], 80 | "last_layer_activation": "tanh", 81 | "no_log_std_activation": false 82 | }, 83 | "fcnet_activation": "swish", 84 | "fcnet_hiddens": [ 85 | 256, 86 | 128 87 | ], 88 | "use_lstm": false 89 | }, 90 | "normalize_actions": true, 91 | "num_gpus": 1, 92 | "num_sgd_iter": 16, 93 | "num_workers": 12, 94 | "rollout_fragment_length": 4160, 95 | "sgd_minibatch_size": 1024, 96 | "train_batch_size": 49920, 97 | "use_gae": true, 98 | "vf_clip_param": 10.0, 99 | "vf_loss_coeff": 1.0 100 | } 101 | -------------------------------------------------------------------------------- /safemotions/trained_networks/industrial/two_robots/collision/simultaneous/checkpoint/.is_checkpoint: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/translearn/safeMotions/36a97fb0f71fa7f81543dd94cdb58578b6a1022d/safemotions/trained_networks/industrial/two_robots/collision/simultaneous/checkpoint/.is_checkpoint -------------------------------------------------------------------------------- /safemotions/trained_networks/industrial/two_robots/collision/simultaneous/checkpoint/checkpoint: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/translearn/safeMotions/36a97fb0f71fa7f81543dd94cdb58578b6a1022d/safemotions/trained_networks/industrial/two_robots/collision/simultaneous/checkpoint/checkpoint -------------------------------------------------------------------------------- /safemotions/trained_networks/industrial/two_robots/collision/simultaneous/checkpoint/checkpoint.tune_metadata: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/translearn/safeMotions/36a97fb0f71fa7f81543dd94cdb58578b6a1022d/safemotions/trained_networks/industrial/two_robots/collision/simultaneous/checkpoint/checkpoint.tune_metadata -------------------------------------------------------------------------------- /safemotions/trained_networks/industrial/two_robots/collision/simultaneous/params.json: -------------------------------------------------------------------------------- 1 | { 2 | "batch_mode": "complete_episodes", 3 | "callbacks": "", 4 | "clip_param": 0.3, 5 | "entropy_coeff": 0.0, 6 | "env": "SafeMotionsEnv", 7 | "env_config": { 8 | "acc_limit_factor": 1.0, 9 | "acc_limit_factor_braking": 1.0, 10 | "action_max_punishment": 0.4, 11 | "action_punishment_min_threshold": 0.95, 12 | "braking_trajectory_max_punishment": 0.5, 13 | "braking_trajectory_max_torque_min_threshold": 0.8, 14 | "braking_trajectory_min_distance_max_threshold": 0.05, 15 | "check_braking_trajectory_collisions": true, 16 | "check_braking_trajectory_torque_limits": false, 17 | "closest_point_safety_distance": 0.01, 18 | "collision_check_time": 0.033, 19 | "experiment_name": "industrial/two_robots/collision/simultaneous", 20 | "jerk_limit_factor": 1.0, 21 | "jerk_limit_factor_braking": 1.0, 22 | "klimits_version": "1.1.1", 23 | "log_obstacle_data": false, 24 | "logging_level": "WARNING", 25 | "m_prev": 0, 26 | "normalize_reward_to_frequency": false, 27 | "normalize_reward_to_initial_target_point_distance": true, 28 | "obs_add_target_point_pos": true, 29 | "obs_add_target_point_relative_pos": true, 30 | "obstacle_scene": 1, 31 | "obstacle_use_computed_actual_values": false, 32 | "online_trajectory_duration": 8.0, 33 | "online_trajectory_time_step": 0.1, 34 | "pos_limit_factor": 1.0, 35 | "punish_action": true, 36 | "punish_braking_trajectory_max_torque": false, 37 | "punish_braking_trajectory_min_distance": true, 38 | "ray_version": "1.4.1", 39 | "robot_scene": 1, 40 | "solver_iterations": 50, 41 | "target_link_offset": [ 42 | 0, 43 | 0, 44 | 0.126 45 | ], 46 | "target_point_cartesian_range_scene": 1, 47 | "target_point_radius": 0.065, 48 | "target_point_reached_reward_bonus": 5.0, 49 | "target_point_relative_pos_scene": 0, 50 | "target_point_reward_factor": 1.0, 51 | "target_point_sequence": 0, 52 | "target_point_use_actual_position": false, 53 | "torque_limit_factor": 1.0, 54 | "use_target_points": true, 55 | "vel_limit_factor": 1.0 56 | }, 57 | "evaluation_config": { 58 | "explore": false, 59 | "rollout_fragment_length": 1 60 | }, 61 | "evaluation_interval": 50, 62 | "evaluation_num_episodes": 624, 63 | "evaluation_num_workers": 0, 64 | "evaluation_parallel_to_training": false, 65 | "gamma": 0.99, 66 | "kl_coeff": 0.2, 67 | "kl_target": 0.01, 68 | "lambda": 1.0, 69 | "lr": 5e-05, 70 | "lr_schedule": null, 71 | "model": { 72 | "conv_filters": null, 73 | "custom_model": "keras_fcnet_last_layer_activation", 74 | "custom_model_config": { 75 | "fcnet_activation": "swish", 76 | "fcnet_hiddens": [ 77 | 256, 78 | 128 79 | ], 80 | "last_layer_activation": "tanh", 81 | "no_log_std_activation": false 82 | }, 83 | "fcnet_activation": "swish", 84 | "fcnet_hiddens": [ 85 | 256, 86 | 128 87 | ], 88 | "use_lstm": false 89 | }, 90 | "normalize_actions": true, 91 | "num_gpus": 1, 92 | "num_sgd_iter": 16, 93 | "num_workers": 12, 94 | "rollout_fragment_length": 4160, 95 | "sgd_minibatch_size": 1024, 96 | "train_batch_size": 49920, 97 | "use_gae": true, 98 | "vf_clip_param": 10.0, 99 | "vf_loss_coeff": 1.0 100 | } 101 | -------------------------------------------------------------------------------- /safemotions/utils/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/translearn/safeMotions/36a97fb0f71fa7f81543dd94cdb58578b6a1022d/safemotions/utils/__init__.py -------------------------------------------------------------------------------- /safemotions/utils/braking_trajectory_generator.py: -------------------------------------------------------------------------------- 1 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE 2 | # WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 3 | # COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR 4 | # OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 5 | 6 | import numpy as np 7 | from klimits import PosVelJerkLimitation 8 | from klimits import denormalize 9 | 10 | 11 | def _denormalize(norm_value, value_range): 12 | actual_value = value_range[0] + 0.5 * (norm_value + 1) * (value_range[1] - value_range[0]) 13 | return actual_value 14 | 15 | 16 | class BrakingTrajectoryGenerator(object): 17 | 18 | def __init__(self, 19 | trajectory_time_step, 20 | acc_limits_braking, 21 | jerk_limits_braking): 22 | 23 | self._trajectory_time_step = trajectory_time_step 24 | self._acc_limits_braking = acc_limits_braking 25 | self._acc_limits_braking_min_max = np.swapaxes(self._acc_limits_braking, 0, 1) 26 | self._jerk_limits_braking = jerk_limits_braking 27 | self._num_joints = len(self._acc_limits_braking) 28 | self._vel_limits_braking = [[0, 0]] * self._num_joints 29 | self._action_mapping_factor = 1.0 30 | 31 | self._acc_calculator = PosVelJerkLimitation(time_step=self._trajectory_time_step, 32 | pos_limits=None, vel_limits=self._vel_limits_braking, 33 | acc_limits=self._acc_limits_braking, 34 | jerk_limits=self._jerk_limits_braking, 35 | acceleration_after_max_vel_limit_factor=0.0000, 36 | set_velocity_after_max_pos_to_zero=True, 37 | limit_velocity=True, 38 | limit_position=False, 39 | num_threads=None, 40 | soft_velocity_limits=True, 41 | soft_position_limits=False, 42 | normalize_acc_range=False) 43 | 44 | def get_braking_acceleration(self, start_velocity, start_acceleration, index=0): 45 | if np.all(np.abs(start_velocity) < 0.01) and np.all(np.abs(start_acceleration) < 0.01): 46 | end_acceleration = np.zeros(self._num_joints) 47 | robot_stopped = True 48 | else: 49 | robot_stopped = False 50 | 51 | if self._action_mapping_factor == 1: 52 | limit_min_max = np.array([(0.0, 1.0) if start_velocity[i] < 0 else (1.0, 0.0) 53 | for i in range(len(start_velocity))]) 54 | else: 55 | limit_min_max = None 56 | 57 | safe_acc_range, _ = self._acc_calculator.calculate_valid_acceleration_range(current_pos=None, 58 | current_vel=start_velocity, 59 | current_acc=start_acceleration, 60 | braking_trajectory=True, 61 | time_step_counter=index, 62 | limit_min_max=limit_min_max) 63 | 64 | safe_acc_range_min_max = safe_acc_range.T 65 | if self._action_mapping_factor == 1: 66 | end_acceleration = np.where(start_velocity < 0, safe_acc_range_min_max[1], 67 | safe_acc_range_min_max[0]) 68 | else: 69 | normalized_mapping_factor = np.where(start_velocity < 0, self._action_mapping_factor * 2 - 1, 70 | (1 - self._action_mapping_factor) * 2 - 1) 71 | 72 | end_acceleration = denormalize(normalized_mapping_factor, safe_acc_range_min_max) 73 | 74 | end_acceleration = np.where(np.logical_and(np.abs(start_velocity) < 0.01, 75 | np.abs(start_acceleration) < 0.01), 76 | 0.0, end_acceleration) 77 | 78 | return end_acceleration, robot_stopped 79 | 80 | def get_clipped_braking_acceleration(self, start_velocity, start_acceleration, next_acc_min, next_acc_max, index=0): 81 | end_acceleration, robot_stopped = self.get_braking_acceleration(start_velocity, start_acceleration, index) 82 | # avoid oscillations around p_max or p_min by reducing the valid range by x percent 83 | action_mapping_factor = 1.0 84 | 85 | if action_mapping_factor != 1.0: 86 | next_acc_diff = next_acc_max - next_acc_min 87 | next_acc_max_no_oscillation = next_acc_min + action_mapping_factor * next_acc_diff 88 | next_acc_min_no_oscillation = next_acc_min + (1 - action_mapping_factor) * next_acc_diff 89 | else: 90 | next_acc_max_no_oscillation = next_acc_max 91 | next_acc_min_no_oscillation = next_acc_min 92 | 93 | return np.core.umath.clip(end_acceleration, next_acc_min_no_oscillation, next_acc_max_no_oscillation), robot_stopped 94 | 95 | -------------------------------------------------------------------------------- /safemotions/utils/trajectory_manager.py: -------------------------------------------------------------------------------- 1 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE 2 | # WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 3 | # COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR 4 | # OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 5 | 6 | import numpy as np 7 | 8 | 9 | def clip_index(index, list_length): 10 | if index < 0 and abs(index) > list_length: 11 | return 0 12 | if index > 0 and index > list_length - 1: 13 | return -1 14 | else: 15 | return index 16 | 17 | 18 | class TrajectoryManager(object): 19 | 20 | def __init__(self, 21 | trajectory_time_step, 22 | trajectory_duration, 23 | obstacle_wrapper, 24 | **kwargs): 25 | 26 | self._trajectory_time_step = trajectory_time_step 27 | self._trajectory_duration = trajectory_duration 28 | self._obstacle_wrapper = obstacle_wrapper 29 | 30 | self._trajectory_start_position = None 31 | self._trajectory_length = None 32 | self._num_manip_joints = None 33 | self._zero_joint_vector = None 34 | self._generated_trajectory = None 35 | self._measured_actual_trajectory_control_points = None 36 | self._computed_actual_trajectory_control_points = None 37 | self._generated_trajectory_control_points = None 38 | 39 | self._controller_model_coefficient_a = None 40 | self._controller_model_coefficient_b = None 41 | 42 | @property 43 | def generated_trajectory_control_points(self): 44 | return self._generated_trajectory_control_points 45 | 46 | @property 47 | def measured_actual_trajectory_control_points(self): 48 | return self._measured_actual_trajectory_control_points 49 | 50 | @property 51 | def computed_actual_trajectory_control_points(self): 52 | return self._computed_actual_trajectory_control_points 53 | 54 | @property 55 | def trajectory_time_step(self): 56 | return self._trajectory_time_step 57 | 58 | @property 59 | def trajectory_length(self): 60 | return self._trajectory_length 61 | 62 | def reset(self, get_new_trajectory=True): 63 | if get_new_trajectory: 64 | self._trajectory_start_position = self._get_new_trajectory_start_position() 65 | self._trajectory_length = int(self._trajectory_duration / self._trajectory_time_step) + 1 66 | self._num_manip_joints = len(self._trajectory_start_position) 67 | self._zero_joint_vector = np.array([0.0] * self._num_manip_joints) 68 | self._generated_trajectory = {'positions': [self.get_trajectory_start_position()], 69 | 'velocities': [self._zero_joint_vector], 70 | 'accelerations': [self._zero_joint_vector]} 71 | self._measured_actual_trajectory_control_points = {'positions': [self.get_trajectory_start_position()], 72 | 'velocities': [self._zero_joint_vector], 73 | 'accelerations': [self._zero_joint_vector]} 74 | self._computed_actual_trajectory_control_points = {'positions': [self.get_trajectory_start_position()], 75 | 'velocities': [self._zero_joint_vector], 76 | 'accelerations': [self._zero_joint_vector]} 77 | self._generated_trajectory_control_points = {'positions': [self.get_trajectory_start_position()], 78 | 'velocities': [self._zero_joint_vector], 79 | 'accelerations': [self._zero_joint_vector]} 80 | 81 | def get_trajectory_start_position(self): 82 | return self._trajectory_start_position 83 | 84 | def get_generated_trajectory_point(self, index, key='positions'): 85 | i = clip_index(index, len(self._generated_trajectory[key])) 86 | 87 | return self._generated_trajectory[key][i] 88 | 89 | def get_measured_actual_trajectory_control_point(self, index, key='positions'): 90 | i = clip_index(index, len(self._measured_actual_trajectory_control_points[key])) 91 | 92 | return self._measured_actual_trajectory_control_points[key][i] 93 | 94 | def get_computed_actual_trajectory_control_point(self, index, key='positions'): 95 | i = clip_index(index, len(self._computed_actual_trajectory_control_points[key])) 96 | 97 | return self._computed_actual_trajectory_control_points[key][i] 98 | 99 | def get_generated_trajectory_control_point(self, index, key='positions'): 100 | i = clip_index(index, len(self._generated_trajectory_control_points[key])) 101 | 102 | return self._generated_trajectory_control_points[key][i] 103 | 104 | def add_generated_trajectory_point(self, positions, velocities, accelerations): 105 | self._generated_trajectory['positions'].append(positions) 106 | self._generated_trajectory['velocities'].append(velocities) 107 | self._generated_trajectory['accelerations'].append(accelerations) 108 | 109 | def add_measured_actual_trajectory_control_point(self, positions, velocities, accelerations): 110 | self._measured_actual_trajectory_control_points['positions'].append(positions) 111 | self._measured_actual_trajectory_control_points['velocities'].append(velocities) 112 | self._measured_actual_trajectory_control_points['accelerations'].append(accelerations) 113 | 114 | def add_computed_actual_trajectory_control_point(self, positions, velocities, accelerations): 115 | self._computed_actual_trajectory_control_points['positions'].append(positions) 116 | self._computed_actual_trajectory_control_points['velocities'].append(velocities) 117 | self._computed_actual_trajectory_control_points['accelerations'].append(accelerations) 118 | 119 | def add_generated_trajectory_control_point(self, positions, velocities, accelerations): 120 | self._generated_trajectory_control_points['positions'].append(positions) 121 | self._generated_trajectory_control_points['velocities'].append(velocities) 122 | self._generated_trajectory_control_points['accelerations'].append(accelerations) 123 | 124 | def compute_controller_model_coefficients(self, time_constants, sampling_time): 125 | self._controller_model_coefficient_a = 1 + (2 * np.array(time_constants) / sampling_time) 126 | self._controller_model_coefficient_b = 1 - (2 * np.array(time_constants) / sampling_time) 127 | 128 | def model_position_controller_to_compute_actual_values(self, current_setpoint, last_setpoint, key='positions'): 129 | # models the position controller as a discrete transfer function and returns the 130 | # computed actual position, given the next position setpoint and previous computed actual positions 131 | # the controller is modelled as a first order low-pass with a (continuous) transfer function of 132 | # G(s) = 1 / (1 + T * s) 133 | # the transfer function is discretized using Tustins approximation: s = 2 / Ta * (z - 1) / (z + 1) 134 | # the following difference equation can be derived: 135 | # y_n = 1/a * (x_n + x_n_minus_one - b * y_n_minus_one) with a = 1 + (2 * T / Ta) and b = 1 - (2 * T / Ta) 136 | 137 | x_n = np.asarray(current_setpoint) 138 | x_n_minus_one = np.asarray(last_setpoint) 139 | y_n_minus_one = self.get_computed_actual_trajectory_control_point(-1, key=key) 140 | computed_actual_position = 1 / self._controller_model_coefficient_a * \ 141 | (x_n + x_n_minus_one - self._controller_model_coefficient_b * y_n_minus_one) 142 | 143 | return computed_actual_position 144 | 145 | def is_trajectory_finished(self, index): 146 | return index >= self._trajectory_length - 1 147 | 148 | def _get_new_trajectory_start_position(self): 149 | return self._obstacle_wrapper.get_starting_point_joint_pos() 150 | 151 | 152 | 153 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | import os 2 | import re 3 | from setuptools import find_packages 4 | from setuptools import setup 5 | 6 | 7 | def get_version(): 8 | with open(os.path.join(os.path.abspath(os.path.dirname(__file__)), 9 | 'safemotions', '__init__.py'), encoding='utf-8') as f: 10 | init_file = f.read() 11 | version = re.search(r"__version__\W*=\W*'([^']+)'", init_file) 12 | return version.group(1) if version is not None else '0.0.0' 13 | 14 | 15 | with open(os.path.join(os.path.abspath(os.path.dirname(__file__)), 'README.md'), encoding='utf-8') as f: 16 | readme_file = f.read() 17 | 18 | setup(name='safemotions', 19 | version=get_version(), 20 | packages=find_packages(), 21 | include_package_data=True, 22 | author='Jonas C. Kiemel', 23 | author_email='jonas.kiemel@kit.edu', 24 | url='https://github.com/translearn/safemotions', 25 | description='Learning Collision-free and Torque-limited Robot Trajectories based on Alternative Safe Behaviors.', 26 | long_description=readme_file, 27 | long_description_content_type='text/markdown', 28 | license='MIT', 29 | classifiers=[ 30 | "License :: OSI Approved :: MIT License", 31 | "Intended Audience :: Developers", 32 | 'Programming Language :: Python :: 3.6', 33 | 'Programming Language :: Python :: 3.7', 34 | 'Programming Language :: Python :: 3.8', 35 | 'Programming Language :: Python :: 3.9'], 36 | python_requires='>=3.5', 37 | install_requires=[ 38 | 'numpy', 39 | 'klimits>=1.1.1', 40 | 'matplotlib', 41 | 'pybullet', 42 | 'gym', 43 | 'Pillow' 44 | ], 45 | extras_require={'train': ['aiohttp==3.7.4.post0', 'aiohttp-cors==0.7.0', 'aioredis==1.3.1', 'redis==3.5.3', 'prometheus-client==0.11.0', 46 | 'ray[default]==1.4.1', 'ray[rllib]==1.4.1', 'tensorflow']} 47 | ) 48 | --------------------------------------------------------------------------------