ground n and o as parameter

Author: liminchen

friction/BarrierEnergy.py

@@ -3,10 +3,8 @@
 
 dhat = 0.01
 kappa = 1e5
-n = np.array([0.0, 1.0])    #TODO: make as parameter
-o = np.array([0.0, -1.0])
 
-def val(x, y_ground, contact_area):
+def val(x, n, o, contact_area):
     sum = 0.0
     for i in range(0, len(x)):
         d = n.dot(x[i] - o)
@@ -15,7 +13,7 @@ def val(x, y_ground, contact_area):
             sum += contact_area[i] * dhat * kappa / 2 * (s - 1) * math.log(s)
     return sum
 
-def grad(x, y_ground, contact_area):
+def grad(x, n, o, contact_area):
     g = np.array([[0.0, 0.0]] * len(x))
     for i in range(0, len(x)):
         d = n.dot(x[i] - o)
@@ -24,7 +22,7 @@ def grad(x, y_ground, contact_area):
             g[i] = contact_area[i] * dhat * (kappa / 2 * (math.log(s) / dhat + (s - 1) / d)) * n
     return g
 
-def hess(x, y_ground, contact_area):
+def hess(x, n, o, contact_area):
     IJV = [[0] * 0, [0] * 0, np.array([0.0] * 0)]
     for i in range(0, len(x)):
         d = n.dot(x[i] - o)
@@ -37,7 +35,7 @@ def hess(x, y_ground, contact_area):
                     IJV[2] = np.append(IJV[2], local_hess[r, c])
     return IJV
 
-def init_step_size(x, y_ground, p):
+def init_step_size(x, n, o, p):
     alpha = 1
     for i in range(0, len(x)):
         p_n = p[i].dot(n)

friction/simulator.py

@@ -14,8 +14,8 @@
 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])   # height of the planar ground
-ground_o = np.array([0.0, -1.0])
+ground_n = np.array([0.0, 1.0])     # normal of the slope
+ground_o = np.array([0.0, -1.0])    # a point on the slope  
 
 # initialize simulation
 [x, e] = square_mesh.generate(side_len, n_seg)  # node positions and edge node indices
@@ -32,6 +32,7 @@
 for i in DBC:
     is_DBC[i] = True
 contact_area = [side_len / n_seg] * len(x)     # perimeter split to each node
+ground_n /= np.linalg.norm(ground_n)    # normalize ground normal vector just in case
 
 # simulation with visualization
 resolution = np.array([900, 900])
@@ -53,7 +54,8 @@ def screen_projection(x):
 
     # 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
+    pygame.draw.aaline(screen, (0, 0, 255), screen_projection([ground_o[0] - 3.0 * ground_n[1], ground_o[1] + 3.0 * ground_n[0]]), 
+        screen_projection([ground_o[0] + 3.0 * ground_n[1], ground_o[1] - 3.0 * ground_n[0]]))   # 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:
@@ -62,7 +64,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, y_ground, 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, is_DBC, h, 1e-2)
     time_step += 1
     pygame.time.wait(int(h * 1000))
 

friction/time_integrator.py

@@ -11,52 +11,52 @@
 import GravityEnergy
 import BarrierEnergy
 
-def step_forward(x, e, v, m, l2, k, y_ground, contact_area, is_DBC, h, tol):
+def step_forward(x, e, v, m, l2, k, n, o, 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)
+    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)
     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, 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 = 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:
             alpha /= 2
         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)
+        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)
         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_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_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_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_hess(x, e, x_tilde, m, l2, k, y_ground, contact_area, h):
+def IP_hess(x, e, x_tilde, m, l2, k, n, o, 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_B = BarrierEnergy.hess(x, n, o, 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)
+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)
     # 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)]: