├── .gitignore ├── LICENSE ├── README.md ├── actpermoma.gif ├── actpermoma ├── __init__.py ├── algos │ ├── active_grasp.py │ ├── actpermoma.py │ ├── naive.py │ ├── random.py │ ├── reachability │ │ └── reachability_evaluator.py │ ├── sample_2D │ │ ├── env.py │ │ ├── plotting.py │ │ ├── queue.py │ │ ├── rrt.py │ │ ├── rrt_star.py │ │ └── utils.py │ └── search_2D │ │ ├── Astar.py │ │ ├── env.py │ │ └── plotting.py ├── cfg │ ├── config.yaml │ ├── task │ │ ├── TiagoDualActivePerception.yaml │ │ └── TiagoDualActivePerceptionNaive.yaml │ └── train │ │ ├── TiagoDualActPerMoMa.yaml │ │ ├── TiagoDualActiveGrasp.yaml │ │ ├── TiagoDualNaive.yaml │ │ └── TiagoDualRandom.yaml ├── envs │ └── isaac_env_mushroom.py ├── handlers │ ├── TiagoDualHandler.py │ ├── TiagoDualNaiveHandler.py │ └── base │ │ └── tiagohandler.py ├── robots │ └── articulations │ │ └── tiago_dual_holo.py ├── scripts │ ├── active_grasp_pipeline.py │ ├── active_grasp_pipeline_loop.py │ └── active_grasp_pipeline_loop_section.py ├── tasks │ ├── base │ │ └── rl_task.py │ ├── tiago_dual_active_perception.py │ ├── tiago_dual_active_perception_naive.py │ └── utils │ │ ├── pinoc_utils.py │ │ ├── scene_utils.py │ │ └── usd_utils.py ├── urdf │ └── tiago_dual_holobase.urdf ├── usd │ ├── Props │ │ ├── Shapenet │ │ │ ├── godishus │ │ │ │ └── models │ │ │ │ │ ├── model_cubic_approx.usd │ │ │ │ │ └── model_normalized.usd │ │ │ └── mammut │ │ │ │ └── models │ │ │ │ ├── model_cubic_approx.usd │ │ │ │ ├── model_normalized.json │ │ │ │ ├── model_normalized.mtl │ │ │ │ ├── model_normalized.obj │ │ │ │ ├── model_normalized.solid.binvox │ │ │ │ ├── model_normalized.surface.binvox │ │ │ │ ├── model_normalized.usd │ │ │ │ └── model_normalized_pink.usd │ │ └── YCB │ │ │ └── Axis_Aligned │ │ │ ├── 004_sugar_box.usd │ │ │ ├── 008_pudding_box.usd │ │ │ ├── 010_potted_meat_can.usd │ │ │ ├── 061_foam_brick.usd │ │ │ └── Materials │ │ │ └── Textures │ │ │ ├── 004_sugar_box_COLOR.png │ │ │ ├── 008_pudding_box_COLOR.png │ │ │ ├── 010_potted_meat_can_COLOR.png │ │ │ └── 061_foam_brick_COLOR.png │ └── tiago_dual_holobase_zed │ │ ├── tiago_dual_holobase_zed.usd │ │ └── zed_descr │ │ └── zed_descr.usd └── utils │ ├── algo_utils.py │ ├── config_utils │ ├── default_scene_params.py │ ├── path_utils.py │ └── sim_config.py │ ├── domain_randomization │ └── randomize.py │ ├── files.py │ ├── hydra_cfg │ ├── hydra_utils.py │ └── reformat.py │ ├── simple_planar_collision_avoidance.py │ ├── spatial_utils.py │ ├── task_util.py │ ├── usd_utils │ ├── create_instanceable_assets.py │ └── ov_utils.py │ ├── visualisation_utils.py │ └── wandb_utils.py ├── env.yaml ├── env_2022.2.0.yaml └── setup.py /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | share/python-wheels/ 24 | *.egg-info/ 25 | .installed.cfg 26 | *.egg 27 | MANIFEST 28 | 29 | # PyInstaller 30 | # Usually these files are written by a python script from a template 31 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 32 | *.manifest 33 | *.spec 34 | 35 | # Installer logs 36 | pip-log.txt 37 | pip-delete-this-directory.txt 38 | 39 | # Unit test / coverage reports 40 | htmlcov/ 41 | .tox/ 42 | .nox/ 43 | .coverage 44 | .coverage.* 45 | .cache 46 | nosetests.xml 47 | coverage.xml 48 | *.cover 49 | *.py,cover 50 | .hypothesis/ 51 | .pytest_cache/ 52 | cover/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | .pybuilder/ 76 | target/ 77 | 78 | # Jupyter Notebook 79 | .ipynb_checkpoints 80 | 81 | # IPython 82 | profile_default/ 83 | ipython_config.py 84 | 85 | # pyenv 86 | # For a library or package, you might want to ignore these files since the code is 87 | # intended to run in multiple environments; otherwise, check them in: 88 | # .python-version 89 | 90 | # pipenv 91 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 92 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 93 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 94 | # install all needed dependencies. 95 | #Pipfile.lock 96 | 97 | # poetry 98 | # Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. 99 | # This is especially recommended for binary packages to ensure reproducibility, and is more 100 | # commonly ignored for libraries. 101 | # https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control 102 | #poetry.lock 103 | 104 | # pdm 105 | # Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. 106 | #pdm.lock 107 | # pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it 108 | # in version control. 109 | # https://pdm.fming.dev/#use-with-ide 110 | .pdm.toml 111 | 112 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm 113 | __pypackages__/ 114 | 115 | # Celery stuff 116 | celerybeat-schedule 117 | celerybeat.pid 118 | 119 | # SageMath parsed files 120 | *.sage.py 121 | 122 | # Environments 123 | .env 124 | .venv 125 | env/ 126 | venv/ 127 | ENV/ 128 | env.bak/ 129 | venv.bak/ 130 | 131 | # Spyder project settings 132 | .spyderproject 133 | .spyproject 134 | 135 | # Rope project settings 136 | .ropeproject 137 | 138 | # mkdocs documentation 139 | /site 140 | 141 | # mypy 142 | .mypy_cache/ 143 | .dmypy.json 144 | dmypy.json 145 | 146 | # Pyre type checker 147 | .pyre/ 148 | 149 | # pytype static type analyzer 150 | .pytype/ 151 | 152 | # Cython debug symbols 153 | cython_debug/ 154 | 155 | # PyCharm 156 | # JetBrains specific template is maintained in a separate JetBrains.gitignore that can 157 | # be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore 158 | # and can be added to the global gitignore or merged into this file. For a more nuclear 159 | # option (not recommended) you can uncomment the following to ignore the entire idea folder. 160 | .idea/ 161 | 162 | # ignore logs 163 | actpermoma/logs/ 164 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2024, PEARL lab 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Act*Per*MoMa 2 | 3 | Code for the research paper: **Active-Perceptive Motion Generation for Mobile Manipulation** [1] [[Paper](https://arxiv.org/abs/2310.00433)] [[Project site](https://sites.google.com/view/actpermoma?pli=1/)] 4 | 5 |

6 | 7 |

8 | 9 | This code is an active perception pipeline for mobile manipulators with embodied cameras to grasp in cluttered and unscructured scenes. 10 | Specifically, it employs a receding horizon planning approach considering expected information gain and reachability of detected grasps. 11 | 12 | This repository contains a gym-style environment for the Tiago++ mobile manipulator and uses the NVIDIA Isaac Sim simulator (Adapted from OmniIsaacGymEnvs [2]). 13 | 14 | ## Installation 15 | 16 | __Requirements:__ The NVIDIA ISAAC Sim simulator requires a GPU with RT (RayTracing) cores. This typically means an RTX GPU. The recommended specs are provided [here](https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/requirements.html) 17 | Besides this, in our experience, to run the Act*Per*MoMA pipeline, at least 32GB CPU RAM is needed. 18 | 19 | ### Isaac Sim 20 | 21 | - Install isaac-sim on your PC by following the procedure outlined [here](https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/install_workstation.html\) \ 22 | **Note:** This code was tested on isaac-sim **version 2023.1.0-hotfix.1**. \ 23 | [Troubleshooting](https://forums.developer.nvidia.com/t/since-2022-version-error-failed-to-create-change-watch-no-space-left-on-device/218198) (common error when starting up) 24 | 25 | - As we use pinocchio for kinematics [3], we may need to disable isaac motion planning, because at the moment it is incompatible with pinocchio. 26 | In your isaac installation, edit the file omni.isaac.sim.python.kit and comment out lula, motion planning. 27 | 28 | ### Conda Environment 29 | 30 | - Follow the isaac-sim python conda environment installation [here](https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/install_python.html#advanced-running-with-anaconda\) \ 31 | Note that we use a modified version of the isaac-sim conda environment `actpermoma` which needs to be used instead and is available at `env.yaml` in this repo. Don't forget to source the `setup_conda_env.sh` script in the isaac-sim directory before running experiments. (You could also add it to the .bashrc) 32 | 33 | ### Mushroom-RL 34 | Install our fork of the mushroom library [4]: 35 | ``` 36 | git clone https://github.com/iROSA-lab/mushroom-rl.git 37 | cd mushroom-rl 38 | pip install -e . 39 | ``` 40 | 41 | ### VGN 42 | Install the devel branch of the VGN network [5]: 43 | ``` 44 | git clone -b devel https://github.com/ethz-asl/vgn.git 45 | cd vgn 46 | pip install -e . 47 | ``` 48 | Also download the network weights from the VGN repo [https://drive.google.com/file/d/1J3cPjyVQ59LpcLZZrA7EfeV3xTmITchr] and place them inside `vgn/assets`. 49 | 50 | ### robot_helpers 51 | ``` 52 | git clone https://github.com/mbreyer/robot_helpers 53 | cd robot_helpers 54 | pip install -e . 55 | ``` 56 | ### Add the robot reachability map 57 | 58 | Left arm: [https://hessenbox.tu-darmstadt.de/getlink/fiLmB2dHKinaEvugZrNgcuxP/smaller_full_reach_map_gripper_left_grasping_frame_torso_False_0.05.pkl] 59 | Right arm: [https://hessenbox.tu-darmstadt.de/getlink/fiGe1B2vaHZdYZVHuovhze68/smaller_full_reach_map_gripper_right_grasping_frame_torso_False_0.05.pkl] 60 | 61 | Download the reachability maps from the above links and place them in the reachability folder (/actpermoma/algos/reachability/<>). 62 | If you need to generate reachability maps for another robot, have a look at the repo: [https://github.com/iROSA-lab/sampled_reachability_maps] 63 | 64 | ## Launch 65 | - Activate the conda environment: 66 | ``` 67 | conda activate actpermoma 68 | ``` 69 | - source the isaac-sim conda_setup file: 70 | ``` 71 | source /isaac_sim-2023.1.0-hotfix.1/setup_conda_env.sh 72 | ``` 73 | - run the desired method: 74 | ``` 75 | python actpermoma/scripts/active_grasp_pipeline.py task=TiagoDualActivePerception train=TiagoDualActPerMoMa 76 | ``` 77 | 78 | ## References 79 | 80 | [1]: S. Jauhri*, S. Lueth*, and G. Chalvatzaki. Active-perceptive motion generation for mobile manipulation. International Conference on Robotics and Automation (ICRA 2024), 2024. \ 81 | [2]: https://github.com/NVIDIA-Omniverse/OmniIsaacGymEnvs \ 82 | [3]: https://github.com/stack-of-tasks/pinocchio \ 83 | [4]: C. D’Eramo, D. Tateo, A. Bonarini, M. Restelli, and J. Peters, “Mushroom-rl: Simplifying reinforcement learning research,” JMLR, vol. 22, pp. 131:1–131:5, 2021 \ 84 | [5]: M. Breyer, J. J. Chung, L. Ott, R. Siegwart, and J. Nieto. Volumetric Grasping Network: Real-time 6 DOF Grasp Detection in Clutter. Conference on Robot Learning (CoRL 2020), 2020. \ 85 | 86 | ## Troubleshooting 87 | 88 | - **"[Error] [omni.physx.plugin] PhysX error: PxRigidDynamic::setGlobalPose: pose is not valid."** This error can be **ignored** for now. Isaac-sim may have some trouble handling the set_world_pose() function for RigidPrims, but this doesn't affect the experiments. 89 | - **"[Error] no space left on device"** https://forums.developer.nvidia.com/t/since-2022-version-error-failed-to-create-change-watch-no-space-left-on-device/218198 90 | -------------------------------------------------------------------------------- /actpermoma.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pearl-robot-lab/ActPerMoMa/fdc15f5dd6b6c2cafbdbe18e98e97170e7cf5cd0/actpermoma.gif -------------------------------------------------------------------------------- /actpermoma/__init__.py: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /actpermoma/algos/naive.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from copy import deepcopy as cp 3 | 4 | from mushroom_rl.policy import Policy 5 | from mushroom_rl.core import Agent 6 | 7 | import vgn 8 | from pathlib import Path 9 | from vgn.detection import VGN, select_local_maxima 10 | 11 | from actpermoma.utils.visualisation_utils import Visualizer 12 | from actpermoma.utils.algo_utils import AABBox 13 | 14 | from robot_helpers.spatial import Rotation, Transform 15 | 16 | """ 17 | This is the naive policy, going towards an object and trying to execute a grasp. 18 | """ 19 | 20 | 21 | def select_best_grasp(grasps, qualities): 22 | i = np.argmax(qualities) 23 | return grasps[i], qualities[i] 24 | 25 | 26 | class NaivePolicy(Policy): 27 | def __init__(self, cfg): 28 | self.qual_threshold = cfg['quality_threshold'] 29 | self.T = cfg['window_size'] # window size 30 | self.base_goals_radius_grasp = cfg['base_goals_radius_grasp'] 31 | self.base_goals_radius_ig = cfg['base_goals_radius_ig'] 32 | self.max_views = cfg['max_views'] 33 | self.min_path_length = cfg['min_path_length'] 34 | self.min_gain = cfg['min_gain'] 35 | self.arm_activation_radius_base = cfg['arm_activation_radius_base'] 36 | self.arm_activation_radius_goal = cfg['arm_activation_radius_goal'] 37 | self.planner_type = cfg['planner_2d'] 38 | 39 | self.bbox = None 40 | self.T_base_task = None 41 | self.T_task_base = None 42 | self.robot_xy = None 43 | self.robot_R = None 44 | self.goal_pos = None 45 | 46 | self.last_index = None # save the index chosen at the last step 47 | 48 | self.view_template = Rotation.from_matrix(np.array([[ 0, 0, 1], 49 | [-1, 0, 0], 50 | [ 0, -1, 0]])) 51 | self.views = [] # will be a list of tuples (x, y, theta) 52 | 53 | # handle theta coverage 54 | self.min_theta = None 55 | self.max_theta = None 56 | self.last_theta = None 57 | self.seen_theta_turns = 0 # handling crossovers at -pi,pi 58 | self.seen_theta_coverage = 0.0 # total angle covered by views until now 59 | 60 | self.best_grasp = None 61 | self.filtered_grasps = [] 62 | self.stable_filtered_grasps = [] 63 | self.nbv = None 64 | self.aborted = False 65 | 66 | self.base_to_zed_pose = Transform.from_matrix(np.array([[-1.08913264e-05, -4.99811739e-02, 9.98750160e-01, 2.12868273e-01], 67 | [-1.00000000e+00, 5.44363617e-07, -1.08777139e-05, -9.63753913e-04], 68 | [-2.34190445e-12, -9.98750160e-01, -4.99811739e-02, 1.38999854e+00], 69 | [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])) 70 | 71 | self.grasp_network = VGN(Path(vgn.__path__[0] + '/../../assets/models/vgn_conv.pth')) 72 | self.qual_hist = np.zeros((self.T,) + (40,) * 3, np.float32) 73 | self.vis = Visualizer() 74 | self.activated = False 75 | 76 | def activate(self, goal_pos, goal_aabbox, voxel_size): 77 | self.voxel_size = voxel_size 78 | self.bbox = AABBox(goal_aabbox[0], goal_aabbox[1]) 79 | box_height = goal_aabbox[1,2] - goal_aabbox[0,2] 80 | self.T_base_task = Transform.from_translation(goal_pos - np.array([0.15, 0.15, 0])) 81 | self.T_task_base = self.T_base_task.inv() 82 | 83 | self.views = [] # will be a list of tuples (x, y, theta) 84 | self.best_grasp = None 85 | self.nbv = None 86 | self.aborted = False 87 | 88 | return True 89 | 90 | def __call__(self, state, action, *args): 91 | raise NotImplementedError 92 | 93 | def draw_action(self, state): 94 | """ 95 | Sample an action in ``state`` using the policy. 96 | 97 | Args: 98 | state (dict): 'tsdf': current TSDF around the goal (vgn.perception.UniformTSDFVolume), extrinsic already in goal frame 99 | 'goal_pos': np.ndarray with size (3,) (x, y, z relative to the world frame) torch tensor? 100 | 'robot_xy': np.ndarray with size (2,) (x, y relative to the world frame) 101 | 'robot_R': np.ndarray with size (1,) (theta relative to the world frame) 102 | 'view_pose': robot_helpers.spatial.Transfrom 103 | 'obj_states': np.ndarray with size n*(6,) ... -> only relevant for object collision avoidance 104 | 105 | Returns: 106 | The action sampled from the policy: A new 3D base position calculated back from the viewpoint as in 107 | np.ndarray(polar_angle, radius, yaw_angle (view angle o)) 108 | """ 109 | 110 | tsdf = state['tsdf'] # origin will be at the goal_pos 111 | self.goal_pos = state['goal_pos'] # in x, y, z relative to the world frame 112 | self.robot_xy = state['robot_xy'] # in x, y relative to the world frame 113 | self.robot_R = state['robot_R'] # in radians relative to the world frame 114 | self.goal_pos = state['goal_pos'] # in x, y, z relative to the world frame 115 | self.robot_xy = state['robot_xy'] # in x, y relative to the world frame 116 | self.robot_R = state['robot_R'] # in radians relative to the world frame 117 | 118 | if (not self.activated) or state['reset_agent']: 119 | self.activated = self.activate(self.goal_pos, state['goal_aabbox'], tsdf.voxel_size) 120 | 121 | arms = np.array([0, 0]) 122 | # calculate current view 123 | current_view = Transform(translation=state['view_pos'], rotation=self.view_template * Rotation.from_euler('YX', [state['view_theta'], state['view_phi']])) 124 | self.views.append(current_view) 125 | self.calculate_theta_coverage(state['view_theta']) 126 | self.integrate(tsdf) 127 | 128 | base_vel = self.goal_pos[:2] - self.robot_xy 129 | # if 0.8 m close to the object, execute grasp or abort 130 | print(np.linalg.norm(base_vel)) 131 | if np.linalg.norm(base_vel) <= self.arm_activation_radius_goal: 132 | if self.best_grasp is not None: 133 | arms[0] = 1 # always choose left arm 134 | best_grasp = cp(self.best_grasp.pose) 135 | best_grasp.translation = best_grasp.translation + self.T_base_task.translation 136 | self.vis.draw_grasps_at(self.T_base_task * self.best_grasp.pose, self.best_grasp.width) 137 | return best_grasp, (self.seen_theta_coverage, 0), arms 138 | else: 139 | self.aborted = True 140 | return None, (self.seen_theta_coverage, None), arms 141 | 142 | # else move straight towards the object 143 | elif len(self.views) < self.max_views: 144 | # limit base_vel, so that robot doesn't get to close to the goal: 145 | if np.linalg.norm(base_vel) <= self.arm_activation_radius_goal + self.min_path_length - 0.1: 146 | base_vel = base_vel / np.linalg.norm(base_vel) * (np.linalg.norm(base_vel) - self.arm_activation_radius_goal + 0.1) 147 | nbv = self.get_nbv_from_base_vel(base_vel, up_or_down_next=False) # alwasy choose up 148 | self.vis.draw_frame_matrix(nbv.as_matrix(), axis_length=0.1, point_size=2) 149 | return nbv, (self.seen_theta_coverage, None), arms 150 | else: 151 | # no grasps were found after max number of views 152 | self.aborted = True 153 | return None, (self.seen_theta_coverage, None), arms 154 | 155 | def calculate_theta_coverage(self, theta): 156 | # handle infinitie rotation/overturning 157 | theta += self.seen_theta_turns * 2*np.pi 158 | 159 | # if thetas have different signs at the -pi/pi change 160 | if (self.last_theta is not None and 161 | self.last_theta * theta < 0 and 162 | abs(self.last_theta) > np.pi/2 and 163 | abs(theta) > np.pi/2): 164 | 165 | if np.mod(self.last_theta, 2*np.pi) < np.pi: # motion counterclockwise 166 | theta_step = abs(np.mod(self.last_theta, 2*np.pi) - 2 * np.pi - theta) 167 | theta = self.last_theta + theta_step 168 | self.seen_theta_turns +=1 169 | else: # motion clockwise, assuming never going round more than once (will be aborted before that) 170 | theta_step = 2*np.pi - abs(np.mod(self.last_theta, 2*np.pi) - 2 * np.pi - theta) 171 | theta = self.last_theta - theta_step 172 | self.seen_theta_turns -=1 173 | print('theta = last_theta + theta_step: ' + str(theta*180/np.pi) + ' = ' + str(self.last_theta*180/np.pi) + ' + ' + str(theta_step*180/np.pi)) 174 | 175 | if self.min_theta is None: 176 | self.min_theta = theta 177 | elif theta < self.min_theta: 178 | self.seen_theta_coverage += abs(self.min_theta - theta) 179 | self.min_theta = theta 180 | 181 | if self.max_theta is None: 182 | self.max_theta = theta 183 | elif theta > self.max_theta: 184 | self.seen_theta_coverage += abs(self.max_theta - theta) 185 | self.max_theta = theta 186 | 187 | self.last_theta = theta 188 | 189 | def get_nbv_from_base_vel(self, base_vel, up_or_down_next): 190 | nbv_base_pos = self.robot_xy + base_vel 191 | nbv_theta = -(np.arctan2(nbv_base_pos[1] - self.goal_pos[1], nbv_base_pos[0] - self.goal_pos[0]) + np.pi) 192 | base_pose = Transform(translation=np.hstack((nbv_base_pos, np.array([0]))), rotation=Rotation.from_euler('Z', -nbv_theta)) 193 | 194 | nbv_pos = base_pose * self.base_to_zed_pose 195 | nbv_pos.translation[2] = 1.08 if up_or_down_next else 1.42 196 | 197 | nbv_phi = -np.arctan2(np.linalg.norm(nbv_pos.translation[:2] - self.goal_pos[:2]), nbv_pos.translation[2] - self.goal_pos[2]) 198 | 199 | nbv = Transform(translation=nbv_pos.translation, rotation=self.view_template * Rotation.from_euler('YX', [nbv_theta, nbv_phi])) 200 | return nbv 201 | 202 | def draw_noisy_action(self, state): 203 | raise NotImplementedError('Tried to draw a noisy action from the ActiveGraspPolicy') 204 | 205 | def integrate(self, tsdf): 206 | # src:https://github.com/ethz-asl/active_grasp/blob/devel/src/active_grasp/policy.py 207 | # predict grasps on the tsdf using the vgn net 208 | tsdf_grid = tsdf.get_grid() 209 | out = self.grasp_network.predict(tsdf_grid) 210 | colors = self.vis.interpolate_quality_colors(qualities=out.qual[out.qual >= self.qual_threshold].flatten(), qual_min=self.qual_threshold, opacity=0.4) 211 | x, y, z = np.mgrid[0:40, 0:40, 0:40][:, out.qual >= self.qual_threshold] * tsdf.voxel_size 212 | x, y, z = x + self.T_base_task.translation[0], y + self.T_base_task.translation[1], z + self.T_base_task.translation[2] 213 | pcl = np.concatenate((np.expand_dims(x, -1), np.expand_dims(y, -1), np.expand_dims(z, -1)), axis=-1) 214 | self.vis.draw_points(pcl=pcl, colors=colors, point_size=3) 215 | 216 | # record grasp quality 217 | t = (len(self.views) - 1) % self.T 218 | self.qual_hist[t, ...] = out.qual 219 | 220 | # grasp filtering 221 | self.filtered_grasps, self.filtered_qualitites = self.filter_grasps(out, tsdf) 222 | 223 | if len(self.filtered_grasps) > 0: 224 | self.best_grasp, quality = select_best_grasp(self.filtered_grasps, self.filtered_qualitites) 225 | for grasp in self.filtered_grasps: 226 | self.vis.draw_grasps_at(self.T_base_task * grasp.pose, grasp.width) 227 | 228 | else: 229 | self.best_grasp = None 230 | 231 | def filter_grasps(self, out, tsdf): 232 | grasps, qualities = select_local_maxima(tsdf.voxel_size, out, self.qual_threshold) 233 | 234 | filtered_grasps, filtered_qualities = [], [] 235 | for grasp, quality in zip(grasps, qualities): 236 | pose = self.T_base_task * grasp.pose 237 | tip = pose.rotation.apply([0, 0, 0.05]) + pose.translation 238 | if self.bbox.is_inside(tip): 239 | # ignore grasps grasping from 3 cm above the table or under; come from below 240 | high_enough = pose.translation[2] > self.T_base_task.translation[2] + 0.03 241 | from_below = pose.rotation.as_matrix()[2, 2] > 0.0 242 | if high_enough and not from_below: 243 | filtered_grasps.append(grasp) 244 | filtered_qualities.append(quality) 245 | return filtered_grasps, filtered_qualities 246 | 247 | 248 | class NaiveAgent(Agent): 249 | """ 250 | Agent that uses the ActiveGraspTrajPolicy to generate a trajectory of views and grasps to execute. 251 | """ 252 | def __init__(self, mdp_info, agent_params_config): 253 | self._actor_last_loss = None 254 | policy = NaivePolicy(agent_params_config) 255 | 256 | # self.tiago_handler = agent_params_config['robot_handler'] 257 | self.view_template_inv = Rotation.from_matrix(np.array([[0, -1, 0], 258 | [0, 0, -1], 259 | [1, 0, 0]])) 260 | 261 | super().__init__(mdp_info, policy) 262 | 263 | def fit(self, dataset): 264 | pass # this agent does not change based on experience 265 | 266 | def draw_action(self, state): 267 | """ 268 | Return the action to execute in the given state. 269 | Args: 270 | state (np.ndarray): the state where the agent is. 271 | Returns: 272 | The action to be executed. 273 | """ 274 | if self.phi is not None: 275 | state = self.phi(state) 276 | 277 | if self.next_action is None: 278 | # check for empty depth map in first steps 279 | if state['tsdf'].get_scene_cloud().is_empty(): 280 | return np.zeros((10,)) 281 | 282 | tmp, tmp2, arms = self.policy.draw_action(state) # tmp2[0] is seen theta coverage 283 | 284 | if arms[0] == 1 or arms[1] == 1: # arm activation 285 | if tmp is not None: 286 | pos = tmp.translation 287 | rot = tmp.rotation.as_euler('ZYX') 288 | else: 289 | pos = np.zeros(3) 290 | rot = np.zeros(3) 291 | tmp2 = [0] 292 | return np.hstack((pos, rot, *tmp2, arms)) 293 | else: 294 | nbv = tmp 295 | if nbv is not None: 296 | ## Transform nbv to 5D velocity 297 | view_vel_trans = nbv.translation - state['view_pos'] # relative to base 298 | 299 | # (psi), theta, phi defined as local z-y-x euler angles relative to the view template frame 300 | nbv_rel_mat = (self.view_template_inv * nbv.rotation).as_matrix() 301 | theta_nbv = np.arctan2(-nbv_rel_mat[2, 0], nbv_rel_mat[0, 0]) 302 | phi_nbv = np.arctan2(-nbv_rel_mat[1, 2], nbv_rel_mat[1, 1]) 303 | 304 | theta_vel = theta_nbv - state['view_theta'] 305 | if abs(theta_vel) > np.pi: theta_vel -= np.sign(theta_vel) * 2 * np.pi 306 | phi_vel = phi_nbv - state['view_phi'] 307 | 308 | return np.hstack((view_vel_trans, theta_vel, phi_vel, [0], tmp2[0] ,[0], arms)) 309 | else: 310 | # no grasps found 311 | return np.ones((10,)) 312 | else: 313 | action = self.next_action 314 | self.next_action = None 315 | 316 | return action -------------------------------------------------------------------------------- /actpermoma/algos/reachability/reachability_evaluator.py: -------------------------------------------------------------------------------- 1 | import os 2 | import pickle 3 | import numpy as np 4 | from scipy.spatial.transform import Rotation 5 | 6 | 7 | ######################################################################## 8 | # Reachability score evaluator 9 | # (based on https://github.com/iROSA-lab/sampled_reachability_maps) 10 | # Author: Snehal Jauhri, 2023 11 | ######################################################################## 12 | 13 | class ReachabilityEvaluator: 14 | def __init__(self, reach_map_path=None, arm='both', cartesian_res=0.05, angular_res=np.pi / 8): 15 | if reach_map_path is None: 16 | reach_map_path = os.path.dirname(os.path.realpath(__file__)) 17 | self.arm = arm 18 | self.cartesian_res = cartesian_res 19 | self.angular_res = angular_res 20 | 21 | # Load reachability maps 22 | if arm == 'right' or arm == 'both': 23 | with open(os.path.join(reach_map_path, 24 | 'smaller_full_reach_map_gripper_right_grasping_frame_torso_False_0.05.pkl'), 25 | 'rb') as f: 26 | self.right_reachability_map = pickle.load(f) 27 | if arm == 'left' or arm == 'both': 28 | with open(os.path.join(reach_map_path, 29 | 'smaller_full_reach_map_gripper_left_grasping_frame_torso_False_0.05.pkl'), 30 | 'rb') as f: 31 | self.left_reachability_map = pickle.load(f) 32 | 33 | def get_scores(self, goal_tfs, arm='both', return_max=False): 34 | # Get reachability scores for 6D goal poses 35 | 36 | left_scores = np.zeros((len(goal_tfs))) 37 | right_scores = np.zeros((len(goal_tfs))) 38 | 39 | goal_poses = np.zeros((len(goal_tfs), 6)) 40 | for id, goal_tf in enumerate(goal_tfs): 41 | # Get the 6D euler goal pose 42 | goal_poses[id] = np.hstack( 43 | (goal_tf[:3, -1], Rotation.from_matrix(goal_tf[:3, :3]).as_euler('XYZ'))) # INTRINSIC XYZ 44 | 45 | if arm == 'left' or arm == 'both': 46 | min_y, max_y, = (-0.6, 1.35) 47 | min_x, max_x, = (-1.2, 1.2) 48 | min_z, max_z, = (-0.35, 2.1) 49 | min_roll, max_roll, = (-np.pi, np.pi) 50 | min_pitch, max_pitch, = (-np.pi / 2, np.pi / 2) 51 | min_yaw, max_yaw, = (-np.pi, np.pi) 52 | cartesian_res = self.cartesian_res 53 | angular_res = self.angular_res 54 | 55 | # Mask valid goal_poses that are inside the min-max xyz bounds of the reachability map 56 | mask = np.logical_and.reduce((goal_poses[:, 0] > min_x, goal_poses[:, 0] < max_x, 57 | goal_poses[:, 1] > min_y, goal_poses[:, 1] < max_y, 58 | goal_poses[:, 2] > min_z, goal_poses[:, 2] < max_z)) 59 | 60 | x_bins = np.ceil((max_x - min_x) / cartesian_res) 61 | y_bins = np.ceil((max_y - min_y) / cartesian_res) 62 | z_bins = np.ceil((max_z - min_z) / cartesian_res) 63 | roll_bins = np.ceil((max_roll - min_roll) / angular_res) 64 | pitch_bins = np.ceil((max_pitch - min_pitch) / angular_res) 65 | yaw_bins = np.ceil((max_yaw - min_yaw) / angular_res) 66 | 67 | # Define the offset values for indexing the map 68 | x_ind_offset = y_bins * z_bins * roll_bins * pitch_bins * yaw_bins 69 | y_ind_offset = z_bins * roll_bins * pitch_bins * yaw_bins 70 | z_ind_offset = roll_bins * pitch_bins * yaw_bins 71 | roll_ind_offset = pitch_bins * yaw_bins 72 | pitch_ind_offset = yaw_bins 73 | yaw_ind_offset = 1 74 | 75 | # Convert the input pose to voxel coordinates 76 | x_idx = (np.floor((goal_poses[mask, 0] - min_x) / cartesian_res)).astype(int) 77 | y_idx = (np.floor((goal_poses[mask, 1] - min_y) / cartesian_res)).astype(int) 78 | z_idx = (np.floor((goal_poses[mask, 2] - min_z) / cartesian_res)).astype(int) 79 | roll_idx = (np.floor((goal_poses[mask, 3] - min_roll) / angular_res)).astype(int) 80 | pitch_idx = (np.floor((goal_poses[mask, 4] - min_pitch) / angular_res)).astype(int) 81 | yaw_idx = (np.floor((goal_poses[mask, 5] - min_yaw) / angular_res)).astype(int) 82 | # Handle edge cases of discretization (angles can especially cause issues if values contain both ends [-pi, pi] which we don't want 83 | roll_idx = np.clip(roll_idx, 0, roll_bins - 1) 84 | pitch_idx = np.clip(pitch_idx, 0, pitch_bins - 1) 85 | yaw_idx = np.clip(yaw_idx, 0, yaw_bins - 1) 86 | 87 | # Compute the index in the reachability map array 88 | map_idx = x_idx * x_ind_offset + y_idx * y_ind_offset + z_idx * z_ind_offset + roll_idx \ 89 | * roll_ind_offset + pitch_idx * pitch_ind_offset + yaw_idx * yaw_ind_offset 90 | 91 | # Get the score from the score map array 92 | left_scores[mask] = self.left_reachability_map[map_idx.astype(int), -1] # -1 is the score index 93 | 94 | if arm == 'right' or arm == 'both': 95 | min_y, max_y, = (-1.35, 0.6) 96 | min_x, max_x, = (-1.2, 1.2) 97 | min_z, max_z, = (-0.35, 2.1) 98 | min_roll, max_roll, = (-np.pi, np.pi) 99 | min_pitch, max_pitch, = (-np.pi / 2, np.pi / 2) 100 | min_yaw, max_yaw, = (-np.pi, np.pi) 101 | cartesian_res = self.cartesian_res 102 | angular_res = self.angular_res 103 | 104 | # Mask valid goal_poses that are inside the min-max xyz bounds of the reachability map 105 | mask = np.logical_and.reduce((goal_poses[:, 0] > min_x, goal_poses[:, 0] < max_x, 106 | goal_poses[:, 1] > min_y, goal_poses[:, 1] < max_y, 107 | goal_poses[:, 2] > min_z, goal_poses[:, 2] < max_z)) 108 | 109 | x_bins = np.ceil((max_x - min_x) / cartesian_res) 110 | y_bins = np.ceil((max_y - min_y) / cartesian_res) 111 | z_bins = np.ceil((max_z - min_z) / cartesian_res) 112 | roll_bins = np.ceil((max_roll - min_roll) / angular_res) 113 | pitch_bins = np.ceil((max_pitch - min_pitch) / angular_res) 114 | yaw_bins = np.ceil((max_yaw - min_yaw) / angular_res) 115 | 116 | # Define the offset values for indexing the map 117 | x_ind_offset = y_bins * z_bins * roll_bins * pitch_bins * yaw_bins 118 | y_ind_offset = z_bins * roll_bins * pitch_bins * yaw_bins 119 | z_ind_offset = roll_bins * pitch_bins * yaw_bins 120 | roll_ind_offset = pitch_bins * yaw_bins 121 | pitch_ind_offset = yaw_bins 122 | yaw_ind_offset = 1 123 | 124 | # Convert the input pose to voxel coordinates 125 | x_idx = (np.floor((goal_poses[mask, 0] - min_x) / cartesian_res)).astype(int) 126 | y_idx = (np.floor((goal_poses[mask, 1] - min_y) / cartesian_res)).astype(int) 127 | z_idx = (np.floor((goal_poses[mask, 2] - min_z) / cartesian_res)).astype(int) 128 | roll_idx = (np.floor((goal_poses[mask, 3] - min_roll) / angular_res)).astype(int) 129 | pitch_idx = (np.floor((goal_poses[mask, 4] - min_pitch) / angular_res)).astype(int) 130 | yaw_idx = (np.floor((goal_poses[mask, 5] - min_yaw) / angular_res)).astype(int) 131 | # Handle edge cases of discretization (angles can especially cause issues if values contain both ends [-pi, pi] which we don't want 132 | roll_idx = np.clip(roll_idx, 0, roll_bins - 1) 133 | pitch_idx = np.clip(pitch_idx, 0, pitch_bins - 1) 134 | yaw_idx = np.clip(yaw_idx, 0, yaw_bins - 1) 135 | 136 | # Compute the index in the reachability map array 137 | map_idx = x_idx * x_ind_offset + y_idx * y_ind_offset + z_idx * z_ind_offset + roll_idx \ 138 | * roll_ind_offset + pitch_idx * pitch_ind_offset + yaw_idx * yaw_ind_offset 139 | 140 | # Get the score from the score map array 141 | right_scores[mask] = self.right_reachability_map[map_idx.astype(int), -1] # -1 is the score index 142 | 143 | if arm == 'both': 144 | if return_max: 145 | return np.maximum(left_scores, right_scores) 146 | else: 147 | return left_scores, right_scores 148 | elif arm == 'left': 149 | return left_scores 150 | elif arm == 'right': 151 | return right_scores 152 | -------------------------------------------------------------------------------- /actpermoma/algos/sample_2D/env.py: -------------------------------------------------------------------------------- 1 | """ 2 | Environment for rrt_2D 3 | @author: huiming zhou 4 | """ 5 | 6 | class Env: 7 | def __init__(self, x_range=50, y_range=30, obs_coords=None): 8 | self.x_range = (0, x_range) 9 | self.y_range = (0, y_range) 10 | if obs_coords is not None: 11 | self.obs_boundary = [] 12 | self.obs_circle = [[x, y, 1] for x, y in zip(obs_coords[:,0], obs_coords[:,1])] 13 | self.obs_rectangle = [] 14 | else: 15 | self.obs_boundary = self.obs_boundary() 16 | self.obs_circle = self.obs_circle() 17 | self.obs_rectangle = self.obs_rectangle() 18 | 19 | @staticmethod 20 | def obs_boundary(): 21 | obs_boundary = [ 22 | [0, 0, 1, 30], 23 | [0, 30, 50, 1], 24 | [1, 0, 50, 1], 25 | [50, 1, 1, 30] 26 | ] 27 | return obs_boundary 28 | 29 | @staticmethod 30 | def obs_rectangle(): 31 | obs_rectangle = [ 32 | [14, 12, 8, 2], 33 | [18, 22, 8, 3], 34 | [26, 7, 2, 12], 35 | [32, 14, 10, 2] 36 | ] 37 | return obs_rectangle 38 | 39 | @staticmethod 40 | def obs_circle(): 41 | obs_cir = [ 42 | [7, 12, 3], 43 | [46, 20, 2], 44 | [15, 5, 2], 45 | [37, 7, 3], 46 | [37, 23, 3] 47 | ] 48 | 49 | return obs_cir -------------------------------------------------------------------------------- /actpermoma/algos/sample_2D/plotting.py: -------------------------------------------------------------------------------- 1 | """ 2 | Plotting tools for Sampling-based algorithms 3 | @author: huiming zhou 4 | """ 5 | 6 | import matplotlib.pyplot as plt 7 | import matplotlib.patches as patches 8 | from . import env 9 | 10 | class Plotting: 11 | def __init__(self, x_start, x_goal, Env=None): 12 | self.xI, self.xG = x_start, x_goal 13 | if Env is not None: 14 | self.env = Env 15 | else: 16 | self.env = env.Env() 17 | self.obs_bound = self.env.obs_boundary 18 | self.obs_circle = self.env.obs_circle 19 | self.obs_rectangle = self.env.obs_rectangle 20 | 21 | def animation(self, nodelist, path, name, animation=False): 22 | self.plot_grid(name) 23 | self.plot_visited(nodelist, animation) 24 | self.plot_path(path) 25 | 26 | def animation_connect(self, V1, V2, path, name): 27 | self.plot_grid(name) 28 | self.plot_visited_connect(V1, V2) 29 | self.plot_path(path) 30 | 31 | def plot_grid(self, name): 32 | fig, ax = plt.subplots() 33 | 34 | for (ox, oy, w, h) in self.obs_bound: 35 | ax.add_patch( 36 | patches.Rectangle( 37 | (ox, oy), w, h, 38 | edgecolor='black', 39 | facecolor='black', 40 | fill=True 41 | ) 42 | ) 43 | 44 | for (ox, oy, w, h) in self.obs_rectangle: 45 | ax.add_patch( 46 | patches.Rectangle( 47 | (ox, oy), w, h, 48 | edgecolor='black', 49 | facecolor='gray', 50 | fill=True 51 | ) 52 | ) 53 | 54 | for (ox, oy, r) in self.obs_circle: 55 | ax.add_patch( 56 | patches.Circle( 57 | (ox, oy), r, 58 | edgecolor='black', 59 | facecolor='gray', 60 | fill=True 61 | ) 62 | ) 63 | 64 | plt.plot(self.xI[0], self.xI[1], "bs", linewidth=3) 65 | plt.plot(self.xG[0], self.xG[1], "gs", linewidth=3) 66 | 67 | plt.title(name) 68 | plt.axis("equal") 69 | 70 | @staticmethod 71 | def plot_visited(nodelist, animation): 72 | if animation: 73 | count = 0 74 | for node in nodelist: 75 | count += 1 76 | if node.parent: 77 | plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g") 78 | plt.gcf().canvas.mpl_connect('key_release_event', 79 | lambda event: 80 | [exit(0) if event.key == 'escape' else None]) 81 | if count % 10 == 0: 82 | plt.pause(0.001) 83 | else: 84 | for node in nodelist: 85 | if node.parent: 86 | plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g") 87 | 88 | @staticmethod 89 | def plot_visited_connect(V1, V2): 90 | len1, len2 = len(V1), len(V2) 91 | 92 | for k in range(max(len1, len2)): 93 | if k < len1: 94 | if V1[k].parent: 95 | plt.plot([V1[k].x, V1[k].parent.x], [V1[k].y, V1[k].parent.y], "-g") 96 | if k < len2: 97 | if V2[k].parent: 98 | plt.plot([V2[k].x, V2[k].parent.x], [V2[k].y, V2[k].parent.y], "-g") 99 | 100 | plt.gcf().canvas.mpl_connect('key_release_event', 101 | lambda event: [exit(0) if event.key == 'escape' else None]) 102 | 103 | if k % 2 == 0: 104 | plt.pause(0.001) 105 | 106 | plt.pause(0.01) 107 | 108 | @staticmethod 109 | def plot_path(path): 110 | if len(path) != 0: 111 | plt.plot([x[0] for x in path], [x[1] for x in path], '-r', linewidth=2) 112 | plt.pause(0.01) 113 | plt.show() -------------------------------------------------------------------------------- /actpermoma/algos/sample_2D/queue.py: -------------------------------------------------------------------------------- 1 | import collections 2 | import heapq 3 | 4 | 5 | class QueueFIFO: 6 | """ 7 | Class: QueueFIFO 8 | Description: QueueFIFO is designed for First-in-First-out rule. 9 | """ 10 | 11 | def __init__(self): 12 | self.queue = collections.deque() 13 | 14 | def empty(self): 15 | return len(self.queue) == 0 16 | 17 | def put(self, node): 18 | self.queue.append(node) # enter from back 19 | 20 | def get(self): 21 | return self.queue.popleft() # leave from front 22 | 23 | 24 | class QueueLIFO: 25 | """ 26 | Class: QueueLIFO 27 | Description: QueueLIFO is designed for Last-in-First-out rule. 28 | """ 29 | 30 | def __init__(self): 31 | self.queue = collections.deque() 32 | 33 | def empty(self): 34 | return len(self.queue) == 0 35 | 36 | def put(self, node): 37 | self.queue.append(node) # enter from back 38 | 39 | def get(self): 40 | return self.queue.pop() # leave from back 41 | 42 | 43 | class QueuePrior: 44 | """ 45 | Class: QueuePrior 46 | Description: QueuePrior reorders elements using value [priority] 47 | """ 48 | 49 | def __init__(self): 50 | self.queue = [] 51 | 52 | def empty(self): 53 | return len(self.queue) == 0 54 | 55 | def put(self, item, priority): 56 | heapq.heappush(self.queue, (priority, item)) # reorder s using priority 57 | 58 | def get(self): 59 | return heapq.heappop(self.queue)[1] # pop out the smallest item 60 | 61 | def enumerate(self): 62 | return self.queue -------------------------------------------------------------------------------- /actpermoma/algos/sample_2D/rrt.py: -------------------------------------------------------------------------------- 1 | """ 2 | RRT_2D 3 | @author: huiming zhou 4 | """ 5 | import math 6 | import numpy as np 7 | 8 | from . import env, plotting, utils 9 | 10 | 11 | class Node: 12 | def __init__(self, n): 13 | self.x = n[0] 14 | self.y = n[1] 15 | self.parent = None 16 | 17 | 18 | class Rrt: 19 | def __init__(self, s_start, s_goal, step_len, goal_sample_rate, iter_max): 20 | self.s_start = Node(s_start) 21 | self.s_goal = Node(s_goal) 22 | self.step_len = step_len 23 | self.goal_sample_rate = goal_sample_rate 24 | self.iter_max = iter_max 25 | self.vertex = [self.s_start] 26 | 27 | self.env = env.Env() 28 | self.plotting = plotting.Plotting(s_start, s_goal) 29 | self.utils = utils.Utils() 30 | 31 | self.x_range = self.env.x_range 32 | self.y_range = self.env.y_range 33 | self.obs_circle = self.env.obs_circle 34 | self.obs_rectangle = self.env.obs_rectangle 35 | self.obs_boundary = self.env.obs_boundary 36 | 37 | def planning(self): 38 | for i in range(self.iter_max): 39 | node_rand = self.generate_random_node(self.goal_sample_rate) 40 | node_near = self.nearest_neighbor(self.vertex, node_rand) 41 | node_new = self.new_state(node_near, node_rand) 42 | 43 | if node_new and not self.utils.is_collision(node_near, node_new): 44 | self.vertex.append(node_new) 45 | dist, _ = self.get_distance_and_angle(node_new, self.s_goal) 46 | 47 | if dist <= self.step_len and not self.utils.is_collision(node_new, self.s_goal): 48 | self.new_state(node_new, self.s_goal) 49 | return self.extract_path(node_new) 50 | 51 | return None 52 | 53 | def generate_random_node(self, goal_sample_rate): 54 | delta = self.utils.delta 55 | 56 | if np.random.random() > goal_sample_rate: 57 | return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta), 58 | np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta))) 59 | 60 | return self.s_goal 61 | 62 | @staticmethod 63 | def nearest_neighbor(node_list, n): 64 | return node_list[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y) 65 | for nd in node_list]))] 66 | 67 | def new_state(self, node_start, node_end): 68 | dist, theta = self.get_distance_and_angle(node_start, node_end) 69 | 70 | dist = min(self.step_len, dist) 71 | node_new = Node((node_start.x + dist * math.cos(theta), 72 | node_start.y + dist * math.sin(theta))) 73 | node_new.parent = node_start 74 | 75 | return node_new 76 | 77 | def extract_path(self, node_end): 78 | path = [(self.s_goal.x, self.s_goal.y)] 79 | node_now = node_end 80 | 81 | while node_now.parent is not None: 82 | node_now = node_now.parent 83 | path.append((node_now.x, node_now.y)) 84 | 85 | return path 86 | 87 | @staticmethod 88 | def get_distance_and_angle(node_start, node_end): 89 | dx = node_end.x - node_start.x 90 | dy = node_end.y - node_start.y 91 | return math.hypot(dx, dy), math.atan2(dy, dx) 92 | 93 | 94 | def main(): 95 | x_start = (2, 2) # Starting node 96 | x_goal = (49, 24) # Goal node 97 | 98 | rrt = Rrt(x_start, x_goal, 0.5, 0.05, 10000) 99 | path = rrt.planning() 100 | 101 | if path: 102 | rrt.plotting.animation(rrt.vertex, path, "RRT", True) 103 | else: 104 | print("No Path Found!") 105 | 106 | 107 | if __name__ == '__main__': 108 | main() -------------------------------------------------------------------------------- /actpermoma/algos/sample_2D/rrt_star.py: -------------------------------------------------------------------------------- 1 | """ 2 | RRT_star 2D 3 | @author: huiming zhou 4 | """ 5 | import math 6 | import numpy as np 7 | 8 | from . import env, plotting, utils, queue 9 | 10 | 11 | class Node: 12 | def __init__(self, n): 13 | self.x = n[0] 14 | self.y = n[1] 15 | self.parent = None 16 | 17 | 18 | class RrtStar: 19 | def __init__(self, x_start, x_goal, step_len, 20 | goal_sample_rate, search_radius, iter_max, x_range=50, y_range=30, obs_coords=None): 21 | self.s_start = Node(x_start) 22 | self.s_goal = Node(x_goal) 23 | self.step_len = step_len 24 | self.goal_sample_rate = goal_sample_rate 25 | self.search_radius = search_radius 26 | self.iter_max = iter_max 27 | self.vertex = [self.s_start] 28 | self.path = [] 29 | 30 | self.env = env.Env(x_range, y_range, obs_coords) 31 | self.plotting = plotting.Plotting(x_start, x_goal, Env=self.env) 32 | self.utils = utils.Utils() 33 | 34 | self.x_range = self.env.x_range 35 | self.y_range = self.env.y_range 36 | self.obs_circle = self.env.obs_circle 37 | self.obs_rectangle = self.env.obs_rectangle 38 | self.obs_boundary = self.env.obs_boundary 39 | 40 | def reset(self): 41 | self.vertex = [self.s_start] 42 | self.path = [] 43 | 44 | def planning(self, plotting=False): 45 | for k in range(self.iter_max): 46 | node_rand = self.generate_random_node(self.goal_sample_rate) 47 | node_near = self.nearest_neighbor(self.vertex, node_rand) 48 | node_new = self.new_state(node_near, node_rand) 49 | 50 | if k % 500 == 0: 51 | print(k) 52 | 53 | if node_new and not self.utils.is_collision(node_near, node_new): 54 | neighbor_index = self.find_near_neighbor(node_new) 55 | self.vertex.append(node_new) 56 | 57 | if neighbor_index: 58 | self.choose_parent(node_new, neighbor_index) 59 | self.rewire(node_new, neighbor_index) 60 | 61 | index = self.search_goal_parent() 62 | self.path = self.extract_path(self.vertex[index]) 63 | 64 | if plotting: 65 | self.plotting.animation(self.vertex, self.path, "rrt*, N = " + str(self.iter_max)) 66 | 67 | def new_state(self, node_start, node_goal): 68 | dist, theta = self.get_distance_and_angle(node_start, node_goal) 69 | 70 | dist = min(self.step_len, dist) 71 | node_new = Node((node_start.x + dist * math.cos(theta), 72 | node_start.y + dist * math.sin(theta))) 73 | 74 | node_new.parent = node_start 75 | 76 | return node_new 77 | 78 | def choose_parent(self, node_new, neighbor_index): 79 | cost = [self.get_new_cost(self.vertex[i], node_new) for i in neighbor_index] 80 | 81 | cost_min_index = neighbor_index[int(np.argmin(cost))] 82 | node_new.parent = self.vertex[cost_min_index] 83 | 84 | def rewire(self, node_new, neighbor_index): 85 | for i in neighbor_index: 86 | node_neighbor = self.vertex[i] 87 | 88 | if self.cost(node_neighbor) > self.get_new_cost(node_new, node_neighbor): 89 | node_neighbor.parent = node_new 90 | 91 | def search_goal_parent(self): 92 | dist_list = [math.hypot(n.x - self.s_goal.x, n.y - self.s_goal.y) for n in self.vertex] 93 | node_index = [i for i in range(len(dist_list)) if dist_list[i] <= self.step_len] 94 | 95 | if len(node_index) > 0: 96 | cost_list = [dist_list[i] + self.cost(self.vertex[i]) for i in node_index 97 | if not self.utils.is_collision(self.vertex[i], self.s_goal)] 98 | return node_index[int(np.argmin(cost_list))] 99 | 100 | return len(self.vertex) - 1 101 | 102 | def get_new_cost(self, node_start, node_end): 103 | dist, _ = self.get_distance_and_angle(node_start, node_end) 104 | 105 | return self.cost(node_start) + dist 106 | 107 | def generate_random_node(self, goal_sample_rate): 108 | delta = self.utils.delta 109 | 110 | if np.random.random() > goal_sample_rate: 111 | return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta), 112 | np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta))) 113 | 114 | return self.s_goal 115 | 116 | def find_near_neighbor(self, node_new): 117 | n = len(self.vertex) + 1 118 | r = min(self.search_radius * math.sqrt((math.log(n) / n)), self.step_len) 119 | 120 | dist_table = [math.hypot(nd.x - node_new.x, nd.y - node_new.y) for nd in self.vertex] 121 | dist_table_index = [ind for ind in range(len(dist_table)) if dist_table[ind] <= r and 122 | not self.utils.is_collision(node_new, self.vertex[ind])] 123 | 124 | return dist_table_index 125 | 126 | @staticmethod 127 | def nearest_neighbor(node_list, n): 128 | return node_list[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y) 129 | for nd in node_list]))] 130 | 131 | @staticmethod 132 | def cost(node_p): 133 | node = node_p 134 | cost = 0.0 135 | 136 | while node.parent: 137 | cost += math.hypot(node.x - node.parent.x, node.y - node.parent.y) 138 | node = node.parent 139 | 140 | return cost 141 | 142 | def update_cost(self, parent_node): 143 | OPEN = queue.QueueFIFO() 144 | OPEN.put(parent_node) 145 | 146 | while not OPEN.empty(): 147 | node = OPEN.get() 148 | 149 | if len(node.child) == 0: 150 | continue 151 | 152 | for node_c in node.child: 153 | node_c.Cost = self.get_new_cost(node, node_c) 154 | OPEN.put(node_c) 155 | 156 | def extract_path(self, node_end): 157 | path = [[self.s_goal.x, self.s_goal.y]] 158 | node = node_end 159 | 160 | while node.parent is not None: 161 | path.append([node.x, node.y]) 162 | node = node.parent 163 | path.append([node.x, node.y]) 164 | 165 | return path 166 | 167 | @staticmethod 168 | def get_distance_and_angle(node_start, node_end): 169 | dx = node_end.x - node_start.x 170 | dy = node_end.y - node_start.y 171 | return math.hypot(dx, dy), math.atan2(dy, dx) 172 | 173 | 174 | def main(): 175 | x_start = (18, 8) # Starting node 176 | x_goal = (37, 18) # Goal node 177 | 178 | rrt_star = RrtStar(x_start, x_goal, 10, 0.10, 20, 10000) 179 | rrt_star.planning(plotting=True) 180 | 181 | 182 | if __name__ == '__main__': 183 | main() -------------------------------------------------------------------------------- /actpermoma/algos/sample_2D/utils.py: -------------------------------------------------------------------------------- 1 | """ 2 | utils for collision check 3 | @author: huiming zhou 4 | """ 5 | 6 | import math 7 | import numpy as np 8 | 9 | from . import env 10 | from .rrt import Node 11 | 12 | 13 | class Utils: 14 | def __init__(self): 15 | self.env = env.Env() 16 | 17 | self.delta = 0.5 18 | self.obs_circle = self.env.obs_circle 19 | self.obs_rectangle = self.env.obs_rectangle 20 | self.obs_boundary = self.env.obs_boundary 21 | 22 | def update_obs(self, obs_cir, obs_bound, obs_rec): 23 | self.obs_circle = obs_cir 24 | self.obs_boundary = obs_bound 25 | self.obs_rectangle = obs_rec 26 | 27 | def get_obs_vertex(self): 28 | delta = self.delta 29 | obs_list = [] 30 | 31 | for (ox, oy, w, h) in self.obs_rectangle: 32 | vertex_list = [[ox - delta, oy - delta], 33 | [ox + w + delta, oy - delta], 34 | [ox + w + delta, oy + h + delta], 35 | [ox - delta, oy + h + delta]] 36 | obs_list.append(vertex_list) 37 | 38 | return obs_list 39 | 40 | def is_intersect_rec(self, start, end, o, d, a, b): 41 | v1 = [o[0] - a[0], o[1] - a[1]] 42 | v2 = [b[0] - a[0], b[1] - a[1]] 43 | v3 = [-d[1], d[0]] 44 | 45 | div = np.dot(v2, v3) 46 | 47 | if div == 0: 48 | return False 49 | 50 | t1 = np.linalg.norm(np.cross(v2, v1)) / div 51 | t2 = np.dot(v1, v3) / div 52 | 53 | if t1 >= 0 and 0 <= t2 <= 1: 54 | shot = Node((o[0] + t1 * d[0], o[1] + t1 * d[1])) 55 | dist_obs = self.get_dist(start, shot) 56 | dist_seg = self.get_dist(start, end) 57 | if dist_obs <= dist_seg: 58 | return True 59 | 60 | return False 61 | 62 | def is_intersect_circle(self, o, d, a, r): 63 | d2 = np.dot(d, d) 64 | delta = self.delta 65 | 66 | if d2 == 0: 67 | return False 68 | 69 | t = np.dot([a[0] - o[0], a[1] - o[1]], d) / d2 70 | 71 | if 0 <= t <= 1: 72 | shot = Node((o[0] + t * d[0], o[1] + t * d[1])) 73 | if self.get_dist(shot, Node(a)) <= r + delta: 74 | return True 75 | 76 | return False 77 | 78 | def is_collision(self, start, end): 79 | if self.is_inside_obs(start) or self.is_inside_obs(end): 80 | return True 81 | 82 | o, d = self.get_ray(start, end) 83 | obs_vertex = self.get_obs_vertex() 84 | 85 | for (v1, v2, v3, v4) in obs_vertex: 86 | if self.is_intersect_rec(start, end, o, d, v1, v2): 87 | return True 88 | if self.is_intersect_rec(start, end, o, d, v2, v3): 89 | return True 90 | if self.is_intersect_rec(start, end, o, d, v3, v4): 91 | return True 92 | if self.is_intersect_rec(start, end, o, d, v4, v1): 93 | return True 94 | 95 | for (x, y, r) in self.obs_circle: 96 | if self.is_intersect_circle(o, d, [x, y], r): 97 | return True 98 | 99 | return False 100 | 101 | def is_inside_obs(self, node): 102 | delta = self.delta 103 | 104 | for (x, y, r) in self.obs_circle: 105 | if math.hypot(node.x - x, node.y - y) <= r + delta: 106 | return True 107 | 108 | for (x, y, w, h) in self.obs_rectangle: 109 | if 0 <= node.x - (x - delta) <= w + 2 * delta \ 110 | and 0 <= node.y - (y - delta) <= h + 2 * delta: 111 | return True 112 | 113 | for (x, y, w, h) in self.obs_boundary: 114 | if 0 <= node.x - (x - delta) <= w + 2 * delta \ 115 | and 0 <= node.y - (y - delta) <= h + 2 * delta: 116 | return True 117 | 118 | return False 119 | 120 | @staticmethod 121 | def get_ray(start, end): 122 | orig = [start.x, start.y] 123 | direc = [end.x - start.x, end.y - start.y] 124 | return orig, direc 125 | 126 | @staticmethod 127 | def get_dist(start, end): 128 | return math.hypot(end.x - start.x, end.y - start.y) -------------------------------------------------------------------------------- /actpermoma/algos/search_2D/Astar.py: -------------------------------------------------------------------------------- 1 | """ 2 | A_star 2D 3 | @author: huiming zhou 4 | """ 5 | 6 | import math 7 | import heapq 8 | 9 | from . import env 10 | 11 | 12 | class AStar: 13 | """AStar set the cost + heuristics as the priority 14 | """ 15 | def __init__(self, s_start, s_goal, heuristic_type, x_range=51, y_range=31, obs_coords=None): 16 | self.s_start = s_start 17 | self.s_goal = s_goal 18 | self.heuristic_type = heuristic_type 19 | 20 | self.Env = env.Env(x_range, y_range, obs_coords) # class Env 21 | 22 | self.u_set = self.Env.motions # feasible input set 23 | self.obs = self.Env.obs # position of obstacles 24 | 25 | self.OPEN = [] # priority queue / OPEN set 26 | self.CLOSED = [] # CLOSED set / VISITED order 27 | self.PARENT = dict() # recorded parent 28 | self.g = dict() # cost to come 29 | 30 | def reset(self): 31 | self.OPEN = [] 32 | self.CLOSED = [] 33 | self.PARENT = dict() 34 | self.g = dict() 35 | 36 | def searching(self): 37 | """ 38 | A_star Searching. 39 | :return: path, visited order 40 | """ 41 | 42 | self.PARENT[self.s_start] = self.s_start 43 | self.g[self.s_start] = 0 44 | self.g[self.s_goal] = math.inf 45 | heapq.heappush(self.OPEN, 46 | (self.f_value(self.s_start), self.s_start)) 47 | 48 | while self.OPEN: 49 | _, s = heapq.heappop(self.OPEN) 50 | self.CLOSED.append(s) 51 | 52 | if s == self.s_goal: # stop condition 53 | break 54 | 55 | for s_n in self.get_neighbor(s): 56 | new_cost = self.g[s] + self.cost(s, s_n) 57 | 58 | if s_n not in self.g: 59 | self.g[s_n] = math.inf 60 | 61 | if new_cost < self.g[s_n]: # conditions for updating Cost 62 | self.g[s_n] = new_cost 63 | self.PARENT[s_n] = s 64 | heapq.heappush(self.OPEN, (self.f_value(s_n), s_n)) 65 | 66 | return self.extract_path(self.PARENT), self.CLOSED 67 | 68 | def searching_repeated_astar(self, e): 69 | """ 70 | repeated A*. 71 | :param e: weight of A* 72 | :return: path and visited order 73 | """ 74 | 75 | path, visited = [], [] 76 | 77 | while e >= 1: 78 | p_k, v_k = self.repeated_searching(self.s_start, self.s_goal, e) 79 | path.append(p_k) 80 | visited.append(v_k) 81 | e -= 0.5 82 | 83 | return path, visited 84 | 85 | def repeated_searching(self, s_start, s_goal, e): 86 | """ 87 | run A* with weight e. 88 | :param s_start: starting state 89 | :param s_goal: goal state 90 | :param e: weight of a* 91 | :return: path and visited order. 92 | """ 93 | 94 | g = {s_start: 0, s_goal: float("inf")} 95 | PARENT = {s_start: s_start} 96 | OPEN = [] 97 | CLOSED = [] 98 | heapq.heappush(OPEN, 99 | (g[s_start] + e * self.heuristic(s_start), s_start)) 100 | 101 | while OPEN: 102 | _, s = heapq.heappop(OPEN) 103 | CLOSED.append(s) 104 | 105 | if s == s_goal: 106 | break 107 | 108 | for s_n in self.get_neighbor(s): 109 | new_cost = g[s] + self.cost(s, s_n) 110 | 111 | if s_n not in g: 112 | g[s_n] = math.inf 113 | 114 | if new_cost < g[s_n]: # conditions for updating Cost 115 | g[s_n] = new_cost 116 | PARENT[s_n] = s 117 | heapq.heappush(OPEN, (g[s_n] + e * self.heuristic(s_n), s_n)) 118 | 119 | return self.extract_path(PARENT), CLOSED 120 | 121 | def get_neighbor(self, s): 122 | """ 123 | find neighbors of state s that not in obstacles. 124 | :param s: state 125 | :return: neighbors 126 | """ 127 | 128 | return [(s[0] + u[0], s[1] + u[1]) for u in self.u_set] 129 | 130 | def cost(self, s_start, s_goal): 131 | """ 132 | Calculate Cost for this motion 133 | :param s_start: starting node 134 | :param s_goal: end node 135 | :return: Cost for this motion 136 | :note: Cost function could be more complicate! 137 | """ 138 | 139 | if self.is_collision(s_start, s_goal): 140 | return math.inf 141 | 142 | return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1]) 143 | 144 | def is_collision(self, s_start, s_end): 145 | """ 146 | check if the line segment (s_start, s_end) is collision. 147 | :param s_start: start node 148 | :param s_end: end node 149 | :return: True: is collision / False: not collision 150 | """ 151 | 152 | if s_start in self.obs or s_end in self.obs: 153 | return True 154 | 155 | if s_start[0] != s_end[0] and s_start[1] != s_end[1]: 156 | if s_end[0] - s_start[0] == s_start[1] - s_end[1]: 157 | s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1])) 158 | s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1])) 159 | else: 160 | s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1])) 161 | s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1])) 162 | 163 | if s1 in self.obs or s2 in self.obs: 164 | return True 165 | 166 | return False 167 | 168 | def f_value(self, s): 169 | """ 170 | f = g + h. (g: Cost to come, h: heuristic value) 171 | :param s: current state 172 | :return: f 173 | """ 174 | 175 | return self.g[s] + self.heuristic(s) 176 | 177 | def extract_path(self, PARENT): 178 | """ 179 | Extract the path based on the PARENT set. 180 | :return: The planning path 181 | """ 182 | 183 | path = [self.s_goal] 184 | s = self.s_goal 185 | 186 | while True: 187 | s = PARENT[s] 188 | path.append(s) 189 | 190 | if s == self.s_start: 191 | break 192 | 193 | return list(path) 194 | 195 | def heuristic(self, s): 196 | """ 197 | Calculate heuristic. 198 | :param s: current node (state) 199 | :return: heuristic function value 200 | """ 201 | 202 | heuristic_type = self.heuristic_type # heuristic type 203 | goal = self.s_goal # goal node 204 | 205 | if heuristic_type == "manhattan": 206 | return abs(goal[0] - s[0]) + abs(goal[1] - s[1]) 207 | else: 208 | return math.hypot(goal[0] - s[0], goal[1] - s[1]) 209 | 210 | 211 | def main(): 212 | s_start = (5, 5) 213 | s_goal = (45, 25) 214 | 215 | astar = AStar(s_start, s_goal, "euclidean") 216 | import plotting 217 | plot = plotting.Plotting(s_start, s_goal) 218 | 219 | path, visited = astar.searching() 220 | plot.animation(path, visited, "A*") # animation 221 | 222 | 223 | if __name__ == '__main__': 224 | main() 225 | -------------------------------------------------------------------------------- /actpermoma/algos/search_2D/env.py: -------------------------------------------------------------------------------- 1 | """ 2 | Env 2D 3 | @author: huiming zhou 4 | """ 5 | 6 | 7 | class Env: 8 | def __init__(self, x_range=51, y_range=31, obs_coords=None): 9 | self.x_range = x_range # size of background 10 | self.y_range = y_range 11 | self.motions = [(-1, 0), (-1, 1), (0, 1), (1, 1), 12 | (1, 0), (1, -1), (0, -1), (-1, -1)] 13 | self.obs = self.obs_map(obs_coords) 14 | 15 | def update_obs(self, obs): 16 | self.obs = obs 17 | 18 | def obs_map(self, obs_coords=None): 19 | """ 20 | Initialize obstacles' positions 21 | :return: map of obstacles 22 | """ 23 | 24 | x = self.x_range 25 | y = self.y_range 26 | obs = set() 27 | 28 | if obs_coords is not None: 29 | for coord in obs_coords: 30 | obs.add(tuple(coord)) 31 | else: 32 | # pre-defined map 33 | for i in range(x): 34 | obs.add((i, 0)) 35 | for i in range(x): 36 | obs.add((i, y - 1)) 37 | 38 | for i in range(y): 39 | obs.add((0, i)) 40 | for i in range(y): 41 | obs.add((x - 1, i)) 42 | 43 | for i in range(10, 21): 44 | obs.add((i, 15)) 45 | for i in range(15): 46 | obs.add((20, i)) 47 | 48 | for i in range(15, 30): 49 | obs.add((30, i)) 50 | for i in range(16): 51 | obs.add((40, i)) 52 | 53 | return obs -------------------------------------------------------------------------------- /actpermoma/algos/search_2D/plotting.py: -------------------------------------------------------------------------------- 1 | """ 2 | Plot tools 2D 3 | @author: huiming zhou 4 | """ 5 | 6 | import matplotlib.pyplot as plt 7 | 8 | 9 | class Plotting: 10 | def __init__(self, xI, xG, env=None): 11 | self.xI, self.xG = xI, xG 12 | if env is not None: 13 | self.env = env 14 | self.obs = env.obs 15 | else: 16 | self.env = env.Env() 17 | self.obs = self.env.obs_map() 18 | 19 | def update_obs(self, obs): 20 | self.obs = obs 21 | 22 | def animation(self, path, visited, name): 23 | self.plot_grid(name) 24 | self.plot_visited(visited) 25 | self.plot_path(path) 26 | plt.show() 27 | 28 | def animation_lrta(self, path, visited, name): 29 | self.plot_grid(name) 30 | cl = self.color_list_2() 31 | path_combine = [] 32 | 33 | for k in range(len(path)): 34 | self.plot_visited(visited[k], cl[k]) 35 | plt.pause(0.2) 36 | self.plot_path(path[k]) 37 | path_combine += path[k] 38 | plt.pause(0.2) 39 | if self.xI in path_combine: 40 | path_combine.remove(self.xI) 41 | self.plot_path(path_combine) 42 | plt.show() 43 | 44 | def animation_ara_star(self, path, visited, name): 45 | self.plot_grid(name) 46 | cl_v, cl_p = self.color_list() 47 | 48 | for k in range(len(path)): 49 | self.plot_visited(visited[k], cl_v[k]) 50 | self.plot_path(path[k], cl_p[k], True) 51 | 52 | plt.show() 53 | 54 | def animation_bi_astar(self, path, v_fore, v_back, name): 55 | self.plot_grid(name) 56 | self.plot_visited_bi(v_fore, v_back) 57 | self.plot_path(path) 58 | plt.show() 59 | 60 | def plot_grid(self, name): 61 | obs_x = [x[0] for x in self.obs] 62 | obs_y = [x[1] for x in self.obs] 63 | 64 | plt.plot(self.xI[0], self.xI[1], "bs") 65 | plt.plot(self.xG[0], self.xG[1], "gs") 66 | plt.plot(obs_x, obs_y, "sk") 67 | plt.title(name) 68 | plt.axis("equal") 69 | 70 | def plot_visited(self, visited, cl='gray'): 71 | if self.xI in visited: 72 | visited.remove(self.xI) 73 | 74 | if self.xG in visited: 75 | visited.remove(self.xG) 76 | 77 | count = 0 78 | 79 | for x in visited: 80 | count += 1 81 | plt.plot(x[0], x[1], color=cl, marker='o') 82 | plt.gcf().canvas.mpl_connect('key_release_event', 83 | lambda event: [exit(0) if event.key == 'escape' else None]) 84 | 85 | if count < len(visited) / 3: 86 | length = 20 87 | elif count < len(visited) * 2 / 3: 88 | length = 30 89 | else: 90 | length = 40 91 | 92 | if count % length == 0: 93 | plt.pause(0.001) 94 | plt.pause(0.01) 95 | 96 | def plot_path(self, path, cl='r', flag=False): 97 | path_x = [path[i][0] for i in range(len(path))] 98 | path_y = [path[i][1] for i in range(len(path))] 99 | 100 | if not flag: 101 | plt.plot(path_x, path_y, linewidth='3', color='r') 102 | else: 103 | plt.plot(path_x, path_y, linewidth='3', color=cl) 104 | 105 | plt.plot(self.xI[0], self.xI[1], "bs") 106 | plt.plot(self.xG[0], self.xG[1], "gs") 107 | 108 | plt.pause(0.01) 109 | 110 | def plot_visited_bi(self, v_fore, v_back): 111 | if self.xI in v_fore: 112 | v_fore.remove(self.xI) 113 | 114 | if self.xG in v_back: 115 | v_back.remove(self.xG) 116 | 117 | len_fore, len_back = len(v_fore), len(v_back) 118 | 119 | for k in range(max(len_fore, len_back)): 120 | if k < len_fore: 121 | plt.plot(v_fore[k][0], v_fore[k][1], linewidth='3', color='gray', marker='o') 122 | if k < len_back: 123 | plt.plot(v_back[k][0], v_back[k][1], linewidth='3', color='cornflowerblue', marker='o') 124 | 125 | plt.gcf().canvas.mpl_connect('key_release_event', 126 | lambda event: [exit(0) if event.key == 'escape' else None]) 127 | 128 | if k % 10 == 0: 129 | plt.pause(0.001) 130 | plt.pause(0.01) 131 | 132 | @staticmethod 133 | def color_list(): 134 | cl_v = ['silver', 135 | 'wheat', 136 | 'lightskyblue', 137 | 'royalblue', 138 | 'slategray'] 139 | cl_p = ['gray', 140 | 'orange', 141 | 'deepskyblue', 142 | 'red', 143 | 'm'] 144 | return cl_v, cl_p 145 | 146 | @staticmethod 147 | def color_list_2(): 148 | cl = ['silver', 149 | 'steelblue', 150 | 'dimgray', 151 | 'cornflowerblue', 152 | 'dodgerblue', 153 | 'royalblue', 154 | 'plum', 155 | 'mediumslateblue', 156 | 'mediumpurple', 157 | 'blueviolet', 158 | ] 159 | return cl -------------------------------------------------------------------------------- /actpermoma/cfg/config.yaml: -------------------------------------------------------------------------------- 1 | # set default task and default training config based on task 2 | defaults: 3 | - task: TiagoDualActiveGraspTraj 4 | - train: TiagoDualActiveGraspTraj 5 | - hydra/job_logging: disabled 6 | 7 | # Task name - used to pick the class to load 8 | task_name: ${task.name} 9 | # experiment name. defaults to name of training config 10 | experiment: '' 11 | 12 | # if set to positive integer, overrides the default number of environments 13 | num_envs: 1 14 | # enable/disable headless mode and rendering (in pygame window) 15 | headless: False 16 | render: True 17 | 18 | # seed - set to -1 to choose random seed 19 | seed: -1 20 | # set to True for deterministic performance 21 | torch_deterministic: True 22 | # (Optional) Use the isaac sim configuration from file 23 | sim_app_cfg_path: "" # "/isaac_app_configs/omni.isaac.sim.python.kit" 24 | # set the maximum number of learning iterations to train for. overrides default per-environment setting 25 | # max_iterations: 500 26 | 27 | ## Device config 28 | physics_engine: 'physx' 29 | # whether to use cpu or gpu pipeline 30 | pipeline: 'cpu' # 'gpu' 31 | # whether to use cpu or gpu physx 32 | sim_device: 'cpu' # 'gpu' 33 | # used for gpu simulation only - device id for running sim and task if pipeline=gpu 34 | device_id: 0 35 | # device to run RL 36 | rl_device: 'cuda:0' #'cpu' # 'cuda:0' 37 | 38 | ## PhysX arguments 39 | num_threads: 4 # Number of worker threads per scene used by PhysX - for CPU PhysX only. 40 | solver_type: 1 # 0: pgs, 1: tgs 41 | 42 | # RL Arguments 43 | # test - if set, run policy in inference mode (requires setting checkpoint to load) 44 | test: False 45 | # used to set checkpoint path 46 | checkpoint: '' 47 | # number of seeds to run (run sequentially by default) 48 | num_seeds: 1 49 | 50 | # Optional - set to True to enable wandb logging 51 | wandb_activate: False 52 | 53 | 54 | # set the directory where the output files get saved 55 | hydra: 56 | output_subdir: null 57 | run: 58 | dir: . -------------------------------------------------------------------------------- /actpermoma/cfg/task/TiagoDualActivePerception.yaml: -------------------------------------------------------------------------------- 1 | # used to create the object 2 | name: TiagoDualActivePerception 3 | 4 | physics_engine: ${..physics_engine} 5 | 6 | # if given, will override the device setting in gym. 7 | env: 8 | numEnvs: 1 9 | envSpacing: 3.0 10 | resetDist: 1.0 11 | maxEffort: 400.0 12 | 13 | horizon: 30 14 | 15 | enableDebugVis: False 16 | use_torso: True # Use torso joint too 17 | randomize_robot_on_reset: False 18 | # Hybrid action space 19 | continous_actions: 8 # (SE2 base pose, torso joint, head_joint_2 ~ depending on the 5D camera velocity output by the policy) 6DOF grasp pose & best_base_rot AND in both cases theta_covered 20 | discrete_actions: 2 # 0 or 1 (arm activation) 21 | # Set custom state and action space limits 22 | world_xy_radius: 2.0 # metres 23 | scene_type: 'simple' # 'simple' or 'complex' 24 | num_obstacles: 2 # can be tabular 25 | num_grasp_objects: 6 # YCB objects 26 | action_xy_radius: 0.3 # position control # calculated from 0.1 m/s max with 3 s max time 27 | occ_map_radius: 4.0 # metres 28 | occ_map_cell_size: 0.08 # ).1 metres 29 | action_ang_lim: 3.14159265 # pi 30 | action_head_2_center: -0.13 # possible movement from -0.98 to 0.72 31 | action_head_2_rad: 0.85 32 | 33 | # camera intrinsics 34 | fx: 230.5 35 | fy: 230.5 36 | cx: 160.0 37 | cy: 120.0 38 | 39 | # success and rewards: 40 | goal_pos_thresh: 0.01 # metres 41 | goal_ang_thresh: 0.08726646259 # 5*np.pi/180 42 | reward_success: 1.0 43 | 44 | controlFrequencyInv: 5 # ~24 Hz 45 | 46 | 47 | sim: 48 | dt: 0.0083 # 1/120 s 49 | use_gpu_pipeline: ${eq:${...pipeline},"gpu"} 50 | gravity: [0.0, 0.0, -9.81] 51 | add_ground_plane: True 52 | add_distant_light: True 53 | use_flatcache: False 54 | enable_scene_query_support: False 55 | disable_contact_processing: False 56 | 57 | enable_cameras: True 58 | 59 | default_physics_material: 60 | static_friction: 0.7 61 | dynamic_friction: 0.6 62 | restitution: 0.0 63 | 64 | physx: 65 | worker_thread_count: ${....num_threads} 66 | solver_type: ${....solver_type} 67 | use_gpu: ${eq:${....sim_device},"gpu"} 68 | solver_position_iteration_count: 4 69 | solver_velocity_iteration_count: 0 70 | contact_offset: 0.005 71 | rest_offset: 0.0 72 | bounce_threshold_velocity: 0.2 73 | friction_offset_threshold: 0.01 74 | friction_correlation_distance: 0.025 75 | enable_sleeping: True 76 | enable_stabilization: True 77 | max_depenetration_velocity: 5.0 78 | 79 | # GPU buffers 80 | gpu_max_rigid_contact_count: 524288 81 | gpu_max_rigid_patch_count: 81920 82 | gpu_found_lost_pairs_capacity: 8192 83 | gpu_found_lost_aggregate_pairs_capacity: 262144 84 | gpu_total_aggregate_pairs_capacity: 8192 85 | gpu_max_soft_body_contacts: 1048576 86 | gpu_max_particle_contacts: 1048576 87 | gpu_heap_capacity: 67108864 88 | gpu_temp_buffer_capacity: 16777216 89 | gpu_max_num_partitions: 8 90 | 91 | # sim asset configs here 92 | TiagoDualHolo: 93 | # -1 to use default values 94 | override_usd_defaults: False 95 | fixed_base: False # Needs to be false here but can be set in usd file. Even with fixed_base, virtual joints can still be used to move the base. 96 | enable_self_collisions: False 97 | enable_gyroscopic_forces: True 98 | # also in stage params 99 | # per-actor 100 | solver_position_iteration_count: 4 101 | solver_velocity_iteration_count: 0 102 | sleep_threshold: 0.005 103 | stabilization_threshold: 0.005 104 | # per-body 105 | density: -1 106 | -------------------------------------------------------------------------------- /actpermoma/cfg/task/TiagoDualActivePerceptionNaive.yaml: -------------------------------------------------------------------------------- 1 | # used to create the object 2 | name: TiagoDualActivePerceptionNaive 3 | 4 | physics_engine: ${..physics_engine} 5 | 6 | # if given, will override the device setting in gym. 7 | env: 8 | numEnvs: 1 9 | envSpacing: 3.0 10 | resetDist: 1.0 11 | maxEffort: 400.0 12 | 13 | horizon: 30 14 | 15 | enableDebugVis: False 16 | # Move group to be used 17 | move_group: "arm_right" # String. Can be arm_left or arm_right 18 | use_torso: True # Use torso joint too 19 | randomize_robot_on_reset: False 20 | # Hybrid action space 21 | continous_actions: 7 # (SE2 base pose, torso joint, head_joint_2 ~ depending on the 5D camera velocity output by the policy) 6DOF grasp pose & width 22 | discrete_actions: 2 # 0 or 1 (arm activation) 23 | # Set custom state and action space limits 24 | world_xy_radius: 2.0 # metres 25 | num_obstacles: 1 # table 26 | num_grasp_objects: 5 # YCB objects 27 | action_xy_radius: 0.3 # position control # calculated from 0.1 m/s max with 3 s max time 28 | action_ang_lim: 3.14159265 # pi 29 | action_head_2_center: -0.13 # possible movement from -0.98 to 0.72 30 | action_head_2_rad: 0.85 31 | 32 | # camera intrinsics 33 | fx: 230.5 34 | fy: 230.5 35 | cx: 160.0 36 | cy: 120.0 37 | 38 | # success and rewards: 39 | goal_pos_thresh: 0.01 # metres 40 | goal_ang_thresh: 0.08726646259 # 5*np.pi/180 41 | reward_success: 1.0 42 | 43 | controlFrequencyInv: 5 # 60 Hz 44 | 45 | 46 | sim: 47 | dt: 0.0083 # 1/120 s 48 | use_gpu_pipeline: ${eq:${...pipeline},"gpu"} 49 | gravity: [0.0, 0.0, -9.81] 50 | add_ground_plane: True 51 | add_distant_light: True 52 | use_flatcache: False 53 | enable_scene_query_support: False 54 | disable_contact_processing: False 55 | 56 | enable_cameras: True 57 | 58 | default_physics_material: 59 | static_friction: 0.7 60 | dynamic_friction: 0.6 61 | restitution: 0.0 62 | 63 | physx: 64 | worker_thread_count: ${....num_threads} 65 | solver_type: ${....solver_type} 66 | use_gpu: ${eq:${....sim_device},"gpu"} 67 | solver_position_iteration_count: 4 68 | solver_velocity_iteration_count: 0 69 | contact_offset: 0.005 70 | rest_offset: 0.0 71 | bounce_threshold_velocity: 0.2 72 | friction_offset_threshold: 0.01 73 | friction_correlation_distance: 0.025 74 | enable_sleeping: True 75 | enable_stabilization: True 76 | max_depenetration_velocity: 5.0 77 | 78 | # GPU buffers 79 | gpu_max_rigid_contact_count: 524288 80 | gpu_max_rigid_patch_count: 81920 81 | gpu_found_lost_pairs_capacity: 8192 82 | gpu_found_lost_aggregate_pairs_capacity: 262144 83 | gpu_total_aggregate_pairs_capacity: 8192 84 | gpu_max_soft_body_contacts: 1048576 85 | gpu_max_particle_contacts: 1048576 86 | gpu_heap_capacity: 67108864 87 | gpu_temp_buffer_capacity: 16777216 88 | gpu_max_num_partitions: 8 89 | 90 | # sim asset configs here 91 | TiagoDualHolo: 92 | # -1 to use default values 93 | override_usd_defaults: False 94 | fixed_base: False # Needs to be false here but can be set in usd file. Even with fixed_base, virtual joints can still be used to move the base. 95 | enable_self_collisions: False 96 | enable_gyroscopic_forces: True 97 | # also in stage params 98 | # per-actor 99 | solver_position_iteration_count: 4 100 | solver_velocity_iteration_count: 0 101 | sleep_threshold: 0.005 102 | stabilization_threshold: 0.005 103 | # per-body 104 | density: -1 105 | -------------------------------------------------------------------------------- /actpermoma/cfg/train/TiagoDualActPerMoMa.yaml: -------------------------------------------------------------------------------- 1 | name: TiagoDualActPerMoMa 2 | 3 | params: 4 | algo: 5 | name: ActPerMoMa 6 | 7 | load_checkpoint: ${if:${...checkpoint},True,False} # flag which sets whether to load the checkpoint 8 | load_path: ${...checkpoint} # path to the checkpoint to load 9 | 10 | config: 11 | name: ${resolve_default:${....train.name},${....experiment}} 12 | device: ${....rl_device} 13 | device_name: ${....rl_device} 14 | 15 | quality_threshold: 0.8 # 0.85 TODO 16 | window_size: 1 17 | min_z_dist: 1.9 # 1.4 18 | max_views: 40 19 | min_gain: 25 20 | gain_discount_fac: 0.9 # TODO 21 | base_goals_radius_grasp: 0.75 # 0.8 radius of candidate goals around target object 22 | base_goals_radius_ig: 1 # radius of candidate goals around target object 23 | min_path_length: 0.3 # minimum path length to consider for weighting 24 | 25 | momentum: 800 26 | deadlock_theta: 0.0175 # 1 deg 27 | gain_weighting: None # '/dist**2', None 28 | gain_weighting_factor: 0.5 # prob that information can be collected after grasp execution stage reached # 0.5, 1.0 (w_ig) 29 | seen_theta_thresh: 0.78539816339 # np.pi/4 30 | use_reachability: True 31 | arm_activation_radius_base: 0.10 # 10 cms 32 | arm_activation_radius_goal: 0.85 # 80 cm (~ dist to arm joint 5 when extended) 33 | planner_2d: 'astar' # 'rrtstar' 34 | steps_along_path: 5 # steps along path to take as next base_goal 35 | 36 | prior_agents: null 37 | n_epochs: 1 38 | n_episodes: 30 # training 39 | n_episodes_test: 500 -------------------------------------------------------------------------------- /actpermoma/cfg/train/TiagoDualActiveGrasp.yaml: -------------------------------------------------------------------------------- 1 | name: TiagoDualActiveGrasp 2 | 3 | params: 4 | algo: 5 | name: ActiveGrasp 6 | 7 | load_checkpoint: ${if:${...checkpoint},True,False} # flag which sets whether to load the checkpoint 8 | load_path: ${...checkpoint} # path to the checkpoint to load 9 | 10 | config: 11 | name: ${resolve_default:${....train.name},${....experiment}} 12 | device: ${....rl_device} 13 | device_name: ${....rl_device} 14 | 15 | quality_threshold: 0.8 # 0.85 TODO 16 | window_size: 1 17 | min_z_dist: 1.9 # 1.4 18 | max_views: 30 19 | min_gain: 25 20 | gain_discount_fac: 0.9 # TODO 21 | base_goals_radius_grasp: 0.75 # 0.8 radius of candidate goals around target object 22 | base_goals_radius_ig: 1 # radius of candidate goals around target object 23 | min_path_length: 0.3 # minimum path length to consider for weighting 24 | 25 | momentum: 800 26 | deadlock_theta: 0.0175 # 1 deg 27 | gain_weighting: None # '/dist**2', None 28 | gain_weighting_factor: 0.5 # prob that information can be collected after grasp execution stage reached # 0.5, 1.0 (w_ig) 29 | seen_theta_thresh: 0.78539816339 # np.pi/4 30 | use_reachability: False 31 | arm_activation_radius_base: 0.10 # 10 cms 32 | arm_activation_radius_goal: 0.85 # 80 cm (~ dist to arm joint 5 when extended) 33 | planner_2d: 'astar' # 'rrtstar' 34 | steps_along_path: 5 # steps along path to take as next base_goal 35 | 36 | prior_agents: null 37 | n_epochs: 1 38 | n_episodes: 30 # training 39 | n_episodes_test: 100 -------------------------------------------------------------------------------- /actpermoma/cfg/train/TiagoDualNaive.yaml: -------------------------------------------------------------------------------- 1 | name: TiagoDualNaive 2 | 3 | params: 4 | algo: 5 | name: Naive 6 | 7 | load_checkpoint: ${if:${...checkpoint},True,False} # flag which sets whether to load the checkpoint 8 | load_path: ${...checkpoint} # path to the checkpoint to load 9 | 10 | config: 11 | name: ${resolve_default:${....train.name},${....experiment}} 12 | device: ${....rl_device} 13 | device_name: ${....rl_device} 14 | 15 | quality_threshold: 0.8 # 0.85 TODO 16 | window_size: 1 17 | min_z_dist: 1.9 # 1.4 18 | max_views: 40 19 | min_gain: 25 20 | gain_discount_fac: 0.9 # TODO 21 | base_goals_radius_grasp: 0.75 # 0.8 radius of candidate goals around target object 22 | base_goals_radius_ig: 1 # radius of candidate goals around target object 23 | min_path_length: 0.3 # minimum path length to consider for weighting 24 | 25 | momentum: 800 26 | deadlock_theta: 0.0175 # 1 deg 27 | gain_weighting: None # '/dist**2', None 28 | gain_weighting_factor: 0.5 # prob that information can be collected after grasp execution stage reached # 0.5, 1.0 (w_ig) 29 | seen_theta_thresh: 0.78539816339 # np.pi/4 30 | use_reachability: False 31 | arm_activation_radius_base: 0.10 # 10 cms 32 | arm_activation_radius_goal: 0.85 # 80 cm (~ dist to arm joint 5 when extended) 33 | planner_2d: 'astar' # 'rrtstar' 34 | steps_along_path: 5 # steps along path to take as next base_goal 35 | 36 | prior_agents: null 37 | n_epochs: 1 38 | n_episodes: 30 # training 39 | n_episodes_test: 500 -------------------------------------------------------------------------------- /actpermoma/cfg/train/TiagoDualRandom.yaml: -------------------------------------------------------------------------------- 1 | name: TiagoDualRandom 2 | 3 | params: 4 | algo: 5 | name: Random 6 | 7 | load_checkpoint: ${if:${...checkpoint},True,False} # flag which sets whether to load the checkpoint 8 | load_path: ${...checkpoint} # path to the checkpoint to load 9 | 10 | config: 11 | name: ${resolve_default:${....train.name},${....experiment}} 12 | device: ${....rl_device} 13 | device_name: ${....rl_device} 14 | 15 | quality_threshold: 0.8 # 0.85 TODO 16 | window_size: 1 17 | min_z_dist: 1.9 # 1.4 18 | max_views: 40 19 | min_gain: 25 20 | gain_discount_fac: 0.9 # TODO 21 | base_goals_radius_grasp: 0.75 # 0.8 radius of candidate goals around target object 22 | base_goals_radius_ig: 1 # radius of candidate goals around target object 23 | min_path_length: 0.3 # minimum path length to consider for weighting 24 | 25 | momentum: 800 26 | deadlock_theta: 0.0175 # 1 deg 27 | gain_weighting: None # '/dist**2', None 28 | gain_weighting_factor: 0.5 # prob that information can be collected after grasp execution stage reached # 0.5, 1.0 (w_ig) 29 | seen_theta_thresh: 0.78539816339 # np.pi/4 30 | use_reachability: False 31 | arm_activation_radius_base: 0.10 # 10 cms 32 | arm_activation_radius_goal: 0.85 # 80 cm (~ dist to arm joint 5 when extended) 33 | planner_2d: 'astar' # 'rrtstar' 34 | steps_along_path: 5 # steps along path to take as next base_goal 35 | 36 | prior_agents: null 37 | n_epochs: 1 38 | n_episodes: 30 # training 39 | n_episodes_test: 500 -------------------------------------------------------------------------------- /actpermoma/envs/isaac_env_mushroom.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2021, NVIDIA CORPORATION. All rights reserved. 2 | # 3 | # NVIDIA CORPORATION and its licensors retain all intellectual property 4 | # and proprietary rights in and to this software, related documentation 5 | # and any modifications thereto. Any use, reproduction, disclosure or 6 | # distribution of this software and related documentation without an express 7 | # license agreement from NVIDIA CORPORATION is strictly prohibited. 8 | # 9 | import pinocchio as pin # Optional: Needs to be imported before SimApp to avoid dependency issues 10 | from omni.isaac.kit import SimulationApp 11 | import actpermoma 12 | import numpy as np 13 | import torch 14 | import carb 15 | import os 16 | 17 | from mushroom_rl.core import Environment, MDPInfo 18 | from mushroom_rl.utils.viewer import ImageViewer 19 | 20 | RENDER_WIDTH = 1280 # 1600 21 | RENDER_HEIGHT = 720 # 900 22 | # RENDER_DT = 1.0/60.0 # 60 Hz # Unused 23 | 24 | class IsaacEnvMushroom(Environment): 25 | """ This class provides a base interface for connecting RL policies with task implementations. 26 | APIs provided in this interface follow the interface in gym.Env and Mushroom Environemnt. 27 | This class also provides utilities for initializing simulation apps, creating the World, 28 | and registering a task. 29 | """ 30 | 31 | def __init__(self, headless: bool, render: bool, sim_app_cfg_path: str) -> None: 32 | """ Initializes RL and task parameters. 33 | 34 | Args: 35 | headless (bool): Whether to run training headless. 36 | render (bool): Whether to run simulation rendering (if rendering using the ImageViewer or saving the renders). 37 | """ 38 | # TODO: Set isaac sim config from file (if provided) 39 | # if sim_app_cfg_path: sim_app_cfg_path = os.path.dirname(actpermoma.__file__) + sim_app_cfg_path 40 | self._simulation_app = SimulationApp({#"experience": sim_app_cfg_path, 41 | "headless": headless, 42 | "window_width": 1920, 43 | "window_height": 1080, 44 | "width": RENDER_WIDTH, 45 | "height": RENDER_HEIGHT}) 46 | carb.settings.get_settings().set("/persistent/omnihydra/useSceneGraphInstancing", True) 47 | self._run_sim_rendering = ((not headless) or render) # tells the simulator to also perform rendering in addition to physics 48 | self._render = self._run_sim_rendering 49 | 50 | # # Optional ImageViewer 51 | # self._viewer = ImageViewer([RENDER_WIDTH, RENDER_HEIGHT], RENDER_DT) 52 | self.sim_frame_count = 0 53 | 54 | def set_task(self, task, backend="torch", sim_params=None, init_sim=True) -> None: 55 | """ Creates a World object and adds Task to World. 56 | Initializes and registers task to the environment interface. 57 | Triggers task start-up. 58 | 59 | Args: 60 | task (RLTask): The task to register to the env. 61 | backend (str): Backend to use for task. Can be "numpy" or "torch". 62 | sim_params (dict): Simulation parameters for physics settings. Defaults to None. 63 | init_sim (Optional[bool]): Automatically starts simulation. Defaults to True. 64 | """ 65 | 66 | from omni.isaac.core.world import World 67 | 68 | self._device = "cpu" 69 | if sim_params and "use_gpu_pipeline" in sim_params: 70 | if sim_params["use_gpu_pipeline"]: 71 | self._device = sim_params["sim_device"] 72 | 73 | # Set render_dt to 0.0 to speed up rendering 74 | self._world = World( 75 | stage_units_in_meters=1.0, rendering_dt=0.0, backend=backend, sim_params=sim_params, device=self._device 76 | ) 77 | self._world.add_task(task) 78 | self._task = task 79 | self._num_envs = self._task.num_envs 80 | # assert (self._num_envs == 1), "Mushroom Env cannot currently handle running multiple environments in parallel! Set num_envs to 1" 81 | 82 | self.observation_space = self._task.observation_space 83 | self.action_space = self._task.action_space 84 | self.num_states = self._task.num_states # Optional 85 | self.state_space = self._task.state_space # Optional 86 | gamma = 1.0 87 | horizon = self._task._max_episode_length 88 | 89 | # Create MDP info for mushroom 90 | mdp_info = MDPInfo(self.observation_space, self.action_space, gamma, horizon) 91 | super().__init__(mdp_info) 92 | 93 | if init_sim: 94 | self._world.reset() 95 | 96 | def render(self) -> None: 97 | """ Step the simulation renderer and display task render in ImageViewer. 98 | """ 99 | 100 | self._world.render() 101 | # Get render from task 102 | task_render = self._task.get_render() 103 | # Display 104 | self._viewer.display(task_render) 105 | return 106 | 107 | def get_render(self): 108 | """ Step the simulation renderer and return the render as per the task. 109 | """ 110 | 111 | self._world.render() 112 | return self._task.get_render() 113 | 114 | def close(self) -> None: 115 | """ Closes simulation. 116 | """ 117 | 118 | self._simulation_app.close() 119 | return 120 | 121 | def seed(self, seed=-1): 122 | """ Sets a seed. Pass in -1 for a random seed. 123 | 124 | Args: 125 | seed (int): Seed to set. Defaults to -1. 126 | Returns: 127 | seed (int): Seed that was set. 128 | """ 129 | 130 | from omni.isaac.core.utils.torch.maths import set_seed 131 | 132 | return set_seed(seed) 133 | 134 | def step(self, action): 135 | """ Basic implementation for stepping simulation. 136 | Can be overriden by inherited Env classes 137 | to satisfy requirements of specific RL libraries. This method passes actions to task 138 | for processing, steps simulation, and computes observations, rewards, and resets. 139 | 140 | Args: 141 | action (numpy.ndarray): Action from policy. 142 | Returns: 143 | observation(numpy.ndarray): observation data. 144 | reward(numpy.ndarray): rewards data. 145 | done(numpy.ndarray): reset/done data. 146 | info(dict): Dictionary of extra data. 147 | """ 148 | # pass action to task for processing 149 | task_actions = torch.unsqueeze(torch.tensor(action,dtype=torch.float,device=self._device),dim=0) 150 | self._task.pre_physics_step(task_actions) 151 | 152 | # allow users to specify the control frequency through config 153 | for _ in range(self._task.control_frequency_inv): 154 | self._world.step(render=self._run_sim_rendering) 155 | self.sim_frame_count += 1 156 | 157 | obs, rews, resets, extras = self._task.post_physics_step() # buffers of obs, reward, dones and infos. Need to be squeezed 158 | 159 | observation = obs[0].cpu().numpy() 160 | reward = rews[0].cpu().item() 161 | done = resets[0].cpu().item() 162 | info = extras 163 | 164 | return observation, reward, done, info 165 | 166 | def reset(self, state=None): 167 | """ Resets the task and updates observations. """ 168 | self._task.reset() 169 | 170 | # allow users to specify the control frequency through config 171 | for _ in range(self._task.control_frequency_inv): 172 | self._world.step(render=self._run_sim_rendering) 173 | observation = self._task.get_observations()[0].cpu().numpy() 174 | 175 | return observation 176 | 177 | def stop(self): 178 | pass 179 | 180 | def shutdown(self): 181 | pass 182 | 183 | @property 184 | def num_envs(self): 185 | """ Retrieves number of environments. 186 | 187 | Returns: 188 | num_envs(int): Number of environments. 189 | """ 190 | return self._num_envs 191 | -------------------------------------------------------------------------------- /actpermoma/handlers/base/tiagohandler.py: -------------------------------------------------------------------------------- 1 | from abc import ABC, abstractmethod 2 | 3 | class TiagoBaseHandler(ABC): 4 | def __init__(self, ): 5 | pass 6 | 7 | @abstractmethod 8 | def get_robot(self): 9 | pass 10 | 11 | @abstractmethod 12 | def post_reset(self): 13 | pass 14 | 15 | @abstractmethod 16 | def apply_actions(self): 17 | pass 18 | 19 | # @abstractmethod 20 | # def get_obs_dict(self): 21 | # pass 22 | 23 | @abstractmethod 24 | def reset(self): 25 | pass 26 | 27 | -------------------------------------------------------------------------------- /actpermoma/robots/articulations/tiago_dual_holo.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2018-2022, NVIDIA Corporation 2 | # All rights reserved. 3 | # 4 | # Redistribution and use in source and binary forms, with or without 5 | # modification, are permitted provided that the following conditions are met: 6 | # 7 | # 1. Redistributions of source code must retain the above copyright notice, this 8 | # list of conditions and the following disclaimer. 9 | # 10 | # 2. Redistributions in binary form must reproduce the above copyright notice, 11 | # this list of conditions and the following disclaimer in the documentation 12 | # and/or other materials provided with the distribution. 13 | # 14 | # 3. Neither the name of the copyright holder nor the names of its 15 | # contributors may be used to endorse or promote products derived from 16 | # this software without specific prior written permission. 17 | # 18 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | 30 | from typing import Optional 31 | import numpy as np 32 | import torch 33 | from omni.isaac.core.robots.robot import Robot 34 | from omni.isaac.core.utils.nucleus import get_assets_root_path 35 | from omni.isaac.core.utils.stage import add_reference_to_stage 36 | 37 | import carb 38 | 39 | from actpermoma.utils.files import get_usd_path 40 | 41 | 42 | class TiagoDualHolo(Robot): 43 | def __init__( 44 | self, 45 | prim_path: str, 46 | name: Optional[str] = "TiagoDualHolo", 47 | usd_path: Optional[str] = None, 48 | translation: Optional[np.ndarray] = None, 49 | orientation: Optional[np.ndarray] = None, 50 | ) -> None: 51 | 52 | self._usd_path = usd_path 53 | self._name = name 54 | 55 | if self._usd_path is None: 56 | # assets_root_path = get_assets_root_path() 57 | # if assets_root_path is None: 58 | # carb.log_error("Could not find Isaac Sim assets folder") 59 | self._usd_path = (get_usd_path() / 'tiago_dual_holobase_zed' / 'tiago_dual_holobase_zed.usd').as_posix() 60 | 61 | add_reference_to_stage(self._usd_path, prim_path) 62 | 63 | super().__init__( 64 | prim_path=prim_path, 65 | name=name, 66 | translation=translation, 67 | orientation=orientation, 68 | articulation_controller=None, 69 | ) 70 | 71 | 72 | class TiagoDualOmniHolo(Robot): 73 | def __init__( 74 | self, 75 | prim_path: str, 76 | name: Optional[str] = "TiagoDualHolo", 77 | usd_path: Optional[str] = None, 78 | translation: Optional[np.ndarray] = None, 79 | orientation: Optional[np.ndarray] = None, 80 | ) -> None: 81 | 82 | self._usd_path = usd_path 83 | self._name = name 84 | 85 | if self._usd_path is None: 86 | # assets_root_path = get_assets_root_path() 87 | # if assets_root_path is None: 88 | # carb.log_error("Could not find Isaac Sim assets folder") 89 | self._usd_path = (get_usd_path() / 'tiago_dual_omni_holobase' / 'tiago_dual_omni_holobase.usd').as_posix() 90 | 91 | add_reference_to_stage(self._usd_path, prim_path) 92 | 93 | super().__init__( 94 | prim_path=prim_path, 95 | name=name, 96 | translation=translation, 97 | orientation=orientation, 98 | articulation_controller=None, 99 | ) -------------------------------------------------------------------------------- /actpermoma/scripts/active_grasp_pipeline.py: -------------------------------------------------------------------------------- 1 | import os 2 | from datetime import datetime 3 | import argparse 4 | import itertools 5 | from copy import deepcopy as cp 6 | 7 | import numpy as np 8 | import hydra 9 | from omegaconf import DictConfig, OmegaConf, open_dict 10 | 11 | import actpermoma 12 | from actpermoma.utils.hydra_cfg.hydra_utils import * 13 | from actpermoma.utils.hydra_cfg.reformat import omegaconf_to_dict, print_dict 14 | 15 | from actpermoma.utils.task_util import initialize_task 16 | from actpermoma.envs.isaac_env_mushroom import IsaacEnvMushroom 17 | from actpermoma.algos.active_grasp import ActiveGraspAgent 18 | from actpermoma.algos.naive import NaiveAgent 19 | from actpermoma.algos.random import RandomAgent 20 | from actpermoma.algos.actpermoma import ActPerMoMaAgent 21 | # Use Mushroom RL library 22 | import torch 23 | import torch.nn as nn 24 | import torch.optim as optim 25 | import torch.nn.functional as F 26 | from mushroom_rl.core import Core, Logger 27 | from mushroom_rl.algorithms.actor_critic import * 28 | from mushroom_rl.utils.dataset import compute_J, parse_dataset 29 | from tqdm import trange 30 | 31 | def parse_data_and_save(eval_dataset, results_dir, name_spc=None, wandb_logger=None): 32 | # eval_dataset is a list of tuples respresenting (s, a, r, s', absorbing, info, last); where s is a dict with keys 'tsdf', 'goal_pos', 'view_pos', 'view_theta', 'view_phi', 'obj_states', 'reset_agent' 33 | # Save dataset 34 | distances_list = [] 35 | num_views_list = [] 36 | successes_list = [] 37 | failures_list = [] 38 | aborted_list = [] 39 | num_IK_fails_list = [] 40 | num_grasps_attempted_list = [] 41 | theta_covered_list = [] 42 | 43 | # np.save(os.path.join(results_dir, 'eval_dataset.npy'), eval_dataset) 44 | # Save samples 45 | for i, (s, a, r, s_, absorbing, info, last) in enumerate(eval_dataset): 46 | if 'tsdf' in s: del s['tsdf'] 47 | if 'tsdf' in s_: del s_['tsdf'] 48 | if last: 49 | distances_list.append(info['distance'][0]) 50 | num_views_list.append(info['num_views'][0]) 51 | successes_list.append(info['grasp_success'][0]) 52 | failures_list.append(info['grasp_failure'][0]) 53 | aborted_list.append(info['aborted'][0]) 54 | num_IK_fails_list.append(info['num_IK_fails'][0]) 55 | num_grasps_attempted_list.append(info['num_grasps_attempted'][0]) 56 | theta_covered_list.append(info['theta_covered'][0]) 57 | 58 | d_mean = np.mean(distances_list) 59 | d_std = np.std(distances_list) 60 | nv_mean = np.mean(num_views_list) 61 | nv_std = np.std(num_views_list) 62 | nIK_mean = np.mean(num_IK_fails_list) 63 | nIK_std = np.std(num_IK_fails_list) 64 | ngrasps_mean = np.mean(num_grasps_attempted_list) 65 | ngrasps_std = np.std(num_grasps_attempted_list) 66 | th_mean = np.mean(theta_covered_list) 67 | th_std = np.std(theta_covered_list) 68 | sr = np.sum(successes_list) / len(successes_list) 69 | fr = np.sum(failures_list) / len(failures_list) 70 | ar = np.sum(aborted_list) / len(aborted_list) 71 | 72 | metrics = {'distance_mean': d_mean, 73 | 'distance_std': d_std, 74 | 'num_views_mean': nv_mean, 75 | 'num_views_std': nv_std, 76 | 'num_IK_fails_mean': nIK_mean, 77 | 'num_IK_fails_std': nIK_std, 78 | 'num_grasps_att_mean': ngrasps_mean, 79 | 'num_grasps_att_std': ngrasps_std, 80 | 'theta_covered_mean': th_mean, 81 | 'theta_covered_std': th_std, 82 | 'success_rate': sr, 83 | 'failure_rate': fr, 84 | 'aborted_rate': ar} 85 | 86 | print(metrics) 87 | 88 | if name_spc is None: 89 | name_spc = datetime.now().strftime('%Y-%m-%d-%H-%M-%S') 90 | if wandb_logger is not None: 91 | wandb_logger.log_metrics(metrics, name_spc) 92 | wandb_logger.finish() 93 | np.save(os.path.join(results_dir, f'eval_dataset-{name_spc}.npy'), eval_dataset, allow_pickle=True) 94 | np.save(os.path.join(results_dir, f'metrics-{name_spc}.npy'), metrics, allow_pickle=True) 95 | 96 | return d_mean, nv_mean, sr, fr, ar 97 | 98 | def experiment(cfg: DictConfig = None, cfg_file_path: str = "", seed: int = 0, results_dir: str = "", env=None, task=None): 99 | # Get configs 100 | if (cfg_file_path): 101 | # Get config file from path 102 | cfg = OmegaConf.load(cfg_file_path) 103 | 104 | cfg_dict = omegaconf_to_dict(cfg) 105 | print_dict(cfg_dict) 106 | headless = cfg.headless 107 | render = cfg.render 108 | sim_app_cfg_path = cfg.sim_app_cfg_path 109 | agent_params_cfg = cfg.train.params.config 110 | algo_map = {"ActiveGrasp": ActiveGraspAgent, 111 | "ActPerMoMa": ActPerMoMaAgent, 112 | "Naive": NaiveAgent, 113 | "Random": RandomAgent} 114 | algo = algo_map[cfg.train.params.algo.name] 115 | algo_name = cfg.train.params.algo.name 116 | 117 | # Set up environment 118 | if env is None or task is None: 119 | env = IsaacEnvMushroom(headless=headless, render=render, sim_app_cfg_path=sim_app_cfg_path) 120 | task = initialize_task(cfg_dict, env) 121 | render=False # TODO 122 | 123 | # Set up logging paths/directories 124 | exp_name = cfg.train.params.config.name 125 | exp_stamp = datetime.now().strftime('%Y-%m-%d-%H-%M-%S') # append datetime for logging 126 | results_dir = os.path.join(actpermoma.__path__[0], 'logs', cfg.task.name, exp_name) 127 | if (cfg.test): results_dir = os.path.join(results_dir, 'test') 128 | results_dir = os.path.join(results_dir, exp_stamp) 129 | os.makedirs(results_dir, exist_ok=True) 130 | # log experiment config 131 | with open(os.path.join(results_dir, 'config.yaml'), 'w') as f: 132 | f.write(OmegaConf.to_yaml(cfg)) 133 | 134 | obs_to_states = [task.obs_to_state] 135 | 136 | 137 | # Loop over num_seq_seeds: 138 | for exp in range(cfg.num_seeds): 139 | np.random.seed(seed) 140 | torch.manual_seed(seed) 141 | seed += 1 142 | 143 | # Logger 144 | logger = Logger(results_dir=results_dir, log_console=True) 145 | logger.strong_line() 146 | # if cfg.name_spec exists 147 | if hasattr(cfg, 'name_spec'): 148 | name_spec = cfg.name_spec 149 | else: 150 | name_spec = datetime.now().strftime('%Y-%m-%d-%H-%M-%S') 151 | logger.info(f'Experiment: {exp_name}, Algo{algo_name}: ' + name_spec) 152 | 153 | exp_eval_dataset = list() # This will be a list of dicts with datasets from every epoch 154 | if cfg.wandb_activate is True: 155 | # (Optional) Logging with Weights & biases 156 | from actpermoma.utils.wandb_utils import wandbLogger 157 | wandb_logger = wandbLogger(exp_config=cfg, run_name=logger._log_id, group_name=algo_name+'_'+exp_stamp) # Optional 158 | if hasattr(cfg, 'short_name_spec'): 159 | wandb_logger = wandbLogger(exp_config=cfg, group_name=algo_name+'_'+cfg.short_name_spec+'_'+exp_stamp) 160 | else: 161 | wandb_logger = wandbLogger(exp_config=cfg, group_name=algo_name+'_'+exp_stamp) # Optional 162 | else: 163 | wandb_logger = None 164 | 165 | # Agent 166 | if not cfg_dict["task"] is None and cfg_dict["task"]["name"] == 'TiagoDualActivePerception' or cfg_dict["task"]["name"] == 'TiagoDualActivePerceptionNaive': 167 | # src: https://stackoverflow.com/questions/66295334/create-a-new-key-in-hydra-dictconfig-from-python-file 168 | OmegaConf.set_struct(agent_params_cfg, True) 169 | with open_dict(agent_params_cfg): 170 | agent_params_cfg['fx'] = cfg_dict["task"]["env"]["fx"] 171 | agent_params_cfg['fy'] = cfg_dict["task"]["env"]["fy"] 172 | agent_params_cfg['cx'] = cfg_dict["task"]["env"]["cx"] 173 | agent_params_cfg['cy'] = cfg_dict["task"]["env"]["cy"] 174 | else: 175 | raise Exception("No task config provided, but camera parameters required") 176 | 177 | agent = algo(env.info, agent_params_cfg) 178 | agent.tiago_handler = task.tiago_handler # didn't figure out how to pass this to the agent in a cleaner way 179 | 180 | # Algorithm 181 | core = Core(agent, env, preprocessors=obs_to_states) 182 | 183 | # RUN 184 | eval_dataset = core.evaluate(n_episodes=agent_params_cfg.n_episodes_test, render=render) 185 | # eval_dataset is a list of tuples respresenting (s, a, r, s', absorbing, info, last); where s is a dict with keys 'tsdf', 'goal_pos', 'view_pos', 'view_theta', 'view_phi', 'obj_states', 'reset_agent' 186 | d_mean, nv_mean, sr, fr, ar = parse_data_and_save(eval_dataset, results_dir, name_spc=f'Algo{algo_name}_' + name_spec, wandb_logger=wandb_logger) 187 | 188 | logger.epoch_info(0, success_rate=sr, avg_episode_length=nv_mean) 189 | # Optional wandb logging 190 | # wandb_logger.run_log_wandb(success_rate, J, R, E, avg_episode_length, q_loss) 191 | 192 | # Get video snippet of final learnt behavior (enable internal rendering for this) 193 | # prev_env_render_setting = bool(env._run_sim_rendering) 194 | # env._run_sim_rendering = True 195 | # img_dataset = core.evaluate(n_episodes=5, get_renders=True) 196 | # env._run_sim_rendering = prev_env_render_setting 197 | # log dataset and video 198 | # logger.log_dataset(exp_eval_dataset) 199 | # run_log_wandb(exp_config=cfg, run_name=logger._log_id, group_name=exp_name+'_'+exp_stamp, dataset=exp_eval_dataset) 200 | # img_dataset = img_dataset[::15] # Reduce size of img_dataset. Take every 15th image 201 | # wandb_logger.vid_log_wandb(img_dataset=img_dataset) 202 | 203 | # Shutdown 204 | env._simulation_app.close() 205 | 206 | 207 | @hydra.main(config_name="config", config_path="../cfg") 208 | def parse_hydra_configs_and_run_exp(cfg: DictConfig): 209 | experiment(cfg) 210 | 211 | 212 | if __name__ == '__main__': 213 | #parse_hydra_configs_and_run_exp() 214 | #parser = argparse.ArgumentParser() 215 | #parser.add_argument('results_dir') 216 | #parser.add_argument('seed') 217 | #parser.add_argument('cfg_file_path') 218 | #args = parser.parse_args() 219 | #experiment(cfg_file_path = args.cfg_file_path, seed=args.seed, results_dir=args.results_dir) 220 | parser = argparse.ArgumentParser("Train/Test agent: (Local or SLURM)") 221 | parser.add_argument("--cfg_file_path", type=str, default="", help="Optional config file to run experiment (typically when using slurm)") 222 | args, _ = parser.parse_known_args() 223 | 224 | parse_hydra_configs_and_run_exp() -------------------------------------------------------------------------------- /actpermoma/scripts/active_grasp_pipeline_loop.py: -------------------------------------------------------------------------------- 1 | import os 2 | from datetime import datetime 3 | import argparse 4 | import itertools 5 | 6 | import numpy as np 7 | import hydra 8 | from omegaconf import DictConfig, OmegaConf, open_dict 9 | 10 | import actpermoma 11 | from actpermoma.utils.hydra_cfg.hydra_utils import * 12 | from actpermoma.utils.hydra_cfg.reformat import omegaconf_to_dict, print_dict 13 | 14 | from actpermoma.utils.task_util import initialize_task 15 | from actpermoma.envs.isaac_env_mushroom import IsaacEnvMushroom 16 | from actpermoma.algos.active_grasp import ActiveGraspAgent 17 | from actpermoma.algos.naive import NaiveAgent 18 | from actpermoma.algos.random import RandomAgent 19 | from actpermoma.algos.actpermoma import ActPerMoMaAgent 20 | # Use Mushroom RL library 21 | import torch 22 | import torch.nn as nn 23 | import torch.optim as optim 24 | import torch.nn.functional as F 25 | from mushroom_rl.core import Core, Logger 26 | from mushroom_rl.algorithms.actor_critic import * 27 | from mushroom_rl.utils.dataset import compute_J, parse_dataset 28 | from tqdm import trange 29 | 30 | def parse_data_and_save(eval_dataset, results_dir, name_spc=None): 31 | # eval_dataset is a list of tuples respresenting (s, a, r, s', absorbing, info, last); where s is a dict with keys 'tsdf', 'goal_pos', 'view_pos', 'view_theta', 'view_phi', 'obj_states', 'reset_agent' 32 | # Save dataset 33 | distances_list = [] 34 | num_views_list = [] 35 | successes_list = [] 36 | failures_list = [] 37 | aborted_list = [] 38 | num_IK_fails_list = [] 39 | num_grasps_attempted_list = [] 40 | theta_covered_list = [] 41 | 42 | # np.save(os.path.join(results_dir, 'eval_dataset.npy'), eval_dataset) 43 | # Save samples 44 | for i, (s, a, r, s_, absorbing, info, last) in enumerate(eval_dataset): 45 | if 'tsdf' in s: del s['tsdf'] 46 | if 'tsdf' in s_: del s_['tsdf'] 47 | if last: 48 | distances_list.append(info['distance'][0]) 49 | num_views_list.append(info['num_views'][0]) 50 | successes_list.append(info['grasp_success'][0]) 51 | failures_list.append(info['grasp_failure'][0]) 52 | aborted_list.append(info['aborted'][0]) 53 | num_IK_fails_list.append(info['num_IK_fails'][0]) 54 | num_grasps_attempted_list.append(info['num_grasps_attempted'][0]) 55 | theta_covered_list.append(info['theta_covered'][0]) 56 | 57 | d_mean = np.mean(distances_list) 58 | d_std = np.std(distances_list) 59 | nv_mean = np.mean(num_views_list) 60 | nv_std = np.std(num_views_list) 61 | nIK_mean = np.mean(num_IK_fails_list) 62 | nIK_std = np.std(num_IK_fails_list) 63 | ngrasps_mean = np.mean(num_grasps_attempted_list) 64 | ngrasps_std = np.std(num_grasps_attempted_list) 65 | th_mean = np.mean(theta_covered_list) 66 | th_std = np.std(theta_covered_list) 67 | sr = np.sum(successes_list) / len(successes_list) 68 | fr = np.sum(failures_list) / len(failures_list) 69 | ar = np.sum(aborted_list) / len(aborted_list) 70 | 71 | metrics = {'distance_mean': d_mean, 72 | 'distance_std': d_std, 73 | 'num_views_mean': nv_mean, 74 | 'num_views_std': nv_std, 75 | 'num_IK_fails_mean': nIK_mean, 76 | 'num_IK_fails_std': nIK_std, 77 | 'num_grasps_att_mean': ngrasps_mean, 78 | 'num_grasps_att_std': ngrasps_std, 79 | 'theta_covered_mean': th_mean, 80 | 'theta_covered_std': th_std, 81 | 'success_rate': sr, 82 | 'failure_rate': fr, 83 | 'aborted_rate': ar} 84 | 85 | print(metrics) 86 | 87 | if name_spc is None: 88 | name_spc = datetime.now().strftime('%Y-%m-%d-%H-%M-%S') 89 | np.save(os.path.join(results_dir, f'eval_dataset-{name_spc}.npy'), eval_dataset, allow_pickle=True) 90 | np.save(os.path.join(results_dir, f'metrics-{name_spc}.npy'), metrics, allow_pickle=True) 91 | 92 | return d_mean, nv_mean, sr, fr, ar 93 | 94 | def experiment(cfg: DictConfig = None, cfg_file_path: str = "", seed: int = 0, results_dir: str = ""): 95 | # Get configs 96 | if (cfg_file_path): 97 | # Get config file from path 98 | cfg = OmegaConf.load(cfg_file_path) 99 | 100 | cfg_dict = omegaconf_to_dict(cfg) 101 | print_dict(cfg_dict) 102 | headless = cfg.headless 103 | render = cfg.render 104 | sim_app_cfg_path = cfg.sim_app_cfg_path 105 | agent_params_cfg = cfg.train.params.config 106 | algo_map = {"ActiveGrasp": ActiveGraspAgent, 107 | "ActPerMoMa": ActPerMoMaAgent, 108 | "Naive": NaiveAgent, 109 | "Random": RandomAgent} 110 | algo = algo_map[cfg.train.params.algo.name] 111 | algo_name = cfg.train.params.algo.name 112 | 113 | # Set up environment 114 | env = IsaacEnvMushroom(headless=headless, render=render, sim_app_cfg_path=sim_app_cfg_path) 115 | render=False # TODO 116 | task = initialize_task(cfg_dict, env) 117 | 118 | # Set up logging paths/directories 119 | exp_name = cfg.train.params.config.name 120 | exp_stamp = datetime.now().strftime('%Y-%m-%d-%H-%M-%S') # append datetime for logging 121 | results_dir = os.path.join(actpermoma.__path__[0], 'logs', cfg.task.name, exp_name) 122 | if (cfg.test): results_dir = os.path.join(results_dir, 'test') 123 | results_dir = os.path.join(results_dir, exp_stamp) 124 | os.makedirs(results_dir, exist_ok=True) 125 | # log experiment config 126 | with open(os.path.join(results_dir, 'config.yaml'), 'w') as f: 127 | f.write(OmegaConf.to_yaml(cfg)) 128 | 129 | obs_to_states = [task.obs_to_state] 130 | # window_sizes = [3, 6, 9, 12] 131 | # norm_functions = ['/dist**2', '/dist', '*exp(-dist)', 'asdf'] 132 | norm_functions = ['/dist**2'] 133 | use_reachs = [True] 134 | 135 | # Loop over num_seq_seeds: 136 | # for exp in range(len(window_sizes)): 137 | for norm_function, use_reach in itertools.product(norm_functions, use_reachs): 138 | np.random.seed(seed) 139 | torch.manual_seed(seed) 140 | # seed += 1 # Same seed for different settings 141 | 142 | # window_size = window_sizes[exp] 143 | 144 | # Logger 145 | logger = Logger(results_dir=results_dir, log_console=True) 146 | logger.strong_line() 147 | logger.info(f'Experiment: {exp_name}, Algo{algo_name}-norm_function{norm_function}-use_reach{use_reach}') 148 | 149 | # Agent 150 | if not cfg_dict["task"] is None and cfg_dict["task"]["name"] == 'TiagoDualActivePerception' or cfg_dict["task"]["name"] == 'TiagoDualActivePerceptionNaive': 151 | # src: https://stackoverflow.com/questions/66295334/create-a-new-key-in-hydra-dictconfig-from-python-file 152 | OmegaConf.set_struct(agent_params_cfg, True) 153 | with open_dict(agent_params_cfg): 154 | agent_params_cfg['fx'] = cfg_dict["task"]["env"]["fx"] 155 | agent_params_cfg['fy'] = cfg_dict["task"]["env"]["fy"] 156 | agent_params_cfg['cx'] = cfg_dict["task"]["env"]["cx"] 157 | agent_params_cfg['cy'] = cfg_dict["task"]["env"]["cy"] 158 | # agent_params_cfg['window_size'] = window_size 159 | agent_params_cfg['use_reachability'] = use_reach 160 | agent_params_cfg['gain_weighting'] = norm_function 161 | else: 162 | raise Exception("No task config provided, but camera parameters required") 163 | 164 | agent = algo(env.info, agent_params_cfg) 165 | agent.tiago_handler = task.tiago_handler # didn't figure out how to pass this to the agent in a cleaner way 166 | 167 | # Algorithm 168 | core = Core(agent, env, preprocessors=obs_to_states) 169 | 170 | # RUN 171 | eval_dataset = core.evaluate(n_episodes=agent_params_cfg.n_episodes_test, render=render) 172 | # eval_dataset is a list of tuples respresenting (s, a, r, s', absorbing, info, last); where s is a dict with keys 'tsdf', 'goal_pos', 'view_pos', 'view_theta', 'view_phi', 'obj_states', 'reset_agent' 173 | d_mean, nv_mean, sr, fr, ar = parse_data_and_save(eval_dataset, results_dir, name_spc=f'Algo{algo_name}-norm_function{norm_function[1:]}-use_reach{use_reach}') 174 | 175 | logger.epoch_info(0, success_rate=sr, avg_episode_length=nv_mean) 176 | 177 | # Shutdown 178 | env._simulation_app.close() 179 | 180 | 181 | @hydra.main(config_name="config", config_path="../cfg") 182 | def parse_hydra_configs_and_run_exp(cfg: DictConfig): 183 | experiment(cfg) 184 | 185 | 186 | if __name__ == '__main__': 187 | parse_hydra_configs_and_run_exp() -------------------------------------------------------------------------------- /actpermoma/scripts/active_grasp_pipeline_loop_section.py: -------------------------------------------------------------------------------- 1 | import os 2 | from datetime import datetime 3 | import argparse 4 | import itertools 5 | from copy import deepcopy as cp 6 | 7 | import numpy as np 8 | import hydra 9 | from omegaconf import DictConfig, OmegaConf, open_dict 10 | 11 | import actpermoma 12 | from actpermoma.utils.hydra_cfg.hydra_utils import * 13 | from actpermoma.utils.hydra_cfg.reformat import omegaconf_to_dict, print_dict 14 | 15 | from actpermoma.utils.task_util import initialize_task 16 | from actpermoma.envs.isaac_env_mushroom import IsaacEnvMushroom 17 | from actpermoma.algos.active_grasp import ActiveGraspAgent 18 | from actpermoma.algos.naive import NaiveAgent 19 | from actpermoma.algos.random import RandomAgent 20 | from actpermoma.algos.actpermoma import ActPerMoMaAgent 21 | 22 | # Use Mushroom RL library 23 | import torch 24 | import torch.nn as nn 25 | import torch.optim as optim 26 | import torch.nn.functional as F 27 | from mushroom_rl.core import Core, Logger 28 | from mushroom_rl.algorithms.actor_critic import * 29 | from mushroom_rl.utils.dataset import compute_J, parse_dataset 30 | from tqdm import trange 31 | 32 | def parse_data_and_save(eval_dataset, results_dir, name_spc=None): 33 | # eval_dataset is a list of tuples respresenting (s, a, r, s', absorbing, info, last); where s is a dict with keys 'tsdf', 'goal_pos', 'view_pos', 'view_theta', 'view_phi', 'obj_states', 'reset_agent' 34 | # Save dataset 35 | distances_list = [] 36 | num_views_list = [] 37 | successes_list = [] 38 | failures_list = [] 39 | aborted_list = [] 40 | num_IK_fails_list = [] 41 | num_grasps_attempted_list = [] 42 | theta_covered_list = [] 43 | 44 | # np.save(os.path.join(results_dir, 'eval_dataset.npy'), eval_dataset) 45 | # Save samples 46 | for i, (s, a, r, s_, absorbing, info, last) in enumerate(eval_dataset): 47 | if 'tsdf' in s: del s['tsdf'] 48 | if 'tsdf' in s_: del s_['tsdf'] 49 | if last: 50 | distances_list.append(info['distance'][0]) 51 | num_views_list.append(info['num_views'][0]) 52 | successes_list.append(info['grasp_success'][0]) 53 | failures_list.append(info['grasp_failure'][0]) 54 | aborted_list.append(info['aborted'][0]) 55 | num_IK_fails_list.append(info['num_IK_fails'][0]) 56 | num_grasps_attempted_list.append(info['num_grasps_attempted'][0]) 57 | theta_covered_list.append(info['theta_covered'][0]) 58 | 59 | d_mean = np.mean(distances_list) 60 | d_std = np.std(distances_list) 61 | nv_mean = np.mean(num_views_list) 62 | nv_std = np.std(num_views_list) 63 | nIK_mean = np.mean(num_IK_fails_list) 64 | nIK_std = np.std(num_IK_fails_list) 65 | ngrasps_mean = np.mean(num_grasps_attempted_list) 66 | ngrasps_std = np.std(num_grasps_attempted_list) 67 | th_mean = np.mean(theta_covered_list) 68 | th_std = np.std(theta_covered_list) 69 | sr = np.sum(successes_list) / len(successes_list) 70 | fr = np.sum(failures_list) / len(failures_list) 71 | ar = np.sum(aborted_list) / len(aborted_list) 72 | 73 | metrics = {'distance_mean': d_mean, 74 | 'distance_std': d_std, 75 | 'num_views_mean': nv_mean, 76 | 'num_views_std': nv_std, 77 | 'num_IK_fails_mean': nIK_mean, 78 | 'num_IK_fails_std': nIK_std, 79 | 'num_grasps_att_mean': ngrasps_mean, 80 | 'num_grasps_att_std': ngrasps_std, 81 | 'theta_covered_mean': th_mean, 82 | 'theta_covered_std': th_std, 83 | 'success_rate': sr, 84 | 'failure_rate': fr, 85 | 'aborted_rate': ar} 86 | 87 | print(metrics) 88 | 89 | if name_spc is None: 90 | name_spc = datetime.now().strftime('%Y-%m-%d-%H-%M-%S') 91 | np.save(os.path.join(results_dir, f'eval_dataset-{name_spc}.npy'), eval_dataset, allow_pickle=True) 92 | np.save(os.path.join(results_dir, f'metrics-{name_spc}.npy'), metrics, allow_pickle=True) 93 | 94 | return d_mean, nv_mean, sr, fr, ar 95 | 96 | def experiment(cfg: DictConfig = None, cfg_file_path: str = "", seed: int = 0, results_dir: str = ""): 97 | # Get configs 98 | if (cfg_file_path): 99 | # Get config file from path 100 | cfg = OmegaConf.load(cfg_file_path) 101 | 102 | cfg_dict = omegaconf_to_dict(cfg) 103 | print_dict(cfg_dict) 104 | headless = cfg.headless 105 | render = cfg.render 106 | sim_app_cfg_path = cfg.sim_app_cfg_path 107 | agent_params_cfg = cfg.train.params.config 108 | algo_map = {"ActiveGrasp": ActiveGraspAgent, 109 | "ActPerMoMa": ActPerMoMaAgent, 110 | "Naive": NaiveAgent, 111 | "Random": RandomAgent} 112 | algo = algo_map[cfg.train.params.algo.name] 113 | algo_name = cfg.train.params.algo.name 114 | 115 | # Set up environment 116 | env = IsaacEnvMushroom(headless=headless, render=render, sim_app_cfg_path=sim_app_cfg_path) 117 | render=False # TODO 118 | task = initialize_task(cfg_dict, env) 119 | 120 | # Set up logging paths/directories 121 | exp_name = cfg.train.params.config.name 122 | exp_stamp = datetime.now().strftime('%Y-%m-%d-%H-%M-%S') # append datetime for logging 123 | results_dir = os.path.join(actpermoma.__path__[0], 'logs', cfg.task.name, exp_name) 124 | if (cfg.test): results_dir = os.path.join(results_dir, 'test') 125 | results_dir = os.path.join(results_dir, exp_stamp) 126 | os.makedirs(results_dir, exist_ok=True) 127 | # log experiment config 128 | with open(os.path.join(results_dir, 'config.yaml'), 'w') as f: 129 | f.write(OmegaConf.to_yaml(cfg)) 130 | 131 | obs_to_states = [task.obs_to_state] 132 | 133 | ## parameters 134 | parameters = {'window_sizes': [1, 3], 135 | 'gain_weighting_factors': [0.1, 0.5, 0.8], 136 | 'seen_theta_threshs': [0, 30*np.pi/180, 45*np.pi/180], 137 | 'quality_thresholds': [0.8, 0.85, 0.9], 138 | 'base_goal_radius_grasps': [0.75, 0.8], 139 | 'momentums': [400, 500, 600], 140 | 'deadlock_thetas': [0.0, 3.0 *np.pi/180, 5.0 * np.pi / 180]} 141 | 142 | defaults = {'window_size': 1, 143 | 'gain_weighting_factor': 0.5, 144 | 'seen_theta_thresh': 45*np.pi/180, 145 | 'quality_threshold': 0.9, 146 | 'base_goal_radius_grasp': 0.75, 147 | 'momentum': 500, 148 | 'deadlock_theta': 3.0 * np.pi / 180} 149 | 150 | param_combinations = [] 151 | for key in parameters.keys(): 152 | for key_val in parameters[key]: 153 | param_combi = cp(defaults) 154 | param_combi[key[:-1]] = key_val 155 | 156 | param_combinations.append(param_combi) 157 | 158 | # Loop over num_seq_seeds: 159 | for param_combi in param_combinations: 160 | np.random.seed(seed) 161 | torch.manual_seed(seed) 162 | # seed += 1 # Same seed for different settings 163 | 164 | # Logger 165 | logger = Logger(results_dir=results_dir, log_console=True) 166 | logger.strong_line() 167 | name_spec = ', '.join([key + '=' + str(param_combi[key]) for key in param_combi.keys()]) 168 | logger.info(f'Experiment: {exp_name}, Algo{algo_name}: ' + name_spec) 169 | 170 | # Agent 171 | if not cfg_dict["task"] is None and cfg_dict["task"]["name"] == 'TiagoDualActivePerception' or cfg_dict["task"]["name"] == 'TiagoDualActivePerceptionNaive': 172 | # src: https://stackoverflow.com/questions/66295334/create-a-new-key-in-hydra-dictconfig-from-python-file 173 | OmegaConf.set_struct(agent_params_cfg, True) 174 | with open_dict(agent_params_cfg): 175 | agent_params_cfg['fx'] = cfg_dict["task"]["env"]["fx"] 176 | agent_params_cfg['fy'] = cfg_dict["task"]["env"]["fy"] 177 | agent_params_cfg['cx'] = cfg_dict["task"]["env"]["cx"] 178 | agent_params_cfg['cy'] = cfg_dict["task"]["env"]["cy"] 179 | for key in param_combi.keys(): 180 | agent_params_cfg[key] = param_combi[key] 181 | else: 182 | raise Exception("No task config provided, but camera parameters required") 183 | 184 | agent = algo(env.info, agent_params_cfg) 185 | agent.tiago_handler = task.tiago_handler # didn't figure out how to pass this to the agent in a cleaner way 186 | 187 | # Algorithm 188 | core = Core(agent, env, preprocessors=obs_to_states) 189 | 190 | # RUN 191 | eval_dataset = core.evaluate(n_episodes=agent_params_cfg.n_episodes_test, render=render) 192 | # eval_dataset is a list of tuples respresenting (s, a, r, s', absorbing, info, last); where s is a dict with keys 'tsdf', 'goal_pos', 'view_pos', 'view_theta', 'view_phi', 'obj_states', 'reset_agent' 193 | name_spec = '_'.join([key + '=' + str(param_combi[key]) for key in param_combi.keys()]) 194 | d_mean, nv_mean, sr, fr, ar = parse_data_and_save(eval_dataset, results_dir, name_spc=f'Algo{algo_name}_' + name_spec) 195 | 196 | logger.epoch_info(0, success_rate=sr, avg_episode_length=nv_mean) 197 | 198 | # Shutdown 199 | env._simulation_app.close() 200 | 201 | 202 | @hydra.main(config_name="config", config_path="../cfg") 203 | def parse_hydra_configs_and_run_exp(cfg: DictConfig): 204 | experiment(cfg) 205 | 206 | 207 | if __name__ == '__main__': 208 | parse_hydra_configs_and_run_exp() -------------------------------------------------------------------------------- /actpermoma/tasks/base/rl_task.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2018-2022, NVIDIA Corporation 2 | # All rights reserved. 3 | # 4 | # Redistribution and use in source and binary forms, with or without 5 | # modification, are permitted provided that the following conditions are met: 6 | # 7 | # 1. Redistributions of source code must retain the above copyright notice, this 8 | # list of conditions and the following disclaimer. 9 | # 10 | # 2. Redistributions in binary form must reproduce the above copyright notice, 11 | # this list of conditions and the following disclaimer in the documentation 12 | # and/or other materials provided with the distribution. 13 | # 14 | # 3. Neither the name of the copyright holder nor the names of its 15 | # contributors may be used to endorse or promote products derived from 16 | # this software without specific prior written permission. 17 | # 18 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | 30 | from abc import abstractmethod 31 | import numpy as np 32 | import torch 33 | # from gym import spaces 34 | from mushroom_rl.utils.spaces import * 35 | from omni.isaac.core.tasks import BaseTask 36 | from omni.isaac.core.utils.types import ArticulationAction 37 | from omni.isaac.core.utils.prims import define_prim 38 | from omni.isaac.cloner import GridCloner 39 | from actpermoma.tasks.utils.usd_utils import create_distant_light 40 | from actpermoma.utils.domain_randomization.randomize import Randomizer 41 | import omni.kit 42 | from omni.kit.viewport.utility.camera_state import ViewportCameraState 43 | from omni.kit.viewport.utility import get_viewport_from_window_name 44 | from pxr import Gf 45 | 46 | class RLTask(BaseTask): 47 | 48 | """ This class provides a PyTorch RL-specific interface for setting up RL tasks. 49 | It includes utilities for setting up RL task related parameters, 50 | cloning environments, and data collection for RL algorithms. 51 | """ 52 | 53 | def __init__(self, name, env, offset=None) -> None: 54 | 55 | """ Initializes RL parameters, cloner object, and buffers. 56 | 57 | Args: 58 | name (str): name of the task. 59 | env (VecEnvBase): an instance of the environment wrapper class to register task. 60 | offset (Optional[np.ndarray], optional): offset applied to all assets of the task. Defaults to None. 61 | """ 62 | 63 | super().__init__(name=name, offset=offset) 64 | 65 | self.test = self._cfg["test"] 66 | self._device = self._cfg["sim_device"] 67 | self._dr_randomizer = Randomizer(self._sim_config) 68 | print("Task Device:", self._device) 69 | 70 | self.randomize_actions = False 71 | self.randomize_observations = False 72 | 73 | self.clip_obs = self._cfg["task"]["env"].get("clipObservations", np.Inf) 74 | self.clip_actions = self._cfg["task"]["env"].get("clipActions", np.Inf) 75 | self.rl_device = self._cfg.get("rl_device", "cuda:0") 76 | 77 | self.control_frequency_inv = self._cfg["task"]["env"].get("controlFrequencyInv", 1) 78 | 79 | print("RL device: ", self.rl_device) 80 | 81 | self._env = env 82 | 83 | if not hasattr(self, "_num_agents"): 84 | self._num_agents = 1 # used for multi-agent environments 85 | if not hasattr(self, "_num_states"): 86 | self._num_states = 0 87 | 88 | # initialize data spaces (defaults to gym.Box or Mushroom Box) 89 | if not hasattr(self, "action_space"): 90 | self.action_space = Box(np.ones(self.num_actions) * -1.0, np.ones(self.num_actions) * 1.0) 91 | if not hasattr(self, "observation_space"): 92 | self.observation_space = Box(np.ones(self.num_observations) * -np.Inf, np.ones(self.num_observations) * np.Inf) 93 | if not hasattr(self, "state_space"): 94 | self.state_space = Box(np.ones(self.num_states) * -np.Inf, np.ones(self.num_states) * np.Inf) 95 | 96 | self._cloner = GridCloner(spacing=self._env_spacing) 97 | self._cloner.define_base_env(self.default_base_env_path) 98 | define_prim(self.default_zero_env_path) 99 | 100 | self.cleanup() 101 | 102 | def cleanup(self) -> None: 103 | """ Prepares torch buffers for RL data collection.""" 104 | 105 | # prepare tensors 106 | self.obs_buf = torch.zeros((self._num_envs, self.num_observations), device=self._device, dtype=torch.float) 107 | self.states_buf = torch.zeros((self._num_envs, self.num_states), device=self._device, dtype=torch.float) 108 | self.rew_buf = torch.zeros(self._num_envs, device=self._device, dtype=torch.float) 109 | self.reset_buf = torch.ones(self._num_envs, device=self._device, dtype=torch.long) 110 | self.progress_buf = torch.zeros(self._num_envs, device=self._device, dtype=torch.long) 111 | # self.extras = torch.zeros(self._num_envs, device=self._device, dtype=torch.long) 112 | self.extras = dict() 113 | 114 | def set_up_scene(self, scene, replicate_physics=True) -> None: 115 | """ Clones environments based on value provided in task config and applies collision filters to mask 116 | collisions across environments. 117 | 118 | Args: 119 | scene (Scene): Scene to add objects to. 120 | replicate_physics (bool): Clone physics using PhysX API for better performance 121 | """ 122 | 123 | super().set_up_scene(scene) 124 | 125 | collision_filter_global_paths = list() 126 | if self._sim_config.task_config["sim"].get("add_ground_plane", True): 127 | self._ground_plane_path = "/World/defaultGroundPlane" 128 | collision_filter_global_paths.append(self._ground_plane_path) 129 | scene.add_ground_plane(prim_path=self._ground_plane_path, color=np.array([0.5, 0.1, 0.3])) 130 | prim_paths = self._cloner.generate_paths("/World/envs/env", self._num_envs) 131 | self._env_pos = self._cloner.clone(source_prim_path="/World/envs/env_0", prim_paths=prim_paths, replicate_physics=replicate_physics) 132 | self._env_pos = torch.tensor(np.array(self._env_pos), device=self._device, dtype=torch.float) 133 | self._cloner.filter_collisions( 134 | self._env._world.get_physics_context().prim_path, "/World/collisions", prim_paths, collision_filter_global_paths) 135 | self.set_initial_camera_params(camera_position=[10, 10, 3], camera_target=[0, 0, 0]) 136 | if self._sim_config.task_config["sim"].get("add_distant_light", True): 137 | create_distant_light(intensity=3000) 138 | 139 | def set_initial_camera_params(self, camera_position=[10, 10, 3], camera_target=[0, 0, 0]): 140 | if self._env._render: 141 | viewport_api_2 = get_viewport_from_window_name("Viewport") 142 | viewport_api_2.set_active_camera("/OmniverseKit_Persp") 143 | camera_state = ViewportCameraState("/OmniverseKit_Persp", viewport_api_2) 144 | camera_state.set_position_world(Gf.Vec3d(camera_position[0], camera_position[1], camera_position[2]), True) 145 | camera_state.set_target_world(Gf.Vec3d(camera_target[0], camera_target[1], camera_target[2]), True) 146 | # viewport = omni.kit.viewport_legacy.get_default_viewport_window() 147 | # viewport.set_camera_position("/OmniverseKit_Persp", camera_position[0], camera_position[1], camera_position[2], True) 148 | # viewport.set_camera_target("/OmniverseKit_Persp", camera_target[0], camera_target[1], camera_target[2], True) 149 | 150 | @property 151 | def default_base_env_path(self): 152 | """ Retrieves default path to the parent of all env prims. 153 | 154 | Returns: 155 | default_base_env_path(str): Defaults to "/World/envs". 156 | """ 157 | return "/World/envs" 158 | 159 | @property 160 | def default_zero_env_path(self): 161 | """ Retrieves default path to the first env prim (index 0). 162 | 163 | Returns: 164 | default_zero_env_path(str): Defaults to "/World/envs/env_0". 165 | """ 166 | return f"{self.default_base_env_path}/env_0" 167 | 168 | @property 169 | def num_envs(self): 170 | """ Retrieves number of environments for task. 171 | 172 | Returns: 173 | num_envs(int): Number of environments. 174 | """ 175 | return self._num_envs 176 | 177 | @property 178 | def num_actions(self): 179 | """ Retrieves dimension of actions. 180 | 181 | Returns: 182 | num_actions(int): Dimension of actions. 183 | """ 184 | return self._num_actions 185 | 186 | @property 187 | def num_observations(self): 188 | """ Retrieves dimension of observations. 189 | 190 | Returns: 191 | num_observations(int): Dimension of observations. 192 | """ 193 | return self._num_observations 194 | 195 | @property 196 | def num_states(self): 197 | """ Retrieves dimesion of states. 198 | 199 | Returns: 200 | num_states(int): Dimension of states. 201 | """ 202 | return self._num_states 203 | 204 | @property 205 | def num_agents(self): 206 | """ Retrieves number of agents for multi-agent environments. 207 | 208 | Returns: 209 | num_agents(int): Dimension of states. 210 | """ 211 | return self._num_agents 212 | 213 | def get_states(self): 214 | """ API for retrieving states buffer, used for asymmetric AC training. 215 | 216 | Returns: 217 | states_buf(torch.Tensor): States buffer. 218 | """ 219 | return self.states_buf 220 | 221 | def get_extras(self): 222 | """ API for retrieving extras data for RL. 223 | 224 | Returns: 225 | extras(dict): Dictionary containing extras data. 226 | """ 227 | return self.extras 228 | 229 | def reset(self): 230 | """ Flags all environments for reset. 231 | """ 232 | self.reset_buf = torch.ones_like(self.reset_buf) 233 | 234 | def pre_physics_step(self, actions): 235 | """ Optionally implemented by individual task classes to process actions. 236 | 237 | Args: 238 | actions (torch.Tensor): Actions generated by RL policy. 239 | """ 240 | pass 241 | 242 | def post_physics_step(self): 243 | """ Processes RL required computations for observations, states, rewards, resets, and extras. 244 | Also maintains progress buffer for tracking step count per environment. 245 | 246 | Returns: 247 | obs_buf(torch.Tensor): Tensor of observation data. 248 | rew_buf(torch.Tensor): Tensor of rewards data. 249 | reset_buf(torch.Tensor): Tensor of resets/dones data. 250 | extras(dict): Dictionary of extras data. 251 | """ 252 | 253 | self.progress_buf[:] += 1 254 | 255 | if self._env._world.is_playing(): 256 | self.get_observations() 257 | self.get_states() 258 | self.calculate_metrics() 259 | self.is_done() 260 | self.get_extras() 261 | 262 | return self.obs_buf, self.rew_buf, self.reset_buf, self.extras 263 | -------------------------------------------------------------------------------- /actpermoma/tasks/utils/pinoc_utils.py: -------------------------------------------------------------------------------- 1 | from __future__ import print_function 2 | import numpy as np 3 | import os 4 | import pinocchio as pin 5 | from scipy.spatial.transform import Rotation 6 | 7 | 8 | def get_se3_err(pos_first, quat_first, pos_second, quat_second): 9 | # Retruns 6 dimensional log.SE3 error between two poses expressed as position and quaternion rotation 10 | 11 | rot_first = Rotation.from_quat(np.array([quat_first[1],quat_first[2],quat_first[3],quat_first[0]])).as_matrix() # Quaternion in scalar last format!!! 12 | rot_second = Rotation.from_quat(np.array([quat_second[1],quat_second[2],quat_second[3],quat_second[0]])).as_matrix() # Quaternion in scalar last format!!! 13 | 14 | oMfirst = pin.SE3(rot_first, pos_first) 15 | oMsecond = pin.SE3(rot_second, pos_second) 16 | firstMsecond = oMfirst.actInv(oMsecond) 17 | 18 | return pin.log(firstMsecond).vector # log gives us a spatial vector (exp co-ords) 19 | 20 | 21 | class PinTiagoIKSolver(object): 22 | def __init__( 23 | self, 24 | urdf_name: str = "tiago_dual_holobase.urdf", 25 | move_group: str = "arm_right", # Can be 'arm_right' or 'arm_left' 26 | include_torso: bool = False, # Use torso in th IK solution 27 | include_base: bool = False, # Use base in th IK solution 28 | include_base_rotation: bool = False, # Use base rotation in the IK solution, only relevant/considered if include_base is False 29 | include_head: bool = False, # Use head in th IK solution 30 | max_rot_vel: float = 1.0472 31 | ) -> None: 32 | # Settings 33 | self.damp = 1e-10 # Damping co-efficient for linalg solve (to avoid singularities) 34 | self._include_torso = include_torso 35 | self._include_base = include_base 36 | self._include_base_rotation = include_base_rotation 37 | self._include_head = include_head 38 | self.max_rot_vel = max_rot_vel # Maximum rotational velocity of all joints 39 | 40 | ## Load urdf 41 | urdf_file = os.path.dirname(os.path.abspath(__file__)) + "/../../urdf/" + urdf_name 42 | self.model = pin.buildModelFromUrdf(urdf_file) 43 | # Choose joints 44 | name_end_effector = "gripper_"+move_group[4:]+"_grasping_frame" 45 | 46 | jointsOfInterest = [move_group+'_1_joint', move_group+'_2_joint', 47 | move_group+'_3_joint', move_group+'_4_joint', move_group+'_5_joint', 48 | move_group+'_6_joint', move_group+'_7_joint'] 49 | if self._include_torso: 50 | # Add torso joint 51 | jointsOfInterest = ['torso_lift_joint'] + jointsOfInterest 52 | if self._include_base: 53 | # Add base joints 54 | jointsOfInterest = ['X','Y','R'] + jointsOfInterest # 10 DOF with holo base joints included (11 with torso) 55 | elif self._include_base_rotation: 56 | # Add base rotation 57 | jointsOfInterest = ['R'] + jointsOfInterest 58 | if self._include_head: 59 | # Add head joints 60 | jointsOfInterest = jointsOfInterest + ['head_1_joint','head_2_joint'] # 13 DOF with holo base joints & torso included 61 | 62 | remove_ids = list() 63 | for jnt in jointsOfInterest: 64 | if self.model.existJointName(jnt): 65 | remove_ids.append(self.model.getJointId(jnt)) 66 | else: 67 | print('[IK WARNING]: joint ' + str(jnt) + ' does not belong to the model!') 68 | jointIdsToExclude = np.delete(np.arange(0,self.model.njoints), remove_ids) 69 | # Lock extra joints except joint 0 (root) 70 | reference_configuration=pin.neutral(self.model) 71 | if not self._include_torso: 72 | reference_configuration[26] = 0.25 # lock torso_lift_joint at 0.25 73 | self.model = pin.buildReducedModel(self.model, jointIdsToExclude[1:].tolist(), reference_configuration=reference_configuration) 74 | assert (len(self.model.joints)==(len(jointsOfInterest)+1)), "[IK Error]: Joints != nDoFs" 75 | self.model_data = self.model.createData() 76 | # Define Joint-Limits 77 | self.joint_pos_min = np.array([-1.1780972451, -1.1780972451, -0.785398163397, -0.392699081699, -2.09439510239, -1.41371669412, -2.09439510239]) 78 | self.joint_pos_max = np.array([+1.57079632679, +1.57079632679, +3.92699081699, +2.35619449019, +2.09439510239, +1.41371669412, +2.09439510239]) 79 | if self._include_torso: 80 | self.joint_pos_min = np.hstack((np.array([0.0]), self.joint_pos_min)) 81 | self.joint_pos_max = np.hstack((np.array([0.35]), self.joint_pos_max)) 82 | if self._include_base: 83 | self.joint_pos_min = np.hstack((np.array([-100.0, -100.0, -1.0, -1.0]), self.joint_pos_min)) 84 | self.joint_pos_max = np.hstack((np.array([+100.0, +100.0, +1.0, +1.0]),self.joint_pos_max)) 85 | elif self._include_base_rotation: 86 | self.joint_pos_min = np.hstack((np.array([-1.0, -1.0]), self.joint_pos_min)) 87 | self.joint_pos_max = np.hstack((np.array([+1.0, +1.0]), self.joint_pos_max)) 88 | if self._include_head: 89 | self.joint_pos_min = np.hstack((self.joint_pos_min, np.array([-1.24, -0.98]))) 90 | self.joint_pos_max = np.hstack((self.joint_pos_max, np.array([+1.24, +0.72]))) 91 | 92 | self.joint_pos_mid = (self.joint_pos_max + self.joint_pos_min)/2.0 93 | # Get End Effector Frame ID 94 | self.name_end_effector = name_end_effector 95 | self.id_EE = self.model.getFrameId(name_end_effector) 96 | 97 | def solve_fk_tiago(self, curr_joints, frame=None): 98 | """Get current Cartesian Pose of specified frame or end-effector if not specified 99 | 100 | @param curr_joints: list or ndarray of joint states with shape (num_joints,), joints referring to the ones set in __init__ as joints of interest 101 | @param frame: str name of frame to get pose of 102 | @returns pos 103 | np.array(w, x, y, z) rotation as quat 104 | """ 105 | if frame is None: 106 | frame_id = self.id_EE 107 | else: 108 | frame_id = self.model.getFrameId(frame) 109 | 110 | pin.framesForwardKinematics(self.model,self.model_data, curr_joints) 111 | oMf = self.model_data.oMf[frame_id] 112 | pos = oMf.translation 113 | quat = pin.Quaternion(oMf.rotation) 114 | 115 | return pos, np.array([quat.w, quat.x, quat.y, quat.z]) 116 | 117 | def solve_ik_pos_tiago(self, des_pos, des_quat, curr_joints=None, frame=None, n_trials=7, dt=0.1, pos_threshold=0.05, angle_threshold=15.*np.pi/180, verbose=False, debug=False): 118 | """Get IK joint positions for desired pose of frame or end-effector if not specified 119 | 120 | @param des_pos, des_quat: desired pose, Quaternion in scalar first format!!! 121 | @param curr_joints: list or ndarray of joint states with shape (num_joints,), joints referring to the ones set in __init__ as joints of interest 122 | @param frame: str name of frame to compute IK for 123 | @returns success: boolean 124 | best_q: ndarray of joint states 125 | """ 126 | damp = 1e-10 127 | success = False 128 | 129 | if des_quat is not None: 130 | # quaternion to rot matrix 131 | des_rot = Rotation.from_quat(np.array([des_quat[1],des_quat[2],des_quat[3],des_quat[0]])).as_matrix() # Quaternion in scalar last format!!! 132 | oMdes = pin.SE3(des_rot, des_pos) 133 | else: 134 | # 3D position error only 135 | des_rot = None 136 | 137 | if curr_joints is None: 138 | curr_joints = np.random.uniform(self.joint_pos_min, self.joint_pos_max) 139 | 140 | if frame is None: 141 | frame_id = self.id_EE 142 | else: 143 | frame_id = self.model.getFrameId(frame) 144 | 145 | q = curr_joints 146 | q_list = [] # list of joint states over iterations 147 | 148 | for n in range(n_trials): 149 | for i in range(800): # was 800 150 | pin.framesForwardKinematics(self.model,self.model_data, q) 151 | oMf = self.model_data.oMf[frame_id] 152 | if des_rot is None: 153 | oMdes = pin.SE3(oMf.rotation, des_pos) # Set rotation equal to current rotation to exclude this error 154 | dMf = oMdes.actInv(oMf) 155 | err = pin.log(dMf).vector 156 | if (np.linalg.norm(err[0:3]) < pos_threshold) and (np.linalg.norm(err[3:6]) < angle_threshold): 157 | success = True 158 | break 159 | J = pin.computeFrameJacobian(self.model,self.model_data,q,frame_id) 160 | if des_rot is None: 161 | J = J[:3,:] # Only pos errors 162 | err = err[:3] 163 | v = - J.T.dot(np.linalg.solve(J.dot(J.T) + damp * np.eye(6), err)) 164 | q = pin.integrate(self.model,q,v*dt) 165 | # Clip q to within joint limits 166 | q = np.clip(q, self.joint_pos_min, self.joint_pos_max) 167 | 168 | # debug 169 | q_list.append(q) 170 | if verbose: 171 | if not i % 100: 172 | print('Trial %d: iter %d: error = %s' % (n+1, i, err.T)) 173 | i += 1 174 | if success: 175 | best_q = np.array(q) 176 | break 177 | else: 178 | # Save current solution 179 | best_q = np.array(q) 180 | # Reset q to random configuration 181 | q = np.random.uniform(self.joint_pos_min, self.joint_pos_max) 182 | if verbose: 183 | if success: 184 | print("[[[[IK: Convergence achieved!]]]") 185 | else: 186 | print("[Warning: the IK iterative algorithm has not reached convergence to the desired precision]") 187 | 188 | if debug: 189 | return success, best_q, q_list 190 | else: 191 | return success, best_q 192 | -------------------------------------------------------------------------------- /actpermoma/tasks/utils/usd_utils.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2018-2022, NVIDIA Corporation 2 | # All rights reserved. 3 | # 4 | # Redistribution and use in source and binary forms, with or without 5 | # modification, are permitted provided that the following conditions are met: 6 | # 7 | # 1. Redistributions of source code must retain the above copyright notice, this 8 | # list of conditions and the following disclaimer. 9 | # 10 | # 2. Redistributions in binary form must reproduce the above copyright notice, 11 | # this list of conditions and the following disclaimer in the documentation 12 | # and/or other materials provided with the distribution. 13 | # 14 | # 3. Neither the name of the copyright holder nor the names of its 15 | # contributors may be used to endorse or promote products derived from 16 | # this software without specific prior written permission. 17 | # 18 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | 30 | from omni.isaac.core.utils.prims import get_prim_at_path 31 | from omni.isaac.core.utils.stage import get_current_stage 32 | from pxr import UsdPhysics, UsdLux 33 | 34 | def set_drive_type(prim_path, drive_type): 35 | joint_prim = get_prim_at_path(prim_path) 36 | 37 | # set drive type ("angular" or "linear") 38 | drive = UsdPhysics.DriveAPI.Apply(joint_prim, drive_type) 39 | return drive 40 | 41 | def set_drive_target_position(drive, target_value): 42 | if not drive.GetTargetPositionAttr(): 43 | drive.CreateTargetPositionAttr(target_value) 44 | else: 45 | drive.GetTargetPositionAttr().Set(target_value) 46 | 47 | def set_drive_target_velocity(drive, target_value): 48 | if not drive.GetTargetVelocityAttr(): 49 | drive.CreateTargetVelocityAttr(target_value) 50 | else: 51 | drive.GetTargetVelocityAttr().Set(target_value) 52 | 53 | def set_drive_stiffness(drive, stiffness): 54 | if not drive.GetStiffnessAttr(): 55 | drive.CreateStiffnessAttr(stiffness) 56 | else: 57 | drive.GetStiffnessAttr().Set(stiffness) 58 | 59 | def set_drive_damping(drive, damping): 60 | if not drive.GetDampingAttr(): 61 | drive.CreateDampingAttr(damping) 62 | else: 63 | drive.GetDampingAttr().Set(damping) 64 | 65 | def set_drive_max_force(drive, max_force): 66 | if not drive.GetMaxForceAttr(): 67 | drive.CreateMaxForceAttr(max_force) 68 | else: 69 | drive.GetMaxForceAttr().Set(max_force) 70 | 71 | def set_drive(prim_path, drive_type, target_type, target_value, stiffness, damping, max_force) -> None: 72 | drive = set_drive_type(prim_path, drive_type) 73 | 74 | # set target type ("position" or "velocity") 75 | if target_type == "position": 76 | set_drive_target_position(drive, target_value) 77 | elif target_type == "velocity": 78 | set_drive_target_velocity(drive, target_value) 79 | 80 | set_drive_stiffness(drive, stiffness) 81 | set_drive_damping(drive, damping) 82 | set_drive_max_force(drive, max_force) 83 | 84 | def create_distant_light(prim_path="/World/defaultDistantLight", intensity=5000): 85 | stage = get_current_stage() 86 | light = UsdLux.DistantLight.Define(stage, prim_path) 87 | light.CreateIntensityAttr().Set(intensity) -------------------------------------------------------------------------------- /actpermoma/usd/Props/Shapenet/godishus/models/model_cubic_approx.usd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pearl-robot-lab/ActPerMoMa/fdc15f5dd6b6c2cafbdbe18e98e97170e7cf5cd0/actpermoma/usd/Props/Shapenet/godishus/models/model_cubic_approx.usd -------------------------------------------------------------------------------- /actpermoma/usd/Props/Shapenet/godishus/models/model_normalized.usd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pearl-robot-lab/ActPerMoMa/fdc15f5dd6b6c2cafbdbe18e98e97170e7cf5cd0/actpermoma/usd/Props/Shapenet/godishus/models/model_normalized.usd -------------------------------------------------------------------------------- /actpermoma/usd/Props/Shapenet/mammut/models/model_cubic_approx.usd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pearl-robot-lab/ActPerMoMa/fdc15f5dd6b6c2cafbdbe18e98e97170e7cf5cd0/actpermoma/usd/Props/Shapenet/mammut/models/model_cubic_approx.usd -------------------------------------------------------------------------------- /actpermoma/usd/Props/Shapenet/mammut/models/model_normalized.json: -------------------------------------------------------------------------------- 1 | {"max": [0.77, 0.5014, 0.0], "centroid": [0.37699580954054024, 0.32128378378378325, -0.2801749295135136], "id": "a56200b624a644f7df6cfab91d65bb91", "numVertices": 296, "min": [0.0, 0.0, -0.55]} 2 | -------------------------------------------------------------------------------- /actpermoma/usd/Props/Shapenet/mammut/models/model_normalized.mtl: -------------------------------------------------------------------------------- 1 | # File produced by Open Asset Import Library (http://www.assimp.sf.net) 2 | # (assimp v3.2.202087883) 3 | 4 | newmtl material_0_24 5 | Kd 1 0.411765 0.705882 6 | Ka 0 0 0 7 | Ks 0.4 0.4 0.4 8 | Ke 0 0 0 9 | Ns 10 10 | illum 2 11 | 12 | newmtl material_1_24 13 | Kd 1 0.396078 0.698039 14 | Ka 0 0 0 15 | Ks 0.4 0.4 0.4 16 | Ke 0 0 0 17 | Ns 10 18 | illum 2 19 | 20 | -------------------------------------------------------------------------------- /actpermoma/usd/Props/Shapenet/mammut/models/model_normalized.solid.binvox: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pearl-robot-lab/ActPerMoMa/fdc15f5dd6b6c2cafbdbe18e98e97170e7cf5cd0/actpermoma/usd/Props/Shapenet/mammut/models/model_normalized.solid.binvox -------------------------------------------------------------------------------- /actpermoma/usd/Props/Shapenet/mammut/models/model_normalized.surface.binvox: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pearl-robot-lab/ActPerMoMa/fdc15f5dd6b6c2cafbdbe18e98e97170e7cf5cd0/actpermoma/usd/Props/Shapenet/mammut/models/model_normalized.surface.binvox -------------------------------------------------------------------------------- /actpermoma/usd/Props/Shapenet/mammut/models/model_normalized.usd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pearl-robot-lab/ActPerMoMa/fdc15f5dd6b6c2cafbdbe18e98e97170e7cf5cd0/actpermoma/usd/Props/Shapenet/mammut/models/model_normalized.usd -------------------------------------------------------------------------------- /actpermoma/usd/Props/Shapenet/mammut/models/model_normalized_pink.usd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pearl-robot-lab/ActPerMoMa/fdc15f5dd6b6c2cafbdbe18e98e97170e7cf5cd0/actpermoma/usd/Props/Shapenet/mammut/models/model_normalized_pink.usd -------------------------------------------------------------------------------- /actpermoma/usd/Props/YCB/Axis_Aligned/004_sugar_box.usd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pearl-robot-lab/ActPerMoMa/fdc15f5dd6b6c2cafbdbe18e98e97170e7cf5cd0/actpermoma/usd/Props/YCB/Axis_Aligned/004_sugar_box.usd -------------------------------------------------------------------------------- /actpermoma/usd/Props/YCB/Axis_Aligned/008_pudding_box.usd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pearl-robot-lab/ActPerMoMa/fdc15f5dd6b6c2cafbdbe18e98e97170e7cf5cd0/actpermoma/usd/Props/YCB/Axis_Aligned/008_pudding_box.usd -------------------------------------------------------------------------------- /actpermoma/usd/Props/YCB/Axis_Aligned/010_potted_meat_can.usd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pearl-robot-lab/ActPerMoMa/fdc15f5dd6b6c2cafbdbe18e98e97170e7cf5cd0/actpermoma/usd/Props/YCB/Axis_Aligned/010_potted_meat_can.usd -------------------------------------------------------------------------------- /actpermoma/usd/Props/YCB/Axis_Aligned/061_foam_brick.usd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pearl-robot-lab/ActPerMoMa/fdc15f5dd6b6c2cafbdbe18e98e97170e7cf5cd0/actpermoma/usd/Props/YCB/Axis_Aligned/061_foam_brick.usd -------------------------------------------------------------------------------- /actpermoma/usd/Props/YCB/Axis_Aligned/Materials/Textures/004_sugar_box_COLOR.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pearl-robot-lab/ActPerMoMa/fdc15f5dd6b6c2cafbdbe18e98e97170e7cf5cd0/actpermoma/usd/Props/YCB/Axis_Aligned/Materials/Textures/004_sugar_box_COLOR.png -------------------------------------------------------------------------------- /actpermoma/usd/Props/YCB/Axis_Aligned/Materials/Textures/008_pudding_box_COLOR.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pearl-robot-lab/ActPerMoMa/fdc15f5dd6b6c2cafbdbe18e98e97170e7cf5cd0/actpermoma/usd/Props/YCB/Axis_Aligned/Materials/Textures/008_pudding_box_COLOR.png -------------------------------------------------------------------------------- /actpermoma/usd/Props/YCB/Axis_Aligned/Materials/Textures/010_potted_meat_can_COLOR.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pearl-robot-lab/ActPerMoMa/fdc15f5dd6b6c2cafbdbe18e98e97170e7cf5cd0/actpermoma/usd/Props/YCB/Axis_Aligned/Materials/Textures/010_potted_meat_can_COLOR.png -------------------------------------------------------------------------------- /actpermoma/usd/Props/YCB/Axis_Aligned/Materials/Textures/061_foam_brick_COLOR.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pearl-robot-lab/ActPerMoMa/fdc15f5dd6b6c2cafbdbe18e98e97170e7cf5cd0/actpermoma/usd/Props/YCB/Axis_Aligned/Materials/Textures/061_foam_brick_COLOR.png -------------------------------------------------------------------------------- /actpermoma/usd/tiago_dual_holobase_zed/tiago_dual_holobase_zed.usd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pearl-robot-lab/ActPerMoMa/fdc15f5dd6b6c2cafbdbe18e98e97170e7cf5cd0/actpermoma/usd/tiago_dual_holobase_zed/tiago_dual_holobase_zed.usd -------------------------------------------------------------------------------- /actpermoma/usd/tiago_dual_holobase_zed/zed_descr/zed_descr.usd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pearl-robot-lab/ActPerMoMa/fdc15f5dd6b6c2cafbdbe18e98e97170e7cf5cd0/actpermoma/usd/tiago_dual_holobase_zed/zed_descr/zed_descr.usd -------------------------------------------------------------------------------- /actpermoma/utils/algo_utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import itertools 3 | from vgn.utils import look_at, spherical_to_cartesian 4 | 5 | class ViewHalfSphere: 6 | #src: https://github.com/ethz-asl/active_grasp/blob/devel/src/active_grasp/controller.py 7 | def __init__(self, bbox, min_z_dist): 8 | self.center = bbox.center 9 | self.r = 0.5 * bbox.size[2] + min_z_dist 10 | 11 | def get_view(self, theta, phi): 12 | eye = self.center + spherical_to_cartesian(self.r, theta, phi) 13 | up = np.r_[1.0, 0.0, 0.0] 14 | return look_at(eye, self.center, up) 15 | 16 | def sample_view(self): 17 | raise NotImplementedError 18 | 19 | class AABBox: 20 | # src: https://github.com/ethz-asl/active_grasp/blob/devel/src/active_grasp/bbox.py 21 | def __init__(self, bbox_min, bbox_max): 22 | self.min = np.asarray(bbox_min) 23 | self.max = np.asarray(bbox_max) 24 | self.center = 0.5 * (self.min + self.max) 25 | self.size = self.max - self.min 26 | 27 | @property 28 | def corners(self): 29 | return list(itertools.product(*np.vstack((self.min, self.max)).T)) 30 | 31 | def is_inside(self, p): 32 | return np.all(p > self.min) and np.all(p < self.max) -------------------------------------------------------------------------------- /actpermoma/utils/config_utils/default_scene_params.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2018-2022, NVIDIA Corporation 2 | # All rights reserved. 3 | # 4 | # Redistribution and use in source and binary forms, with or without 5 | # modification, are permitted provided that the following conditions are met: 6 | # 7 | # 1. Redistributions of source code must retain the above copyright notice, this 8 | # list of conditions and the following disclaimer. 9 | # 10 | # 2. Redistributions in binary form must reproduce the above copyright notice, 11 | # this list of conditions and the following disclaimer in the documentation 12 | # and/or other materials provided with the distribution. 13 | # 14 | # 3. Neither the name of the copyright holder nor the names of its 15 | # contributors may be used to endorse or promote products derived from 16 | # this software without specific prior written permission. 17 | # 18 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | 30 | default_physx_params = { 31 | 32 | ### Per-scene settings 33 | "use_gpu": False, 34 | "worker_thread_count": 4, 35 | "solver_type": 1, # 0: PGS, 1:TGS 36 | "bounce_threshold_velocity": 0.2, 37 | "friction_offset_threshold": 0.04, # A threshold of contact separation distance used to decide if a contact 38 | # point will experience friction forces. 39 | "friction_correlation_distance": 0.025, # Contact points can be merged into a single friction anchor if the 40 | # distance between the contacts is smaller than correlation distance. 41 | # disabling these can be useful for debugging 42 | "enable_sleeping": True, 43 | "enable_stabilization": True, 44 | 45 | # GPU buffers 46 | "gpu_max_rigid_contact_count": 512 * 1024, 47 | "gpu_max_rigid_patch_count": 80 * 1024, 48 | "gpu_found_lost_pairs_capacity": 1024, 49 | "gpu_found_lost_aggregate_pairs_capacity": 1024, 50 | "gpu_total_aggregate_pairs_capacity": 1024, 51 | "gpu_max_soft_body_contacts": 1024 * 1024, 52 | "gpu_max_particle_contacts": 1024 * 1024, 53 | "gpu_heap_capacity": 64 * 1024 * 1024, 54 | "gpu_temp_buffer_capacity": 16 * 1024 * 1024, 55 | "gpu_max_num_partitions": 8, 56 | 57 | ### Per-actor settings ( can override in actor_options ) 58 | "solver_position_iteration_count": 4, 59 | "solver_velocity_iteration_count": 1, 60 | "sleep_threshold": 0.0, # Mass-normalized kinetic energy threshold below which an actor may go to sleep. 61 | # Allowed range [0, max_float). 62 | "stabilization_threshold": 0.0, # Mass-normalized kinetic energy threshold below which an actor may 63 | # participate in stabilization. Allowed range [0, max_float). 64 | 65 | ### Per-body settings ( can override in actor_options ) 66 | "enable_gyroscopic_forces": False, 67 | "density": 1000.0, # density to be used for bodies that do not specify mass or density 68 | "max_depenetration_velocity": 100.0, 69 | 70 | ### Per-shape settings ( can override in actor_options ) 71 | "contact_offset": 0.02, 72 | "rest_offset": 0.001 73 | } 74 | 75 | default_physics_material = { 76 | "static_friction": 1.0, 77 | "dynamic_friction": 1.0, 78 | "restitution": 0.0 79 | } 80 | 81 | default_sim_params = { 82 | "gravity": [0.0, 0.0, -9.81], 83 | "dt": 1.0 / 60.0, 84 | "substeps": 1, 85 | "use_gpu_pipeline": True, 86 | "add_ground_plane": True, 87 | "add_distant_light": True, 88 | "use_flatcache": True, 89 | "enable_scene_query_support": False, 90 | "enable_cameras": False, 91 | "disable_contact_processing": False, 92 | "default_physics_material": default_physics_material 93 | } 94 | 95 | default_actor_options = { 96 | # -1 means use authored value from USD or default values from default_sim_params if not explicitly authored in USD. 97 | 98 | # If an attribute value is not explicitly authored in USD, add one with the value given here, 99 | # which overrides the USD default. 100 | "override_usd_defaults": False, 101 | "fixed_base": -1, 102 | "enable_self_collisions": -1, 103 | "enable_gyroscopic_forces": -1, 104 | "solver_position_iteration_count": -1, 105 | "solver_velocity_iteration_count": -1, 106 | "sleep_threshold": -1, 107 | "stabilization_threshold": -1, 108 | "max_depenetration_velocity": -1, 109 | "density": -1, 110 | "mass": -1, 111 | "contact_offset": -1, 112 | "rest_offset": -1 113 | } 114 | -------------------------------------------------------------------------------- /actpermoma/utils/config_utils/path_utils.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2018-2022, NVIDIA Corporation 2 | # All rights reserved. 3 | # 4 | # Redistribution and use in source and binary forms, with or without 5 | # modification, are permitted provided that the following conditions are met: 6 | # 7 | # 1. Redistributions of source code must retain the above copyright notice, this 8 | # list of conditions and the following disclaimer. 9 | # 10 | # 2. Redistributions in binary form must reproduce the above copyright notice, 11 | # this list of conditions and the following disclaimer in the documentation 12 | # and/or other materials provided with the distribution. 13 | # 14 | # 3. Neither the name of the copyright holder nor the names of its 15 | # contributors may be used to endorse or promote products derived from 16 | # this software without specific prior written permission. 17 | # 18 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | 30 | import carb 31 | from hydra.utils import to_absolute_path 32 | import omni.client 33 | import os 34 | 35 | def is_valid_local_file(path): 36 | return os.path.isfile(path) 37 | 38 | def is_valid_ov_file(path): 39 | result, entry = omni.client.stat(path) 40 | return result == omni.client.Result.OK 41 | 42 | def download_ov_file(source_path, target_path): 43 | result = omni.client.copy(source_path, target_path) 44 | 45 | if result == omni.client.Result.OK: 46 | return True 47 | return False 48 | 49 | def break_ov_path(path): 50 | return omni.client.break_url(path) 51 | 52 | def retrieve_checkpoint_path(path): 53 | # check if it's a local path 54 | if is_valid_local_file(path): 55 | return to_absolute_path(path) 56 | # check if it's an OV path 57 | elif is_valid_ov_file(path): 58 | ov_path = break_ov_path(path) 59 | file_name = os.path.basename(ov_path.path) 60 | target_path = f"checkpoints/{file_name}" 61 | copy_to_local = download_ov_file(path, target_path) 62 | return to_absolute_path(target_path) 63 | else: 64 | carb.log_error(f"Invalid checkpoint path: {path}") 65 | return None -------------------------------------------------------------------------------- /actpermoma/utils/files.py: -------------------------------------------------------------------------------- 1 | import actpermoma 2 | from pathlib import Path 3 | 4 | # get paths 5 | def get_root_path(): 6 | path = Path(actpermoma.__path__[0]).resolve() / '..' / '..' 7 | return path 8 | 9 | 10 | def get_urdf_path(): 11 | path = get_root_path() / 'urdf' 12 | return path 13 | 14 | 15 | def get_usd_path(): 16 | path = Path(actpermoma.__path__[0]).resolve()/ 'usd' 17 | return path 18 | 19 | 20 | def get_cfg_path(): 21 | path = path = Path(actpermoma.__path__[0]).resolve()/ 'cfg' 22 | return path 23 | -------------------------------------------------------------------------------- /actpermoma/utils/hydra_cfg/hydra_utils.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2018-2022, NVIDIA Corporation 2 | # All rights reserved. 3 | # 4 | # Redistribution and use in source and binary forms, with or without 5 | # modification, are permitted provided that the following conditions are met: 6 | # 7 | # 1. Redistributions of source code must retain the above copyright notice, this 8 | # list of conditions and the following disclaimer. 9 | # 10 | # 2. Redistributions in binary form must reproduce the above copyright notice, 11 | # this list of conditions and the following disclaimer in the documentation 12 | # and/or other materials provided with the distribution. 13 | # 14 | # 3. Neither the name of the copyright holder nor the names of its 15 | # contributors may be used to endorse or promote products derived from 16 | # this software without specific prior written permission. 17 | # 18 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | 30 | import hydra 31 | from omegaconf import DictConfig, OmegaConf 32 | 33 | ## OmegaConf & Hydra Config 34 | 35 | # Resolvers used in hydra configs (see https://omegaconf.readthedocs.io/en/2.1_branch/usage.html#resolvers) 36 | OmegaConf.register_new_resolver('eq', lambda x, y: x.lower()==y.lower()) 37 | OmegaConf.register_new_resolver('contains', lambda x, y: x.lower() in y.lower()) 38 | OmegaConf.register_new_resolver('if', lambda pred, a, b: a if pred else b) 39 | # allows us to resolve default arguments which are copied in multiple places in the config. used primarily for 40 | # num_ensv 41 | OmegaConf.register_new_resolver('resolve_default', lambda default, arg: default if arg=='' else arg) 42 | -------------------------------------------------------------------------------- /actpermoma/utils/hydra_cfg/reformat.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2018-2022, NVIDIA Corporation 2 | # All rights reserved. 3 | # 4 | # Redistribution and use in source and binary forms, with or without 5 | # modification, are permitted provided that the following conditions are met: 6 | # 7 | # 1. Redistributions of source code must retain the above copyright notice, this 8 | # list of conditions and the following disclaimer. 9 | # 10 | # 2. Redistributions in binary form must reproduce the above copyright notice, 11 | # this list of conditions and the following disclaimer in the documentation 12 | # and/or other materials provided with the distribution. 13 | # 14 | # 3. Neither the name of the copyright holder nor the names of its 15 | # contributors may be used to endorse or promote products derived from 16 | # this software without specific prior written permission. 17 | # 18 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | 30 | from omegaconf import DictConfig, OmegaConf 31 | from typing import Dict 32 | 33 | def omegaconf_to_dict(d: DictConfig)->Dict: 34 | """Converts an omegaconf DictConfig to a python Dict, respecting variable interpolation.""" 35 | ret = {} 36 | for k, v in d.items(): 37 | if isinstance(v, DictConfig): 38 | ret[k] = omegaconf_to_dict(v) 39 | else: 40 | ret[k] = v 41 | return ret 42 | 43 | def print_dict(val, nesting: int = -4, start: bool = True): 44 | """Outputs a nested dictionory.""" 45 | if type(val) == dict: 46 | if not start: 47 | print('') 48 | nesting += 4 49 | for k in val: 50 | print(nesting * ' ', end='') 51 | print(k, end=': ') 52 | print_dict(val[k], nesting, start=False) 53 | else: 54 | print(val) -------------------------------------------------------------------------------- /actpermoma/utils/simple_planar_collision_avoidance.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from actpermoma.utils.spatial_utils import * 3 | 4 | def avoid_collision_2d(current, radius, step, obstacle_bbox, alpha=0.1, beta=5, max_iter=100, is_aabb=False): 5 | """ 6 | :param current: [x, y] np.ndarray 7 | :param radius: float, assumed robot radius 8 | :param step: [x_step, y_step] np.ndarray 9 | :param obstacle_bboxes: [x_min, y_min, x_max, y_max] np.ndarray (axis-aligned bounding box) 10 | :return: [x, y] np.ndarray 11 | """ 12 | ## DO NOT MAKE THE STEP_SIZE LARGER THAN THE RADIUS OF THE ROBOT IF USING THIS FUNCTION 13 | 14 | step_length = np.linalg.norm(step) 15 | 16 | # check for collision with obstacle, if taking step 17 | new_space = create_circle_points(center=current + step, radius=radius) 18 | if is_aabb: 19 | collision_mask = check_collision_2d_aabbox(new_space, obstacle_bbox) 20 | else: 21 | collision_mask = check_collision_2d(new_space, obstacle_bbox) 22 | 23 | i = 0 24 | while sum(collision_mask) > 0 and i < max_iter: # collision detected 25 | i+=1 26 | 27 | # determine chord of collision 28 | coll_indices = np.arange(len(new_space))[collision_mask] 29 | first_coll = coll_indices[0] 30 | last_coll = coll_indices[-1] 31 | 32 | # handle case of collision sequence going over start/end 33 | if first_coll == 0 and last_coll == len(collision_mask) - 1: 34 | no_coll_indices = np.arange(len(new_space))[~collision_mask] 35 | first_coll = no_coll_indices[-1] + 1 36 | last_coll = no_coll_indices[0] - 1 37 | 38 | chord = new_space[last_coll] - new_space[first_coll] 39 | 40 | # adjust +- if needed 41 | if np.dot(chord, step) < 0: 42 | chord = -chord 43 | 44 | # determine depth of chord d 45 | if is_aabb: 46 | center_inside = check_collision_2d_aabbox([current], obstacle_bbox)[0] # we'll need this determine the correct case for calculating depth d 47 | else: 48 | center_inside = check_collision_2d([current], obstacle_bbox)[0] 49 | 50 | if center_inside: 51 | d = radius + np.sqrt(radius**2 - (np.linalg.norm(chord)/2)**2) 52 | else: 53 | d = radius - np.sqrt(radius**2 - (np.linalg.norm(chord)/2)**2) 54 | 55 | step = (beta * d + alpha) * chord + (step_length - beta * d - alpha) * step 56 | # resize it 57 | step = step/np.linalg.norm(step) * step_length 58 | 59 | # check for collision with obstacle, if taking newly determined step 60 | new_space = create_circle_points(center=current + step, radius=radius) 61 | if is_aabb: 62 | collision_mask = check_collision_2d_aabbox(new_space, obstacle_bbox) 63 | else: 64 | collision_mask = check_collision_2d(new_space, obstacle_bbox) 65 | 66 | if d < 0.02: 67 | break 68 | 69 | return current + step 70 | 71 | def check_collision_2d(points, obstacle_bbox, safety_margin=0.1): 72 | """ 73 | :param points: list of ([x, y] np.ndarray) 74 | :param obstacle_bbox: [x_min, y_min, x_max, y_max, z_to_ground, yaw] np.ndarray (bounding box) 75 | :return: list of bool, length of points 76 | """ 77 | # create bbox frame (add safety margin) 78 | obstacle_rot = Rotation.from_euler('z', obstacle_bbox[5]) 79 | safety_margin_applied = obstacle_rot.apply(np.array([safety_margin/2, safety_margin/2, 0])) 80 | obstacle_frame = Transform(translation=np.hstack((obstacle_bbox[:2], [0])) - safety_margin_applied, 81 | rotation=obstacle_rot) 82 | 83 | obstacle_frame_inv = obstacle_frame.inv() 84 | 85 | max_xy = obstacle_frame_inv.apply(np.array([obstacle_bbox[2], obstacle_bbox[3], 0]))[:2] + np.array([safety_margin/2, safety_margin/2]) 86 | 87 | collision_mask = [] 88 | for point in points: 89 | point = np.hstack((point, [0])) 90 | point = obstacle_frame_inv.apply(point) 91 | collision_mask.append(0 <= point[0] <= max_xy[0] and 0 <= point[1] <= max_xy[1]) 92 | 93 | return np.array(collision_mask) 94 | 95 | def check_collision_2d_aabbox(points, obstacle_aabbox): 96 | """ 97 | :param points: list of ([x, y] np.ndarray) 98 | :param obstacle_aabbox: [x_min, y_min, x_max, y_max] np.ndarray (axis-aligned bounding box) 99 | :return: list of bool, length of points 100 | """ 101 | return np.array([obstacle_aabbox[0] <= point[0] <= obstacle_aabbox[2] and \ 102 | obstacle_aabbox[1] <= point[1] <= obstacle_aabbox[3] for point in points]) 103 | 104 | def create_circle_points(center, radius, num=100): 105 | theta = np.linspace(0, 2*np.pi, num=num) 106 | x = center[0] + radius * np.cos(theta) 107 | y = center[1] + radius * np.sin(theta) 108 | return np.vstack((x, y)).T -------------------------------------------------------------------------------- /actpermoma/utils/spatial_utils.py: -------------------------------------------------------------------------------- 1 | # src: https://github.com/mbreyer/robot_helpers/blob/main/robot_helpers/spatial.py 2 | 3 | import copy 4 | 5 | import numpy as np 6 | from scipy.spatial.transform import Rotation 7 | 8 | 9 | class Transform: 10 | def __init__(self, rotation, translation): 11 | self.rotation = copy.deepcopy(rotation) 12 | self.translation = np.asarray(translation, np.double).copy() 13 | 14 | @classmethod 15 | def from_rotation(cls, rotation): 16 | translation = np.zeros(3) 17 | return cls(rotation, translation) 18 | 19 | @classmethod 20 | def from_translation(cls, translation): 21 | rotation = Rotation.identity() 22 | return cls(rotation, translation) 23 | 24 | @classmethod 25 | def from_matrix(cls, m): 26 | rotation = Rotation.from_matrix(m[:3, :3]) 27 | translation = m[:3, 3] 28 | return cls(rotation, translation) 29 | 30 | @classmethod 31 | def from_list(cls, l): 32 | return cls(Rotation.from_quat(l[:4]), l[4:]) 33 | 34 | @classmethod 35 | def identity(cls): 36 | rotation = Rotation.identity() 37 | translation = np.array([0.0, 0.0, 0.0]) 38 | return cls(rotation, translation) 39 | 40 | @classmethod 41 | def look_at(cls, eye, target, up): 42 | forward = np.subtract(target, eye) 43 | forward = np.divide(forward, np.linalg.norm(forward)) 44 | 45 | right = np.cross(forward, up) 46 | if np.linalg.norm(right) < 1e-3: 47 | right = np.cross(forward, up + np.r_[1e-3, 0, 0]) 48 | right = np.divide(right, np.linalg.norm(right)) 49 | 50 | up = np.cross(right, forward) 51 | up = np.divide(up, np.linalg.norm(up)) 52 | 53 | m = np.array( 54 | [ 55 | [right[0], -up[0], forward[0], eye[0]], 56 | [right[1], -up[1], forward[1], eye[1]], 57 | [right[2], -up[2], forward[2], eye[2]], 58 | [0.0, 0.0, 0.0, 1.0], 59 | ] 60 | ) 61 | 62 | return cls.from_matrix(m) 63 | 64 | def __mul__(self, other): 65 | rotation = self.rotation * other.rotation 66 | translation = self.rotation.apply(other.translation) + self.translation 67 | return self.__class__(rotation, translation) 68 | 69 | def inv(self): 70 | rotation = self.rotation.inv() 71 | translation = -rotation.apply(self.translation) 72 | return self.__class__(rotation, translation) 73 | 74 | def apply(self, point): 75 | return self.rotation.apply(point) + self.translation 76 | 77 | def as_matrix(self): 78 | return np.vstack( 79 | ( 80 | np.c_[self.rotation.as_matrix(), self.translation], 81 | [0.0, 0.0, 0.0, 1.0], 82 | ) 83 | ) 84 | 85 | def to_list(self): 86 | return np.r_[self.rotation.as_quat(), self.translation] 87 | 88 | class TClass: 89 | """ 90 | Convenient way to create a pure translation. 91 | 92 | Transform.t_[x, y, z] is equivalent to Transform.from_translation(np.r_[x, y, z]). 93 | """ 94 | 95 | def __getitem__(self, key): 96 | return Transform.from_translation(np.r_[key]) 97 | 98 | t_ = TClass() 99 | 100 | 101 | def look_at(eye, center, up): 102 | eye = np.asarray(eye) 103 | center = np.asarray(center) 104 | forward = center - eye 105 | forward /= np.linalg.norm(forward) 106 | right = np.cross(forward, up) 107 | right /= np.linalg.norm(right) 108 | up = np.asarray(up) / np.linalg.norm(up) 109 | up = np.cross(right, forward) 110 | m = np.eye(4, 4) 111 | m[:3, 0] = right 112 | m[:3, 1] = -up 113 | m[:3, 2] = forward 114 | m[:3, 3] = eye 115 | return Transform.from_matrix(m) 116 | -------------------------------------------------------------------------------- /actpermoma/utils/task_util.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2018-2022, NVIDIA Corporation 2 | # All rights reserved. 3 | # 4 | # Redistribution and use in source and binary forms, with or without 5 | # modification, are permitted provided that the following conditions are met: 6 | # 7 | # 1. Redistributions of source code must retain the above copyright notice, this 8 | # list of conditions and the following disclaimer. 9 | # 10 | # 2. Redistributions in binary form must reproduce the above copyright notice, 11 | # this list of conditions and the following disclaimer in the documentation 12 | # and/or other materials provided with the distribution. 13 | # 14 | # 3. Neither the name of the copyright holder nor the names of its 15 | # contributors may be used to endorse or promote products derived from 16 | # this software without specific prior written permission. 17 | # 18 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | 30 | def initialize_task(config, env, init_sim=True): 31 | from actpermoma.tasks.tiago_dual_active_perception_naive import TiagoDualActivePerceptionNaiveTask 32 | from actpermoma.tasks.tiago_dual_active_perception import TiagoDualActivePerceptionTask 33 | 34 | # Mappings from strings to environments 35 | task_map = { 36 | "TiagoDualActivePerceptionNaive": TiagoDualActivePerceptionNaiveTask, 37 | "TiagoDualActivePerception": TiagoDualActivePerceptionTask, 38 | } 39 | 40 | from .config_utils.sim_config import SimConfig 41 | sim_config = SimConfig(config) 42 | 43 | cfg = sim_config.config 44 | task = task_map[cfg["task_name"]]( 45 | name=cfg["task_name"], sim_config=sim_config, env=env 46 | ) 47 | 48 | env.set_task(task=task, sim_params=sim_config.get_physics_params(), backend="torch", init_sim=init_sim) 49 | 50 | return task -------------------------------------------------------------------------------- /actpermoma/utils/usd_utils/create_instanceable_assets.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2018-2022, NVIDIA Corporation 2 | # All rights reserved. 3 | # 4 | # Redistribution and use in source and binary forms, with or without 5 | # modification, are permitted provided that the following conditions are met: 6 | # 7 | # 1. Redistributions of source code must retain the above copyright notice, this 8 | # list of conditions and the following disclaimer. 9 | # 10 | # 2. Redistributions in binary form must reproduce the above copyright notice, 11 | # this list of conditions and the following disclaimer in the documentation 12 | # and/or other materials provided with the distribution. 13 | # 14 | # 3. Neither the name of the copyright holder nor the names of its 15 | # contributors may be used to endorse or promote products derived from 16 | # this software without specific prior written permission. 17 | # 18 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | 30 | import omni.usd 31 | import omni.client 32 | 33 | from pxr import UsdGeom, Sdf 34 | 35 | 36 | def update_reference(source_prim_path, source_reference_path, target_reference_path): 37 | stage = omni.usd.get_context().get_stage() 38 | 39 | prims = [stage.GetPrimAtPath(source_prim_path)] 40 | while len(prims) > 0: 41 | prim = prims.pop(0) 42 | prim_spec = stage.GetRootLayer().GetPrimAtPath(prim.GetPath()) 43 | reference_list = prim_spec.referenceList 44 | refs = reference_list.GetAddedOrExplicitItems() 45 | if len(refs) > 0: 46 | for ref in refs: 47 | if ref.assetPath == source_reference_path: 48 | prim.GetReferences().RemoveReference(ref) 49 | prim.GetReferences().AddReference(assetPath=target_reference_path, primPath=prim.GetPath()) 50 | 51 | prims = prims + prim.GetChildren() 52 | 53 | def create_parent_xforms(asset_usd_path, source_prim_path, save_as_path=None): 54 | """ Adds a new UsdGeom.Xform prim for each Mesh/Geometry prim under source_prim_path. 55 | Moves material assignment to new parent prim if any exists on the Mesh/Geometry prim. 56 | 57 | Args: 58 | asset_usd_path (str): USD file path for asset 59 | source_prim_path (str): USD path of root prim 60 | save_as_path (str): USD file path for modified USD stage. Defaults to None, will save in same file. 61 | """ 62 | omni.usd.get_context().open_stage(asset_usd_path) 63 | stage = omni.usd.get_context().get_stage() 64 | 65 | prims = [stage.GetPrimAtPath(source_prim_path)] 66 | edits = Sdf.BatchNamespaceEdit() 67 | while len(prims) > 0: 68 | prim = prims.pop(0) 69 | print(prim) 70 | if prim.GetTypeName() in ["Mesh", "Capsule", "Sphere", "Box"]: 71 | new_xform = UsdGeom.Xform.Define(stage, str(prim.GetPath()) + "_xform") 72 | print(prim, new_xform) 73 | edits.Add(Sdf.NamespaceEdit.Reparent(prim.GetPath(), new_xform.GetPath(), 0)) 74 | continue 75 | 76 | children_prims = prim.GetChildren() 77 | prims = prims + children_prims 78 | 79 | stage.GetRootLayer().Apply(edits) 80 | 81 | if save_as_path is None: 82 | omni.usd.get_context().save_stage() 83 | else: 84 | omni.usd.get_context().save_as_stage(save_as_path) 85 | 86 | def convert_asset_instanceable(asset_usd_path, source_prim_path, save_as_path=None, create_xforms=True): 87 | """ Makes all mesh/geometry prims instanceable. 88 | Can optionally add UsdGeom.Xform prim as parent for all mesh/geometry prims. 89 | Makes a copy of the asset USD file, which will be used for referencing. 90 | Updates asset file to convert all parent prims of mesh/geometry prims to reference cloned USD file. 91 | 92 | Args: 93 | asset_usd_path (str): USD file path for asset 94 | source_prim_path (str): USD path of root prim 95 | save_as_path (str): USD file path for modified USD stage. Defaults to None, will save in same file. 96 | create_xforms (bool): Whether to add new UsdGeom.Xform prims to mesh/geometry prims. 97 | """ 98 | 99 | if create_xforms: 100 | create_parent_xforms(asset_usd_path, source_prim_path, save_as_path) 101 | asset_usd_path = save_as_path 102 | 103 | instance_usd_path = ".".join(asset_usd_path.split(".")[:-1]) + "_meshes.usd" 104 | omni.client.copy(asset_usd_path, instance_usd_path) 105 | omni.usd.get_context().open_stage(asset_usd_path) 106 | stage = omni.usd.get_context().get_stage() 107 | 108 | prims = [stage.GetPrimAtPath(source_prim_path)] 109 | while len(prims) > 0: 110 | prim = prims.pop(0) 111 | if prim: 112 | if prim.GetTypeName() in ["Mesh", "Capsule", "Sphere", "Box"]: 113 | parent_prim = prim.GetParent() 114 | if parent_prim and not parent_prim.IsInstance(): 115 | parent_prim.GetReferences().AddReference(assetPath=instance_usd_path, primPath=str(parent_prim.GetPath())) 116 | parent_prim.SetInstanceable(True) 117 | continue 118 | 119 | children_prims = prim.GetChildren() 120 | prims = prims + children_prims 121 | 122 | if save_as_path is None: 123 | omni.usd.get_context().save_stage() 124 | else: 125 | omni.usd.get_context().save_as_stage(save_as_path) 126 | -------------------------------------------------------------------------------- /actpermoma/utils/usd_utils/ov_utils.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2018-2022, NVIDIA Corporation 2 | # All rights reserved. 3 | # 4 | # Redistribution and use in source and binary forms, with or without 5 | # modification, are permitted provided that the following conditions are met: 6 | # 7 | # 1. Redistributions of source code must retain the above copyright notice, this 8 | # list of conditions and the following disclaimer. 9 | # 10 | # 2. Redistributions in binary form must reproduce the above copyright notice, 11 | # this list of conditions and the following disclaimer in the documentation 12 | # and/or other materials provided with the distribution. 13 | # 14 | # 3. Neither the name of the copyright holder nor the names of its 15 | # contributors may be used to endorse or promote products derived from 16 | # this software without specific prior written permission. 17 | # 18 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | 30 | import omni.client 31 | 32 | def is_valid_ov_file(path): 33 | result, entry = omni.client.stat(path) 34 | return result == omni.client.Result.OK 35 | 36 | def download_ov_file(source_path, target_path): 37 | result = omni.client.copy(source_path, target_path) 38 | 39 | if result == omni.client.Result.OK: 40 | return True 41 | return False 42 | 43 | def break_ov_path(path): 44 | return omni.client.break_url(path) -------------------------------------------------------------------------------- /actpermoma/utils/visualisation_utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from actpermoma.utils.spatial_utils import Rotation, Transform 3 | from copy import deepcopy as cp 4 | 5 | class Visualizer: 6 | def __init__(self) -> None: 7 | from omni.isaac.debug_draw import _debug_draw as dd 8 | self.dd = dd.acquire_debug_draw_interface() 9 | 10 | # colors 11 | self.blue = (0, 0, 1, 1) 12 | self.red = (1, 0, 0, 1) 13 | self.green = (0, 1, 0, 1) 14 | self.light_blue = (0.68, 0.85, 0.9, 1) 15 | self.pink = (1, 0.75, 0.8, 1) 16 | self.light_green = (0.7, 1, 0.7, 1) 17 | self.light_grey = (0.8, 0.8, 0.8, 1) 18 | self.mid_grey = (0.5, 0.5, 0.5, 1) 19 | self.light_lilac = (0.8, 0.8, 1, 1) 20 | self.mid_blue = (0.34, 0.43, 1, 1) 21 | self.light_teal = (0.6, 0.86, 0.85, 1) 22 | 23 | def draw_frame_matrix(self, frame_matrix, point_size=5, axis_length=0.3, complement_length=0.0) -> None: 24 | """ 25 | Draw a frame matrix as a set of three lines representing the x (red), y (green) and z (blues) axes 26 | :param frame_matrix: 4x4 matrix representing the frame 27 | :param point_size: size of the point size used to draw the axes 28 | :param axis_length: length of the axes in m 29 | :return: 30 | """ 31 | frame = np.asarray(frame_matrix) 32 | frame_template = [np.array([[0.0], [0.0], [0.0], [1]]), # center point 33 | np.array([[axis_length], [0.0], [0.0], [1]]), 34 | np.array([[0.0], [axis_length], [0.0], [1]]), 35 | np.array([[0.0], [0.0], [axis_length], [1]])] 36 | 37 | c, x, y, z = [frame @ point for point in frame_template] 38 | 39 | point_list_start = [c, c, c] 40 | point_list_end = [x, y, z] 41 | self.dd.draw_lines(point_list_start, point_list_end, [self.red, self.green, self.blue], [point_size]*3) 42 | 43 | if complement_length != 0.0: 44 | com_frame = frame.copy() 45 | com_frame[:3,:3] = -com_frame[:3,:3] 46 | com_frame[:3,-1] += complement_length 47 | self.draw_frame_matrix(com_frame) 48 | 49 | def draw_frame_pos_quat(self, frame_pos_quat, point_size=5, axis_length=0.3) -> None: 50 | """ 51 | Draw a frame matrix as a set of three lines representing the x (red), y (green) and z (blues) axes 52 | :param frame_pos_quat: tuple of (pos, quat), np.arras of shape (3,) and (4,) respectively; quat: [w, x, y, z] 53 | :param point_size: size of the point size used to draw the axes 54 | :param axis_length: length of the axes in m 55 | :return: 56 | """ 57 | pos, quat = frame_pos_quat 58 | T = Transform.from_translation(pos) 59 | T.rotation = Rotation.from_quat([quat[1], quat[2], quat[3], quat[0]]) 60 | 61 | self.draw_frame_matrix(T.as_matrix(), point_size, axis_length) 62 | 63 | def draw_voxels_from_center(self, pcl, voxel_size, point_size=3, colors=None) -> None: 64 | """ 65 | Draw voxels as cubes around a list of points as centers 66 | :param pcl: np.array of shape (N, 3) or list of np.arrays of shape (3,) representing the point cloud 67 | :param voxel_size: sidelength of the voxels 68 | :param point_size: size of the point size used to draw the cubes 69 | :param colors: color (RGBA tupel)/colors (list of N) used 70 | :return: 71 | """ 72 | if colors is None: colors = [self.mid_blue]*len(pcl) 73 | if type(colors) is tuple: colors = [colors]*len(pcl) 74 | 75 | box_template = [np.array([-voxel_size / 2, -voxel_size / 2, -voxel_size / 2]), 76 | np.array([-voxel_size / 2, -voxel_size / 2, voxel_size / 2]), 77 | np.array([-voxel_size / 2, voxel_size / 2, -voxel_size / 2]), 78 | np.array([-voxel_size / 2, voxel_size / 2, voxel_size / 2]), 79 | np.array([ voxel_size / 2, -voxel_size / 2, -voxel_size / 2]), 80 | np.array([ voxel_size / 2, -voxel_size / 2, voxel_size / 2]), 81 | np.array([ voxel_size / 2, voxel_size / 2, -voxel_size / 2]), 82 | np.array([ voxel_size / 2, voxel_size / 2, voxel_size / 2])] 83 | 84 | for point, color in zip(pcl, colors): 85 | m_m_m, m_m_p, m_p_m, m_p_p, p_m_m, p_m_p, p_p_m, p_p_p = point + box_template 86 | point_list_start = [m_m_m, m_m_m, m_m_m, m_m_p, m_m_p, m_p_m, m_p_m, p_m_m, p_m_m, m_p_p, p_m_p, p_p_m] 87 | point_list_end = [m_m_p, m_p_m, p_m_m, m_p_p, p_m_p, m_p_p, p_p_m, p_m_p, p_p_m, p_p_p, p_p_p, p_p_p] 88 | 89 | self.dd.draw_lines(point_list_start, point_list_end, [color]*len(point_list_start), [point_size]*len(point_list_start)) 90 | 91 | def draw_points(self, pcl, point_size=10, colors=None) -> None: 92 | """ 93 | Draw points/spheres a list of points as centers 94 | :param pcl: array-like of shape (N, 3) or representing the point cloud 95 | :param point_size: size of the point size 96 | :param colors: colors used list of N RGBA colors 97 | :return: 98 | """ 99 | point_tuples = [] 100 | for point in pcl: 101 | point_tuples.append(tuple(point)) 102 | 103 | if colors is None: colors = [self.pink]*len(point_tuples) 104 | elif colors=='light_green': colors = [self.light_green]*len(point_tuples) 105 | elif type(colors) is tuple: colors = [colors]*len(point_tuples) 106 | 107 | self.dd.draw_points(point_tuples, colors, [point_size] * len(point_tuples)) 108 | 109 | def draw_box_min_max(self, box_min, box_max, point_size=1, color=None) -> None: 110 | """ 111 | Draw a box as a set of lines 112 | :param box_min: np.array of shape (3,) representing the minimum corner of the box 113 | :param box_max: np.array of shape (3,) representing the maximum corner of the box 114 | :param point_size: size of the point size used to draw the box 115 | :param color: color used to draw the box 116 | :return: 117 | """ 118 | if color is None: color = self.light_grey 119 | 120 | box_template = [np.array([box_min[0], box_min[1], box_min[2]]), 121 | np.array([box_min[0], box_min[1], box_max[2]]), 122 | np.array([box_min[0], box_max[1], box_min[2]]), 123 | np.array([box_min[0], box_max[1], box_max[2]]), 124 | np.array([box_max[0], box_min[1], box_min[2]]), 125 | np.array([box_max[0], box_min[1], box_max[2]]), 126 | np.array([box_max[0], box_max[1], box_min[2]]), 127 | np.array([box_max[0], box_max[1], box_max[2]])] 128 | 129 | m_m_m, m_m_p, m_p_m, m_p_p, p_m_m, p_m_p, p_p_m, p_p_p = box_template 130 | point_list_start = [m_m_m, m_m_m, m_m_m, m_m_p, m_m_p, m_p_m, m_p_m, p_m_m, p_m_m, m_p_p, p_m_p, p_p_m] 131 | point_list_end = [m_m_p, m_p_m, p_m_m, m_p_p, p_m_p, m_p_p, p_p_m, p_m_p, p_p_m, p_p_p, p_p_p, p_p_p] 132 | 133 | self.dd.draw_lines(point_list_start, point_list_end, [color]*len(point_list_start), [point_size]*len(point_list_start)) 134 | 135 | def draw_views(self, views, colors=None, draw_frames=True, point_size=1, axis_length=0.1, connect_views=False, intrinsics=None) -> None: 136 | """ 137 | Draw voxels as cubes around a list of points as centers 138 | :param views: list of Transform objects representing the views (z pointing towards view direction) 139 | :param draw_frames: Boolean indicating whether to draw the frame of the view 140 | :param point_size: size of the point size used to draw the camera pose & frame 141 | :param axis_length: length of the frame axis in m 142 | :param connect_views: Boolean indicating whether to draw lines connecting the views' origins 143 | :param intrinsics: list of intrinsic parameters: [fx, fy, width, height] 144 | :return: 145 | """ 146 | if colors is None: colors = [self.light_green]*len(views) 147 | 148 | if intrinsics is None: 149 | camera_template = [Transform.from_translation([0.0, 0.0, 0.0]), # center point 150 | Transform.from_translation([-0.032, -0.024, 0.03]), 151 | Transform.from_translation([-0.032, 0.024, 0.03]), 152 | Transform.from_translation([0.032, -0.024, 0.03]), 153 | Transform.from_translation([0.032, 0.024, 0.03])] 154 | else: 155 | fx, fy, width, height = intrinsics 156 | half_aperture_x = width/fx * 0.03 157 | half_aperture_y = height/fy * 0.03 158 | 159 | camera_template = [Transform.from_translation([0.0, 0.0, 0.0]), # center point 160 | Transform.from_translation([-half_aperture_x, -half_aperture_y, 0.03]), 161 | Transform.from_translation([-half_aperture_x, half_aperture_y, 0.03]), 162 | Transform.from_translation([half_aperture_x, -half_aperture_y, 0.03]), 163 | Transform.from_translation([half_aperture_x, half_aperture_y, 0.03])] 164 | 165 | camera_poses = [[view * point for point in camera_template] for view in views] 166 | 167 | for index, view_sign_points in enumerate(camera_poses): 168 | c, lb, lt, rb, rt = [tuple(point.translation) for point in view_sign_points] 169 | point_list_start = [lt, lb, c, lt, rt, c, rb, rt, rb] 170 | point_list_end = [lb, c, lt, rt, c, rb, rt, rb, lb] 171 | self.dd.draw_lines(point_list_start, point_list_end, [colors[index]] * len(point_list_start), [point_size] * len(point_list_start)) 172 | 173 | if draw_frames: 174 | for T in views: 175 | T = T.as_matrix() 176 | self.draw_frame_matrix(T, point_size=point_size, axis_length=axis_length) 177 | 178 | if connect_views: 179 | point_list_start = [view.translation for view in views] 180 | point_list_end = cp(point_list_start[1:]) 181 | point_list_start = point_list_start[:-1] 182 | 183 | self.dd.draw_lines(point_list_start, point_list_end, colors[:-1], [point_size]*len(point_list_start)) 184 | 185 | def interpolate_quality_colors(self, qualities, qual_min=None, qual_max=None, opacity=1) -> list: 186 | """ 187 | Interpolate colors from red to green according to qual_min and qual_max 188 | :param qualities: list of N qualities 189 | :param qual_min: minimum quality value 190 | :param qual_max: maximum quality value 191 | :param opacity: opacity of the colors 192 | :return: list of N RGBA colors 193 | """ 194 | if qualities is None or len(qualities)==0: return [(0, 1, 0, opacity)] # green 195 | if qual_min is None: qual_min = np.min(qualities) 196 | if qual_max is None: qual_max = np.max(qualities) 197 | 198 | if qual_max > qual_min: 199 | colors = [] 200 | for q in qualities: 201 | g = (q - qual_min) / (qual_max - qual_min) 202 | r = 1.0 - g 203 | colors.append((r, g, 0, opacity)) 204 | 205 | return colors 206 | else: 207 | return [(0, 1, 0, opacity)]*len(qualities) # green 208 | 209 | def draw_grasps_at(self, grasp_poses, depth=0.05, width=0.05, point_size=5, colors=None): 210 | """ 211 | Creates a list of VisualCylinder objects representing the grasp 212 | :param poses: Transform representing the grasp poses 213 | :param depth: depth of the grasp 214 | :param width: width of the grasp 215 | :param radius: radius of the cylinders 216 | :param colors: color the grasps are drawn in 217 | :return: 218 | """ 219 | if type(grasp_poses) is not list: grasp_poses = [grasp_poses] 220 | if colors is None: colors = [self.green]*len(grasp_poses) 221 | elif type(colors) is tuple: colors = [colors]*len(grasp_poses) 222 | 223 | # z pointing towards the grasp direction, y along the width 224 | grasp_template = [Transform.from_translation([0.0, -width/2, depth]), 225 | Transform.from_translation([0.0, -width/2, 0]), 226 | Transform.from_translation([0.0, 0.0, -depth]), 227 | Transform.from_translation([0.0, 0.0, 0.0]), 228 | Transform.from_translation([0.0, width/2, 0]), 229 | Transform.from_translation([0.0, width/2, depth])] 230 | 231 | grasp_poses = [[pose * point for point in grasp_template] for pose in grasp_poses] 232 | 233 | for index, grasp_sign_points in enumerate(grasp_poses): 234 | dl, ul, cu, cd, ur, dr = [tuple(point.translation) for point in grasp_sign_points] 235 | point_list_start = [dl, ul, ur, cu] 236 | point_list_end = [ul, ur, dr, cd] 237 | 238 | self.dd.draw_lines(point_list_start, point_list_end, [colors[index]] * len(point_list_start), [point_size] * len(point_list_start)) 239 | 240 | def clear_all(self) -> None: 241 | self.dd.clear_lines() 242 | self.dd.clear_points() -------------------------------------------------------------------------------- /actpermoma/utils/wandb_utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import wandb 3 | 4 | class wandbLogger: 5 | def __init__(self, exp_config, run_name=None, group_name=None, disabled=False): 6 | if disabled: 7 | mode="disabled" 8 | else: 9 | mode=None 10 | self.run = wandb.init(project="looking_good", entity="irosa-ias", group=group_name, id=run_name, config=exp_config, 11 | reinit=True, resume="allow", settings=wandb.Settings(start_method="fork"), mode=mode) 12 | 13 | def run_log_wandb(self,success_rate, J, R, E, avg_episode_length, q_loss): 14 | wandb.log({"success_rate": success_rate, "J": J, "R": R, "entropy": E, 15 | "avg_episode_length": avg_episode_length, "q_loss": q_loss}) 16 | 17 | def log_metrics(self, metrics, name_spc): 18 | wandb.log(metrics) 19 | wandb.log({"name_spec": name_spc}) 20 | 21 | def vid_log_wandb(self,img_dataset): 22 | np_video = np.expand_dims(np.moveaxis(img_dataset[0][0],-1,0), axis=0) 23 | for tup in img_dataset: 24 | np_video = np.vstack((np_video, np.expand_dims(np.moveaxis(tup[0],-1,0), axis=0))) 25 | wandb.log({"video": wandb.Video(np_video, fps=60, format="gif")}) 26 | 27 | def finish(self): 28 | self.run.finish() -------------------------------------------------------------------------------- /env.yaml: -------------------------------------------------------------------------------- 1 | name: actpermoma 2 | channels: 3 | - pytorch 4 | - nvidia 5 | - conda-forge 6 | - defaults 7 | dependencies: 8 | - _libgcc_mutex=0.1=conda_forge 9 | - _openmp_mutex=4.5=2_kmp_llvm 10 | - assimp=5.2.3=h4c92fa2_1 11 | - blas=1.0=openblas 12 | - boost=1.74.0=py310h7c3ba0c_5 13 | - boost-cpp=1.74.0=h75c5d50_8 14 | - brotli-python=1.0.9=py310h6a678d5_7 15 | - bzip2=1.0.8=h7b6447c_0 16 | - ca-certificates=2024.6.2=hbcca054_0 17 | - certifi=2024.6.2=pyhd8ed1ab_0 18 | - cffi=1.16.0=py310h5eee18b_0 19 | - charset-normalizer=2.0.4=pyhd3eb1b0_0 20 | - console_bridge=1.0.2=h924138e_1 21 | - cuda-cccl=12.3.101=0 22 | - cuda-command-line-tools=12.1.1=0 23 | - cuda-compiler=12.3.2=0 24 | - cuda-cudart=12.1.105=0 25 | - cuda-cudart-dev=12.1.105=0 26 | - cuda-cudart-static=12.1.105=0 27 | - cuda-cuobjdump=12.3.101=0 28 | - cuda-cupti=12.1.105=0 29 | - cuda-cupti-static=12.1.105=0 30 | - cuda-cuxxfilt=12.3.101=0 31 | - cuda-documentation=12.3.101=0 32 | - cuda-driver-dev=12.3.101=0 33 | - cuda-gdb=12.3.101=0 34 | - cuda-libraries=12.1.0=0 35 | - cuda-libraries-dev=12.1.0=0 36 | - cuda-libraries-static=12.1.0=0 37 | - cuda-nsight=12.3.101=0 38 | - cuda-nsight-compute=12.3.2=0 39 | - cuda-nvcc=12.3.107=0 40 | - cuda-nvdisasm=12.3.101=0 41 | - cuda-nvml-dev=12.3.101=0 42 | - cuda-nvprof=12.3.101=0 43 | - cuda-nvprune=12.3.101=0 44 | - cuda-nvrtc=12.1.105=0 45 | - cuda-nvrtc-dev=12.1.105=0 46 | - cuda-nvrtc-static=12.1.105=0 47 | - cuda-nvtx=12.1.105=0 48 | - cuda-nvvp=12.3.101=0 49 | - cuda-opencl=12.3.101=0 50 | - cuda-opencl-dev=12.3.101=0 51 | - cuda-profiler-api=12.3.101=0 52 | - cuda-runtime=12.1.0=0 53 | - cuda-sanitizer-api=12.3.101=0 54 | - cuda-toolkit=11.7.1=0 55 | - cuda-tools=12.1.0=0 56 | - cuda-visual-tools=12.1.0=0 57 | - cudatoolkit=11.8.0=h6a678d5_0 58 | - eigen=3.4.0=h4bd325d_0 59 | - eigenpy=2.7.5=py310hf3e5c9c_0 60 | - filelock=3.13.1=py310h06a4308_0 61 | - freetype=2.12.1=h4a9f257_0 62 | - fsspec=2023.10.0=py310h06a4308_0 63 | - gds-tools=1.8.1.2=0 64 | - gmp=6.2.1=h295c915_3 65 | - gmpy2=2.1.2=py310heeb90bb_0 66 | - hpp-fcl=2.0.1=py310hd26c143_1 67 | - icu=70.1=h27087fc_0 68 | - idna=3.4=py310h06a4308_0 69 | - jinja2=3.1.3=py310h06a4308_0 70 | - jpeg=9e=h5eee18b_1 71 | - lcms2=2.12=h3be6417_0 72 | - ld_impl_linux-64=2.38=h1181459_1 73 | - lerc=3.0=h295c915_0 74 | - libcublas=12.1.0.26=0 75 | - libcublas-dev=12.1.0.26=0 76 | - libcublas-static=12.1.0.26=0 77 | - libcufft=11.0.2.4=0 78 | - libcufft-dev=11.0.2.4=0 79 | - libcufft-static=11.0.2.4=0 80 | - libcufile=1.8.1.2=0 81 | - libcufile-dev=1.8.1.2=0 82 | - libcufile-static=1.8.1.2=0 83 | - libcurand=10.3.4.107=0 84 | - libcurand-dev=10.3.4.107=0 85 | - libcurand-static=10.3.4.107=0 86 | - libcusolver=11.4.4.55=0 87 | - libcusolver-dev=11.4.4.55=0 88 | - libcusolver-static=11.4.4.55=0 89 | - libcusparse=12.0.2.55=0 90 | - libcusparse-dev=12.0.2.55=0 91 | - libcusparse-static=12.0.2.55=0 92 | - libdeflate=1.17=h5eee18b_1 93 | - libffi=3.4.4=h6a678d5_0 94 | - libgcc-ng=13.2.0=h807b86a_5 95 | - libgfortran-ng=11.2.0=h00389a5_1 96 | - libgfortran5=11.2.0=h1234567_1 97 | - libgomp=13.2.0=h807b86a_5 98 | - libnpp=12.0.2.50=0 99 | - libnpp-dev=12.0.2.50=0 100 | - libnpp-static=12.0.2.50=0 101 | - libnvjitlink=12.1.105=0 102 | - libnvjitlink-dev=12.1.105=0 103 | - libnvjpeg=12.1.1.14=0 104 | - libnvjpeg-dev=12.1.1.14=0 105 | - libnvjpeg-static=12.1.1.14=0 106 | - libopenblas=0.3.21=h043d6bf_0 107 | - libpng=1.6.39=h5eee18b_0 108 | - libprotobuf=3.20.3=he621ea3_0 109 | - libstdcxx-ng=11.2.0=h1234567_1 110 | - libtiff=4.5.1=h6a678d5_0 111 | - libuuid=1.41.5=h5eee18b_0 112 | - libwebp-base=1.3.2=h5eee18b_0 113 | - libzlib=1.2.13=h4ab18f5_6 114 | - llvm-openmp=18.1.4=ha31de31_0 115 | - lz4-c=1.9.4=h6a678d5_0 116 | - markupsafe=2.1.3=py310h5eee18b_0 117 | - mpc=1.1.0=h10f8cd9_1 118 | - mpfr=4.0.2=hb69a4c5_1 119 | - mpmath=1.3.0=py310h06a4308_0 120 | - ncurses=6.4=h6a678d5_0 121 | - networkx=3.1=py310h06a4308_0 122 | - ninja=1.10.2=h06a4308_5 123 | - ninja-base=1.10.2=hd09550d_5 124 | - nsight-compute=2023.3.1.1=0 125 | - numpy=1.26.4=py310heeff2f4_0 126 | - numpy-base=1.26.4=py310h8a23956_0 127 | - octomap=1.9.7=h4bd325d_0 128 | - openjpeg=2.4.0=h3ad879b_0 129 | - openssl=3.3.1=h4ab18f5_0 130 | - pinocchio=2.6.7=py310hf3e5c9c_2 131 | - pip=23.3.1=py310h06a4308_0 132 | - pycparser=2.21=pyhd3eb1b0_0 133 | - pysocks=1.7.1=py310h06a4308_0 134 | - python=3.10.13=h955ad1f_0 135 | - python_abi=3.10=2_cp310 136 | - pytorch=2.2.0=cpu_py310hab5cca8_0 137 | - pytorch-cuda=12.1=ha16c6d3_5 138 | - pytorch-mutex=1.0=cuda 139 | - qhull=2020.2=h4bd325d_2 140 | - readline=8.2=h5eee18b_0 141 | - requests=2.31.0=py310h06a4308_1 142 | - setuptools=68.2.2=py310h06a4308_0 143 | - sqlite=3.41.2=h5eee18b_0 144 | - sympy=1.12=py310h06a4308_0 145 | - tinyxml=2.6.2=h4bd325d_2 146 | - tk=8.6.12=h1ccaba5_0 147 | - torchaudio=2.2.0=py310_cu121 148 | - torchvision=0.15.2=cuda118py310h196c800_0 149 | - typing_extensions=4.9.0=py310h06a4308_1 150 | - urdfdom=3.0.2=h27087fc_2 151 | - urdfdom_headers=1.0.6=h924138e_2 152 | - urllib3=2.1.0=py310h06a4308_1 153 | - wheel=0.41.2=py310h06a4308_0 154 | - xz=5.4.5=h5eee18b_0 155 | - zlib=1.2.13=h4ab18f5_6 156 | - zstd=1.5.5=hc292b87_0 157 | - pip: 158 | - absl-py==2.1.0 159 | - addict==2.4.0 160 | - affine==2.4.0 161 | - antlr4-python3-runtime==4.9.3 162 | - asttokens==2.4.1 163 | - attrs==23.2.0 164 | - beautifulsoup4==4.12.3 165 | - blinker==1.8.2 166 | - cachetools==5.3.3 167 | - catkin-pkg==1.0.0 168 | - click-plugins==1.1.1 169 | - cligj==0.7.2 170 | - cloudpickle==3.0.0 171 | - cmake==3.28.3 172 | - comm==0.2.2 173 | - configargparse==1.7 174 | - contourpy==1.2.0 175 | - cython==3.0.10 176 | - dash==2.17.1 177 | - dash-core-components==2.0.0 178 | - dash-html-components==2.0.0 179 | - dash-table==5.0.0 180 | - decorator==5.1.1 181 | - docker-pycreds==0.4.0 182 | - docutils==0.21.2 183 | - executing==2.0.1 184 | - farama-notifications==0.0.4 185 | - fastjsonschema==2.20.0 186 | - flask==3.0.3 187 | - fonttools==4.49.0 188 | - gdown==5.2.0 189 | - gitdb==4.0.11 190 | - gitpython==3.1.43 191 | - google-auth==2.28.1 192 | - google-auth-oauthlib==0.4.6 193 | - grpcio==1.62.0 194 | - hydra-core==1.3.2 195 | - importlib-metadata==7.1.0 196 | - iniconfig==2.0.0 197 | - ipython==8.25.0 198 | - ipywidgets==8.1.3 199 | - itsdangerous==2.2.0 200 | - jax-jumpy==1.0.0 201 | - jedi==0.19.1 202 | - joblib==1.4.2 203 | - jsonschema==4.22.0 204 | - jsonschema-specifications==2023.12.1 205 | - jupyter-core==5.7.2 206 | - jupyterlab-widgets==3.0.11 207 | - lit==17.0.6 208 | - markdown==3.5.2 209 | - matplotlib-inline==0.1.7 210 | - mushroom-rl==1.7.1 211 | - nbformat==5.7.0 212 | - numpy-ml==0.1.2 213 | - omegaconf==2.3.0 214 | - open3d==0.17.0 215 | - opencv-python==4.10.0.82 216 | - pandas==2.2.1 217 | - parso==0.8.4 218 | - pexpect==4.9.0 219 | - pillow==10.3.0 220 | - platformdirs==4.2.2 221 | - pluggy==1.5.0 222 | - prompt-toolkit==3.0.47 223 | - protobuf==3.20.3 224 | - ptyprocess==0.7.0 225 | - pure-eval==0.2.2 226 | - pyasn1==0.5.1 227 | - pyasn1-modules==0.3.0 228 | - pygame==2.5.2 229 | - pygments==2.18.0 230 | - pyquaternion==0.9.9 231 | - pytest==8.2.2 232 | - rasterio==1.3.10 233 | - referencing==0.35.1 234 | - retrying==1.3.4 235 | - rpds-py==0.18.1 236 | - rsa==4.9 237 | - scikit-learn==1.5.0 238 | - setproctitle==1.3.3 239 | - shapely==2.0.4 240 | - smmap==5.0.1 241 | - snuggs==1.4.7 242 | - soupsieve==2.5 243 | - stable-baselines3==2.0.0 244 | - stack-data==0.6.3 245 | - tenacity==8.3.0 246 | - tensorboard==2.11.0 247 | - tensorboard-data-server==0.6.1 248 | - tensorboard-plugin-wit==1.8.1 249 | - threadpoolctl==3.5.0 250 | - tomli==2.0.1 251 | - tqdm==4.66.4 252 | - traitlets==5.14.3 253 | - triton==2.0.0 254 | - typing-extensions==4.10.0 255 | - tzdata==2024.1 256 | - wandb==0.17.1 257 | - wcwidth==0.2.13 258 | - werkzeug==3.0.1 259 | - widgetsnbextension==4.0.11 260 | - zipp==3.19.2 261 | -------------------------------------------------------------------------------- /env_2022.2.0.yaml: -------------------------------------------------------------------------------- 1 | name: actpermoma-2022.2.0 2 | channels: 3 | - pytorch 4 | - nvidia 5 | - defaults 6 | dependencies: 7 | - _libgcc_mutex=0.1=main 8 | - _openmp_mutex=5.1=1_gnu 9 | - blas=1.0=mkl 10 | - brotlipy=0.7.0=py37h27cfd23_1003 11 | - ca-certificates=2024.3.11=h06a4308_0 12 | - certifi=2022.12.7=py37h06a4308_0 13 | - cffi=1.15.1=py37h5eee18b_3 14 | - charset-normalizer=2.0.4=pyhd3eb1b0_0 15 | - cryptography=39.0.1=py37h9ce1e76_0 16 | - cuda-cccl=12.4.127=0 17 | - cuda-command-line-tools=11.7.1=0 18 | - cuda-compiler=12.4.1=0 19 | - cuda-cudart=11.7.99=0 20 | - cuda-cudart-dev=11.7.99=0 21 | - cuda-cuobjdump=12.4.127=0 22 | - cuda-cupti=11.7.101=0 23 | - cuda-cuxxfilt=12.4.127=0 24 | - cuda-documentation=12.4.127=0 25 | - cuda-driver-dev=12.4.127=0 26 | - cuda-gdb=12.4.127=0 27 | - cuda-libraries=11.7.1=0 28 | - cuda-libraries-dev=11.7.1=0 29 | - cuda-memcheck=11.8.86=0 30 | - cuda-nsight=12.4.127=0 31 | - cuda-nsight-compute=12.4.1=0 32 | - cuda-nvcc=12.4.131=0 33 | - cuda-nvdisasm=12.4.127=0 34 | - cuda-nvml-dev=12.4.127=0 35 | - cuda-nvprof=12.4.127=0 36 | - cuda-nvprune=12.4.127=0 37 | - cuda-nvrtc=11.7.99=0 38 | - cuda-nvrtc-dev=11.7.99=0 39 | - cuda-nvtx=11.7.91=0 40 | - cuda-nvvp=12.4.127=0 41 | - cuda-runtime=11.7.1=0 42 | - cuda-sanitizer-api=12.4.127=0 43 | - cuda-toolkit=11.7.1=0 44 | - cuda-tools=11.7.1=0 45 | - cuda-version=12.4=hbda6634_3 46 | - cuda-visual-tools=11.7.1=0 47 | - flit-core=3.6.0=pyhd3eb1b0_0 48 | - freetype=2.12.1=h4a9f257_0 49 | - gds-tools=1.9.1.3=0 50 | - giflib=5.2.1=h5eee18b_3 51 | - idna=3.4=py37h06a4308_0 52 | - intel-openmp=2021.4.0=h06a4308_3561 53 | - jpeg=9e=h5eee18b_1 54 | - lcms2=2.12=h3be6417_0 55 | - ld_impl_linux-64=2.38=h1181459_1 56 | - lerc=3.0=h295c915_0 57 | - libcublas=11.10.3.66=0 58 | - libcublas-dev=11.10.3.66=0 59 | - libcufft=10.7.2.124=h4fbf590_0 60 | - libcufft-dev=10.7.2.124=h98a8f43_0 61 | - libcufile=1.9.1.3=0 62 | - libcufile-dev=1.9.1.3=0 63 | - libcurand=10.3.5.147=h99ab3db_1 64 | - libcurand-dev=10.3.5.147=h99ab3db_1 65 | - libcusolver=11.4.0.1=0 66 | - libcusolver-dev=11.4.0.1=0 67 | - libcusparse=11.7.4.91=0 68 | - libcusparse-dev=11.7.4.91=0 69 | - libdeflate=1.17=h5eee18b_1 70 | - libffi=3.4.4=h6a678d5_1 71 | - libgcc-ng=11.2.0=h1234567_1 72 | - libgomp=11.2.0=h1234567_1 73 | - libnpp=11.7.4.75=0 74 | - libnpp-dev=11.7.4.75=0 75 | - libnvjpeg=11.8.0.2=0 76 | - libnvjpeg-dev=11.8.0.2=0 77 | - libpng=1.6.39=h5eee18b_0 78 | - libstdcxx-ng=11.2.0=h1234567_1 79 | - libtiff=4.5.1=h6a678d5_0 80 | - libwebp=1.2.4=h11a3e52_1 81 | - libwebp-base=1.2.4=h5eee18b_1 82 | - lz4-c=1.9.4=h6a678d5_1 83 | - mkl=2021.4.0=h06a4308_640 84 | - mkl-service=2.4.0=py37h7f8727e_0 85 | - mkl_fft=1.3.1=py37hd3c417c_0 86 | - mkl_random=1.2.2=py37h51133e4_0 87 | - ncurses=6.4=h6a678d5_0 88 | - nsight-compute=2024.1.1.4=0 89 | - numpy=1.21.5=py37h6c91a56_3 90 | - numpy-base=1.21.5=py37ha15fc14_3 91 | - openssl=1.1.1w=h7f8727e_0 92 | - pillow=9.4.0=py37h6a678d5_0 93 | - pip=22.3.1=py37h06a4308_0 94 | - pycparser=2.21=pyhd3eb1b0_0 95 | - pyopenssl=23.0.0=py37h06a4308_0 96 | - pysocks=1.7.1=py37_1 97 | - python=3.7.16=h7a1cb2a_0 98 | - pytorch=1.13.1=py3.7_cuda11.7_cudnn8.5.0_0 99 | - pytorch-cuda=11.7=h778d358_5 100 | - pytorch-mutex=1.0=cuda 101 | - readline=8.2=h5eee18b_0 102 | - requests=2.28.1=py37h06a4308_0 103 | - setuptools=65.6.3=py37h06a4308_0 104 | - six=1.16.0=pyhd3eb1b0_1 105 | - sqlite=3.45.3=h5eee18b_0 106 | - tk=8.6.12=h1ccaba5_0 107 | - torchaudio=0.13.1=py37_cu117 108 | - torchvision=0.13.1=cpu_py37h164cc8f_0 109 | - typing_extensions=4.4.0=py37h06a4308_0 110 | - urllib3=1.26.14=py37h06a4308_0 111 | - wheel=0.38.4=py37h06a4308_0 112 | - xz=5.4.6=h5eee18b_1 113 | - zlib=1.2.13=h5eee18b_1 114 | - zstd=1.5.5=hc292b87_2 115 | - pip: 116 | - absl-py==2.1.0 117 | - addict==2.4.0 118 | - affine==2.4.0 119 | - antlr4-python3-runtime==4.9.3 120 | - appdirs==1.4.4 121 | - backcall==0.2.0 122 | - cachetools==5.3.3 123 | - catkin-pkg==1.0.0 124 | - click==8.1.7 125 | - click-plugins==1.1.1 126 | - cligj==0.7.2 127 | - cloudpickle==2.2.1 128 | - cmeel==0.46.0 129 | - cmeel-assimp==5.2.5 130 | - cmeel-boost==1.81.0 131 | - cmeel-console-bridge==1.0.2.1 132 | - cmeel-octomap==1.9.8.1 133 | - cmeel-tinyxml==2.6.2.1 134 | - cmeel-urdfdom==3.1.0.2 135 | - comm==0.1.4 136 | - configargparse==1.7 137 | - cycler==0.11.0 138 | - cython==3.0.10 139 | - dash==2.15.0 140 | - dash-core-components==2.0.0 141 | - dash-html-components==2.0.0 142 | - dash-table==5.0.0 143 | - decorator==5.1.1 144 | - docker-pycreds==0.4.0 145 | - docutils==0.20.1 146 | - eigenpy==3.0.0 147 | - exceptiongroup==1.2.1 148 | - fastjsonschema==2.19.1 149 | - flask==2.2.5 150 | - fonttools==4.38.0 151 | - gitdb==4.0.11 152 | - gitpython==3.1.43 153 | - google-auth==2.29.0 154 | - google-auth-oauthlib==0.4.6 155 | - grpcio==1.62.2 156 | - gym==0.21.0 157 | - hpp-fcl==2.3.2 158 | - hydra-core==1.3.2 159 | - importlib-metadata==4.13.0 160 | - importlib-resources==5.12.0 161 | - iniconfig==2.0.0 162 | - ipython==7.34.0 163 | - ipywidgets==8.1.2 164 | - itsdangerous==2.1.2 165 | - jedi==0.19.1 166 | - jinja2==3.1.4 167 | - joblib==1.3.2 168 | - jupyter-core==4.12.0 169 | - jupyterlab-widgets==3.0.10 170 | - kiwisolver==1.4.5 171 | - markdown==3.4.4 172 | - markupsafe==2.1.5 173 | - matplotlib==3.5.3 174 | - matplotlib-inline==0.1.6 175 | - mushroom-rl==1.7.1 176 | - nbformat==5.7.0 177 | - numpy-ml==0.1.2 178 | - oauthlib==3.2.2 179 | - omegaconf==2.3.0 180 | - open3d==0.17.0 181 | - opencv-python==4.9.0.80 182 | - packaging==24.0 183 | - pandas==1.3.5 184 | - parso==0.8.4 185 | - pexpect==4.9.0 186 | - pickleshare==0.7.5 187 | - pin==2.6.18 188 | - plotly==5.18.0 189 | - pluggy==1.2.0 190 | - promise==2.3 191 | - prompt-toolkit==3.0.43 192 | - protobuf==3.20.3 193 | - ptyprocess==0.7.0 194 | - pyasn1==0.5.1 195 | - pyasn1-modules==0.3.0 196 | - pygame==2.5.2 197 | - pygments==2.17.2 198 | - pyparsing==3.1.2 199 | - pyquaternion==0.9.9 200 | - pytest==7.4.4 201 | - python-dateutil==2.9.0.post0 202 | - pytz==2024.1 203 | - rasterio==1.2.10 204 | - requests-oauthlib==2.0.0 205 | - retrying==1.3.4 206 | - rsa==4.9 207 | - scikit-learn==1.0.2 208 | - setproctitle==1.3.3 209 | - shapely==2.0.4 210 | - shortuuid==1.0.13 211 | - smmap==5.0.1 212 | - snuggs==1.4.7 213 | - stable-baselines3==1.6.2 214 | - tenacity==8.2.3 215 | - tensorboard==2.11.0 216 | - tensorboard-data-server==0.6.1 217 | - tensorboard-plugin-wit==1.8.1 218 | - threadpoolctl==3.1.0 219 | - tomli==2.0.1 220 | - tqdm==4.66.4 221 | - traitlets==5.9.0 222 | - typing-extensions==4.7.1 223 | - wandb==0.13.1 224 | - wcwidth==0.2.13 225 | - werkzeug==2.2.3 226 | - widgetsnbextension==4.0.10 227 | - zipp==3.15.0 228 | prefix: /home/sophie/miniconda3/envs/actpermoma-2022.2.0 229 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | """Installation scripts for the 'actpermoma' python package.""" 2 | 3 | from setuptools import setup, find_packages 4 | 5 | INSTALL_REQUIRES = [ 6 | "protobuf", 7 | "omegaconf", 8 | "hydra-core", 9 | ] 10 | 11 | # Installation operation 12 | setup( 13 | name="actpermoma", 14 | author="Sophie Lueth & Snehal Jauhri", 15 | version="1.0.0", 16 | description="Active-Perceptive Motion Generation for Mobile Manipulation in NVIDIA Isaac Sim. Adapted from omniisaacgymenvs (https://github.com/NVIDIA-Omniverse/OmniIsaacGymEnvs)", 17 | keywords=["robotics", "active perception"], 18 | include_package_data=True, 19 | install_requires=INSTALL_REQUIRES, 20 | packages=find_packages("."), 21 | classifiers=["Natural Language :: English", "Programming Language :: Python :: 3.8, 3.10"], 22 | zip_safe=False, 23 | ) --------------------------------------------------------------------------------