slope

Author: liminchen

friction/BarrierEnergy.py

@@ -3,11 +3,13 @@
 
 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):
     sum = 0.0
     for i in range(0, len(x)):
-        d = x[i][1] - y_ground
+        d = n.dot(x[i] - o)
         if d < dhat:
             s = d / dhat
             sum += contact_area[i] * dhat * kappa / 2 * (s - 1) * math.log(s)
@@ -16,27 +18,29 @@ def val(x, y_ground, 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
+        d = n.dot(x[i] - o)
         if d < dhat:
             s = d / dhat
-            g[i][1] = contact_area[i] * dhat * (kappa / 2 * (math.log(s) / dhat + (s - 1) / d))
+            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):
-    IJV = [[0] * len(x), [0] * len(x), np.array([0.0] * len(x))]
+    IJV = [[0] * 0, [0] * 0, np.array([0.0] * 0)]
     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
+        d = n.dot(x[i] - o)
         if d < dhat:
-            IJV[2][i] = contact_area[i] * dhat * kappa / (2 * d * d * dhat) * (d + dhat)
-        else:
-            IJV[2][i] = 0.0
+            local_hess = contact_area[i] * dhat * kappa / (2 * d * d * dhat) * (d + dhat) * np.outer(n, n)
+            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
 
 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])
+        p_n = p[i].dot(n)
+        if p_n < 0:
+            alpha = min(alpha, 0.9 * n.dot(x[i] - o) / -p_n)
     return alpha

friction/simulator.py

@@ -14,7 +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
-y_ground = -1   # height of the planar ground
+ground_n = np.array([0.0, 1.0])   # height of the planar ground
+ground_o = np.array([0.0, -1.0])
 
 # initialize simulation
 [x, e] = square_mesh.generate(side_len, n_seg)  # node positions and edge node indices