compatible with torch 1.4.0 and trajectories

demos/control_demo.py

@@ -0,0 +1,86 @@
+import sys
+import torch
+import pygame
+
+from lcp_physics.physics.bodies import Circle, Rect, Hull
+from lcp_physics.physics.constraints import Joint, YConstraint, XConstraint, RotConstraint, TotalConstraint
+from lcp_physics.physics.constraints import FixedJoint
+from lcp_physics.physics.forces import ExternalForce, Gravity, vert_impulse, hor_impulse, MDP
+from lcp_physics.physics.utils import Defaults, Recorder
+from lcp_physics.physics.world import World, run_world_traj, Trajectory
+import numpy as np
+from numpy import loadtxt
+
+vel = np.array([[1.0,1.0,1.0,1.0,1.0],[-0.0,-0.0,-0.0,-0.0,-0.0]])/10
+traj1 = vel.copy() 
+traj1[0,:] = 2500*vel[0,:]
+traj1[1,:] = -2500*vel[1,:]
+
+traj2 = vel.copy() 
+traj2[0,:] = 2500*vel[0,:]
+traj2[1,:] = 2500*vel[1,:] 
+
+pygame.init()
+screen = pygame.display.set_mode((1000, 1000), pygame.DOUBLEBUF)
+screen.set_alpha(None)
+
+polygon = np.array((loadtxt("../DynamicAffordances/data/polygons_1_2f_sq.csv", delimiter=',')))
+
+bodies = []
+joints = []
+restitution = 0.00 # no impacts in quasi-dynamics
+fric_coeff = 0.01
+n_pol = int(polygon[0,0])
+
+xr = 500
+yr = 500
+
+# adds body based on triangulation
+r0 = Hull([xr, yr], [[1, 1], [-1, 1], [-1, -1], [1, -1]],
+         restitution=0.00, fric_coeff=0.00, mass = 0.01, name="obj")
+bodies.append(r0)
+
+for i in range(n_pol):
+	x2 = [polygon[0,1+8*i], -polygon[0,2+8*i]]
+	x1 = [polygon[0,3+8*i], -polygon[0,4+8*i]]
+	x0 = [polygon[0,5+8*i], -polygon[0,6+8*i]]
+	verts = 250*np.array([x0, x1, x2])
+	print(verts)
+	p0 = np.array([xr + polygon[0,7+8*i], yr - polygon[0,8+8*i]])
+	r1 = Hull(p0, verts, restitution=restitution, mass = 0.0001, fric_coeff=1, name="obj_"+str(i))
+	print('disp1')
+	r1.add_force(MDP(g=100))
+	# r1.add_force(Gravity(g=100))
+	bodies.append(r1)
+	joints += [FixedJoint(r1, r0)]
+	r1.add_no_contact(r0)
+	r0 = r1
+
+# Manipulators
+c = Circle([200, 550], 5, mass = 100000000, vel=(0, 0, 0), restitution=restitution,
+            fric_coeff=1, name = "f1")
+bodies.append(c)
+
+c = Circle([200, 450], 5, mass = 100000000, vel=(0, 0, 0), restitution=restitution,
+            fric_coeff=1, name = "f2")
+bodies.append(c)
+
+vel = torch.tensor(traj1)
+traj_f = [] 
+traj_f.append(Trajectory(vel = vel, name = "f1"))
+
+vel = torch.tensor(traj2)
+traj_f.append(Trajectory(vel = vel, name = "f2"))
+
+# Environment
+# r = Rect([0, 500, 505], [900, 10],
+#          restitution=restitution, fric_coeff=1)
+# bodies.append(r)
+# joints.append(TotalConstraint(r))
+
+recorder = None
+world = World(bodies, joints, dt=0.1)
+run_world_traj(world, run_time=5, screen=screen, recorder=recorder, traj=traj_f)
+print('\n')
+print(world.states)
+print('\n')

demos/demo.py

@@ -1,3 +1,4 @@
+
 import math
 import sys
 
@@ -165,7 +166,7 @@ def timed_force(t):
         screen.set_alpha(None)
         pygame.display.set_caption('2D Engine')
 
-    slide_demo(screen)
-    fric_demo(screen)
-    chain_demo(screen)
-    debug_demo(screen)
+    # slide_demo(screen)
+    # fric_demo(screen)
+    # chain_demo(screen)
+    debug_demo(screen)

lcp_physics/lcp/lcp.py

@@ -1,24 +1,19 @@
 import torch
-from torch.autograd import Function
-
 from .solvers import pdipm
+import numpy as np
 from .util import bger, extract_batch_size
 
-
-class LCPFunction(Function):
+class LCPFunction(torch.autograd.Function):
     """A differentiable LCP solver, uses the primal dual interior point method
        implemented in pdipm.
     """
-    def __init__(self, eps=1e-12, verbose=-1, not_improved_lim=3,
-                 max_iter=10):
+    # @staticmethod
+    def __init__(self):
         super().__init__()
-        self.eps = eps
-        self.verbose = verbose
-        self.not_improved_lim = not_improved_lim
-        self.max_iter = max_iter
         self.Q_LU = self.S_LU = self.R = None
 
     # @profile
+    @staticmethod
     def forward(self, Q, p, G, h, A, b, F):
         _, nineq, nz = G.size()
         neq = A.size(1) if A.ndimension() > 1 else 0
@@ -28,20 +23,23 @@ def forward(self, Q, p, G, h, A, b, F):
         self.Q_LU, self.S_LU, self.R = pdipm.pre_factor_kkt(Q, G, F, A)
         zhats, self.nus, self.lams, self.slacks = pdipm.forward(
             Q, p, G, h, A, b, F, self.Q_LU, self.S_LU, self.R,
-            eps=self.eps, max_iter=self.max_iter, verbose=self.verbose,
-            not_improved_lim=self.not_improved_lim)
+            eps=1e-18, max_iter=10, verbose=-1,
+            not_improved_lim=3)
 
         self.save_for_backward(zhats, Q, p, G, h, A, b, F)
         return zhats
 
+    @staticmethod
     def backward(self, dl_dzhat):
+
+        # print(dl_dzhat)
         zhats, Q, p, G, h, A, b, F = self.saved_tensors
         batch_size = extract_batch_size(Q, p, G, h, A, b)
 
         neq, nineq, nz = self.neq, self.nineq, self.nz
 
         # D = torch.diag((self.lams / self.slacks).squeeze(0)).unsqueeze(0)
-        d = self.lams / self.slacks
+        d = self.lams / (self.slacks + 1e-30)
 
         pdipm.factor_kkt(self.S_LU, self.R, d)
         dx, _, dlam, dnu = pdipm.solve_kkt(self.Q_LU, d, G, A, self.S_LU,
@@ -61,12 +59,26 @@ def backward(self, dl_dzhat):
         dQs = 0.5 * (bger(dx, zhats) + bger(zhats, dx))
 
         grads = (dQs, dps, dGs, dhs, dAs, dbs, dFs)
+
+        if np.isnan(grads[0].sum()).item() > 0:
+            import pdb
+            pdb.set_trace()
+
+        # print('\n\n\na\n\n')
+        # print(grads)
+        # print('\n\n\nb\n\n')
+
         return grads
 
+    @staticmethod
     def numerical_backward(self, dl_dzhat):
         # XXX experimental
         # adapted from pytorch's grad check
         # from torch.autograd.gradcheck import get_numerical_jacobian
+
+
+        print('dl_dzhat')
+
         from torch.autograd import Variable
         from collections import Iterable