.. note::
:class: sphx-glr-download-link-note
Click :ref:`here <sphx_glr_download_auto_examples_physics_using_pybullet_viz_wrecking_ball.py>` to download the full example code
.. rst-class:: sphx-glr-example-title
.. _sphx_glr_auto_examples_physics_using_pybullet_viz_wrecking_ball.py:
========================
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 being destroyed by
a wrecking ball.
First some imports.
.. code-block:: default
from fury import actor, window, utils, ui
import numpy as np
import pybullet as p
import itertools
Initiate pybullet and enable gravity.
.. code-block:: default
p.connect(p.DIRECT)
p.setGravity(0, 0, -10)
Define some handy parameters to customize simulation.
.. code-block:: default
# Parameters
wall_length = 5
wall_breadth = 5
wall_height = 5
brick_size = np.array([0.2, 0.4, 0.2])
n_links = 15
# Size of segments
dx_link = 0.1
link_mass = 0.5
base_mass = 0.1
# radius of the cylindrical links or the rope
radii = 0.1
ball_mass = 10
# radius of the wrecking ball
ball_radius = 0.5
ball_color = np.array([[1, 0, 0]])
joint_friction = 0.0005
Creating the base plane actor.
.. code-block:: default
# 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 definitions are made to render a NxNxN brick wall.
.. code-block:: default
# 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.
.. code-block:: default
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.
.. code-block:: default
# 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=ball_radius)
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.
.. code-block:: default
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.
.. code-block:: default
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=ball_radius,
colors=np.array([1, 0, 1]))
Now we add the necessary actors to the scene and set the camera for better
visualization.
.. code-block:: default
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.
.. code-block:: default
base_pos, base_orn = p.getBasePositionAndOrientation(base)
base_actor.SetPosition(*base_pos)
Calculate the vertices of the bricks.
.. code-block:: default
brick_vertices = utils.vertices_from_actor(brick_actor)
num_vertices = brick_vertices.shape[0]
num_objects = brick_centers.shape[0]
brick_sec = int(num_vertices / num_objects)
Calculate the vertices of the wrecking ball.
.. code-block:: default
chain_vertices = utils.vertices_from_actor(rope_actor)
num_vertices = chain_vertices.shape[0]
num_objects = brick_centers.shape[0]
chain_sec = int(num_vertices / num_objects)
We define methods to sync bricks and wrecking ball.
.. code-block:: default
# 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.
.. code-block:: default
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.
.. code-block:: default
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")
.. image:: /auto_examples/physics_using_pybullet/images/sphx_glr_viz_wrecking_ball_001.png
:class: sphx-glr-single-img
.. rst-class:: sphx-glr-timing
**Total running time of the script:** ( 0 minutes 0.235 seconds)
.. _sphx_glr_download_auto_examples_physics_using_pybullet_viz_wrecking_ball.py:
.. only :: html
.. container:: sphx-glr-footer
:class: sphx-glr-footer-example
.. container:: sphx-glr-download
:download:`Download Python source code: viz_wrecking_ball.py <viz_wrecking_ball.py>`
.. container:: sphx-glr-download
:download:`Download Jupyter notebook: viz_wrecking_ball.ipynb <viz_wrecking_ball.ipynb>`
.. only:: html
.. rst-class:: sphx-glr-signature
`Gallery generated by Sphinx-Gallery <https://sphinx-gallery.github.io>`_