init 9_reduced_DOF using 3_contact

Author: liminchen

9_reduced_DOF/BarrierEnergy.py

@@ -0,0 +1,46 @@
+# ANCHOR: val_grad_hess
+import math
+import numpy as np
+
+dhat = 0.01
+kappa = 1e5
+
+def val(x, y_ground, 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, 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
+        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, 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
+        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
+# ANCHOR_END: val_grad_hess
+
+# ANCHOR: init_step_size
+def init_step_size(x, y_ground, 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
+# ANCHOR_END: init_step_size

9_reduced_DOF/GravityEnergy.py

@@ -0,0 +1,17 @@
+import numpy as np
+
+gravity = [0.0, -9.81]
+
+def val(x, m):
+    sum = 0.0
+    for i in range(0, len(x)):
+        sum += -m[i] * x[i].dot(gravity)
+    return sum
+
+def grad(x, m):
+    g = np.array([gravity] * len(x))
+    for i in range(0, len(x)):
+        g[i] *= -m[i]
+    return g
+
+# Hessian is 0

9_reduced_DOF/InertiaEnergy.py

@@ -0,0 +1,23 @@
+import numpy as np
+
+def val(x, x_tilde, m):
+    sum = 0.0
+    for i in range(0, len(x)):
+        diff = x[i] - x_tilde[i]
+        sum += 0.5 * m[i] * diff.dot(diff)
+    return sum
+
+def grad(x, x_tilde, m):
+    g = np.array([[0.0, 0.0]] * len(x))
+    for i in range(0, len(x)):
+        g[i] = m[i] * (x[i] - x_tilde[i])
+    return g
+
+def hess(x, x_tilde, m):
+    IJV = [[0] * (len(x) * 2), [0] * (len(x) * 2), np.array([0.0] * (len(x) * 2))]
+    for i in range(0, len(x)):
+        for d in range(0, 2):
+            IJV[0][i * 2 + d] = i * 2 + d
+            IJV[1][i * 2 + d] = i * 2 + d
+            IJV[2][i * 2 + d] = m[i]
+    return IJV

9_reduced_DOF/MassSpringEnergy.py

@@ -0,0 +1,35 @@
+import numpy as np
+import utils
+
+def val(x, e, l2, k):
+    sum = 0.0
+    for i in range(0, len(e)):
+        diff = x[e[i][0]] - x[e[i][1]]
+        sum += l2[i] * 0.5 * k[i] * (diff.dot(diff) / l2[i] - 1) ** 2
+    return sum
+
+def grad(x, e, l2, k):
+    g = np.array([[0.0, 0.0]] * len(x))
+    for i in range(0, len(e)):
+        diff = x[e[i][0]] - x[e[i][1]]
+        g_diff = 2 * k[i] * (diff.dot(diff) / l2[i] - 1) * diff
+        g[e[i][0]] += g_diff
+        g[e[i][1]] -= g_diff
+    return g
+
+def hess(x, e, l2, k):
+    IJV = [[0] * (len(e) * 16), [0] * (len(e) * 16), np.array([0.0] * (len(e) * 16))]
+    for i in range(0, len(e)):
+        diff = x[e[i][0]] - x[e[i][1]]
+        H_diff = 2 * k[i] / l2[i] * (2 * np.outer(diff, diff) + (diff.dot(diff) - l2[i]) * np.identity(2))
+        H_local = utils.make_PSD(np.block([[H_diff, -H_diff], [-H_diff, H_diff]]))
+        # add to global matrix
+        for nI in range(0, 2):
+            for nJ in range(0, 2):
+                indStart = i * 16 + (nI * 2 + nJ) * 4
+                for r in range(0, 2):
+                    for c in range(0, 2):
+                        IJV[0][indStart + r * 2 + c] = e[i][nI] * 2 + r
+                        IJV[1][indStart + r * 2 + c] = e[i][nJ] * 2 + c
+                        IJV[2][indStart + r * 2 + c] = H_local[nI * 2 + r, nJ * 2 + c]
+    return IJV

9_reduced_DOF/readme.md

@@ -0,0 +1,14 @@
+# Mass-Spring Solids Simulation
+
+A square falling onto the ground under gravity is simulated with mass-spring elasticity potential and implicit Euler time integration.
+Each time step is solved by minimizing the Incremental Potential with the projected Newton method.
+
+## Dependencies
+```
+pip install numpy scipy pygame
+```
+
+## Run
+```
+python simulator.py
+```

9_reduced_DOF/simulator.py

@@ -0,0 +1,72 @@
+# Mass-Spring Solids Simulation
+
+import numpy as np  # numpy for linear algebra
+import pygame       # pygame for visualization
+pygame.init()
+
+import square_mesh   # square mesh
+import time_integrator
+
+# simulation setup
+side_len = 1
+rho = 1000      # density of square
+k = 2e5         # spring stiffness
+n_seg = 4       # num of segments per side of the square
+h = 0.005        # 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
+v = np.array([[0.0, 0.0]] * len(x))             # velocity
+m = [rho * side_len * side_len / ((n_seg + 1) * (n_seg + 1))] * len(x)  # calculate node mass evenly
+# rest length squared
+l2 = []
+for i in range(0, len(e)):
+    diff = x[e[i][0]] - x[e[i][1]]
+    l2.append(diff.dot(diff))
+k = [k] * len(e)    # spring stiffness
+# identify whether a node is Dirichlet
+is_DBC = [False] * len(x)
+for i in DBC:
+    is_DBC[i] = True
+# ANCHOR: contact_area
+contact_area = [side_len / n_seg] * len(x)     # perimeter split to each node
+# ANCHOR_END: contact_area
+
+# 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
+square_mesh.write_to_file(time_step, x, n_seg)
+screen = pygame.display.set_mode(resolution)
+running = True
+while running:
+    # run until the user asks to quit
+    for event in pygame.event.get():
+        if event.type == pygame.QUIT:
+            running = False
+    
+    print('### Time step', time_step, '###')
+
+    # fill the background and draw the square
+    screen.fill((255, 255, 255))
+    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:
+        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, y_ground, contact_area, is_DBC, h, 1e-2)
+    time_step += 1
+    pygame.time.wait(int(h * 1000))
+    square_mesh.write_to_file(time_step, x, n_seg)
+
+pygame.quit()

9_reduced_DOF/square_mesh.py

@@ -0,0 +1,46 @@
+import numpy as np
+import os
+
+def generate(side_length, n_seg):
+    # sample nodes uniformly on a square
+    x = np.array([[0.0, 0.0]] * ((n_seg + 1) ** 2))
+    step = side_length / n_seg
+    for i in range(0, n_seg + 1):
+        for j in range(0, n_seg + 1):
+            x[i * (n_seg + 1) + j] = [-side_length / 2 + i * step, -side_length / 2 + j * step]
+    
+    # connect the nodes with edges
+    e = []
+    # horizontal edges
+    for i in range(0, n_seg):
+        for j in range(0, n_seg + 1):
+            e.append([i * (n_seg + 1) + j, (i + 1) * (n_seg + 1) + j])
+    # vertical edges
+    for i in range(0, n_seg + 1):
+        for j in range(0, n_seg):
+            e.append([i * (n_seg + 1) + j, i * (n_seg + 1) + j + 1])
+    # diagonals
+    for i in range(0, n_seg):
+        for j in range(0, n_seg):
+            e.append([i * (n_seg + 1) + j, (i + 1) * (n_seg + 1) + j + 1])
+            e.append([(i + 1) * (n_seg + 1) + j, i * (n_seg + 1) + j + 1])
+
+    return [x, e]
+
+def write_to_file(frameNum, x, n_seg):
+    # Check if 'output' directory exists; if not, create it
+    if not os.path.exists('output'):
+        os.makedirs('output')
+
+    # create obj file
+    filename = f"output/{frameNum}.obj"
+    with open(filename, 'w') as f:
+        # write vertex coordinates
+        for row in x:
+            f.write(f"v {float(row[0]):.6f} {float(row[1]):.6f} 0.0\n") 
+        # write vertex indices for each triangle
+        for i in range(0, n_seg):
+            for j in range(0, n_seg):
+                #NOTE: each cell is exported as 2 triangles for rendering
+                f.write(f"f {i * (n_seg+1) + j + 1} {(i+1) * (n_seg+1) + j + 1} {(i+1) * (n_seg+1) + j+1 + 1}\n")
+                f.write(f"f {i * (n_seg+1) + j + 1} {(i+1) * (n_seg+1) + j+1 + 1} {i * (n_seg+1) + j+1 + 1}\n")

9_reduced_DOF/time_integrator.py

@@ -0,0 +1,69 @@
+import copy
+from cmath import inf
+
+import numpy as np
+import numpy.linalg as LA
+import scipy.sparse as sparse
+from scipy.sparse.linalg import spsolve
+
+import InertiaEnergy
+import MassSpringEnergy
+import GravityEnergy
+import BarrierEnergy
+
+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, 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)
+
+        # ANCHOR: filter_ls
+        # filter line search
+        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
+        # ANCHOR_END: filter_ls
+        print('step size =', alpha)
+
+        x += alpha * p
+        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, 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, 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, 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, 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, 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)]: 
+            projected_hess[i, j] = (i == j)
+    for i in range(0, len(x)):
+        if is_DBC[i]:
+            reshaped_grad[i * 2] = reshaped_grad[i * 2 + 1] = 0.0
+    return spsolve(projected_hess, -reshaped_grad).reshape(len(x), 2)

9_reduced_DOF/utils.py

@@ -0,0 +1,9 @@
+import numpy as np
+import numpy.linalg as LA
+
+def make_PSD(hess):
+    [lam, V] = LA.eigh(hess)    # Eigen decomposition on symmetric matrix
+    # set all negative Eigenvalues to 0
+    for i in range(0, len(lam)):
+        lam[i] = max(0, lam[i])
+    return np.matmul(np.matmul(V, np.diag(lam)), np.transpose(V))