.. DO NOT EDIT. .. THIS FILE WAS AUTOMATICALLY GENERATED BY SPHINX-GALLERY. .. TO MAKE CHANGES, EDIT THE SOURCE PYTHON FILE: .. "auto_examples/17_pybullet/viz_wrecking_ball.py" .. LINE NUMBERS ARE GIVEN BELOW. .. only:: html .. note:: :class: sphx-glr-download-link-note :ref:`Go to the end ` to download the full example code .. rst-class:: sphx-glr-example-title .. _sphx_glr_auto_examples_17_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. .. GENERATED FROM PYTHON SOURCE LINES 12-19 .. code-block:: Python import itertools import numpy as np import pybullet as p from fury import actor, ui, utils, window .. GENERATED FROM PYTHON SOURCE LINES 20-21 Initiate pybullet and enable gravity. .. GENERATED FROM PYTHON SOURCE LINES 21-25 .. code-block:: Python p.connect(p.DIRECT) p.setGravity(0, 0, -10) .. GENERATED FROM PYTHON SOURCE LINES 26-27 Define some handy parameters to customize simulation. .. GENERATED FROM PYTHON SOURCE LINES 27-50 .. code-block:: Python # 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 .. GENERATED FROM PYTHON SOURCE LINES 51-52 Creating the base plane actor. .. GENERATED FROM PYTHON SOURCE LINES 52-68 .. code-block:: Python # 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) .. GENERATED FROM PYTHON SOURCE LINES 69-70 The following definitions are made to render a NxNxN brick wall. .. GENERATED FROM PYTHON SOURCE LINES 70-89 .. code-block:: Python # 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) .. GENERATED FROM PYTHON SOURCE LINES 90-92 The following is the logic to position the bricks in our desired location and generate the actor. .. GENERATED FROM PYTHON SOURCE LINES 92-117 .. code-block:: Python 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, ) .. GENERATED FROM PYTHON SOURCE LINES 118-119 Now we render the wrecking ball consisting of a fixed hinge, a ball and rope. .. GENERATED FROM PYTHON SOURCE LINES 119-191 .. code-block:: Python # 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.astype(int), linkVisualShapeIndices=linkVisualShapeIndices.astype(int), linkPositions=linkPositions.astype(int), linkOrientations=linkOrientations.astype(int), linkInertialFramePositions=linkInertialFramePositions.astype(int), linkInertialFrameOrientations=linkInertialFrameOrns.astype(int), linkParentIndices=indices.astype(int), linkJointTypes=jointTypes.astype(int), linkJointAxis=axis.astype(int), ) .. GENERATED FROM PYTHON SOURCE LINES 192-193 Next we define the frictional force between the joints of wrecking ball. .. GENERATED FROM PYTHON SOURCE LINES 193-208 .. code-block:: Python 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, ) .. GENERATED FROM PYTHON SOURCE LINES 209-210 We add the following constraint to keep the cubical hinge fixed. .. GENERATED FROM PYTHON SOURCE LINES 210-226 .. code-block:: Python 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]) ) .. GENERATED FROM PYTHON SOURCE LINES 227-229 Now we add the necessary actors to the scene and set the camera for better visualization. .. GENERATED FROM PYTHON SOURCE LINES 229-240 .. code-block:: Python 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 ) .. GENERATED FROM PYTHON SOURCE LINES 241-242 Position the base correctly. .. GENERATED FROM PYTHON SOURCE LINES 242-246 .. code-block:: Python base_pos, base_orn = p.getBasePositionAndOrientation(base) base_actor.SetPosition(*base_pos) .. GENERATED FROM PYTHON SOURCE LINES 247-248 Calculate the vertices of the bricks. .. GENERATED FROM PYTHON SOURCE LINES 248-254 .. code-block:: Python 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) .. GENERATED FROM PYTHON SOURCE LINES 255-256 Calculate the vertices of the wrecking ball. .. GENERATED FROM PYTHON SOURCE LINES 256-263 .. code-block:: Python 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) .. GENERATED FROM PYTHON SOURCE LINES 264-265 We define methods to sync bricks and wrecking ball. .. GENERATED FROM PYTHON SOURCE LINES 265-312 .. code-block:: Python # 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 .. GENERATED FROM PYTHON SOURCE LINES 313-314 Some helper tools to keep track of avg. FPS and simulation steps. .. GENERATED FROM PYTHON SOURCE LINES 314-322 .. code-block:: Python 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) .. GENERATED FROM PYTHON SOURCE LINES 323-324 Timer callback to sync objects, simulate steps and apply force. .. GENERATED FROM PYTHON SOURCE LINES 324-377 .. code-block:: Python 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 = showm.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 = True #False # start simulation if interactive: showm.start() window.record(scene, size=(900, 768), out_path='viz_wrecking_ball.png') .. image-sg:: /auto_examples/17_pybullet/images/sphx_glr_viz_wrecking_ball_001.png :alt: viz wrecking ball :srcset: /auto_examples/17_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 3.473 seconds) .. _sphx_glr_download_auto_examples_17_pybullet_viz_wrecking_ball.py: .. only:: html .. container:: sphx-glr-footer sphx-glr-footer-example .. container:: sphx-glr-download sphx-glr-download-jupyter :download:`Download Jupyter notebook: viz_wrecking_ball.ipynb ` .. container:: sphx-glr-download sphx-glr-download-python :download:`Download Python source code: viz_wrecking_ball.py ` .. only:: html .. rst-class:: sphx-glr-signature `Gallery generated by Sphinx-Gallery `_