ground contact

Author: liminchen

contact/BarrierEnergy.py

@@ -0,0 +1,43 @@
+import math
+import numpy as np
+
+y_ground = -1
+dhat = 0.01
+kappa = 1e5
+
+def val(x, contact_area):
+    sum = 0.0
+    for i in range(0, len(x)):
+        d = x[i][1] - y_ground
+        if d < dhat:
+            s = d / dhat
+            sum += contact_area[i] * dhat * kappa / 2 * (s - 1) * math.log(s)
+    return sum
+
+def grad(x, contact_area):
+    g = np.array([[0.0, 0.0]] * len(x))
+    for i in range(0, len(x)):
+        d = x[i][1] - y_ground
+        if d < dhat:
+            s = d / dhat
+            g[i][1] = contact_area[i] * dhat * (kappa / 2 * (math.log(s) / dhat + (s - 1) / d))
+    return g
+
+def hess(x, 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
+        IJV[1][i] = i * 2 + 1
+        d = x[i][1] - y_ground
+        if d < dhat:
+            IJV[2][i] = contact_area[i] * dhat * kappa / (2 * d * d * dhat) * (d + dhat)
+        else:
+            IJV[2][i] = 0.0
+    return IJV
+
+def init_step_size(x, p):
+    alpha = 1
+    for i in range(0, len(x)):
+        if p[i][1] < 0:
+            alpha = min(alpha, 0.9 * (y_ground - x[i][1]) / p[i][1])
+    return alpha

contact/GravityEnergy.py

@@ -1,6 +1,6 @@
 import numpy as np
 
-gravity = [0.0, 9.81]
+gravity = [0.0, -9.81]
 
 def val(x, m):
     sum = 0.0

contact/simulator.py

@@ -10,10 +10,10 @@
 # simulation setup
 side_len = 1
 rho = 1000  # density of square
-k = 1e3     # spring stiffness
+k = 2e4     # spring stiffness
 n_seg = 4   # num of segments per side of the square
-h = 0.02    # time step size in s
-DBC = [0, (n_seg + 1) * n_seg]  # fix the left and right top nodes
+h = 0.01    # time step size in s
+DBC = []    # no nodes need to be fixed
 
 # initialize simulation
 [x, e] = square_mesh.generate(side_len, n_seg)  # node positions and edge node indices
@@ -29,10 +29,17 @@
 is_DBC = [False] * len(x)
 for i in DBC:
     is_DBC[i] = True
+contact_area = [side_len / n_seg] * len(x)     # perimeter split to each node
 
 # simulation with visualization
+resolution = np.array([900, 900])
+offset = resolution / 2
+scale = 200
+def screen_projection(x):
+    return [offset[0] + scale * x[0], resolution[1] - (offset[1] + scale * x[1])]
+
 time_step = 0
-screen = pygame.display.set_mode([900, 900])
+screen = pygame.display.set_mode(resolution)
 running = True
 while running:
     # run until the user asks to quit
@@ -44,15 +51,16 @@
 
     # 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
     for eI in e:
-        pygame.draw.aaline(screen, (0, 0, 255), 450 + x[eI[0]] * 200, 450 + x[eI[1]] * 200)
+        pygame.draw.aaline(screen, (0, 0, 255), screen_projection(x[eI[0]]), screen_projection(x[eI[1]]))
     for xI in x:
-        pygame.draw.circle(screen, (0, 0, 255), 450 + xI * 200, 0.1 * side_len / n_seg * 200)
+        pygame.draw.circle(screen, (0, 0, 255), screen_projection(xI), 0.1 * side_len / n_seg * scale)
 
     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, is_DBC, h, 1e-2)
+    [x, v] = time_integrator.step_forward(x, e, v, m, l2, k, contact_area, is_DBC, h, 1e-2)
     time_step += 1
     pygame.time.wait(int(h * 1000))
 

contact/time_integrator.py

@@ -9,50 +9,54 @@
 import InertiaEnergy
 import MassSpringEnergy
 import GravityEnergy
+import BarrierEnergy
 
-def step_forward(x, e, v, m, l2, k, is_DBC, h, tol):
+def step_forward(x, e, v, m, l2, k, 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, h)
-    p = search_dir(x, e, x_tilde, m, l2, k, is_DBC, h)
+    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)
     while LA.norm(p, inf) / h > tol:
         print('Iteration', iter, ':')
         print('residual =', LA.norm(p, inf) / h)
 
         # line search
-        alpha = 1
-        while IP_val(x + alpha * p, e, x_tilde, m, l2, k, h) > E_last:
+        alpha = BarrierEnergy.init_step_size(x, p)    # filter line search to avoid interpenetration and tunneling
+        while IP_val(x + alpha * p, e, x_tilde, m, l2, k, 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, h)
-        p = search_dir(x, e, x_tilde, m, l2, k, is_DBC, h)
+        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)
         iter += 1
 
     v = (x - x_n) / h   # implicit Euler velocity update
     return [x, v]
 
-def IP_val(x, e, x_tilde, m, l2, k, h):
-    return InertiaEnergy.val(x, x_tilde, m) + h * h * (MassSpringEnergy.val(x, e, l2, k) + GravityEnergy.val(x, m))     # implicit Euler
+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_grad(x, e, x_tilde, m, l2, k, h):
-    return InertiaEnergy.grad(x, x_tilde, m) + h * h * (MassSpringEnergy.grad(x, e, l2, k) + GravityEnergy.grad(x, m))   # 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_hess(x, e, x_tilde, m, l2, k, h):
+def IP_hess(x, e, x_tilde, m, l2, k, 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_MS[2] *= h * h    # implicit Euler
-    IJV = np.append(IJV_In, IJV_MS, axis=1)
+    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, is_DBC, h):
-    projected_hess = IP_hess(x, e, x_tilde, m, l2, k, h)
-    reshaped_grad = IP_grad(x, e, x_tilde, m, l2, k, h).reshape(len(x) * 2, 1)
+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)
     # 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)]: