square on slope with friction

Author: liminchen

friction/BarrierEnergy.py

@@ -41,4 +41,13 @@ def init_step_size(x, n, o, p):
         p_n = p[i].dot(n)
         if p_n < 0:
             alpha = min(alpha, 0.9 * n.dot(x[i] - o) / -p_n)
-    return alpha
+    return alpha
+
+def compute_mu_lambda(x, n, o, contact_area, mu):
+    mu_lambda = np.array([0.0] * len(x))
+    for i in range(0, len(x)):
+        d = n.dot(x[i] - o)
+        if d < dhat:
+            s = d / dhat
+            mu_lambda[i] = mu * -contact_area[i] * dhat * (kappa / 2 * (math.log(s) / dhat + (s - 1) / d))
+    return mu_lambda

friction/FrictionEnergy.py

@@ -0,0 +1,60 @@
+import numpy as np
+import utils
+
+epsv = 1e-3
+
+def f0(vhatnorm, epsv, hhat):
+    if vhatnorm >= epsv:
+        return vhatnorm * hhat
+    else:
+        vhatnormhhat = vhatnorm * hhat
+        epsvhhat = epsv * hhat
+        return vhatnormhhat * vhatnormhhat * (-vhatnormhhat / 3.0 + epsvhhat) / (epsvhhat * epsvhhat) + epsvhhat / 3.0
+
+def f1_div_vhatnorm(vhatnorm, epsv):
+    if vhatnorm >= epsv:
+        return 1.0 / vhatnorm
+    else:
+        return (-vhatnorm + 2.0 * epsv) / (epsv * epsv)
+
+def f_hess_term(vhatnorm, epsv):
+    if vhatnorm >= epsv:
+        return -1.0 / (vhatnorm * vhatnorm)
+    else:
+        return -1.0 / (epsv * epsv)
+
+def val(v, mu_lambda, hhat, n):
+    sum = 0.0
+    T = np.identity(2) - np.outer(n, n) # tangent of slope is constant
+    for i in range(0, len(v)):
+        if mu_lambda[i] > 0:
+            vhat = np.transpose(T).dot(v[i])
+            sum += mu_lambda[i] * f0(np.linalg.norm(vhat), epsv, hhat)
+    return sum
+
+def grad(v, mu_lambda, hhat, n):
+    g = np.array([[0.0, 0.0]] * len(v))
+    T = np.identity(2) - np.outer(n, n) # tangent of slope is constant
+    for i in range(0, len(v)):
+        if mu_lambda[i] > 0:
+            vhat = np.transpose(T).dot(v[i])
+            g[i] = mu_lambda[i] * f1_div_vhatnorm(np.linalg.norm(vhat), epsv) * T.dot(vhat)
+    return g
+
+def hess(v, mu_lambda, hhat, n):
+    IJV = [[0] * 0, [0] * 0, np.array([0.0] * 0)]
+    T = np.identity(2) - np.outer(n, n) # tangent of slope is constant
+    for i in range(0, len(v)):
+        if mu_lambda[i] > 0:
+            vhat = np.transpose(T).dot(v[i])
+            vhatnorm = np.linalg.norm(vhat)
+            inner_term = f1_div_vhatnorm(vhatnorm, epsv) * np.identity(2)
+            if vhatnorm != 0:
+                inner_term += f_hess_term(vhatnorm, epsv) / vhatnorm * np.outer(vhat, vhat)
+            local_hess = mu_lambda[i] * T.dot(utils.make_PD(inner_term)).dot(np.transpose(T)) / hhat
+            for c in range(0, 2):
+                for r in range(0, 2):
+                    IJV[0].append(i * 2 + r)
+                    IJV[1].append(i * 2 + c)
+                    IJV[2] = np.append(IJV[2], local_hess[r, c])
+    return IJV

friction/readme.md

@@ -1,6 +1,6 @@
 # Mass-Spring Solids Simulation
 
-A square falling onto the ground under gravity is simulated with mass-spring elasticity potential and implicit Euler time integration.
+A square sliding/residing on a slope 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

friction/simulator.py

@@ -14,8 +14,9 @@
 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
-ground_n = np.array([0.0, 1.0])     # normal of the slope
+ground_n = np.array([0.1, 1.0])     # normal of the slope
 ground_o = np.array([0.0, -1.0])    # a point on the slope  
+mu = 0.11        # friction coefficient of the slope
 
 # initialize simulation
 [x, e] = square_mesh.generate(side_len, n_seg)  # node positions and edge node indices
@@ -64,7 +65,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, ground_n, ground_o, contact_area, is_DBC, h, 1e-2)
+    [x, v] = time_integrator.step_forward(x, e, v, m, l2, k, ground_n, ground_o, contact_area, mu, is_DBC, h, 1e-2)
     time_step += 1
     pygame.time.wait(int(h * 1000))
 

friction/time_integrator.py

@@ -10,53 +10,58 @@
 import MassSpringEnergy
 import GravityEnergy
 import BarrierEnergy
+import FrictionEnergy
 
-def step_forward(x, e, v, m, l2, k, n, o, contact_area, is_DBC, h, tol):
+def step_forward(x, e, v, m, l2, k, n, o, contact_area, mu, is_DBC, h, tol):
     x_tilde = x + v * h     # implicit Euler predictive position
     x_n = copy.deepcopy(x)
+    mu_lambda = BarrierEnergy.compute_mu_lambda(x, n, o, contact_area, mu)  # compute mu * lambda for each node using x^n
 
     # Newton loop
     iter = 0
-    E_last = IP_val(x, e, x_tilde, m, l2, k, n, o, contact_area, h)
-    p = search_dir(x, e, x_tilde, m, l2, k, n, o, contact_area, is_DBC, h)
+    E_last = IP_val(x, e, x_tilde, m, l2, k, n, o, contact_area, (x - x_n) / h, mu_lambda, h)
+    p = search_dir(x, e, x_tilde, m, l2, k, n, o, contact_area, (x - x_n) / h, mu_lambda, 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, n, o, p)  # avoid interpenetration and tunneling
-        while IP_val(x + alpha * p, e, x_tilde, m, l2, k, n, o, contact_area, h) > E_last:
+        while IP_val(x + alpha * p, e, x_tilde, m, l2, k, n, o, contact_area, (x - x_n) / h, mu_lambda, h) > E_last:
             alpha /= 2
         print('step size =', alpha)
 
         x += alpha * p
-        E_last = IP_val(x, e, x_tilde, m, l2, k, n, o, contact_area, h)
-        p = search_dir(x, e, x_tilde, m, l2, k, n, o, contact_area, is_DBC, h)
+        E_last = IP_val(x, e, x_tilde, m, l2, k, n, o, contact_area, (x - x_n) / h, mu_lambda, h)
+        p = search_dir(x, e, x_tilde, m, l2, k, n, o, contact_area, (x - x_n) / h, mu_lambda, 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, n, o, 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, n, o, contact_area))     # implicit Euler
+def IP_val(x, e, x_tilde, m, l2, k, n, o, contact_area, v, mu_lambda, h):
+    return InertiaEnergy.val(x, x_tilde, m) + h * h * (MassSpringEnergy.val(x, e, l2, k) + GravityEnergy.val(x, m) + BarrierEnergy.val(x, n, o, contact_area) + FrictionEnergy.val(v, mu_lambda, h, n))     # implicit Euler
 
-def IP_grad(x, e, x_tilde, m, l2, k, n, o, 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, n, o, contact_area))   # implicit Euler
+def IP_grad(x, e, x_tilde, m, l2, k, n, o, contact_area, v, mu_lambda, h):
+    return InertiaEnergy.grad(x, x_tilde, m) + h * h * (MassSpringEnergy.grad(x, e, l2, k) + GravityEnergy.grad(x, m) + BarrierEnergy.grad(x, n, o, contact_area) + FrictionEnergy.grad(v, mu_lambda, h, n))   # implicit Euler
 
-def IP_hess(x, e, x_tilde, m, l2, k, n, o, contact_area, h):
+def IP_hess(x, e, x_tilde, m, l2, k, n, o, contact_area, v, mu_lambda, h):
     IJV_In = InertiaEnergy.hess(x, x_tilde, m)
     IJV_MS = MassSpringEnergy.hess(x, e, l2, k)
     IJV_B = BarrierEnergy.hess(x, n, o, contact_area)
+    IJV_F = FrictionEnergy.hess(v, mu_lambda, h, n)
     IJV_MS[2] *= h * h    # implicit Euler
     IJV_B[2] *= h * h     # implicit Euler
+    IJV_F[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)
+    IJV_In_MS_B = np.append(IJV_In_MS, IJV_B, axis=1)
+    IJV = np.append(IJV_In_MS_B, IJV_F, 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, n, o, contact_area, is_DBC, h):
-    projected_hess = IP_hess(x, e, x_tilde, m, l2, k, n, o, contact_area, h)
-    reshaped_grad = IP_grad(x, e, x_tilde, m, l2, k, n, o, contact_area, h).reshape(len(x) * 2, 1)
+def search_dir(x, e, x_tilde, m, l2, k, n, o, contact_area, v, mu_lambda, is_DBC, h):
+    projected_hess = IP_hess(x, e, x_tilde, m, l2, k, n, o, contact_area, v, mu_lambda, h)
+    reshaped_grad = IP_grad(x, e, x_tilde, m, l2, k, n, o, contact_area, v, mu_lambda, 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)]: