Fixed wrecking ball radius bug

Increasing 'radii' was changing the link's radius instead of the wrecking ball.
Fixed some typos in Physics simulation folder.

fixed physics demos according to pep8

docs/examples/physics_using_pybullet/README.rst

@@ -1,4 +1,4 @@
 Integrate Physics using pybullet
--------
+--------------------------------
 
-These tutorials show how to use connect FURY with a physics engine library.
+These demos show how to use connect FURY with a physics engine library.

docs/examples/physics_using_pybullet/viz_ball_collide.py

@@ -17,7 +17,7 @@
 client = p.connect(p.DIRECT)
 
 ###############################################################################
-# Parameters and defination of red and blue balls.
+# Parameters and definition of red and blue balls.
 
 red_radius = 0.5
 blue_radius = 0.5

docs/examples/physics_using_pybullet/viz_brick_wall.py

@@ -59,7 +59,7 @@
 ball_coll = p.createCollisionShape(p.GEOM_SPHERE,
                                    radius=ball_radius)
 
-# Creating a Multibody which will be tracked by pybullet.
+# Creating a multi-body which will be tracked by pybullet.
 ball = p.createMultiBody(baseMass=3,
                          baseCollisionShapeIndex=ball_coll,
                          basePosition=ball_position,
@@ -77,21 +77,21 @@
                        colors=base_color)
 
 base_coll = p.createCollisionShape(p.GEOM_BOX,
-                                   halfExtents=base_size/2)
+                                   halfExtents=base_size / 2)
 # half of the actual size.
 
 base = p.createMultiBody(
-                          baseCollisionShapeIndex=base_coll,
-                          basePosition=base_position,
-                          baseOrientation=base_orientation)
+    baseCollisionShapeIndex=base_coll,
+    basePosition=base_position,
+    baseOrientation=base_orientation)
 
 p.changeDynamics(base, -1, lateralFriction=0.3, restitution=0.5)
 
 ###############################################################################
 # Now we render the bricks. All the bricks are rendered by a single actor for
 # better performance.
 
-nb_bricks = wall_height*wall_width
+nb_bricks = wall_height * wall_width
 
 brick_centers = np.zeros((nb_bricks, 3))
 
@@ -106,7 +106,7 @@
 brick_colors = np.random.rand(nb_bricks, 3)
 
 brick_coll = p.createCollisionShape(p.GEOM_BOX,
-                                    halfExtents=brick_size/2)
+                                    halfExtents=brick_size / 2)
 
 # We use this array to store the reference of brick objects in pybullet world.
 bricks = np.zeros(nb_bricks, dtype=np.int8)
@@ -115,7 +115,7 @@
 i = 0
 for k in range(wall_height):
     for j in range(wall_width):
-        center_pos = np.array([-1, (j*0.4)-1.8, (0.2*k)+0.1])
+        center_pos = np.array([-1, (j * 0.4) - 1.8, (0.2 * k) + 0.1])
         brick_centers[i] = center_pos
         brick_orns[i] = np.array([0, 0, 0, 1])
         bricks[i] = p.createMultiBody(baseMass=brick_mass,
@@ -146,7 +146,7 @@
 
 showm.initialize()
 
-# Counter interator for tracking simulation steps.
+# Counter iterator for tracking simulation steps.
 counter = itertools.count()
 
 # Variable for tracking applied force.
@@ -169,6 +169,7 @@
 num_objects = brick_centers.shape[0]
 sec = np.int(num_vertices / num_objects)
 
+
 ###############################################################################
 # ==============
 # Syncing Bricks
@@ -194,11 +195,12 @@ def sync_brick(object_index, multibody):
 
     vertices[object_index * sec: object_index * sec + sec] = \
         (vertices[object_index * sec: object_index * sec + sec] -
-         brick_centers[object_index])@rot_mat + pos
+         brick_centers[object_index]) @ rot_mat + pos
 
     brick_centers[object_index] = pos
     brick_orns[object_index] = orn
 
+
 ###############################################################################
 # A simpler but inaccurate approach is used here to update the position and
 # orientation.
@@ -226,6 +228,7 @@ def sync_actor(actor, multibody):
                  focal_point=(0.0, 0.0, 0.79),
                  view_up=(-0.27, 0.26, 0.90))
 
+
 ###############################################################################
 # Timer callback is created which is responsible for calling the sync and
 # simulation methods.
@@ -240,7 +243,7 @@ def timer_callback(_obj, _event):
     if cnt % 1 == 0:
         fps = scene.frame_rate
         fpss = np.append(fpss, fps)
-        tb.message = "Avg. FPS: " + str(np.round(np.mean(fpss), 0)) +\
+        tb.message = "Avg. FPS: " + str(np.round(np.mean(fpss), 0)) + \
                      "\nSim Steps: " + str(cnt)
 
     # Get the position and orientation of the ball.

docs/examples/physics_using_pybullet/viz_chain.py

@@ -22,11 +22,11 @@
 p.setGravity(0, 0, -10)
 
 ###############################################################################
-# Now we render the Chain using the following parameters and definations.
+# Now we render the Chain using the following parameters and definitions.
 
 # Parameters
 n_links = 20
-dx_link = 0.1       # Size of segments
+dx_link = 0.1  # Size of segments
 link_mass = 0.5
 base_mass = 0.1
 radii = 0.5
@@ -36,7 +36,7 @@
 link_shape = p.createCollisionShape(p.GEOM_CYLINDER,
                                     radius=radii,
                                     height=dx_link,
-                                    collisionFramePosition=[0, 0, -dx_link/2])
+                                    collisionFramePosition=[0, 0, -dx_link / 2])
 
 base_shape = p.createCollisionShape(p.GEOM_BOX,
                                     halfExtents=[0.01, 0.01, 0.01])
@@ -98,8 +98,8 @@
 ###############################################################################
 # We remove stiffness among the joints by adding friction to them.
 
-friction_vec = [joint_friction]*3   # same all axis
-control_mode = p.POSITION_CONTROL   # set pos control mode
+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],
@@ -108,7 +108,6 @@
                                    velocityGain=1,
                                    force=friction_vec)
 
-
 ###############################################################################
 # Next, we define a constraint base that will help us in the oscillation of the
 # chain.
@@ -151,7 +150,7 @@
 ###############################################################################
 # We define a couple of syncing methods for the base and chain.
 
-# Function for syncing actors with multibodies.
+# Function for syncing actors with multi-bodies.
 def sync_actor(actor, multibody):
     pos, orn = p.getBasePositionAndOrientation(multibody)
     actor.SetPosition(*pos)
@@ -177,9 +176,9 @@ def sync_joints(actor_list, multibody):
                 p.getDifferenceQuaternion(orn, linkOrientations[joint])),
             (3, 3))
 
-        vertices[joint * sec: joint * sec + sec] =\
+        vertices[joint * sec: joint * sec + sec] = \
             (vertices[joint * sec: joint * sec + sec] -
-             linkPositions[joint])@rot_mat + pos
+             linkPositions[joint]) @ rot_mat + pos
 
         linkPositions[joint] = pos
         linkOrientations[joint] = orn
@@ -196,6 +195,7 @@ def sync_joints(actor_list, multibody):
 t = 0.0
 freq_sim = 240
 
+
 ###############################################################################
 # Timer callback to sync objects, simulate steps and oscillate the base.
 
@@ -205,20 +205,20 @@ def timer_callback(_obj, _event):
     global t, fpss
     showm.render()
 
-    t += 1./freq_sim
+    t += 1. / freq_sim
 
     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)
+        tb.message = "Avg. FPS: " + str(np.round(np.mean(fpss), 0)) + \
+                     "\nSim Steps: " + str(cnt)
 
     # some trajectory
-    ux = amplitude_x*np.sin(2*np.pi*freq*t)
-    uy = amplitude_y*np.cos(2*np.pi*freq*t)
+    ux = amplitude_x * np.sin(2 * np.pi * freq * t)
+    uy = amplitude_y * np.cos(2 * np.pi * freq * t)
 
-    # move base arround
-    pivot = [3*ux, uy, 2]
+    # move base around
+    pivot = [3 * ux, uy, 2]
     orn = p.getQuaternionFromEuler([0, 0, 0])
     p.changeConstraint(root_robe_c, pivot, jointChildFrameOrientation=orn,
                        maxForce=500)

docs/examples/physics_using_pybullet/viz_wrecking_ball.py

@@ -4,7 +4,7 @@
 ========================
 
 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
+in fury. In this example we specifically render a brick wall being destroyed by
 a wrecking ball.
 
 First some imports.
@@ -31,12 +31,15 @@
 brick_size = np.array([0.2, 0.4, 0.2])
 
 n_links = 15
-dx_link = 0.1       # Size of segments
+# 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]])
 
@@ -53,16 +56,16 @@
 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])
+    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.
+# The following definitions are made to render a NxNxN brick wall.
 
 # Generate bricks.
-nb_bricks = wall_length*wall_breadth*wall_height
+nb_bricks = wall_length * wall_breadth * wall_height
 brick_centers = np.zeros((nb_bricks, 3))
 
 brick_directions = np.zeros((nb_bricks, 3))
@@ -76,7 +79,7 @@
 brick_colors = np.random.rand(nb_bricks, 3)
 
 brick_coll = p.createCollisionShape(p.GEOM_BOX,
-                                    halfExtents=brick_size/2)
+                                    halfExtents=brick_size / 2)
 
 bricks = np.zeros(nb_bricks, dtype=np.int16)
 
@@ -89,7 +92,7 @@
 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])
+            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,
@@ -112,13 +115,12 @@
 link_shape = p.createCollisionShape(p.GEOM_CYLINDER,
                                     radius=radii,
                                     height=dx_link,
-                                    collisionFramePosition=[0, 0, -dx_link/2])
+                                    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)
-
+                                    radius=ball_radius)
 
 visualShapeId = -1
 
@@ -178,8 +180,8 @@
 ###############################################################################
 # 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
+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],
@@ -201,7 +203,7 @@
                       colors=np.array([[1, 0, 0]]))
 
 ball_actor = actor.sphere(centers=np.array([[0, 0, 0]]),
-                          radii=0.2,
+                          radii=ball_radius,
                           colors=np.array([1, 0, 1]))
 
 ###############################################################################
@@ -258,7 +260,7 @@ def sync_brick(object_index, multibody):
 
     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]) @ rot_mat + pos
 
     brick_centers[object_index] = pos
     brick_orns[object_index] = orn
@@ -278,9 +280,9 @@ def sync_chain(actor_list, multibody):
 
         sec = chain_sec
 
-        chain_vertices[joint * sec: joint * sec + sec] =\
+        chain_vertices[joint * sec: joint * sec + sec] = \
             (chain_vertices[joint * sec: joint * sec + sec] -
-             linkPositions[joint])@rot_mat + pos
+             linkPositions[joint]) @ rot_mat + pos
 
         linkPositions[joint] = pos
         linkOrientations[joint] = orn
@@ -310,8 +312,8 @@ def timer_callback(_obj, _event):
     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)
+        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):
@@ -326,7 +328,7 @@ def timer_callback(_obj, _event):
                              flags=p.WORLD_FRAME)
         apply_force = False
 
-    pos = p.getLinkState(rope, p.getNumJoints(rope)-1)[4]
+    pos = p.getLinkState(rope, p.getNumJoints(rope) - 1)[4]
     ball_actor.SetPosition(*pos)
     sync_chain(rope_actor, rope)
     utils.update_actor(brick_actor)