pybullet_planning

Documentation Status Travis CI Coveralls License MIT

pybullet_planning is a suite of utility functions to facilitate robotic planning related research on the pybullet physics simulation engine.

Contents

pybullet_planning

Documentation Status Github Actions Integration Status Coveralls License MIT pybullet version

pybullet_planning is a suite of utility functions to facilitate robotic planning related research on the pybullet physics simulation engine. Planning research made easy.

Main features

  • easy-to-use functions to connect with pybullet, tailored for task and motion planning research

  • built-in implementations of standard motion planners, including PRM, RRT, biRRT, A* etc.

Getting Started

pybullet_planning can be installed using pip:

pip install pybullet_planning

Note

On Windows, you may need to install Microsoft Visual C++ 14.0, if pybullet installation fails.

Once the installation is completed, you can verify your setup. Start Python from the command prompt and run the following:

>>> import pybullet_planning as pp

First Steps

Examples can be found at the unit tests. You will be need to install pytest to run these tests (pip install pytest).

Then, individual tests can be run by using their pytest markers by:

pytest -s -m collision_fn --viewer
pytest -s -m motion_planning_2D --viewer

Additional pytest-free examples can be found at pybullet_planning_tutorials.

Contributing

Check the Contributor’s Guide for more details.

PyBullet Resources

Credits

Caelan Reed Garrett. PyBullet Planning. https://pypi.org/project/pybullet-planning/. 2020.

This package was initiated and maintained by Caelan Garrett @caelan and other contributors.

History

This repo is a stable and documented fork of Caelan’s pybullet-planning (previously called ss-pybullet) and motion-planners. New features will continue to be introduced first through these separate repos, and integrated into the current package later.

API Reference

pybullet_planning

This library is a suite of utility functions to facilitate robotic planning related research on the pybullet physics simulation engine.

interfaces

interfaces.debug_utils

TODO: module description

Debug utils

add_text

add_line

[summary]

remove_handles

remove_all_debug

add_body_name

add_segments

draw_link_name

draw_pose

draw_base_limits

draw_circle

draw_aabb

draw_point

Diagnosis utils

draw_collision_diagnosis

interfaces.env_manager

pose transformation, shape creation, and interacting with the pb environment.

Basic Geometric Representation

Point

Representing a point in 3D

Pose

Representing a pose (or frame) in 3D

Euler

Representing a 3D rotation by Eulerian angles
Conversion functions

point_from_pose

get the origin point from a pose

quat_from_pose

get the quaternion from a pose
Transformation operations

tform_point

transform a given point

apply_affine

apply affine transformation on the given list of points
Create shapes

create_body

create_box

create a box body

create_capsule

create_sphere

create_cylinder

create a cylinder body

create_plane

create_obj

Create a body from a given mesh file.
interfaces.geometry

TODO: module description

Main Types

AABB

Bounding box operations

aabb_from_points

aabb_union

get_bodies_in_region

This query will return all the unique ids of objects that have axis aligned bounding box overlap with a given axis aligned bounding box.
interfaces.kinematics
Kinematics interface

get_ik_tool_link_pose

Use the given forward_kinematics function to compute ik_tool_link pose based on current joint configurations in pybullet.

get_ik_generator

get an ik generator

sample_tool_ik

sample ik joints for a given tcp pose in the world frame

compute_forward_kinematics

use the given fk function to compute FK

compute_inverse_kinematics

compute ik solutions using the given ik function handle

select_solution

select one joint configuration given a list of them

interfaces to the motion planners

interfaces.robots

TODO: module description

Body
Collision checking

get_collision_fn

get collision checking function collision_fn(joint_values) -> bool.

get_floating_body_collision_fn

get collision checking function collision_fn(joint_values) -> bool for a floating body (no movable joint).
Body Approximation

approximate_as_prism

get the AABB bounding box of a body

approximate_as_cylinder

get the bounding cylinder of the AABB bounding box of a body
Dynamics

DynamicsInfo

set_static

set all the body’s links to be static (infinite mass, doesn’t move under gravity)
Verbose

dump_body

dump_world

print out information for all bodies that’re currently in the scene
interfaces.task_modeling

TODO: module description

Attachment

Attachment

create_attachment

create an Attachment between the parent body’s parent_link and child body, based on their current pose
Grasp

GraspInfo

body_from_end_effector

get world_from_brick pose from a given world_from_gripper and gripper_from_brick

end_effector_from_body

get world_from_gripper from the target brick’s pose and the grasp pose

approach_from_grasp

get approach_from_brick

get_grasp_pose

get grasp poses from a constraint

primitives

These modules are transported from kuka_primitives, pr2_primitives and pr2_utils from ss-pybullet.

Grasp

In pybullet_planning, a grasp is modeled as an operator on the object frame, i.e. world_from_object = world_from_gripper * grasp, where grasp = gripper_from_object.

We provide some simple grasp generator based on bounding geometry (box, cylinder). Inside these generators’ implementation, however, the modeling start from the object frame to the gripper’s frame to obtain object_from_gripper and then we return its inverse gripper_from_object. (Yijiang thinks this is more straightforward to think about.)

get_side_cylinder_grasps

generate side grasp using the body’s bounding cylinder.
Convenient classes

EndEffector

a convenient class for creating and manipulating an end effector

motion_planners

Python implementations of several robotic motion planners. This is a fork of Caelan’s motion-planners repo, which is designed to be flexible and independent of simulation platforms. pybullet_planning includes this package as a built-in component as it is frequently used.

We are now up-to-date with commit e6f23053e of motion-planners.

Sampling-based:

  • Probabilistic Roadmap (PRM)

  • Rapidly-Exploring Random Tree (RRT)

  • RRT-Connect (BiRRT)

  • Linear Shortcutting

  • MultiRRT

  • RRT*

Grid search:

  • Breadth-First Search (BFS)

  • A*

  • Ladder graph DAG Search

Probabilistic Roadmap (PRM)

lazy_prm

param start

Start configuration - conf
RRT-Connect (BiRRT)

rrt_connect

RRT connect algorithm: http://www.kuffner.org/james/papers/kuffner_icra2000.pdf

birrt

param start

Start configuration - conf
Smoothing

rrt_connect

utils

Package containing a set of utility functions and variables

File system functions

read_pickle

[summary]

write_pickle

[summary]
Sampling functions

randomize

Iteration utilities

roundrobin

roundrobin(‘ABC’, ‘D’, ‘EF’) –> A D E B F C

chunks

get [i, i+n] sublist of a given sequence

get_pairs

get a sequece of (seq[i], seq[i+1]), i=0~n-1
Transformation functions

rotation_from_matrix

Return rotation angle and axis from rotation matrix.

Contributor’s Guide

Contributions are always welcome and greatly appreciated!

Code contributions

We love pull requests from everyone! Here’s a quick guide to improve the code:

  1. Fork the repository and clone the fork.

  2. Create a virtual environment using your tool of choice (e.g. virtualenv, conda, etc).

  3. Install development dependencies:

pip install -r requirements-dev.txt

  1. Make sure all tests pass:

invoke test

  1. Start making your changes to the master branch (or branch off of it).

  2. Make sure all tests still pass:

invoke test

  1. Add yourself to AUTHORS.rst.

  2. Commit your changes and push your branch to GitHub.

  3. Create a pull request through the GitHub website.

During development, use pyinvoke tasks on the command line to ease recurring operations:

  • invoke clean: Clean all generated artifacts.

  • invoke check: Run various code and documentation style checks.

  • invoke docs: Generate documentation.

  • invoke test: Run all tests and checks in one swift command.

  • invoke: Show available tasks.

Releasing this project

Ready to release a new version of pybullet_planning? Here’s how to do it:

  • We use semver, i.e. we bump versions as follows:

    • patch: bugfixes.

    • minor: backwards-compatible features added.

    • major: backwards-incompatible changes.

  • Update the CHANGELOG.rst with all novelty!

  • Ready? Release everything in one command:

invoke release [patch|minor|major]
# with -b to bump version

  • Celebrate! 💃

Documentation improvements

We could always use more documentation, whether as part of the introduction/examples/usage documentation or API documentation in docstrings.

Documentation is written in reStructuredText and use Sphinx to generate the HTML output.

Once you made the documentation changes locally, run the documentation generation:

invoke docs

Bug reports

When reporting a bug please include:

  • Operating system name and version.

  • Python version.

  • Any details about your local setup that might be helpful in troubleshooting.

  • Detailed steps to reproduce the bug.

Feature requests and feedback

The best way to send feedback is to file an issue on Github. If you are proposing a feature:

  • Explain in detail how it would work.

  • Keep the scope as narrow as possible, to make it easier to implement.

Authors

Acknowledgements

We thank the authors for the following models that we use in the unit tests.

Changelog

All notable changes to this project will be documented in this file.

The format is based on Keep a Changelog and this project adheres to Semantic Versioning.

Unreleased

Added

Changed

Fixed * Fixed clone_body bug when input links contains BASE_LINK * Fixed inverse_kinematics_helper to use target_quat when null_space is used

Deprecated

Removed

Known Unresolved Issues

  • get_body_collision_vertices does not support cloned body now. We don’t have a good way to do catch-throw for this now.

  • Cloning body from an already cloned body introduces unexpected result (visual or collision data erased etc).

0.6.0

Added * Added distance_threshold to pairwise_link_collision_info and pairwise_link_collision to allow collision checking given a penetration threshold * Added sweep_collision_fn to birrt to allow sweep collision check in edge expansion * Added coarse_waypoints to the smooth_path function to give options for use refined shortcut to ensure collision-free results. * LOGGER introduced to replace print * Added get_data_filename_and_height to include cached mesh filename and scale from get_model_info * Added CreateVisualArray and CreateCollisionArray for visual_shape_from_data and visual_shape_from_data. We use the new get_data_filename_and_height when file_name is UNKNOWN_FILE. * Added load_pybullet support for stl files by create_obj. * Added get_body_collision_vertices for getting body collision vertices in its current configuration.

Changed * Apply HideOutput to pybullet IK error printouts in inverse_kinematics_helper * motion_planners module up-to-date with commit e6f23053e. * Changed the mesh reading procedure in vertices_from_data from pp.read_obj to meshio.read. This fixes #9. * smooth_path’s max_iterations argument changed to max_smooth_iterations * ‘set_color’ defaulted to set the color of all the links of body. * vertices_from_data get filename and scale from cached INFO_FROM_BODY (through get_data_filename_and_height) if the data’s filename does not end with .urdf

Fixed * Fixed read_obj returns empty dict if obj file does not start with objects (o object_name) * Fixed get_extend_fn(q1,q2) to include q1 in its output path. This bug was causing incorrect planning result in birrt, because the two trees will have a gap in the middle that is unchecked. * Fixed a minor mesh vertex scaling bug in vertices_from_data * catch get_num_joints(body) == 0 when a rigid body without joint and link are passed to clone_body * clear INFO_FROM_DATA when disconnect or reset_simulation

0.5.1

Added * Added current_conf to as the single-node ladder in the ladder graph Cartesian planning plan_cartesian_motion_lg

Changed * Changed Attachment.from_data to construct parent and child bodies from body name data * Changed EdgeBuilder from using upper_tm and joint_vel_limits to directly using jump_threshold

0.5.0

Changed

Added

  • Added SE3 floating body motion planning

  • Added ladder graph cartesian planner

Removed

  • Removed requirments.txt, moved dependencies into setup.py

0.1.1

Fixed

  • Fixed Windows OS “Display not in os.environ” issue in connect

0.1.0

Added

  • diagnosis collision checking function to help visualizing the collision bodies’ information

  • add workspace_bodies to the get_collision_fn to check collisions with the obstacles specified in a URDF file.

  • move ik_interface module from application side to this repo, since it’s “universal” for fixed-end robot. Might need to add a separete one for robots with moving base later.

  • enable travis ci unit test, collision_fn well tested

  • get_floating_body_collision_fn to check a body without joints’s collision. Associated test added.

  • Added snap_sols to kinematics.ik_utils

Changed

  • add extra_disabled_collisions parameter to get_collision_fn, plan_joint_motion

  • Changed get_name to return name without index if name is not ‘’

Removed

  • get_collision_diagnosis_fn removed, integrated into the get_collision_fn

Fixed

  • utils.numeric_sample.randomize: random.shuffle cannot operate on a range in py 3.x. Enforced conversion to list to fix it.

  • Fixed get_collision_fn to ignore checking between bodies both specified in attachment and obstacles (prioritize its role as attachment)

Deprecated

TODO

  • add body name for bodies from create_obj

Requested features

  • clone_body work for bodies from create_obj

0.0.1

Added

  • Initial version

  • Divide the original utils.py file into separate modules

  • Modules cycle dependency resolved.