Wrecking Ball Simulation

This example simulation shows how to use pybullet to render physics simulations in fury. In this example we specifically render a brick wall beign destroyed by a wrecking ball.

First some imports.

from fury import actor, window, utils, ui
import numpy as np
import pybullet as p
import itertools

Initiate pybullet and enable gravity.

p.connect(p.DIRECT)
p.setGravity(0, 0, -10)

Define some handy parameters to customize simulation.

# Parameters
wall_length = 5
wall_breadth = 5
wall_height = 5

brick_size = np.array([0.2, 0.4, 0.2])

n_links = 15
dx_link = 0.1       # Size of segments
link_mass = 0.5
base_mass = 0.1
radii = 0.1

ball_mass = 10
ball_radius = 0.5
ball_color = np.array([[1, 0, 0]])

joint_friction = 0.0005

Creating the base plane actor.

# Base
base_actor = actor.box(centers=np.array([[0, 0, 0]]),
                       directions=[0, 0, 0],
                       scales=(5, 5, 0.2),
                       colors=(1, 1, 1))
base_coll = p.createCollisionShape(p.GEOM_BOX,
                                   halfExtents=[2.5, 2.5, 0.1])
base = p.createMultiBody(
                          baseCollisionShapeIndex=base_coll,
                          basePosition=[0, 0, -0.1],
                          baseOrientation=[0, 0, 0, 1])
p.changeDynamics(base, -1, lateralFriction=0.3, restitution=0.5)

The following definations are made to render a NxNxN brick wall.

# Generate bricks.
nb_bricks = wall_length*wall_breadth*wall_height
brick_centers = np.zeros((nb_bricks, 3))

brick_directions = np.zeros((nb_bricks, 3))
brick_directions[:] = np.array([1.57, 0, 0])

brick_orns = np.zeros((nb_bricks, 4))

brick_sizes = np.zeros((nb_bricks, 3))
brick_sizes[:] = brick_size

brick_colors = np.random.rand(nb_bricks, 3)

brick_coll = p.createCollisionShape(p.GEOM_BOX,
                                    halfExtents=brick_size/2)

bricks = np.zeros(nb_bricks, dtype=np.int16)

The following is the logic to position the bricks in our desired location and generate the actor.

idx = 0
# Setting up wall
for i in range(wall_length):
    for k in range(wall_height):
        for j in range(wall_breadth):
            center_pos = np.array([(i*0.2)-1.8, (j*0.4)-0.9, (0.2*k)+0.1])
            brick_centers[idx] = center_pos
            brick_orns[idx] = np.array([0, 0, 0, 1])
            bricks[idx] = p.createMultiBody(baseMass=0.5,
                                            baseCollisionShapeIndex=brick_coll,
                                            basePosition=center_pos,
                                            baseOrientation=brick_orns[i])
            p.changeDynamics(bricks[idx], -1, lateralFriction=0.1,
                             restitution=0.1)
            idx += 1

brick_actor = actor.box(centers=brick_centers,
                        directions=brick_directions,
                        scales=brick_sizes,
                        colors=brick_colors)

Now we render the wrecking ball consisting of a fixed hinge, a ball and rope.

# Generate wrecking ball
link_shape = p.createCollisionShape(p.GEOM_CYLINDER,
                                    radius=radii,
                                    height=dx_link,
                                    collisionFramePosition=[0, 0, -dx_link/2])

base_shape = p.createCollisionShape(p.GEOM_BOX,
                                    halfExtents=[0.01, 0.01, 0.01])
ball_shape = p.createCollisionShape(p.GEOM_SPHERE,
                                    radius=0.2)


visualShapeId = -1

link_Masses = np.zeros(n_links)
link_Masses[:] = link_mass
link_Masses[-1] = 5
linkCollisionShapeIndices = np.zeros(n_links)
linkCollisionShapeIndices[:] = np.array(link_shape)
linkCollisionShapeIndices[-1] = ball_shape
linkVisualShapeIndices = -1 * np.ones(n_links)
linkPositions = np.zeros((n_links, 3))
linkPositions[:] = np.array([0, 0, -dx_link])
linkOrientations = np.zeros((n_links, 4))
linkOrientations[:] = np.array([0, 0, 0, 1])
linkInertialFramePositions = np.zeros((n_links, 3))
linkInertialFrameOrns = np.zeros((n_links, 4))
linkInertialFrameOrns[:] = np.array([0, 0, 0, 1])
indices = np.arange(n_links)
jointTypes = np.zeros(n_links)
jointTypes[:] = np.array(p.JOINT_SPHERICAL)
axis = np.zeros((n_links, 3))
axis[:] = np.array([1, 0, 0])

linkDirections = np.zeros((n_links, 3))
linkDirections[:] = np.array([1, 1, 1])

link_radii = np.zeros(n_links)
link_radii[:] = radii

link_heights = np.zeros(n_links)
link_heights[:] = dx_link

rope_actor = actor.cylinder(centers=linkPositions,
                            directions=linkDirections,
                            colors=np.random.rand(n_links, 3),
                            radius=radii,
                            heights=link_heights, capped=True)

basePosition = [0, 0, 2]
baseOrientation = [0, 0, 0, 1]
rope = p.createMultiBody(base_mass,
                         base_shape,
                         visualShapeId,
                         basePosition,
                         baseOrientation,
                         linkMasses=link_Masses,
                         linkCollisionShapeIndices=linkCollisionShapeIndices,
                         linkVisualShapeIndices=linkVisualShapeIndices,
                         linkPositions=linkPositions,
                         linkOrientations=linkOrientations,
                         linkInertialFramePositions=linkInertialFramePositions,
                         linkInertialFrameOrientations=linkInertialFrameOrns,
                         linkParentIndices=indices,
                         linkJointTypes=jointTypes,
                         linkJointAxis=axis)

Next we define the frictional force between the joints of wrecking ball.

friction_vec = [joint_friction]*3   # same all axis
control_mode = p.POSITION_CONTROL   # set pos control mode
for j in range(p.getNumJoints(rope)):
    p.setJointMotorControlMultiDof(rope, j, control_mode,
                                   targetPosition=[0, 0, 0, 1],
                                   targetVelocity=[0, 0, 0],
                                   positionGain=0,
                                   velocityGain=1,
                                   force=friction_vec)

We add the following constraint to keep the cubical hinge fixed.

root_robe_c = p.createConstraint(rope, -1, -1, -1,
                                 p.JOINT_FIXED, [0, 0, 0],
                                 [0, 0, 0], [0, 0, 2])

box_actor = actor.box(centers=np.array([[0, 0, 0]]),
                      directions=np.array([[0, 0, 0]]),
                      scales=(0.02, 0.02, 0.02),
                      colors=np.array([[1, 0, 0]]))

ball_actor = actor.sphere(centers=np.array([[0, 0, 0]]),
                          radii=0.2,
                          colors=np.array([1, 0, 1]))

Now we add the necessary actors to the scene and set the camera for better visualization.

scene = window.Scene()
scene.set_camera((10.28, -7.10, 6.39), (0.0, 0.0, 0.4), (-0.35, 0.26, 1.0))
scene.add(actor.axes(scale=(0.5, 0.5, 0.5)), base_actor, brick_actor)
scene.add(rope_actor, box_actor, ball_actor)

showm = window.ShowManager(scene,
                           size=(900, 768), reset_camera=False,
                           order_transparent=True)

showm.initialize()

Position the base correctly.

base_pos, base_orn = p.getBasePositionAndOrientation(base)
base_actor.SetPosition(*base_pos)

Calculate the vertices of the bricks.

brick_vertices = utils.vertices_from_actor(brick_actor)
num_vertices = brick_vertices.shape[0]
num_objects = brick_centers.shape[0]
brick_sec = np.int(num_vertices / num_objects)

Calculate the vertices of the wrecking ball.

chain_vertices = utils.vertices_from_actor(rope_actor)
num_vertices = chain_vertices.shape[0]
num_objects = brick_centers.shape[0]
chain_sec = np.int(num_vertices / num_objects)

We define methods to sync bricks and wrecking ball.

# Function for syncing actors with multibodies.
def sync_brick(object_index, multibody):
    pos, orn = p.getBasePositionAndOrientation(multibody)

    rot_mat = np.reshape(
        p.getMatrixFromQuaternion(
            p.getDifferenceQuaternion(orn, brick_orns[object_index])),
        (3, 3))

    sec = brick_sec

    brick_vertices[object_index * sec: object_index * sec + sec] = \
        (brick_vertices[object_index * sec: object_index * sec + sec] -
         brick_centers[object_index])@rot_mat + pos

    brick_centers[object_index] = pos
    brick_orns[object_index] = orn


def sync_chain(actor_list, multibody):
    for joint in range(p.getNumJoints(multibody)):
        # `p.getLinkState` offers various information about the joints
        # as a list and the values in 4th and 5th index refer to the joint's
        # position and orientation respectively.
        pos, orn = p.getLinkState(multibody, joint)[4:6]

        rot_mat = np.reshape(
            p.getMatrixFromQuaternion(
                p.getDifferenceQuaternion(orn, linkOrientations[joint])),
            (3, 3))

        sec = chain_sec

        chain_vertices[joint * sec: joint * sec + sec] =\
            (chain_vertices[joint * sec: joint * sec + sec] -
             linkPositions[joint])@rot_mat + pos

        linkPositions[joint] = pos
        linkOrientations[joint] = orn

Some helper tools to keep track of avg. FPS and simulation steps.

counter = itertools.count()
fpss = np.array([])
tb = ui.TextBlock2D(position=(0, 680), font_size=30, color=(1, 0.5, 0),
                    text="Avg. FPS: \nSim Steps: ")
scene.add(tb)

Timer callback to sync objects, simulate steps and apply force.

apply_force = True


# Create timer callback which will execute at each step of simulation.
def timer_callback(_obj, _event):
    global apply_force, fpss
    cnt = next(counter)
    showm.render()

    if cnt % 1 == 0:
        fps = scene.frame_rate
        fpss = np.append(fpss, fps)
        tb.message = "Avg. FPS: " + str(np.round(np.mean(fpss), 0)) +\
            "\nSim Steps: " + str(cnt)

    # Updating the position and orientation of each individual brick.
    for idx, brick in enumerate(bricks):
        sync_brick(idx, brick)

    pos, _ = p.getBasePositionAndOrientation(rope)

    if apply_force:
        p.applyExternalForce(rope, -1,
                             forceObj=[-500, 0, 0],
                             posObj=pos,
                             flags=p.WORLD_FRAME)
        apply_force = False

    pos = p.getLinkState(rope, p.getNumJoints(rope)-1)[4]
    ball_actor.SetPosition(*pos)
    sync_chain(rope_actor, rope)
    utils.update_actor(brick_actor)
    utils.update_actor(rope_actor)

    # Simulate a step.
    p.stepSimulation()

    if cnt == 130:
        showm.exit()


# Add the timer callback to showmanager.
# Increasing the duration value will slow down the simulation.
showm.add_timer_callback(True, 1, timer_callback)

interactive = False

# start simulation
if interactive:
    showm.start()

window.record(scene, size=(900, 768), out_path="viz_wrecking_ball.png")