New Features in pyb_utils

I've recently added a few things to pyb_utils, which is a small package designed to make things easier when working with the PyBullet simulator. In my previous post (quite some time ago now) I discussed my initial reason for creating pyb_utils, which was to easily perform collision detection in parallel with the main simulation, for things like planning or control. Since then I've added some new things that I think are generally useful.

Rigid body construction

First, there is now the BulletBody class for quickly creating rigid bodies. Creating and adding a box to a simulation is as easy as

>>> import pybullet as pyb
>>> import pyb_utils

>>> pyb.connect(pyb.GUI)

# create a 1x1x1 cube at the origin
>>> box = pyb_utils.BulletBody.box(position=[0, 0, 0], half_extents=[0.5, 0.5, 0.5])

The BulletBody class takes care of the boilerplate for creating the visual and collision objects and combining them into a multibody. It also provides methods for getting and setting position, orientation, and velocity, as well as applying external forces and torques.

In addition to the box, there are dedicated constructors for spheres, cylinders, and capsules. For example:

# put a ball on top of the cube
>>> ball = pyb_utils.BulletBody.sphere(position=[0, 0, 1.5], radius=0.5)

# now put it somewhere else
>>> ball.set_pose(position=[2, 0, 0.5])

The BulletBody class makes it easy to add simple objects to act as obstacles or manipulation targets, for instance.

Named tuples

Second, pyb_utils now wraps some PyBullet functions and returns named tuples rather than just vanilla tuples (but the calling API is exactly the same). This makes it a lot easier to remember what the values in the returned tuples actually mean. In particular, there are wrappers for getDynamicsInfo, getContactPoints, getClosestPoints, and getConstraintInfo. These functions all return tuples containing over ten values each. Continuing our example from above, with regular PyBullet we have something like

# hard to read!
>>> pyb.getDynamicsInfo(box.uid, -1)
(1.0,
 0.5,
 (0.16666666666666666, 0.16666666666666666, 0.16666666666666666),
 (0.0, 0.0, 0.0),
 (0.0, 0.0, 0.0, 1.0),
 0.0,
 0.0,
 0.0,
 -1.0,
 -1.0,
 2,
 0.001)

Instead, we can easily replace the call with the pyb_utils equivalent:

>>> info = pyb_utils.getDynamicsInfo(box.uid, -1)
# now we can access fields by name
>>> info.mass
1.0
>>> info.localInertiaPos
(0.0, 0.0, 0.0)

Now there is no need to count through the field names in the PyBullet documentation every time one of these functions is used. And since the calling API is exactly the same, there is no barrier to switching to the pyb_utils wrappers.

Quaternion utilities

Finally, I want to mention a couple of simple quaternion utilities. They've actually been present in pyb_utils for quite a while, but I've come to appreciate them more over time for fast prototyping. PyBullet represents 3D orientation using quaternions in $(x,y,z,w)$ order (i.e., with the scalar part last). Using the spatialmath library under the hood, pyb_utils provides the functions:

quaternion_to_matrix  # convert quaternion to rotation matrix
matrix_to_quaternion  # convert rotation matrix to quaternion
quaternion_multiply   # multiply two quaternions together (Hamilton product)
quaternion_rotate     # rotate a point by a rotation represented by a quaternion

which make it easy to apply and compound rotations. The main reason to use these functions rather than just those from spatialmath directly is because by default spatialmath uses the convention that the scalar part of the quaternion is first; to switch to the PyBullet convention one needs to constantly pass the order keyword argument to all of the functions, which is tiresome and hard to debug if forgotten.

Conclusion

I wrote and continue adding to pyb_utils because it provides a set of tools useful for my research on robotics. I hope it can be useful for others as well. Pull requests are welcome!