├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── goal.json ├── image.def ├── log_manager ├── README.md ├── after_job.sh ├── download_logs.sh ├── funcs.sh ├── make_plots.sh ├── make_video.sh ├── plot_scripts │ ├── finger_position.py │ └── plot.py ├── replay_scripts │ ├── compute_reward.py │ ├── generate_replay_video.sh │ └── replay.py ├── roboch.json ├── run.sh ├── sshkey ├── submit_job.sh ├── submit_jobs_in_sequence.sh └── user.txt ├── package.xml ├── python ├── cic │ ├── __init__.py │ ├── bayesian_opt │ │ ├── README.md │ │ ├── bayes_opt_main.py │ │ ├── const.py │ │ ├── content │ │ │ ├── diff.txt │ │ │ ├── iter_idx.txt │ │ │ ├── model.txt │ │ │ ├── params.pkl │ │ │ └── pos.pkl │ │ ├── eval_code.py │ │ ├── eval_code_randomized.py │ │ └── utils │ │ │ ├── adapt_image.def │ │ │ ├── automated_submission_real.sh │ │ │ ├── env.yml │ │ │ ├── evaluate_trajectory.py │ │ │ ├── functionality.py │ │ │ ├── normalization_tools.py │ │ │ ├── push_github.sh │ │ │ ├── roboch.json │ │ │ ├── run_automate.py │ │ │ ├── run_episode_bo.py │ │ │ ├── run_local_episode_bo.py │ │ │ ├── sampling_functions.py │ │ │ └── upload_json.sh │ ├── control_main_class.py │ ├── grasping_class.py │ ├── math_tools.py │ ├── motion_primitives.py │ ├── object_class.py │ ├── parameters.py │ ├── parameters_new_grasp.py │ ├── rotation_primitives.py │ ├── states │ │ ├── __init__.py │ │ ├── base_states.py │ │ ├── custom_state_machines.py │ │ ├── sequences.py │ │ └── state_machine.py │ ├── trifinger_mod.urdf │ └── utils.py ├── combined_code │ ├── __init__.py │ └── mix_and_match.py ├── cpc │ ├── __init__.py │ ├── parameters.py │ ├── state_machine.py │ └── states │ │ ├── __init__.py │ │ ├── base_states.py │ │ ├── sequence_state.py │ │ └── utils.py ├── env │ ├── __init__.py │ ├── cube_env.py │ ├── initializers.py │ ├── make_env.py │ ├── pinocchio_utils.py │ ├── reward_fns.py │ ├── termination_fns.py │ ├── viz.py │ └── wrappers.py ├── mp │ ├── __init__.py │ ├── action_sequences.py │ ├── align_rotation.py │ ├── base_policies │ │ ├── __init__.py │ │ ├── fc.py │ │ └── mpfc.py │ ├── const.py │ ├── grasping │ │ ├── __init__.py │ │ ├── collision_config.py │ │ ├── force_closure.py │ │ ├── grasp_functions.py │ │ ├── grasp_motions.py │ │ ├── grasp_sampling.py │ │ ├── ik.py │ │ └── wholebody_planning.py │ ├── set_hyperparams.py │ ├── state_machines.py │ ├── states │ │ ├── __init__.py │ │ ├── base_states.py │ │ ├── sequences.py │ │ └── state_machine.py │ └── utils.py ├── pybullet_planning │ ├── README.md │ ├── __init__.py │ ├── __version__.py │ ├── interfaces │ │ ├── __init__.py │ │ ├── control │ │ │ ├── __init__.py │ │ │ └── control.py │ │ ├── debug_utils │ │ │ ├── __init__.py │ │ │ └── debug_utils.py │ │ ├── env_manager │ │ │ ├── __init__.py │ │ │ ├── pose_transformation.py │ │ │ ├── savers.py │ │ │ ├── shape_creation.py │ │ │ ├── simulation.py │ │ │ └── user_io.py │ │ ├── geometry │ │ │ ├── __init__.py │ │ │ ├── bounding_box.py │ │ │ ├── camera.py │ │ │ ├── mesh.py │ │ │ ├── pointcloud.py │ │ │ └── polygon.py │ │ ├── kinematics │ │ │ ├── __init__.py │ │ │ ├── ik_interface.py │ │ │ ├── ik_utils.py │ │ │ └── reachability.py │ │ ├── planner_interface │ │ │ ├── SE2_pose_motion_planning.py │ │ │ ├── __init__.py │ │ │ ├── cartesian_motion_planning.py │ │ │ ├── dag_search.py │ │ │ ├── joint_motion_planning.py │ │ │ ├── ladder_graph.py │ │ │ ├── nonholonomic_motion_planning.py │ │ │ └── utils.py │ │ ├── robots │ │ │ ├── __init__.py │ │ │ ├── body.py │ │ │ ├── collision.py │ │ │ ├── dynamics.py │ │ │ ├── joint.py │ │ │ └── link.py │ │ └── task_modeling │ │ │ ├── __init__.py │ │ │ ├── constraint.py │ │ │ ├── grasp.py │ │ │ ├── path_interpolation.py │ │ │ └── placement.py │ ├── motion_planners │ │ ├── __init__.py │ │ ├── discrete.py │ │ ├── graph.py │ │ ├── lazy_prm.py │ │ ├── multi_rrt.py │ │ ├── prm.py │ │ ├── rrt.py │ │ ├── rrt_connect.py │ │ ├── rrt_star.py │ │ ├── smoothing.py │ │ ├── star_roadmap.py │ │ ├── utils.py │ │ └── wholebody_rrt_connect.py │ ├── primitives │ │ ├── __init__.py │ │ ├── grasp_gen.py │ │ └── trajectory.py │ └── utils │ │ ├── __init__.py │ │ ├── _file_path_archived.py │ │ ├── debug_utils.py │ │ ├── file_io.py │ │ ├── iter_utils.py │ │ ├── numeric_sample.py │ │ ├── shared_const.py │ │ └── transformations.py └── residual_learning │ ├── __init__.py │ ├── domain_randomization.py │ ├── eval.py │ ├── get_ob_norm.py │ ├── make_training_env.py │ ├── models │ ├── cic_lvl3 │ │ └── logs │ │ │ ├── base.gin │ │ │ ├── ckpts │ │ │ └── 000880000.pt │ │ │ ├── config.gin │ │ │ └── singularity_config.yaml │ ├── cic_lvl4 │ │ └── logs │ │ │ ├── base.gin │ │ │ ├── ckpts │ │ │ └── 000780000.pt │ │ │ ├── config.gin │ │ │ └── singularity_config.yaml │ ├── cpc_lvl3 │ │ └── logs │ │ │ ├── base.gin │ │ │ ├── ckpts │ │ │ └── 001020000.pt │ │ │ ├── config.gin │ │ │ └── singularity_config.yaml │ ├── cpc_lvl4 │ │ └── logs │ │ │ ├── base.gin │ │ │ ├── ckpts │ │ │ └── 000600000.pt │ │ │ ├── config.gin │ │ │ └── singularity_config.yaml │ ├── mp_lvl3 │ │ └── logs │ │ │ ├── base.gin │ │ │ ├── ckpts │ │ │ └── 001100000.pt │ │ │ ├── config.gin │ │ │ └── singularity_config.yaml │ └── mp_lvl4 │ │ └── logs │ │ ├── base.gin │ │ ├── ckpts │ │ └── 000920000.pt │ │ ├── config.gin │ │ └── singularity_config.yaml │ ├── modules.py │ ├── networks.py │ ├── obs_norm.pt │ ├── obs_norm_rand.pt │ ├── residual_sac.py │ ├── residual_state.py │ ├── residual_state_machines.py │ ├── residual_wrappers.py │ ├── state_machines.py │ ├── test_domain_rand.py │ ├── train.py │ └── viz.py ├── run ├── run_locally.sh ├── run_locally_bo.sh ├── run_real_bo ├── run_real_normal ├── scripts ├── run_episode.py └── run_local_episode.py ├── setup.py ├── tools └── plot │ ├── exp_align_obj.py │ ├── exp_approach_goal.py │ ├── plot.py │ ├── readme.md │ └── utils.py └── training_scripts ├── build_ws.py ├── configs ├── cic_cg.yaml ├── cpc_tg_lvl3.yml ├── cpc_tg_lvl4.yml ├── mp_pg.yaml └── residual_sac.gin ├── evaluate.sh ├── run_script_local.sh └── visualize.sh /.gitignore: -------------------------------------------------------------------------------- 1 | # RRC files 2 | *.sif 3 | 4 | # Byte-compiled / optimized / DLL files 5 | __pycache__/ 6 | *.py[cod] 7 | *$py.class 8 | 9 | # C extensions 10 | *.so 11 | 12 | # Distribution / packaging 13 | .Python 14 | build/ 15 | develop-eggs/ 16 | dist/ 17 | downloads/ 18 | eggs/ 19 | .eggs/ 20 | lib/ 21 | lib64/ 22 | parts/ 23 | sdist/ 24 | var/ 25 | wheels/ 26 | pip-wheel-metadata/ 27 | share/python-wheels/ 28 | *.egg-info/ 29 | .installed.cfg 30 | *.egg 31 | MANIFEST 32 | 33 | # PyInstaller 34 | # Usually these files are written by a python script from a template 35 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 36 | *.manifest 37 | *.spec 38 | 39 | # Installer logs 40 | pip-log.txt 41 | pip-delete-this-directory.txt 42 | 43 | # Unit test / coverage reports 44 | htmlcov/ 45 | .tox/ 46 | .nox/ 47 | .coverage 48 | .coverage.* 49 | .cache 50 | nosetests.xml 51 | coverage.xml 52 | *.cover 53 | *.py,cover 54 | .hypothesis/ 55 | .pytest_cache/ 56 | 57 | # Translations 58 | *.mo 59 | *.pot 60 | 61 | # Django stuff: 62 | *.log 63 | local_settings.py 64 | db.sqlite3 65 | db.sqlite3-journal 66 | 67 | # Flask stuff: 68 | instance/ 69 | .webassets-cache 70 | 71 | # Scrapy stuff: 72 | .scrapy 73 | 74 | # Sphinx documentation 75 | docs/_build/ 76 | 77 | # PyBuilder 78 | target/ 79 | 80 | # Jupyter Notebook 81 | .ipynb_checkpoints 82 | 83 | # IPython 84 | profile_default/ 85 | ipython_config.py 86 | 87 | # pyenv 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 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 98 | __pypackages__/ 99 | 100 | # Celery stuff 101 | celerybeat-schedule 102 | celerybeat.pid 103 | 104 | # SageMath parsed files 105 | *.sage.py 106 | 107 | # Environments 108 | .env 109 | .venv 110 | # env/ 111 | venv/ 112 | ENV/ 113 | env.bak/ 114 | venv.bak/ 115 | 116 | # Spyder project settings 117 | .spyderproject 118 | .spyproject 119 | 120 | # Rope project settings 121 | .ropeproject 122 | 123 | # mkdocs documentation 124 | /site 125 | 126 | # mypy 127 | .mypy_cache/ 128 | .dmypy.json 129 | dmypy.json 130 | 131 | # Pyre type checker 132 | .pyre/ 133 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(rrc) 3 | 4 | find_package(catkin REQUIRED) 5 | catkin_python_setup() 6 | catkin_package() 7 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2020, Max Planck Gesellschaft. 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Benchmarking Structured Policies and Policy Optimization for Real-World Dexterous Object Manipulation 2 | ============================================================= 3 | 4 | This repository contains the code for the publication "Benchmarking Structured Policies and Policy 5 | Optimization for Real-World Dexterous Object Manipulation". 6 | 7 | **[arXiv](https://arxiv.org/abs/2105.02087) | [project website](https://sites.google.com/view/benchmark-rrc)** 8 | 9 | For an updated version of the code which is compatible with the 2021 RRC competion, see the [ros2](https://github.com/cbschaff/benchmark-rrc/tree/ros2) branch. 10 | 11 | ## Quickstart 12 | 13 | ### Running the code in simulation 14 | 15 | **Make sure to download the Phase 2 image** 16 | 17 | To run the code locally, first install [Singularity](https://sylabs.io/guides/3.5/user-guide/quick_start.html) 18 | and download [singularity image for Phase 2](https://people.tuebingen.mpg.de/felixwidmaier/realrobotchallenge/robot_phase/singularity.html#singularity-download-image) 19 | from the Real Robot Challenge. A couple extra dependencies are required to run our code. To create the required singularity image, run: 20 | ```singularity build --fakeroot image.sif image.def``` 21 | 22 | Use the `run_locally.sh` script to build the catkin workspace and run commands 23 | inside the singularity image. 24 | For example, to run the Motion Planning with Planned Grasp (MP-PG) on a random goal of difficulty 4, use the following 25 | command: 26 | ```bash 27 | ./run_locally.sh /path/to/singularity/image.sif rosrun rrc run_local_episode.py 4 mp-pg 28 | ``` 29 | 30 | Use ```scripts/run_local_episode.py``` to visualize all of our implemented approaches. You can run our code with and without residual models and BO optimized parameters. See ```scripts/run_local_episode.py``` for the arguments. 31 | 32 | 33 | ### Running the code on the robot cluster 34 | 35 | Similarly to simulation, ```scripts/run_episode.py``` can be used to run our methods on the real platform. Edit ```run``` to set the correct arguments. 36 | 37 | For detailed instructions on how to run this code on the robot cluster, see [this](https://people.tuebingen.mpg.de/felixwidmaier/realrobotchallenge/robot_phase/submission_system.html) page. 38 | 39 | This repository contains code for automatically submitting jobs and analyzing logs in the [log_manager](https://github.com/ripl-ttic/rrc_phase_3/tree/cleanup/log_manager) directory. 40 | 41 | 42 | ## Optimizing hyperparameters using BO 43 | 44 | The functionality to run BO for optimizing the hyperparameters is contained inside `python/cic/bayesian_opt`. 45 | In this location, also a README file is provided which details how the experiments can be run. 46 | 47 | 48 | ## Improving controllers with Residual Policy Learning 49 | 50 | We use a residual version of Soft Actor Critic to train residual controllers using [this Deep RL library](https://github.com/cbschaff/dl). 51 | To train a controller for the MP-PG controller use the following command: 52 | 53 | ```bash 54 | ./training_scripts/run_script_local.sh /path/to/singularity/image ./training_scripts/configs/mp_pg.yaml 55 | ``` 56 | 57 | Similar configs for the other controllers exist in the ```training_scripts/configs``` folder. 58 | 59 | To record or evaluate the model call the ```visualize.sh``` and ```evaluate.sh``` scripts respectively: 60 | 61 | 62 | ```bash 63 | ./training_scripts/visualize.sh /path/to/singularity/image /path/to/log/directory -n num_episodes -t ckpt 64 | 65 | ./training_scripts/evaluate.sh /path/to/singularity/image /path/to/log/directory -n num_episodes -t ckpt 66 | ``` 67 | 68 | ## Contributions 69 | 70 | The code is the joint work of [Niklas Funk](https://github.com/nifunk), [Charles Schaff](https://github.com/cbschaff), [Rishabh Madan](https://github.com/madan96), and [Takuma Yoneda](https://github.com/takuma-ynd). 71 | 72 | The repository is structured as a catkin package and builds on the 73 | [example package](https://github.com/rr-learning/rrc_example_package) provided by the Real Robot Challenge, 74 | and [this planning library](https://github.com/yijiangh/pybullet_planning). 75 | 76 | The paper and this repository combines the code and algorithms from three teams that participated in the Real Robot Challenge: 77 | - TTI-Chicago: [Motion Planning with Planned Grasps](http://arxiv.org/abs/2101.02842) 78 | - University of Washington: [Cartesian Position Control with a Triangulated Grasp](https://openreview.net/pdf?id=9tYX-lukeq) 79 | - TU Darmstadt: [Cartesian Impedance Control with Centered Grasps](https://openreview.net/pdf?id=JWUqwie0--W) 80 | -------------------------------------------------------------------------------- /goal.json: -------------------------------------------------------------------------------- 1 | { 2 | "difficulty": 4, 3 | "_goal": { 4 | "position": [0, 0, 0.05], 5 | "orientation": [0, 0, 0, 1] 6 | } 7 | } 8 | -------------------------------------------------------------------------------- /image.def: -------------------------------------------------------------------------------- 1 | # Specify the name of the base image below 2 | Bootstrap: localimage 3 | From: ./realrobotchallenge_phase2.sif 4 | 5 | %setup 6 | # cp setup.py package.xml ${SINGULARITY_ROOTFS}/ 7 | # cp -r python ${SINGULARITY_ROOTFS}/ 8 | 9 | %files 10 | 11 | %post 12 | # Put commands to install additional dependencies here. 13 | # Make sure everything runs automatically without human input (e.g. add 14 | # `-y` to automatically say "yes" below). 15 | python3 -m pip install torch==1.4.0 torchvision==0.5.0 16 | python3 -m pip install git+https://github.com/cbschaff/dl.git@91d1085b1df6cd2184214b1bcfcc7b9ee909ef41 17 | apt-get update && apt-get install -y ffmpeg 18 | -------------------------------------------------------------------------------- /log_manager/README.md: -------------------------------------------------------------------------------- 1 | # Submitting and visualizing jobs run on the TriFinger robot cluster 2 | 3 | This directory provides code for submitting jobs and analyzing logs by generating plots and videos. 4 | 5 | ### Account Verification 6 | 7 | In order to use this code, you must first add your account information in two files placed in this directory: 8 | 9 | 1) user.txt: This should contain only the username and password (each on their own line) of your account 10 | 11 | 2) sshkey: This should contain the ssh private key used to log into the submission system 12 | 13 | **Account information is provided by the competition organizers.** 14 | 15 | 16 | ### Submitting Jobs 17 | 18 | To submit a job, edit the `roboch.json` configuration file to contain the proper information (see [this]( https://people.tuebingen.mpg.de/felixwidmaier/realrobotchallenge/robot_phase/submission_system.html#configuration-file-roboch-json) 19 | for details). 20 | 21 | Then simply calling `./submit_job.sh` will upload the `roboch.json` file and start a job on the cluster. 22 | To run multiple, identical jobs in sequence, run `./submit_jobs_in_sequence.sh n`, where `n` is the number of jobs. 23 | 24 | ### Downloading and Analyzing Log Data 25 | 26 | This code analyzes log data in the following ways: 27 | - computes the accumulated reward of the episode 28 | - generates a plot of desired vs actual joint positions of each finger of the TriFinger robot 29 | - generates a video showing synchronized images from the 3 cameras observing the robot 30 | - generates a video in which robot and object poses are rendered in pybullet and shown alongside camera images from the above video 31 | 32 | Generating the videos requires ffmpeg to be installed with the libvpx codec, although another codec could be used in its place. 33 | 34 | To generate the above files for all submitted jobs, run the following command: 35 | ```bash 36 | ./run.sh /path/to/log/directory /path/to/singularity/image.sif 37 | ``` 38 | 39 | If called multiple times, the `run.sh` script skips jobs that have already been processed. 40 | -------------------------------------------------------------------------------- /log_manager/after_job.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | if (( $# != 4 )) 3 | then 4 | echo "Invalid number of arguments." 5 | echo "Usage: $0 " 6 | exit 1 7 | fi 8 | logdir=$1 9 | image=$2 10 | jobid=$3 11 | status=$4 12 | dir=`dirname $0` 13 | 14 | if [ -d ${logdir}/${jobid} ]; then 15 | exit 16 | fi 17 | 18 | if [ ! ${status} == 'C' ]; then 19 | exit 20 | fi 21 | 22 | bash ${dir}/download_logs.sh ${jobid} ${logdir} 23 | singularity run --nv ${image} python3 ${dir}/replay_scripts/compute_reward.py ${logdir}/${jobid} 24 | bash ${dir}/make_plots.sh ${image} ${logdir}/${jobid} 25 | bash ${dir}/make_video.sh ${image} ${logdir}/${jobid} 26 | -------------------------------------------------------------------------------- /log_manager/download_logs.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Download all logs from the specified job 4 | 5 | 6 | # expects output directory (to save downloaded files) as argument 7 | if (( $# != 2 )) 8 | then 9 | echo "Invalid number of arguments." 10 | echo "Usage: $0 " 11 | exit 1 12 | fi 13 | 14 | job_id=$1 15 | output_directory="$2" 16 | dir=`dirname $0` 17 | 18 | if ! [ -d "${output_directory}" ] 19 | then 20 | echo "${output_directory} does not exist or is not a directory" 21 | exit 1 22 | fi 23 | 24 | # prompt for username and password (to avoid having user credentials in the 25 | # bash history) 26 | username=`cat ${dir}/user.txt | head -n 1` 27 | password=`cat ${dir}/user.txt | sed -n '2 p'` 28 | # there is no automatic new line after the password prompt 29 | echo 30 | 31 | 32 | # URL to the webserver at which the recorded data can be downloaded 33 | base_url=https://robots.real-robot-challenge.com/output/${username}/data 34 | 35 | 36 | # Check if the file/path given as argument exists in the "data" directory of 37 | # the user 38 | function curl_check_if_exists() 39 | { 40 | local filename=$1 41 | 42 | http_code=$(curl -sI --user ${username}:${password} -o /dev/null -w "%{http_code}" ${base_url}/${filename}) 43 | 44 | case "$http_code" in 45 | 200|301) return 0;; 46 | *) return 1;; 47 | esac 48 | } 49 | 50 | 51 | echo "Check ${job_id}" 52 | # create directory for this job 53 | job_dir="${output_directory}/${job_id}" 54 | mkdir "${job_dir}" 55 | if curl_check_if_exists ${job_id}/report.json 56 | then 57 | # create directory for this job 58 | job_dir="${output_directory}/${job_id}" 59 | mkdir "${job_dir}" 60 | mkdir "${job_dir}/user" 61 | 62 | echo "Download data to ${job_dir}" 63 | 64 | # Download data. Here only the report file is downloaded as example. Add 65 | # equivalent commands for other files as needed. 66 | for file in report.json info.json goal.json robot_data.dat camera_data.dat camera60.yml camera180.yml camera300.yml user_stderr.txt user_stdout.txt user/custom_data.db 67 | do 68 | curl --user ${username}:${password} -o "${job_dir}/${file}" ${base_url}/${job_id}/${file} 69 | done 70 | else 71 | echo "No data for ${job_id} found." 72 | exit 1 73 | fi 74 | -------------------------------------------------------------------------------- /log_manager/funcs.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | # prompt for username and password (to avoid having user credentials in the 3 | # bash history) 4 | username=`cat user.txt | head -n 1` 5 | password=`cat user.txt | sed -n '2 p'` 6 | 7 | # URL to the webserver at which the recorded data can be downloaded 8 | hostname=robots.real-robot-challenge.com 9 | data_url=https://${hostname}/output/${username}/data 10 | 11 | # Check if the file/path given as argument exists in the "data" directory of 12 | # the user 13 | function curl_check_if_exists() 14 | { 15 | local filename=$1 16 | 17 | http_code=$(curl -sI --user ${username}:${password} -o /dev/null -w "%{http_code}" ${data_url}/${filename}) 18 | 19 | case "$http_code" in 20 | 200|301) return 0;; 21 | *) return 1;; 22 | esac 23 | } 24 | 25 | function check_if_job_submittable() 26 | { 27 | status=$(ssh -T -i sshkey ${username}@${hostname} << /tmp/${file} 48 | if grep -q "true" "/tmp/${file}" 49 | then 50 | echo "WARNING: Backend failed!" 51 | return 1 52 | else 53 | return 0 54 | fi 55 | } 56 | 57 | function submit_job() 58 | { 59 | echo "Submit job" 60 | submit_result=$(ssh -T -i sshkey ${username}@${hostname} << " 6 | exit 1 7 | fi 8 | image=$1 9 | logdir=$2 10 | dir=`dirname $0` 11 | ${image} rosrun trifinger_object_tracking tricamera_log_converter.py ${logdir}/camera_data.dat ${logdir}/video60.avi -c camera60 12 | ${image} rosrun trifinger_object_tracking tricamera_log_converter.py ${logdir}/camera_data.dat ${logdir}/video180.avi -c camera180 13 | ${image} rosrun trifinger_object_tracking tricamera_log_converter.py ${logdir}/camera_data.dat ${logdir}/video300.avi -c camera300 14 | 15 | ffmpeg -i ${logdir}/video60.avi -i ${logdir}/video180.avi -filter_complex hstack -q:v 1 ${logdir}/video_temp.avi 16 | ffmpeg -i ${logdir}/video_temp.avi -i ${logdir}/video300.avi -filter_complex hstack -q:v 1 ${logdir}/video.avi 17 | ffmpeg -i ${logdir}/video.avi ${logdir}/video.webm 18 | 19 | rm ${logdir}/video60.avi 20 | rm ${logdir}/video180.avi 21 | rm ${logdir}/video300.avi 22 | rm ${logdir}/video_temp.avi 23 | rm ${logdir}/video.avi 24 | singularity run --nv ${image} python3 ${dir}/replay_scripts/replay.py ${logdir} ${logdir}/replay.avi 25 | ffmpeg -i ${logdir}/replay.avi ${logdir}/replay.webm 26 | rm ${logdir}/replay.avi 27 | -------------------------------------------------------------------------------- /log_manager/plot_scripts/finger_position.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | """Simply script to quickly plot data from a log file.""" 3 | 4 | import argparse 5 | import pandas 6 | import matplotlib 7 | matplotlib.use('Agg') 8 | import matplotlib.pyplot as plt 9 | import os 10 | 11 | 12 | def main(): 13 | parser = argparse.ArgumentParser(description=__doc__) 14 | parser.add_argument("filename", type=str) 15 | parser.add_argument("--base", type=str, default="0") 16 | args = parser.parse_args() 17 | 18 | data = pandas.read_csv( 19 | args.filename, delim_whitespace=True, header=0, low_memory=False 20 | ) 21 | 22 | # convert to int if indices are given instead of names 23 | base = int(args.base) if args.base.isdigit() else args.base 24 | columns = ['observation_position', 'desired_action_position'] 25 | outdir = os.path.dirname(args.filename) 26 | 27 | fig, axes = plt.subplots(ncols=1, nrows=3, sharex=True, figsize=(18, 18)) 28 | for i, ax in enumerate(axes): 29 | inds = range(3*i, 3*(i+1)) 30 | cols = [c + f'_{ind}' for c in columns for ind in inds] 31 | data.plot(x=base, y=cols, ax=ax) 32 | ax.set_title(f"Finger {i}") 33 | ax.set_ylabel("Radians") 34 | plt.subplots_adjust(wspace=0, hspace=0) 35 | plt.savefig(os.path.join(outdir, "plot_finger_positions.pdf")) 36 | 37 | 38 | if __name__ == "__main__": 39 | main() 40 | -------------------------------------------------------------------------------- /log_manager/plot_scripts/plot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | """Simply script to quickly plot data from a log file.""" 3 | 4 | import argparse 5 | import pandas 6 | import matplotlib 7 | matplotlib.use('Agg') 8 | import matplotlib.pyplot as plt 9 | 10 | 11 | def main(): 12 | parser = argparse.ArgumentParser(description=__doc__) 13 | parser.add_argument("filename", type=str) 14 | parser.add_argument("outfile", type=str) 15 | parser.add_argument("columns", type=str, nargs="+") 16 | parser.add_argument("--base", type=str, default="0") 17 | args = parser.parse_args() 18 | 19 | data = pandas.read_csv( 20 | args.filename, delim_whitespace=True, header=0, low_memory=False 21 | ) 22 | 23 | # convert to int if indices are given instead of names 24 | base = int(args.base) if args.base.isdigit() else args.base 25 | columns = [int(c) if c.isdigit() else c for c in args.columns] 26 | 27 | plt.figure(figsize=(12, 8)) 28 | data.plot(x=base, y=columns, ax=plt.gca()) 29 | plt.legend(loc='center left', bbox_to_anchor=(1.0, 0.5)) 30 | plt.subplots_adjust(right=0.8) 31 | plt.savefig(args.outfile) 32 | 33 | 34 | if __name__ == "__main__": 35 | main() 36 | -------------------------------------------------------------------------------- /log_manager/replay_scripts/compute_reward.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import os 4 | import argparse 5 | import robot_fingers 6 | import numpy as np 7 | from trifinger_simulation.tasks import move_cube 8 | import json 9 | 10 | 11 | def compute_reward(logdir): 12 | log = robot_fingers.TriFingerPlatformLog(os.path.join(logdir, "robot_data.dat"), 13 | os.path.join(logdir, "camera_data.dat")) 14 | with open(os.path.join(logdir, "goal.json"), 'r') as f: 15 | goal = json.load(f) 16 | difficulty = goal['difficulty'] 17 | goal_pose = move_cube.Pose(position=np.array(goal['goal']['position']), 18 | orientation=np.array(goal['goal']['orientation'])) 19 | 20 | reward = 0.0 21 | for t in range(log.get_first_timeindex(), log.get_last_timeindex() + 1): 22 | camera_observation = log.get_camera_observation(t) 23 | reward -= move_cube.evaluate_state( 24 | goal_pose, camera_observation.filtered_object_pose, difficulty 25 | 26 | ) 27 | return reward 28 | 29 | 30 | if __name__ == '__main__': 31 | parser = argparse.ArgumentParser() 32 | parser.add_argument("logdir", help="path to the log directory") 33 | args = parser.parse_args() 34 | reward = compute_reward(args.logdir) 35 | with open(os.path.join(args.logdir, 'reward.json'), 'w') as f: 36 | json.dump({'reward': reward}, f) 37 | -------------------------------------------------------------------------------- /log_manager/replay_scripts/generate_replay_video.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | image=$1 4 | logdir=$2 5 | mkdir -p ${logdir}/out/user 6 | 7 | echo "copying files..." 8 | cp ${logdir}/user/custom_data.db ${logdir}/out/user/ 9 | cp ${logdir}/*.dat ${logdir}/out/user/ 10 | 11 | pushd . 12 | script_dir=$(cd `dirname $0` && pwd) 13 | work_dir=${script_dir}/../.. 14 | popd 15 | 16 | ${work_dir}/run_to_replay.py --backend-image ${image} --repository ${work_dir} --output-dir ${logdir}/out 17 | mv ${logdir}/out/user/comparison.avi ${logdir}/comparison.avi 18 | # ffmpeg -i ${logdir}/out/user/comparison.avi ${logdir}/comparison.mp4 19 | rm -r ${logdir}/out 20 | -------------------------------------------------------------------------------- /log_manager/roboch.json: -------------------------------------------------------------------------------- 1 | { 2 | "repository": "git@github.com:your_git_repo", 3 | "branch": "main", 4 | "email": "your@email.com", 5 | "git_deploy_key": "git_deploy_key", 6 | "singularity_image": "singularity_image.sif" 7 | } 8 | -------------------------------------------------------------------------------- /log_manager/run.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | if (( $# != 2 )) 3 | then 4 | echo "Invalid number of arguments." 5 | echo "Usage: $0 " 6 | exit 1 7 | fi 8 | logdir=$1 9 | image=$2 10 | dir=`dirname $0` 11 | 12 | hostname=robots.real-robot-challenge.com 13 | username=`cat ${dir}/user.txt | head -n 1` 14 | 15 | jobs=$(ssh -T -i ${dir}/sshkey ${username}@${hostname} <<< history | tail -n +2 | tr -s " " | awk -F'[. ]' '{print $1, $7}') 16 | 17 | echo ${jobs} | xargs -n 2 echo 18 | echo ${jobs} | xargs -t -n 2 -P 10 bash ${dir}/after_job.sh ${logdir} ${image} 19 | -------------------------------------------------------------------------------- /log_manager/sshkey: -------------------------------------------------------------------------------- 1 | put sshkey here 2 | -------------------------------------------------------------------------------- /log_manager/submit_job.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | . ./funcs.sh 4 | wait_for_job_submission 5 | fetch_and_upload_roboch_file 6 | submit_job 7 | wait_for_job_start ${job_id} 8 | wait_for_job_finish ${job_id} 9 | restore_roboch_file 10 | -------------------------------------------------------------------------------- /log_manager/submit_jobs_in_sequence.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | num_jobs=$1 4 | for (( i=0; i<${num_jobs}; i++)) 5 | do 6 | echo "======== Processing job [${i}/${num_jobs}] ========" 7 | ./submit_job.sh 8 | done 9 | -------------------------------------------------------------------------------- /log_manager/user.txt: -------------------------------------------------------------------------------- 1 | username 2 | password 3 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rrc 4 | 1.0.0 5 | 6 | Catkin package for the Real Robot Challenge Submission System. 7 | 8 | 9 | Chip Schaff 10 | 11 | BSD 3-clause 12 | 13 | https://real-robot-challenge.com/ 14 | 15 | catkin 16 | 17 | robot_fingers 18 | robot_interfaces 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /python/cic/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cbschaff/benchmark-rrc/6fb12eecbaa778c60c5728dba4414330c901b09b/python/cic/__init__.py -------------------------------------------------------------------------------- /python/cic/bayesian_opt/const.py: -------------------------------------------------------------------------------- 1 | SIMULATION = True 2 | GITHUB_BRANCH = 'cleanup' 3 | DIFFICULTY_LEVEL = 4 4 | EVALUATE_TRAJ = 'standart' 5 | PATH_TO_IMAGE = '/home/funk/production_test1.sif' 6 | USERNAME = 'emptyheron' 7 | PWD = '' 8 | 9 | RELATIVE_MOVEMENT = True 10 | SAMPLE_FCT = 'sample_lift_w_orientations_directions'#'sample_rot_ground_directions' 11 | 12 | NUM_INIT_SAMPLES = 4 13 | NUM_ROLLOUTS_PER_SAMPLE = 4 14 | NUM_LOCAL_THREADS = 15 # only has an effect when running in simulation 15 | NUM_ITERATIONS = 50 16 | NUM_ACQ_RESTARTS = 500 17 | ACQ_SAMPLES = 1000 18 | 19 | EPISODE_LEN_SIM = 25000 20 | EPISODE_LEN_REAL = 60000 21 | 22 | # EVALUATION SPECIFIC PARAMETERS - ONLY CARE ABOUT THEM WHEN RUNNING eval_code.py 23 | SAMPLE_NEW = True 24 | SPLIT_JOBS_EVENLY = True # tells whether jobs should be split evenly across robots 25 | MODELS_TO_RUN = ['model_1','model_2'] 26 | # MODELS_TO_RUN = ['tud_align','ttic_align','uw_align'] 27 | # MODELS_TO_RUN = ['tud_with_tud_grasp','tud_with_ttic_grasp','tud_with_uw_grasp','ttic_with_tud_grasp','ttic_with_ttic_grasp','ttic_with_uw_grasp','uw_with_tud_grasp','uw_with_ttic_grasp','uw_with_uw_grasp'] 28 | ROBOTS_AVAILABLE = [None]#['roboch5','roboch1'] #[None] # ['roboch5','roboch1'] # -------------------------------------------------------------------------------- /python/cic/bayesian_opt/content/diff.txt: -------------------------------------------------------------------------------- 1 | 4 -------------------------------------------------------------------------------- /python/cic/bayesian_opt/content/iter_idx.txt: -------------------------------------------------------------------------------- 1 | 1 -------------------------------------------------------------------------------- /python/cic/bayesian_opt/content/model.txt: -------------------------------------------------------------------------------- 1 | model_2 -------------------------------------------------------------------------------- /python/cic/bayesian_opt/content/params.pkl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cbschaff/benchmark-rrc/6fb12eecbaa778c60c5728dba4414330c901b09b/python/cic/bayesian_opt/content/params.pkl -------------------------------------------------------------------------------- /python/cic/bayesian_opt/content/pos.pkl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cbschaff/benchmark-rrc/6fb12eecbaa778c60c5728dba4414330c901b09b/python/cic/bayesian_opt/content/pos.pkl -------------------------------------------------------------------------------- /python/cic/bayesian_opt/utils/adapt_image.def: -------------------------------------------------------------------------------- 1 | # Specify the name of the base image below 2 | Bootstrap: localimage 3 | From: ./production_test.sif 4 | 5 | %setup 6 | # cp setup.py package.xml ${SINGULARITY_ROOTFS}/ 7 | # cp -r python ${SINGULARITY_ROOTFS}/ 8 | 9 | %files 10 | 11 | %post 12 | # Put commands to install additional dependencies here. 13 | # Make sure everything runs automatically without human input (e.g. add 14 | # `-y` to automatically say "yes" below). 15 | pip install urllib3 16 | python3 -m pip install gin 17 | pip install gin-config 18 | -------------------------------------------------------------------------------- /python/cic/bayesian_opt/utils/env.yml: -------------------------------------------------------------------------------- 1 | name: myenv 2 | channels: 3 | - defaults 4 | dependencies: 5 | - _libgcc_mutex=0.1=main 6 | - ca-certificates=2021.1.19=h06a4308_0 7 | - certifi=2020.12.5=py38h06a4308_0 8 | - ld_impl_linux-64=2.33.1=h53a641e_7 9 | - libedit=3.1.20191231=h14c3975_1 10 | - libffi=3.3=he6710b0_2 11 | - libgcc-ng=9.1.0=hdf63c60_0 12 | - libstdcxx-ng=9.1.0=hdf63c60_0 13 | - ncurses=6.2=he6710b0_1 14 | - openssl=1.1.1i=h27cfd23_0 15 | - pip=20.3.3=py38h06a4308_0 16 | - python=3.8.5=h7579374_1 17 | - readline=8.1=h27cfd23_0 18 | - setuptools=52.0.0=py38h06a4308_0 19 | - sqlite=3.33.0=h62c20be_0 20 | - tk=8.6.10=hbc83047_0 21 | - wheel=0.36.2=pyhd3eb1b0_0 22 | - xz=5.2.5=h7b6447c_0 23 | - zlib=1.2.11=h7b6447c_3 24 | - pip: 25 | - attrs==20.3.0 26 | - botorch==0.4.0 27 | - cloudpickle==1.6.0 28 | - colorama==0.4.4 29 | - cycler==0.10.0 30 | - cython==0.29.21 31 | - future==0.18.2 32 | - gin==0.1.6 33 | - gpytorch==1.4.0 34 | - gym==0.18.0 35 | - iniconfig==1.1.1 36 | - joblib==1.0.1 37 | - kiwisolver==1.3.1 38 | - matplotlib==3.3.4 39 | - numpy==1.20.1 40 | - numpy-ml==0.1.2 41 | - opencv-python==4.5.1.48 42 | - packaging==20.9 43 | - pillow==7.2.0 44 | - pluggy==0.13.1 45 | - py==1.10.0 46 | - pybullet==3.0.8 47 | - pygame==2.0.1 48 | - pyglet==1.5.0 49 | - pyparsing==2.4.7 50 | - pytest==6.2.2 51 | - python-dateutil==2.8.1 52 | - scikit-learn==0.24.1 53 | - scipy==1.6.0 54 | - six==1.15.0 55 | - sobol-seq==0.2.0 56 | - threadpoolctl==2.1.0 57 | - toml==0.10.2 58 | - torch==1.7.1 59 | - tqdm==4.56.1 60 | - typing-extensions==3.7.4.3 61 | prefix: /home/funk/miniconda3/envs/myenv 62 | 63 | -------------------------------------------------------------------------------- /python/cic/bayesian_opt/utils/push_github.sh: -------------------------------------------------------------------------------- 1 | git add ../* 2 | git commit -m $1 3 | git push origin $2 4 | -------------------------------------------------------------------------------- /python/cic/bayesian_opt/utils/roboch.json: -------------------------------------------------------------------------------- 1 | { 2 | "repository": "git@github.com:cbschaff/real-robot-challenge-collab.git", 3 | "branch": "cleanup", 4 | "email": "funk@ias.tu-darmstadt.de", 5 | "git_deploy_key": "deploy_key1", 6 | "episode_length": 60000, 7 | "singularity_image": "production_test1.sif", 8 | "group": "USER" 9 | } -------------------------------------------------------------------------------- /python/cic/bayesian_opt/utils/run_episode_bo.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | """Run a single episode with our controller. 3 | This script expects the following arguments in the given order: 4 | - Difficulty level (needed for reward computation) 5 | - goal pose of the object (as JSON string) (optional) 6 | """ 7 | import sys 8 | import json 9 | 10 | from trifinger_simulation.tasks import move_cube 11 | from env.make_env import make_env 12 | from mp.utils import set_seed 13 | 14 | from cic import rotation_primitives 15 | 16 | from cic.bayesian_opt.const import RELATIVE_MOVEMENT, EPISODE_LEN_REAL 17 | 18 | from cic import parameters_new_grasp as cic_parameters_new_grasp 19 | from cic.states import CICStateMachineLvl4 as CICwithCG 20 | 21 | 22 | def _init_env(goal_pose_json, difficulty, path=None): 23 | eval_config = { 24 | 'action_space': 'torque_and_position', 25 | 'frameskip': 3, 26 | 'reward_fn': 'competition_reward', 27 | 'termination_fn': 'no_termination', 28 | 'initializer': 'bo_init', 29 | 'monitor': False, 30 | 'visualization': False, 31 | 'sim': False, 32 | 'rank': 0, 33 | 'episode_length': EPISODE_LEN_REAL-1000, 34 | } 35 | 36 | set_seed(0) 37 | goal_pose_dict = json.loads(goal_pose_json) 38 | env = make_env(goal_pose_dict, difficulty, path=path, **eval_config) 39 | return env 40 | 41 | 42 | def main(): 43 | with open("/ws/src/usercode/python/cic/bayesian_opt/content/diff.txt", 'r') as f: 44 | difficulty = int(f.readline()) 45 | 46 | import numpy as np 47 | import pickle as pkl 48 | with open("/ws/src/usercode/python/cic/bayesian_opt/content/pos.pkl", 'rb') as f: 49 | init_arr_params = pkl.load(f) 50 | 51 | with open("/ws/src/usercode/python/cic/bayesian_opt/content/params.pkl", 'rb') as f: 52 | bo_params = np.asarray(pkl.load(f),dtype=float) 53 | 54 | with open("/ws/src/usercode/python/cic/bayesian_opt/content/iter_idx.txt", 'r') as f: 55 | curr_idx = int(f.readline()) 56 | 57 | with open("/ws/src/usercode/python/cic/bayesian_opt/content/model.txt", 'r') as f: 58 | model_to_be_loaded = (f.readline()) 59 | print ("The model to be loaded is: ", model_to_be_loaded) 60 | 61 | print ("The current index is : " + str(curr_idx)) 62 | init_information = np.asarray(init_arr_params[:, curr_idx], dtype=float) 63 | # Override goal pose: 64 | goal_pose_json = json.dumps({ 65 | 'position': init_information[7:10].tolist(), 66 | 'orientation': init_information[10:14].tolist() 67 | }) 68 | 69 | print ("The specified goal is: ", init_information[7:10]) 70 | print ("The parameters to be optimized are: " + str(bo_params)) 71 | 72 | 73 | env = _init_env(goal_pose_json, difficulty) 74 | 75 | 76 | parameters = cic_parameters_new_grasp.CubeLvl4Params(env) 77 | parameters.orient_grasp_xy_lift += float(bo_params[0]) 78 | parameters.orient_grasp_h_lift += float(bo_params[1]) 79 | parameters.orient_gain_xy_lift_lift = float(bo_params[2]) 80 | parameters.orient_gain_z_lift_lift = float(bo_params[3]) 81 | parameters.orient_pos_gain_impedance_lift_lift = float(bo_params[4]) 82 | parameters.orient_force_factor_lift = float(bo_params[5]) 83 | parameters.orient_force_factor_rot_lift = float(bo_params[6]) 84 | parameters.orient_int_orient_gain = float(bo_params[7]) 85 | parameters.orient_int_pos_gain = float(bo_params[8]) 86 | 87 | state_machine = CICwithCG(env, parameters=parameters) 88 | 89 | obs = env.reset() 90 | state_machine.reset() 91 | 92 | if (RELATIVE_MOVEMENT): 93 | # after the observation has been reset -> we now define the goal relative to the current position,... 94 | goal_list = rotation_primitives.calculate_mutltiple_goals(init_information, obs) 95 | env.goal_list = goal_list 96 | env.set_goal(orientation=goal_list[1][0], pos=goal_list[1][1], log_timestep=True) 97 | 98 | done = False 99 | while not done: 100 | 101 | if (RELATIVE_MOVEMENT): 102 | goal_list = rotation_primitives.calculate_mutltiple_goals(init_information, obs) 103 | env.goal_list = goal_list 104 | 105 | action = state_machine(obs) 106 | obs, _, done, _ = env.step(action) 107 | 108 | 109 | if __name__ == "__main__": 110 | main() -------------------------------------------------------------------------------- /python/cic/bayesian_opt/utils/upload_json.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | username=$1 4 | password=$2 5 | 6 | echo "Upload JSON" 7 | sshpass -p ${password} scp roboch.json ${username}@robots.real-robot-challenge.com: -------------------------------------------------------------------------------- /python/cic/math_tools.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import math 3 | 4 | 5 | def rpy2Mat(roll, pitch, yaw): 6 | yawMatrix = np.matrix([ 7 | [math.cos(yaw), -math.sin(yaw), 0], 8 | [math.sin(yaw), math.cos(yaw), 0], 9 | [0, 0, 1] 10 | ]) 11 | 12 | pitchMatrix = np.matrix([ 13 | [math.cos(pitch), 0, math.sin(pitch)], 14 | [0, 1, 0], 15 | [-math.sin(pitch), 0, math.cos(pitch)] 16 | ]) 17 | 18 | rollMatrix = np.matrix([ 19 | [1, 0, 0], 20 | [0, math.cos(roll), -math.sin(roll)], 21 | [0, math.sin(roll), math.cos(roll)] 22 | ]) 23 | 24 | R = yawMatrix * pitchMatrix * rollMatrix 25 | 26 | return R -------------------------------------------------------------------------------- /python/cic/states/__init__.py: -------------------------------------------------------------------------------- 1 | from .state_machine import StateMachineCIC 2 | from .base_states import * 3 | from .sequences import * 4 | from .custom_state_machines import * 5 | -------------------------------------------------------------------------------- /python/cic/states/state_machine.py: -------------------------------------------------------------------------------- 1 | from mp import states 2 | from cic import object_class as cic_object_class 3 | from cic import control_main_class as cic_control_main_class 4 | from cic import parameters as cic_parameters 5 | from cic import parameters_new_grasp as cic_parameters_new_grasp 6 | 7 | 8 | class StateMachineCIC(states.StateMachine): 9 | def __init__(self, env, main_ctrl=None, object=None, parameters=None, new_grasp=True): 10 | if (main_ctrl is None): 11 | main_ctrl = cic_control_main_class.TriFingerController(env) 12 | if (object is None): 13 | object = cic_object_class.Cube() 14 | 15 | if (parameters is None): 16 | if not(new_grasp): 17 | difficulty = env.difficulty 18 | if (difficulty == 1): 19 | parameters = cic_parameters.CubeLvl1Params(env) 20 | elif (difficulty == 2 or difficulty == 3): 21 | parameters = cic_parameters.CubeLvl2Params(env) 22 | elif (difficulty == 4): 23 | parameters = cic_parameters.CubeLvl4Params(env) 24 | else: 25 | difficulty = env.difficulty 26 | if (difficulty == 1): 27 | parameters = cic_parameters_new_grasp.CubeLvl1Params(env) 28 | elif (difficulty == 2 or difficulty == 3): 29 | parameters = cic_parameters_new_grasp.CubeLvl2Params(env) 30 | elif (difficulty == 4): 31 | parameters = cic_parameters_new_grasp.CubeLvl4Params(env) 32 | 33 | self.env = env 34 | self.main_ctrl = main_ctrl 35 | self.object = object 36 | self.parameters = parameters 37 | self.new_grasp = new_grasp 38 | self.init_state = self.build() 39 | -------------------------------------------------------------------------------- /python/cic/utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import copy 3 | import pybullet 4 | 5 | def create_robot_state(obs,kinematics): 6 | robot_state = [obs["robot_position"], obs["robot_velocity"], kinematics.get_tip_pos_pinnochio(obs["robot_position"])] 7 | return copy.deepcopy(robot_state) 8 | 9 | def create_object_state(obs,kinematics): 10 | object_state = [obs["object_position"], obs["object_orientation"], np.asarray(pybullet.getEulerFromQuaternion(obs["object_orientation"])), 11 | obs["goal_object_position"], obs["goal_object_orientation"], 12 | np.asarray(pybullet.getEulerFromQuaternion(obs["goal_object_orientation"]))] 13 | return copy.deepcopy(object_state) 14 | 15 | def get_robot_and_obj_state(obs,kinematics, last_states=None): 16 | if (last_states is None): 17 | return create_robot_state(obs, kinematics), create_object_state(obs, kinematics) 18 | else: 19 | robot_states = create_robot_state(obs, kinematics) 20 | obj_state = create_object_state(obs, kinematics) 21 | # now additionally propagate the states: 22 | last_states[:, 1:] = last_states[:, :-1] 23 | last_states[:, 0] = copy.deepcopy(obj_state[0]) 24 | return robot_states, obj_state -------------------------------------------------------------------------------- /python/cpc/__init__.py: -------------------------------------------------------------------------------- 1 | from .state_machine import * 2 | -------------------------------------------------------------------------------- /python/cpc/parameters.py: -------------------------------------------------------------------------------- 1 | class CubeLvl1Params(): 2 | def __init__(self, env): 3 | if env.simulation: 4 | self.k_p_goal = 0.75 # Range: 0.3 - 1.5, same for l4 5 | self.k_p_into = 0.18 # Range: 0.1 - 0.6, same for l4 6 | self.k_i_goal = 0.008 # Range: 0.0008 - 0.1, same for l4 7 | self.interval = 1 # Range: 500 - 3000 not super important 8 | self.gain_increase_factor = 1.004 # Range: 1.01 - 2.0 9 | self.max_interval_ctr = 2000 10 | else: 11 | self.k_p_goal = 0.65 12 | self.k_p_into = 0.2 13 | self.k_i_goal = 0.004 14 | self.interval = 1800 # Range: 500 - 3000 not super important 15 | self.gain_increase_factor = 1.04 # Range: 1.01 - 2.0 16 | self.max_interval_ctr = 30 17 | 18 | 19 | class CubeParams(): 20 | def __init__(self, env): 21 | if env.simulation: 22 | self.k_p_goal = 0.9 # Range: 0.3 - 1.5, same for l4 23 | self.k_p_into = 0.20 # Range: 0.1 - 0.6, same for l4 24 | self.k_i_goal = 0.007 # Range: 0.0008 - 0.1, same for l4 25 | self.interval = 1 # Range: 500 - 3000 not super important 26 | self.gain_increase_factor = 1.004 # Range: 1.01 - 2.0 27 | self.max_interval_ctr = 2000 28 | else: 29 | self.k_p_goal = 0.65 30 | self.k_p_into = 0.25 31 | self.k_i_goal = 0.004 32 | self.interval = 1800 # Range: 500 - 3000 not super important 33 | self.gain_increase_factor = 1.04 # Range: 1.01 - 2.0 34 | self.max_interval_ctr = 30 35 | 36 | 37 | class CubeLvl4Params(): 38 | def __init__(self,env): 39 | if env.simulation: 40 | self.gain_increase_factor = 1.002 41 | self.k_p_goal = 0.9 42 | self.k_p_into = 0.25 43 | self.k_i_goal = 0.006 44 | self.k_p_ang = 0.01 45 | self.k_i_ang = 0.0005 46 | self.pitch_k_p_goal = 0.45 47 | self.pitch_k_p_into = 0.22 48 | self.pitch_k_i_goal = 0.004 49 | self.pitch_k_p_ang = 0.25 50 | self.pitch_k_i_ang = 0.0005 51 | self.yaw_k_p_goal = 0.9 52 | self.yaw_k_p_into = 0.25 53 | self.yaw_k_i_goal = 0.006 54 | self.yaw_k_p_ang = 0.01 55 | self.yaw_k_i_ang = 0.0005 56 | self.interval = 3 57 | self.max_interval_ctr = 2000 58 | self.pitch_rot_factor = 1.5 59 | self.pitch_lift_factor = 2.46 60 | else: 61 | self.gain_increase_factor = 1.04 62 | self.k_p_goal = 0.65 63 | self.k_p_into = 0.28 64 | self.k_i_goal = 0.004 65 | self.k_p_ang = 0.05 66 | self.k_i_ang = 0.0005 67 | self.pitch_k_p_goal = 0.5 68 | self.pitch_k_p_into = 0.36 69 | self.pitch_k_i_goal = 0.005 70 | self.pitch_k_p_ang = 0.15 71 | self.pitch_k_i_ang = 0.002 72 | self.yaw_k_p_goal = 0.65 73 | self.yaw_k_p_into = 0.28 74 | self.yaw_k_i_goal = 0.004 75 | self.yaw_k_p_ang = 0.05 76 | self.yaw_k_i_ang = 0.001 77 | self.interval = 1800 78 | self.max_interval_ctr = 30 79 | self.pitch_rot_factor = 1.18 80 | self.pitch_lift_factor = 1.38 81 | -------------------------------------------------------------------------------- /python/cpc/states/__init__.py: -------------------------------------------------------------------------------- 1 | from .base_states import * 2 | from .sequence_state import * 3 | -------------------------------------------------------------------------------- /python/env/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cbschaff/benchmark-rrc/6fb12eecbaa778c60c5728dba4414330c901b09b/python/env/__init__.py -------------------------------------------------------------------------------- /python/env/make_env.py: -------------------------------------------------------------------------------- 1 | from .cube_env import RealRobotCubeEnv, ActionType 2 | import env.wrappers as wrappers 3 | 4 | 5 | def get_initializer(name): 6 | from env import initializers 7 | if name is None: 8 | return None 9 | if hasattr(initializers, name): 10 | return getattr(initializers, name) 11 | else: 12 | raise ValueError(f"Can't find initializer: {name}") 13 | 14 | 15 | def get_reward_fn(name): 16 | from env import reward_fns 17 | if name is None: 18 | return reward_fns.competition_reward 19 | if hasattr(reward_fns, name): 20 | return getattr(reward_fns, name) 21 | else: 22 | raise ValueError(f"Can't find reward function: {name}") 23 | 24 | 25 | def get_termination_fn(name): 26 | from env import termination_fns 27 | if name is None: 28 | return None 29 | if hasattr(termination_fns, name): 30 | return getattr(termination_fns, name) 31 | elif hasattr(termination_fns, "generate_" + name): 32 | return getattr(termination_fns, "generate_" + name)() 33 | else: 34 | raise ValueError(f"Can't find termination function: {name}") 35 | 36 | 37 | def make_env(cube_goal_pose, goal_difficulty, action_space, frameskip=1, 38 | sim=False, visualization=False, reward_fn=None, 39 | termination_fn=None, initializer=None, episode_length=119000, 40 | rank=0, monitor=False, path=None): 41 | reward_fn = get_reward_fn(reward_fn) 42 | initializer = get_initializer(initializer)(goal_difficulty) 43 | termination_fn = get_termination_fn(termination_fn) 44 | if action_space not in ['torque', 'position', 'torque_and_position', 'position_and_torque']: 45 | raise ValueError(f"Unknown action space: {action_space}.") 46 | if action_space == 'torque': 47 | action_type = ActionType.TORQUE 48 | elif action_space in ['torque_and_position', 'position_and_torque']: 49 | action_type = ActionType.TORQUE_AND_POSITION 50 | else: 51 | action_type = ActionType.POSITION 52 | env = RealRobotCubeEnv(cube_goal_pose, 53 | goal_difficulty, 54 | action_type=action_type, 55 | frameskip=frameskip, 56 | sim=sim, 57 | visualization=visualization, 58 | reward_fn=reward_fn, 59 | termination_fn=termination_fn, 60 | initializer=initializer, 61 | episode_length=episode_length, 62 | path=path) 63 | env.seed(seed=rank) 64 | env.action_space.seed(seed=rank) 65 | env = wrappers.NewToOldObsWrapper(env) 66 | env = wrappers.AdaptiveActionSpaceWrapper(env) 67 | if not sim: 68 | env = wrappers.TimingWrapper(env, 0.001) 69 | if visualization: 70 | env = wrappers.PyBulletClearGUIWrapper(env) 71 | if monitor: 72 | env = wrappers.RenderWrapper(env) 73 | return env 74 | -------------------------------------------------------------------------------- /python/env/termination_fns.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from scipy.spatial.transform import Rotation 3 | 4 | 5 | def no_termination(observation): 6 | return False 7 | 8 | 9 | def position_close_to_goal(observation): 10 | dist_to_goal = np.linalg.norm( 11 | observation["desired_goal"]["position"] 12 | - observation["achieved_goal"]["position"] 13 | ) 14 | return dist_to_goal < 0.01 15 | 16 | 17 | def pos_and_rot_close_to_goal(observation): 18 | rot_error_deg = _orientation_error(observation) * 180 19 | allowance = 5.0 20 | return position_close_to_goal(observation) and rot_error_deg < allowance 21 | 22 | 23 | def _orientation_error(observation): 24 | '''copied from reward_fns.py''' 25 | goal_rot = Rotation.from_quat(observation['desired_goal']['orientation']) 26 | actual_rot = Rotation.from_quat(observation['achieved_goal']['orientation']) 27 | error_rot = goal_rot.inv() * actual_rot 28 | return error_rot.magnitude() / np.pi 29 | 30 | 31 | class StayCloseToGoal(object): 32 | def __init__(self, success_steps=30, is_level_4=False): 33 | self.counter = 0 34 | self.success_steps = success_steps 35 | self.goal_check = pos_and_rot_close_to_goal if is_level_4 else position_close_to_goal 36 | 37 | def __call__(self, observation): 38 | if self.goal_check(observation): 39 | self.counter += 1 40 | if self.counter >= self.success_steps: 41 | self.counter = 0 42 | return True 43 | else: 44 | self.counter = 0 45 | return False 46 | 47 | 48 | stay_close_to_goal = StayCloseToGoal(is_level_4=False) 49 | stay_close_to_goal_level_4 = StayCloseToGoal(is_level_4=True) 50 | -------------------------------------------------------------------------------- /python/mp/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cbschaff/benchmark-rrc/6fb12eecbaa778c60c5728dba4414330c901b09b/python/mp/__init__.py -------------------------------------------------------------------------------- /python/mp/align_rotation.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from scipy.spatial.transform import Rotation as R 3 | 4 | 5 | def get_most_vertical_axis(orientation): 6 | axes = np.eye(3) 7 | axes_in_base_frame = R.from_quat(orientation).apply(axes) 8 | z = axes_in_base_frame[:, 2] # dot product with z axis 9 | ind = np.argmax(np.abs(z)) 10 | sign = np.sign(z[ind]) 11 | return sign * axes[ind], sign * axes_in_base_frame[ind] 12 | 13 | 14 | def project_cube_xy_plane(orientation): 15 | ax_cube_frame, ax_base_frame = get_most_vertical_axis(orientation) 16 | rot_align = vector_align_rotation(ax_base_frame, np.array([0, 0, 1])) 17 | return (rot_align * R.from_quat(orientation)).as_quat() 18 | 19 | 20 | def vector_align_rotation(a, b): 21 | """ 22 | return Rotation that transform vector a to vector b 23 | 24 | input 25 | a : np.array(3) 26 | b : np.array(3) 27 | 28 | return 29 | rot : scipy.spatial.transform.Rotation 30 | """ 31 | norm_a = np.linalg.norm(a) 32 | norm_b = np.linalg.norm(b) 33 | assert norm_a != 0 and norm_b != 0 34 | 35 | a = a / norm_a 36 | b = b / norm_b 37 | 38 | cross = np.cross(a, b) 39 | norm_cross = np.linalg.norm(cross) 40 | cross = cross / norm_cross 41 | dot = np.dot(a, b) 42 | 43 | if norm_cross < 1e-8 and dot > 0: 44 | '''same direction, no rotation a == b''' 45 | return R.from_quat([0, 0, 0, 1]) 46 | elif norm_cross < 1e-8 and dot < 0: 47 | '''opposite direction a == -b''' 48 | c = np.eye(3)[np.argmax(np.linalg.norm(np.eye(3) - a, axis=1))] 49 | cross = np.cross(a, c) 50 | norm_cross = np.linalg.norm(cross) 51 | cross = cross / norm_cross 52 | 53 | return R.from_rotvec(cross * np.pi) 54 | 55 | rot = R.from_rotvec(cross * np.arctan2(norm_cross, dot)) 56 | 57 | assert np.linalg.norm(rot.apply(a) - b) < 1e-7 58 | return rot 59 | 60 | 61 | def roll_and_pitch_aligned(cube_orientation, goal_orientation): 62 | ax_cube, _ = get_most_vertical_axis(cube_orientation) 63 | ax_goal, _ = get_most_vertical_axis(goal_orientation) 64 | return np.allclose(ax_cube, ax_goal) 65 | 66 | 67 | def get_yaw_diff(ori, goal_ori): 68 | # NOTE: This assertion should be here only when the object is a cube 69 | # assert roll_and_pitch_aligned(ori, goal_ori) 70 | proj_goal_ori = project_cube_xy_plane(goal_ori) 71 | rot = R.from_quat(proj_goal_ori) * R.from_quat(ori).inv() 72 | return rot.as_rotvec()[2] 73 | 74 | 75 | def get_roll_pitch_axis_and_angle(cube_orientation, goal_orientation): 76 | if roll_and_pitch_aligned(cube_orientation, goal_orientation): 77 | return None, None 78 | rot_cube = R.from_quat(cube_orientation) 79 | rot_goal = R.from_quat(project_cube_xy_plane(goal_orientation)) 80 | ax_up_cube, _ = get_most_vertical_axis(cube_orientation) 81 | 82 | diffs = [] 83 | axis_angles = [] 84 | for i, axis in enumerate(np.eye(3)): 85 | if ax_up_cube[i] != 0: 86 | continue 87 | for angle in [np.pi / 2, -np.pi / 2]: 88 | rot_align = R.from_rotvec(axis * angle) 89 | rot_diff = (rot_goal * (rot_cube * rot_align).inv()).magnitude() 90 | diffs.append(rot_diff) 91 | axis_angles.append((axis, angle)) 92 | 93 | return axis_angles[np.argmin(diffs)] 94 | -------------------------------------------------------------------------------- /python/mp/base_policies/__init__.py: -------------------------------------------------------------------------------- 1 | from .fc import ZeroTorquePolicy, CancelGravityPolicy 2 | from .mpfc import PlanningAndForceControlPolicy 3 | -------------------------------------------------------------------------------- /python/mp/base_policies/fc.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import numpy as np 3 | import pybullet as p 4 | from trifinger_simulation import TriFingerPlatform 5 | 6 | 7 | class ZeroTorquePolicy(object): 8 | def __call__(self, *args, **kwargs): 9 | return np.zeros(9) 10 | 11 | 12 | class CancelGravityPolicy(object): 13 | def __init__(self, env): 14 | self.id = env.platform.simfinger.finger_id 15 | 16 | def __call__(self, obs, *args, **kwargs): 17 | torque = p.calculateInverseDynamics(self.id, 18 | list(obs['robot_position']), 19 | list(obs['robot_velocity']), 20 | [0. for _ in range(9)]) 21 | tas = TriFingerPlatform.spaces.robot_torque.gym 22 | return np.clip(np.array(torque), tas.low, tas.high) 23 | -------------------------------------------------------------------------------- /python/mp/const.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import numpy as np 3 | from trifinger_simulation.tasks import move_cube 4 | from trifinger_simulation.trifinger_platform import TriFingerPlatform 5 | 6 | COLLISION_TOLERANCE = 3.5 * 1e-03 7 | MU = 0.5 8 | CUBOID_SIZE = np.array((0.065, 0.065, 0.065)) 9 | CUBOID_HALF_SIZE = CUBOID_SIZE / 2 10 | CUBOID_MASS = 0.094 11 | VIRTUAL_CUBOID_HALF_SIZE = CUBOID_HALF_SIZE + 0.007 12 | MIN_HEIGHT = min(CUBOID_HALF_SIZE) 13 | MAX_HEIGHT = move_cube._max_height 14 | ARENA_RADIUS = move_cube._ARENA_RADIUS 15 | AVG_POSE_STEPS = 200 16 | # INIT_JOINT_CONF = TriFingerPlatform.spaces.robot_position.default 17 | INIT_JOINT_CONF = np.array([0.0, 0.9, -2.0, 0.0, 0.9, -2.0, 0.0, 0.9, -2.0], dtype=np.float32) 18 | CONTRACTED_JOINT_CONF = np.array([0.0, 1.4, -2.4, 0.0, 1.4, -2.4, 0.0, 1.4, -2.4], dtype=np.float32) 19 | CLEARED_JOINT_CONF = np.array([0.0, 1.4, -0.6, 0.0, 1.4, -0.6, 0.0, 1.4, -0.6], dtype=np.float32) 20 | 21 | TMP_VIDEO_DIR = '/tmp/rrc_videos' 22 | CUSTOM_LOGDIR = '/output' # only applicable when it is running on Singularity 23 | 24 | EXCEP_MSSG = "================= captured exception =================\n" + \ 25 | "{message}\n" + "{error}\n" + '==================================' 26 | 27 | # colors 28 | TRANSLU_CYAN = (0, 1, 1, 0.4) 29 | TRANSLU_YELLOW = (1, 1, 0, 0.4) 30 | TRANSLU_BLUE = (0, 0, 1, 0.4) 31 | TRANSLU_RED = (1, 0, 0, 0.4) 32 | -------------------------------------------------------------------------------- /python/mp/grasping/__init__.py: -------------------------------------------------------------------------------- 1 | from .grasp_functions import * 2 | from .grasp_motions import get_grasp_approach_actions, get_safe_pregrasp 3 | from .collision_config import CollisionConfig 4 | -------------------------------------------------------------------------------- /python/mp/grasping/collision_config.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | from mp.const import COLLISION_TOLERANCE 3 | workspace_id = 0 4 | 5 | class CollisionConfig: 6 | def __init__(self, env): 7 | self.env = env 8 | self.finger_id = env.platform.simfinger.finger_id 9 | self.tip_ids = env.platform.simfinger.pybullet_tip_link_indices 10 | self.link_ids = env.platform.simfinger.pybullet_link_indices 11 | self.cube_id = env.platform.cube.block 12 | self. disabled_collisions = [((self.finger_id, tip_id), (self.cube_id, -1)) 13 | for tip_id in self.tip_ids] 14 | 15 | def get_collision_conf(self, config_type): 16 | if config_type == "mpfc": # previously: slacky_collision == True 17 | return { 18 | 'body': self.finger_id, 19 | 'joints': self.link_ids, 20 | 'obstacles': [self.cube_id, workspace_id], 21 | 'self_collisions': True, 22 | 'extra_disabled_collisions': self.disabled_collisions, 23 | 'max_distance': -COLLISION_TOLERANCE 24 | } 25 | elif config_type == "yaw_flip_path": 26 | # NOTE: This is for collision between finger tips! 27 | return { 28 | 'body': self.finger_id, 29 | 'joints': self.link_ids, 30 | 'obstacles': [workspace_id], # ignore collisions with cube 31 | 'self_collisions': True, 32 | 'extra_disabled_collisions': self.disabled_collisions, 33 | 'max_distance': -COLLISION_TOLERANCE * 3 34 | } 35 | elif config_type == "yaw_flip_grasp": 36 | return { 37 | 'body': self.finger_id, 38 | 'joints': self.link_ids, 39 | 'obstacles': [self.cube_id, workspace_id], 40 | 'self_collisions': True, 41 | 'extra_disabled_collisions': self.disabled_collisions, 42 | 'max_distance': -COLLISION_TOLERANCE * 3 # larger collision admittance 43 | } 44 | else: # previsouly: slacky_collision == False 45 | return { 46 | 'body': self.finger_id, 47 | 'joints': self.link_ids, 48 | 'obstacles': [self.cube_id, workspace_id], 49 | 'self_collisions': False 50 | } 51 | 52 | def get_collision_fn(self, config_type, diagnosis=False): 53 | from pybullet_planning.interfaces.robots.collision import get_collision_fn 54 | import functools 55 | config = self.get_collision_conf(config_type) 56 | return functools.partial(get_collision_fn(**config), diagnosis=diagnosis) 57 | -------------------------------------------------------------------------------- /python/mp/grasping/grasp_functions.py: -------------------------------------------------------------------------------- 1 | from .grasp_sampling import GraspSampler 2 | from .wholebody_planning import WholeBodyPlanner 3 | from mp.const import VIRTUAL_CUBOID_HALF_SIZE 4 | from mp.utils import keep_state 5 | import mp.align_rotation as rot_util 6 | from scipy.spatial.transform import Rotation as R 7 | import copy 8 | import numpy as np 9 | 10 | 11 | def get_heuristic_grasp(env, pos, quat): 12 | sampler = GraspSampler(env, pos, quat) 13 | try: 14 | return sampler.get_heuristic_grasps()[0] 15 | except StopIteration: 16 | return sampler() 17 | 18 | 19 | def get_all_heuristic_grasps(env, pos, quat, avoid_edge_faces=True): 20 | return GraspSampler( 21 | env, pos, quat, avoid_edge_faces=avoid_edge_faces 22 | ).get_heuristic_grasps() 23 | 24 | 25 | def sample_grasp(env, pos, quat): 26 | return GraspSampler(env, pos, quat)() 27 | 28 | 29 | def sample_partial_grasp(env, pos, quat): 30 | return GraspSampler(env, pos, quat, allow_partial_sol=True)() 31 | 32 | 33 | def get_planned_grasp(env, pos, quat, goal_pos, goal_quat, tight=False, 34 | **kwargs): 35 | planner = WholeBodyPlanner(env) 36 | path = planner.plan(pos, quat, goal_pos, goal_quat, **kwargs) 37 | grasp = copy.deepcopy(path.grasp) 38 | if tight: 39 | path = path.tighten(env, path, coef=0.5) 40 | 41 | # save planned trajectory 42 | env.unwrapped.register_custom_log('wholebody_path', {'cube': path.cube, 'tip_path': path.tip_path}) 43 | env.unwrapped.save_custom_logs() 44 | return grasp, path 45 | 46 | 47 | def get_pitching_grasp(env, pos, quat, goal_quat): 48 | axis, angle = rot_util.get_roll_pitch_axis_and_angle(quat, goal_quat) 49 | vert, _ = rot_util.get_most_vertical_axis(quat) 50 | turning_axis = np.cross(vert, axis) 51 | 52 | cube_tip_positions = np.asarray([ 53 | VIRTUAL_CUBOID_HALF_SIZE * axis, 54 | VIRTUAL_CUBOID_HALF_SIZE * turning_axis * np.sign(angle), 55 | VIRTUAL_CUBOID_HALF_SIZE * -axis, 56 | ]) 57 | 58 | grasp_sampler = GraspSampler(env, pos, quat) 59 | with keep_state(env): 60 | try: 61 | return grasp_sampler.get_feasible_grasps_from_tips( 62 | grasp_sampler.T_cube_to_base(cube_tip_positions) 63 | ).__next__(), axis, angle 64 | 65 | except StopIteration: 66 | grasp_sampler = GraspSampler(env, pos, quat, ignore_collision=True) 67 | return grasp_sampler.get_feasible_grasps_from_tips( 68 | grasp_sampler.T_cube_to_base(cube_tip_positions) 69 | ).__next__(), axis, angle 70 | 71 | 72 | def get_yawing_grasp(env, pos, quat, goal_quat, step_angle=np.pi / 2): 73 | from mp.align_rotation import get_yaw_diff 74 | from mp.const import COLLISION_TOLERANCE 75 | print("[get_yawing_grasp] step_angle:", step_angle * 180 / np.pi) 76 | angle = get_yaw_diff(quat, goal_quat) 77 | print('[get_yawing_grasp] get_yaw_diff:', angle * 180 / np.pi) 78 | angle_clip = np.clip(angle, -step_angle, step_angle) 79 | print('[get_yawing_grasp] clipped angle:', angle_clip * 180 / np.pi) 80 | goal_quat = (R.from_euler('Z', angle_clip) * R.from_quat(quat)).as_quat() 81 | planner = WholeBodyPlanner(env) 82 | try: 83 | path = planner.plan(pos, quat, pos, goal_quat, use_ori=True, avoid_edge_faces=True, yawing_grasp=True, 84 | collision_tolerance=-COLLISION_TOLERANCE * 3, retry_grasp=0, direct_path=True) 85 | except RuntimeError as e: 86 | print(f'[get_yawing_grasp] wholebody planning failed for step_angle: {step_angle}') 87 | return None, None 88 | 89 | grasp = copy.deepcopy(path.grasp) 90 | # save planned trajectory 91 | env.unwrapped.register_custom_log('wholebody_path', {'cube': path.cube, 'tip_path': path.tip_path}) 92 | env.unwrapped.save_custom_logs() 93 | return grasp, path 94 | -------------------------------------------------------------------------------- /python/mp/grasping/grasp_motions.py: -------------------------------------------------------------------------------- 1 | from .ik import IKUtils 2 | from mp.action_sequences import ScriptedActions 3 | from mp.const import INIT_JOINT_CONF 4 | import numpy as np 5 | 6 | 7 | def get_grasp_approach_actions(env, obs, grasp): 8 | action_sequence = ScriptedActions(env, obs['robot_tip_positions'], grasp) 9 | pregrasp_joint_conf, pregrasp_tip_pos = get_safe_pregrasp( 10 | env, obs, grasp 11 | ) 12 | if pregrasp_joint_conf is None: 13 | raise RuntimeError('Feasible heuristic grasp approach is not found.') 14 | 15 | action_sequence.add_raise_tips() 16 | action_sequence.add_heuristic_pregrasp(pregrasp_tip_pos) 17 | action_sequence.add_grasp(coef=0.6) 18 | 19 | act_seq = action_sequence.get_action_sequence( 20 | action_repeat=4 if env.simulation else 12 * 4, 21 | action_repeat_end=40 if env.simulation else 400 22 | ) 23 | 24 | return act_seq 25 | 26 | 27 | def get_safe_pregrasp(env, obs, grasp, candidate_margins=[1.1, 1.3, 1.5]): 28 | pregrasp_tip_pos = [] 29 | pregrasp_jconfs = [] 30 | ik_utils = IKUtils(env) 31 | init_tip_pos = env.platform.forward_kinematics(INIT_JOINT_CONF) 32 | mask = np.eye(3)[grasp.valid_tips, :].sum(0).reshape(3, -1) 33 | 34 | for margin in candidate_margins: 35 | tip_pos = grasp.T_cube_to_base(grasp.cube_tip_pos * margin) 36 | tip_pos = tip_pos * mask + (1 - mask) * init_tip_pos 37 | qs = ik_utils.sample_no_collision_ik(tip_pos) 38 | if len(qs) > 0: 39 | pregrasp_tip_pos.append(tip_pos) 40 | pregrasp_jconfs.append(qs[0]) 41 | print('candidate margin coef {}: safe'.format(margin)) 42 | else: 43 | print('candidate margin coef {}: no ik solution found'.format(margin)) 44 | 45 | if len(pregrasp_tip_pos) == 0: 46 | print('warning: no safe pregrasp pose with a margin') 47 | tip_pos = grasp.T_cube_to_base(grasp.cube_tip_pos * candidate_margins[0]) 48 | tip_pos = tip_pos * mask + (1 - mask) * init_tip_pos 49 | qs = ik_utils.sample_ik(tip_pos) 50 | if len(qs) == 0: 51 | return None, None 52 | else: 53 | pregrasp_tip_pos.append(tip_pos) 54 | pregrasp_jconfs.append(qs[0]) 55 | return pregrasp_jconfs[-1], pregrasp_tip_pos[-1] 56 | -------------------------------------------------------------------------------- /python/mp/set_hyperparams.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | from mp.states import MoveToGoalState 3 | from mp.base_policies import PlanningAndForceControlPolicy 4 | 5 | 6 | def set_hyperparams(simulation): 7 | '''Set two hyperparameters that are Bayesian-Optimized''' 8 | # NOTE: These values will NOT be resetted when state.reset() is called. 9 | if simulation: 10 | MoveToGoalState.BO_action_repeat = 13 # (int) [1, 100], default: 12 11 | PlanningAndForceControlPolicy.BO_num_tipadjust_steps = 161 # (int) [10, 200], default: 50 12 | else: 13 | MoveToGoalState.BO_action_repeat = 29 # (int) [1, 100], default: 12 14 | PlanningAndForceControlPolicy.BO_num_tipadjust_steps = 182 # (int) [10, 200], default: 50 15 | -------------------------------------------------------------------------------- /python/mp/state_machines.py: -------------------------------------------------------------------------------- 1 | from mp import states 2 | 3 | 4 | class MPStateMachine(states.StateMachine): 5 | def build(self): 6 | self.goto_init_pose = states.GoToInitPoseState(self.env) 7 | self.align_object = states.AlignObjectSequenceState(self.env) 8 | self.planned_grasp = states.PlannedGraspState(self.env) 9 | self.move_to_goal = states.MoveToGoalState(self.env) 10 | self.wait = states.WaitState( 11 | self.env, 30 if self.env.simulation else 1000) 12 | self.failure = states.FailureState(self.env) 13 | 14 | # define transitions between states 15 | self.goto_init_pose.connect(next_state=self.align_object, 16 | failure_state=self.failure) 17 | self.align_object.connect(next_state=self.planned_grasp, 18 | failure_state=self.goto_init_pose) 19 | self.planned_grasp.connect(next_state=self.move_to_goal, 20 | failure_state=self.align_object) 21 | self.move_to_goal.connect(next_state=None, failure_state=self.wait) 22 | self.wait.connect(next_state=self.goto_init_pose, 23 | failure_state=self.failure) 24 | 25 | # return initial state 26 | return self.goto_init_pose 27 | -------------------------------------------------------------------------------- /python/mp/states/__init__.py: -------------------------------------------------------------------------------- 1 | from .state_machine import State, StateMachine 2 | from .base_states import * 3 | from .sequences import * 4 | -------------------------------------------------------------------------------- /python/mp/states/state_machine.py: -------------------------------------------------------------------------------- 1 | class State(object): 2 | def __init__(self, env): 3 | self.env = env 4 | 5 | def connect(self, *states): 6 | raise NotImplementedError 7 | 8 | def __call__(self, obs, info=None): 9 | raise NotImplementedError 10 | 11 | def reset(self): 12 | """clear any internal variables this state may keep""" 13 | raise NotImplementedError 14 | 15 | def get_action(self, position=None, torque=None, frameskip=1): 16 | return {'position': position, 'torque': torque, 'frameskip': frameskip} 17 | 18 | 19 | class StateMachine(object): 20 | def __init__(self, env): 21 | self.env = env 22 | self.init_state = self.build() 23 | 24 | def build(self): 25 | """Instantiate states and connect them. 26 | 27 | make sure to make all of the states instance variables so that 28 | they are reset when StateMachine.reset is called! 29 | 30 | Returns: base_states.State (initial state) 31 | 32 | 33 | Ex: 34 | 35 | self.state1 = State1(env, args) 36 | self.state2 = State2(env, args) 37 | 38 | self.state1.connect(...) 39 | self.state2.connect(...) 40 | 41 | return self.state1 42 | """ 43 | raise NotImplementedError 44 | 45 | def reset(self): 46 | self.state = self.init_state 47 | self.info = {} 48 | print("==========================================") 49 | print("Resetting State Machine...") 50 | print(f"Entering State: {self.state.__class__.__name__}") 51 | print("==========================================") 52 | for attr in vars(self).values(): 53 | if isinstance(attr, State): 54 | attr.reset() 55 | 56 | def __call__(self, obs): 57 | prev_state = self.state 58 | action, self.state, self.info = self.state(obs, self.info) 59 | if prev_state != self.state: 60 | print("==========================================") 61 | print(f"Entering State: {self.state.__class__.__name__}") 62 | print("==========================================") 63 | if action['frameskip'] == 0: 64 | return self.__call__(obs) 65 | else: 66 | return action 67 | -------------------------------------------------------------------------------- /python/pybullet_planning/README.md: -------------------------------------------------------------------------------- 1 | pybullet_planning 2 | ============================================================== 3 | 4 | Code in this directory is developed on top of https://github.com/yijiangh/pybullet_planning 5 | -------------------------------------------------------------------------------- /python/pybullet_planning/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | ******************************************************************************** 3 | pybullet_planning 4 | ******************************************************************************** 5 | 6 | .. currentmodule:: pybullet_planning 7 | 8 | This library is a suite of utility functions to facilitate robotic planning related research on the `pybullet `_ physics simulation engine. 9 | 10 | .. toctree:: 11 | :maxdepth: 1 12 | 13 | pybullet_planning.interfaces 14 | pybullet_planning.primitives 15 | pybullet_planning.motion_planners 16 | pybullet_planning.utils 17 | 18 | """ 19 | 20 | from .__version__ import __author__, __author_email__, __copyright__, __description__, __license__, \ 21 | __title__, __url__, __version__ 22 | 23 | from .utils import * 24 | from .interfaces import * 25 | from .motion_planners import * 26 | from .primitives import * 27 | 28 | __all__ = ['__author__', '__author_email__', '__copyright__', '__description__', '__license__', '__title__', '__url__', '__version__'] 29 | -------------------------------------------------------------------------------- /python/pybullet_planning/__version__.py: -------------------------------------------------------------------------------- 1 | __title__ = 'pybullet_planning' 2 | __description__ = 'a suite of utility functions to facilitate robotic planning related research on the pybullet physics simulation engine.' 3 | __url__ = 'https://github.com/yijiangh/pybullet_planning' 4 | __version__ = '0.5.0' 5 | __author__ = 'Caelan Garrett' 6 | __author_email__ = 'yijiangh@mit.edu' # this is only for the purpose of pypi... 7 | # __author_email__ = 'caelan@csail.mit.edu; ' 8 | __license__ = 'MIT license' 9 | __copyright__ = 'Copyright 2019 Caelan Garrett' 10 | 11 | __all__ = ['__author__', '__author_email__', '__copyright__', '__description__', '__license__', '__title__', '__url__', '__version__'] 12 | -------------------------------------------------------------------------------- /python/pybullet_planning/interfaces/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | ******************************************************************************** 3 | interfaces 4 | ******************************************************************************** 5 | 6 | .. currentmodule:: pybullet_planning.interfaces 7 | 8 | .. toctree:: 9 | :maxdepth: 2 10 | 11 | pybullet_planning.interfaces.control 12 | pybullet_planning.interfaces.debug_utils 13 | pybullet_planning.interfaces.env_manager 14 | pybullet_planning.interfaces.geometry 15 | pybullet_planning.interfaces.kinematics 16 | pybullet_planning.interfaces.planner_interface 17 | pybullet_planning.interfaces.robots 18 | pybullet_planning.interfaces.task_modeling 19 | 20 | """ 21 | 22 | from __future__ import absolute_import 23 | 24 | from .env_manager import * 25 | from .geometry import * 26 | from .robots import * 27 | from .kinematics import * 28 | from .planner_interface import * 29 | from .task_modeling import * 30 | from .control import * 31 | from .debug_utils import * 32 | 33 | __all__ = [name for name in dir() if not name.startswith('_')] 34 | -------------------------------------------------------------------------------- /python/pybullet_planning/interfaces/control/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | """ 4 | 5 | from .control import * 6 | 7 | __all__ = [name for name in dir() if not name.startswith('_')] 8 | -------------------------------------------------------------------------------- /python/pybullet_planning/interfaces/debug_utils/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | ******************************************************************************** 3 | interfaces.debug_utils 4 | ******************************************************************************** 5 | 6 | .. currentmodule:: pybullet_planning.interfaces.debug_utils 7 | 8 | TODO: module description 9 | 10 | Debug utils 11 | -------------------- 12 | 13 | .. autosummary:: 14 | :toctree: generated/ 15 | :nosignatures: 16 | 17 | add_text 18 | add_line 19 | remove_handles 20 | remove_all_debug 21 | add_body_name 22 | add_segments 23 | draw_link_name 24 | draw_pose 25 | draw_base_limits 26 | draw_circle 27 | draw_aabb 28 | draw_point 29 | 30 | Diagnosis utils 31 | ---------------------- 32 | 33 | .. autosummary:: 34 | :toctree: generated/ 35 | :nosignatures: 36 | 37 | draw_collision_diagnosis 38 | 39 | """ 40 | 41 | from __future__ import absolute_import 42 | 43 | from .debug_utils import * 44 | 45 | __all__ = [name for name in dir() if not name.startswith('_')] 46 | -------------------------------------------------------------------------------- /python/pybullet_planning/interfaces/env_manager/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | ******************************************************************************** 3 | interfaces.env_manager 4 | ******************************************************************************** 5 | 6 | .. currentmodule:: pybullet_planning.interfaces.env_manager 7 | 8 | pose transformation, shape creation, and interacting with the pb environment. 9 | 10 | Basic Geometric Representation 11 | -------------------------------- 12 | 13 | .. autosummary:: 14 | :toctree: generated/ 15 | :nosignatures: 16 | 17 | Point 18 | Pose 19 | Euler 20 | 21 | Conversion functions 22 | -------------------- 23 | 24 | .. autosummary:: 25 | :toctree: generated/ 26 | :nosignatures: 27 | 28 | point_from_pose 29 | quat_from_pose 30 | 31 | Transformation operations 32 | -------------------------------- 33 | 34 | .. autosummary:: 35 | :toctree: generated/ 36 | :nosignatures: 37 | 38 | tform_point 39 | apply_affine 40 | 41 | Create shapes 42 | -------------------------------- 43 | 44 | .. autosummary:: 45 | :toctree: generated/ 46 | :nosignatures: 47 | 48 | create_body 49 | create_box 50 | create_capsule 51 | create_sphere 52 | create_cylinder 53 | create_plane 54 | create_obj 55 | 56 | """ 57 | 58 | from __future__ import absolute_import 59 | 60 | from .pose_transformation import * 61 | from .savers import * 62 | from .shape_creation import * 63 | from .simulation import * 64 | from .user_io import * 65 | 66 | __all__ = [name for name in dir() if not name.startswith('_')] 67 | -------------------------------------------------------------------------------- /python/pybullet_planning/interfaces/env_manager/savers.py: -------------------------------------------------------------------------------- 1 | import os 2 | import pybullet as p 3 | 4 | from pybullet_planning.utils import CLIENT, set_client 5 | 6 | ##################################### 7 | # Savers 8 | # TODO: contextlib 9 | 10 | class Saver(object): 11 | def restore(self): 12 | raise NotImplementedError() 13 | def __enter__(self): 14 | # TODO: move the saving to enter? 15 | pass 16 | def __exit__(self, type, value, traceback): 17 | self.restore() 18 | 19 | class ClientSaver(Saver): 20 | def __init__(self, new_client=None): 21 | self.client = CLIENT 22 | if new_client is not None: 23 | set_client(new_client) 24 | 25 | def restore(self): 26 | set_client(self.client) 27 | 28 | def __repr__(self): 29 | return '{}({})'.format(self.__class__.__name__, self.client) 30 | 31 | class VideoSaver(Saver): 32 | def __init__(self, path): 33 | self.path = path 34 | if path is None: 35 | self.log_id = None 36 | else: 37 | name, ext = os.path.splitext(path) 38 | assert ext == '.mp4' 39 | # STATE_LOGGING_PROFILE_TIMINGS, STATE_LOGGING_ALL_COMMANDS 40 | # p.submitProfileTiming("pythontest") 41 | self.log_id = p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, fileName=path, physicsClientId=CLIENT) 42 | 43 | def restore(self): 44 | if self.log_id is not None: 45 | p.stopStateLogging(self.log_id) 46 | 47 | ##################################### 48 | 49 | class ConfSaver(Saver): 50 | def __init__(self, body): #, joints): 51 | from pybullet_planning.interfaces.robots.joint import get_configuration 52 | self.body = body 53 | self.conf = get_configuration(body) 54 | 55 | def apply_mapping(self, mapping): 56 | self.body = mapping.get(self.body, self.body) 57 | 58 | def restore(self): 59 | from pybullet_planning.interfaces.robots.joint import set_configuration 60 | set_configuration(self.body, self.conf) 61 | 62 | def __repr__(self): 63 | return '{}({})'.format(self.__class__.__name__, self.body) 64 | 65 | class BodySaver(Saver): 66 | def __init__(self, body): #, pose=None): 67 | #if pose is None: 68 | # pose = get_pose(body) 69 | self.body = body 70 | self.pose_saver = PoseSaver(body) 71 | self.conf_saver = ConfSaver(body) 72 | self.savers = [self.pose_saver, self.conf_saver] 73 | # TODO: store velocities 74 | 75 | def apply_mapping(self, mapping): 76 | for saver in self.savers: 77 | saver.apply_mapping(mapping) 78 | 79 | def restore(self): 80 | for saver in self.savers: 81 | saver.restore() 82 | 83 | def __repr__(self): 84 | return '{}({})'.format(self.__class__.__name__, self.body) 85 | 86 | class WorldSaver(Saver): 87 | def __init__(self): 88 | from pybullet_planning.interfaces.robots.body import get_bodies 89 | self.body_savers = [BodySaver(body) for body in get_bodies()] 90 | # TODO: add/remove new bodies 91 | 92 | def restore(self): 93 | for body_saver in self.body_savers: 94 | body_saver.restore() 95 | 96 | class PoseSaver(Saver): 97 | def __init__(self, body): 98 | from pybullet_planning.interfaces.robots.body import get_pose, get_velocity 99 | self.body = body 100 | self.pose = get_pose(self.body) 101 | self.velocity = get_velocity(self.body) 102 | 103 | def apply_mapping(self, mapping): 104 | self.body = mapping.get(self.body, self.body) 105 | 106 | def restore(self): 107 | from pybullet_planning.interfaces.robots.body import set_pose, set_velocity 108 | set_pose(self.body, self.pose) 109 | set_velocity(self.body, *self.velocity) 110 | 111 | def __repr__(self): 112 | return '{}({})'.format(self.__class__.__name__, self.body) 113 | -------------------------------------------------------------------------------- /python/pybullet_planning/interfaces/geometry/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | ******************************************************************************** 3 | interfaces.geometry 4 | ******************************************************************************** 5 | 6 | .. currentmodule:: pybullet_planning.interfaces.geometry 7 | 8 | TODO: module description 9 | 10 | Main Types 11 | -------------- 12 | 13 | .. autosummary:: 14 | :toctree: generated/ 15 | :nosignatures: 16 | 17 | AABB 18 | 19 | Bounding box operations 20 | ----------------------- 21 | 22 | .. currentmodule:: pybullet_planning.interfaces.geometry.bounding_box 23 | 24 | .. autosummary:: 25 | :toctree: generated/ 26 | :nosignatures: 27 | 28 | aabb_from_points 29 | aabb_union 30 | get_bodies_in_region 31 | 32 | """ 33 | 34 | from .mesh import * 35 | from .polygon import * 36 | from .bounding_box import * 37 | from .pointcloud import * 38 | from .camera import * 39 | 40 | __all__ = [name for name in dir() if not name.startswith('_')] 41 | -------------------------------------------------------------------------------- /python/pybullet_planning/interfaces/geometry/pointcloud.py: -------------------------------------------------------------------------------- 1 | 2 | 3 | def read_pcd_file(path): 4 | """ 5 | Reads a *.pcd pointcloud file 6 | :param path: path to the *.pcd pointcloud file 7 | :return: list of points 8 | """ 9 | with open(path) as f: 10 | data = f.readline().split() 11 | num_points = 0 12 | while data[0] != 'DATA': 13 | if data[0] == 'POINTS': 14 | num_points = int(data[1]) 15 | data = f.readline().split() 16 | continue 17 | return [tuple(map(float, f.readline().split())) for _ in range(num_points)] 18 | -------------------------------------------------------------------------------- /python/pybullet_planning/interfaces/kinematics/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | ******************************************************************************** 3 | interfaces.kinematics 4 | ******************************************************************************** 5 | 6 | .. currentmodule:: pybullet_planning.interfaces.kinematics 7 | 8 | Kinematics interface 9 | -------------------- 10 | 11 | .. autosummary:: 12 | :toctree: generated/ 13 | :nosignatures: 14 | 15 | get_ik_tool_link_pose 16 | get_ik_generator 17 | sample_tool_ik 18 | compute_forward_kinematics 19 | compute_inverse_kinematics 20 | select_solution 21 | 22 | """ 23 | 24 | 25 | from .ik_utils import * 26 | from .ik_interface import * 27 | from .reachability import * 28 | 29 | __all__ = [name for name in dir() if not name.startswith('_')] 30 | -------------------------------------------------------------------------------- /python/pybullet_planning/interfaces/kinematics/reachability.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | from pybullet_planning.utils import CIRCULAR_LIMITS 4 | 5 | ##################################### 6 | # Reachability 7 | 8 | def sample_reachable_base(robot, point, reachable_range=(0.25, 1.0)): 9 | from pybullet_planning.interfaces.env_manager.pose_transformation import unit_from_theta, point_from_pose 10 | radius = np.random.uniform(*reachable_range) 11 | x, y = radius*unit_from_theta(np.random.uniform(-np.pi, np.pi)) + point[:2] 12 | yaw = np.random.uniform(*CIRCULAR_LIMITS) 13 | base_values = (x, y, yaw) 14 | #set_base_values(robot, base_values) 15 | return base_values 16 | 17 | def uniform_pose_generator(robot, gripper_pose, **kwargs): 18 | from pybullet_planning.interfaces.env_manager.pose_transformation import point_from_pose 19 | point = point_from_pose(gripper_pose) 20 | while True: 21 | base_values = sample_reachable_base(robot, point, **kwargs) 22 | if base_values is None: 23 | break 24 | yield base_values 25 | #set_base_values(robot, base_values) 26 | #yield get_pose(robot) 27 | -------------------------------------------------------------------------------- /python/pybullet_planning/interfaces/planner_interface/SE2_pose_motion_planning.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | from pybullet_planning.utils import MAX_DISTANCE, CIRCULAR_LIMITS 4 | from pybullet_planning.motion_planners import birrt, direct_path 5 | 6 | ##################################### 7 | # SE(2) pose motion planning 8 | 9 | def get_base_difference_fn(): 10 | from pybullet_planning.interfaces.env_manager.pose_transformation import circular_difference 11 | def fn(q2, q1): 12 | dx, dy = np.array(q2[:2]) - np.array(q1[:2]) 13 | dtheta = circular_difference(q2[2], q1[2]) 14 | return (dx, dy, dtheta) 15 | return fn 16 | 17 | def get_base_distance_fn(weights=1*np.ones(3)): 18 | difference_fn = get_base_difference_fn() 19 | def fn(q1, q2): 20 | difference = np.array(difference_fn(q2, q1)) 21 | return np.sqrt(np.dot(weights, difference * difference)) 22 | return fn 23 | 24 | def plan_base_motion(body, end_conf, base_limits, obstacles=[], direct=False, 25 | weights=1*np.ones(3), resolutions=0.05*np.ones(3), 26 | max_distance=MAX_DISTANCE, **kwargs): 27 | from pybullet_planning.interfaces.robots.collision import pairwise_collision 28 | from pybullet_planning.interfaces.robots.body import set_base_values, get_base_values 29 | 30 | def sample_fn(): 31 | x, y = np.random.uniform(*base_limits) 32 | theta = np.random.uniform(*CIRCULAR_LIMITS) 33 | return (x, y, theta) 34 | 35 | difference_fn = get_base_difference_fn() 36 | distance_fn = get_base_distance_fn(weights=weights) 37 | 38 | def extend_fn(q1, q2): 39 | steps = np.abs(np.divide(difference_fn(q2, q1), resolutions)) 40 | n = int(np.max(steps)) + 1 41 | q = q1 42 | for i in range(n): 43 | q = tuple((1. / (n - i)) * np.array(difference_fn(q2, q)) + q) 44 | yield q 45 | # TODO: should wrap these joints 46 | 47 | def collision_fn(q): 48 | # TODO: update this function 49 | set_base_values(body, q) 50 | return any(pairwise_collision(body, obs, max_distance=max_distance) for obs in obstacles) 51 | 52 | start_conf = get_base_values(body) 53 | if collision_fn(start_conf): 54 | print("Warning: initial configuration is in collision") 55 | return None 56 | if collision_fn(end_conf): 57 | print("Warning: end configuration is in collision") 58 | return None 59 | if direct: 60 | return direct_path(start_conf, end_conf, extend_fn, collision_fn) 61 | return birrt(start_conf, end_conf, distance_fn, 62 | sample_fn, extend_fn, collision_fn, **kwargs) 63 | -------------------------------------------------------------------------------- /python/pybullet_planning/interfaces/planner_interface/__init__.py: -------------------------------------------------------------------------------- 1 | """interfaces to the motion planners 2 | 3 | """ 4 | 5 | from .cartesian_motion_planning import * 6 | from .joint_motion_planning import * 7 | from .nonholonomic_motion_planning import * 8 | from .SE2_pose_motion_planning import * 9 | 10 | __all__ = [name for name in dir() if not name.startswith('_')] 11 | -------------------------------------------------------------------------------- /python/pybullet_planning/interfaces/planner_interface/dag_search.py: -------------------------------------------------------------------------------- 1 | import warnings 2 | 3 | import numpy as np 4 | from .ladder_graph import LadderGraph, EdgeBuilder 5 | 6 | class SolutionRung(object): 7 | def __init__(self): 8 | self.distance = [] 9 | self.predecessor = [] 10 | 11 | def extract_min(self): 12 | # min_dist = min(self.distance) 13 | # min_id = self.distance.index(min_dist) 14 | min_id = np.argmin(self.distance) 15 | min_dist = self.distance[min_id] 16 | return min_dist, min_id 17 | 18 | def __len__(self): 19 | assert(len(self.distance) == len(self.predecessor)) 20 | return len(self.distance) 21 | # 22 | # def __repr__(self): 23 | # return 'min dist: {0}, min pred id: {1}'.format(self.extract_min()) 24 | 25 | class DAGSearch(object): 26 | def __init__(self, graph): 27 | assert(isinstance(graph, LadderGraph)) 28 | if graph.size == 0: 29 | raise ValueError('input ladder graph is empty!') 30 | self.graph = graph 31 | self.solution = [SolutionRung() for i in range(graph.get_rungs_size())] 32 | 33 | # allocate everything we need 34 | for i in range(graph.get_rungs_size()): 35 | n_verts = graph.get_rung_vert_size(i) 36 | assert(n_verts > 0) 37 | self.solution[i].distance = np.zeros(n_verts) 38 | self.solution[i].predecessor = np.zeros(n_verts, dtype=int) 39 | 40 | @classmethod 41 | def from_ladder_graph(cls, graph): 42 | return cls(graph) 43 | 44 | def distance(self, r_id, v_id): 45 | return self.solution[r_id].distance[v_id] 46 | 47 | def predecessor(self, r_id, v_id): 48 | return self.solution[r_id].predecessor[v_id] 49 | 50 | def run(self): 51 | """forward cost propagation""" 52 | if len(self.solution) == 0: 53 | raise ValueError('The initial solution is empty!') 54 | 55 | # TODO: add st_conf cost to SolutionRung 0 56 | # * first rung init to 0 57 | self.solution[0].distance = np.zeros(len(self.solution[0])) 58 | # * other rungs init to inf 59 | for j in range(1, len(self.solution)): 60 | self.solution[j].distance = np.inf*np.ones(len(self.solution[j])) 61 | 62 | for r_id in range(0, len(self.solution)-1): 63 | n_verts = self.graph.get_rung_vert_size(r_id) 64 | next_r_id = r_id + 1 65 | # for each vert in the out edge list 66 | max_rung_cost = np.inf 67 | for v_id in range(n_verts): 68 | u_cost = self.distance(r_id, v_id) 69 | edges = self.graph.get_edges(r_id)[v_id] 70 | dv = np.inf 71 | for edge in edges: 72 | dv = u_cost + edge.cost 73 | if dv < self.distance(next_r_id, edge.idx): 74 | self.solution[next_r_id].distance[edge.idx] = dv 75 | self.solution[next_r_id].predecessor[edge.idx] = v_id 76 | if dv < max_rung_cost: 77 | max_rung_cost = dv 78 | # print('Rung #{}: {}'.format(r_id, max_rung_cost)) 79 | 80 | return min(self.solution[-1].distance) 81 | 82 | def shortest_path(self): 83 | if len(self.solution) == 0: 84 | # TODO: more detailed checks 85 | raise ValueError('The initial solution is empty!') 86 | 87 | _, min_val_id = self.solution[-1].extract_min() 88 | path_idx = np.zeros(len(self.solution), dtype=int) 89 | 90 | current_v_id = min_val_id 91 | count = len(path_idx) - 1 92 | while count >= 0: 93 | path_idx[count] = current_v_id 94 | current_v_id = self.predecessor(count, current_v_id) 95 | count -= 1 96 | 97 | sol = [] 98 | for r_id, v_id in enumerate(path_idx): 99 | data = self.graph.get_vert_data(r_id, v_id) 100 | sol.append(data) 101 | 102 | return sol 103 | -------------------------------------------------------------------------------- /python/pybullet_planning/interfaces/planner_interface/nonholonomic_motion_planning.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | from pybullet_planning.utils import PI, MAX_DISTANCE 4 | 5 | from pybullet_planning.motion_planners import birrt 6 | 7 | from pybullet_planning.interfaces.robots.collision import get_collision_fn 8 | from pybullet_planning.interfaces.planner_interface.joint_motion_planning import get_distance_fn, get_extend_fn, get_sample_fn, check_initial_end 9 | 10 | ##################################### 11 | 12 | def get_closest_angle_fn(body, joints, linear_weight=1., angular_weight=1., reversible=True): 13 | from pybullet_planning.interfaces.env_manager.pose_transformation import get_angle 14 | assert len(joints) == 3 15 | linear_extend_fn = get_distance_fn(body, joints[:2], weights=linear_weight*np.ones(2)) 16 | angular_extend_fn = get_distance_fn(body, joints[2:], weights=[angular_weight]) 17 | 18 | def closest_fn(q1, q2): 19 | angle_and_distance = [] 20 | for direction in [0, PI] if reversible else [PI]: 21 | angle = get_angle(q1[:2], q2[:2]) + direction 22 | distance = angular_extend_fn(q1[2:], [angle]) \ 23 | + linear_extend_fn(q1[:2], q2[:2]) \ 24 | + angular_extend_fn([angle], q2[2:]) 25 | angle_and_distance.append((angle, distance)) 26 | return min(angle_and_distance, key=lambda pair: pair[1]) 27 | return closest_fn 28 | 29 | def get_nonholonomic_distance_fn(body, joints, weights=None, **kwargs): 30 | assert weights is None 31 | closest_angle_fn = get_closest_angle_fn(body, joints, **kwargs) 32 | 33 | def distance_fn(q1, q2): 34 | _, distance = closest_angle_fn(q1, q2) 35 | return distance 36 | return distance_fn 37 | 38 | def get_nonholonomic_extend_fn(body, joints, resolutions=None, **kwargs): 39 | assert resolutions is None 40 | assert len(joints) == 3 41 | linear_extend_fn = get_extend_fn(body, joints[:2]) 42 | angular_extend_fn = get_extend_fn(body, joints[2:]) 43 | closest_angle_fn = get_closest_angle_fn(body, joints, **kwargs) 44 | 45 | def extend_fn(q1, q2): 46 | angle, _ = closest_angle_fn(q1, q2) 47 | path = [] 48 | for aq in angular_extend_fn(q1[2:], [angle]): 49 | path.append(np.append(q1[:2], aq)) 50 | for lq in linear_extend_fn(q1[:2], q2[:2]): 51 | path.append(np.append(lq, [angle])) 52 | for aq in angular_extend_fn([angle], q2[2:]): 53 | path.append(np.append(q2[:2], aq)) 54 | return path 55 | return extend_fn 56 | 57 | def plan_nonholonomic_motion(body, joints, end_conf, obstacles=[], attachments=[], 58 | self_collisions=True, disabled_collisions=set(), 59 | weights=None, resolutions=None, reversible=True, 60 | max_distance=MAX_DISTANCE, custom_limits={}, **kwargs): 61 | from pybullet_planning.interfaces.robots.joint import get_joint_positions 62 | 63 | assert len(joints) == len(end_conf) 64 | sample_fn = get_sample_fn(body, joints, custom_limits=custom_limits) 65 | distance_fn = get_nonholonomic_distance_fn(body, joints, weights=weights, reversible=reversible) 66 | extend_fn = get_nonholonomic_extend_fn(body, joints, resolutions=resolutions, reversible=reversible) 67 | collision_fn = get_collision_fn(body, joints, obstacles, attachments, 68 | self_collisions, disabled_collisions, 69 | custom_limits=custom_limits, max_distance=max_distance) 70 | 71 | start_conf = get_joint_positions(body, joints) 72 | if not check_initial_end(start_conf, end_conf, collision_fn): 73 | return None 74 | return birrt(start_conf, end_conf, distance_fn, sample_fn, extend_fn, collision_fn, **kwargs) 75 | -------------------------------------------------------------------------------- /python/pybullet_planning/interfaces/planner_interface/utils.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import functools 3 | from pybullet_planning.interfaces.robots.joint import get_joint_positions, get_joint_velocities, set_joint_positions_and_velocities 4 | from pybullet_planning.interfaces.robots.body import get_pose, get_velocity, set_pose, set_velocity 5 | 6 | 7 | def preserve_pos_and_vel(func): 8 | @functools.wraps(func) 9 | def wrapper_preserve_pos_and_vel(body, joints, *args, **kwargs): 10 | # save initial pose and velocities 11 | joint_pos = get_joint_positions(body, joints) 12 | joint_vel = get_joint_velocities(body, joints) 13 | 14 | ret = func(body, joints, *args, **kwargs) 15 | 16 | # put positions and velocities back! 17 | set_joint_positions_and_velocities(body, joints, joint_pos, joint_vel) 18 | return ret 19 | return wrapper_preserve_pos_and_vel 20 | 21 | 22 | def preserve_pos_and_vel_wholebody(func): 23 | @functools.wraps(func) 24 | def wrapper_preserve_pos_and_vel_wholebody(cube_body, joints, finger_body, finger_joints, *args, **kwargs): 25 | # save initial pose and velocities 26 | cube_pose = get_pose(cube_body) 27 | cube_vel = get_velocity(cube_body) 28 | joint_pos = get_joint_positions(finger_body, finger_joints) 29 | joint_vel = get_joint_velocities(finger_body, finger_joints) 30 | 31 | ret = func(cube_body, joints, finger_body, finger_joints, *args, **kwargs) 32 | 33 | # put positions and velocities back! 34 | set_pose(cube_body, cube_pose) 35 | set_velocity(cube_body, linear=cube_vel[0], angular=cube_vel[1]) 36 | set_joint_positions_and_velocities(finger_body, finger_joints, joint_pos, joint_vel) 37 | return ret 38 | return wrapper_preserve_pos_and_vel_wholebody 39 | 40 | -------------------------------------------------------------------------------- /python/pybullet_planning/interfaces/robots/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | ******************************************************************************** 3 | interfaces.robots 4 | ******************************************************************************** 5 | 6 | .. currentmodule:: pybullet_planning.interfaces.robots 7 | 8 | TODO: module description 9 | 10 | Body 11 | -------------- 12 | 13 | .. autosummary:: 14 | :toctree: generated/ 15 | :nosignatures: 16 | 17 | vertices_from_rigid 18 | 19 | Collision checking 20 | -------------------- 21 | 22 | .. autosummary:: 23 | :toctree: generated/ 24 | :nosignatures: 25 | 26 | get_collision_fn 27 | get_floating_body_collision_fn 28 | 29 | Body Approximation 30 | ------------------- 31 | 32 | .. autosummary:: 33 | :toctree: generated/ 34 | :nosignatures: 35 | 36 | approximate_as_prism 37 | approximate_as_cylinder 38 | 39 | Dynamics 40 | ------------------- 41 | 42 | .. autosummary:: 43 | :toctree: generated/ 44 | :nosignatures: 45 | 46 | DynamicsInfo 47 | set_static 48 | 49 | Verbose 50 | ------------------- 51 | 52 | .. autosummary:: 53 | :toctree: generated/ 54 | :nosignatures: 55 | 56 | dump_body 57 | dump_world 58 | 59 | """ 60 | 61 | from .joint import * 62 | from .link import * 63 | from .dynamics import * 64 | from .body import * 65 | from .collision import * 66 | 67 | __all__ = [name for name in dir() if not name.startswith('_')] 68 | -------------------------------------------------------------------------------- /python/pybullet_planning/interfaces/robots/dynamics.py: -------------------------------------------------------------------------------- 1 | 2 | from collections import defaultdict, deque, namedtuple 3 | 4 | import numpy as np 5 | import pybullet as p 6 | 7 | from pybullet_planning.utils import CLIENT, BASE_LINK, STATIC_MASS 8 | 9 | ##################################### 10 | # https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/edit# 11 | DynamicsInfo = namedtuple('DynamicsInfo', ['mass', 'lateral_friction', 12 | 'local_inertia_diagonal', 'local_inertial_pos', 'local_inertial_orn', 13 | 'restitution', 'rolling_friction', 'spinning_friction', 14 | 'contact_damping', 'contact_stiffness', 'body_type', 'collision_margin']) 15 | 16 | def get_dynamics_info(body, link=BASE_LINK): 17 | return DynamicsInfo(*p.getDynamicsInfo(body, link, physicsClientId=CLIENT)) 18 | 19 | get_link_info = get_dynamics_info 20 | 21 | def get_mass(body, link=BASE_LINK): 22 | # TOOD: get full mass 23 | return get_dynamics_info(body, link).mass 24 | 25 | def set_dynamics(body, link=BASE_LINK, **kwargs): 26 | # TODO: iterate over all links 27 | p.changeDynamics(body, link, physicsClientId=CLIENT, **kwargs) 28 | 29 | def set_mass(body, mass, link=BASE_LINK): 30 | set_dynamics(body, link=link, mass=mass) 31 | 32 | def set_static(body): 33 | """set all the body's links to be static (infinite mass, doesn't move under gravity) 34 | 35 | Parameters 36 | ---------- 37 | body : int 38 | [description] 39 | """ 40 | from pybullet_planning.interfaces.robots.link import get_all_links 41 | for link in get_all_links(body): 42 | set_mass(body, mass=STATIC_MASS, link=link) 43 | 44 | def set_all_static(): 45 | from pybullet_planning.interfaces.env_manager.simulation import disable_gravity 46 | from pybullet_planning.interfaces.robots.body import get_bodies 47 | # TODO: mass saver 48 | disable_gravity() 49 | for body in get_bodies(): 50 | set_static(body) 51 | 52 | def get_joint_inertial_pose(body, joint): 53 | dynamics_info = get_dynamics_info(body, joint) 54 | return dynamics_info.local_inertial_pos, dynamics_info.local_inertial_orn 55 | 56 | def get_local_link_pose(body, joint): 57 | from pybullet_planning.interfaces.env_manager.pose_transformation import Pose, multiply, invert 58 | from pybullet_planning.interfaces.robots.joint import get_joint_parent_frame 59 | from pybullet_planning.interfaces.robots.link import parent_link_from_joint 60 | 61 | parent_joint = parent_link_from_joint(body, joint) 62 | #world_child = get_link_pose(body, joint) 63 | #world_parent = get_link_pose(body, parent_joint) 64 | ##return multiply(invert(world_parent), world_child) 65 | #return multiply(world_child, invert(world_parent)) 66 | 67 | # https://github.com/bulletphysics/bullet3/blob/9c9ac6cba8118544808889664326fd6f06d9eeba/examples/pybullet/gym/pybullet_utils/urdfEditor.py#L169 68 | parent_com = get_joint_parent_frame(body, joint) 69 | tmp_pose = invert(multiply(get_joint_inertial_pose(body, joint), parent_com)) 70 | parent_inertia = get_joint_inertial_pose(body, parent_joint) 71 | #return multiply(parent_inertia, tmp_pose) # TODO: why is this wrong... 72 | _, orn = multiply(parent_inertia, tmp_pose) 73 | pos, _ = multiply(parent_inertia, Pose(parent_com[0])) 74 | return (pos, orn) 75 | -------------------------------------------------------------------------------- /python/pybullet_planning/interfaces/task_modeling/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | ******************************************************************************** 3 | interfaces.task_modeling 4 | ******************************************************************************** 5 | 6 | .. currentmodule:: pybullet_planning.interfaces.task_modeling 7 | 8 | TODO: module description 9 | 10 | Attachment 11 | -------------------- 12 | 13 | .. autosummary:: 14 | :toctree: generated/ 15 | :nosignatures: 16 | 17 | Attachment 18 | create_attachment 19 | 20 | Grasp 21 | -------------------- 22 | 23 | .. autosummary:: 24 | :toctree: generated/ 25 | :nosignatures: 26 | 27 | GraspInfo 28 | body_from_end_effector 29 | end_effector_from_body 30 | approach_from_grasp 31 | get_grasp_pose 32 | 33 | """ 34 | 35 | from .constraint import * 36 | from .grasp import * 37 | from .path_interpolation import * 38 | from .placement import * 39 | 40 | __all__ = [name for name in dir() if not name.startswith('_')] 41 | -------------------------------------------------------------------------------- /python/pybullet_planning/interfaces/task_modeling/constraint.py: -------------------------------------------------------------------------------- 1 | 2 | from collections import namedtuple 3 | import pybullet as p 4 | 5 | from pybullet_planning.utils import CLIENT, BASE_LINK 6 | 7 | ##################################### 8 | # Constraints - applies forces when not satisfied 9 | 10 | def get_constraints(): 11 | """ 12 | getConstraintUniqueId will take a serial index in range 0..getNumConstraints, and reports the constraint unique id. 13 | Note that the constraint unique ids may not be contiguous, since you may remove constraints. 14 | """ 15 | return [p.getConstraintUniqueId(i, physicsClientId=CLIENT) 16 | for i in range(p.getNumConstraints(physicsClientId=CLIENT))] 17 | 18 | def remove_constraint(constraint): 19 | p.removeConstraint(constraint, physicsClientId=CLIENT) 20 | 21 | ConstraintInfo = namedtuple('ConstraintInfo', ['parentBodyUniqueId', 'parentJointIndex', 22 | 'childBodyUniqueId', 'childLinkIndex', 'constraintType', 23 | 'jointAxis', 'jointPivotInParent', 'jointPivotInChild', 24 | 'jointFrameOrientationParent', 'jointFrameOrientationChild', 'maxAppliedForce']) 25 | 26 | def get_constraint_info(constraint): # getConstraintState 27 | # TODO: four additional arguments 28 | return ConstraintInfo(*p.getConstraintInfo(constraint, physicsClientId=CLIENT)[:11]) 29 | 30 | def get_fixed_constraints(): 31 | fixed_constraints = [] 32 | for constraint in get_constraints(): 33 | constraint_info = get_constraint_info(constraint) 34 | if constraint_info.constraintType == p.JOINT_FIXED: 35 | fixed_constraints.append(constraint) 36 | return fixed_constraints 37 | 38 | def add_fixed_constraint(body, robot, robot_link, max_force=None): 39 | from pybullet_planning.interfaces.env_manager.pose_transformation import get_pose, unit_point, unit_quat, multiply, invert 40 | from pybullet_planning.interfaces.robots import get_com_pose 41 | 42 | body_link = BASE_LINK 43 | body_pose = get_pose(body) 44 | #body_pose = get_com_pose(body, link=body_link) 45 | #end_effector_pose = get_link_pose(robot, robot_link) 46 | end_effector_pose = get_com_pose(robot, robot_link) 47 | grasp_pose = multiply(invert(end_effector_pose), body_pose) 48 | point, quat = grasp_pose 49 | # TODO: can I do this when I'm not adjacent? 50 | # joint axis in local frame (ignored for JOINT_FIXED) 51 | #return p.createConstraint(robot, robot_link, body, body_link, 52 | # p.JOINT_FIXED, jointAxis=unit_point(), 53 | # parentFramePosition=unit_point(), 54 | # childFramePosition=point, 55 | # parentFrameOrientation=unit_quat(), 56 | # childFrameOrientation=quat) 57 | constraint = p.createConstraint(robot, robot_link, body, body_link, # Both seem to work 58 | p.JOINT_FIXED, jointAxis=unit_point(), 59 | parentFramePosition=point, 60 | childFramePosition=unit_point(), 61 | parentFrameOrientation=quat, 62 | childFrameOrientation=unit_quat(), 63 | physicsClientId=CLIENT) 64 | if max_force is not None: 65 | p.changeConstraint(constraint, maxForce=max_force, physicsClientId=CLIENT) 66 | return constraint 67 | 68 | def remove_fixed_constraint(body, robot, robot_link): 69 | for constraint in get_fixed_constraints(): 70 | constraint_info = get_constraint_info(constraint) 71 | if (body == constraint_info.childBodyUniqueId) and \ 72 | (BASE_LINK == constraint_info.childLinkIndex) and \ 73 | (robot == constraint_info.parentBodyUniqueId) and \ 74 | (robot_link == constraint_info.parentJointIndex): 75 | remove_constraint(constraint) 76 | -------------------------------------------------------------------------------- /python/pybullet_planning/interfaces/task_modeling/path_interpolation.py: -------------------------------------------------------------------------------- 1 | 2 | import math 3 | import numpy as np 4 | import pybullet as p 5 | 6 | from pybullet_planning.interfaces.env_manager.pose_transformation import get_length, get_unit_vector, quat_angle_between, get_distance 7 | 8 | ##################################### 9 | 10 | def get_position_waypoints(start_point, direction, quat, step_size=0.01): 11 | distance = get_length(direction) 12 | unit_direction = get_unit_vector(direction) 13 | for t in np.arange(0, distance, step_size): 14 | point = start_point + t*unit_direction 15 | yield (point, quat) 16 | yield (start_point + direction, quat) 17 | 18 | def get_quaternion_waypoints(point, start_quat, end_quat, step_size=np.pi/16): 19 | angle = quat_angle_between(start_quat, end_quat) 20 | for t in np.arange(0, angle, step_size): 21 | fraction = t/angle 22 | quat = p.getQuaternionSlerp(start_quat, end_quat, interpolationFraction=fraction) 23 | #quat = quaternion_slerp(start_quat, end_quat, fraction=fraction) 24 | yield (point, quat) 25 | yield (point, end_quat) 26 | 27 | def interpolate_poses(pose1, pose2, pos_step_size=0.01, ori_step_size=np.pi/16): 28 | pos1, quat1 = pose1 29 | pos2, quat2 = pose2 30 | num_steps = int(math.ceil(max(get_distance(pos1, pos2)/pos_step_size, 31 | quat_angle_between(quat1, quat2)/ori_step_size))) 32 | for i in range(num_steps): 33 | fraction = float(i) / num_steps 34 | pos = (1-fraction)*np.array(pos1) + fraction*np.array(pos2) 35 | quat = p.getQuaternionSlerp(quat1, quat2, interpolationFraction=fraction) 36 | #quat = quaternion_slerp(quat1, quat2, fraction=fraction) 37 | yield (pos, quat) 38 | yield pose2 39 | 40 | def interpolate_poses_by_num_steps(pose1, pose2, num_steps=5): 41 | pos1, quat1 = pose1 42 | pos2, quat2 = pose2 43 | for i in range(num_steps): 44 | fraction = float(i) / num_steps 45 | pos = (1-fraction)*np.array(pos1) + fraction*np.array(pos2) 46 | quat = p.getQuaternionSlerp(quat1, quat2, interpolationFraction=fraction) 47 | #quat = quaternion_slerp(quat1, quat2, fraction=fraction) 48 | yield (pos, quat) 49 | yield pose2 50 | 51 | # def workspace_trajectory(robot, link, start_point, direction, quat, **kwargs): 52 | # # TODO: pushing example 53 | # # TODO: just use current configuration? 54 | # # TODO: check collisions? 55 | # # TODO: lower intermediate tolerance 56 | # traj = [] 57 | # for pose in get_cartesian_waypoints(start_point, direction, quat): 58 | # conf = inverse_kinematics(robot, link, pose, **kwargs) 59 | # if conf is None: 60 | # return None 61 | # traj.append(conf) 62 | # return traj 63 | -------------------------------------------------------------------------------- /python/pybullet_planning/interfaces/task_modeling/placement.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import pybullet as p 3 | 4 | from pybullet_planning.utils import CIRCULAR_LIMITS 5 | from pybullet_planning.interfaces.env_manager.pose_transformation import Euler, Pose, unit_pose, multiply, set_pose, get_pose 6 | from pybullet_planning.interfaces.geometry.bounding_box import get_center_extent, get_aabb, aabb_contains_aabb, aabb2d_from_aabb, \ 7 | aabb_contains_point 8 | 9 | from pybullet_planning.interfaces.robots.body import get_point 10 | 11 | ##################################### 12 | # Placements 13 | 14 | def stable_z_on_aabb(body, aabb): 15 | center, extent = get_center_extent(body) 16 | _, upper = aabb 17 | return (upper + extent/2 + (get_point(body) - center))[2] 18 | 19 | def stable_z(body, surface, surface_link=None): 20 | return stable_z_on_aabb(body, get_aabb(surface, link=surface_link)) 21 | 22 | def is_placed_on_aabb(body, bottom_aabb, above_epsilon=1e-2, below_epsilon=0.0): 23 | assert (0 <= above_epsilon) and (0 <= below_epsilon) 24 | top_aabb = get_aabb(body) # TODO: approximate_as_prism 25 | top_z_min = top_aabb[0][2] 26 | bottom_z_max = bottom_aabb[1][2] 27 | return ((bottom_z_max - below_epsilon) <= top_z_min <= (bottom_z_max + above_epsilon)) and \ 28 | (aabb_contains_aabb(aabb2d_from_aabb(top_aabb), aabb2d_from_aabb(bottom_aabb))) 29 | 30 | def is_placement(body, surface, **kwargs): 31 | return is_placed_on_aabb(body, get_aabb(surface), **kwargs) 32 | 33 | def is_center_on_aabb(body, bottom_aabb, above_epsilon=1e-2, below_epsilon=0.0): 34 | # TODO: compute AABB in origin 35 | # TODO: use center of mass? 36 | assert (0 <= above_epsilon) and (0 <= below_epsilon) 37 | center, extent = get_center_extent(body) # TODO: approximate_as_prism 38 | base_center = center - np.array([0, 0, extent[2]])/2 39 | top_z_min = base_center[2] 40 | bottom_z_max = bottom_aabb[1][2] 41 | return ((bottom_z_max - abs(below_epsilon)) <= top_z_min <= (bottom_z_max + abs(above_epsilon))) and \ 42 | (aabb_contains_point(base_center[:2], aabb2d_from_aabb(bottom_aabb))) 43 | 44 | def is_center_stable(body, surface, **kwargs): 45 | return is_center_on_aabb(body, get_aabb(surface), **kwargs) 46 | 47 | def sample_placement_on_aabb(top_body, bottom_aabb, top_pose=unit_pose(), 48 | percent=1.0, max_attempts=50, epsilon=1e-3): 49 | # TODO: transform into the coordinate system of the bottom 50 | # TODO: maybe I should instead just require that already in correct frame 51 | for _ in range(max_attempts): 52 | theta = np.random.uniform(*CIRCULAR_LIMITS) 53 | rotation = Euler(yaw=theta) 54 | set_pose(top_body, multiply(Pose(euler=rotation), top_pose)) 55 | center, extent = get_center_extent(top_body) 56 | lower = (np.array(bottom_aabb[0]) + percent*extent/2)[:2] 57 | upper = (np.array(bottom_aabb[1]) - percent*extent/2)[:2] 58 | if np.less(upper, lower).any(): 59 | continue 60 | x, y = np.random.uniform(lower, upper) 61 | z = (bottom_aabb[1] + extent/2.)[2] + epsilon 62 | point = np.array([x, y, z]) + (get_point(top_body) - center) 63 | pose = multiply(Pose(point, rotation), top_pose) 64 | set_pose(top_body, pose) 65 | return pose 66 | return None 67 | 68 | def sample_placement(top_body, bottom_body, bottom_link=None, **kwargs): 69 | bottom_aabb = get_aabb(bottom_body, link=bottom_link) 70 | return sample_placement_on_aabb(top_body, bottom_aabb, **kwargs) 71 | -------------------------------------------------------------------------------- /python/pybullet_planning/motion_planners/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | ******************************************************************************** 3 | motion_planners 4 | ******************************************************************************** 5 | 6 | .. currentmodule:: pybullet_planning.motion_planners 7 | 8 | Python implementations of several robotic motion planners 9 | 10 | Sampling-based: 11 | 12 | - Probabilistic Roadmap (PRM) 13 | - Rapidly-Exploring Random Tree (RRT) 14 | - RRT-Connect (BiRRT) 15 | - Linear Shortcutting 16 | - MultiRRT 17 | - RRT* 18 | 19 | Grid search: 20 | 21 | - Breadth-First Search (BFS) 22 | - A* 23 | - Ladder graph DAG Search 24 | 25 | Probabilistic Roadmap (PRM) 26 | ---------------------------- 27 | 28 | .. currentmodule:: pybullet_planning.motion_planners.lazy_prm 29 | 30 | .. autosummary:: 31 | :toctree: generated/ 32 | :nosignatures: 33 | 34 | lazy_prm 35 | 36 | RRT-Connect (BiRRT) 37 | ---------------------------- 38 | 39 | .. currentmodule:: pybullet_planning.motion_planners.rrt_connect 40 | 41 | .. autosummary:: 42 | :toctree: generated/ 43 | :nosignatures: 44 | 45 | rrt_connect 46 | birrt 47 | direct_path 48 | 49 | Ladder graph search 50 | --------------------------- 51 | 52 | TODO 53 | 54 | """ 55 | 56 | from .rrt_connect import * 57 | from .wholebody_rrt_connect import * 58 | from .lazy_prm import * 59 | 60 | __all__ = [name for name in dir() if not name.startswith('_')] 61 | -------------------------------------------------------------------------------- /python/pybullet_planning/motion_planners/discrete.py: -------------------------------------------------------------------------------- 1 | from collections import deque 2 | from heapq import heappop, heappush 3 | 4 | from recordclass import recordclass 5 | import numpy as np 6 | 7 | from .utils import INF 8 | 9 | Node = recordclass('Node', ['g', 'parent']) 10 | 11 | 12 | def retrace(visited, q): 13 | if q is None: 14 | return [] 15 | return retrace(visited, visited[tuple(q)].parent) + [q] 16 | 17 | 18 | def bfs(start, goal, neighbors, collision, max_iterations=INF): 19 | if collision(start) or collision(goal): 20 | return None 21 | iterations = 0 22 | visited = {tuple(start): Node(0, None)} 23 | expanded = [] 24 | queue = deque([start]) 25 | while len(queue) != 0 and iterations < max_iterations: 26 | current = queue.popleft() 27 | iterations += 1 28 | expanded.append(current) 29 | if goal is not None and tuple(current) == tuple(goal): 30 | return retrace(visited, current) 31 | for next in neighbors(current): 32 | # TODO - make edges for real (and store bad edges) 33 | if tuple(next) not in visited and not collision(next): 34 | visited[tuple(next)] = Node( 35 | next, visited[tuple(current)].g + 1, current) 36 | queue.append(next) 37 | return None 38 | 39 | 40 | def astar(start, goal, distance, neighbors, collision, 41 | max_iterations=INF, cost=lambda g, h: g + h): # TODO - put start and goal in neighbors 42 | if collision(start) or collision(goal): 43 | return None 44 | queue = [(cost(0, distance(start, goal)), 0, start)] 45 | visited = {tuple(start): Node(0, None)} 46 | iterations = 0 47 | while len(queue) != 0 and iterations < max_iterations: 48 | _, current_g, current = heappop(queue) 49 | current = np.array(current) 50 | if visited[tuple(current)].g != current_g: 51 | continue 52 | iterations += 1 53 | if tuple(current) == tuple(goal): 54 | return retrace(visited, current) 55 | for next in neighbors(current): 56 | next_g = current_g + distance(current, next) 57 | if (tuple(next) not in visited or next_g < visited[tuple(next)].g) and not collision(next): 58 | visited[tuple(next)] = Node(next_g, current) 59 | # ValueError: The truth value of an array with more than one 60 | # element is ambiguous. 61 | heappush(queue, (cost(next_g, distance(next, goal)), next_g, next)) 62 | return None 63 | -------------------------------------------------------------------------------- /python/pybullet_planning/motion_planners/graph.py: -------------------------------------------------------------------------------- 1 | from collections import namedtuple, Mapping 2 | from heapq import heappush, heappop 3 | 4 | 5 | class Vertex(object): 6 | 7 | def __init__(self, value): 8 | self.value = value 9 | self.edges = [] 10 | 11 | def __repr__(self): 12 | return self.__class__.__name__ + '(' + str(self.value) + ')' 13 | 14 | 15 | class Edge(object): 16 | 17 | def __init__(self, v1, v2, value, cost): 18 | self.v1, self.v2 = v1, v2 19 | self.v1.edges.append(self) 20 | self.value = value 21 | self.cost = cost 22 | 23 | def __repr__(self): 24 | return self.__class__.__name__ + '(' + str(self.v1.value) + ' -> ' + str(self.v2.value) + ')' 25 | 26 | SearchNode = namedtuple('SearchNode', ['cost', 'edge']) 27 | 28 | 29 | class Graph(Mapping, object): 30 | 31 | def __init__(self): 32 | self.vertices = {} 33 | self.edges = [] 34 | 35 | def __getitem__(self, value): 36 | return self.vertices[value] 37 | 38 | def __len__(self): 39 | return len(self.vertices) 40 | 41 | def __iter__(self): 42 | return iter(self.vertices) 43 | 44 | def __call__(self, value1, value2): # TODO - goal_test 45 | if value1 not in self or value2 not in self: 46 | return None 47 | 48 | start, goal = self[value1], self[value2] 49 | queue = [(0, start)] 50 | nodes, processed = {start: SearchNode(0, None)}, set() 51 | 52 | def retrace(v): 53 | edge = nodes[v].edge 54 | if edge is None: 55 | return [v.value], [] 56 | vertices, edges = retrace(edge.v1) 57 | return vertices + [v.value], edges + [edge.value] 58 | 59 | while len(queue) != 0: 60 | _, cv = heappop(queue) 61 | if cv in processed: 62 | continue 63 | processed.add(cv) 64 | if cv == goal: 65 | return retrace(cv) 66 | for edge in cv.edges: 67 | cost = nodes[cv].cost + edge.cost 68 | if edge.v2 not in nodes or cost < nodes[edge.v2].cost: 69 | nodes[edge.v2] = SearchNode(cost, edge) 70 | heappush(queue, (cost, edge.v2)) 71 | return None 72 | 73 | def add(self, value): 74 | if value not in self: 75 | self.vertices[value] = Vertex(value) 76 | return self.vertices[value] 77 | 78 | def connect(self, value1, value2, edge_value=None, edge_cost=1): 79 | v1, v2 = self.add(value1), self.add(value2) 80 | edge = Edge(v1, v2, edge_value, edge_cost) 81 | self.edges.append(edge) 82 | return edge 83 | -------------------------------------------------------------------------------- /python/pybullet_planning/motion_planners/rrt.py: -------------------------------------------------------------------------------- 1 | from random import random 2 | 3 | from .utils import irange, argmin, RRT_ITERATIONS 4 | 5 | 6 | class TreeNode(object): 7 | 8 | def __init__(self, config, parent=None, ik_solution=None, group=None): 9 | self.config = config 10 | self.parent = parent 11 | self.ik_solution = ik_solution 12 | if group is None: 13 | self.group = [self] # By default, itself belongs to the group without any other nodes 14 | else: 15 | self.group = group 16 | 17 | #def retrace(self): 18 | # if self.parent is None: 19 | # return [self] 20 | # return self.parent.retrace() + [self] 21 | 22 | def retrace(self): 23 | sequence = [] 24 | node = self 25 | while node is not None: 26 | sequence.append(node) 27 | node = node.parent 28 | return sequence[::-1] 29 | 30 | def retrace_all(self): 31 | sequence = [] 32 | joint_conf_sequence = [] 33 | node = self 34 | while node is not None: 35 | sequence.append(node) 36 | joint_conf_sequence.append(node.ik_solution) 37 | node = node.parent 38 | return sequence[::-1], joint_conf_sequence[::-1] 39 | 40 | def clear(self): 41 | self.node_handle = None 42 | self.edge_handle = None 43 | 44 | def draw(self, env, color=(1, 0, 0, .5)): 45 | from manipulation.primitives.display import draw_node, draw_edge 46 | self.node_handle = draw_node(env, self.config, color=color) 47 | if self.parent is not None: 48 | self.edge_handle = draw_edge( 49 | env, self.config, self.parent.config, color=color) 50 | 51 | def __str__(self): 52 | return 'TreeNode(' + str(self.config) + ')' 53 | __repr__ = __str__ 54 | 55 | 56 | def configs(nodes): 57 | if nodes is None: 58 | return None 59 | return list(map(lambda n: n.config, nodes)) 60 | 61 | def extract_ik_solutions(nodes): 62 | if nodes is None: 63 | return None 64 | return list(map(lambda n: n.ik_solution, nodes)) 65 | 66 | def rrt(start, goal_sample, distance, sample, extend, collision, goal_test=lambda q: False, iterations=RRT_ITERATIONS, goal_probability=.2): 67 | if collision(start): 68 | return None 69 | if not callable(goal_sample): 70 | g = goal_sample 71 | goal_sample = lambda: g 72 | nodes = [TreeNode(start)] 73 | for i in irange(iterations): 74 | goal = random() < goal_probability or i == 0 75 | s = goal_sample() if goal else sample() 76 | 77 | last = argmin(lambda n: distance(n.config, s), nodes) 78 | for q in extend(last.config, s): 79 | if collision(q): 80 | break 81 | last = TreeNode(q, parent=last) 82 | nodes.append(last) 83 | if goal_test(last.config): 84 | return configs(last.retrace()) 85 | else: 86 | if goal: 87 | return configs(last.retrace()) 88 | return None 89 | -------------------------------------------------------------------------------- /python/pybullet_planning/motion_planners/smoothing.py: -------------------------------------------------------------------------------- 1 | from random import randint 2 | 3 | 4 | def smooth_path(path, extend, collision, iterations=200): 5 | """smooth a trajectory path, randomly replace jigged subpath with shortcuts 6 | 7 | Parameters 8 | ---------- 9 | path : list 10 | [description] 11 | extend : function 12 | [description] 13 | collision : function 14 | [description] 15 | iterations : int, optional 16 | number of iterations for the random smoothing procedure, by default 50 17 | 18 | Returns 19 | ------- 20 | [type] 21 | [description] 22 | """ 23 | smoothed_path = path 24 | for _ in range(iterations): 25 | if len(smoothed_path) <= 2: 26 | return smoothed_path 27 | i = randint(0, len(smoothed_path) - 1) 28 | j = randint(0, len(smoothed_path) - 1) 29 | if abs(i - j) <= 1: 30 | continue 31 | if j < i: 32 | i, j = j, i 33 | shortcut = list(extend(smoothed_path[i], smoothed_path[j])) 34 | if (len(shortcut) < (j - i)) and all(not collision(q) for q in shortcut): 35 | smoothed_path = smoothed_path[:i + 1] + shortcut + smoothed_path[j + 1:] 36 | return smoothed_path 37 | 38 | # TODO: sparsify path to just waypoints 39 | 40 | 41 | def wholebody_smooth_path(path, joint_conf_path, extend, collision, ik, calc_tippos_fn, sample_joint_conf_fn, iterations=50): 42 | import functools 43 | from pybullet_planning.interfaces.kinematics.ik_utils import sample_multiple_ik_with_collision 44 | smoothed_path = path 45 | smoothed_jconf_path = joint_conf_path 46 | for _ in range(iterations): 47 | if len(smoothed_path) <= 2: 48 | return smoothed_path 49 | 50 | i = randint(0, len(smoothed_path) - 1) 51 | j = randint(0, len(smoothed_path) - 1) 52 | if abs(i - j) <= 1: 53 | continue 54 | 55 | if j < i: 56 | i, j = j, i 57 | shortcut = list(extend(smoothed_path[i], smoothed_path[j])) 58 | 59 | failed = False 60 | shortcut_jconfs = [] 61 | if len(shortcut) < (j - i): 62 | for cube_pose in shortcut: 63 | tip_positions = calc_tippos_fn(cube_pose) 64 | # Keep num_samples=1!!!!! This reallly slows down at larger values (and we only use the first one anyway) 65 | ik_sols = sample_multiple_ik_with_collision(ik, functools.partial(collision, cube_pose), 66 | sample_joint_conf_fn, tip_positions, num_samples=1) 67 | if len(ik_sols) == 0: 68 | failed = True 69 | break 70 | shortcut_jconfs.append(ik_sols[0]) 71 | 72 | if failed: 73 | continue 74 | 75 | print(f'shortcut is found!!: {i}:{j} {len(shortcut)}') 76 | smoothed_path = smoothed_path[:i + 1] + shortcut + smoothed_path[j + 1:] 77 | smoothed_jconf_path = smoothed_jconf_path[:i + 1] + shortcut_jconfs + smoothed_jconf_path[j + 1:] 78 | return smoothed_path, smoothed_jconf_path 79 | -------------------------------------------------------------------------------- /python/pybullet_planning/motion_planners/star_roadmap.py: -------------------------------------------------------------------------------- 1 | from collections import Mapping 2 | 3 | #class StarRoadmap(Mapping, object): 4 | class StarRoadmap(Mapping, object): 5 | 6 | def __init__(self, center, planner): 7 | self.center = center 8 | self.planner = planner 9 | self.roadmap = {} 10 | 11 | """ 12 | def __getitem__(self, q): 13 | return self.roadmap[q] 14 | 15 | def __len__(self): 16 | return len(self.roadmap) 17 | 18 | def __iter__(self): 19 | return iter(self.roadmap) 20 | """ 21 | 22 | def grow(self, goal): 23 | if goal not in self.roadmap: 24 | self.roadmap[goal] = self.planner(self.center, goal) 25 | return self.roadmap[goal] 26 | 27 | def __call__(self, start, goal): 28 | start_traj = self.grow(start) 29 | if start_traj is None: 30 | return None 31 | goal_traj = self.grow(goal) 32 | if goal_traj is None: 33 | return None 34 | return start_traj.reverse(), goal_traj 35 | -------------------------------------------------------------------------------- /python/pybullet_planning/primitives/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | ******************************************************************************** 3 | primitives 4 | ******************************************************************************** 5 | 6 | These modules are transported from `kuka_primitives`, `pr2_primitives` and `pr2_utils` from 7 | `ss-pybullet `_. 8 | 9 | .. currentmodule:: pybullet_planning.primitives 10 | 11 | Grasp 12 | ------ 13 | 14 | In ``pybullet_planning``, a grasp is modeled as an operator on the object frame, i.e. 15 | ``world_from_object = world_from_gripper * grasp``, where ``grasp = gripper_from_object``. 16 | 17 | We provide some simple grasp generator based on bounding geometry (box, cylinder). Inside these 18 | generators' implementation, however, the modeling start from the object frame to the gripper's 19 | frame to obtain ``object_from_gripper`` and then we return its inverse ``gripper_from_object``. 20 | (Yijiang thinks this is more straightforward to think about.) 21 | 22 | .. currentmodule:: pybullet_planning.primitives.grasp_gen 23 | 24 | .. autosummary:: 25 | :toctree: generated/ 26 | :nosignatures: 27 | 28 | get_side_cylinder_grasps 29 | 30 | Convenient classes 31 | ------------------ 32 | 33 | .. currentmodule:: pybullet_planning.primitives.trajectory 34 | 35 | .. autosummary:: 36 | :toctree: generated/ 37 | :nosignatures: 38 | 39 | EndEffector 40 | 41 | """ 42 | 43 | from __future__ import absolute_import 44 | 45 | from .grasp_gen import * 46 | from .trajectory import * 47 | 48 | __all__ = [name for name in dir() if not name.startswith('_')] 49 | -------------------------------------------------------------------------------- /python/pybullet_planning/primitives/grasp_gen.py: -------------------------------------------------------------------------------- 1 | import math 2 | import random 3 | import numpy as np 4 | 5 | from pybullet_planning.interfaces import Pose, Point, Euler, unit_pose, point_from_pose, multiply, quat_from_euler, invert 6 | from pybullet_planning.interfaces import approximate_as_prism, approximate_as_cylinder 7 | 8 | 9 | def get_top_grasps(body, under=False): 10 | raise NotImplementedError() 11 | 12 | 13 | def get_side_grasps(body, under=False): 14 | raise NotImplementedError() 15 | 16 | 17 | def get_side_cylinder_grasps(body, tool_pose=unit_pose(), body_pose=unit_pose(), reverse_grasp=False, 18 | safety_margin_length=0.0): 19 | """generate side grasp using the body's bounding cylinder. 20 | 21 | The generated grasp should be used as: ``world_from_body = world_from_gripper * grasp`` 22 | 23 | The image below shows the default gripper axis convention. 24 | 25 | .. image:: ../images/cylinder_grasp_convention.png 26 | :align: center 27 | 28 | Parameters 29 | ---------- 30 | body : int 31 | body id for the body being grasped 32 | tool_pose : Pose, optional 33 | additional default_ee_from_customized_ee, by default unit_pose() 34 | body_pose : Pose, optional 35 | the body's current pose, used for computing bounding box, by default unit_pose() 36 | reverse_grasp : bool, optional 37 | generate flipped gripper for pi-symmetry around the gripper's z axis, by default False 38 | safety_margin_length : float, optional 39 | safety margin along the gripper's y axis, i.e. the ``top_offset`` shown in the picture, by default 0.0 40 | 41 | Yields 42 | ------- 43 | Pose 44 | gripper_from_body 45 | """ 46 | center, (_, height) = approximate_as_cylinder(body, body_pose=body_pose) 47 | assert safety_margin_length <= height/2, \ 48 | 'safety margin length must be smaller than half of the bounding cylinder\'s height {}'.format(height/2) 49 | object_from_center = Pose(center - point_from_pose(body_pose)) 50 | 51 | # rotate the cylinder's frame to make x axis align with the longitude axis 52 | longitude_x = Pose(euler=Euler(pitch=np.pi/2)) 53 | while True: 54 | # rotational symmetry around the longitude axis 55 | theta = random.uniform(0, 2*np.pi) 56 | rotate_around_x_axis = Pose(euler=Euler(theta, 0, 0)) 57 | # translation along the longitude axis 58 | slide_dist = random.uniform(-height/2+safety_margin_length, height/2-safety_margin_length) 59 | translate_along_x_axis = Pose(point=Point(slide_dist,0,0)) 60 | 61 | for j in range(1 + reverse_grasp): 62 | # the base pi/2 is to make y align with the longitude axis, conforming to the convention (see image in the doc) 63 | # flip the gripper, gripper symmetry 64 | rotate_around_z = Pose(euler=[0, 0, math.pi/2 + j * math.pi]) 65 | 66 | object_from_gripper = multiply(object_from_center, longitude_x, translate_along_x_axis, rotate_around_x_axis, \ 67 | rotate_around_z, tool_pose) 68 | yield invert(object_from_gripper) 69 | -------------------------------------------------------------------------------- /python/pybullet_planning/primitives/trajectory.py: -------------------------------------------------------------------------------- 1 | from pybullet_planning.interfaces import get_relative_pose, get_link_subtree, clone_body, set_static, get_link_pose, \ 2 | set_pose, multiply, get_pose, invert 3 | 4 | ############################################## 5 | 6 | class EndEffector(object): 7 | """a convenient class for creating and manipulating an end effector 8 | 9 | Note: the end effector needs to be modeled in the robot's URDF. 10 | 11 | """ 12 | def __init__(self, robot, ee_link, tool_link, **kwargs): 13 | """[summary] 14 | 15 | Parameters 16 | ---------- 17 | robot : [type] 18 | [description] 19 | ee_link : int 20 | pb link index of the link where the end effector gets attached to 21 | tool_link : int 22 | pb link index of the TCP (tip) link 23 | """ 24 | self.robot = robot 25 | self.ee_link = ee_link 26 | self.tool_link = tool_link 27 | self.tool_from_ee = get_relative_pose(self.robot, self.ee_link, self.tool_link) 28 | tool_links = get_link_subtree(robot, self.ee_link) 29 | self.body = clone_body(robot, links=tool_links, **kwargs) 30 | set_static(self.body) 31 | # for link in get_all_links(tool_body): 32 | # set_color(tool_body, np.zeros(4), link) 33 | def get_tool_pose(self, get_cloned_pose=True): 34 | """[summary] 35 | 36 | Parameters 37 | ---------- 38 | get_cloned_pose : bool, optional 39 | if True return the cloned floating EE's tool pose, the robot's tool pose otherwise, by default True 40 | 41 | Returns 42 | ------- 43 | [type] 44 | [description] 45 | """ 46 | if not clone_body: 47 | return get_link_pose(self.robot, self.tool_link) 48 | else: 49 | ee_pose = get_pose(self.body) 50 | return multiply(ee_pose, invert(self.tool_from_ee)) 51 | 52 | def set_pose(self, tool_pose): 53 | pose = multiply(tool_pose, self.tool_from_ee) 54 | set_pose(self.body, pose) 55 | return pose 56 | 57 | @property 58 | def tool_from_root(self): 59 | return self.tool_from_ee 60 | 61 | def __repr__(self): 62 | return '{}({}, {})'.format(self.__class__.__name__, self.robot, self.body) 63 | -------------------------------------------------------------------------------- /python/pybullet_planning/utils/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | ******************************************************************************** 3 | utils 4 | ******************************************************************************** 5 | 6 | .. currentmodule:: pybullet_planning.utils 7 | 8 | Package containing a set of utility functions and variables 9 | 10 | File system functions 11 | ===================== 12 | 13 | .. autosummary:: 14 | :toctree: generated/ 15 | :nosignatures: 16 | 17 | read_pickle 18 | write_pickle 19 | 20 | Sampling functions 21 | =================== 22 | 23 | .. autosummary:: 24 | :toctree: generated/ 25 | :nosignatures: 26 | 27 | randomize 28 | 29 | Iteration utilities 30 | =================== 31 | 32 | .. autosummary:: 33 | :toctree: generated/ 34 | :nosignatures: 35 | 36 | roundrobin 37 | chunks 38 | get_pairs 39 | 40 | 41 | Transformation functions 42 | ======================== 43 | 44 | .. autosummary:: 45 | :toctree: generated/ 46 | :nosignatures: 47 | 48 | rotation_from_matrix 49 | 50 | """ 51 | 52 | from .shared_const import * 53 | from .file_io import * 54 | from .numeric_sample import * 55 | from .transformations import * 56 | from .iter_utils import * 57 | from .debug_utils import * 58 | # from ._file_path_archived import * 59 | 60 | __all__ = [name for name in dir() if not name.startswith('_')] 61 | -------------------------------------------------------------------------------- /python/pybullet_planning/utils/_file_path_archived.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | ##################################### 4 | # TODO: clean up these data paths 5 | 6 | DRAKE_PATH = 'models/drake/' 7 | 8 | # Models 9 | 10 | # Robots 11 | MODEL_DIRECTORY = os.path.abspath(os.path.join(os.path.dirname(__file__), os.pardir, 'models/')) 12 | ROOMBA_URDF = 'models/turtlebot/roomba.urdf' 13 | TURTLEBOT_URDF = 'models/turtlebot/turtlebot_holonomic.urdf' 14 | DRAKE_IIWA_URDF = 'models/drake/iiwa_description/urdf/iiwa14_polytope_collision.urdf' 15 | WSG_50_URDF = 'models/drake/wsg_50_description/urdf/wsg_50_mesh_visual.urdf' # wsg_50 | wsg_50_mesh_visual | wsg_50_mesh_collision 16 | #SCHUNK_URDF = 'models/drake/wsg_50_description/sdf/schunk_wsg_50.sdf' 17 | PANDA_HAND_URDF = "models/franka_description/robots/hand.urdf" 18 | PANDA_ARM_URDF = "models/franka_description/robots/panda_arm_hand.urdf" 19 | 20 | # Pybullet Robots 21 | #PYBULLET_DIRECTORY = add_data_path() 22 | KUKA_IIWA_URDF = "kuka_iiwa/model.urdf" 23 | KUKA_IIWA_GRIPPER_SDF = "kuka_iiwa/kuka_with_gripper.sdf" 24 | R2D2_URDF = "r2d2.urdf" 25 | MINITAUR_URDF = "quadruped/minitaur.urdf" 26 | HUMANOID_MJCF = "mjcf/humanoid.xml" 27 | HUSKY_URDF = "husky/husky.urdf" 28 | RACECAR_URDF = 'racecar/racecar.urdf' # racecar_differential.urdf 29 | PR2_GRIPPER = 'pr2_gripper.urdf' 30 | WSG_GRIPPER = 'gripper/wsg50_one_motor_gripper.sdf' # wsg50_one_motor_gripper | wsg50_one_motor_gripper_new 31 | 32 | # Pybullet Objects 33 | KIVA_SHELF_SDF = "kiva_shelf/model.sdf" 34 | FLOOR_URDF = 'plane.urdf' 35 | TABLE_URDF = 'table' 36 | 37 | # Objects 38 | SMALL_BLOCK_URDF = "models/drake/objects/block_for_pick_and_place.urdf" 39 | BLOCK_URDF = "models/drake/objects/block_for_pick_and_place_mid_size.urdf" 40 | SINK_URDF = 'models/sink.urdf' 41 | STOVE_URDF = 'models/stove.urdf' 42 | -------------------------------------------------------------------------------- /python/pybullet_planning/utils/debug_utils.py: -------------------------------------------------------------------------------- 1 | import os 2 | import platform 3 | import math 4 | import inspect 5 | import signal 6 | from contextlib import contextmanager 7 | 8 | from .shared_const import INF 9 | 10 | def is_remote(): 11 | return 'SSH_CONNECTION' in os.environ 12 | 13 | def is_darwin(): # TODO: change loading accordingly 14 | return platform.system() == 'Darwin' # platform.release() 15 | #return sys.platform == 'darwin' 16 | 17 | def is_windows(): 18 | return platform.system() == 'Windows' 19 | 20 | ################################################## 21 | 22 | def get_function_name(depth=1): 23 | return inspect.stack()[depth][3] 24 | 25 | ################################################## 26 | 27 | BYTES_PER_KILOBYTE = math.pow(2, 10) 28 | BYTES_PER_GIGABYTE = math.pow(2, 30) 29 | KILOBYTES_PER_GIGABYTE = BYTES_PER_GIGABYTE / BYTES_PER_KILOBYTE 30 | 31 | def get_memory_in_kb(): 32 | # https://pypi.org/project/psutil/ 33 | # https://psutil.readthedocs.io/en/latest/ 34 | import psutil 35 | #rss: aka "Resident Set Size", this is the non-swapped physical memory a process has used. (bytes) 36 | #vms: aka "Virtual Memory Size", this is the total amount of virtual memory used by the process. (bytes) 37 | #shared: (Linux) memory that could be potentially shared with other processes. 38 | #text (Linux, BSD): aka TRS (text resident set) the amount of memory devoted to executable code. 39 | #data (Linux, BSD): aka DRS (data resident set) the amount of physical memory devoted to other than executable code. 40 | #lib (Linux): the memory used by shared libraries. 41 | #dirty (Linux): the number of dirty pages. 42 | #pfaults (macOS): number of page faults. 43 | #pageins (macOS): number of actual pageins. 44 | process = psutil.Process(os.getpid()) 45 | #process.pid() 46 | #process.ppid() 47 | pmem = process.memory_info() # this seems to actually get the current memory! 48 | return pmem.vms / BYTES_PER_KILOBYTE 49 | #print(process.memory_full_info()) 50 | #print(process.memory_percent()) 51 | # process.rlimit(psutil.RLIMIT_NOFILE) # set resource limits (Linux only) 52 | #print(psutil.virtual_memory()) 53 | #print(psutil.swap_memory()) 54 | #print(psutil.pids()) 55 | 56 | def raise_timeout(signum, frame): 57 | raise TimeoutError() 58 | 59 | @contextmanager 60 | def timeout(duration): 61 | # https://www.jujens.eu/posts/en/2018/Jun/02/python-timeout-function/ 62 | # https://code-maven.com/python-timeout 63 | # https://pypi.org/project/func-timeout/ 64 | # https://pypi.org/project/timeout-decorator/ 65 | # https://eli.thegreenplace.net/2011/08/22/how-not-to-set-a-timeout-on-a-computation-in-python 66 | # https://docs.python.org/3/library/signal.html 67 | # https://docs.python.org/3/library/contextlib.html 68 | # https://stackoverflow.com/a/22348885 69 | # TODO: this is *nix only, raise on Windows environment 70 | if is_windows(): 71 | warnings.warn('Currently, we do not support clone bodies that are created from an obj file.') 72 | return 73 | 74 | assert 0 < duration 75 | if duration == INF: 76 | yield 77 | return 78 | # Register a function to raise a TimeoutError on the signal 79 | signal.signal(signal.SIGALRM, raise_timeout) 80 | # Schedule the signal to be sent after ``duration`` 81 | signal.alarm(int(math.ceil(duration))) 82 | try: 83 | yield 84 | except TimeoutError as e: 85 | print('Timeout after {} sec'.format(duration)) 86 | #traceback.print_exc() 87 | pass 88 | finally: 89 | # Unregister the signal so it won't be triggered 90 | # if the timeout is not reached 91 | signal.signal(signal.SIGALRM, signal.SIG_IGN) 92 | -------------------------------------------------------------------------------- /python/pybullet_planning/utils/file_io.py: -------------------------------------------------------------------------------- 1 | """ 2 | file I/O 3 | """ 4 | 5 | import os 6 | import pickle 7 | import json 8 | import datetime 9 | from .shared_const import DATE_FORMAT 10 | 11 | SEPARATOR = '\n' + 50*'-' + '\n' 12 | 13 | #def inf_generator(): 14 | # return iter(int, 1) 15 | # inf_generator = count 16 | 17 | def print_separator(n=50): 18 | print('\n' + n*'-' + '\n') 19 | 20 | def read(filename): 21 | with open(filename, 'r') as f: 22 | return f.read() 23 | 24 | def write(filename, string): 25 | with open(filename, 'w') as f: 26 | f.write(string) 27 | 28 | def read_pickle(filename): 29 | """[summary] 30 | 31 | Parameters 32 | ---------- 33 | filename : [type] 34 | [description] 35 | 36 | Returns 37 | ------- 38 | [type] 39 | [description] 40 | """ 41 | # Can sometimes read pickle3 from python2 by calling twice 42 | # Can possibly read pickle2 from python3 by using encoding='latin1' 43 | with open(filename, 'rb') as f: 44 | return pickle.load(f) 45 | 46 | def write_pickle(filename, data): # NOTE - cannot pickle lambda or nested functions 47 | """[summary] 48 | """ 49 | with open(filename, 'wb') as f: 50 | pickle.dump(data, f) 51 | 52 | def read_json(path): 53 | return json.loads(read(path)) 54 | 55 | def write_json(path, data): 56 | with open(path, 'w') as f: 57 | json.dump(data, f, indent=2, sort_keys=True) 58 | 59 | def safe_remove(p): 60 | if os.path.exists(p): 61 | os.remove(p) 62 | 63 | def ensure_dir(f): 64 | d = os.path.dirname(f) 65 | if not os.path.exists(d): 66 | os.makedirs(d) 67 | 68 | def get_date(): 69 | return datetime.datetime.now().strftime(DATE_FORMAT) 70 | 71 | def load_yaml(path): 72 | import yaml 73 | # grep -r --include="*.py" "yaml\." * 74 | # yaml.dump() 75 | with open(path, 'r') as f: 76 | try: 77 | return yaml.safe_load(f) 78 | except yaml.YAMLError as exc: 79 | raise exc 80 | -------------------------------------------------------------------------------- /python/pybullet_planning/utils/iter_utils.py: -------------------------------------------------------------------------------- 1 | from itertools import cycle, islice 2 | 3 | def implies(p1, p2): 4 | return not p1 or p2 5 | 6 | def roundrobin(*iterables): 7 | """roundrobin('ABC', 'D', 'EF') --> A D E B F C 8 | 9 | https://docs.python.org/3.1/library/itertools.html#recipes 10 | Recipe credited to George Sakkis 11 | """ 12 | pending = len(iterables) 13 | nexts = cycle(iter(it).__next__ for it in iterables) 14 | while pending: 15 | try: 16 | for next in nexts: 17 | yield next() 18 | except StopIteration: 19 | pending -= 1 20 | nexts = cycle(islice(nexts, pending)) 21 | 22 | def chunks(sequence, n=1): 23 | """get [i, i+n] sublist of a given sequence 24 | """ 25 | for i in range(0, len(sequence), n): 26 | yield sequence[i:i + n] 27 | 28 | def safe_zip(sequence1, sequence2): 29 | """zip with safeguarding on the length 30 | """ 31 | assert len(sequence1) == len(sequence2) 32 | return zip(sequence1, sequence2) 33 | 34 | def get_pairs(sequence): 35 | """get a sequece of (seq[i], seq[i+1]), i=0~n-1 36 | """ 37 | return list(safe_zip(sequence[:-1], sequence[1:])) 38 | -------------------------------------------------------------------------------- /python/pybullet_planning/utils/numeric_sample.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import random 3 | 4 | from .shared_const import INF 5 | 6 | def clip(value, min_value=-INF, max_value=+INF): 7 | """clamp a value 8 | """ 9 | return min(max(min_value, value), max_value) 10 | 11 | def randomize(iterable): # TODO: bisect 12 | sequence = list(iterable) 13 | random.shuffle(sequence) 14 | return sequence 15 | 16 | def get_random_seed(): 17 | return random.getstate()[1][0] 18 | 19 | def get_numpy_seed(): 20 | return np.random.get_state()[1][0] 21 | 22 | def set_random_seed(seed): 23 | if seed is not None: 24 | random.seed(seed) 25 | 26 | def set_numpy_seed(seed): 27 | # These generators are different and independent 28 | if seed is not None: 29 | np.random.seed(seed % (2**32)) 30 | #print('Seed:', seed) 31 | -------------------------------------------------------------------------------- /python/pybullet_planning/utils/shared_const.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import pybullet as p 3 | 4 | # constants 5 | INF = np.inf 6 | PI = np.pi 7 | EPS = 1e-12 8 | CIRCULAR_LIMITS = -PI, PI 9 | UNBOUNDED_LIMITS = -INF, INF 10 | DEFAULT_TIME_STEP = 1./240. # seconds 11 | 12 | # pybullet setup parameters 13 | CLIENTS = {} # TODO: rename to include locked 14 | CLIENT = 0 15 | 16 | INFO_FROM_BODY = {} 17 | 18 | # colors 19 | RED = (1, 0, 0, 1) 20 | GREEN = (0, 1, 0, 1) 21 | BLUE = (0, 0, 1, 1) 22 | BLACK = (0, 0, 0, 1) 23 | WHITE = (1, 1, 1, 1) 24 | BROWN = (0.396, 0.263, 0.129, 1) 25 | TAN = (0.824, 0.706, 0.549, 1) 26 | GREY = (0.5, 0.5, 0.5, 1) 27 | YELLOW = (1, 1, 0, 1) 28 | 29 | GRAVITY = 9.8 30 | 31 | # links 32 | BASE_LINK = -1 33 | STATIC_MASS = 0 34 | 35 | # geometry 36 | DEFAULT_MESH = '' 37 | DEFAULT_EXTENTS = [1, 1, 1] 38 | DEFAULT_RADIUS = 0.5 39 | DEFAULT_HEIGHT = 1 40 | DEFAULT_SCALE = [1, 1, 1] 41 | DEFAULT_NORMAL = [0, 0, 1] 42 | 43 | # planning 44 | DEFAULT_RESOLUTION = 0.05 45 | 46 | # Collision 47 | # MAX_DISTANCE = 1e-3 48 | # Used in collision checking query, e.g. pybullet.getClosestPoint 49 | # If the distance between objects exceeds this maximum distance, no points may be returned. 50 | MAX_DISTANCE = 0. 51 | 52 | UNKNOWN_FILE = 'unknown_file' 53 | NULL_ID = -1 54 | TEMP_DIR = 'temp/' 55 | 56 | OBJ_MESH_CACHE = {} 57 | """a cache keeping track of loaded obj objects 58 | 59 | """ 60 | 61 | CARTESIAN_TYPES = { 62 | 'x': (p.JOINT_PRISMATIC, [1, 0, 0]), 63 | 'y': (p.JOINT_PRISMATIC, [0, 1, 0]), 64 | 'z': (p.JOINT_PRISMATIC, [0, 0, 1]), 65 | 'roll': (p.JOINT_REVOLUTE, [1, 0, 0]), 66 | 'pitch': (p.JOINT_REVOLUTE, [0, 1, 0]), 67 | 'yaw': (p.JOINT_REVOLUTE, [0, 0, 1]), 68 | } 69 | 70 | T2 = ['x', 'y'] 71 | T3 = ['x', 'y', 'z'] 72 | 73 | SE2 = T2 + ['yaw'] 74 | SE3 = T3 + ['roll', 'pitch', 'yaw'] 75 | 76 | def get_client(client=None): 77 | if client is None: 78 | return CLIENT 79 | return client 80 | 81 | def set_client(client): 82 | global CLIENT 83 | CLIENT = client 84 | 85 | DATE_FORMAT = '%y-%m-%d_%H-%M-%S' 86 | -------------------------------------------------------------------------------- /python/residual_learning/__init__.py: -------------------------------------------------------------------------------- 1 | from .residual_sac import ResidualSAC 2 | from .make_training_env import make_training_env 3 | -------------------------------------------------------------------------------- /python/residual_learning/get_ob_norm.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | from dl import nest 3 | from residual_learning.make_training_env import make_training_env 4 | from residual_learning.state_machines import MPPGStateMachine 5 | import torch 6 | import numpy as np 7 | import os 8 | 9 | 10 | def get_norm_params(n, difficulty, use_domain_rand): 11 | term_fn = 'position_close_to_goal' if difficulty < 4 else 'pos_and_rot_close_to_goal' 12 | env = make_training_env(32, MPPGStateMachine, difficulty, 'torque_and_position', 13 | frameskip=3, 14 | sim=True, 15 | visualization=False, 16 | reward_fn='competition_reward', 17 | termination_fn=term_fn, 18 | initializer='training_init', 19 | episode_length=3750, 20 | monitor=False, 21 | seed=0, 22 | norm_observations=True, 23 | max_torque=0.0, 24 | max_position=0.0, # set all residual actions to 0 25 | denylist_states=['FailureState'], 26 | domain_randomization=use_domain_rand 27 | ) 28 | env.steps = n 29 | env.find_norm_params() 30 | 31 | def get_var(std): 32 | if std is not None: 33 | return std ** 2 34 | return env.mean, nest.map_structure(get_var, env.std), env 35 | 36 | 37 | class RunningNorm(object): 38 | def __init__(self, eps=1e-5): 39 | self.count = 0 40 | self.eps = eps 41 | 42 | def _update(self, item): 43 | mean, var, batch_mean, batch_var = item 44 | if mean is not None: 45 | delta = batch_mean - mean 46 | new_mean = mean + delta * (self.batch_count / self.new_count) 47 | new_var = self.count * var + self.batch_count * batch_var 48 | new_var += (delta**2) * self.count * (self.batch_count / self.new_count) 49 | new_var /= self.new_count 50 | mean[:] = new_mean 51 | var[:] = new_var 52 | 53 | def update(self, mean, var, count): 54 | if self.count == 0: 55 | self.mean = mean 56 | self.var = var 57 | self.count = count 58 | 59 | else: 60 | self.batch_count = count 61 | self.new_count = count + self.count 62 | nest.map_structure( 63 | self._update, nest.zip_structure(self.mean, self.var, mean, var) 64 | ) 65 | self.count = self.new_count 66 | 67 | 68 | if __name__ == '__main__': 69 | parser = argparse.ArgumentParser(description='Train Agent.') 70 | parser.add_argument('n', type=int, help='steps for each difficulty') 71 | parser.add_argument('--domain_rand', default=False, action='store_true', 72 | help=' use domain randomization') 73 | args = parser.parse_args() 74 | 75 | means, stds = [], [] 76 | rn = RunningNorm() 77 | for difficulty in range(1, 5): 78 | mean, var, env = get_norm_params(args.n, difficulty, args.domain_rand) 79 | rn.update(mean, var, args.n) 80 | 81 | env.mean = rn.mean 82 | 83 | def get_std(x): 84 | if x is not None: 85 | return np.sqrt(x) 86 | env.std = nest.map_structure(get_std, rn.var) 87 | 88 | state_dict = env.state_dict() 89 | dirpath = os.path.dirname(__file__) 90 | f = 'obs_norm.pt' if not args.domain_rand else 'obs_norm_rand.pt' 91 | torch.save(state_dict, os.path.join(dirpath, f)) 92 | env.close() 93 | -------------------------------------------------------------------------------- /python/residual_learning/make_training_env.py: -------------------------------------------------------------------------------- 1 | """Define training env.""" 2 | import torch 3 | import gin 4 | import os 5 | from dl.rl.envs import SubprocVecEnv, DummyVecEnv, EpisodeInfo, VecObsNormWrapper 6 | from env.make_env import make_env 7 | from trifinger_simulation.tasks import move_cube 8 | from .residual_wrappers import ResidualWrapper, RandomizedEnvWrapper 9 | 10 | 11 | @gin.configurable 12 | def make_training_env(nenv, state_machine, goal_difficulty, action_space, 13 | residual_state, frameskip=1, sim=True, visualization=False, 14 | reward_fn=None, termination_fn=None, initializer=None, 15 | episode_length=100000, monitor=False, seed=0, 16 | domain_randomization=False, norm_observations=False, 17 | max_torque=0.1): 18 | 19 | # dummy goal dict 20 | goal = move_cube.sample_goal(goal_difficulty) 21 | 22 | goal_dict = { 23 | 'position': goal.position, 24 | 'orientation': goal.orientation 25 | } 26 | 27 | def _env(rank): 28 | def _thunk(): 29 | env = make_env( 30 | cube_goal_pose=goal_dict, 31 | goal_difficulty=goal_difficulty, 32 | action_space=action_space, 33 | frameskip=frameskip, 34 | sim=sim, 35 | visualization=visualization, 36 | reward_fn=reward_fn, 37 | termination_fn=termination_fn, 38 | initializer=initializer, 39 | episode_length=10*episode_length, # make this long enough to ensure that we have "episode_length" steps in the residual_state. 40 | rank=rank, 41 | monitor=monitor, 42 | ) 43 | if domain_randomization: 44 | env = RandomizedEnvWrapper(env) 45 | env = ResidualWrapper(env, state_machine, frameskip, 46 | max_torque, residual_state, 47 | max_length=episode_length) 48 | env = EpisodeInfo(env) 49 | env.seed(seed + rank) 50 | return env 51 | return _thunk 52 | 53 | if nenv > 1: 54 | env = SubprocVecEnv([_env(i) for i in range(nenv)], context='fork') 55 | else: 56 | env = DummyVecEnv([_env(0)]) 57 | env.reward_range = env.envs[0].reward_range 58 | 59 | if norm_observations: 60 | env = VecObsNormWrapper(env) 61 | norm_params = torch.load( 62 | os.path.join(os.path.dirname(__file__), 'obs_norm.pt') 63 | ) 64 | env.load_state_dict(norm_params) 65 | assert env.mean is not None 66 | assert env.std is not None 67 | return env 68 | -------------------------------------------------------------------------------- /python/residual_learning/models/cic_lvl3/logs/base.gin: -------------------------------------------------------------------------------- 1 | import residual_learning.residual_sac 2 | import residual_learning.networks 3 | import residual_learning.state_machines 4 | import residual_learning.make_training_env 5 | import cpc.state_machine 6 | import cic.states.custom_state_machines 7 | 8 | train.algorithm = @ResidualSAC 9 | train.maxt = 1000000 10 | train.seed = 0 11 | train.eval = True 12 | train.eval_period = 100000 13 | train.save_period = 10000 14 | train.maxseconds = None 15 | 16 | optim.Adam.betas = (0.9, 0.999) 17 | 18 | 19 | max_torque = 0.1 20 | 21 | 22 | NetworkParams.size = 256 23 | NetworkParams.embedding_size = 64 24 | NetworkParams.max_torque = %max_torque 25 | NetworkParams.pi_layers = 2 26 | NetworkParams.vf_layers = 3 27 | NetworkParams.init_std = 1.0 28 | 29 | Checkpointer.ckpt_period = 10000 30 | 31 | ResidualSAC.policy_training_start = 40000 32 | ResidualSAC.policy_zero_end = 100000 33 | ResidualSAC.env_fn = @make_training_env 34 | ResidualSAC.policy_fn = @policy_fn 35 | ResidualSAC.qf_fn = @qf_fn 36 | ResidualSAC.nenv = 1 37 | ResidualSAC.eval_num_episodes = 20 38 | ResidualSAC.record_num_episodes = 0 39 | ResidualSAC.buffer_size = 1000000 40 | ResidualSAC.frame_stack = 1 41 | ResidualSAC.learning_starts = 20000 42 | ResidualSAC.update_period = 1 43 | ResidualSAC.gpu = True 44 | ResidualSAC.optimizer = @optim.Adam 45 | ResidualSAC.batch_size = 256 46 | ResidualSAC.policy_lr = 3e-4 47 | ResidualSAC.qf_lr = 3e-4 48 | ResidualSAC.gamma = 0.99 49 | ResidualSAC.target_update_period = 1 50 | ResidualSAC.policy_update_period = 4 51 | ResidualSAC.alpha = 0.2 52 | ResidualSAC.target_smoothing_coef = 0.001 53 | ResidualSAC.automatic_entropy_tuning = True 54 | ResidualSAC.target_entropy = None 55 | ResidualSAC.log_period = 100 56 | 57 | make_training_env.goal_difficulty = 3 58 | make_training_env.action_space = "torque_and_position" 59 | make_training_env.frameskip = 3 60 | make_training_env.visualization = False 61 | make_training_env.reward_fn = "gaussian_reward" 62 | make_training_env.termination_fn = "no_termination" 63 | make_training_env.initializer = "training_init" 64 | make_training_env.episode_length = 1000 65 | make_training_env.monitor = False 66 | make_training_env.seed = 0 67 | make_training_env.domain_randomization = False 68 | make_training_env.norm_observations = True 69 | make_training_env.state_machine = @MPPGStateMachine 70 | make_training_env.max_torque = %max_torque 71 | make_training_env.residual_state = 'MoveToGoalState' 72 | 73 | VecObsNormWrapper.steps = 10000 74 | VecObsNormWrapper.mean = None 75 | VecObsNormWrapper.std = None 76 | VecObsNormWrapper.eps = 1e-2 77 | VecObsNormWrapper.log = True 78 | VecObsNormWrapper.log_prob = 0.01 79 | -------------------------------------------------------------------------------- /python/residual_learning/models/cic_lvl3/logs/ckpts/000880000.pt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cbschaff/benchmark-rrc/6fb12eecbaa778c60c5728dba4414330c901b09b/python/residual_learning/models/cic_lvl3/logs/ckpts/000880000.pt -------------------------------------------------------------------------------- /python/residual_learning/models/cic_lvl3/logs/singularity_config.yaml: -------------------------------------------------------------------------------- 1 | base_config: /logdir/base.gin 2 | gin_bindings: {NetworkParams.init_std: 1.0, ResidualSAC.action_reg_weight: 0.0, ResidualSAC.alpha: 0.2, 3 | ResidualSAC.learning_starts: 20000, ResidualSAC.policy_lr: 0.0001, ResidualSAC.policy_training_start: 40000, 4 | ResidualSAC.policy_update_period: 1, ResidualSAC.policy_zero_end: 40000, ResidualSAC.q_reg_weight: 0.0, 5 | ResidualSAC.qf_fn: '@qf_append_fn', ResidualSAC.target_smoothing_coef: 0.0001, make_training_env.domain_randomization: false, 6 | make_training_env.episode_length: 1000, make_training_env.goal_difficulty: 3, make_training_env.residual_state: MoveLiftCubePrimitiveLvl2, 7 | make_training_env.reward_fn: gaussian_training_reward, make_training_env.state_machine: '@CICStateMachineLvl2', 8 | make_training_env.termination_fn: no_termination, max_torque: 0.05, train.maxt: 2000000} 9 | logdir: /logdir 10 | -------------------------------------------------------------------------------- /python/residual_learning/models/cic_lvl4/logs/base.gin: -------------------------------------------------------------------------------- 1 | import residual_learning.residual_sac 2 | import residual_learning.networks 3 | import residual_learning.state_machines 4 | import residual_learning.make_training_env 5 | import cpc.state_machine 6 | import cic.states.custom_state_machines 7 | 8 | train.algorithm = @ResidualSAC 9 | train.maxt = 1000000 10 | train.seed = 0 11 | train.eval = True 12 | train.eval_period = 100000 13 | train.save_period = 10000 14 | train.maxseconds = None 15 | 16 | optim.Adam.betas = (0.9, 0.999) 17 | 18 | 19 | max_torque = 0.1 20 | 21 | 22 | NetworkParams.size = 256 23 | NetworkParams.embedding_size = 64 24 | NetworkParams.max_torque = %max_torque 25 | NetworkParams.pi_layers = 2 26 | NetworkParams.vf_layers = 3 27 | NetworkParams.init_std = 1.0 28 | 29 | Checkpointer.ckpt_period = 10000 30 | 31 | ResidualSAC.policy_training_start = 40000 32 | ResidualSAC.policy_zero_end = 100000 33 | ResidualSAC.env_fn = @make_training_env 34 | ResidualSAC.policy_fn = @policy_fn 35 | ResidualSAC.qf_fn = @qf_fn 36 | ResidualSAC.nenv = 1 37 | ResidualSAC.eval_num_episodes = 20 38 | ResidualSAC.record_num_episodes = 0 39 | ResidualSAC.buffer_size = 1000000 40 | ResidualSAC.frame_stack = 1 41 | ResidualSAC.learning_starts = 20000 42 | ResidualSAC.update_period = 1 43 | ResidualSAC.gpu = True 44 | ResidualSAC.optimizer = @optim.Adam 45 | ResidualSAC.batch_size = 256 46 | ResidualSAC.policy_lr = 3e-4 47 | ResidualSAC.qf_lr = 3e-4 48 | ResidualSAC.gamma = 0.99 49 | ResidualSAC.target_update_period = 1 50 | ResidualSAC.policy_update_period = 4 51 | ResidualSAC.alpha = 0.2 52 | ResidualSAC.target_smoothing_coef = 0.001 53 | ResidualSAC.automatic_entropy_tuning = True 54 | ResidualSAC.target_entropy = None 55 | ResidualSAC.log_period = 100 56 | 57 | make_training_env.goal_difficulty = 3 58 | make_training_env.action_space = "torque_and_position" 59 | make_training_env.frameskip = 3 60 | make_training_env.visualization = False 61 | make_training_env.reward_fn = "gaussian_reward" 62 | make_training_env.termination_fn = "no_termination" 63 | make_training_env.initializer = "training_init" 64 | make_training_env.episode_length = 1000 65 | make_training_env.monitor = False 66 | make_training_env.seed = 0 67 | make_training_env.domain_randomization = False 68 | make_training_env.norm_observations = True 69 | make_training_env.state_machine = @MPPGStateMachine 70 | make_training_env.max_torque = %max_torque 71 | make_training_env.residual_state = 'MoveToGoalState' 72 | 73 | VecObsNormWrapper.steps = 10000 74 | VecObsNormWrapper.mean = None 75 | VecObsNormWrapper.std = None 76 | VecObsNormWrapper.eps = 1e-2 77 | VecObsNormWrapper.log = True 78 | VecObsNormWrapper.log_prob = 0.01 79 | -------------------------------------------------------------------------------- /python/residual_learning/models/cic_lvl4/logs/ckpts/000780000.pt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cbschaff/benchmark-rrc/6fb12eecbaa778c60c5728dba4414330c901b09b/python/residual_learning/models/cic_lvl4/logs/ckpts/000780000.pt -------------------------------------------------------------------------------- /python/residual_learning/models/cic_lvl4/logs/singularity_config.yaml: -------------------------------------------------------------------------------- 1 | base_config: /logdir/base.gin 2 | gin_bindings: {NetworkParams.init_std: 1.0, ResidualSAC.action_reg_weight: 0.0, ResidualSAC.alpha: 0.2, 3 | ResidualSAC.learning_starts: 20000, ResidualSAC.policy_lr: 0.0001, ResidualSAC.policy_training_start: 40000, 4 | ResidualSAC.policy_update_period: 1, ResidualSAC.policy_zero_end: 40000, ResidualSAC.q_reg_weight: 0.0, 5 | ResidualSAC.qf_fn: '@qf_append_fn', ResidualSAC.target_smoothing_coef: 0.0001, make_training_env.domain_randomization: false, 6 | make_training_env.episode_length: 1000, make_training_env.goal_difficulty: 4, make_training_env.residual_state: MoveLiftCubeOrientPrimitiveLvl4, 7 | make_training_env.reward_fn: gaussian_training_reward, make_training_env.state_machine: '@CICStateMachineLvl4', 8 | make_training_env.termination_fn: no_termination, max_torque: 0.05, train.maxt: 2000000} 9 | logdir: /logdir 10 | -------------------------------------------------------------------------------- /python/residual_learning/models/cpc_lvl3/logs/base.gin: -------------------------------------------------------------------------------- 1 | import residual_learning.residual_sac 2 | import residual_learning.networks 3 | import residual_learning.state_machines 4 | import residual_learning.make_training_env 5 | import cpc.state_machine 6 | import cic.states.custom_state_machines 7 | 8 | train.algorithm = @ResidualSAC 9 | train.maxt = 1000000 10 | train.seed = 0 11 | train.eval = True 12 | train.eval_period = 100000 13 | train.save_period = 10000 14 | train.maxseconds = None 15 | 16 | optim.Adam.betas = (0.9, 0.999) 17 | 18 | 19 | max_torque = 0.1 20 | 21 | 22 | NetworkParams.size = 256 23 | NetworkParams.embedding_size = 64 24 | NetworkParams.max_torque = %max_torque 25 | NetworkParams.pi_layers = 2 26 | NetworkParams.vf_layers = 3 27 | NetworkParams.init_std = 1.0 28 | 29 | Checkpointer.ckpt_period = 10000 30 | 31 | ResidualSAC.policy_training_start = 40000 32 | ResidualSAC.policy_zero_end = 100000 33 | ResidualSAC.env_fn = @make_training_env 34 | ResidualSAC.policy_fn = @policy_fn 35 | ResidualSAC.qf_fn = @qf_fn 36 | ResidualSAC.nenv = 1 37 | ResidualSAC.eval_num_episodes = 20 38 | ResidualSAC.record_num_episodes = 0 39 | ResidualSAC.buffer_size = 1000000 40 | ResidualSAC.frame_stack = 1 41 | ResidualSAC.learning_starts = 20000 42 | ResidualSAC.update_period = 1 43 | ResidualSAC.gpu = True 44 | ResidualSAC.optimizer = @optim.Adam 45 | ResidualSAC.batch_size = 256 46 | ResidualSAC.policy_lr = 3e-4 47 | ResidualSAC.qf_lr = 3e-4 48 | ResidualSAC.gamma = 0.99 49 | ResidualSAC.target_update_period = 1 50 | ResidualSAC.policy_update_period = 4 51 | ResidualSAC.alpha = 0.2 52 | ResidualSAC.target_smoothing_coef = 0.001 53 | ResidualSAC.automatic_entropy_tuning = True 54 | ResidualSAC.target_entropy = None 55 | ResidualSAC.log_period = 100 56 | 57 | make_training_env.goal_difficulty = 3 58 | make_training_env.action_space = "torque_and_position" 59 | make_training_env.frameskip = 3 60 | make_training_env.visualization = False 61 | make_training_env.reward_fn = "gaussian_reward" 62 | make_training_env.termination_fn = "no_termination" 63 | make_training_env.initializer = "training_init" 64 | make_training_env.episode_length = 1000 65 | make_training_env.monitor = False 66 | make_training_env.seed = 0 67 | make_training_env.domain_randomization = False 68 | make_training_env.norm_observations = True 69 | make_training_env.state_machine = @MPPGStateMachine 70 | make_training_env.max_torque = %max_torque 71 | make_training_env.residual_state = 'MoveToGoalState' 72 | 73 | VecObsNormWrapper.steps = 10000 74 | VecObsNormWrapper.mean = None 75 | VecObsNormWrapper.std = None 76 | VecObsNormWrapper.eps = 1e-2 77 | VecObsNormWrapper.log = True 78 | VecObsNormWrapper.log_prob = 0.01 79 | -------------------------------------------------------------------------------- /python/residual_learning/models/cpc_lvl3/logs/ckpts/001020000.pt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cbschaff/benchmark-rrc/6fb12eecbaa778c60c5728dba4414330c901b09b/python/residual_learning/models/cpc_lvl3/logs/ckpts/001020000.pt -------------------------------------------------------------------------------- /python/residual_learning/models/cpc_lvl3/logs/singularity_config.yaml: -------------------------------------------------------------------------------- 1 | base_config: /logdir/base.gin 2 | gin_bindings: {NetworkParams.init_std: 1.0, ResidualSAC.action_reg_weight: 0.0, ResidualSAC.alpha: 0.2, 3 | ResidualSAC.learning_starts: 20000, ResidualSAC.policy_lr: 0.0001, ResidualSAC.policy_training_start: 40000, 4 | ResidualSAC.policy_update_period: 1, ResidualSAC.policy_zero_end: 40000, ResidualSAC.q_reg_weight: 0.0, 5 | ResidualSAC.qf_fn: '@qf_append_fn', ResidualSAC.target_smoothing_coef: 0.0001, make_training_env.domain_randomization: false, 6 | make_training_env.episode_length: 1000, make_training_env.goal_difficulty: 3, make_training_env.residual_state: MoveToGoalState, 7 | make_training_env.reward_fn: gaussian_training_reward, make_training_env.state_machine: '@CPCStateMachine', 8 | make_training_env.termination_fn: no_termination, max_torque: 0.05, train.maxt: 2000000} 9 | logdir: /logdir 10 | -------------------------------------------------------------------------------- /python/residual_learning/models/cpc_lvl4/logs/base.gin: -------------------------------------------------------------------------------- 1 | import residual_learning.residual_sac 2 | import residual_learning.networks 3 | import residual_learning.state_machines 4 | import residual_learning.make_training_env 5 | import cpc.state_machine 6 | import cic.states.custom_state_machines 7 | 8 | train.algorithm = @ResidualSAC 9 | train.maxt = 1000000 10 | train.seed = 0 11 | train.eval = True 12 | train.eval_period = 100000 13 | train.save_period = 10000 14 | train.maxseconds = None 15 | 16 | optim.Adam.betas = (0.9, 0.999) 17 | 18 | 19 | max_torque = 0.1 20 | 21 | 22 | NetworkParams.size = 256 23 | NetworkParams.embedding_size = 64 24 | NetworkParams.max_torque = %max_torque 25 | NetworkParams.pi_layers = 2 26 | NetworkParams.vf_layers = 3 27 | NetworkParams.init_std = 1.0 28 | 29 | Checkpointer.ckpt_period = 10000 30 | 31 | ResidualSAC.policy_training_start = 40000 32 | ResidualSAC.policy_zero_end = 100000 33 | ResidualSAC.env_fn = @make_training_env 34 | ResidualSAC.policy_fn = @policy_fn 35 | ResidualSAC.qf_fn = @qf_fn 36 | ResidualSAC.nenv = 1 37 | ResidualSAC.eval_num_episodes = 20 38 | ResidualSAC.record_num_episodes = 0 39 | ResidualSAC.buffer_size = 1000000 40 | ResidualSAC.frame_stack = 1 41 | ResidualSAC.learning_starts = 20000 42 | ResidualSAC.update_period = 1 43 | ResidualSAC.gpu = True 44 | ResidualSAC.optimizer = @optim.Adam 45 | ResidualSAC.batch_size = 256 46 | ResidualSAC.policy_lr = 3e-4 47 | ResidualSAC.qf_lr = 3e-4 48 | ResidualSAC.gamma = 0.99 49 | ResidualSAC.target_update_period = 1 50 | ResidualSAC.policy_update_period = 4 51 | ResidualSAC.alpha = 0.2 52 | ResidualSAC.target_smoothing_coef = 0.001 53 | ResidualSAC.automatic_entropy_tuning = True 54 | ResidualSAC.target_entropy = None 55 | ResidualSAC.log_period = 100 56 | 57 | make_training_env.goal_difficulty = 3 58 | make_training_env.action_space = "torque_and_position" 59 | make_training_env.frameskip = 3 60 | make_training_env.visualization = False 61 | make_training_env.reward_fn = "gaussian_reward" 62 | make_training_env.termination_fn = "no_termination" 63 | make_training_env.initializer = "training_init" 64 | make_training_env.episode_length = 1000 65 | make_training_env.monitor = False 66 | make_training_env.seed = 0 67 | make_training_env.domain_randomization = False 68 | make_training_env.norm_observations = True 69 | make_training_env.state_machine = @MPPGStateMachine 70 | make_training_env.max_torque = %max_torque 71 | make_training_env.residual_state = 'MoveToGoalState' 72 | 73 | VecObsNormWrapper.steps = 10000 74 | VecObsNormWrapper.mean = None 75 | VecObsNormWrapper.std = None 76 | VecObsNormWrapper.eps = 1e-2 77 | VecObsNormWrapper.log = True 78 | VecObsNormWrapper.log_prob = 0.01 79 | -------------------------------------------------------------------------------- /python/residual_learning/models/cpc_lvl4/logs/ckpts/000600000.pt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cbschaff/benchmark-rrc/6fb12eecbaa778c60c5728dba4414330c901b09b/python/residual_learning/models/cpc_lvl4/logs/ckpts/000600000.pt -------------------------------------------------------------------------------- /python/residual_learning/models/cpc_lvl4/logs/singularity_config.yaml: -------------------------------------------------------------------------------- 1 | base_config: /logdir/base.gin 2 | gin_bindings: {NetworkParams.init_std: 1.0, ResidualSAC.action_reg_weight: 0.0, ResidualSAC.alpha: 0.2, 3 | ResidualSAC.learning_starts: 20000, ResidualSAC.policy_lr: 0.0001, ResidualSAC.policy_training_start: 40000, 4 | ResidualSAC.policy_update_period: 1, ResidualSAC.policy_zero_end: 40000, ResidualSAC.q_reg_weight: 0.0, 5 | ResidualSAC.qf_fn: '@qf_append_fn', ResidualSAC.target_smoothing_coef: 0.0001, make_training_env.domain_randomization: false, 6 | make_training_env.episode_length: 1000, make_training_env.goal_difficulty: 4, make_training_env.residual_state: GoalWithOrientState, 7 | make_training_env.reward_fn: gaussian_training_reward, make_training_env.state_machine: '@CPCStateMachineL4', 8 | make_training_env.termination_fn: no_termination, max_torque: 0.05, train.maxt: 2000000} 9 | logdir: /logdir 10 | -------------------------------------------------------------------------------- /python/residual_learning/models/mp_lvl3/logs/base.gin: -------------------------------------------------------------------------------- 1 | import residual_learning.residual_sac 2 | import residual_learning.networks 3 | import residual_learning.state_machines 4 | import residual_learning.make_training_env 5 | import cpc.state_machine 6 | import cic.states.custom_state_machines 7 | 8 | train.algorithm = @ResidualSAC 9 | train.maxt = 1000000 10 | train.seed = 0 11 | train.eval = True 12 | train.eval_period = 100000 13 | train.save_period = 10000 14 | train.maxseconds = None 15 | 16 | optim.Adam.betas = (0.9, 0.999) 17 | 18 | 19 | max_torque = 0.1 20 | 21 | 22 | NetworkParams.size = 256 23 | NetworkParams.embedding_size = 64 24 | NetworkParams.max_torque = %max_torque 25 | NetworkParams.pi_layers = 2 26 | NetworkParams.vf_layers = 3 27 | NetworkParams.init_std = 1.0 28 | 29 | Checkpointer.ckpt_period = 10000 30 | 31 | ResidualSAC.policy_training_start = 40000 32 | ResidualSAC.policy_zero_end = 100000 33 | ResidualSAC.env_fn = @make_training_env 34 | ResidualSAC.policy_fn = @policy_fn 35 | ResidualSAC.qf_fn = @qf_fn 36 | ResidualSAC.nenv = 1 37 | ResidualSAC.eval_num_episodes = 20 38 | ResidualSAC.record_num_episodes = 0 39 | ResidualSAC.buffer_size = 1000000 40 | ResidualSAC.frame_stack = 1 41 | ResidualSAC.learning_starts = 20000 42 | ResidualSAC.update_period = 1 43 | ResidualSAC.gpu = True 44 | ResidualSAC.optimizer = @optim.Adam 45 | ResidualSAC.batch_size = 256 46 | ResidualSAC.policy_lr = 3e-4 47 | ResidualSAC.qf_lr = 3e-4 48 | ResidualSAC.gamma = 0.99 49 | ResidualSAC.target_update_period = 1 50 | ResidualSAC.policy_update_period = 4 51 | ResidualSAC.alpha = 0.2 52 | ResidualSAC.target_smoothing_coef = 0.001 53 | ResidualSAC.automatic_entropy_tuning = True 54 | ResidualSAC.target_entropy = None 55 | ResidualSAC.log_period = 100 56 | 57 | make_training_env.goal_difficulty = 3 58 | make_training_env.action_space = "torque_and_position" 59 | make_training_env.frameskip = 3 60 | make_training_env.visualization = False 61 | make_training_env.reward_fn = "gaussian_reward" 62 | make_training_env.termination_fn = "no_termination" 63 | make_training_env.initializer = "training_init" 64 | make_training_env.episode_length = 1000 65 | make_training_env.monitor = False 66 | make_training_env.seed = 0 67 | make_training_env.domain_randomization = False 68 | make_training_env.norm_observations = True 69 | make_training_env.state_machine = @MPPGStateMachine 70 | make_training_env.max_torque = %max_torque 71 | make_training_env.residual_state = 'MoveToGoalState' 72 | 73 | VecObsNormWrapper.steps = 10000 74 | VecObsNormWrapper.mean = None 75 | VecObsNormWrapper.std = None 76 | VecObsNormWrapper.eps = 1e-2 77 | VecObsNormWrapper.log = True 78 | VecObsNormWrapper.log_prob = 0.01 79 | -------------------------------------------------------------------------------- /python/residual_learning/models/mp_lvl3/logs/ckpts/001100000.pt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cbschaff/benchmark-rrc/6fb12eecbaa778c60c5728dba4414330c901b09b/python/residual_learning/models/mp_lvl3/logs/ckpts/001100000.pt -------------------------------------------------------------------------------- /python/residual_learning/models/mp_lvl3/logs/singularity_config.yaml: -------------------------------------------------------------------------------- 1 | base_config: /logdir/base.gin 2 | gin_bindings: {NetworkParams.init_std: 1.0, ResidualSAC.action_reg_weight: 0.0, ResidualSAC.alpha: 0.2, 3 | ResidualSAC.learning_starts: 20000, ResidualSAC.policy_lr: 0.0001, ResidualSAC.policy_training_start: 40000, 4 | ResidualSAC.policy_update_period: 1, ResidualSAC.policy_zero_end: 40000, ResidualSAC.q_reg_weight: 0.0, 5 | ResidualSAC.qf_fn: '@qf_append_fn', ResidualSAC.target_smoothing_coef: 0.0001, make_training_env.domain_randomization: false, 6 | make_training_env.episode_length: 1000, make_training_env.goal_difficulty: 3, make_training_env.residual_state: MoveToGoalState, 7 | make_training_env.reward_fn: gaussian_training_reward, make_training_env.state_machine: '@MPPGStateMachine', 8 | make_training_env.termination_fn: no_termination, max_torque: 0.05, train.maxt: 2000000} 9 | logdir: /logdir 10 | -------------------------------------------------------------------------------- /python/residual_learning/models/mp_lvl4/logs/base.gin: -------------------------------------------------------------------------------- 1 | import residual_learning.residual_sac 2 | import residual_learning.networks 3 | import residual_learning.state_machines 4 | import residual_learning.make_training_env 5 | import cpc.state_machine 6 | import cic.states.custom_state_machines 7 | 8 | train.algorithm = @ResidualSAC 9 | train.maxt = 1000000 10 | train.seed = 0 11 | train.eval = True 12 | train.eval_period = 100000 13 | train.save_period = 10000 14 | train.maxseconds = None 15 | 16 | optim.Adam.betas = (0.9, 0.999) 17 | 18 | 19 | max_torque = 0.1 20 | 21 | 22 | NetworkParams.size = 256 23 | NetworkParams.embedding_size = 64 24 | NetworkParams.max_torque = %max_torque 25 | NetworkParams.pi_layers = 2 26 | NetworkParams.vf_layers = 3 27 | NetworkParams.init_std = 1.0 28 | 29 | Checkpointer.ckpt_period = 10000 30 | 31 | ResidualSAC.policy_training_start = 40000 32 | ResidualSAC.policy_zero_end = 100000 33 | ResidualSAC.env_fn = @make_training_env 34 | ResidualSAC.policy_fn = @policy_fn 35 | ResidualSAC.qf_fn = @qf_fn 36 | ResidualSAC.nenv = 1 37 | ResidualSAC.eval_num_episodes = 20 38 | ResidualSAC.record_num_episodes = 0 39 | ResidualSAC.buffer_size = 1000000 40 | ResidualSAC.frame_stack = 1 41 | ResidualSAC.learning_starts = 20000 42 | ResidualSAC.update_period = 1 43 | ResidualSAC.gpu = True 44 | ResidualSAC.optimizer = @optim.Adam 45 | ResidualSAC.batch_size = 256 46 | ResidualSAC.policy_lr = 3e-4 47 | ResidualSAC.qf_lr = 3e-4 48 | ResidualSAC.gamma = 0.99 49 | ResidualSAC.target_update_period = 1 50 | ResidualSAC.policy_update_period = 4 51 | ResidualSAC.alpha = 0.2 52 | ResidualSAC.target_smoothing_coef = 0.001 53 | ResidualSAC.automatic_entropy_tuning = True 54 | ResidualSAC.target_entropy = None 55 | ResidualSAC.log_period = 100 56 | 57 | make_training_env.goal_difficulty = 3 58 | make_training_env.action_space = "torque_and_position" 59 | make_training_env.frameskip = 3 60 | make_training_env.visualization = False 61 | make_training_env.reward_fn = "gaussian_reward" 62 | make_training_env.termination_fn = "no_termination" 63 | make_training_env.initializer = "training_init" 64 | make_training_env.episode_length = 1000 65 | make_training_env.monitor = False 66 | make_training_env.seed = 0 67 | make_training_env.domain_randomization = False 68 | make_training_env.norm_observations = True 69 | make_training_env.state_machine = @MPPGStateMachine 70 | make_training_env.max_torque = %max_torque 71 | make_training_env.residual_state = 'MoveToGoalState' 72 | 73 | VecObsNormWrapper.steps = 10000 74 | VecObsNormWrapper.mean = None 75 | VecObsNormWrapper.std = None 76 | VecObsNormWrapper.eps = 1e-2 77 | VecObsNormWrapper.log = True 78 | VecObsNormWrapper.log_prob = 0.01 79 | -------------------------------------------------------------------------------- /python/residual_learning/models/mp_lvl4/logs/ckpts/000920000.pt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cbschaff/benchmark-rrc/6fb12eecbaa778c60c5728dba4414330c901b09b/python/residual_learning/models/mp_lvl4/logs/ckpts/000920000.pt -------------------------------------------------------------------------------- /python/residual_learning/models/mp_lvl4/logs/singularity_config.yaml: -------------------------------------------------------------------------------- 1 | base_config: /logdir/base.gin 2 | gin_bindings: {NetworkParams.init_std: 1.0, ResidualSAC.action_reg_weight: 0.0, ResidualSAC.alpha: 0.2, 3 | ResidualSAC.learning_starts: 20000, ResidualSAC.policy_lr: 0.0001, ResidualSAC.policy_training_start: 40000, 4 | ResidualSAC.policy_update_period: 1, ResidualSAC.policy_zero_end: 40000, ResidualSAC.q_reg_weight: 0.0, 5 | ResidualSAC.qf_fn: '@qf_append_fn', ResidualSAC.target_smoothing_coef: 0.0001, make_training_env.domain_randomization: false, 6 | make_training_env.episode_length: 1000, make_training_env.goal_difficulty: 4, make_training_env.residual_state: MoveToGoalState, 7 | make_training_env.reward_fn: gaussian_training_reward, make_training_env.state_machine: '@MPPGStateMachine', 8 | make_training_env.termination_fn: no_termination, max_torque: 0.05, train.maxt: 2000000} 9 | logdir: /logdir 10 | -------------------------------------------------------------------------------- /python/residual_learning/modules.py: -------------------------------------------------------------------------------- 1 | """Define networks and ResidualPPO2.""" 2 | from dl.modules import TanhDiagGaussian, ProductDistribution 3 | from dl import nest 4 | import torch 5 | import torch.nn as nn 6 | import numpy as np 7 | 8 | 9 | class ObservationFilter(object): 10 | """Filters information to policy and value networks for residual_learning 11 | and domain randomization.""" 12 | 13 | def get_policy_ob_shape(self, ob_space): 14 | shapes = [ 15 | ob_space['obs'][k].shape for k in ob_space['obs'].spaces 16 | if k not in ['clean', 'params', 'action'] 17 | ] 18 | return np.sum([np.prod(s) for s in shapes]) 19 | 20 | def get_value_fn_ob_shape(self, ob_space): 21 | if 'clean' not in ob_space['obs'].spaces: # dict ob space with no domain randomization 22 | shapes = [ob_space['obs'][k].shape for k in ob_space['obs'].spaces] 23 | return np.sum([np.prod(s) for s in shapes]) 24 | else: 25 | shapes = [ob_space['obs'][k].shape for k in ob_space['obs']['clean'].spaces] 26 | n = np.sum([np.prod(s) for s in shapes]) 27 | return n + np.prod(ob_space['obs']['params'].shape) 28 | 29 | def get_policy_observation(self, ob): 30 | flat_obs = nest.flatten({ 31 | k: v for k, v in ob['obs'].items() if k not in ['clean', 'params'] 32 | }) 33 | return { 34 | 'obs': torch.cat([torch.flatten(ob, 1).float() for ob in flat_obs], dim=1), 35 | 'action': ob['action'] 36 | } 37 | 38 | def get_value_fn_observation(self, ob): 39 | if 'clean' not in ob['obs']: 40 | flat_obs = torch.cat([torch.flatten(o, 1) 41 | for o in nest.flatten(ob['obs'])], 42 | dim=1) 43 | else: 44 | flat_obs = nest.flatten(ob['obs']['clean']) + nest.flatten(ob['obs']['params']) 45 | flat_obs = torch.cat([torch.flatten(ob, 1).float() for ob in flat_obs], 46 | dim=1) 47 | return { 48 | 'obs': flat_obs, 49 | 'action': ob['action'] 50 | } 51 | 52 | 53 | class TorqueAndPositionAction(nn.Module): 54 | def __init__(self, n_in, n_out_torque, n_out_position, **kwargs): 55 | super().__init__() 56 | self.torque_dist = TanhDiagGaussian(n_in, n_out_torque, **kwargs) 57 | self.position_dist = TanhDiagGaussian(n_in, n_out_position, **kwargs) 58 | 59 | def forward(self, x): 60 | return ProductDistribution({ 61 | 'torque': self.torque_dist(x), 62 | 'position': self.position_dist(x) 63 | }) 64 | 65 | 66 | class ObservationEmbedding(nn.Module): 67 | def __init__(self, obs_shape, embedding_size): 68 | super().__init__() 69 | 70 | def net(n_in): 71 | return nn.Sequential( 72 | nn.Linear(n_in, 2 * embedding_size), 73 | nn.LayerNorm(2 * embedding_size), 74 | nn.ReLU(), 75 | nn.Linear(2 * embedding_size, embedding_size), 76 | nn.LayerNorm(embedding_size) 77 | ) 78 | 79 | self.torque_embedding = net(9) 80 | self.position_embedding = net(9) 81 | self.tip_pos_embedding = net(9) 82 | self.obs_embedding = net(obs_shape) 83 | self.activation = nn.ReLU() 84 | 85 | def forward(self, obs): 86 | action = obs['action'] 87 | te = action['torque_enabled'] * self.torque_embedding(action['torque']) 88 | pe = action['position_enabled'] * ( 89 | self.position_embedding(action['position']) 90 | + self.tip_pos_embedding(torch.flatten(action['tip_positions'], 1)) 91 | ) 92 | return self.activation( 93 | torch.cat([self.obs_embedding(obs['obs']), te + pe], dim=1) 94 | ) 95 | -------------------------------------------------------------------------------- /python/residual_learning/obs_norm.pt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cbschaff/benchmark-rrc/6fb12eecbaa778c60c5728dba4414330c901b09b/python/residual_learning/obs_norm.pt -------------------------------------------------------------------------------- /python/residual_learning/obs_norm_rand.pt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cbschaff/benchmark-rrc/6fb12eecbaa778c60c5728dba4414330c901b09b/python/residual_learning/obs_norm_rand.pt -------------------------------------------------------------------------------- /python/residual_learning/residual_state_machines.py: -------------------------------------------------------------------------------- 1 | from .residual_state import ResidualState 2 | from mp.state_machines import MPStateMachine 3 | from cic.states.custom_state_machines import CICStateMachineLvl2, CICStateMachineLvl4 4 | from cpc.state_machine import CPCStateMachine, CPCStateMachineL4 5 | import os 6 | 7 | MP_LVL3_LOGDIR = os.path.join(os.path.dirname(__file__), 'models/mp_lvl3') 8 | CIC_LVL3_LOGDIR = os.path.join(os.path.dirname(__file__), 'models/cic_lvl3') 9 | CPC_LVL3_LOGDIR = os.path.join(os.path.dirname(__file__), 'models/cpc_lvl3') 10 | MP_LVL4_LOGDIR = os.path.join(os.path.dirname(__file__), 'models/mp_lvl4') 11 | CIC_LVL4_LOGDIR = os.path.join(os.path.dirname(__file__), 'models/cic_lvl4') 12 | CPC_LVL4_LOGDIR = os.path.join(os.path.dirname(__file__), 'models/cpc_lvl4') 13 | 14 | 15 | class ResidualMP_with_PG_LVL3(MPStateMachine): 16 | def build(self): 17 | init_state = super().build() 18 | self.move_to_goal = ResidualState(self.move_to_goal, MP_LVL3_LOGDIR) 19 | # rewire connections to use new state 20 | self.planned_grasp.connect(self.move_to_goal, self.align_object) 21 | return init_state 22 | 23 | 24 | class ResidualMP_with_PG_LVL4(MPStateMachine): 25 | def build(self): 26 | init_state = super().build() 27 | self.move_to_goal = ResidualState(self.move_to_goal, MP_LVL4_LOGDIR) 28 | # rewire connections to use new state 29 | self.planned_grasp.connect(self.move_to_goal, self.align_object) 30 | return init_state 31 | 32 | 33 | class ResidualCIC_with_CG_LVL3(CICStateMachineLvl2): 34 | def build(self): 35 | init_state = super().build() 36 | self.move_cube_lift = ResidualState(self.move_cube_lift, CIC_LVL3_LOGDIR) 37 | # rewire connections to use new state 38 | if not (self.new_grasp): 39 | self.approach.connect(next_state=self.move_cube_lift, done_state=self.done) 40 | self.set_cic_grasp1.connect(next_state=self.move_cube_lift, done_state=self.done) 41 | return init_state 42 | 43 | 44 | class ResidualCIC_with_CG_LVL4(CICStateMachineLvl4): 45 | def build(self): 46 | init_state = super().build() 47 | self.bring_final.move_cube_lift = ResidualState( 48 | self.bring_final.move_cube_lift, CIC_LVL4_LOGDIR) 49 | # rewire connections to use new state 50 | self.align_axes.connect(next_state=self.bring_final, done_state=self.done) 51 | self.bring_final.connect(next_state=self.align_axes, done_state=self.done) 52 | return init_state 53 | 54 | 55 | class ResidualCPC_with_TG_LVL3(CPCStateMachine): 56 | def build(self): 57 | init_state = super().build() 58 | self.move_to_goal = ResidualState(self.move_to_goal, CPC_LVL3_LOGDIR) 59 | # rewire connections to use new state 60 | self.grasp.connect(next_state=self.move_to_goal, 61 | failure_state=self.goto_init_pose) 62 | self.move_to_goal.connect( 63 | next_state=self.move_to_goal, failure_state=self.goto_init_pose) 64 | return init_state 65 | 66 | 67 | class ResidualCPC_with_TG_LVL4(CPCStateMachineL4): 68 | def build(self): 69 | init_state = super().build() 70 | self.move_to_goal = ResidualState(self.move_to_goal, CPC_LVL4_LOGDIR) 71 | # rewire connections to use new state 72 | self.grasp.connect(next_state=self.move_to_goal, 73 | failure_state=self.goto_init_pose) 74 | self.move_to_goal.connect( 75 | next_state=self.move_to_goal, failure_state=self.goto_init_pose) 76 | return init_state 77 | -------------------------------------------------------------------------------- /python/residual_learning/state_machines.py: -------------------------------------------------------------------------------- 1 | import mp.states as states 2 | import gin 3 | 4 | 5 | @gin.configurable 6 | class MPPGStateMachine(states.StateMachine): 7 | def build(self): 8 | self.goto_init_pose = states.GoToInitPoseState(self.env) 9 | self.align_object = states.AlignObjectSequenceState(self.env) 10 | self.planned_grasp = states.PlannedGraspState(self.env) 11 | self.move_to_goal = states.MoveToGoalState(self.env) 12 | self.wait = states.WaitState( 13 | self.env, 30 if self.env.simulation else 1000) 14 | self.failure = states.FailureState(self.env) 15 | 16 | # define transitions between states 17 | self.goto_init_pose.connect(next_state=self.align_object, 18 | failure_state=self.failure) 19 | self.align_object.connect(next_state=self.planned_grasp, 20 | failure_state=self.failure) 21 | self.planned_grasp.connect(next_state=self.move_to_goal, 22 | failure_state=self.align_object) 23 | self.move_to_goal.connect(next_state=None, failure_state=self.wait) 24 | self.wait.connect(next_state=self.goto_init_pose, 25 | failure_state=self.failure) 26 | 27 | # return initial state 28 | return self.goto_init_pose 29 | -------------------------------------------------------------------------------- /python/residual_learning/test_domain_rand.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | from code.make_env import make_training_env 3 | from trifinger_simulation.tasks import move_cube 4 | 5 | import statistics 6 | import argparse 7 | import pybullet as p 8 | import time 9 | 10 | import dl 11 | import numpy as np 12 | 13 | 14 | def main(args): 15 | goal = move_cube.sample_goal(args.difficulty) 16 | goal_dict = { 17 | 'position': goal.position, 18 | 'orientation': goal.orientation 19 | } 20 | eval_config = { 21 | 'cube_goal_pose': goal_dict, 22 | 'goal_difficulty': args.difficulty, 23 | 'action_space': 'torque' if args.policy == 'fc' else 'torque_and_position', 24 | 'frameskip': 3, 25 | 'reward_fn': 'competition_reward', 26 | 'termination_fn': 'no_termination', 27 | 'initializer': 'training_init', 28 | 'sim': True, 29 | 'monitor': False, 30 | 'rank': args.seed, 31 | 'training': True 32 | } 33 | env = make_training_env(visualization=True, **eval_config) 34 | 35 | acc_rewards = [] 36 | wallclock_times = [] 37 | aligning_steps = [] 38 | env_steps = [] 39 | avg_rewards = [] 40 | for i in range(args.num_episodes): 41 | start = time.time() 42 | is_done = False 43 | observation = env.reset() 44 | accumulated_reward = 0 45 | aligning_steps.append(env.unwrapped.step_count) 46 | 47 | #clear some windows in GUI 48 | p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) 49 | #change camera parameters # You can also rotate the camera by CTRL + drag 50 | p.resetDebugVisualizerCamera( cameraDistance=0.6, cameraYaw=0, cameraPitch=-40, cameraTargetPosition=[0,0,0]) 51 | 52 | step = 0 53 | while not is_done and step < 2000: 54 | step += 1 55 | action = env.action_space.sample() 56 | if isinstance(action, dict): 57 | action['torque'] *= 0 58 | action['position'] *= 0 59 | else: 60 | action *= 0 61 | observation, reward, is_done, info = env.step(action) 62 | accumulated_reward += reward 63 | acc_rewards.append(accumulated_reward) 64 | env_steps.append(env.unwrapped.step_count - aligning_steps[-1]) 65 | avg_rewards.append(accumulated_reward / env_steps[-1]) 66 | print("Episode {}\tAccumulated reward: {}".format(i, accumulated_reward)) 67 | print("Episode {}\tAlinging steps: {}".format(i, aligning_steps[-1])) 68 | print("Episode {}\tEnv steps: {}".format(i, env_steps[-1])) 69 | print("Episode {}\tAvg reward: {}".format(i, avg_rewards[-1])) 70 | end = time.time() 71 | print('Elapsed:', end - start) 72 | wallclock_times.append(end - start) 73 | 74 | env.close() 75 | 76 | def _print_stats(name, data): 77 | print('======================================') 78 | print(f'Mean {name}\t{np.mean(data):.2f}') 79 | print(f'Max {name}\t{max(data):.2f}') 80 | print(f'Min {name}\t{min(data):.2f}') 81 | print(f'Median {name}\t{statistics.median(data):2f}') 82 | 83 | print('Total elapsed time\t{:.2f}'.format(sum(wallclock_times))) 84 | print('Mean elapsed time\t{:.2f}'.format(sum(wallclock_times) / len(wallclock_times))) 85 | _print_stats('acc reward', acc_rewards) 86 | _print_stats('aligning steps', aligning_steps) 87 | _print_stats('step reward', avg_rewards) 88 | 89 | 90 | if __name__ == "__main__": 91 | parser = argparse.ArgumentParser() 92 | parser.add_argument("--difficulty", type=int, default=1, help="difficulty") 93 | parser.add_argument("--num_episodes", default=10, type=int, help="number of episodes to record a video") 94 | parser.add_argument("--seed", default=0, type=int, help="random seed") 95 | parser.add_argument("--policy", default='mpfc', choices=["fc", "mpfc"], help="which policy to run") 96 | args = parser.parse_args() 97 | 98 | print('For faster recording, run `git apply faster_recording_patch.diff`. This temporary changes episode length and window size.') 99 | 100 | dl.rng.seed(args.seed) 101 | main(args) 102 | -------------------------------------------------------------------------------- /python/residual_learning/train.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import dl 3 | import yaml 4 | 5 | 6 | if __name__ == '__main__': 7 | parser = argparse.ArgumentParser(description='Train Agent.') 8 | parser.add_argument('config', type=str, help='config') 9 | args = parser.parse_args() 10 | with open(args.config, 'r') as f: 11 | config = yaml.load(f) 12 | 13 | gin_bindings = [] 14 | for k, v in config['gin_bindings'].items(): 15 | if isinstance(v, str) and v[0] != '@': 16 | gin_bindings.append(f'{k}="{v}"') 17 | else: 18 | gin_bindings.append(f"{k}={v}") 19 | dl.load_config(config['base_config'], gin_bindings) 20 | dl.train(config['logdir']) 21 | -------------------------------------------------------------------------------- /run: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | # run the script via rosrun (for python scripts a simple symlink would also 4 | # work but using rosrun works for C++ executables as well) 5 | 6 | # See scripts/run_episode.py for arguments. 7 | rosrun rrc run_episode.py "$@" mp-pg 8 | -------------------------------------------------------------------------------- /run_locally.sh: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | 3 | if (( $# < 2 )) 4 | then 5 | echo "Invalid number of arguments." 6 | echo "Usage: $0 " 7 | exit 8 | fi 9 | 10 | rrc_root=`pwd` 11 | rrc_image=$1 12 | expdir=${rrc_root}/build 13 | export SINGULARITYENV_DISPLAY=$DISPLAY 14 | 15 | # build dir 16 | if [ -d ${expdir}/catkin_ws ] 17 | then 18 | rm -r ${expdir} 19 | fi 20 | 21 | # build catkin workspace 22 | mkdir -p ${expdir}/catkin_ws/src/usercode 23 | mkdir -p ${expdir}/logs 24 | cp -r ${rrc_root}/python ${expdir}/catkin_ws/src/usercode 25 | cp -r ${rrc_root}/*.txt ${expdir}/catkin_ws/src/usercode 26 | cp -r ${rrc_root}/*.json ${expdir}/catkin_ws/src/usercode 27 | cp -r ${rrc_root}/*.xml ${expdir}/catkin_ws/src/usercode 28 | cp -r ${rrc_root}/setup.py ${expdir}/catkin_ws/src/usercode 29 | cp -r ${rrc_root}/scripts ${expdir}/catkin_ws/src/usercode 30 | singularity exec --cleanenv --contain -B ${expdir}/catkin_ws:/ws ${rrc_image} bash -c ". /setup.bash; cd /ws; catbuild" 31 | 32 | # run command 33 | singularity exec --cleanenv --contain --nv -B ${expdir}/catkin_ws:/ws,${expdir}/logs:/logdir,/run,/dev ${rrc_image} bash -c ". /setup.bash; . /ws/devel/setup.bash; ${*:2}" 34 | -------------------------------------------------------------------------------- /run_locally_bo.sh: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | 3 | if (( $# < 2 )) 4 | then 5 | echo "Invalid number of arguments." 6 | echo "Usage: $0 " 7 | exit 8 | fi 9 | 10 | rrc_root=`pwd` 11 | rrc_image=$1 12 | expdir=${rrc_root}/build 13 | export SINGULARITYENV_DISPLAY=$DISPLAY 14 | 15 | # build dir 16 | if [ -d ${expdir}/catkin_ws ] 17 | then 18 | rm -r ${expdir} 19 | fi 20 | 21 | # build catkin workspace 22 | mkdir -p ${expdir}/catkin_ws/src/usercode 23 | mkdir -p ${expdir}/logs 24 | cp -r ${rrc_root}/python ${expdir}/catkin_ws/src/usercode 25 | cp -r ${rrc_root}/*.txt ${expdir}/catkin_ws/src/usercode 26 | cp -r ${rrc_root}/*.json ${expdir}/catkin_ws/src/usercode 27 | cp -r ${rrc_root}/*.xml ${expdir}/catkin_ws/src/usercode 28 | cp -r ${rrc_root}/setup.py ${expdir}/catkin_ws/src/usercode 29 | cp -r ${rrc_root}/scripts ${expdir}/catkin_ws/src/usercode 30 | singularity exec --cleanenv --contain -B ${expdir}/catkin_ws:/ws ${rrc_image} bash -c ". /setup.bash; cd /ws; catbuild" 31 | 32 | # run command 33 | singularity exec --cleanenv --contain --nv -B '/':/root_dir,${expdir}/catkin_ws:/ws,${expdir}/logs:/logdir,/run,/dev ${rrc_image} bash -c ". /setup.bash; . /ws/devel/setup.bash; ${*:2}" 34 | -------------------------------------------------------------------------------- /run_real_bo: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | # run the script via rosrun (for python scripts a simple symlink would also 4 | # work but using rosrun works for C++ executables as well) 5 | 6 | rosrun rrc run_episode_bo.py "$@" 7 | -------------------------------------------------------------------------------- /run_real_normal: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | # run the script via rosrun (for python scripts a simple symlink would also 4 | # work but using rosrun works for C++ executables as well) 5 | 6 | rosrun rrc run_episode.py "$@" 7 | -------------------------------------------------------------------------------- /scripts/run_episode.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | """Run a single episode with a controller on the real system.""" 3 | import argparse 4 | import json 5 | 6 | from env.make_env import make_env 7 | from mp.utils import set_seed 8 | from combined_code import create_state_machine 9 | 10 | 11 | def _init_env(goal_pose_json, difficulty): 12 | eval_config = { 13 | 'action_space': 'torque_and_position', 14 | 'frameskip': 3, 15 | 'reward_fn': 'competition_reward', 16 | 'termination_fn': 'no_termination', 17 | 'initializer': 'random_init', 18 | 'monitor': False, 19 | 'visualization': False, 20 | 'sim': False, 21 | 'rank': 0 22 | } 23 | 24 | set_seed(0) 25 | goal_pose_dict = json.loads(goal_pose_json) 26 | env = make_env(goal_pose_dict, difficulty, **eval_config) 27 | return env 28 | 29 | 30 | def main(): 31 | parser = argparse.ArgumentParser('args') 32 | parser.add_argument('difficulty', type=int) 33 | parser.add_argument('goal_pose_json', type=str) 34 | parser.add_argument('method', type=str, help="The method to run. One of 'mp-pg', 'cic-cg', 'cpc-tg'") 35 | parser.add_argument('--residual', default=False, action='store_true', 36 | help="add to use residual policies. Only compatible with difficulties 3 and 4.") 37 | parser.add_argument('--bo', default=False, action='store_true', 38 | help="add to use BO optimized parameters.") 39 | args = parser.parse_args() 40 | 41 | env = _init_env(args.goal_pose_json, args.difficulty) 42 | state_machine = create_state_machine(args.difficulty, args.method, env, 43 | args.residual, args.bo) 44 | 45 | ##################### 46 | # Run state machine 47 | ##################### 48 | obs = env.reset() 49 | state_machine.reset() 50 | 51 | done = False 52 | while not done: 53 | action = state_machine(obs) 54 | obs, _, done, _ = env.step(action) 55 | 56 | 57 | if __name__ == "__main__": 58 | main() 59 | -------------------------------------------------------------------------------- /scripts/run_local_episode.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | """Run a single episode with a controller in simulation.""" 3 | import argparse 4 | 5 | from env.make_env import make_env 6 | from trifinger_simulation.tasks import move_cube 7 | from mp.utils import set_seed 8 | from combined_code import create_state_machine 9 | 10 | 11 | def _init_env(goal_pose_dict, difficulty): 12 | eval_config = { 13 | 'action_space': 'torque_and_position', 14 | 'frameskip': 3, 15 | 'reward_fn': 'competition_reward', 16 | 'termination_fn': 'no_termination', 17 | 'initializer': 'random_init', 18 | 'monitor': False, 19 | 'visualization': True, 20 | 'sim': True, 21 | 'rank': 0 22 | } 23 | 24 | set_seed(0) 25 | env = make_env(goal_pose_dict, difficulty, **eval_config) 26 | return env 27 | 28 | 29 | def main(): 30 | parser = argparse.ArgumentParser('args') 31 | parser.add_argument('difficulty', type=int) 32 | parser.add_argument('method', type=str, help="The method to run. One of 'mp-pg', 'cic-cg', 'cpc-tg'") 33 | parser.add_argument('--residual', default=False, action='store_true', 34 | help="add to use residual policies. Only compatible with difficulties 3 and 4.") 35 | parser.add_argument('--bo', default=False, action='store_true', 36 | help="add to use BO optimized parameters.") 37 | args = parser.parse_args() 38 | goal_pose = move_cube.sample_goal(args.difficulty) 39 | goal_pose_dict = { 40 | 'position': goal_pose.position.tolist(), 41 | 'orientation': goal_pose.orientation.tolist() 42 | } 43 | 44 | env = _init_env(goal_pose_dict, args.difficulty) 45 | state_machine = create_state_machine(args.difficulty, args.method, env, 46 | args.residual, args.bo) 47 | 48 | ##################### 49 | # Run state machine 50 | ##################### 51 | obs = env.reset() 52 | state_machine.reset() 53 | 54 | done = False 55 | while not done: 56 | action = state_machine(obs) 57 | obs, _, done, _ = env.step(action) 58 | 59 | 60 | if __name__ == "__main__": 61 | main() 62 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | from distutils.core import setup 2 | from catkin_pkg.python_setup import generate_distutils_setup 3 | 4 | d = generate_distutils_setup( 5 | packages=["env", "mp", "pybullet_planning", "residual_learning", "cic", "cpc"], 6 | package_dir={"": "python"}, 7 | ) 8 | 9 | setup(**d) 10 | -------------------------------------------------------------------------------- /tools/plot/exp_align_obj.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | ''' 3 | This code traverses a directories of evaluation log files and 4 | record evaluation scores as well as plotting the results. 5 | ''' 6 | import os 7 | import argparse 8 | import json 9 | import copy 10 | from shutil import copyfile 11 | import pandas as pd 12 | import seaborn as sns 13 | import matplotlib.pyplot as plt 14 | from utils import * 15 | 16 | 17 | MAX_ALIGN_STEPS = 75000 - 1 # This depends on the evaluation code used to generate the logs 18 | 19 | def generate_csv(log_dir, csv_file): 20 | ''' 21 | Traverse and read log files, and then output csv file from the eval data. 22 | - file to be generated: 'eval_scores.csv' 23 | - columns: state_machine_id, timesteps, rot_error 24 | ''' 25 | df = pd.DataFrame(columns=['state_machine_id', 'state_machine_name', 'timesteps', 'rot_error']) 26 | model_names = extract_model_names(log_dir) 27 | 28 | # Traverse all episodes and add each entry to data frame 29 | for state_machine_id, episode_idx, episode_dir in traverse_all_episodes(log_dir): 30 | json_util = JsonUtil(os.path.join(episode_dir, 'goal.json')) 31 | entry = { 32 | 'state_machine_id': state_machine_id, 33 | 'state_machine_name': model_names[state_machine_id], 34 | **json_util.load() 35 | } 36 | 37 | # Handling the timesteps==-1 case 38 | if entry['reachfinish'] == -1: 39 | entry['reachfinish'] = MAX_ALIGN_STEPS 40 | 41 | if entry['reachstart'] == -1: 42 | raise ValueError('\'reachstart\' in {episode_dir}/goal.json does not contain a valid value.') 43 | 44 | # Rename dict keys 45 | entry['timesteps'] = entry.pop('reachfinish') - entry.pop('reachstart') 46 | entry['rot_error'] = entry.pop('align_obj_error') 47 | entry['init_rot_error'] = entry.pop('init_align_obj_error', None) 48 | 49 | # Add a new entry 50 | entry['rot_error_diff'] = entry['init_rot_error'] - entry['rot_error'] 51 | 52 | df = df.append(entry, ignore_index=True) # df.append works differently from python since it is stupid 53 | df.to_csv(csv_file, index=False) 54 | 55 | 56 | def generate_plot(input_csv_file, plot_file): 57 | data = pd.read_csv(input_csv_file) 58 | sns.scatterplot(data=data, x="timesteps", y="rot_error", hue="state_machine_name", alpha=0.8) 59 | plt.savefig(plot_file) 60 | 61 | -------------------------------------------------------------------------------- /tools/plot/exp_approach_goal.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | ''' 3 | This code traverses a directories of evaluation log files and 4 | record evaluation scores as well as plotting the results. 5 | ''' 6 | import os 7 | import argparse 8 | import json 9 | from shutil import copyfile 10 | import sys 11 | import numpy as np 12 | import pandas as pd 13 | import seaborn as sns 14 | import matplotlib.pyplot as plt 15 | from utils import * 16 | 17 | def _calc_pos_error(pos, goal_pos): 18 | xyz_dist = np.linalg.norm( 19 | np.asarray(pos) - np.asarray(goal_pos) 20 | ) 21 | return xyz_dist 22 | 23 | 24 | def generate_csv(log_dir, csv_file, df=None, tag=None, use_degree=True): 25 | ''' 26 | Traverse and read log files, and then output csv file from the eval data. 27 | - file to be generated: 'eval_scores.csv' 28 | - columns: state_machine_id, timesteps, rot_error 29 | ''' 30 | if df is None: 31 | df = pd.DataFrame(columns=['state_machine_id', 'reward']) 32 | assert 'state_machine_id' in df and 'reward' in df 33 | 34 | model_names = extract_model_names(log_dir) 35 | 36 | # Traverse all episodes and add each entry to data frame 37 | for state_machine_id, episode_idx, episode_dir in traverse_all_episodes(log_dir): 38 | reward_json = JsonUtil(os.path.join(episode_dir, 'reward.json')) 39 | goal_json = JsonUtil(os.path.join(episode_dir, 'goal.json')) 40 | obj_dropped = 'No' if goal_json.load('reachfinish') == -1 else 'Yes' 41 | 42 | goal_pos = goal_json.load(key='goal')['position'] 43 | init_obj_pose = goal_json.load(key='init_obj_pose')['position'] 44 | 45 | # NOTE: 'mp_with_cic_grasp' --> move_to_goal='mp', grasp='cic' 46 | move_to_goal, _, grasp, *_ = model_names[state_machine_id].split('_') 47 | entry = { 48 | 'state_machine_id': state_machine_id, 49 | 'state_machine_name': model_names[state_machine_id], 50 | 'grasp': grasp, 51 | 'move_to_goal': move_to_goal, 52 | 'obj_dropped': obj_dropped, 53 | 'init_pos_error': _calc_pos_error(goal_pos,init_obj_pose), 54 | 'tag': tag, 55 | # 'goal_is_outside': True if episode_idx >= 20 else False, 56 | **goal_json.load(key=['init_align_obj_error', 'rot_error_final', 'pos_error_final']), 57 | **reward_json.load(key=['reward']) 58 | } 59 | 60 | # Convert radian --> degree 61 | if use_degree: 62 | entry['rot_error_final'] *= 180 / np.pi 63 | entry['init_align_obj_error'] *= 180 / np.pi 64 | 65 | df = df.append(entry, ignore_index=True) # df.append works differently from python since it is stupid 66 | df.to_csv(csv_file, index=False) 67 | return df 68 | 69 | 70 | def generate_plot(input_csv_file, plot_file): 71 | data = pd.read_csv(input_csv_file) 72 | print (data.groupby('state_machine_name').mean()) 73 | 74 | 75 | # Create a 3-grasps x 3-move-to-goal plots 76 | sns.set_theme(style="ticks") 77 | g = sns.FacetGrid( 78 | data, col='grasp', row='move_to_goal', hue='obj_dropped', 79 | margin_titles=True, # show col & row titles nicely 80 | despine=False # show each plot in a box 81 | ) 82 | # g.map( 83 | # sns.scatterplot, 'init_align_obj_error', 'rot_error_final', style=data['goal_is_outside'], alpha=.7 84 | # ) 85 | g.map_dataframe( 86 | sns.scatterplot, 'rot_error_final', 'pos_error_final', alpha=.7 87 | ) 88 | # g.fig.subplots_adjust(wspace=0, hspace=0) 89 | g.add_legend() 90 | g.savefig(plot_file) 91 | -------------------------------------------------------------------------------- /tools/plot/plot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import argparse 3 | 4 | 5 | if __name__ == '__main__': 6 | parser = argparse.ArgumentParser() 7 | parser.add_argument('--log-dir', required=True) 8 | parser.add_argument('--csv', default='eval_scores.csv', help='specify a path to save a generated csv (default: eval_scores.csv)') 9 | parser.add_argument('--plot', default='plot.pdf', help='specify a path to save a generated plot (default: plot.pdf)') 10 | parser.add_argument('--exp', default='align', help='specify the type of experiments. Either align or mix-match (default: align)') 11 | args = parser.parse_args() 12 | 13 | if args.exp == 'align': 14 | import exp_align_obj 15 | generate_csv = exp_align_obj.generate_csv 16 | generate_plot = exp_align_obj.generate_plot 17 | elif args.exp == 'mix-match': 18 | import exp_approach_goal 19 | generate_csv = exp_approach_goal.generate_csv 20 | generate_plot = exp_approach_goal.generate_plot 21 | 22 | print('generating a csv file from log files...') 23 | generate_csv(args.log_dir, csv_file=args.csv) 24 | 25 | print('generating plot from the csv file...') 26 | generate_plot(args.csv, plot_file=args.plot) 27 | -------------------------------------------------------------------------------- /tools/plot/readme.md: -------------------------------------------------------------------------------- 1 | # What is this? 2 | 3 | A set of scripts to generate plot for align-obj experiments and mix-and-match experiments. 4 | This requires logs generated from [TUDarmstadt's evaluation code](https://github.com/cbschaff/real-robot-challenge-collab/blob/main/python/cic/bayesian_opt/eval_code.py). 5 | 6 | 7 | # How to use this: 8 | 9 | 1. Make sure that you have a properly structured log directory tree (the format is described in docstring of `traverse_all_episodes` function in `utils.py`) 10 | 2. For align-obj experiments, run following: 11 | ```bash 12 | python3 plot.py --exp align --log-dir /path/to/log/directory 13 | ``` 14 | NOTE: Make sure that you have matplotlib, seaborn and pandas. 15 | You find the scatter plot at `plot.pdf` and raw data entries at `eval_scores.csv` 16 | 17 | 3. For mix-and-match experiments, run following: 18 | ```bash 19 | python3 plot.py --exp mix-match --log-dir /path/to/log/directory 20 | ``` 21 | NOTE: Make sure that you have matplotlib, seaborn and pandas. 22 | You find the scatter plot at `plot.pdf` and raw data entries at `eval_scores.csv` 23 | -------------------------------------------------------------------------------- /tools/plot/utils.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import os 3 | import argparse 4 | import json 5 | import copy 6 | from shutil import copyfile 7 | 8 | 9 | class JsonUtil: 10 | def __init__(self, filepath): 11 | assert filepath.endswith('.json') 12 | self.filepath = filepath 13 | self.dict_ = None 14 | with open(self.filepath, 'r') as f: 15 | self.dict_ = json.load(f) 16 | 17 | def load(self, key=None): 18 | if isinstance(key, str): 19 | return self.dict_.get(key) 20 | elif isinstance(key, list): 21 | return {k: v for k, v in self.dict_.items() if k in key} 22 | elif key is None: 23 | # Returns the entire dictionary 24 | return copy.deepcopy(self.dict_) 25 | else: 26 | raise ValueError() 27 | 28 | def add_and_save(self, key, val): 29 | assert self.dict_ is not None, 'call JsonUtil.load first' 30 | if not os.path.isfile(self.filepath + '.bak'): 31 | copyfile(self.filepath, self.filepath + '.bak') # create a backup just in case 32 | with open(self.filepath, 'w') as f: 33 | self.dict_[key] = val 34 | json.dump(self.dict_, f) 35 | 36 | def listdir(directory): 37 | '''similar to os.listdir but returns fullpath''' 38 | return [os.path.join(directory, file_) for file_ in os.listdir(directory)] 39 | 40 | 41 | def traverse_all_episodes(log_dir): 42 | '''Traverses all episode directories. 43 | 44 | NOTE: expected file structure is as follows 45 | 0 46 | 0 47 | 0_output_0 48 | goal.json ("align_roterror": int, "reachfinish": int # self.reach_finish_point) 49 | ... 50 | 0_output_1 51 | ... 52 | 1 53 | 0 54 | 1_output_0 55 | ... 56 | 1_output_1 57 | ... 58 | ''' 59 | 60 | # only list directories whose names are decimal 61 | dirs = sorted([d for d in listdir(log_dir) if os.path.isdir(d) and d.split('/')[-1].isdecimal()]) 62 | for state_machine_id, dir_ in enumerate(dirs): 63 | # dir_: RRC/0 64 | episode_dirs = sorted(listdir(os.path.join(dir_, '0'))) 65 | for episode_idx, episode_dir in enumerate(episode_dirs): 66 | if (os.path.isdir(episode_dir)): 67 | yield (state_machine_id, episode_idx, episode_dir) 68 | 69 | 70 | def extract_model_names(log_dir): 71 | with open(os.path.join(log_dir, 'MODEL_ID.txt'), "r") as f: 72 | json_str = str(f.read()).replace("'", '"') 73 | return json.loads(json_str) 74 | -------------------------------------------------------------------------------- /training_scripts/build_ws.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | """Execute a submission""" 3 | import argparse 4 | import os 5 | import subprocess 6 | import shutil 7 | from shutil import ignore_patterns 8 | import yaml 9 | 10 | parser = argparse.ArgumentParser(description='Train Agent.') 11 | parser.add_argument('root', type=str, help='root') 12 | parser.add_argument('image', type=str, help='image') 13 | parser.add_argument('config', type=str, help='config') 14 | args = parser.parse_args() 15 | with open(args.config, 'r') as f: 16 | config = yaml.load(f) 17 | 18 | log_dir = os.path.join(config['logdir'], 'logs') 19 | ws_dir = os.path.join(config['logdir'], 'catkin_ws') 20 | code_dir = args.root 21 | image_path = args.image 22 | 23 | if not os.path.exists(ws_dir): 24 | os.makedirs(os.path.join(ws_dir, 'src')) 25 | shutil.copytree(os.path.join(code_dir, 'python'), 26 | os.path.join(ws_dir, 'src', 'usercode', 'python'), 27 | ignore=ignore_patterns("log*", "*.sif"), 28 | ignore_dangling_symlinks=True) 29 | shutil.copytree(os.path.join(code_dir, 'scripts'), 30 | os.path.join(ws_dir, 'src', 'usercode', 'scripts')) 31 | for file in ['CMakeLists.txt', 'goal.json', 'package.xml', 'setup.py']: 32 | shutil.copyfile(os.path.join(code_dir, file), 33 | os.path.join(ws_dir, 'src', 'usercode', file)) 34 | 35 | build_cmd = [ 36 | "singularity", 37 | "exec", 38 | "--cleanenv", 39 | "--contain", 40 | "-B", 41 | "{}:/ws".format(ws_dir), 42 | image_path, 43 | "bash", 44 | "-c", 45 | ". /setup.bash; cd /ws; catbuild", 46 | ] 47 | proc = subprocess.run( 48 | build_cmd, 49 | check=True, 50 | stdout=subprocess.PIPE, 51 | stderr=subprocess.PIPE, 52 | ) 53 | 54 | # copy config file into singularity image 55 | os.makedirs(log_dir) 56 | config['logdir'] = '/logdir' 57 | shutil.copyfile(config['base_config'], os.path.join(log_dir, 'base.gin')) 58 | config['base_config'] = '/logdir/base.gin' 59 | with open(os.path.join(log_dir, 'singularity_config.yaml'), 'w') as f: 60 | yaml.dump(config, f) 61 | 62 | 63 | # print singularity mount points 64 | print(ws_dir + ':/ws,' + log_dir + ':/logdir') 65 | -------------------------------------------------------------------------------- /training_scripts/configs/cic_cg.yaml: -------------------------------------------------------------------------------- 1 | # This is an example config for train_ppo.py that can be used to run it locally. 2 | logdir: logs_cic_cg 3 | base_config: training_scripts/configs/residual_sac.gin 4 | gin_bindings: 5 | max_torque: 0.1 6 | make_training_env.goal_difficulty: 3 7 | make_training_env.state_machine: '@CICStateMachineLvl2' 8 | make_training_env.residual_state: 'MoveLiftCubePrimitiveLvl2' 9 | -------------------------------------------------------------------------------- /training_scripts/configs/cpc_tg_lvl3.yml: -------------------------------------------------------------------------------- 1 | # This is an example config for train_ppo.py that can be used to run it locally. 2 | logdir: logs_cpc_tg_lvl3 3 | base_config: training_scripts/configs/residual_sac.gin 4 | gin_bindings: 5 | max_torque: 0.1 6 | make_training_env.goal_difficulty: 3 7 | make_training_env.state_machine: '@CPCStateMachine' 8 | make_training_env.residual_state: 'MoveToGoalState' 9 | -------------------------------------------------------------------------------- /training_scripts/configs/cpc_tg_lvl4.yml: -------------------------------------------------------------------------------- 1 | # This is an example config for train_ppo.py that can be used to run it locally. 2 | logdir: logs_cpc_tg_lvl4 3 | base_config: training_scripts/configs/residual_sac.gin 4 | gin_bindings: 5 | max_torque: 0.1 6 | make_training_env.goal_difficulty: 4 7 | make_training_env.state_machine: '@CPCStateMachineL4' 8 | make_training_env.residual_state: 'GoalWithOrientState' 9 | -------------------------------------------------------------------------------- /training_scripts/configs/mp_pg.yaml: -------------------------------------------------------------------------------- 1 | # This is an example config for train_ppo.py that can be used to run it locally. 2 | logdir: logs_mp_pg 3 | base_config: training_scripts/configs/residual_sac.gin 4 | gin_bindings: 5 | max_torque: 0.1 6 | make_training_env.goal_difficulty: 4 7 | make_training_env.state_machine: '@MPPGStateMachine' 8 | make_training_env.residual_state: 'MoveToGoalState' 9 | -------------------------------------------------------------------------------- /training_scripts/configs/residual_sac.gin: -------------------------------------------------------------------------------- 1 | import residual_learning.residual_sac 2 | import residual_learning.networks 3 | import residual_learning.state_machines 4 | import residual_learning.make_training_env 5 | import cpc.state_machine 6 | import cic.states.custom_state_machines 7 | 8 | train.algorithm = @ResidualSAC 9 | train.maxt = 1000000 10 | train.seed = 0 11 | train.eval = True 12 | train.eval_period = 100000 13 | train.save_period = 10000 14 | train.maxseconds = None 15 | 16 | optim.Adam.betas = (0.9, 0.999) 17 | 18 | 19 | max_torque = 0.1 20 | 21 | 22 | NetworkParams.size = 256 23 | NetworkParams.embedding_size = 64 24 | NetworkParams.max_torque = %max_torque 25 | NetworkParams.pi_layers = 2 26 | NetworkParams.vf_layers = 3 27 | NetworkParams.init_std = 1.0 28 | 29 | Checkpointer.ckpt_period = 10000 30 | 31 | ResidualSAC.policy_training_start = 40000 32 | ResidualSAC.policy_zero_end = 40000 33 | ResidualSAC.env_fn = @make_training_env 34 | ResidualSAC.policy_fn = @policy_fn 35 | ResidualSAC.qf_fn = @qf_append_fn 36 | ResidualSAC.nenv = 1 37 | ResidualSAC.eval_num_episodes = 20 38 | ResidualSAC.record_num_episodes = 0 39 | ResidualSAC.buffer_size = 1000000 40 | ResidualSAC.frame_stack = 1 41 | ResidualSAC.learning_starts = 20000 42 | ResidualSAC.update_period = 1 43 | ResidualSAC.gpu = True 44 | ResidualSAC.optimizer = @optim.Adam 45 | ResidualSAC.batch_size = 256 46 | ResidualSAC.policy_lr = 1e-4 47 | ResidualSAC.qf_lr = 3e-4 48 | ResidualSAC.gamma = 0.99 49 | ResidualSAC.target_update_period = 1 50 | ResidualSAC.policy_update_period = 1 51 | ResidualSAC.alpha = 0.2 52 | ResidualSAC.target_smoothing_coef = 0.0001 53 | ResidualSAC.automatic_entropy_tuning = True 54 | ResidualSAC.target_entropy = None 55 | ResidualSAC.log_period = 100 56 | ResidualSAC.action_reg_weight = 0.0 57 | ResidualSAC.q_reg_weight = 0.0 58 | 59 | make_training_env.goal_difficulty = 3 60 | make_training_env.action_space = "torque_and_position" 61 | make_training_env.frameskip = 3 62 | make_training_env.visualization = False 63 | make_training_env.reward_fn = "gaussian_training_reward" 64 | make_training_env.termination_fn = "no_termination" 65 | make_training_env.initializer = "training_init" 66 | make_training_env.episode_length = 1000 67 | make_training_env.monitor = False 68 | make_training_env.seed = 0 69 | make_training_env.domain_randomization = False 70 | make_training_env.norm_observations = True 71 | make_training_env.state_machine = @MPPGStateMachine 72 | make_training_env.max_torque = %max_torque 73 | make_training_env.residual_state = 'MoveToGoalState' 74 | 75 | VecObsNormWrapper.steps = 10000 76 | VecObsNormWrapper.mean = None 77 | VecObsNormWrapper.std = None 78 | VecObsNormWrapper.eps = 1e-2 79 | VecObsNormWrapper.log = True 80 | VecObsNormWrapper.log_prob = 0.01 81 | -------------------------------------------------------------------------------- /training_scripts/evaluate.sh: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | image=$1 3 | # remove trailing slash 4 | logdir=${2%/} 5 | rrc_root="$( cd "$( dirname "$( dirname "${BASH_SOURCE[0]}" )" )" &> /dev/null && pwd )" 6 | cd ${rrc_root} 7 | echo `date` 8 | 9 | # remove trailing slash 10 | mounts="${logdir}/catkin_ws:/ws,${logdir}/logs:/logdir" 11 | echo ${mounts} 12 | echo `date` 13 | script_path=src/usercode/python/residual_learning/eval.py 14 | 15 | singularity exec --contain --nv -B ${mounts} ${image} bash -c \ 16 | ". /setup.bash; . /ws/devel/setup.bash; python3 /ws/${script_path} ${*:3}" 17 | -------------------------------------------------------------------------------- /training_scripts/run_script_local.sh: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | image=$1 3 | config=$2 4 | rrc_root="$( cd "$( dirname "$( dirname "${BASH_SOURCE[0]}" )" )" &> /dev/null && pwd )" 5 | echo ${rrc_root} 6 | echo ${image} 7 | echo ${config} 8 | 9 | cd $rrc_root 10 | echo `date` 11 | mounts=`python3 training_scripts/build_ws.py ${rrc_root} ${image} ${config}` 12 | echo ${mounts} 13 | echo `date` 14 | singularity exec --contain --nv -B ${mounts} ${image} bash -c \ 15 | ". /setup.bash; . /ws/devel/setup.bash; timeout -s SIGINT -k 0.1h --foreground 3.5h python3 /ws/src/usercode/python/residual_learning/train.py /logdir/singularity_config.yaml" 16 | -------------------------------------------------------------------------------- /training_scripts/visualize.sh: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | image=$1 3 | # remove trailing slash 4 | logdir=${2%/} 5 | rrc_root="$( cd "$( dirname "$( dirname "${BASH_SOURCE[0]}" )" )" &> /dev/null && pwd )" 6 | cd ${rrc_root} 7 | echo `date` 8 | 9 | mounts="${logdir}/catkin_ws:/ws,${logdir}/logs:/logdir" 10 | echo ${mounts} 11 | echo `date` 12 | script_path=src/usercode/python/residual_learning/viz.py 13 | 14 | singularity exec --contain --nv -B ${mounts} ${image} bash -c \ 15 | ". /setup.bash; . /ws/devel/setup.bash; python3 /ws/${script_path} ${*:3}" 16 | --------------------------------------------------------------------------------