5.1. Using PyBullet

5.1.1. First Step

The first step is to connect to PyBullet and verify that the system is working.

Copy and paste the following example into a Python script or REPL. If, when run, you see the output Connected: True, then everything is working properly.

from compas_fab.backends import PyBulletClient
with PyBulletClient(connection_type='direct') as client:
    print('Connected:', client.is_connected)

Note

From the PyBullet user manual:

The GUI connection will create a new graphical user interface (GUI) with 3D OpenGL rendering, within the same process space as PyBullet. On Linux and Windows this GUI runs in a separate thread, while on OSX it runs in the same thread due to operating system limitations. On Mac OSX you may see a spinning wheel in the OpenGL Window, until you run a ‘stepSimulation’ or other PyBullet command.

Our first example loads the UR5 robot from a URDF and then adds, then removes, a floor as a collision mesh. The calls to sleep are only necessary to prevent the gui from closing this example too quickly.

import time
from compas.datastructures import Mesh

import compas_fab
from compas_fab.backends.pybullet import PyBulletClient
from compas_fab.robots import CollisionMesh

with PyBulletClient() as client:
    urdf_filepath = compas_fab.get('universal_robot/ur_description/urdf/ur5.urdf')
    robot = client.load_robot(urdf_filepath)

    mesh = Mesh.from_stl(compas_fab.get('planning_scene/floor.stl'))
    cm = CollisionMesh(mesh, 'floor')
    client.add_collision_mesh(cm)

    time.sleep(1)

    client.remove_collision_mesh('floor')

    time.sleep(1)

Adding and removing a collision mesh attached to the end effector link of the robot is similar. Again, the calls to sleep and step_simulation exist only to make the GUI rendering smoother. Note that it is required to load the geometry of the robot before calling methods which add and remove attached collision meshes.

import time

from compas.datastructures import Mesh
from compas.robots import LocalPackageMeshLoader

import compas_fab
from compas_fab.backends.pybullet import PyBulletClient
from compas_fab.robots import AttachedCollisionMesh
from compas_fab.robots import CollisionMesh


with PyBulletClient() as client:
    urdf_filepath = compas_fab.get('universal_robot/ur_description/urdf/ur5.urdf')
    loader = LocalPackageMeshLoader(compas_fab.get('universal_robot'), 'ur_description')
    robot = client.load_robot(urdf_filepath, [loader])

    mesh = Mesh.from_stl(compas_fab.get('planning_scene/cone.stl'))
    cm = CollisionMesh(mesh, 'tip')
    acm = AttachedCollisionMesh(cm, 'ee_link')
    client.add_attached_collision_mesh(acm, {'mass': 0.5, 'robot': robot})

    time.sleep(1)
    client.step_simulation()
    time.sleep(1)

    client.remove_attached_collision_mesh('tip', {'robot': robot})

    time.sleep(1)