y_ground in simulator

Author: liminchen

contact/BarrierEnergy.py

@@ -1,11 +1,10 @@
 import math
 import numpy as np
 
-y_ground = -1
 dhat = 0.01
 kappa = 1e5
 
-def val(x, contact_area):
+def val(x, y_ground, contact_area):
     sum = 0.0
     for i in range(0, len(x)):
         d = x[i][1] - y_ground
@@ -14,7 +13,7 @@ def val(x, contact_area):
             sum += contact_area[i] * dhat * kappa / 2 * (s - 1) * math.log(s)
     return sum
 
-def grad(x, contact_area):
+def grad(x, y_ground, contact_area):
     g = np.array([[0.0, 0.0]] * len(x))
     for i in range(0, len(x)):
         d = x[i][1] - y_ground
@@ -23,7 +22,7 @@ def grad(x, contact_area):
             g[i][1] = contact_area[i] * dhat * (kappa / 2 * (math.log(s) / dhat + (s - 1) / d))
     return g
 
-def hess(x, contact_area):
+def hess(x, y_ground, contact_area):
     IJV = [[0] * len(x), [0] * len(x), np.array([0.0] * len(x))]
     for i in range(0, len(x)):
         IJV[0][i] = i * 2 + 1
@@ -35,7 +34,7 @@ def hess(x, contact_area):
             IJV[2][i] = 0.0
     return IJV
 
-def init_step_size(x, p):
+def init_step_size(x, y_ground, p):
     alpha = 1
     for i in range(0, len(x)):
         if p[i][1] < 0:

contact/simulator.py

@@ -9,11 +9,12 @@
 
 # simulation setup
 side_len = 1
-rho = 1000  # density of square
-k = 2e4     # spring stiffness
-n_seg = 4   # num of segments per side of the square
-h = 0.01    # time step size in s
-DBC = []    # no nodes need to be fixed
+rho = 1000      # density of square
+k = 2e4         # spring stiffness
+n_seg = 4       # num of segments per side of the square
+h = 0.01        # time step size in s
+DBC = []        # no nodes need to be fixed
+y_ground = -1   # height of the planar ground
 
 # initialize simulation
 [x, e] = square_mesh.generate(side_len, n_seg)  # node positions and edge node indices
@@ -51,7 +52,7 @@ def screen_projection(x):
 
     # fill the background and draw the square
     screen.fill((255, 255, 255))
-    pygame.draw.aaline(screen, (0, 0, 255), [0, offset[1] + 1 * scale], [resolution[1], offset[1] + 1 * scale])   # ground
+    pygame.draw.aaline(screen, (0, 0, 255), screen_projection([-2, y_ground]), screen_projection([2, y_ground]))   # ground
     for eI in e:
         pygame.draw.aaline(screen, (0, 0, 255), screen_projection(x[eI[0]]), screen_projection(x[eI[1]]))
     for xI in x:
@@ -60,7 +61,7 @@ def screen_projection(x):
     pygame.display.flip()   # flip the display
 
     # step forward simulation and wait for screen refresh
-    [x, v] = time_integrator.step_forward(x, e, v, m, l2, k, contact_area, is_DBC, h, 1e-2)
+    [x, v] = time_integrator.step_forward(x, e, v, m, l2, k, y_ground, contact_area, is_DBC, h, 1e-2)
     time_step += 1
     pygame.time.wait(int(h * 1000))
 

contact/time_integrator.py

@@ -11,52 +11,52 @@
 import GravityEnergy
 import BarrierEnergy
 
-def step_forward(x, e, v, m, l2, k, contact_area, is_DBC, h, tol):
+def step_forward(x, e, v, m, l2, k, y_ground, contact_area, is_DBC, h, tol):
     x_tilde = x + v * h     # implicit Euler predictive position
     x_n = copy.deepcopy(x)
 
     # Newton loop
     iter = 0
-    E_last = IP_val(x, e, x_tilde, m, l2, k, contact_area, h)
-    p = search_dir(x, e, x_tilde, m, l2, k, contact_area, is_DBC, h)
+    E_last = IP_val(x, e, x_tilde, m, l2, k, y_ground, contact_area, h)
+    p = search_dir(x, e, x_tilde, m, l2, k, y_ground, contact_area, is_DBC, h)
     while LA.norm(p, inf) / h > tol:
         print('Iteration', iter, ':')
         print('residual =', LA.norm(p, inf) / h)
 
         # filter line search
-        alpha = BarrierEnergy.init_step_size(x, p)  # avoid interpenetration and tunneling
-        while IP_val(x + alpha * p, e, x_tilde, m, l2, k, contact_area, h) > E_last:
+        alpha = BarrierEnergy.init_step_size(x, y_ground, p)  # avoid interpenetration and tunneling
+        while IP_val(x + alpha * p, e, x_tilde, m, l2, k, y_ground, contact_area, h) > E_last:
             alpha /= 2
         print('step size =', alpha)
 
         x += alpha * p
-        E_last = IP_val(x, e, x_tilde, m, l2, k, contact_area, h)
-        p = search_dir(x, e, x_tilde, m, l2, k, contact_area, is_DBC, h)
+        E_last = IP_val(x, e, x_tilde, m, l2, k, y_ground, contact_area, h)
+        p = search_dir(x, e, x_tilde, m, l2, k, y_ground, contact_area, is_DBC, h)
         iter += 1
 
     v = (x - x_n) / h   # implicit Euler velocity update
     return [x, v]
 
-def IP_val(x, e, x_tilde, m, l2, k, contact_area, h):
-    return InertiaEnergy.val(x, x_tilde, m) + h * h * (MassSpringEnergy.val(x, e, l2, k) + GravityEnergy.val(x, m) + BarrierEnergy.val(x, contact_area))     # implicit Euler
+def IP_val(x, e, x_tilde, m, l2, k, y_ground, contact_area, h):
+    return InertiaEnergy.val(x, x_tilde, m) + h * h * (MassSpringEnergy.val(x, e, l2, k) + GravityEnergy.val(x, m) + BarrierEnergy.val(x, y_ground, contact_area))     # implicit Euler
 
-def IP_grad(x, e, x_tilde, m, l2, k, contact_area, h):
-    return InertiaEnergy.grad(x, x_tilde, m) + h * h * (MassSpringEnergy.grad(x, e, l2, k) + GravityEnergy.grad(x, m) + BarrierEnergy.grad(x, contact_area))   # implicit Euler
+def IP_grad(x, e, x_tilde, m, l2, k, y_ground, contact_area, h):
+    return InertiaEnergy.grad(x, x_tilde, m) + h * h * (MassSpringEnergy.grad(x, e, l2, k) + GravityEnergy.grad(x, m) + BarrierEnergy.grad(x, y_ground, contact_area))   # implicit Euler
 
-def IP_hess(x, e, x_tilde, m, l2, k, contact_area, h):
+def IP_hess(x, e, x_tilde, m, l2, k, y_ground, contact_area, h):
     IJV_In = InertiaEnergy.hess(x, x_tilde, m)
     IJV_MS = MassSpringEnergy.hess(x, e, l2, k)
-    IJV_B = BarrierEnergy.hess(x, contact_area)
+    IJV_B = BarrierEnergy.hess(x, y_ground, contact_area)
     IJV_MS[2] *= h * h    # implicit Euler
     IJV_B[2] *= h * h     # implicit Euler
     IJV_In_MS = np.append(IJV_In, IJV_MS, axis=1)
     IJV = np.append(IJV_In_MS, IJV_B, axis=1)
     H = sparse.coo_matrix((IJV[2], (IJV[0], IJV[1])), shape=(len(x) * 2, len(x) * 2)).tocsr()
     return H
 
-def search_dir(x, e, x_tilde, m, l2, k, contact_area, is_DBC, h):
-    projected_hess = IP_hess(x, e, x_tilde, m, l2, k, contact_area, h)
-    reshaped_grad = IP_grad(x, e, x_tilde, m, l2, k, contact_area, h).reshape(len(x) * 2, 1)
+def search_dir(x, e, x_tilde, m, l2, k, y_ground, contact_area, is_DBC, h):
+    projected_hess = IP_hess(x, e, x_tilde, m, l2, k, y_ground, contact_area, h)
+    reshaped_grad = IP_grad(x, e, x_tilde, m, l2, k, y_ground, contact_area, h).reshape(len(x) * 2, 1)
     # eliminate DOF by modifying gradient and Hessian for DBC:
     for i, j in zip(*projected_hess.nonzero()):
         if is_DBC[int(i / 2)] | is_DBC[int(j / 2)]: