├── .circleci └── config.yml ├── .flake8 ├── .github ├── CONTRIBUTING.md ├── ISSUE_TEMPLATE │ ├── bug_report.md │ └── feature_request.md ├── PULL_REQUEST_TEMPLATE.md └── workflows │ └── main.yml ├── .gitignore ├── .pre-commit-config.yaml ├── BUILD ├── CODE_OF_CONDUCT.md ├── LICENSE ├── MANIFEST.in ├── README.md ├── WORKSPACE ├── docs └── images │ └── demo.jpg ├── examples ├── control_panel.py ├── with_pybulletx.py └── without_pybulletx.py ├── pybulletX ├── __init__.py ├── _wrapper.py ├── body.py ├── client.py ├── contact_point.py ├── dynamics_info.py ├── gui │ ├── __init__.py │ └── control_panel.py ├── helper.py ├── joint_info.py ├── joint_state.py ├── joint_type.py ├── link_state.py ├── mapping_mixin.py ├── robot.py ├── robot_interface.py ├── robot_interface_mixin.py └── utils │ ├── __init__.py │ ├── loop_thread.py │ ├── simulation_thread.py │ ├── soft_real_time_clock.py │ └── space_dict.py ├── requirements-dev.txt ├── requirements.txt ├── setup.py ├── tests ├── conftest.py ├── test_additional_search_path.py ├── test_client.py ├── test_contact_point.py ├── test_dynamics_info.py ├── test_free_joint_indices.py ├── test_joint_info.py ├── test_joint_state.py ├── test_link_state.py ├── test_pybullet_body.py ├── test_pybullet_robot.py ├── test_robot_interface.py ├── test_robot_state_space_configuration.py └── test_space_dict.py └── website ├── .gitignore ├── README.md ├── babel.config.js ├── blog ├── 2019-05-28-hola.md ├── 2019-05-29-hello-world.md └── 2019-05-30-welcome.md ├── docs ├── installation.md ├── intro.md ├── mdx.md └── tutorial │ ├── body.md │ ├── control_panel.md │ ├── joint_info.md │ ├── putting_all_together.md │ ├── pybullet_world.md │ ├── robot.md │ └── simulation_thread.md ├── docusaurus.config.js ├── package.json ├── sidebars.js ├── src ├── css │ └── custom.css └── pages │ ├── index.js │ └── styles.module.css ├── static ├── .nojekyll └── img │ ├── favicon.ico │ └── tutorial │ └── pybullet_world.jpg └── yarn.lock /.circleci/config.yml: -------------------------------------------------------------------------------- 1 | # Copyright (c) Facebook, Inc. and its affiliates. 2 | 3 | # This source code is licensed under the MIT license found in the 4 | # LICENSE file in the root directory of this source tree. 5 | 6 | version: 2.1 7 | 8 | jobs: 9 | py37_linux: 10 | docker: 11 | - image: circleci/python:3.7 12 | steps: 13 | - checkout 14 | - run: 15 | name: Test pybulletX 16 | command: | 17 | pip3 install -U pip 18 | pip3 install -e . 19 | pip3 install -r requirements-dev.txt 20 | pytest 21 | 22 | workflows: 23 | version: 2 24 | build: 25 | jobs: 26 | - py37_linux 27 | -------------------------------------------------------------------------------- /.flake8: -------------------------------------------------------------------------------- 1 | [flake8] 2 | exclude = .git 3 | max-line-length = 119 4 | copyright-check = True 5 | select = E,F,W,C 6 | copyright-regexp=Copyright \(c\) Facebook, Inc. and its affiliates. All Rights Reserved 7 | ignore=W503,E203,E731,E722 8 | -------------------------------------------------------------------------------- /.github/CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | # Contributing to pybulletX 2 | We want to make contributing to this project as easy and transparent as possible. 3 | 4 | ## Pull Requests 5 | We actively welcome your pull requests. 6 | 7 | 1. Fork the repo and create your branch from `main`. 8 | 2. If you've added code that should be tested, add tests. 9 | 3. If you've changed APIs, update the documentation. 10 | 4. Ensure the test suite passes. 11 | 5. Make sure your code lints. 12 | 6. If you haven't already, complete the Contributor License Agreement ("CLA"). 13 | 14 | ## Contributor License Agreement ("CLA") 15 | In order to accept your pull request, we need you to submit a CLA. You only need to do this once to work on any of Facebook's open source projects. 16 | 17 | Complete your CLA here: 18 | 19 | ## Issues 20 | We use GitHub issues to track public bugs. Please ensure your description is clear and has sufficient instructions to be able to reproduce the issue. 21 | 22 | ## License 23 | By contributing to pybulletX, you agree that your contributions will be licensed under the [LICENSE](LICENSE) file in the root directory of this source tree. 24 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/bug_report.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: "\U0001F41B Bug report" 3 | about: Create a bug report to help us improve PyBulletX 4 | title: "[Bug]" 5 | labels: bug 6 | assignees: '' 7 | 8 | --- 9 | 10 | # 🐛 Bug 11 | ## Description 12 | 13 | 14 | ## Checklist 15 | - [ ] I checked on the latest version of PyBulletX 16 | - [ ] I created a minimal repro 17 | 18 | ## To reproduce 19 | 20 | ** Minimal Code/Config snippet to reproduce ** 21 | 22 | ** Stack trace/error message ** 23 | ``` 24 | // Paste the bad output here! 25 | ``` 26 | 27 | ## Expected Behavior 28 | 29 | 30 | ## System information 31 | - **PyBulletX Version** : 32 | - **Python version** : 33 | - **Virtual environment type and version** : 34 | - **Operating system** : 35 | 36 | ## Additional context 37 | Add any other context about the problem here. 38 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/feature_request.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: "\U0001F680 Feature request" 3 | about: Suggest an idea for a new feature in PyBulletX 4 | title: "[Feature Request]" 5 | labels: enhancement 6 | assignees: '' 7 | 8 | --- 9 | 10 | # 🚀 Feature Request 11 | 12 | 13 | 14 | ## Motivation 15 | 16 | **Is your feature request related to a problem? Please describe.** 17 | 18 | 19 | 20 | ## Pitch 21 | 22 | **Describe the solution you'd like** 23 | 24 | 25 | **Describe alternatives you've considered** 26 | 27 | 28 | **Are you willing to open a pull request?** (See [CONTRIBUTING](../CONTRIBUTING.md)) 29 | 30 | ## Additional context 31 | Add any other context or screenshots about the feature request here. 32 | -------------------------------------------------------------------------------- /.github/PULL_REQUEST_TEMPLATE.md: -------------------------------------------------------------------------------- 1 | 2 | 3 | ## Motivation 4 | 5 | (Write your motivation for proposed changes here.) 6 | 7 | ### Have you read the [Contributing Guidelines on pull requests](https://github.com/facebookresearch/pybulletX/blob/main/.github/CONTRIBUTING.md)? 8 | 9 | Yes/No 10 | 11 | ## Test Plan 12 | 13 | (How should this PR be tested? Do you require special setup to run the test or repro the fixed bug?) 14 | 15 | ## Related Issues and PRs 16 | 17 | (Is this PR part of a group of changes? Link the other relevant PRs and Issues here. Use https://help.github.com/en/articles/closing-issues-using-keywords for help on GitHub syntax) 18 | -------------------------------------------------------------------------------- /.github/workflows/main.yml: -------------------------------------------------------------------------------- 1 | # This is a basic workflow to help you get started with Actions 2 | 3 | name: CI 4 | 5 | # Controls when the action will run. Triggers the workflow on push or pull request 6 | # events but only for the master branch 7 | on: push 8 | 9 | # A workflow run is made up of one or more jobs that can run sequentially or in parallel 10 | jobs: 11 | # This workflow contains a single job called "build" 12 | build: 13 | # The type of runner that the job will run on 14 | runs-on: ubuntu-latest 15 | 16 | # Steps represent a sequence of tasks that will be executed as part of the job 17 | steps: 18 | # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it 19 | - uses: actions/checkout@v2 20 | 21 | - name: Setup Python 22 | uses: actions/setup-python@v2 23 | with: 24 | # Version range or exact version of a Python version to use, using SemVer's version range syntax. 25 | python-version: '3.6.9 - 3.8.5' 26 | 27 | # Runs a set of commands using the runners shell 28 | - name: Install & test pybulletX 29 | run: | 30 | python -m pip install -e . 31 | python -m pip install -r requirements-dev.txt 32 | pytest 33 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.egg-info 2 | 3 | *.pyc 4 | __pycache__ 5 | 6 | bazel-* 7 | 8 | .coverage 9 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | default_language_version: 2 | python: python3.7 3 | repos: 4 | - repo: https://github.com/psf/black 5 | rev: stable 6 | hooks: 7 | - id: black 8 | language_version: python3.7 9 | 10 | - repo: https://gitlab.com/pycqa/flake8 11 | rev: 3.7.9 12 | hooks: 13 | - id: flake8 14 | additional_dependencies: [-e, "git+git://github.com/pycqa/pyflakes.git@1911c20#egg=pyflakes"] 15 | -------------------------------------------------------------------------------- /BUILD: -------------------------------------------------------------------------------- 1 | py_library( 2 | name = "pybulletX", 3 | imports = [ 4 | "./", 5 | ], 6 | srcs = glob([ 7 | "pybulletX/**/*.py", 8 | ]), 9 | ) 10 | -------------------------------------------------------------------------------- /CODE_OF_CONDUCT.md: -------------------------------------------------------------------------------- 1 | # Code of Conduct 2 | Facebook has adopted a Code of Conduct that we expect project participants to adhere to. Please [read the full text](https://code.fb.com/codeofconduct) so that you can understand what actions will and will not be tolerated. 3 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) Facebook, Inc. and its affiliates. 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /MANIFEST.in: -------------------------------------------------------------------------------- 1 | global-exclude *.pyc 2 | global-exclude __pycache__ 3 | recursive-include pybulletX/* * 4 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # pybulletX 2 | 3 | [![License: MIT](https://img.shields.io/badge/License-MIT-green.svg)](LICENSE) 4 | [![GitHubCI](https://github.com/facebookresearch/pybulletX/workflows/CI/badge.svg)](https://github.com/facebookresearch/pybulletX/actions) 5 | [![CircleCI](https://circleci.com/gh/facebookresearch/pybulletX.svg?style=shield&circle-token=ad4f47a46ed4cc4ff976cdd2f79fcf7ef4494459)](https://circleci.com/gh/facebookresearch/pybulletX) 6 | [![Code style: black](https://img.shields.io/badge/code%20style-black-000000.svg)](https://github.com/psf/black) 7 | [![Total alerts](https://img.shields.io/lgtm/alerts/g/facebookresearch/pybulletX.svg?logo=lgtm&logoWidth=18)](https://lgtm.com/projects/g/facebookresearch/pybulletX/alerts/) 8 | [![Language grade: Python](https://img.shields.io/lgtm/grade/python/g/facebookresearch/pybulletX.svg?logo=lgtm&logoWidth=18)](https://lgtm.com/projects/g/facebookresearch/pybulletX/context:python) 9 | 10 | 11 | The lightweight pybullet wrapper for robotics researchers. 12 | Build robot simulation with less code. 13 | Scale your research with less boilerplate. 14 | 15 | ## Installation 16 | 17 | The preferred way of installation is through PyPi: 18 | ```bash 19 | pip install pybulletX 20 | ``` 21 | 22 | Alternatively, you can clone the repository and install the package using: 23 | ```bash 24 | git clone https://github.com/facebookresearch/pybulletX.git 25 | cd pybulletX/ && pip install -e . 26 | ``` 27 | 28 | ## Examples 29 | Here is an example of controlling Kuka arm with PyBulletX. 30 | 31 | ```python 32 | import time 33 | 34 | import numpy as np 35 | import pybullet as p 36 | import pybulletX as px 37 | 38 | P_GAIN = 50 39 | desired_joint_positions = np.array([1.218, 0.507, -0.187, 1.235, 0.999, 1.279, 0]) 40 | 41 | def main(): 42 | px.init() 43 | 44 | robot = px.Robot("kuka_iiwa/model.urdf", use_fixed_base=True) 45 | robot.torque_control = True 46 | 47 | while True: 48 | time.sleep(0.01) 49 | 50 | error = desired_joint_positions - robot.get_states().joint_position 51 | actions = robot.action_space.new() 52 | actions.joint_torque = error * P_GAIN 53 | robot.set_actions(actions) 54 | 55 | p.stepSimulation() 56 | 57 | if __name__ == "__main__": 58 | main() 59 | ``` 60 | 61 | Here is the same example but without PyBulletX. 62 | ```python 63 | import time 64 | 65 | import numpy as np 66 | import pybullet as p 67 | import pybullet_data 68 | 69 | P_GAIN = 50 70 | desired_joint_positions = np.array([1.218, 0.507, -0.187, 1.235, 0.999, 1.279, 0]) 71 | 72 | def main(): 73 | p.connect(p.GUI) 74 | 75 | p.setAdditionalSearchPath(pybullet_data.getDataPath()) 76 | p.loadURDF("plane.urdf") 77 | 78 | robot_id = p.loadURDF("kuka_iiwa/model.urdf", useFixedBase=True) 79 | 80 | num_dofs = 7 81 | joint_indices = range(num_dofs) 82 | 83 | # The magic that enables torque control 84 | p.setJointMotorControlArray( 85 | bodyIndex=robot_id, 86 | jointIndices=joint_indices, 87 | controlMode=p.VELOCITY_CONTROL, 88 | forces=np.zeros(num_dofs), 89 | ) 90 | 91 | while True: 92 | time.sleep(0.01) 93 | 94 | joint_states = p.getJointStates(robot_id, joint_indices) 95 | joint_positions = np.array([j[0] for j in joint_states]) 96 | error = desired_joint_positions - joint_positions 97 | torque = error * P_GAIN 98 | 99 | p.setJointMotorControlArray( 100 | bodyIndex=robot_id, 101 | jointIndices=joint_indices, 102 | controlMode=p.TORQUE_CONTROL, 103 | forces=torque, 104 | ) 105 | 106 | p.stepSimulation() 107 | 108 | if __name__ == "__main__": 109 | main() 110 | ``` 111 | 112 | The examples above are available in `examples/with_pybulletX.py` and `examples/without_pybulletX.py`. 113 | 114 | ## License 115 | PyBulletX is licensed under [MIT License](LICENSE). 116 | -------------------------------------------------------------------------------- /WORKSPACE: -------------------------------------------------------------------------------- 1 | workspace(name = "pybulletX") 2 | -------------------------------------------------------------------------------- /docs/images/demo.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/facebookresearch/pybulletX/38877701d2eb875d7493f6f45172af94eae3b5a7/docs/images/demo.jpg -------------------------------------------------------------------------------- /examples/control_panel.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved 2 | import pybulletX as px 3 | 4 | 5 | def main(): 6 | px.init() 7 | 8 | robot = px.Robot("kuka_iiwa/model.urdf") 9 | 10 | # Run the simulation in another thread 11 | t = px.utils.SimulationThread(1.0) 12 | t.start() 13 | 14 | # ControlPanel also takes pybulletX.Body 15 | panel = px.gui.RobotControlPanel(robot) 16 | panel.start() 17 | 18 | input("Press any key to exit ...") 19 | 20 | 21 | if __name__ == "__main__": 22 | main() 23 | -------------------------------------------------------------------------------- /examples/with_pybulletx.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved 2 | import time 3 | import numpy as np 4 | 5 | import pybullet as p 6 | import pybulletX as px 7 | 8 | P_GAIN = 50 9 | desired_joint_positions = np.array([1.218, 0.507, -0.187, 1.235, 0.999, 1.279, 0]) 10 | 11 | 12 | def main(): 13 | px.init() 14 | 15 | robot = px.Robot("kuka_iiwa/model.urdf", use_fixed_base=True) 16 | robot.torque_control = True 17 | 18 | while True: 19 | time.sleep(0.01) 20 | 21 | error = desired_joint_positions - robot.get_states().joint_position 22 | actions = robot.action_space.new() 23 | actions.joint_torque = error * P_GAIN 24 | robot.set_actions(actions) 25 | 26 | p.stepSimulation() 27 | 28 | 29 | if __name__ == "__main__": 30 | main() 31 | -------------------------------------------------------------------------------- /examples/without_pybulletx.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved 2 | import time 3 | 4 | import numpy as np 5 | import pybullet as p 6 | import pybullet_data 7 | 8 | P_GAIN = 50 9 | desired_joint_positions = np.array([1.218, 0.507, -0.187, 1.235, 0.999, 1.279, 0]) 10 | 11 | 12 | def main(): 13 | p.connect(p.GUI) 14 | 15 | p.setAdditionalSearchPath(pybullet_data.getDataPath()) 16 | p.loadURDF("plane.urdf") 17 | 18 | robot_id = p.loadURDF("kuka_iiwa/model.urdf", useFixedBase=True) 19 | 20 | num_dofs = 7 21 | joint_indices = range(num_dofs) 22 | 23 | # The magic that enables torque control 24 | p.setJointMotorControlArray( 25 | bodyIndex=robot_id, 26 | jointIndices=joint_indices, 27 | controlMode=p.VELOCITY_CONTROL, 28 | forces=np.zeros(num_dofs), 29 | ) 30 | 31 | while True: 32 | time.sleep(0.01) 33 | 34 | joint_states = p.getJointStates(robot_id, joint_indices) 35 | joint_positions = np.array([j[0] for j in joint_states]) 36 | error = desired_joint_positions - joint_positions 37 | torque = error * P_GAIN 38 | 39 | p.setJointMotorControlArray( 40 | bodyIndex=robot_id, 41 | jointIndices=joint_indices, 42 | controlMode=p.TORQUE_CONTROL, 43 | forces=torque, 44 | ) 45 | 46 | p.stepSimulation() 47 | 48 | 49 | if __name__ == "__main__": 50 | main() 51 | -------------------------------------------------------------------------------- /pybulletX/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved 2 | __version__ = "0.4.1" 3 | 4 | from .joint_info import JointInfo # noqa: F401 5 | from .joint_state import JointState # noqa: F401 6 | from .link_state import LinkState # noqa: F401 7 | from .contact_point import ContactPoint # noqa: F401 8 | from ._wrapper import _replace_original_methods 9 | from pybullet import stepSimulation, resetDebugVisualizerCamera # noqa: F401 10 | import pybullet_data as _p_data 11 | 12 | _replace_original_methods() 13 | 14 | from . import gui # noqa: F401 15 | from . import helper # noqa: F401 16 | from . import utils # noqa: F401 17 | from .client import current_client, Client # noqa: F401 18 | from .body import Body # noqa: F401 19 | from .robot import Robot # noqa: F401 20 | 21 | from .helper import init, init_pybullet # noqa: F401 22 | 23 | path = [_p_data.getDataPath()] 24 | -------------------------------------------------------------------------------- /pybulletX/_wrapper.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved 2 | import logging as _logging 3 | import pybullet as _pybullet 4 | import numpy as _numpy 5 | 6 | from .joint_info import JointInfo 7 | from .joint_state import JointState 8 | from .link_state import LinkState 9 | from .dynamics_info import DynamicsInfo 10 | 11 | 12 | _log = _logging.getLogger(__name__) 13 | 14 | 15 | def _to_struct_of_array(array_of_struct): 16 | """ 17 | utility function that convert array of struct (AoS) to struct of array (SoA), 18 | which is more suitable for ML application down the road (ex: PyTorch). 19 | """ 20 | if not array_of_struct: 21 | return {} 22 | keys = array_of_struct[0].__dict__.keys() 23 | return { 24 | k: _numpy.array([getattr(struct, k) for struct in array_of_struct]) 25 | for k in keys 26 | } 27 | 28 | 29 | # store the original pybullet global functions here 30 | class _orig_pybullet: 31 | getJointState = _pybullet.getJointState 32 | getJointStates = _pybullet.getJointStates 33 | 34 | getLinkState = _pybullet.getLinkState 35 | getLinkStates = _pybullet.getLinkStates 36 | 37 | # Unlike getJointState/getJointStates and getLinkState/getLinkStates, 38 | # there is only one function for getting joint information in pybullet 39 | # , which is getJointInfo 40 | getJointInfo = _pybullet.getJointInfo 41 | 42 | getDynamicsInfo = _pybullet.getDynamicsInfo 43 | 44 | 45 | def _getJointInfo(*args, **kwargs): 46 | joint_info_tuple = _orig_pybullet.getJointInfo(*args, **kwargs) 47 | joint_info = JointInfo(*joint_info_tuple) 48 | return joint_info 49 | 50 | 51 | def _getJointInfos(bodyUniqueId, jointIndices, **kwargs): 52 | joint_info_tuples = [ 53 | _orig_pybullet.getJointInfo(bodyUniqueId, joint_index, **kwargs) 54 | for joint_index in jointIndices 55 | ] 56 | 57 | if joint_info_tuples: 58 | joint_infos = list(map(_numpy.array, zip(*joint_info_tuples))) 59 | joint_infos = JointInfo(*joint_infos) 60 | joint_infos._plural = True 61 | joint_infos._data = joint_info_tuples 62 | return joint_infos 63 | else: 64 | return None 65 | 66 | 67 | def _getJointState(*args, **kwargs): 68 | joint_state_tuple = _orig_pybullet.getJointState(*args, **kwargs) 69 | joint_state = JointState(*joint_state_tuple) 70 | return joint_state 71 | 72 | 73 | def _getJointStates(*args, **kwargs): 74 | joint_state_tuples = _orig_pybullet.getJointStates(*args, **kwargs) 75 | if not joint_state_tuples: 76 | return None 77 | 78 | # tranpose tuple of tuples using zip & map 79 | joint_states = list(map(_numpy.array, zip(*joint_state_tuples))) 80 | joint_states = JointState(*joint_states) 81 | joint_states._plural = True 82 | joint_states._data = joint_state_tuples 83 | return joint_states 84 | 85 | 86 | def _getLinkState(*args, **kwargs): 87 | link_state_tuple = _orig_pybullet.getLinkState(*args, **kwargs) 88 | link_state = LinkState(*link_state_tuple) 89 | return link_state 90 | 91 | 92 | def _getLinkStates(*args, **kwargs): 93 | link_state_tuples = _orig_pybullet.getLinkStates(*args, **kwargs) 94 | if not link_state_tuples: 95 | return None 96 | 97 | link_states = list(map(_numpy.array, zip(*link_state_tuples))) 98 | link_states = LinkState(*link_states) 99 | link_states._plural = True 100 | link_states._data = link_state_tuples 101 | return link_states 102 | 103 | 104 | def _getDynamicsInfo(bodyUniqueId, linkIndex, **kwargs): 105 | dynamics_info_tuple = _orig_pybullet.getDynamicsInfo( 106 | bodyUniqueId, linkIndex, **kwargs 107 | ) 108 | dynamics_info = DynamicsInfo(*dynamics_info_tuple) 109 | return dynamics_info 110 | 111 | 112 | def _getDynamicsInfos(bodyUniqueId, linkIndices, **kwargs): 113 | dynamics_info_tuples = [ 114 | _orig_pybullet.getDynamicsInfo(bodyUniqueId, link_index, **kwargs) 115 | for link_index in linkIndices 116 | ] 117 | 118 | if dynamics_info_tuples: 119 | dynamics_infos = list(map(_numpy.array, zip(*dynamics_info_tuples))) 120 | dynamics_infos = DynamicsInfo(*dynamics_infos) 121 | dynamics_infos._plural = True 122 | dynamics_infos._data = dynamics_info_tuples 123 | return dynamics_infos 124 | else: 125 | return None 126 | 127 | 128 | def _setParameters(cfg, physicsClientId=None): 129 | r""" 130 | A helper function that sets multiple parameters of pybullet from a dict-like 131 | object. 132 | 133 | Example:: 134 | >>> pybullet.setParameters({ 135 | "gravity": { 136 | "gravX": 0, 137 | "gravY": 0, 138 | "gravZ": -9.81, 139 | }, 140 | "timeStep": 0.001, 141 | "physicsEngineParameter": { 142 | "numSolverIterations": 2000, 143 | "solverResidualThreshold": 1e-30, 144 | "reportSolverAnalytics": True, 145 | } 146 | }) 147 | """ 148 | for key, value in cfg.items(): 149 | setter_method_name = "set{}".format(key[0].upper() + key[1:]) 150 | setter = getattr(_pybullet, setter_method_name) 151 | 152 | args = [] 153 | kwargs = {} 154 | 155 | # if value is a dict-like, copy it to kwargs. Otherwise, treat value as args 156 | try: 157 | # FIXME(poweic): should use isinstance(value, collections.abc.Mapping) 158 | # but some of the useful package (ex: OmegaConf) doesn't inherits from 159 | # collections.abc.Mapping. Use try except instead. 160 | kwargs = dict(**value) 161 | except TypeError: 162 | args.append(value) 163 | 164 | if physicsClientId is not None: 165 | kwargs["physicsClientId"] = physicsClientId 166 | 167 | param_str = ", ".join([f"{k}={v}" for k, v in kwargs.items()]) 168 | _log.info(f"Calling pybullet.{setter_method_name}({param_str})") 169 | setter(*args, **kwargs) 170 | 171 | 172 | def _replace_original_methods(): 173 | _pybullet.getJointInfo = _getJointInfo 174 | _pybullet.getJointInfos = _getJointInfos 175 | 176 | _pybullet.getJointState = _getJointState 177 | _pybullet.getJointStates = _getJointStates 178 | 179 | _pybullet.getLinkState = _getLinkState 180 | _pybullet.getLinkStates = _getLinkStates 181 | 182 | _pybullet.getDynamicsInfo = _getDynamicsInfo 183 | _pybullet.getDynamicsInfos = _getDynamicsInfos 184 | 185 | assert not hasattr(_pybullet, "setParameters") 186 | _pybullet.setParameters = _setParameters 187 | 188 | 189 | _exported_dunders = { 190 | "__version__", 191 | } 192 | 193 | __all__ = [s for s in dir() if s in _exported_dunders or not s.startswith("_")] 194 | -------------------------------------------------------------------------------- /pybulletX/body.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved 2 | import logging 3 | import warnings 4 | import functools 5 | 6 | import pybulletX as px # noqa: F401 7 | import pybullet as p 8 | 9 | log = logging.getLogger(__name__) 10 | 11 | 12 | def _snake_to_camel(name): 13 | return "".join( 14 | word.title() if i > 0 else word for i, word in enumerate(name.split("_")) 15 | ) 16 | 17 | 18 | class Body: 19 | def __init__( 20 | self, 21 | urdf_path, 22 | base_position=(0, 0, 0), 23 | base_orientation=(0, 0, 0, 1), 24 | use_maximal_coordinates=None, 25 | use_fixed_base=False, 26 | flags=0, 27 | global_scaling=None, 28 | physics_client: px.Client = None, 29 | ): 30 | self.urdf_path = px.helper.find_file(urdf_path) 31 | self.init_base_position = list(base_position) 32 | self.init_base_orientation = list(base_orientation) 33 | 34 | self.use_maximal_coordinates = use_maximal_coordinates 35 | self.use_fixed_base = use_fixed_base 36 | self.flags = flags 37 | self.global_scaling = global_scaling 38 | 39 | if physics_client is None: 40 | physics_client = px.current_client() 41 | self._physics_client = physics_client 42 | 43 | opts = { 44 | "file_name": urdf_path, 45 | "base_position": base_position, 46 | "base_orientation": base_orientation, 47 | "use_maximal_coordinates": use_maximal_coordinates, 48 | "use_fixed_base": use_fixed_base, 49 | "flags": flags, 50 | "global_scaling": global_scaling, 51 | } 52 | 53 | # Turn all keys into camel case 54 | opts = {_snake_to_camel(k): v for k, v in opts.items() if v is not None} 55 | 56 | if opts["flags"] == 0: 57 | warnings.warn( 58 | "By default, Bullet recompute the inertia tensor based on mass " 59 | "and volume of the collision shape. If you want to use inertia " 60 | "from file, set flags to p.URDF_USE_INERTIA_FROM_FILE." 61 | ) 62 | 63 | self._id = px.helper.loadURDF(**opts, **self._client_kwargs) 64 | 65 | # getBasePositionAndOrientation != base_position passed to p.loadURDF. 66 | # See issue https://github.com/bulletphysics/bullet3/issues/2411 67 | # Call resetBasePositionAndOrientation to fix it. 68 | self.set_base_pose(self.init_base_position, self.init_base_orientation) 69 | 70 | @property 71 | def id(self): 72 | return self._id 73 | 74 | @property 75 | def physics_client(self): 76 | return self._physics_client 77 | 78 | @property 79 | def _client_kwargs(self): 80 | return {"physicsClientId": self.physics_client.id} 81 | 82 | @property 83 | def num_joints(self): 84 | return p.getNumJoints(self.id, **self._client_kwargs) 85 | 86 | @property 87 | @functools.lru_cache(maxsize=None) 88 | def _joint_name_to_index(self): 89 | return { 90 | j.joint_name.decode(): j.joint_index 91 | for j in self.get_joint_infos(range(self.num_joints)) 92 | } 93 | 94 | def get_joint_index_by_name(self, joint_name): 95 | return self._joint_name_to_index[joint_name] 96 | 97 | def get_joint_indices_by_names(self, joint_names): 98 | return [self._joint_name_to_index[joint_name] for joint_name in joint_names] 99 | 100 | def get_joint_info(self, joint_index): 101 | """ 102 | Get joint information and return as JointInfo, which is a structure. 103 | """ 104 | return p.getJointInfo(self.id, joint_index, **self._client_kwargs) 105 | 106 | def get_joint_info_by_name(self, joint_name): 107 | return self.get_joint_info(self.get_joint_index_by_name(joint_name)) 108 | 109 | def get_joint_infos(self, joint_indices): 110 | """ 111 | Get the joint informations and return JointInfo, which is a structure of arrays (SoA). 112 | """ 113 | return p.getJointInfos(self.id, joint_indices, **self._client_kwargs) 114 | 115 | def get_joint_state(self, joint_index): 116 | """ 117 | Get the state of a specific joint and return JointState, which is a structure. 118 | """ 119 | return p.getJointState(self.id, joint_index, **self._client_kwargs) 120 | 121 | def get_joint_state_by_name(self, joint_name): 122 | return self.get_joint_state(self.get_joint_index_by_name(joint_name)) 123 | 124 | def get_joint_states(self, joint_indices): 125 | """ 126 | Get the states of all controllable joints and return JointState, which is a structure of arrays (SoA). 127 | """ 128 | return p.getJointStates(self.id, joint_indices, **self._client_kwargs) 129 | 130 | def get_link_state(self, link_index, **kwargs): 131 | """ 132 | Get the state of a specific link and return LinkState, which is a structure. 133 | """ 134 | return p.getLinkState(self.id, link_index, **self._client_kwargs, **kwargs) 135 | 136 | def get_link_state_by_name(self, link_name, **kwargs): 137 | return self.get_link_state(self.get_joint_index_by_name(link_name), **kwargs) 138 | 139 | def get_link_states(self, joint_indices, **kwargs): 140 | """ 141 | Get the states of all movable links and return LinkState, which is a structure of arrays (SoA). 142 | """ 143 | return p.getLinkStates(self.id, joint_indices, **self._client_kwargs, **kwargs) 144 | 145 | def get_dynamics_info(self, link_index): 146 | """ 147 | Get dynamics information and return as DynamicsInfo, which is a structure. 148 | """ 149 | return p.getDynamicsInfo(self.id, link_index, **self._client_kwargs) 150 | 151 | def get_dynamics_infos(self, link_indices): 152 | """ 153 | Get dynamics informations and return as DynamicsInfo, which is a structure. 154 | """ 155 | return p.getDynamicsInfos(self.id, link_indices, **self._client_kwargs) 156 | 157 | def set_base_pose(self, position, orientation=(0, 0, 0, 1)): 158 | """ 159 | Set the position and orientation of robot base (link) 160 | """ 161 | p.resetBasePositionAndOrientation( 162 | self.id, position, orientation, **self._client_kwargs 163 | ) 164 | 165 | def get_base_pose(self): 166 | """ 167 | Get the position and orientation of robot base (link) 168 | """ 169 | return p.getBasePositionAndOrientation(self.id, **self._client_kwargs) 170 | 171 | def get_base_velocity(self): 172 | """ 173 | Get the linear and angular velocity of the base of a body 174 | """ 175 | return p.getBaseVelocity(self.id, **self._client_kwargs) 176 | 177 | def set_base_velocity(self, linear_velocity, angular_velocity): 178 | """ 179 | Set the linear and angular velocity of the base of a body 180 | """ 181 | p.resetBaseVelocity( 182 | self.id, linear_velocity, angular_velocity, **self._client_kwargs 183 | ) 184 | 185 | def reset(self): 186 | self.set_base_pose(self.init_base_position, self.init_base_orientation) 187 | -------------------------------------------------------------------------------- /pybulletX/client.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved 2 | import logging 3 | import functools 4 | import threading 5 | 6 | import pybullet as p 7 | import pybulletX as px 8 | 9 | log = logging.getLogger(__name__) 10 | 11 | 12 | class Client: 13 | def __init__(self, mode: int = None, client_id: int = None): 14 | # should provide either mode or client_id but not both 15 | # mode and client_id can't be both None at the same time. 16 | # This is equiv. to XOR test 17 | assert (mode is None) != (client_id is None) 18 | 19 | if client_id is None: 20 | self._initialized_by_us = True 21 | self._id = px.init(mode=mode) 22 | else: 23 | self._initialized_by_us = False 24 | self._id = client_id 25 | 26 | @property 27 | def id(self): 28 | return self._id 29 | 30 | def release(self): 31 | if not self._initialized_by_us: 32 | return 33 | 34 | try: 35 | log.info(f"Physics client {self.id} disconnected.") 36 | self.disconnect() 37 | except: 38 | ... 39 | 40 | def __enter__(self): 41 | self.prev_client = current_client() 42 | set_client(self) 43 | return self 44 | 45 | def __exit__(self, *args, **kwargs): 46 | set_client(self.prev_client) 47 | 48 | """ 49 | # TODO(poweic): this might not always got called 50 | def __del__(self): 51 | self.release() 52 | """ 53 | 54 | def _apply(self, func_name, *args, **kwargs): 55 | func = getattr(p, func_name) 56 | return func(*args, **kwargs, physicsClientId=self._id) 57 | 58 | 59 | # Note(poweic): This is very similar to torch.cuda.current_device, 60 | # torch.cuda.set_device 61 | _tls = threading.local() 62 | 63 | # By default, pybullet use client_id = 0 for all API. 64 | _tls.current_client = Client(client_id=0) 65 | 66 | 67 | def current_client() -> Client: 68 | return _tls.current_client 69 | 70 | 71 | def set_client(client: Client): 72 | _tls.current_client = client 73 | 74 | 75 | func_names = [ 76 | # Basics 77 | "disconnect", 78 | "isConnected", 79 | "setGravity", 80 | "loadURDF", 81 | "loadSDF", 82 | "loadMJCF", 83 | "saveState", 84 | "saveBullet", 85 | "restoreState", 86 | "createCollisionShape", 87 | "createCollisionShapeArray", 88 | "removeCollisionShape", 89 | "createVisualShape", 90 | "createVisualShapeArray", 91 | "createMultiBody", 92 | "getMeshData", 93 | "stepSimulation", 94 | "setRealTimeSimulation", 95 | "getBasePositionAndOrientation", 96 | "resetBasePositionAndOrientation", 97 | # Controlling a robot 98 | "getNumJoints", 99 | "getJointInfo", 100 | "setJointMotorControl2", 101 | "setJointMotorControlArray", 102 | "setJointMotorControlMultiDof", 103 | "setJointMotorControlMultiDofArray", 104 | "getJointState", 105 | "getJointStates", 106 | "getJointStateMultiDof", 107 | "getJointStatesMultiDof", 108 | "resetJointState", 109 | "resetJointStateMultiDof", 110 | "resetJointStatesMultiDof", 111 | "enableJointForceTorqueSensor", 112 | "getLinkState", 113 | "getLinkStates", 114 | "getBaseVelocity", 115 | "resetBaseVelocity", 116 | "applyExternalForce", 117 | "applyExternalTorque", 118 | "getNumBodies", 119 | "getBodyInfo", 120 | "getBodyUniqueId", 121 | "removeBody", 122 | # Constraints 123 | "createConstraint", 124 | "changeConstraint", 125 | "removeConstraint", 126 | "getNumConstraints", 127 | "getConstraintUniqueId", 128 | "getConstraintInfo", 129 | "getConstraintState", 130 | # Dynamics 131 | "getDynamicsInfo", 132 | "changeDynamics", 133 | # Physics Engine Parameters 134 | "setTimeStep", 135 | "setPhysicsEngineParameter", 136 | "getPhysicsEngineParameters", 137 | "resetSimulation", 138 | # Logging 139 | "startStateLogging", 140 | "stopStateLogging", 141 | # Synthetic Camera Rendering 142 | "getCameraImage", 143 | "getVisualShapeData", 144 | "changeVisualShape", 145 | "loadTexture", 146 | # Collision Detection 147 | "getOverlappingObjects", 148 | "getAABB", 149 | "getContactPoints", 150 | "getClosestPoints", 151 | "rayTest", 152 | "rayTestBatch", 153 | "getCollisionShapeData", 154 | "vhacd", 155 | "setCollisionFilterGroupMask", 156 | "setCollisionFilterPair", 157 | # Inverse Dynamics & Kinematics 158 | "calculateInverseDynamics", 159 | "calculateJacobian", 160 | "calculateMassMatrix", 161 | "calculateInverseKinematics", 162 | "calculateInverseKinematics2", 163 | # Virtual Reality 164 | "getVREvents", 165 | "setVRCameraState", 166 | # Debug/GUI 167 | "addUserDebugLine", 168 | "addUserDebugText", 169 | "addUserDebugParameter", 170 | "removeAllUserParameters", 171 | "removeUserDebugItem", 172 | "setDebugObjectColor", 173 | "configureDebugVisualizer", 174 | "resetDebugVisualizerCamera", 175 | "getDebugVisualizerCamera", 176 | "getKeyboardEvents", 177 | "getMouseEvents", 178 | # Plugins 179 | "loadPlugin", 180 | "executePluginCommand", 181 | "unloadPlugin", 182 | ] 183 | 184 | for func_name in func_names: 185 | getattr(p, func_name) 186 | partial = functools.partialmethod(Client._apply, func_name) 187 | setattr(Client, func_name, partial) 188 | 189 | Client.getContactPoints = px.contact_point.decorator(Client.getContactPoints) 190 | -------------------------------------------------------------------------------- /pybulletX/contact_point.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved 2 | import functools 3 | import typing 4 | import textwrap 5 | from dataclasses import dataclass 6 | 7 | from .mapping_mixin import MappingMixin 8 | 9 | 10 | @dataclass 11 | class ContactPoint(MappingMixin): 12 | r""" 13 | A struct wrapper around contact points returned by p.getContactPoints that 14 | provides dot access (ex: .normal_force) to all the attributes. 15 | 16 | Examples 17 | -------- 18 | >>> import pybullet as p 19 | >>> contact_points = p.getContactPoints(0) 20 | >>> print (contact_points[0]) 21 | (0, 1, 3, -1, 19, 22 | (1.09734, 0.13729, 0.31835), 23 | (1.09729, 0.13730, 0.31837), 24 | (0.91382, -0.13049, -0.38456), 25 | 4.56199e-05, 26 | 8.20047, 27 | 0.03797, 28 | (0.14136, 0.98995, 0.0), 29 | 2.04976, 30 | (0.38070, -0.05436, 0.923098) 31 | ) 32 | 33 | >>> contact_point = ContactPoint(contact_points[0]) 34 | >>> print (contact_point) 35 | contact_flag : 0 36 | body_unique_id_a : 1 37 | body_unique_id_b : 3 38 | link_index_a : -1 39 | link_index_b : 19 40 | position_on_a : (1.09734, 0.13729, 0.31835) 41 | position_on_b : (1.09729, 0.1373, 0.31837) 42 | contact_normal_on_b : (0.91382, -0.13049, -0.38456) 43 | contact_distance : 4.56199e-05 44 | normal_force : 8.20047 45 | lateral_friction_1 : 0.03797 46 | lateral_friction_dir_1 : (0.14136, 0.98995, 0.0) 47 | lateral_friction_2 : 2.04976 48 | lateral_friction_dir_2 : (0.3807, -0.05436, 0.923098) 49 | 50 | """ 51 | contact_flag: int 52 | body_unique_id_a: int 53 | body_unique_id_b: int 54 | link_index_a: int 55 | link_index_b: int 56 | position_on_a: typing.Tuple[float] 57 | position_on_b: typing.Tuple[float] 58 | contact_normal_on_b: typing.Tuple[float] 59 | contact_distance: float 60 | normal_force: float 61 | lateral_friction_1: float 62 | lateral_friction_dir_1: typing.Tuple[float] 63 | lateral_friction_2: float 64 | lateral_friction_dir_2: typing.Tuple[float] 65 | 66 | # used to determine whether this is plural (for API consistency) 67 | _plural: bool = False 68 | 69 | def __repr__(self): 70 | return textwrap.dedent( 71 | f"""\ 72 | contact_flag : {self.contact_flag} 73 | body_unique_id_a : {self.body_unique_id_a} 74 | body_unique_id_b : {self.body_unique_id_b} 75 | link_index_a : {self.link_index_a} 76 | link_index_b : {self.link_index_b} 77 | position_on_a : {self.position_on_a} 78 | position_on_b : {self.position_on_b} 79 | contact_normal_on_b : {self.contact_normal_on_b} 80 | contact_distance : {self.contact_distance} 81 | normal_force : {self.normal_force} 82 | lateral_friction_1 : {self.lateral_friction_1} 83 | lateral_friction_dir_1 : {self.lateral_friction_dir_1} 84 | lateral_friction_2 : {self.lateral_friction_2} 85 | lateral_friction_dir_2 : {self.lateral_friction_dir_2} 86 | """ 87 | ) 88 | 89 | 90 | def decorator(func): 91 | @functools.wraps(func) 92 | def wrapper(*args, **kwargs): 93 | contact_points = func(*args, **kwargs) 94 | return [ContactPoint(*_) for _ in contact_points] 95 | 96 | return wrapper 97 | -------------------------------------------------------------------------------- /pybulletX/dynamics_info.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved 2 | import typing 3 | import textwrap 4 | import collections 5 | from dataclasses import dataclass 6 | 7 | from .mapping_mixin import MappingMixin 8 | 9 | _BODY_TYPES = { 10 | 1: "rigid body", 11 | 2: "multi body", 12 | 3: "soft body", 13 | } 14 | 15 | 16 | def GetBodyType(body_type_ids): 17 | if isinstance(body_type_ids, collections.abc.Iterable): 18 | return [_BODY_TYPES[_] for _ in body_type_ids] 19 | else: 20 | return _BODY_TYPES[body_type_ids] 21 | 22 | 23 | @dataclass 24 | class DynamicsInfo(MappingMixin): 25 | """ 26 | A struct wrapper around dynamics info returned by pybullet.getJointInfo that 27 | provides dot access (ex: .lateral_friction) to all the attributes. 28 | """ 29 | 30 | mass: float 31 | lateral_friction: float 32 | local_inertia_diagonal: typing.Tuple[float] 33 | local_inertial_pos: typing.Tuple[float] 34 | local_inertial_orn: typing.Tuple[float] 35 | restitution: float 36 | rolling_friction: float 37 | spinning_friction: float 38 | contact_damping: float 39 | contact_stiffness: float 40 | body_type: int 41 | collision_margin: float 42 | 43 | _plural: bool = False 44 | 45 | def __repr__(self): 46 | return textwrap.dedent( 47 | f"""\ 48 | mass : {self.mass} 49 | lateral_friction : {self.lateral_friction} 50 | local_inertia_diagonal: {self.local_inertia_diagonal} 51 | local_inertial_pos : {self.local_inertial_pos} 52 | local_inertial_orn : {self.local_inertial_orn} 53 | restitution : {self.restitution} 54 | rolling_friction : {self.rolling_friction} 55 | spinning_friction : {self.spinning_friction} 56 | contact_damping : {self.contact_damping} 57 | contact_stiffness : {self.contact_stiffness} 58 | body_type : {GetBodyType(self.body_type)} (= {self.body_type}) 59 | collision_margin : {self.collision_margin} 60 | """ 61 | ) 62 | -------------------------------------------------------------------------------- /pybulletX/gui/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved 2 | from .control_panel import RobotControlPanel, PoseControlPanel # noqa: F401 3 | 4 | del control_panel # noqa: F821 5 | -------------------------------------------------------------------------------- /pybulletX/gui/control_panel.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved 2 | import logging 3 | import collections 4 | 5 | import numpy as np 6 | import pybullet as p 7 | 8 | from ..robot_interface import IRobot 9 | from ..utils.loop_thread import LoopThread 10 | from ..helper import flatten_nested_dict, to_nested_dict 11 | 12 | # TODO(poweic): pybullet is good at physics simulation, but sucks at GUI. 13 | # We should use pybullet for simulation only and use URDFLoader & THREE.js 14 | # for visualization (GUI). 15 | 16 | log = logging.getLogger(__name__) 17 | 18 | 19 | class Slider: 20 | def __init__(self, name, low, high, init_value): 21 | self.name = name 22 | self.low = low 23 | self.high = high 24 | log.info(f"name: {name}, low: {low}, high: {high}, init_value: {init_value}") 25 | self.handle_id = p.addUserDebugParameter(name, low, high, init_value) 26 | 27 | @property 28 | def value(self): 29 | return p.readUserDebugParameter(self.handle_id) 30 | 31 | 32 | class Sliders: 33 | def __init__(self, names, lows, highs, init_values): 34 | self.names = names 35 | 36 | if not isinstance(lows, collections.abc.Iterable): 37 | lows = [lows for _ in names] 38 | if not isinstance(highs, collections.abc.Iterable): 39 | highs = [highs for _ in names] 40 | 41 | self.lows = lows 42 | self.highs = highs 43 | self.sliders = [Slider(*args) for args in zip(names, lows, highs, init_values)] 44 | 45 | @property 46 | def value(self): 47 | return np.array([s.value for s in self.sliders]) 48 | 49 | 50 | class PoseSlider: 51 | def __init__( 52 | self, 53 | name, 54 | position, 55 | orientation, 56 | position_low=[-0.1, -0.1, -0.1], 57 | position_high=[0.1, 0.1, 0.1], 58 | ): 59 | self.name = name 60 | 61 | euler_xyz = p.getEulerFromQuaternion(orientation) 62 | 63 | self.pos_sliders = Sliders( 64 | [f"{name}_{axis}" for axis in "xyz"], position_low, position_high, position 65 | ) 66 | 67 | self.ori_sliders = Sliders( 68 | [f"{name}_euler_{axis}" for axis in "xyz"], -np.pi, np.pi, euler_xyz 69 | ) 70 | 71 | @property 72 | def value(self): 73 | pos = self.pos_sliders.value 74 | ori = self.ori_sliders.value 75 | ori_q = p.getQuaternionFromEuler(ori) 76 | return pos, ori_q 77 | 78 | 79 | class RobotControlWidget: 80 | def __init__(self, robot): 81 | self.robot = robot 82 | 83 | self.sliders = collections.defaultdict(list) 84 | 85 | # get states and action_space 86 | states = self.robot.get_states() 87 | action_space = self.robot.action_space 88 | 89 | # turn states and action_space to flattend dictionary 90 | states = flatten_nested_dict(states) 91 | action_space = flatten_nested_dict(action_space) 92 | 93 | for key, space in action_space.items(): 94 | if key not in states: 95 | continue 96 | state = states[key] 97 | if isinstance(state, collections.abc.Iterable): 98 | names = [f"{key}[{i}]" for i in range(len(state))] 99 | self.sliders[key] = Sliders(names, space.low, space.high, state) 100 | else: 101 | self.sliders[key] = Slider(key, space.low[0], space.high[0], state) 102 | 103 | @property 104 | def value(self): 105 | actions = {k: s.value for k, s in self.sliders.items()} 106 | actions = to_nested_dict(actions) 107 | return actions 108 | 109 | 110 | class ControlPanel: 111 | def __init__(self, interval=0.05): 112 | self._loop_thread = LoopThread(interval, self.update) 113 | 114 | def start(self): 115 | self._loop_thread.start() 116 | 117 | 118 | class PoseControlPanel(ControlPanel): 119 | def __init__(self, robot, max_force=10, slider_params={}): 120 | super().__init__() 121 | self.robot = robot 122 | self.max_force = max_force 123 | 124 | pos, ori = self.robot.get_base_pose() 125 | self.pose_slider = PoseSlider(f"base_{id(robot)}", pos, ori, **slider_params) 126 | 127 | self.cid = p.createConstraint( 128 | self.robot.id, 129 | -1, 130 | -1, 131 | -1, 132 | p.JOINT_FIXED, 133 | [0, 0, 0], 134 | [0, 0, 0], 135 | childFramePosition=pos, 136 | childFrameOrientation=ori, 137 | ) 138 | 139 | def update(self): 140 | pos, ori = self.pose_slider.value 141 | p.changeConstraint(self.cid, pos, ori, maxForce=self.max_force) 142 | 143 | 144 | class RobotControlPanel(ControlPanel): 145 | def __init__(self, robot): 146 | super().__init__() 147 | assert isinstance(robot, IRobot) 148 | self.robot = robot 149 | self._widget = RobotControlWidget(self.robot) 150 | 151 | def update(self): 152 | self.robot.set_actions(self._widget.value) 153 | -------------------------------------------------------------------------------- /pybulletX/helper.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved 2 | import os 3 | import sys 4 | import warnings 5 | import textwrap 6 | import collections 7 | 8 | import numpy as np 9 | import pybullet as p 10 | import pybullet_data 11 | 12 | import pybulletX as px 13 | 14 | DEFAULT_CONFIG = { 15 | "gravity": {"gravX": 0, "gravY": 0, "gravZ": -9.81}, 16 | } 17 | 18 | 19 | def init_pybullet(cfg=DEFAULT_CONFIG, mode=p.GUI): 20 | warnings.warn("init_pybullet() is deprecated. Use init() instead.") 21 | return init(cfg, mode) 22 | 23 | 24 | def init(cfg=DEFAULT_CONFIG, mode=p.GUI): 25 | """ 26 | Initialize pybullet with p.GUI, call pybulletX's setParameters, 27 | and load the classic plane. 28 | """ 29 | # Initialize pybullet 30 | client = p.connect(mode) 31 | 32 | # Use config to set pybullet simulation parameters 33 | p.setParameters(cfg, client) 34 | 35 | # Load the classic plane 36 | p.setAdditionalSearchPath(pybullet_data.getDataPath()) 37 | p.loadURDF("plane.urdf", physicsClientId=client) 38 | 39 | return client 40 | 41 | 42 | class Axes: 43 | def __init__(self, axes): 44 | self.axes = axes 45 | 46 | def remove(self): 47 | if not self.axes: 48 | return 49 | for axis in self.axes: 50 | p.removeUserDebugItem(axis) 51 | 52 | 53 | def add_axes( 54 | object_id, 55 | link_id=None, 56 | size=0.1, 57 | lineWidth=3.0, 58 | position=(0, 0, 0), 59 | orientation=(0, 0, 0, 1), 60 | ): 61 | opts = dict(lineWidth=lineWidth, parentObjectUniqueId=object_id,) 62 | if link_id: 63 | opts["parentLinkIndex"] = link_id 64 | 65 | rotation_matrix = np.array(p.getMatrixFromQuaternion(orientation)).reshape(3, 3) 66 | position = np.array(position) 67 | x = rotation_matrix.dot([size, 0, 0]) 68 | y = rotation_matrix.dot([0, size, 0]) 69 | z = rotation_matrix.dot([0, 0, size]) 70 | 71 | # add axis X (red), Y (green), and Z (blue) 72 | return Axes( 73 | [ 74 | p.addUserDebugLine(position, position + x, [1, 0, 0], **opts), 75 | p.addUserDebugLine(position, position + y, [0, 1, 0], **opts), 76 | p.addUserDebugLine(position, position + z, [0, 0, 1], **opts), 77 | ] 78 | ) 79 | 80 | 81 | def flatten_nested_dict(d, parent_key="", sep="."): 82 | items = [] 83 | for k, v in d.items(): 84 | new_key = parent_key + sep + k if parent_key else k 85 | if isinstance(v, collections.abc.Mapping): 86 | items.extend(flatten_nested_dict(v, new_key, sep=sep).items()) 87 | else: 88 | items.append((new_key, v)) 89 | return dict(items) 90 | 91 | 92 | def to_nested_dict(d, sep="."): 93 | root = {} 94 | for k, v in d.items(): 95 | node = root 96 | keys = k.split(sep) 97 | for key in keys[:-1]: 98 | if key not in node: 99 | node[key] = {} 100 | node = node[key] 101 | node[keys[-1]] = v 102 | return root 103 | 104 | 105 | def dump(obj, indent=2): 106 | def _dfs(obj): 107 | if not isinstance(obj, collections.abc.Mapping): 108 | return repr(obj) 109 | 110 | s = ",\n".join(['"{}": {}'.format(k, _dfs(v)) for k, v in obj.items()]) 111 | 112 | if s == "": 113 | return "{}" 114 | 115 | return "{\n" + textwrap.indent(s, " " * indent) + "\n}" 116 | 117 | return _dfs(obj) 118 | 119 | 120 | def pprint(obj, indent=2, stream=sys.stdout): 121 | stream.write(dump(obj, indent) + "\n") 122 | 123 | 124 | def find_file(file_path): 125 | if os.path.isfile(file_path): 126 | return file_path 127 | 128 | # if file_path is relative path, then we will go through pybulletX.path, 129 | # see if file is in any directories. 130 | # if file_path is absolute path, nothing we can do :( 131 | if not os.path.isabs(file_path): 132 | for search_path in px.path: 133 | path = os.path.join(search_path, file_path) 134 | if os.path.isfile(path): 135 | return path 136 | 137 | raise FileNotFoundError(f"No such file: '{file_path}'") 138 | 139 | 140 | def loadURDF(fileName, *args, **kwargs): 141 | fileName = find_file(fileName) 142 | return p.loadURDF(fileName, *args, **kwargs) 143 | -------------------------------------------------------------------------------- /pybulletX/joint_info.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved 2 | import typing 3 | import textwrap 4 | from dataclasses import dataclass 5 | 6 | from .mapping_mixin import MappingMixin 7 | from .joint_type import GetJointTypeName 8 | 9 | 10 | @dataclass 11 | class JointInfo(MappingMixin): 12 | """ 13 | A struct wrapper around joint info returned by pybullet.getJointInfo that provides 14 | dot access (ex: .joint_upper_limit) to all the attributes. 15 | """ 16 | 17 | joint_index: int 18 | joint_name: bytes 19 | joint_type: int 20 | q_index: int 21 | u_index: int 22 | flags: int 23 | joint_dampling: float 24 | joint_friction: float 25 | joint_lower_limit: float 26 | joint_upper_limit: float 27 | joint_max_force: float 28 | joint_max_velocity: float 29 | link_name: bytes 30 | joint_axis: typing.Tuple[float] 31 | parent_frame_pos: typing.Tuple[float] 32 | parent_frame_orn: typing.Tuple[float] 33 | parent_index: int 34 | 35 | # used to determine whether this is a joint_state or joint states 36 | _plural: bool = False 37 | 38 | def __repr__(self): 39 | return textwrap.dedent( 40 | f"""\ 41 | joint_index : {self.joint_index} 42 | joint_name : {self.joint_name} 43 | joint_type : {GetJointTypeName(self.joint_type)} (= {self.joint_type}) 44 | q_index : {self.q_index} 45 | u_index : {self.u_index} 46 | flags : {self.flags} 47 | joint_dampling : {self.joint_dampling} 48 | joint_friction : {self.joint_friction} 49 | joint_lower_limit : {self.joint_lower_limit} 50 | joint_upper_limit : {self.joint_upper_limit} 51 | joint_max_force : {self.joint_max_force} 52 | joint_max_velocity : {self.joint_max_velocity} 53 | link_name : {self.link_name} 54 | joint_axis : {self.joint_axis} 55 | parent_frame_pos : {self.parent_frame_pos} 56 | parent_frame_orn : {self.parent_frame_orn} 57 | parent_index : {self.parent_index} 58 | """ 59 | ) 60 | -------------------------------------------------------------------------------- /pybulletX/joint_state.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved 2 | import typing 3 | import textwrap 4 | from dataclasses import dataclass 5 | 6 | from .mapping_mixin import MappingMixin 7 | 8 | 9 | @dataclass 10 | class JointState(MappingMixin): 11 | """ 12 | A struct wrapper around joint state returned by pybullet.getJointState that provides 13 | dot access (ex: .joint_position) to all the attributes. 14 | """ 15 | 16 | joint_position: float 17 | joint_velocity: float 18 | joint_reaction_forces: typing.List[float] 19 | applied_joint_motor_torque: float 20 | 21 | # used to determine whether this is a joint_state or joint states 22 | _plural: bool = False 23 | 24 | def __repr__(self): 25 | return textwrap.dedent( 26 | f"""\ 27 | joint_position : {self.joint_position} 28 | joint_velocity : {self.joint_velocity} 29 | joint_reaction_forces : {self.joint_reaction_forces} 30 | applied_joint_motor_torque : {self.applied_joint_motor_torque} 31 | """ 32 | ) 33 | -------------------------------------------------------------------------------- /pybulletX/joint_type.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved 2 | import pybullet as p 3 | import collections 4 | 5 | _JOINT_TYPES = { 6 | p.JOINT_FIXED: "JOINT_FIXED", 7 | p.JOINT_GEAR: "JOINT_GEAR", 8 | p.JOINT_PLANAR: "JOINT_PLANAR", 9 | p.JOINT_POINT2POINT: "JOINT_POINT2POINT", 10 | p.JOINT_PRISMATIC: "JOINT_PRISMATIC", 11 | p.JOINT_REVOLUTE: "JOINT_REVOLUTE", 12 | p.JOINT_SPHERICAL: "JOINT_SPHERICAL", 13 | } 14 | 15 | 16 | def GetJointTypeName(joint_type_ids): 17 | if isinstance(joint_type_ids, collections.abc.Iterable): 18 | return [_JOINT_TYPES[_] for _ in joint_type_ids] 19 | else: 20 | return _JOINT_TYPES[joint_type_ids] 21 | -------------------------------------------------------------------------------- /pybulletX/link_state.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved 2 | import typing 3 | 4 | # import textwrap 5 | from dataclasses import dataclass 6 | 7 | from .mapping_mixin import MappingMixin 8 | 9 | 10 | @dataclass 11 | class LinkState(MappingMixin): 12 | """ 13 | A struct wrapper around link state returned by pybullet.getLinkState that provides 14 | dot access (ex: .link_world_position) to all the attributes. 15 | """ 16 | 17 | link_world_position: typing.List[float] 18 | link_world_orientation: typing.List[float] 19 | 20 | local_inertial_frame_position: typing.List[float] 21 | local_inertial_frame_orientation: typing.List[float] 22 | 23 | world_link_frame_position: typing.List[float] 24 | world_link_frame_orientation: typing.List[float] 25 | 26 | world_link_linear_velocity: typing.List[float] = None 27 | world_link_angular_velocity: typing.List[float] = None 28 | 29 | # used to determine whether this is a joint_state or joint states 30 | _plural: bool = False 31 | 32 | def __repr__(self): 33 | attr_max_length = max(map(len, self.__dict__.keys())) 34 | fstr = "{{:{0}s}} : {{}}".format(attr_max_length) 35 | s = "\n".join([fstr.format(k, v) for k, v in self.__dict__.items()]) 36 | return s 37 | 38 | # return textwrap.dedent( 39 | # f"""\ 40 | # link_world_position : {self.link_world_position} 41 | # link_world_orientation : {self.link_world_orientation} 42 | # local_inertial_frame_position : {self.local_inertial_frame_position} 43 | # local_inertial_frame_orientation : {self.local_inertial_frame_orientation} 44 | # world_link_frame_position : {self.world_link_frame_position} 45 | # world_link_frame_orientation : {self.world_link_frame_orientation} 46 | # world_link_linear_velocity : {self.world_link_linear_velocity} 47 | # world_link_angular_velocity : {self.world_link_angular_velocity} 48 | # """ 49 | # ) 50 | -------------------------------------------------------------------------------- /pybulletX/mapping_mixin.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved 2 | import collections 3 | 4 | 5 | class MappingMixin(collections.abc.Mapping): 6 | def __new__(cls, *args): 7 | inst = super().__new__(cls) 8 | inst.__init__(*args) 9 | inst._data = args 10 | return inst 11 | 12 | def __setitem__(self, key, value): 13 | self.__dict__[key] = value 14 | 15 | def __getitem__(self, key): 16 | if isinstance(key, int) or isinstance(key, slice): 17 | value = self._data[key] 18 | if self._plural: 19 | return self.__class__(*value) 20 | else: 21 | return value 22 | 23 | return self.__dict__[key] 24 | 25 | def keys(self): 26 | for k in self.__dict__.keys(): 27 | if not k.startswith("_"): 28 | yield k 29 | 30 | def values(self): 31 | for key in self.keys(): 32 | yield self[key] 33 | 34 | def items(self): 35 | for key in self.keys(): 36 | yield key, self[key] 37 | 38 | def __iter__(self): 39 | if self._plural: 40 | return (self[i] for i in range(len(self))) 41 | else: 42 | return self.keys() 43 | 44 | def __len__(self): 45 | # return len(self.__dict__) 46 | return len(self._data) 47 | -------------------------------------------------------------------------------- /pybulletX/robot.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved 2 | import logging 3 | import functools 4 | import textwrap 5 | import warnings 6 | 7 | import numpy as np 8 | 9 | import pybulletX as px # noqa: F401 10 | import pybullet as p 11 | from .robot_interface_mixin import RobotInterfaceMixin 12 | 13 | log = logging.getLogger(__name__) 14 | 15 | 16 | # convert any list in input arguments to tuple before calling the function 17 | def list_args_to_tuple(f): 18 | def wrapper(*args, **kwargs): 19 | args = [tuple(x) if type(x) == list else x for x in args] 20 | return f(*args, **kwargs) 21 | 22 | return wrapper 23 | 24 | 25 | class Robot(px.Body, RobotInterfaceMixin): 26 | # TODO(poweic): maximum force applied when we lock the motor. 27 | MAX_FORCE = 1e4 28 | 29 | def __init__(self, *args, **kwargs): 30 | super().__init__(*args, **kwargs) 31 | 32 | self._torque_control = False 33 | 34 | self.free_joint_indices = self._get_free_joint_indices() 35 | 36 | self.zero_pose = self._get_zero_joint_position() 37 | 38 | self.set_joint_force_torque_sensor(True) 39 | 40 | def _set_velocity_control(self, max_forces): 41 | p.setJointMotorControlArray( 42 | self.id, 43 | self.free_joint_indices, 44 | p.VELOCITY_CONTROL, 45 | forces=max_forces, 46 | **self._client_kwargs, 47 | ) 48 | 49 | @property 50 | def torque_control(self): 51 | return self._torque_control 52 | 53 | @torque_control.setter 54 | def torque_control(self, enable): 55 | if self._torque_control == enable: 56 | return 57 | 58 | self._torque_control = enable 59 | if enable: 60 | self._enable_torque_control() 61 | else: 62 | self._disable_torque_control() 63 | 64 | def _enable_torque_control(self): 65 | """ 66 | Please refer to the official pybullet's QuickStart Guide on Google Drive. 67 | By setting control mode to VELOCITY_CONTROL and max forces to zero, this 68 | will enable torque control. 69 | """ 70 | self._set_velocity_control(np.zeros(self.num_dofs)) 71 | 72 | def _disable_torque_control(self): 73 | """ 74 | Restore pybullet's default control mode: "VELOCITY_CONTROL" and zero 75 | target velocity, and set max force to a huge number (MAX_FORCE). If 76 | the motor is strong enough (i.e. has large enough torque upper limit), 77 | this is essentially locking all the motors. 78 | """ 79 | self._set_velocity_control(np.ones(self.num_dofs) * self.MAX_FORCE) 80 | 81 | @property 82 | def num_dofs(self): 83 | return len(self.free_joint_indices) 84 | 85 | @property 86 | def free_joint_indices(self): 87 | return self._free_joint_indices 88 | 89 | @free_joint_indices.setter 90 | def free_joint_indices(self, new_free_joint_indices): 91 | self._free_joint_indices = new_free_joint_indices 92 | 93 | @functools.lru_cache(maxsize=None) 94 | def _get_free_joint_indices(self): 95 | """ 96 | Exclude all fixed joints and return a list of free joint indices 97 | """ 98 | joint_infos = [self.get_joint_info(i) for i in range(self.num_joints)] 99 | return [ 100 | info.joint_index for info in joint_infos if info.joint_type != p.JOINT_FIXED 101 | ] 102 | 103 | @property 104 | def zero_pose(self): 105 | return self._zero_pose 106 | 107 | @zero_pose.setter 108 | def zero_pose(self, new_zero_pose): 109 | self._zero_pose = new_zero_pose 110 | 111 | def _get_zero_joint_position(self): 112 | if not self.free_joint_indices: 113 | return [] 114 | 115 | joint_infos = self.get_joint_infos() 116 | return np.zeros(self.num_dofs).clip( 117 | min=joint_infos.joint_lower_limit, max=joint_infos.joint_upper_limit, 118 | ) 119 | 120 | def joints_within_limits(self): 121 | if not self.free_joint_indices: 122 | return True 123 | curr = self.get_joint_states().joint_position 124 | lower = self.get_joint_infos().joint_lower_limit 125 | upper = self.get_joint_infos().joint_upper_limit 126 | return np.all(curr >= lower) and np.all(curr <= upper) 127 | 128 | def get_joint_by_name(self, joint_name): 129 | joints = [self.get_joint_info(_) for _ in range(self.num_joints)] 130 | joints = {j.joint_name.decode(): j for j in joints} 131 | return joints[joint_name] 132 | 133 | @list_args_to_tuple 134 | @functools.lru_cache(maxsize=None) 135 | def joint_effort_limits(self, joint_indices): 136 | return self.get_joint_infos(joint_indices).joint_max_force 137 | 138 | def get_joint_infos(self, joint_indices=None): 139 | """ 140 | Get the joint informations of all controllable joints (`self.free_joint_indices`) 141 | and return JointInfo, which is a structure of arrays (SoA). 142 | """ 143 | if joint_indices is None: 144 | joint_indices = self.free_joint_indices 145 | return super().get_joint_infos(joint_indices) 146 | 147 | def get_joint_states(self, joint_indices=None): 148 | """ 149 | Get the states of all controllable joints (`self.free_joint_indices`) and 150 | return JointState, which is a structure of arrays (SoA). 151 | """ 152 | if joint_indices is None: 153 | joint_indices = self.free_joint_indices 154 | return super().get_joint_states(joint_indices) 155 | 156 | def get_link_states(self, joint_indices=None, **kwargs): 157 | """ 158 | Get the states of all movable links (`self.free_joint_indices`) and return 159 | LinkState, which is a structure of arrays (SoA). 160 | """ 161 | if joint_indices is None: 162 | joint_indices = self.free_joint_indices 163 | return super().get_link_states(joint_indices, **kwargs) 164 | 165 | def get_dynamics_infos(self, link_indices=None): 166 | if link_indices is None: 167 | link_indices = self.free_joint_indices 168 | return super().get_dynamics_infos(link_indices) 169 | 170 | def set_joint_force_torque_sensor(self, on_off: bool): 171 | """ 172 | Enable/Disable joint force torque sensor 173 | """ 174 | for joint_index in self.free_joint_indices: 175 | p.enableJointForceTorqueSensor( 176 | self.id, joint_index, on_off, **self._client_kwargs 177 | ) 178 | 179 | def attach( 180 | self, new_robot, link_name, position=(0, 0, 0), orientation=(0, 0, 0, 1) 181 | ): 182 | """ 183 | Attach a new robot (`new_robot`) to self by creating a fixed joint between 184 | a specific link (`link_name`) and the base of the new robot. 185 | """ 186 | assert isinstance(new_robot, Robot) 187 | link_idx = self.get_joint_index_by_name(link_name) 188 | 189 | link_pos = self.get_link_state(link_idx).link_world_position 190 | link_pos = np.array(link_pos) + np.array(position) 191 | 192 | new_robot.set_base_pose(link_pos) 193 | 194 | p.createConstraint( 195 | parentBodyUniqueId=self.id, 196 | parentLinkIndex=link_idx, 197 | childBodyUniqueId=new_robot.id, 198 | childLinkIndex=-1, 199 | jointType=p.JOINT_FIXED, 200 | jointAxis=[0, 0, 0], # ignored for JOINT_FIXED 201 | parentFramePosition=position, 202 | childFramePosition=[0, 0, 0], 203 | parentFrameOrientation=orientation, 204 | childFrameOrientation=[0, 0, 0, 1], 205 | **self._client_kwargs, 206 | ) 207 | 208 | def reset(self): 209 | super().reset() 210 | 211 | for joint_index, joint_angle in zip(self.free_joint_indices, self.zero_pose): 212 | p.resetJointState(self.id, joint_index, joint_angle, **self._client_kwargs) 213 | 214 | if not self.joints_within_limits(): 215 | log.warning("joint set to positions outside the limits") 216 | 217 | def summarize(self): 218 | for index in self.free_joint_indices: 219 | info = self.get_joint_info(index) 220 | log.info(f"\33[33mJoint #{index}:\33[0m") 221 | log.info(textwrap.indent(str(info), " ")) 222 | log.info(f"\33[33m# of DOFs = {self.num_dofs}\33[0m") 223 | 224 | def reset_joint_state(self, *args, **kwargs): 225 | p.resetJointState(self.id, *args, **kwargs, **self._client_kwargs) 226 | 227 | def set_joint_position( 228 | self, joint_position, max_forces=None, use_joint_effort_limits=True 229 | ): 230 | self.torque_control = False 231 | 232 | """ 233 | Maximum forces applied when mode set to p.POSITION_CONTROL or p.VELOCITY_CONTROL 234 | can be one of the followings (M for max_forces, U for use_joint_effort_limits) 235 | 1) [M == None, U == True ] use joint effort limit defined in URDF. 236 | (if not provided in URDF, i.e. joint_max_force all zeros, set I to True and reduce to case 2) 237 | 2) [M == None, U == False] infinite joint effort, which is the default if 238 | "forces" is not passed as argument to p.setJointMotorControlArray 239 | 3) [M != None, U == True ] custom joint efforts (list of floats), clip 240 | by the joint effort limits defined in URDF. 241 | 4) [M != None, U == False] same as 3, but ignore joint effort limits in URDF. 242 | """ 243 | assert not np.all(np.array(max_forces) == 0), "max_forces can't be all zero" 244 | 245 | limits = self.joint_effort_limits(self.free_joint_indices) 246 | if max_forces is None and np.all(limits == 0): 247 | warnings.warn( 248 | "Joint maximum efforts provided by URDF are zeros. " 249 | "Set use_joint_effort_limits to False" 250 | ) 251 | use_joint_effort_limits = False 252 | 253 | opts = {} 254 | if max_forces is None: 255 | if use_joint_effort_limits: 256 | # Case 1 257 | opts["forces"] = limits 258 | else: 259 | # Case 2: do nothing 260 | pass 261 | else: 262 | if use_joint_effort_limits: 263 | # Case 3 264 | opts["forces"] = np.minimum(max_forces, limits) 265 | else: 266 | # Case 4 267 | opts["forces"] = max_forces 268 | 269 | assert len(self.free_joint_indices) == len(joint_position), ( 270 | f"number of target positions ({len(joint_position)}) should match " 271 | f"the number of joint indices ({len(self.free_joint_indices)})" 272 | ) 273 | 274 | # If not provided, the default value of targetVelocities, positionGains 275 | # and velocityGains are 0., 0.1, 1.0, respectively. 276 | p.setJointMotorControlArray( 277 | bodyIndex=self.id, 278 | jointIndices=self.free_joint_indices, 279 | controlMode=p.POSITION_CONTROL, 280 | targetPositions=joint_position, 281 | targetVelocities=np.zeros_like(joint_position), # default = 0.0 282 | # positionGains=np.ones_like(joint_position) * 0.1, # default = 0.1 283 | # velocityGains=np.ones_like(joint_position) * 1.0, # default = 1.0 284 | **opts, 285 | **self._client_kwargs, 286 | ) 287 | 288 | def set_joint_torque(self, torque): 289 | self.torque_control = True 290 | 291 | p.setJointMotorControlArray( 292 | bodyIndex=self.id, 293 | jointIndices=self.free_joint_indices, 294 | controlMode=p.TORQUE_CONTROL, 295 | forces=torque, 296 | **self._client_kwargs, 297 | ) 298 | -------------------------------------------------------------------------------- /pybulletX/robot_interface.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved 2 | import io 3 | from abc import ABCMeta 4 | import collections 5 | 6 | from attrdict import AttrMap 7 | 8 | from .helper import dump 9 | from .utils.space_dict import SpaceDict 10 | 11 | 12 | def _remove_empty_dict_leaf(dict_): 13 | return { 14 | k: v 15 | for k, v in dict_.items() 16 | if not isinstance(v, collections.abc.Mapping) or len(v) > 0 17 | } 18 | 19 | 20 | class IRobot(metaclass=ABCMeta): 21 | """ 22 | Default implementations for state_space/action_space/get_states/set_actions/reset 23 | """ 24 | 25 | @property 26 | def state_space(self): 27 | return self.children_state_space 28 | 29 | @property 30 | def action_space(self): 31 | return self.children_action_space 32 | 33 | def get_states(self): 34 | return self.get_children_states() 35 | 36 | def set_actions(self, actions): 37 | self.set_children_actions(actions) 38 | 39 | def reset(self): 40 | self.reset_children() 41 | 42 | """ 43 | Recursive implementations for state_space/action_space/get_states/set_actions/reset 44 | """ 45 | # TODO(poweic): create an iterator class for children() so that we can write 46 | # self.children().get_states() instead of self.get_children_states() 47 | @property 48 | def children_state_space(self): 49 | return SpaceDict( 50 | _remove_empty_dict_leaf( 51 | {k: v.state_space for k, v in self.children().items()} 52 | ) 53 | ) 54 | 55 | @property 56 | def children_action_space(self): 57 | return SpaceDict( 58 | _remove_empty_dict_leaf( 59 | {k: v.action_space for k, v in self.children().items()} 60 | ) 61 | ) 62 | 63 | def get_children_states(self): 64 | return AttrMap( 65 | _remove_empty_dict_leaf( 66 | {k: v.get_states() for k, v in self.children().items()} 67 | ) 68 | ) 69 | 70 | def set_children_actions(self, actions): 71 | children = self.children() 72 | for k, v in actions.items(): 73 | if k not in children: 74 | continue 75 | children[k].set_actions(v) 76 | 77 | def reset_children(self): 78 | for child in self.children().values(): 79 | child.reset() 80 | 81 | def children(self): 82 | """Get all the children that's also instance of IRobot""" 83 | return {k: v for k, v in self.__dict__.items() if isinstance(v, IRobot)} 84 | 85 | def __repr__(self): 86 | output = io.StringIO() 87 | print(f"State Space: {dump(self.state_space)}", file=output) 88 | print(f"Action Space: {dump(self.action_space)}", file=output) 89 | print(f"Current States: {dump(self.get_states())}", file=output) 90 | print("", file=output) 91 | return output.getvalue() 92 | 93 | 94 | def _check_dict_key_collision(d1, d2): 95 | return set(d1.keys()).intersection(d2.keys()) 96 | 97 | 98 | # FIXME(poweic): how should I name this? router? routeable? hub? switch? pluggable? 99 | def router(func): 100 | def upward_wrapper(self): 101 | childrens_attrs = {} 102 | for k, v in self.children().items(): 103 | attr = getattr(v, func.__name__) 104 | if callable(attr): 105 | childrens_attrs[k] = attr() 106 | else: 107 | childrens_attrs[k] = attr 108 | self_attrs = func(self) 109 | 110 | intersection = set(childrens_attrs.keys()).intersection(self_attrs.keys()) 111 | if intersection: 112 | raise AttributeError( 113 | f"Found keys {intersection} in both childrens_attrs and self_attrs" 114 | ) 115 | 116 | attrs = {**childrens_attrs, **self_attrs} 117 | attrs = _remove_empty_dict_leaf(attrs) 118 | 119 | # TODO(poweic): It's confusing to have both SpaceDict & AttrMap at the same 120 | # time. Also, return AttrMap in new() method of SpaceDict is weird. 121 | # Consider creating that's both SpaceDict and AttrMap at the same time. 122 | if func.__name__ == "get_states": 123 | attrs = AttrMap(attrs) 124 | else: 125 | attrs = SpaceDict(attrs) 126 | 127 | return attrs 128 | 129 | def downward_wrapper(self, attrs): 130 | children = self.children() 131 | self_attrs = {k: v for k, v in attrs.items() if k not in children.keys()} 132 | func(self, self_attrs) 133 | 134 | childrens_attrs = {k: v for k, v in attrs.items() if k in children.keys()} 135 | 136 | for k, v in childrens_attrs.items(): 137 | child = children[k] 138 | f = getattr(child, func.__name__) 139 | f(attrs[k]) 140 | 141 | if func.__name__ == "set_actions": 142 | return downward_wrapper 143 | else: 144 | return upward_wrapper 145 | -------------------------------------------------------------------------------- /pybulletX/robot_interface_mixin.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved 2 | import numpy as np 3 | from gym.spaces import Box 4 | from attrdict import AttrMap 5 | import functools 6 | 7 | from .robot_interface import IRobot, router, SpaceDict 8 | 9 | 10 | class RobotInterfaceMixin(IRobot): 11 | @property 12 | @router 13 | def action_space(self): 14 | info = self.get_joint_infos() 15 | if self.torque_control: 16 | return SpaceDict( 17 | joint_torque=Box( 18 | low=-info.joint_max_force, 19 | high=info.joint_max_force, 20 | shape=[self.num_dofs], 21 | dtype=np.float64, 22 | ), 23 | ) 24 | else: 25 | return SpaceDict( 26 | joint_position=Box( 27 | low=info.joint_lower_limit, 28 | high=info.joint_upper_limit, 29 | shape=[self.num_dofs], 30 | dtype=np.float64, 31 | ), 32 | ) 33 | 34 | @property 35 | def _use_state_space(self): 36 | if not hasattr(self, "_use_state_space_dict"): 37 | self._use_state_space_dict = { 38 | "joint_position": True, 39 | "joint_velocity": True, 40 | "joint_reaction_forces": True, 41 | "applied_joint_motor_torque": True, 42 | } 43 | return self._use_state_space_dict 44 | 45 | def configure_state_space( 46 | self, 47 | joint_position=None, 48 | joint_velocity=None, 49 | joint_reaction_forces=None, 50 | applied_joint_motor_torque=None, 51 | ): 52 | if joint_position is not None: 53 | self._use_state_space["joint_position"] = joint_position 54 | if joint_velocity is not None: 55 | self._use_state_space["joint_velocity"] = joint_velocity 56 | if joint_reaction_forces is not None: 57 | self._use_state_space["joint_reaction_forces"] = joint_reaction_forces 58 | if applied_joint_motor_torque is not None: 59 | self._use_state_space[ 60 | "applied_joint_motor_torque" 61 | ] = applied_joint_motor_torque 62 | 63 | @property 64 | @functools.lru_cache(maxsize=None) 65 | def full_state_space(self): 66 | info = self.get_joint_infos() 67 | return SpaceDict( 68 | joint_position=Box( 69 | low=info.joint_lower_limit, 70 | high=info.joint_upper_limit, 71 | shape=[self.num_dofs], 72 | dtype=np.float64, 73 | ), 74 | joint_velocity=Box( 75 | low=-info.joint_max_velocity, 76 | high=info.joint_max_velocity, 77 | shape=[self.num_dofs], 78 | dtype=np.float64, 79 | ), 80 | joint_reaction_forces=Box( 81 | low=-np.inf, high=np.inf, shape=[self.num_dofs, 6], dtype=np.float64 82 | ), 83 | applied_joint_motor_torque=Box( 84 | low=-np.inf, high=np.inf, shape=[self.num_dofs], dtype=np.float64 85 | ), 86 | ) 87 | 88 | @property 89 | @router 90 | def state_space(self): 91 | full_state_space = self.full_state_space 92 | return SpaceDict( 93 | {k: v for k, v in full_state_space.items() if self._use_state_space[k]} 94 | ) 95 | 96 | @router 97 | def set_actions(self, actions): 98 | if self.torque_control: 99 | self.set_joint_torque(actions["joint_torque"]) 100 | else: 101 | self.set_joint_position(actions["joint_position"]) 102 | 103 | @router 104 | def get_states(self): 105 | states = self.get_joint_states() 106 | return AttrMap({k: v for k, v in states.items() if self._use_state_space[k]}) 107 | -------------------------------------------------------------------------------- /pybulletX/utils/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved 2 | from .simulation_thread import SimulationThread # noqa: F401 3 | from .space_dict import SpaceDict # noqa: F401 4 | -------------------------------------------------------------------------------- /pybulletX/utils/loop_thread.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved 2 | import threading 3 | 4 | from .soft_real_time_clock import SoftRealTimeClock 5 | 6 | 7 | class LoopThread(threading.Thread): 8 | def __init__(self, interval, callback): 9 | super().__init__() 10 | self.interval = interval 11 | self._callback = callback 12 | 13 | def run(self): 14 | """ 15 | Use a soft real-time clock (Soft RTC) to call callback periodically. 16 | """ 17 | clock = SoftRealTimeClock(period=self.interval) 18 | while threading.main_thread().is_alive(): 19 | self._callback() 20 | clock.sleep() 21 | -------------------------------------------------------------------------------- /pybulletX/utils/simulation_thread.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved 2 | import threading 3 | 4 | import pybullet as p 5 | 6 | from .soft_real_time_clock import SoftRealTimeClock 7 | 8 | 9 | class SimulationThread(threading.Thread): 10 | def __init__(self, real_time_factor): 11 | super().__init__() 12 | self.real_time_factor = real_time_factor 13 | 14 | def run(self): 15 | """ 16 | Use a soft real-time clock (Soft RTC) to step through the simulation.If 17 | the p.stepSimulation takes too long, slow down the clock by decreasing 18 | the real time factor. 19 | """ 20 | time_step = p.getPhysicsEngineParameters()["fixedTimeStep"] 21 | clock = SoftRealTimeClock(period=time_step / self.real_time_factor) 22 | while threading.main_thread().is_alive(): 23 | p.stepSimulation() 24 | clock.sleep() 25 | -------------------------------------------------------------------------------- /pybulletX/utils/soft_real_time_clock.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved 2 | import time 3 | 4 | 5 | class SoftRealTimeClock: 6 | """ 7 | Convenience class for sleeping in a loop at a specified rate 8 | """ 9 | 10 | def __init__(self, hz=None, period=None): 11 | assert ( 12 | hz is not None or period is not None 13 | ), "Use either SoftRealTimeClock(hz=10) or SoftRealTimeClock(period=0.1)" 14 | self.last_time = self.gettime() 15 | self.sleep_dur = 1.0 / hz if hz is not None else period 16 | 17 | def gettime(self): 18 | return time.clock_gettime(time.CLOCK_REALTIME) 19 | 20 | def _remaining(self, curr_time): 21 | """ 22 | Calculate the time remaining for clock to sleep. 23 | """ 24 | elapsed = curr_time - self.last_time 25 | return self.sleep_dur - elapsed 26 | 27 | def _sleep(self, duration): 28 | if duration < 0: 29 | return 30 | time.sleep(duration) 31 | 32 | def sleep(self): 33 | """ 34 | Attempt sleep at the specified rate. 35 | """ 36 | curr_time = self.gettime() 37 | self._sleep(self._remaining(curr_time)) 38 | self.last_time += self.sleep_dur 39 | 40 | 41 | def test_soft_real_time_clock(): 42 | clock = SoftRealTimeClock(100) 43 | for i in range(200): 44 | print(clock.gettime()) 45 | clock.sleep() 46 | 47 | clock = SoftRealTimeClock(period=0.1) 48 | for i in range(20): 49 | print(clock.gettime()) 50 | clock.sleep() 51 | 52 | 53 | if __name__ == "__main__": 54 | test_soft_real_time_clock() 55 | -------------------------------------------------------------------------------- /pybulletX/utils/space_dict.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved 2 | import gym 3 | import collections 4 | 5 | from attrdict import AttrMap 6 | 7 | 8 | def _override_gym_spaces_dict_constructor(): 9 | """ 10 | Override the existing the constructor of class gym.spaces.Dict. 11 | The only difference between this and the original __init__ is that space 12 | in spaces.values() can not only be gym.Space but also any kind of dict-like 13 | object that conforms to collections.abc.Mapping 14 | """ 15 | 16 | def __init__(self, spaces=None, **spaces_kwargs): 17 | assert (spaces is None) or ( 18 | not spaces_kwargs 19 | ), "Use either Dict(spaces=dict(...)) or Dict(foo=x, bar=z)" 20 | if spaces is None: 21 | spaces = spaces_kwargs 22 | if isinstance(spaces, dict) and not isinstance(spaces, collections.OrderedDict): 23 | spaces = collections.OrderedDict(sorted(list(spaces.items()))) 24 | if isinstance(spaces, list): 25 | spaces = collections.OrderedDict(spaces) 26 | self.spaces = spaces 27 | for key in spaces.keys(): 28 | if isinstance(spaces[key], collections.abc.Mapping): 29 | spaces[key] = SpaceDict(**spaces[key]) 30 | assert isinstance( 31 | spaces[key], gym.Space 32 | ), "Values of the dict should be instances of gym.Space" 33 | super(gym.spaces.Dict, self).__init__( 34 | None, None 35 | ) # None for shape and dtype, since it'll require special handling 36 | 37 | setattr(gym.spaces.Dict, "__init__", __init__) 38 | 39 | 40 | _override_gym_spaces_dict_constructor() 41 | 42 | 43 | class SpaceDict(gym.spaces.Dict, collections.abc.Mapping): 44 | """ 45 | A extension of gym.spaces.Dict that conforms to abc.Mapping 46 | """ 47 | 48 | def __init__(self, *args, **kwargs): 49 | super().__init__(*args, **kwargs) 50 | 51 | def __iter__(self): 52 | return iter(self.spaces) 53 | 54 | def __len__(self): 55 | return len(self.spaces) 56 | 57 | def __getattr__(self, attr): 58 | """ 59 | Allow dot (.) access to items in self.spaces 60 | Note that Python will call __getattr__ whenever you request an attribute 61 | that hasn't already been defined. If self.spaces hasn't already been 62 | defined, return an empty dict by default. This is to make sure SpaceDict 63 | not constructed in the normal way (e.g. by copy.deepcopy) won't get 64 | "RecursionError: maximum recursion depth". If attr is in spaces.keys(), 65 | then return self.spaces[attr]. Otherwise, delegate to super's __getattribute__. 66 | """ 67 | if attr == "spaces": 68 | return {} 69 | if attr in self.spaces.keys(): 70 | return self.spaces[attr] 71 | return super().__getattribute__(attr) 72 | 73 | def __setitem__(self, key, value): 74 | self.spaces[key] = value 75 | 76 | def __delitem__(self, attr): 77 | del self.spaces[attr] 78 | 79 | def __dir__(self): 80 | return object.__dir__(self) + list(self.spaces.keys()) 81 | 82 | def new(self): 83 | # TODO(poweic): instead of None, use torch.Tensor? (placeholder + strict schema) 84 | return AttrMap( 85 | { 86 | k: v.new() if isinstance(v, collections.abc.Mapping) else None 87 | for k, v in self.spaces.items() 88 | } 89 | ) 90 | -------------------------------------------------------------------------------- /requirements-dev.txt: -------------------------------------------------------------------------------- 1 | -r requirements.txt 2 | pytest >= 6.0.1 3 | pytest-cov >= 2.10.1 4 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | pybullet >= 2.8.1 2 | numpy >= 1.18.5 3 | attrdict >= 2.0.1 4 | gym >= 0.17.2 5 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved 2 | import re 3 | from setuptools import setup, find_packages 4 | 5 | install_requires = [line.rstrip() for line in open("requirements.txt", "r")] 6 | 7 | with open("README.md", "r") as f: 8 | long_description = f.read() 9 | 10 | 11 | def find_version(file_path): 12 | with open(file_path, "r") as f: 13 | version_match = re.search(r"^__version__ = ['\"]([^'\"]*)['\"]", f.read(), re.M) 14 | if version_match: 15 | return version_match.group(1) 16 | raise RuntimeError("Unable to find version string.") 17 | 18 | 19 | setup( 20 | name="pybulletX", 21 | version=find_version("pybulletX/__init__.py"), 22 | author="Po-Wei Chou", 23 | author_email="poweic@fb.com", 24 | description="A Pythonic wrapper for pybullet", 25 | long_description=long_description, 26 | long_description_content_type="text/markdown", 27 | url="https://github.com/facebookresearch/pybulletX", 28 | packages=find_packages(), 29 | install_requires=install_requires, 30 | include_package_data=True, 31 | zip_safe=False, 32 | ) 33 | -------------------------------------------------------------------------------- /tests/conftest.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved 2 | import dataclasses 3 | 4 | import pybullet as p 5 | import pybullet_data 6 | 7 | import pytest 8 | 9 | 10 | class Helpers: 11 | @staticmethod 12 | def get_fields(inst): 13 | fields = dataclasses.asdict(inst).keys() 14 | # TODO(poweic): create classes for JointStates, JointInfos, and LinkStates, 15 | # and remove _plural 16 | return [f for f in fields if f != "_plural"] 17 | 18 | @staticmethod 19 | def check_getitem_method(inst): 20 | # only check field up to the length of the tuple returned by pybullet 21 | fields = Helpers.get_fields(inst)[: len(inst)] 22 | 23 | for i, field in enumerate(fields): 24 | assert inst[i] == getattr(inst, field) 25 | 26 | 27 | @pytest.fixture 28 | def helpers(): 29 | return Helpers 30 | 31 | 32 | @pytest.fixture 33 | def kuka_arm(helpers): 34 | p.connect(p.DIRECT) 35 | p.setAdditionalSearchPath(pybullet_data.getDataPath()) 36 | 37 | # Load kuka_iiwa robotics arm for pybullet_data 38 | kukaId = p.loadURDF("kuka_iiwa/model.urdf", [0, 0, 0], useFixedBase=True) 39 | return kukaId 40 | -------------------------------------------------------------------------------- /tests/test_additional_search_path.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved 2 | import pybullet as p 3 | import pybullet_data 4 | import pybulletX as px # noqa: F401 5 | 6 | 7 | def test_add_additional_search_path(): 8 | p.connect(p.DIRECT) 9 | 10 | # since pybullet.getDataPath() is already in px.path, this should work 11 | # just fine 12 | px.helper.loadURDF("plane.urdf") 13 | 14 | # After adding an additional search path, this will success 15 | px.path.append(pybullet_data.getDataPath()) 16 | px.helper.loadURDF("plane.urdf") 17 | -------------------------------------------------------------------------------- /tests/test_client.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved 2 | import pytest 3 | 4 | import pybullet as p 5 | import pybulletX as px 6 | 7 | 8 | @pytest.mark.xfail 9 | def test_client_destructor(): 10 | r""" 11 | Create a few clients, store them in the list. Use p.isConnected 12 | to check connectivity after deleting the list. 13 | """ 14 | clients = [px.Client(mode=p.DIRECT) for i in range(5)] 15 | client_ids = [client.id for client in clients] 16 | 17 | for cid in client_ids: 18 | assert p.isConnected(cid) 19 | 20 | del clients 21 | 22 | for cid in client_ids: 23 | assert not p.isConnected(cid) 24 | -------------------------------------------------------------------------------- /tests/test_contact_point.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved 2 | import pybullet as p 3 | import pybulletX as px # noqa: F401 4 | 5 | 6 | def test_contact_points(helpers): 7 | with px.Client(mode=p.DIRECT) as c: 8 | body = px.Body("teddy_vhacd.urdf") 9 | body.set_base_pose([0.0, 0.0, -0.02]) 10 | c.stepSimulation() 11 | 12 | contact_points = c.getContactPoints(body.id) 13 | 14 | """ 15 | check_getitem_method will check the following: 16 | assert contact_point[0] == contact_point.contact_flag 17 | assert contact_point[1] == contact_point.body_unique_id_a 18 | assert contact_point[2] == contact_point.body_unique_id_b 19 | assert contact_point[3] == contact_point.link_index_a 20 | assert contact_point[4] == contact_point.link_index_b 21 | assert contact_point[5] == contact_point.position_on_a 22 | assert contact_point[6] == contact_point.position_on_b 23 | assert contact_point[7] == contact_point.contact_normal_on_b 24 | assert contact_point[8] == contact_point.contact_distance 25 | assert contact_point[9] == contact_point.normal_force 26 | assert contact_point[10] == contact_point.lateral_friction_1 27 | assert contact_point[11] == contact_point.lateral_friction_dir_1 28 | assert contact_point[12] == contact_point.lateral_friction_2 29 | assert contact_point[13] == contact_point.lateral_friction_dir_2 30 | """ 31 | for contact_point in contact_points: 32 | helpers.check_getitem_method(contact_point) 33 | -------------------------------------------------------------------------------- /tests/test_dynamics_info.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved 2 | import pybullet as p 3 | import pybulletX # noqa: F401 4 | 5 | 6 | def test_get_dynamics_info(helpers, kuka_arm): 7 | # get the dynamics info of link index 0 8 | dynamics_info = p.getDynamicsInfo(bodyUniqueId=kuka_arm, linkIndex=0) 9 | 10 | helpers.check_getitem_method(dynamics_info) 11 | 12 | 13 | def test_get_joint_infos(helpers, kuka_arm): 14 | num_dof = 7 15 | dynamics_infos = p.getDynamicsInfos( 16 | bodyUniqueId=kuka_arm, linkIndices=range(num_dof) 17 | ) 18 | 19 | assert len(dynamics_infos) == num_dof 20 | 21 | for i, dynamics_info in enumerate(dynamics_infos): 22 | assert dynamics_infos[i] == dynamics_info 23 | helpers.check_getitem_method(dynamics_info) 24 | -------------------------------------------------------------------------------- /tests/test_free_joint_indices.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved 2 | import pybullet as p 3 | import pybulletX as px 4 | 5 | 6 | def test_free_joint_indices(): 7 | free_joint_indices = [1, 2, 4, 5] 8 | with px.Client(mode=p.DIRECT): 9 | robot = px.Robot("kuka_iiwa/model.urdf") 10 | robot.free_joint_indices = free_joint_indices 11 | 12 | states = robot.get_states() 13 | assert robot.num_dofs == len(free_joint_indices) 14 | 15 | assert states.joint_position.shape == (robot.num_dofs,) 16 | assert states.joint_velocity.shape == (robot.num_dofs,) 17 | assert states.joint_reaction_forces.shape == (robot.num_dofs, 6) 18 | assert states.applied_joint_motor_torque.shape == (robot.num_dofs,) 19 | 20 | S = robot.state_space 21 | assert S.joint_position.shape == states.joint_position.shape 22 | assert S.joint_velocity.shape == states.joint_velocity.shape 23 | assert S.joint_reaction_forces.shape == states.joint_reaction_forces.shape 24 | assert ( 25 | S.applied_joint_motor_torque.shape 26 | == states.applied_joint_motor_torque.shape 27 | ) 28 | 29 | A = robot.action_space 30 | assert A.joint_position.shape == (robot.num_dofs,) 31 | -------------------------------------------------------------------------------- /tests/test_joint_info.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved 2 | import pybullet as p 3 | import pybulletX # noqa: F401 4 | 5 | 6 | def test_get_joint_info(helpers, kuka_arm): 7 | # get the joint state of joint index 0 8 | joint_info = p.getJointInfo(bodyUniqueId=kuka_arm, jointIndex=0) 9 | 10 | helpers.check_getitem_method(joint_info) 11 | 12 | 13 | def test_get_joint_infos(helpers, kuka_arm): 14 | num_dof = 7 15 | joint_infos = p.getJointInfos(bodyUniqueId=kuka_arm, jointIndices=range(num_dof)) 16 | 17 | assert len(joint_infos) == num_dof 18 | 19 | for i, joint_info in enumerate(joint_infos): 20 | assert joint_infos[i] == joint_info 21 | helpers.check_getitem_method(joint_info) 22 | -------------------------------------------------------------------------------- /tests/test_joint_state.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved 2 | import pybullet as p 3 | import pybulletX # noqa: F401 4 | 5 | 6 | def test_get_joint_state(helpers, kuka_arm): 7 | # get the joint state of joint index 0 8 | joint_state = p.getJointState(bodyUniqueId=kuka_arm, jointIndex=0) 9 | 10 | """ 11 | check_getitem_method will check the following: 12 | assert joint_state[0] == joint_state.joint_position 13 | assert joint_state[1] == joint_state.joint_velocity 14 | assert joint_state[2] == joint_state.joint_reaction_forces 15 | assert joint_state[3] == joint_state.applied_joint_motor_torque 16 | """ 17 | helpers.check_getitem_method(joint_state) 18 | 19 | 20 | def test_get_joint_states(helpers, kuka_arm): 21 | num_dof = 7 22 | joint_states = p.getJointStates(kuka_arm, jointIndices=range(num_dof)) 23 | assert len(joint_states) == num_dof 24 | 25 | for i, joint_state in enumerate(joint_states): 26 | assert joint_states[i] == joint_state 27 | helpers.check_getitem_method(joint_state) 28 | -------------------------------------------------------------------------------- /tests/test_link_state.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved 2 | import pybullet as p 3 | import pybulletX # noqa: F401 4 | 5 | 6 | def test_get_link_state(helpers, kuka_arm): 7 | # get the joint state of joint index 0 8 | link_state = p.getLinkState(bodyUniqueId=kuka_arm, linkIndex=0) 9 | 10 | helpers.check_getitem_method(link_state) 11 | 12 | 13 | def test_get_link_states(helpers, kuka_arm): 14 | num_links = 7 15 | link_states = p.getLinkStates(bodyUniqueId=kuka_arm, linkIndices=range(num_links)) 16 | 17 | assert len(link_states) == num_links 18 | 19 | for i, link_state in enumerate(link_states): 20 | assert link_states[i] == link_state 21 | helpers.check_getitem_method(link_state) 22 | -------------------------------------------------------------------------------- /tests/test_pybullet_body.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved 2 | import numpy as np 3 | 4 | import pybullet as p 5 | import pybulletX as px # noqa: F401 6 | 7 | 8 | def test_pybullet_body(helpers): 9 | with px.Client(mode=p.DIRECT): 10 | init_position = (0, 0, 1) 11 | init_orientation = (0, 0, 0, 1) 12 | body = px.Body("kuka_iiwa/model.urdf", init_position, init_orientation) 13 | 14 | assert body.num_joints == 7 15 | 16 | # Move the robot to a target position/orientation 17 | position, orientation = (0, 1, 4), (0.03427, 0.10602, 0.14357, 0.98334) 18 | 19 | body.set_base_pose(position, orientation) 20 | position_, orientation_ = body.get_base_pose() 21 | assert np.allclose(position_, position) 22 | assert np.allclose(orientation_, orientation) 23 | 24 | # Reset the robot back to init position/orientation 25 | body.reset() 26 | position_, orientation_ = body.get_base_pose() 27 | assert np.allclose(position_, init_position) 28 | assert np.allclose(orientation_, init_orientation) 29 | 30 | print(body) 31 | -------------------------------------------------------------------------------- /tests/test_pybullet_robot.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved 2 | import pybullet as p 3 | import pybulletX as px 4 | 5 | 6 | def test_pybullet_body(): 7 | with px.Client(mode=p.DIRECT): 8 | init_position = (0, 0, 1) 9 | init_orientation = (0, 0, 0, 1) 10 | robot = px.Robot("kuka_iiwa/model.urdf", init_position, init_orientation) 11 | 12 | print(robot.free_joint_indices) 13 | -------------------------------------------------------------------------------- /tests/test_robot_interface.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved 2 | import numpy as np 3 | 4 | # See https://github.com/openai/gym/blob/master/gym/spaces/box.py for more detail 5 | from gym import spaces 6 | 7 | from pybulletX.utils.space_dict import SpaceDict 8 | from pybulletX.robot_interface import IRobot, router 9 | from pybulletX.helper import pprint 10 | 11 | 12 | class Digit(IRobot): 13 | @property 14 | @router 15 | def state_space(self): 16 | return SpaceDict( 17 | {"image": spaces.Box(low=0, high=255, shape=(3, 4), dtype=np.uint8)} 18 | ) 19 | 20 | @router 21 | def get_states(self): 22 | return {"image": np.zeros([2, 3], dtype=np.uint8)} 23 | 24 | 25 | class AllegroHand(IRobot): 26 | num_dof = 16 27 | 28 | @property 29 | @router 30 | def action_space(self): 31 | return SpaceDict( 32 | { 33 | "joint_torque": spaces.Box( 34 | low=0, high=1, shape=(AllegroHand.num_dof,), dtype=np.float32 35 | ) 36 | } 37 | ) 38 | 39 | @property 40 | @router 41 | def state_space(self): 42 | return SpaceDict( 43 | { 44 | "joint_position": spaces.Box( 45 | low=0, high=1, shape=(AllegroHand.num_dof,), dtype=np.float32 46 | ) 47 | } 48 | ) 49 | 50 | @router 51 | def get_states(self): 52 | return {"joint_position": np.zeros([AllegroHand.num_dof])} 53 | 54 | @router 55 | def set_actions(self, actions): 56 | for k, v in actions.items(): 57 | print(f"\33[33mIn AllegroHand's set_actions\33[0m: {k} -> {v}") 58 | 59 | 60 | class Gripper(IRobot): 61 | @property 62 | @router 63 | def action_space(self): 64 | return SpaceDict( 65 | {"force": spaces.Box(low=0, high=1, shape=(), dtype=np.float32)} 66 | ) 67 | 68 | @property 69 | @router 70 | def state_space(self): 71 | return SpaceDict( 72 | {"position": spaces.Box(low=0, high=1, shape=(), dtype=np.float32)} 73 | ) 74 | 75 | @router 76 | def get_states(self): 77 | return {"position": np.zeros([7])} 78 | 79 | @router 80 | def set_actions(self, actions): 81 | for k, v in actions.items(): 82 | print(f"\33[33mIn Gripper's set_actions\33[0m: {k} -> {v}") 83 | 84 | 85 | class Sawyer(IRobot): 86 | @property 87 | @router 88 | def action_space(self): 89 | return SpaceDict( 90 | {"joint_torque": spaces.Box(low=0, high=1, shape=(7,), dtype=np.float32)} 91 | ) 92 | 93 | @property 94 | @router 95 | def state_space(self): 96 | return SpaceDict( 97 | {"joint_position": spaces.Box(low=0, high=1, shape=(7,), dtype=np.float32)} 98 | ) 99 | 100 | @router 101 | def get_states(self): 102 | return {"joint_position": np.zeros([7])} 103 | 104 | @router 105 | def set_actions(self, actions): 106 | for k, v in actions.items(): 107 | print(f"\33[33mIn Sawyer's set_actions\33[0m: {k} -> {v}") 108 | 109 | 110 | # This is just a demonstration 111 | Kuka = Sawyer 112 | 113 | 114 | def my_simple_policy(states, action_space): 115 | # new actions from the action space 116 | actions = action_space.new() 117 | actions.left_arm.joint_torque = np.zeros(7) 118 | actions.right_arm.hand.joint_torque = np.zeros(16) 119 | return actions 120 | 121 | 122 | class MyRobot(IRobot): 123 | @router 124 | def set_actions(self, actions): 125 | print(f"\33[33mIn MyRobot's set_actions\33[0m: {actions}") 126 | 127 | 128 | def test_all(): 129 | # Create a robot 130 | robot = MyRobot() 131 | 132 | # Create a Sawyer, a Gripper, 2 Digits. Put them together as left arm 133 | robot.left_arm = Sawyer() 134 | robot.left_arm.gripper = Gripper() 135 | robot.left_arm.gripper.digits1 = Digit() 136 | robot.left_arm.gripper.digits2 = Digit() 137 | 138 | # Create a Kuka arm, AllegroHand, and 5 Digits. Put them together as right arm 139 | robot.right_arm = Kuka() 140 | robot.right_arm.hand = AllegroHand() 141 | 142 | # TODO(poweic): create class ModuleList like torch.nn.ModuleList 143 | robot.right_arm.hand.digits1 = Digit() 144 | robot.right_arm.hand.digits2 = Digit() 145 | robot.right_arm.hand.digits3 = Digit() 146 | robot.right_arm.hand.digits4 = Digit() 147 | 148 | # Print the robot by summarizing all states/actions space and get the current states 149 | print(robot) 150 | 151 | # Print the action_space and state_space 152 | print(robot.right_arm.action_space) 153 | print(robot.action_space) 154 | print(robot.state_space) 155 | 156 | # get the states of my robot 157 | states = robot.get_states() 158 | 159 | # states can be accessed like attributes 160 | print(states.left_arm.joint_position.shape) 161 | print(states.left_arm.gripper.position.shape) 162 | print(states.left_arm.gripper.digits1.image.shape) 163 | print(states.left_arm.gripper.digits2.image.shape) 164 | 165 | print(states.right_arm.joint_position.shape) 166 | print(states.right_arm.hand.joint_position.shape) 167 | print(states.right_arm.hand.digits1.image.shape) 168 | print(states.right_arm.hand.digits2.image.shape) 169 | 170 | # states -> actions 171 | actions = my_simple_policy(states, robot.action_space) 172 | pprint(actions) 173 | 174 | robot.set_actions(actions) 175 | -------------------------------------------------------------------------------- /tests/test_robot_state_space_configuration.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved 2 | import pytest 3 | import pybullet as p 4 | import pybulletX as px 5 | 6 | 7 | @pytest.mark.parametrize("joint_position", [True, False]) 8 | @pytest.mark.parametrize("joint_velocity", [True, False]) 9 | @pytest.mark.parametrize("joint_reaction_forces", [True, False]) 10 | @pytest.mark.parametrize("applied_joint_motor_torque", [True, False]) 11 | def test_robot_state_space_configuration( 12 | joint_position, joint_velocity, joint_reaction_forces, applied_joint_motor_torque 13 | ): 14 | with px.Client(mode=p.DIRECT): 15 | robot = px.Robot("kuka_iiwa/model.urdf") 16 | 17 | robot.configure_state_space( 18 | joint_position, 19 | joint_velocity, 20 | joint_reaction_forces, 21 | applied_joint_motor_torque, 22 | ) 23 | 24 | for S in [robot.state_space, robot.get_states()]: 25 | assert joint_position == hasattr(S, "joint_position") 26 | assert joint_velocity == hasattr(S, "joint_velocity") 27 | assert joint_reaction_forces == hasattr(S, "joint_reaction_forces") 28 | assert applied_joint_motor_torque == hasattr( 29 | S, "applied_joint_motor_torque" 30 | ) 31 | -------------------------------------------------------------------------------- /tests/test_space_dict.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved 2 | import copy 3 | 4 | import pytest 5 | 6 | from gym import spaces 7 | from pybulletX.utils.space_dict import SpaceDict 8 | 9 | 10 | @pytest.fixture() 11 | def car(): 12 | return SpaceDict( 13 | { 14 | "sensors": SpaceDict( 15 | { 16 | "position": spaces.Box(low=-100, high=100, shape=(3,)), 17 | "velocity": spaces.Box(low=-1, high=1, shape=(3,)), 18 | "front_cam": spaces.Tuple( 19 | ( 20 | spaces.Box(low=0, high=1, shape=(10, 10, 3)), 21 | spaces.Box(low=0, high=1, shape=(10, 10, 3)), 22 | ) 23 | ), 24 | "rear_cam": spaces.Box(low=0, high=1, shape=(10, 10, 3)), 25 | } 26 | ), 27 | "ext_controller": spaces.MultiDiscrete((5, 2, 2)), 28 | "inner_state": { 29 | "charge": spaces.Discrete(100), 30 | "system_checks": spaces.MultiBinary(10), 31 | "job_status": SpaceDict( 32 | { 33 | "task": spaces.Discrete(5), 34 | "progress": spaces.Box(low=0, high=100, shape=()), 35 | } 36 | ), 37 | }, 38 | } 39 | ) 40 | 41 | 42 | def test_space_dict(car): 43 | print(car) 44 | print(car.sensors) 45 | print(car.sensors.position) 46 | print(car.sensors.velocity) 47 | print(car.sensors.front_cam) 48 | print(car.sensors.front_cam[0]) 49 | print(car.sensors.front_cam[1]) 50 | print(car.sensors.rear_cam) 51 | print(car.inner_state) 52 | print(car.inner_state.charge) 53 | print(car.inner_state.system_checks) 54 | print(car.inner_state.job_status) 55 | print(car.inner_state.job_status.task) 56 | print(car.inner_state.job_status.progress) 57 | 58 | 59 | def test_deepcopy_space_dict(car): 60 | dc = copy.deepcopy(car) 61 | print(dc) 62 | -------------------------------------------------------------------------------- /website/.gitignore: -------------------------------------------------------------------------------- 1 | # Dependencies 2 | /node_modules 3 | 4 | # Production 5 | /build 6 | 7 | # Generated files 8 | .docusaurus 9 | .cache-loader 10 | 11 | # Misc 12 | .DS_Store 13 | .env.local 14 | .env.development.local 15 | .env.test.local 16 | .env.production.local 17 | 18 | npm-debug.log* 19 | yarn-debug.log* 20 | yarn-error.log* 21 | -------------------------------------------------------------------------------- /website/README.md: -------------------------------------------------------------------------------- 1 | # Website 2 | 3 | This website is built using [Docusaurus 2](https://v2.docusaurus.io/), a modern static website generator. 4 | 5 | ### Installation 6 | 7 | ``` 8 | $ yarn 9 | ``` 10 | 11 | ### Local Development 12 | 13 | ``` 14 | $ yarn start 15 | ``` 16 | 17 | This command starts a local development server and open up a browser window. Most changes are reflected live without having to restart the server. 18 | 19 | ### Build 20 | 21 | ``` 22 | $ yarn build 23 | ``` 24 | 25 | This command generates static content into the `build` directory and can be served using any static contents hosting service. 26 | 27 | ### Deployment 28 | 29 | ``` 30 | $ GIT_USER= USE_SSH=true yarn deploy 31 | ``` 32 | 33 | If you are using GitHub pages for hosting, this command is a convenient way to build the website and push to the `gh-pages` branch. 34 | -------------------------------------------------------------------------------- /website/babel.config.js: -------------------------------------------------------------------------------- 1 | // Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved 2 | 3 | module.exports = { 4 | presets: [require.resolve('@docusaurus/core/lib/babel/preset')], 5 | }; 6 | -------------------------------------------------------------------------------- /website/blog/2019-05-28-hola.md: -------------------------------------------------------------------------------- 1 | --- 2 | slug: hola 3 | title: Hola 4 | author: Gao Wei 5 | author_title: Docusaurus Core Team 6 | author_url: https://github.com/wgao19 7 | author_image_url: https://avatars1.githubusercontent.com/u/2055384?v=4 8 | tags: [hola, docusaurus] 9 | --- 10 | 11 | Lorem ipsum dolor sit amet, consectetur adipiscing elit. Pellentesque elementum dignissim ultricies. Fusce rhoncus ipsum tempor eros aliquam consequat. Lorem ipsum dolor sit amet 12 | -------------------------------------------------------------------------------- /website/blog/2019-05-29-hello-world.md: -------------------------------------------------------------------------------- 1 | --- 2 | slug: hello-world 3 | title: Hello 4 | author: Endilie Yacop Sucipto 5 | author_title: Maintainer of Docusaurus 6 | author_url: https://github.com/endiliey 7 | author_image_url: https://avatars1.githubusercontent.com/u/17883920?s=460&v=4 8 | tags: [hello, docusaurus] 9 | --- 10 | 11 | Welcome to this blog. This blog is created with [**Docusaurus 2 alpha**](https://v2.docusaurus.io/). 12 | 13 | 14 | 15 | This is a test post. 16 | 17 | A whole bunch of other information. 18 | -------------------------------------------------------------------------------- /website/blog/2019-05-30-welcome.md: -------------------------------------------------------------------------------- 1 | --- 2 | slug: welcome 3 | title: Welcome 4 | author: Yangshun Tay 5 | author_title: Front End Engineer @ Facebook 6 | author_url: https://github.com/yangshun 7 | author_image_url: https://avatars0.githubusercontent.com/u/1315101?s=400&v=4 8 | tags: [facebook, hello, docusaurus] 9 | --- 10 | 11 | Blog features are powered by the blog plugin. Simply add files to the `blog` directory. It supports tags as well! 12 | 13 | Delete the whole directory if you don't want the blog features. As simple as that! 14 | -------------------------------------------------------------------------------- /website/docs/installation.md: -------------------------------------------------------------------------------- 1 | --- 2 | id: installation 3 | title: Installation 4 | sidebar_label: Installation 5 | --- 6 | 7 | You can install `pybulletX` through `pip`: 8 | ```python 9 | pip install pybulletX 10 | ``` 11 | -------------------------------------------------------------------------------- /website/docs/intro.md: -------------------------------------------------------------------------------- 1 | --- 2 | id: intro 3 | title: Introduction 4 | sidebar_label: Introduction 5 | slug: / 6 | --- 7 | Building robot simulation in PyBullet is like writing C code - not OOP and could 8 | get messy very quickly. 9 | PyBulletX is a **Pythonic** lightweight PyBullet wrapper that helps researchers to do more with less code. No more boilerplate. 10 | 11 | ```python 12 | import pybulletX as px 13 | px.init() 14 | 15 | robot = px.Robot("kuka_iiwa/model.urdf") 16 | ``` 17 | -------------------------------------------------------------------------------- /website/docs/mdx.md: -------------------------------------------------------------------------------- 1 | --- 2 | id: mdx 3 | title: Powered by MDX 4 | --- 5 | 6 | You can write JSX and use React components within your Markdown thanks to [MDX](https://mdxjs.com/). 7 | 8 | export const Highlight = ({children, color}) => ( {children} ); 14 | 15 | Docusaurus green and Facebook blue are my favorite colors. 16 | 17 | I can write **Markdown** alongside my _JSX_! 18 | -------------------------------------------------------------------------------- /website/docs/tutorial/body.md: -------------------------------------------------------------------------------- 1 | --- 2 | id: pybulletx_body 3 | title: Creating a Body 4 | --- 5 | 6 | Every model or object you load in PyBullet has a unique id (starting from 0). 7 | For most of the PyBullet functions, you need to pass this id as the first argument for querying information. 8 | Instead of using the unique id directly with `p.SOME_FUNCTION(id, ...)` every time, pybulletX provides 9 | a helper class `pybulletX.Body`. 10 | 11 | import Tabs from '@theme/Tabs'; 12 | import TabItem from '@theme/TabItem'; 13 | 14 | 21 | 22 | 23 | ```python 24 | import pybullet as p 25 | import pybullet_data 26 | 27 | p.connect(p.GUI) 28 | p.setAdditionalSearchPath(pybullet_data.getDataPath()) 29 | kuka_id = p.loadURDF("kuka_iiwa/model.urdf") 30 | 31 | print (kuka_id, type(kuka_id)) # 0, int 32 | print (p.getBasePositionAndOrientation(kuka_id)) # ((0., 0., 0.), (0., 0., 0., 1.)) 33 | print (p.getNumJoints(kuka_id)) # 7 joints 34 | ``` 35 | 36 | 37 | 38 | 39 | ```python 40 | import pybulletX as px 41 | 42 | px.init() 43 | body = px.Body("kuka_iiwa/model.urdf") 44 | 45 | print (body.id) # 0 <= unique id returned by p.loadURDF 46 | print (body.get_base_pose()) # p.getBasePositionAndOrientation(self.id) 47 | print (body.num_joints()) # p.getNumJoints(self.id) 48 | ``` 49 | 50 | 51 | 52 | 53 | Here's the list of helper functions: 54 | * `Body.num_joints` 55 | * `Body.get_joint_index_by_name(joint_name)` 56 | * `Body.get_joint_indices_by_names(joint_names)` 57 | * `Body.get_joint_info(joint_index)` 58 | * `Body.get_joint_info_by_name(joint_name)` 59 | * `Body.get_joint_infos(joint_indices)` 60 | * `Body.get_joint_state(joint_index)` 61 | * `Body.get_joint_state_by_name(joint_name)` 62 | * `Body.get_joint_states(joint_indices)` 63 | * `Body.get_link_state(joint_index)` 64 | * `Body.get_link_state_by_name(joint_name)` 65 | * `Body.get_link_states(joint_indices)` 66 | * `Body.get_base_pose()` 67 | * `Body.set_base_pose(position, orientation)` 68 | * `Body.get_base_velocity()` 69 | * `Body.set_base_velocity(linear_velocity, angular_velocity)` 70 | -------------------------------------------------------------------------------- /website/docs/tutorial/control_panel.md: -------------------------------------------------------------------------------- 1 | --- 2 | id: control_panel 3 | title: Control Panels 4 | --- 5 | 6 | Sometimes you want to use a control panel to play around with the robot (e.g. moving the base around or changing joint positions). 7 | 8 | There are two control panels provided in `px.gui`: 9 | * PoseControlPanel - creates a panel that can control the base position of a `px.Body` with sliders. 10 | * RobotControlPanel - creates a panel that can control the all free joint positions of a `px.Robot` with sliders. 11 | 12 | This saves you the trouble of creating slider for each joint by using `p.addUserDebugParameter` and ... 13 | 14 | The examples in this tutorial are available [here](https://github.com/facebookresearch/pybulletX/blob/master/examples/control_panel.py). 15 | 16 | ```python 17 | import pybulletX as px 18 | 19 | px.init() 20 | robot = px.Robot("kuka_iiwa/model.urdf") 21 | 22 | # Run the simulation in another thread 23 | t = px.utils.SimulationThread(1.0) 24 | t.start() 25 | 26 | # ControlPanel also takes pybulletX.Body 27 | panel = px.gui.RobotControlPanel(robot) 28 | panel.start() 29 | ``` 30 | -------------------------------------------------------------------------------- /website/docs/tutorial/joint_info.md: -------------------------------------------------------------------------------- 1 | --- 2 | id: joint_info 3 | title: Getting the Joint Information 4 | --- 5 | 6 | In pybullet, you can query the information for each joint using `p.getJointInfo()`. 7 | However, it only returns a list of information in plain old data type (see below). 8 | Going through [PyBullet's Getting Started guide on Google Doc](https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/edit#heading=h.2ye70wns7io3) 9 | and trying to figure out which one is which is not fun at all. 10 | ```python 11 | >>> body_id = p.loadURDF("kuka_iiwa/model.urdf") 12 | >>> joint_info = p.getJointInfo(body_id, 0) 13 | >>> print (joint_info) 14 | (0, 15 | b'lbr_iiwa_joint_1', 16 | 0, 17 | 7, 18 | 6, 19 | 1, 20 | 0.5, 21 | 0.0, 22 | -2.96705972839, # <== what's this? 23 | 2.96705972839, 24 | 300.0, 25 | 10.0, 26 | b'lbr_iiwa_link_1', 27 | (0.0, 0.0, 1.0), 28 | (0.1, 0.0, 0.0875), 29 | (0.0, 0.0, 0.0, 1.0), 30 | -1) 31 | ``` 32 | 33 | In `pybulletX`, we provides a dataclass `JointInfo` for storing the joint information. 34 | Once `pybulletX` is imported, all the subsequent `p.getJointInfo()` will return a `JointInfo` instance. 35 | ```python 36 | >>> import pybullet as p 37 | >>> import pybulletX as px 38 | >>> px.init() 39 | >>> body_id = p.loadURDF("kuka_iiwa/model.urdf") 40 | >>> joint_info = p.getJointInfo(body_id, 0) 41 | >>> print (joint_info) 42 | joint_index : 0 43 | joint_name : b'lbr_iiwa_joint_1' 44 | joint_type : JOINT_REVOLUTE (= 0) 45 | q_index : 7 46 | u_index : 6 47 | flags : 1 48 | joint_dampling : 0.5 49 | joint_friction : 0.0 50 | joint_lower_limit : -2.96705972839 51 | joint_upper_limit : 2.96705972839 52 | joint_max_force : 300.0 53 | joint_max_velocity : 10.0 54 | link_name : b'lbr_iiwa_link_1' 55 | joint_axis : (0.0, 0.0, 1.0) 56 | parent_frame_pos : (0.1, 0.0, 0.0875) 57 | parent_frame_orn : (0.0, 0.0, 0.0, 1.0) 58 | parent_index : -1 59 | ``` 60 | 61 | With `JointInfo` dataclass, you can use dot `(.)` to access all the attributes (ex: `.joint_dampling`). 62 | For example: 63 | ```python 64 | >>> print (joint_info.joint_dampling) 65 | 0.5 66 | >>> print (joint_info.joint_lower_limit) 67 | -2.96705972839 68 | ``` 69 | 70 | Here is what class `JointInfo` looks like: 71 | ```python title="joint_info.py" 72 | @dataclass 73 | class JointInfo(MappingMixin): 74 | """ 75 | A struct wrapper around joint info returned by pybullet.getJointInfo that 76 | provides dot access (ex: .joint_upper_limit) to all the attributes. 77 | """ 78 | 79 | joint_index: int 80 | joint_name: bytes 81 | joint_type: int 82 | q_index: int 83 | u_index: int 84 | flags: int 85 | joint_dampling: float 86 | joint_friction: float 87 | joint_lower_limit: float 88 | joint_upper_limit: float 89 | joint_max_force: float 90 | joint_max_velocity: float 91 | link_name: bytes 92 | joint_axis: typing.Tuple[float] 93 | parent_frame_pos: typing.Tuple[float] 94 | parent_frame_orn: typing.Tuple[float] 95 | parent_index: int 96 | 97 | # ... 98 | ``` 99 | -------------------------------------------------------------------------------- /website/docs/tutorial/putting_all_together.md: -------------------------------------------------------------------------------- 1 | --- 2 | id: putting_it_all_together 3 | title: Putting It All Together 4 | --- 5 | 6 | Here are the examples of P controller with and without PyBulletX. 7 | 8 | import Tabs from '@theme/Tabs'; 9 | import TabItem from '@theme/TabItem'; 10 | 11 | 18 | 19 | 20 | ```python 21 | import time 22 | 23 | import numpy as np 24 | import pybullet as p 25 | import pybullet_data 26 | 27 | P_GAIN = 50 28 | desired_joint_positions = np.array([1.218, 0.507, -0.187, 1.235, 0.999, 1.279, 0]) 29 | 30 | 31 | def main(): 32 | p.connect(p.GUI) 33 | 34 | p.setAdditionalSearchPath(pybullet_data.getDataPath()) 35 | p.loadURDF("plane.urdf") 36 | 37 | robot_id = p.loadURDF("kuka_iiwa/model.urdf", useFixedBase=True) 38 | 39 | num_dofs = 7 40 | joint_indices = range(num_dofs) 41 | 42 | # The magic that enables torque control 43 | p.setJointMotorControlArray( 44 | bodyIndex=robot_id, 45 | jointIndices=joint_indices, 46 | controlMode=p.VELOCITY_CONTROL, 47 | forces=np.zeros(num_dofs), 48 | ) 49 | 50 | while True: 51 | time.sleep(0.01) 52 | 53 | joint_states = p.getJointStates(robot_id, joint_indices) 54 | joint_positions = np.array([j[0] for j in joint_states]) 55 | error = desired_joint_positions - joint_positions 56 | torque = error * P_GAIN 57 | 58 | p.setJointMotorControlArray( 59 | bodyIndex=robot_id, 60 | jointIndices=joint_indices, 61 | controlMode=p.TORQUE_CONTROL, 62 | forces=torque, 63 | ) 64 | 65 | p.stepSimulation() 66 | 67 | 68 | if __name__ == "__main__": 69 | main() 70 | ``` 71 | 72 | 73 | 74 | 75 | 76 | ```python 77 | import numpy as np 78 | import pybulletX as px 79 | 80 | P_GAIN = 50 81 | desired_joint_positions = np.array([1.218, 0.507, -0.187, 1.235, 0.999, 1.279, 0]) 82 | 83 | 84 | def main(): 85 | px.init() 86 | 87 | robot = px.Robot("kuka_iiwa/model.urdf", use_fixed_base=True) 88 | robot.torque_control = True 89 | 90 | t = px.utils.SimulationThread(real_time_factor=1.0) 91 | t.start() 92 | 93 | while True: 94 | error = desired_joint_positions - robot.get_states().joint_position 95 | actions = robot.action_space.new() 96 | actions.joint_torque = error * P_GAIN 97 | robot.set_actions(actions) 98 | 99 | 100 | if __name__ == "__main__": 101 | main() 102 | ``` 103 | 104 | 105 | 106 | -------------------------------------------------------------------------------- /website/docs/tutorial/pybullet_world.md: -------------------------------------------------------------------------------- 1 | --- 2 | id: pybullet_world 3 | title: Setting Up a PyBullet World 4 | --- 5 | 6 | `pybulletX.init()` helps you create a simple pybullet application with the classic grid plane and gravity set to `-9.81` pointing downward. 7 | ```python 8 | import pybulletX as px 9 | px.init() 10 | ``` 11 | 12 | ![img](../../static/img/tutorial/pybullet_world.jpg) 13 | 14 | Under the hood, it does the following for you: 15 | ```python 16 | import pybullet as p 17 | 18 | client = p.connect(p.GUI) 19 | p.setGravity(0, 0, -9.81) 20 | p.setAdditionalSearchPath(pybullet_data.getDataPath()) 21 | p.loadURDF("plane.urdf") 22 | ``` 23 | 24 | An optional config can be passed to `init`. For example: 25 | ```python 26 | import pybulletX as px 27 | from omegaconf import OmegaConf 28 | 29 | cfg = OmegaConf.load("pybullet.yaml") 30 | px.init(cfg.pybullet) 31 | ``` 32 | 33 | ```yaml title="pybullet.yaml" 34 | pybullet: 35 | realTimeSimulation: False 36 | gravity: 37 | gravX: 0 38 | gravY: 0 39 | gravZ: -9.81 40 | timeStep: 0.001 41 | physicsEngineParameter: 42 | numSolverIterations: 2000 43 | solverResidualThreshold: 1e-30 44 | reportSolverAnalytics: True 45 | ``` 46 | -------------------------------------------------------------------------------- /website/docs/tutorial/robot.md: -------------------------------------------------------------------------------- 1 | --- 2 | id: pybulletx_robot 3 | title: Creating a Robot 4 | --- 5 | 6 | Class `pybulletX.Robot` inherits from a `pybulletX.Body` and provides even more functionalities. 7 | Here's an example: 8 | ```python 9 | import pybulletX as px 10 | 11 | px.init() 12 | robot = px.Robot("kuka_iiwa/model.urdf") 13 | 14 | # helpers inherits from px.Body 15 | print (robot.id) 16 | print (robot.get_base_pose()) 17 | print (robot.num_joints()) 18 | 19 | # helpers overwritten by px.Robot 20 | print (robot.get_joint_infos()) # joint info of all free joints 21 | print (robot.get_joint_states()) # joint state of all free joints 22 | print (robot.get_link_states()) # link state of all free joints 23 | 24 | # new helpers exclusive to px.Robot 25 | print (robot.num_dofs) # returns number of Degree of Freedom 26 | print (robot.free_joint_indices) 27 | 28 | # Get the joint_position (np.array) of all free joints 29 | joint_position = robot.get_joint_states().joint_position 30 | 31 | # Move to new joint position 32 | joint_position += 0.01 33 | robot.set_joint_position(joint_position) 34 | ``` 35 | 36 | You can also use enable/disable torque control by setting `.torque_control` to `True`/`False`: 37 | ```python 38 | robot.torque_control = True 39 | joint_torque = np.zeros(7) 40 | 41 | # the arm will now fall to the ground 42 | robot.set_joint_torque(joint_torque) 43 | ``` 44 | -------------------------------------------------------------------------------- /website/docs/tutorial/simulation_thread.md: -------------------------------------------------------------------------------- 1 | --- 2 | id: simulation_thread 3 | title: Simulation Thread 4 | --- 5 | 6 | By default, the physics server in PyBullet will not step the simulation, unless you explicitly call `p.stepSimulation()` 7 | (see [here](https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA) for more information). 8 | 9 | You can use `p.setRealTimeSimulation(1)` to let the physics server step the simulation automatically. However, it does **NOT** work in `DIRECT` mode. 10 | Besides that, sometimes you might want to slow down (or speed up) the simulation by changing the real-time factor. 11 | 12 | This can be done by `px.utils.SimulationThread`, which inherits from Python `threading.Thread`. 13 | ```python 14 | import pybulletX as px 15 | 16 | px.init() 17 | robot = px.Robot("kuka_iiwa/model.urdf") 18 | 19 | t = px.utils.SimulationThread(real_time_factor=1.0) 20 | t.start() 21 | ``` 22 | -------------------------------------------------------------------------------- /website/docusaurus.config.js: -------------------------------------------------------------------------------- 1 | // Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved 2 | 3 | module.exports = { 4 | title: 'PyBulletX', 5 | tagline: 'PyBullet, but much more organized.', 6 | url: 'http://facebookresearch.github.io/pybulletX', 7 | baseUrl: '/', 8 | onBrokenLinks: 'throw', 9 | favicon: 'img/favicon.ico', 10 | organizationName: 'facebookresearch', // Usually your GitHub org/user name. 11 | projectName: 'pybulletX', // Usually your repo name. 12 | themeConfig: { 13 | navbar: { 14 | title: 'PyBulletX', 15 | /*logo: { 16 | alt: 'My Site Logo', 17 | src: 'img/logo.svg', 18 | },*/ 19 | items: [ 20 | { 21 | to: 'docs/', 22 | activeBasePath: 'docs', 23 | label: 'Docs', 24 | position: 'left', 25 | }, 26 | {to: 'blog', label: 'Blog', position: 'left'}, 27 | { 28 | href: 'https://github.com/facebookresearch/pybulletX', 29 | label: 'GitHub', 30 | position: 'right', 31 | }, 32 | ], 33 | }, 34 | footer: { 35 | style: 'dark', 36 | links: [ 37 | { 38 | title: 'Links', 39 | items: [ 40 | { 41 | label: 'Docs', 42 | to: 'docs/', 43 | }, 44 | { 45 | label: 'pybulletX@GitHub', 46 | href: 'https://github.com/facebookresearch/pybulletX', 47 | }, 48 | ], 49 | }, 50 | { 51 | title: 'Legal', 52 | // Please do not remove the privacy and terms, it's a legal requirement. 53 | items: [ 54 | { 55 | label: 'Privacy', 56 | href: 'https://opensource.facebook.com/legal/privacy/', 57 | target: '_blank', 58 | rel: 'noreferrer noopener', 59 | }, 60 | { 61 | label: 'Terms', 62 | href: 'https://opensource.facebook.com/legal/terms/', 63 | target: '_blank', 64 | rel: 'noreferrer noopener', 65 | }, 66 | { 67 | label: 'Cookies', 68 | href: 'https://opensource.facebook.com/legal/cookie-policy', 69 | target: '_blank', 70 | rel: 'noreferrer noopener', 71 | }, 72 | ], 73 | }, 74 | ], 75 | copyright: `Copyright © ${new Date().getFullYear()} My Project, Inc. Built with Docusaurus.`, 76 | }, 77 | }, 78 | presets: [ 79 | [ 80 | '@docusaurus/preset-classic', 81 | { 82 | docs: { 83 | sidebarPath: require.resolve('./sidebars.js'), 84 | // Please change this to your repo. 85 | editUrl: 86 | 'https://github.com/facebook/docusaurus/edit/master/website/', 87 | }, 88 | blog: { 89 | showReadingTime: true, 90 | // Please change this to your repo. 91 | editUrl: 92 | 'https://github.com/facebook/docusaurus/edit/master/website/blog/', 93 | }, 94 | theme: { 95 | customCss: require.resolve('./src/css/custom.css'), 96 | }, 97 | }, 98 | ], 99 | ], 100 | }; 101 | -------------------------------------------------------------------------------- /website/package.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "website", 3 | "version": "0.0.0", 4 | "private": true, 5 | "scripts": { 6 | "docusaurus": "docusaurus", 7 | "start": "docusaurus start", 8 | "build": "docusaurus build", 9 | "swizzle": "docusaurus swizzle", 10 | "deploy": "docusaurus deploy", 11 | "serve": "docusaurus serve" 12 | }, 13 | "dependencies": { 14 | "@docusaurus/core": "^2.0.0-alpha.72", 15 | "@docusaurus/preset-classic": "^2.0.0-alpha.72", 16 | "@mdx-js/react": "^1.5.8", 17 | "clsx": "^1.1.1", 18 | "react": "^16.8.4", 19 | "react-dom": "^16.8.4" 20 | }, 21 | "browserslist": { 22 | "production": [ 23 | ">0.2%", 24 | "not dead", 25 | "not op_mini all" 26 | ], 27 | "development": [ 28 | "last 1 chrome version", 29 | "last 1 firefox version", 30 | "last 1 safari version" 31 | ] 32 | } 33 | } 34 | -------------------------------------------------------------------------------- /website/sidebars.js: -------------------------------------------------------------------------------- 1 | // Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved 2 | 3 | module.exports = { 4 | someSidebar: { 5 | pybulletX: ['intro'], 6 | "Getting Started": ['installation'], 7 | Tutorial: [ 8 | 'tutorial/pybullet_world', 9 | 'tutorial/joint_info', 10 | 'tutorial/pybulletx_body', 11 | 'tutorial/pybulletx_robot', 12 | 'tutorial/simulation_thread', 13 | 'tutorial/putting_it_all_together', 14 | 'tutorial/control_panel', 15 | ], 16 | Advanced: [ 17 | // 'advanced/client', 18 | ], 19 | }, 20 | }; 21 | -------------------------------------------------------------------------------- /website/src/css/custom.css: -------------------------------------------------------------------------------- 1 | /* Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved */ 2 | 3 | /* stylelint-disable docusaurus/copyright-header */ 4 | /** 5 | * Any CSS included here will be global. The classic template 6 | * bundles Infima by default. Infima is a CSS framework designed to 7 | * work well for content-centric websites. 8 | */ 9 | 10 | /* You can override the default Infima variables here. */ 11 | :root { 12 | --ifm-color-primary: #25c2a0; 13 | --ifm-color-primary-dark: rgb(33, 175, 144); 14 | --ifm-color-primary-darker: rgb(31, 165, 136); 15 | --ifm-color-primary-darkest: rgb(26, 136, 112); 16 | --ifm-color-primary-light: rgb(70, 203, 174); 17 | --ifm-color-primary-lighter: rgb(102, 212, 189); 18 | --ifm-color-primary-lightest: rgb(146, 224, 208); 19 | --ifm-code-font-size: 95%; 20 | } 21 | 22 | .docusaurus-highlight-code-line { 23 | background-color: rgb(72, 77, 91); 24 | display: block; 25 | margin: 0 calc(-1 * var(--ifm-pre-padding)); 26 | padding: 0 var(--ifm-pre-padding); 27 | } 28 | -------------------------------------------------------------------------------- /website/src/pages/index.js: -------------------------------------------------------------------------------- 1 | // Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved 2 | 3 | import React from 'react'; 4 | import clsx from 'clsx'; 5 | import Layout from '@theme/Layout'; 6 | import Link from '@docusaurus/Link'; 7 | import useDocusaurusContext from '@docusaurus/useDocusaurusContext'; 8 | import useBaseUrl from '@docusaurus/useBaseUrl'; 9 | import styles from './styles.module.css'; 10 | 11 | const features = [ 12 | /* { 13 | title: 'Easy to Use', 14 | imageUrl: 'img/undraw_docusaurus_mountain.svg', 15 | description: ( 16 | <> 17 | Docusaurus was designed from the ground up to be easily installed and 18 | used to get your website up and running quickly. 19 | 20 | ), 21 | }, 22 | { 23 | title: 'Focus on What Matters', 24 | imageUrl: 'img/undraw_docusaurus_tree.svg', 25 | description: ( 26 | <> 27 | Docusaurus lets you focus on your docs, and we'll do the chores. Go 28 | ahead and move your docs into the docs directory. 29 | 30 | ), 31 | }, 32 | { 33 | title: 'Powered by React', 34 | // imageUrl: 'img/undraw_docusaurus_react.svg', 35 | description: ( 36 | <> 37 | Extend or customize your website layout by reusing React. Docusaurus can 38 | be extended while reusing the same header and footer. 39 | 40 | ), 41 | }, */ 42 | ]; 43 | 44 | function Feature({imageUrl, title, description}) { 45 | const imgUrl = useBaseUrl(imageUrl); 46 | return ( 47 |
48 | {imgUrl && ( 49 |
50 | {title} 51 |
52 | )} 53 |

{title}

54 |

{description}

55 |
56 | ); 57 | } 58 | 59 | function Home() { 60 | const context = useDocusaurusContext(); 61 | const {siteConfig = {}} = context; 62 | return ( 63 | 66 |
67 |
68 |

{siteConfig.title}

69 |

{siteConfig.tagline}

70 |
71 | 77 | Get Started 78 | 79 |
80 |
81 |
82 |
83 | {features && features.length > 0 && ( 84 |
85 |
86 |
87 | {features.map((props, idx) => ( 88 | 89 | ))} 90 |
91 |
92 |
93 | )} 94 |
95 | 96 | ); 97 | } 98 | 99 | export default Home; 100 | -------------------------------------------------------------------------------- /website/src/pages/styles.module.css: -------------------------------------------------------------------------------- 1 | /* Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved */ 2 | 3 | /* stylelint-disable docusaurus/copyright-header */ 4 | 5 | /** 6 | * CSS files with the .module.css suffix will be treated as CSS modules 7 | * and scoped locally. 8 | */ 9 | 10 | .heroBanner { 11 | padding: 4rem 0; 12 | text-align: center; 13 | position: relative; 14 | overflow: hidden; 15 | } 16 | 17 | @media screen and (max-width: 966px) { 18 | .heroBanner { 19 | padding: 2rem; 20 | } 21 | } 22 | 23 | .buttons { 24 | display: flex; 25 | align-items: center; 26 | justify-content: center; 27 | } 28 | 29 | .features { 30 | display: flex; 31 | align-items: center; 32 | padding: 2rem 0; 33 | width: 100%; 34 | } 35 | 36 | .featureImage { 37 | height: 200px; 38 | width: 200px; 39 | } 40 | -------------------------------------------------------------------------------- /website/static/.nojekyll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/facebookresearch/pybulletX/38877701d2eb875d7493f6f45172af94eae3b5a7/website/static/.nojekyll -------------------------------------------------------------------------------- /website/static/img/favicon.ico: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/facebookresearch/pybulletX/38877701d2eb875d7493f6f45172af94eae3b5a7/website/static/img/favicon.ico -------------------------------------------------------------------------------- /website/static/img/tutorial/pybullet_world.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/facebookresearch/pybulletX/38877701d2eb875d7493f6f45172af94eae3b5a7/website/static/img/tutorial/pybullet_world.jpg --------------------------------------------------------------------------------