Jan/split out impact solver

DojoEnvironments/src/mechanisms/ant/mechanism.jl

@@ -54,14 +54,14 @@ function get_ant(;
             [0.2; -0.2; 0]
         ]
         contact_radii = [body.shape.shapes[1].rh[1] for body in contact_bodies]
-        contacts = [contacts;contact_constraint(contact_bodies, normals; friction_coefficients, contact_origins, contact_radii)]
+        contacts = [contacts;ContactConstraint(NonlinearContact(contact_bodies, normals, friction_coefficients; contact_origins, contact_radii))]
     end
 
     if contact_body
         # torso contact
         torso = get_body(mechanism, :torso)
         contact_radius = torso.shape.r
-        contacts = [contacts;contact_constraint(torso, Z_AXIS; friction_coefficient, contact_radius)]
+        contacts = [contacts;ContactConstraint(NonlinearContact(torso, Z_AXIS, friction_coefficient; contact_radius))]
 
         # elbow contact
         body_names = [:aux_1, :aux_2, :aux_3, :aux_4]
@@ -76,7 +76,7 @@ function get_ant(;
             -[0.1; -0.1; 0]
         ]
         contact_radii = [body.shape.shapes[1].rh[1] for body in contact_bodies]
-        contacts = [contacts;contact_constraint(contact_bodies, normals; friction_coefficients, contact_origins, contact_radii)]
+        contacts = [contacts;ContactConstraint(NonlinearContact(contact_bodies, normals, friction_coefficients; contact_origins, contact_radii))]
     end
 
     mechanism = Mechanism(mechanism.origin, mechanism.bodies, mechanism.joints, contacts; 

DojoEnvironments/src/mechanisms/atlas/mechanism.jl

@@ -51,8 +51,8 @@ function get_atlas(;
 
         contacts = [
             contacts
-            contact_constraint(left_foot, normals; friction_coefficients, contact_origins, contact_radii, names=left_names)
-            contact_constraint(right_foot, normals; friction_coefficients, contact_origins, contact_radii, names=right_names)
+            ContactConstraint(NonlinearContact(left_foot, normals, friction_coefficients; contact_origins, contact_radii); names=left_names)
+            ContactConstraint(NonlinearContact(right_foot, normals, friction_coefficients; contact_origins, contact_radii); names=right_names)
         ]
     end
 
@@ -93,7 +93,7 @@ function get_atlas(;
         ]
         contacts = [
             contacts
-            contact_constraint(contact_bodies, normals; friction_coefficients, contact_origins, contact_radii, names)
+            ContactConstraint(NonlinearContact(contact_bodies, normals, friction_coefficients; contact_origins, contact_radii); names)
         ]
     end
 

DojoEnvironments/src/mechanisms/block/mechanism.jl

@@ -56,7 +56,7 @@ function get_block(;
         ]
         contact_radii = fill(contact_radius,n)
 
-        contacts = [contacts;contact_constraint(block, normals; friction_coefficients, contact_origins, contact_radii, contact_type, names)]
+        contacts = [contacts;ContactConstraint(contact_type, block, normals; friction_coefficients, contact_origins, contact_radii, names)]
     end
 
     mechanism = Mechanism(mechanism.origin, mechanism.bodies, mechanism.joints, contacts;

DojoEnvironments/src/mechanisms/block2d/mechanism.jl

@@ -51,7 +51,7 @@ function get_block2d(;
         ]
         contact_radii = fill(contact_radius,n)
 
-        contacts = [contacts;contact_constraint(block, normals; friction_coefficients, contact_origins, contact_radii, contact_type, names)]
+        contacts = [contacts;ContactConstraint(contact_type, block, normals; friction_coefficients, contact_origins, contact_radii, names)]
     end
 
     mechanism = Mechanism(mechanism.origin, mechanism.bodies, mechanism.joints, contacts;

DojoEnvironments/src/mechanisms/halfcheetah/mechanism.jl

@@ -52,7 +52,7 @@ function get_halfcheetah(;
             contact_bodies[1].shape.shapes[1].rh[1]
             contact_bodies[2].shape.shapes[1].rh[1]
         ]
-        contacts = [contacts;contact_constraint(contact_bodies, normals; friction_coefficients, contact_origins, contact_radii)]
+        contacts = [contacts;ContactConstraint(NonlinearContact(contact_bodies, normals, friction_coefficients; contact_origins, contact_radii))]
     end
 
     if contact_body
@@ -75,7 +75,7 @@ function get_halfcheetah(;
             contact_bodies[3].shape.shapes[1].shapes[1].rh[1]
             [contact_bodies[i].shape.shapes[1].rh[1] for i = 4:n]
         ]
-        contacts = [contacts;contact_constraint(contact_bodies, normals; friction_coefficients, contact_origins, contact_radii)]
+        contacts = [contacts;ContactConstraint(NonlinearContact(contact_bodies, normals, friction_coefficients; contact_origins, contact_radii))]
     end
 
     mechanism = Mechanism(mechanism.origin, mechanism.bodies, mechanism.joints, contacts; 

DojoEnvironments/src/mechanisms/hopper/mechanism.jl

@@ -49,7 +49,7 @@ function get_hopper(;
             contact_bodies[1].shape.shapes[1].rh[1]
             contact_bodies[2].shape.shapes[1].rh[1]
         ]
-        contacts = [contacts;contact_constraint(contact_bodies, normals; friction_coefficients, contact_origins, contact_radii)]
+        contacts = [contacts;ContactConstraint(NonlinearContact(contact_bodies, normals, friction_coefficients; contact_origins, contact_radii))]
     end
 
     if contact_body
@@ -61,7 +61,7 @@ function get_hopper(;
         friction_coefficients = fill(friction_coefficient,n)
         contact_origins = [[0;0; 0.5 * contact_bodies[i].shape.shapes[1].rh[2]] for i = 1:n]
         contact_radii = [contact_bodies[i].shape.shapes[1].rh[1] for i = 1:n]
-        contacts = [contacts;contact_constraint(contact_bodies, normals; friction_coefficients, contact_origins, contact_radii)]
+        contacts = [contacts;ContactConstraint(NonlinearContact(contact_bodies, normals, friction_coefficients; contact_origins, contact_radii))]
     end
 
     mechanism = Mechanism(mechanism.origin, mechanism.bodies, mechanism.joints, contacts; 

DojoEnvironments/src/mechanisms/humanoid/mechanism.jl

@@ -51,7 +51,7 @@ function get_humanoid(;
             contact_bodies[4].shape.shapes[1].shapes[1].rh[1]
         ]
 
-        contacts = [contacts;contact_constraint(contact_bodies, normals; friction_coefficients, contact_origins, contact_radii)]
+        contacts = [contacts;ContactConstraint(NonlinearContact(contact_bodies, normals, friction_coefficients; contact_origins, contact_radii))]
     end
 
     mechanism = Mechanism(mechanism.origin, mechanism.bodies, mechanism.joints, contacts;

DojoEnvironments/src/mechanisms/quadrotor/mechanism.jl

@@ -45,7 +45,7 @@ function get_quadrotor(;
             [0; 0.21; 0.045], 
             [0; -0.21; 0.045]
         ]
-        contacts = [contacts;contact_constraint(body_in_contact, normals; friction_coefficients, contact_radii, contact_origins)]
+        contacts = [contacts;ContactConstraint(NonlinearContact(body_in_contact, normals, friction_coefficients; contact_radii, contact_origins))]
     end
 
     if contact_body
@@ -60,7 +60,7 @@ function get_quadrotor(;
             [0; 0.11; -0.085], 
             [0; -0.11; -0.085]
         ]
-        contacts = [contacts;contact_constraint(body_in_contact, normals; friction_coefficients, contact_origins)]
+        contacts = [contacts;ContactConstraint(NonlinearContact(body_in_contact, normals, friction_coefficients; contact_origins))]
     end
 
     mechanism = Mechanism(mechanism.origin, mechanism.bodies, mechanism.joints, contacts; 

DojoEnvironments/src/mechanisms/quadruped/mechanism.jl

@@ -66,7 +66,7 @@ function get_quadruped(;
         friction_coefficients = fill(friction_coefficient,n)
         contact_origins = fill([-0.006; 0; -0.092],n)
         contact_radii = fill(0.021,n)
-        contacts = [contacts;contact_constraint(contact_bodies, normals; friction_coefficients, contact_origins, contact_radii, names)]
+        contacts = [contacts;ContactConstraint(NonlinearContact(contact_bodies, normals, friction_coefficients; contact_origins, contact_radii); names)]
     end
 
     if contact_body
@@ -84,7 +84,7 @@ function get_quadruped(;
             [-0.005; 0.023; -0.16],
         ]
         contact_radii = fill(0.023,n)
-        contacts = [contacts;contact_constraint(contact_bodies, normals; friction_coefficients, contact_origins, contact_radii, names)]
+        contacts = [contacts;ContactConstraint(NonlinearContact(contact_bodies, normals, friction_coefficients; contact_origins, contact_radii); names)]
 
         # hip contacts
         body_names = [:FR_hip, :FL_hip, :RR_hip, :RL_hip]
@@ -95,7 +95,7 @@ function get_quadruped(;
         friction_coefficients = fill(friction_coefficient,n)
         contact_origins = fill([0; 0.05; 0],n)
         contact_radii = fill(0.05,n)
-        contacts = [contacts;contact_constraint(contact_bodies, normals; friction_coefficients, contact_origins, contact_radii, names)]
+        contacts = [contacts;ContactConstraint(NonlinearContact(contact_bodies, normals, friction_coefficients; contact_origins, contact_radii); names)]
     end
 
     mechanism = Mechanism(mechanism.origin, mechanism.bodies, mechanism.joints, contacts;

DojoEnvironments/src/mechanisms/raiberthopper/mechanism.jl

@@ -46,13 +46,13 @@ function get_raiberthopper(;
     if contact_foot
         # foot contact
         contact_radius = foot_radius
-        contacts = [contacts;contact_constraint(foot, Z_AXIS; friction_coefficient, contact_radius)]
+        contacts = [contacts;ContactConstraint(NonlinearContact(foot, Z_AXIS, friction_coefficient; contact_radius))]
     end
 
     if contact_body
         # body contact
         contact_radius = body_radius
-        contacts = [contacts;contact_constraint(body, Z_AXIS; friction_coefficient, contact_radius)]
+        contacts = [contacts;ContactConstraint(NonlinearContact(body, Z_AXIS, friction_coefficient; contact_radius))]
     end
 
     mechanism = Mechanism(mechanism.origin, mechanism.bodies, mechanism.joints, contacts; 

DojoEnvironments/src/mechanisms/snake/mechanism.jl

@@ -55,7 +55,7 @@ function get_snake(;
             fill(X_AXIS*link_length/2, Int64(n/2))
             fill(-X_AXIS*link_length/2, Int64(n/2))
         ]
-        contacts = [contacts;contact_constraint(contact_bodies, normals; friction_coefficients, contact_origins, contact_type)]
+        contacts = [contacts;ContactConstraint(contact_type, contact_bodies, normals; friction_coefficients, contact_origins)]
     end
 
     mechanism = Mechanism(mechanism.origin, mechanism.bodies, mechanism.joints, contacts;

DojoEnvironments/src/mechanisms/sphere/mechanism.jl

@@ -37,7 +37,7 @@ function get_sphere(;
 
     if contact
         contact_radius = radius
-        contacts = [contacts;contact_constraint(sphere, Z_AXIS; friction_coefficient, contact_radius, contact_type)]
+        contacts = [contacts;ContactConstraint(contact_type, sphere, Z_AXIS; friction_coefficient, contact_radius)]
     end
 
     mechanism = Mechanism(mechanism.origin, mechanism.bodies, mechanism.joints, contacts;

DojoEnvironments/src/mechanisms/tippetop/mechanism.jl

@@ -50,7 +50,7 @@ function get_tippetop(;
         normals = fill(Z_AXIS,n)
         friction_coefficients = fill(friction_coefficient,n)
         contact_radii = [radius;radius*scale]
-        contacts = [contacts;contact_constraint(bodies, normals; friction_coefficients, contact_radii, contact_type)]
+        contacts = [contacts;ContactConstraint(contact_type, bodies, normals; friction_coefficients, contact_radii)]
     end
 
     mechanism = Mechanism(mechanism.origin, mechanism.bodies, mechanism.joints, contacts;

DojoEnvironments/src/mechanisms/twister/mechanism.jl

@@ -56,7 +56,7 @@ function get_twister(;
             [X_AXIS*height/2]  
             fill(-X_AXIS*height/2,n-1)
         ]
-        contacts = [contacts;contact_constraint(contact_bodies, normals; friction_coefficients, contact_origins, contact_type)]
+        contacts = [contacts;ContactConstraint(contact_type, contact_bodies, normals; friction_coefficients, contact_origins)]
     end
 
     mechanism = Mechanism(mechanism.origin, mechanism.bodies, mechanism.joints, contacts;

DojoEnvironments/src/mechanisms/uuv/mechanism.jl

@@ -42,7 +42,7 @@ function get_uuv(;
             [0.12; 0; 0.07], 
             [-0.12; 0; 0.07], 
         ]
-        contacts = [contacts;contact_constraint(body_in_contact, normals; friction_coefficients, contact_radii, contact_origins)]
+        contacts = [contacts;ContactConstraint(NonlinearContact(body_in_contact, normals, friction_coefficients; contact_radii, contact_origins))]
     end
 
     mechanism = Mechanism(mechanism.origin, mechanism.bodies, mechanism.joints, contacts; 

DojoEnvironments/src/mechanisms/walker/mechanism.jl

@@ -56,7 +56,7 @@ function get_walker(;
             contact_bodies[3].shape.shapes[1].rh[1]
             contact_bodies[4].shape.shapes[1].rh[1]
         ]
-        contacts = [contacts;contact_constraint(contact_bodies, normals; friction_coefficients, contact_origins, contact_radii)]
+        contacts = [contacts;ContactConstraint(NonlinearContact(contact_bodies, normals, friction_coefficients; contact_origins, contact_radii))]
     end
 
     if contact_body
@@ -68,7 +68,7 @@ function get_walker(;
         friction_coefficients = fill(friction_coefficient,n)
         contact_origins = [[0;0; 0.5 * contact_bodies[i].shape.shapes[1].rh[2]] for i = 1:n]
         contact_radii = [contact_bodies[i].shape.shapes[1].rh[1] for i = 1:n]
-        contacts = [contacts;contact_constraint(contact_bodies, normals; friction_coefficients, contact_origins, contact_radii)]
+        contacts = [contacts;ContactConstraint(NonlinearContact(contact_bodies, normals, friction_coefficients; contact_origins, contact_radii))]
     end
 
     mechanism = Mechanism(mechanism.origin, mechanism.bodies, mechanism.joints, contacts; 

Project.toml

@@ -6,6 +6,7 @@ version = "0.7.2"
 [deps]
 Colors = "5ae59095-9a9b-59fe-a467-6f913c188581"
 CoordinateTransformations = "150eb455-5306-5404-9cee-2592286d6298"
+DifferentiableCollisions = "64b45163-ce2a-43f6-8f64-55d720633796"
 DocStringExtensions = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae"
 FFMPEG = "c87230d0-a227-11e9-1b43-d7ebe4e7570a"
 FiniteDiff = "6a86dc24-6348-571c-b903-95158fe2bd41"
@@ -26,6 +27,7 @@ Statistics = "10745b16-79ce-11e8-11f9-7d13ad32a3b2"
 [compat]
 Colors = "<0.12.10, 0.12.10"
 CoordinateTransformations = "<0.6.3, 0.6.3"
+DifferentiableCollisions = "<0.1.4, 0.1.4"
 DocStringExtensions = "<0.9.3, 0.9.3"
 FFMPEG = "0.4.0, 0.4.1"
 FiniteDiff = "2.0, 2.21"

contact_test.jl

@@ -0,0 +1,84 @@
+using Dojo
+import DifferentiableCollisions as dc
+using LinearAlgebra
+using Printf
+using StaticArrays
+import FiniteDiff as FD2
+import MeshCat as mc
+import Random
+using Colors
+using SparseArrays
+using Combinatorics
+
+
+function Gbar_2_body(state1, state2)
+    # attitude jacobian for two rigid bodies
+    Gbar1 = blockdiag(sparse(I(3)),sparse(Dojo.Lmat(state1.q2)*Dojo.Vᵀmat()))
+    Gbar2 = blockdiag(sparse(I(3)),sparse(Dojo.Lmat(state2.q2)*Dojo.Vᵀmat()))
+    Gbar = Matrix(blockdiag(Gbar1,Gbar2))
+end
+
+# modify the torque to account for the extra 2x from quaternion stuff
+const τ_mod = Diagonal(kron(ones(2),[ones(3);0.5*ones(3)]))
+
+function Dojo.impulse_map(relative::Symbol, model::Dojo.Contact, pbody::Dojo.Node, cbody::Dojo.Node)
+    pbody.shape.primitive.r = pbody.state.x2
+    pbody.shape.primitive.q = Dojo.vector(pbody.state.q2)
+    cbody.shape.primitive.r = cbody.state.x2
+    cbody.shape.primitive.q = Dojo.vector(cbody.state.q2)
+
+    D_α = reshape(dc.proximity_gradient(model.collision.primitive1,model.collision.primitive2)[2], 1, 14)
+    E = h * τ_mod * (D_α*Gbar_2_body(pbody.state, cbody.state))'
+
+    if relative == :parent 
+        return E[1:6]
+    elseif relative == :child 
+        return E[7:12]
+    end
+end
+
+# time step
+const h = 0.05
+
+
+Random.seed!(1)
+
+origin = Origin()
+body1 = Cylinder(0.5,3.0,1;orientation_offset=Dojo.RotY(pi/2))
+body1.inertia = diagm([body1.inertia[3,3];body1.inertia[2,2];body1.inertia[1,1]])
+body2 = Sphere(1.0,1)
+body3 = Pyramid(1,1.5,1)
+body4 = Box(1,1,1,1)
+bodies = [body1;body2;body3;body4]
+joint1 = JointConstraint(Floating(origin, body1))
+joint2 = JointConstraint(Floating(origin, body2))
+joint3 = JointConstraint(Floating(origin, body3))
+joint4 = JointConstraint(Floating(origin, body4))
+joints = [joint1;joint2;joint3;joint4]
+dojo_contacts = ContactConstraint(ImpactContact(bodies))
+
+mech = Mechanism(origin, bodies, joints, dojo_contacts; timestep=h)
+
+# create the indexing named tuple
+N_bodies = length(bodies) 
+
+# initial conditions of everything
+rs = [5*(@SVector randn(3)) for i = 1:N_bodies]
+rs[end] = SA[0,0,0.0]
+qs = [SA[1,0,0,0.0] for i = 1:N_bodies]
+
+# initial velocities
+vs =  [SA[1,1,1.0] for i = 1:N_bodies]
+for i = 1:N_bodies
+    vs[i] = -.5*rs[i]
+end
+ωs = [deg2rad.(20*(@SVector randn(3))) for i = 1:N_bodies]
+
+for (i,joint) in enumerate(mech.joints)
+    set_minimal_coordinates!(mech, joint, [rs[i];0;0;0])
+    set_minimal_velocities!(mech, joint, [vs[i];ωs[i]])
+end
+
+storage = simulate!(mech,79*0.05;record=true)
+
+visualize(mech, storage; visualize_floor=false, framerate=Inf)