Merge pull request #95 from gladisor/main

Allows for collisions between spheres and boxes

scripts/test_grad.jl

@@ -0,0 +1,71 @@
+using Dojo, StaticArrays
+
+m = 10.0
+
+pad_r = 0.5
+pad_m = m * 4/3 * pi * pad_r ^ 3
+
+box_x = 20.0
+box_y = 1.0
+box_z = 1.0
+box_m = m * box_x * box_y * box_z
+μ = 0.50
+
+spring = 2000.0
+damper = 0.0
+
+origin = Origin()
+pad = Dojo.Sphere(pad_r, pad_m)
+box = Dojo.Box(box_x, box_y, box_z, box_m)
+bodies = [pad, box]
+
+# pad_joint = JointConstraint(Planar(
+#     origin, pad, Y_AXIS, 
+#     spring = spring,
+#     damper = damper,
+#     child_vertex = - Z_AXIS * (pad_r + box_z),
+#     ))
+
+pad_joint = JointConstraint(Prismatic(
+    origin, pad, Dojo.X_AXIS, 
+    spring = spring,
+    damper = damper,
+    child_vertex = - Dojo.Z_AXIS * (pad_r + box_z),
+    ))
+
+box_joint = JointConstraint(Prismatic(
+    origin, box, Dojo.X_AXIS, 
+    child_vertex = -Dojo.Z_AXIS * box_z / 2 .+ Dojo.X_AXIS * box_x / 2 .- 2.0 * Dojo.X_AXIS * pad_r))
+
+joints = [pad_joint, box_joint]
+
+collision = SphereBoxCollision{Float64, 2, 3, 6}(szeros(3), box_x, box_y, box_z, pad_r)
+friction_parameterization = SA{Float64}[
+    1.0  0.0
+    0.0  1.0]
+contact = NonlinearContact{Float64, 8}(μ, friction_parameterization, collision)
+contacts = [ContactConstraint((contact, pad.id, box.id), name = :pad_belt)]
+
+mech = Mechanism(origin, bodies, joints, contacts)
+
+zero_coordinates!(mech)
+zero_velocities!(mech)
+# set_maximal_configurations!(pad, x = Z_AXIS * pad_r * 5.0)
+
+u = zeros(input_dimension(mech))
+u[1] = 0.0 #-100#-100#0.0 ## z axis force on pad
+# u[2] = 0.0 ## x axis force on pad
+# u[3] = 26.0 ## x axis force on box
+u[2] = 380.0
+
+K = 1000
+storage = Storage(K, length(mech.bodies))
+
+for k in 1:K
+    Jx, Ju = get_minimal_gradients!(mech, get_minimal_state(mech), u)
+    Dojo.save_to_storage!(mech, storage, k)
+end
+
+vis = Visualizer()
+open(vis)
+visualize(mech, storage, vis = vis)

src/Dojo.jl

@@ -3,6 +3,10 @@ module Dojo
 # constants
 global const REG = 1.0e-10::Float64
 
+global const X_AXIS = [1;0;0]
+global const Y_AXIS = [0;1;0]
+global const Z_AXIS = [0;0;1]
+
 #TODO: remove
 using FiniteDiff
 

src/gradients/data.jl

@@ -149,41 +149,85 @@ function body_constraint_jacobian_joint_data(mechanism::Mechanism{T}, body::Body
     return [∇u ∇spring ∇damper]
 end
 
-function body_constraint_jacobian_contact_data(mechanism::Mechanism, body::Body{T},
-        contact::ContactConstraint{T,N,Nc,Cs,N½}) where {T,N,Nc,Cs<:NonlinearContact{T,N},N½}
-    Nd = data_dim(contact)
+# function body_constraint_jacobian_contact_data(mechanism::Mechanism, body::Body{T},
+#         contact::ContactConstraint{T,N,Nc,Cs,N½}) where {T,N,Nc,Cs<:NonlinearContact{T,N},N½}
+#     Nd = data_dim(contact)
+#     model = contact.model
+#     contact_radius = model.collision.contact_radius
+#     offset = model.collision.contact_normal' * contact_radius
+#     xp3, qp3 = next_configuration(get_body(mechanism, contact.parent_id).state, mechanism.timestep)
+#     xc3, qc3 = next_configuration(get_body(mechanism, contact.child_id).state, mechanism.timestep)
+
+#     γ = contact.impulses[2]
+
+#     ∇friction_coefficient = szeros(T,3,1)
+
+#     X = force_mapping(:parent, model, xp3, qp3, xc3, qc3)
+#     ∇p = - ∂skew∂p(VRmat(qp3) * LᵀVᵀmat(qp3) * X * γ)
+#     ∇contact_radius = - ∂skew∂p(VRmat(qp3) * LᵀVᵀmat(qp3) * X * γ) * -rotation_matrix(inv(qp3)) * model.collision.contact_normal'
+
+#     ∇X = szeros(T,3,Nd)
+#     ∇Q = -[∇friction_coefficient ∇contact_radius ∇p]
+#     return [∇X; ∇Q]
+# end
+
+function body_constraint_jacobian_contact_data(mechanism::Mechanism, body::Body{T}, contact::ContactConstraint{T,N,Nc,Cs,N½}) where {T,N,Nc,Cs<:NonlinearContact{T,N},N½}
+    Nd = Dojo.data_dim(contact)
     model = contact.model
-    contact_radius = model.collision.contact_radius
-    offset = model.collision.contact_normal' * contact_radius
-    xp3, qp3 = next_configuration(get_body(mechanism, contact.parent_id).state, mechanism.timestep)
-    xc3, qc3 = next_configuration(get_body(mechanism, contact.child_id).state, mechanism.timestep)
+    xp3, qp3 = Dojo.next_configuration(get_body(mechanism, contact.parent_id).state, mechanism.timestep)
+    xc3, qc3 = Dojo.next_configuration(get_body(mechanism, contact.child_id).state, mechanism.timestep)
 
     γ = contact.impulses[2]
 
     ∇friction_coefficient = szeros(T,3,1)
 
-    X = force_mapping(:parent, model, xp3, qp3, xc3, qc3)
-    ∇p = - ∂skew∂p(VRmat(qp3) * LᵀVᵀmat(qp3) * X * γ)
-    ∇contact_radius = - ∂skew∂p(VRmat(qp3) * LᵀVᵀmat(qp3) * X * γ) * -rotation_matrix(inv(qp3)) * model.collision.contact_normal'
+    X = Dojo.force_mapping(:parent, model, xp3, qp3, xc3, qc3)
+    ∇p = - Dojo.∂skew∂p(Dojo.VRmat(qp3) * Dojo.LᵀVᵀmat(qp3) * X * γ)
+
+    cn = contact_normal(model.collision, xp3, qp3, xc3, qc3)
+    ∇contact_radius = - Dojo.∂skew∂p(Dojo.VRmat(qp3) * Dojo.LᵀVᵀmat(qp3) * X * γ) * -Dojo.rotation_matrix(inv(qp3)) * cn'
 
     ∇X = szeros(T,3,Nd)
     ∇Q = -[∇friction_coefficient ∇contact_radius ∇p]
     return [∇X; ∇Q]
 end
 
+# function contact_constraint_jacobian_contact_data(mechanism::Mechanism, contact::ContactConstraint{T,N,Nc,Cs,N½}, body::Body{T}) where {T,N,Nc,Cs<:NonlinearContact{T,N},N½}
+#     Nd = data_dim(contact)
+#     model = contact.model
+
+#     xp3, vp25, qp3, ωp25 = next_configuration_velocity(get_body(mechanism, contact.parent_id).state, mechanism.timestep)
+#     xc3, vc25, qc3, ωc25 = next_configuration_velocity(get_body(mechanism, contact.child_id).state, mechanism.timestep)
+
+#     s = contact.impulses_dual[2]
+#     γ = contact.impulses[2]
+
+#     ∇friction_coefficient = SA[0,γ[1],0,0]
+#     ∇contact_radius = [-model.collision.contact_normal; szeros(T,1,3); -model.collision.contact_tangent * skew(vector_rotate(ωp25, qp3))] * model.collision.contact_normal'
+#     ∇p = [model.collision.contact_normal * rotation_matrix(qp3); szeros(T,1,3); model.collision.contact_tangent * skew(vector_rotate(ωp25, qp3)) * rotation_matrix(qp3)]
+
+#     ∇compμ = szeros(T,N½,Nd)
+#     ∇g = -[∇friction_coefficient ∇contact_radius ∇p]
+
+#     return [∇compμ; ∇g]
+# end
+
 function contact_constraint_jacobian_contact_data(mechanism::Mechanism, contact::ContactConstraint{T,N,Nc,Cs,N½}, body::Body{T}) where {T,N,Nc,Cs<:NonlinearContact{T,N},N½}
-    Nd = data_dim(contact)
+    Nd = Dojo.data_dim(contact)
     model = contact.model
 
-    xp3, vp25, qp3, ωp25 = next_configuration_velocity(get_body(mechanism, contact.parent_id).state, mechanism.timestep)
-    xc3, vc25, qc3, ωc25 = next_configuration_velocity(get_body(mechanism, contact.child_id).state, mechanism.timestep)
+    xp3, vp25, qp3, ωp25 = Dojo.next_configuration_velocity(get_body(mechanism, contact.parent_id).state, mechanism.timestep)
+    xc3, vc25, qc3, ωc25 = Dojo.next_configuration_velocity(get_body(mechanism, contact.child_id).state, mechanism.timestep)
+
+    cn = contact_normal(model.collision, xp3, qp3, xc3, qc3)
+    ct = contact_tangent(model.collision, xp3, qp3, xc3, qc3)
 
     s = contact.impulses_dual[2]
     γ = contact.impulses[2]
 
     ∇friction_coefficient = SA[0,γ[1],0,0]
-    ∇contact_radius = [-model.collision.contact_normal; szeros(T,1,3); -model.collision.contact_tangent * skew(vector_rotate(ωp25, qp3))] * model.collision.contact_normal'
-    ∇p = [model.collision.contact_normal * rotation_matrix(qp3); szeros(T,1,3); model.collision.contact_tangent * skew(vector_rotate(ωp25, qp3)) * rotation_matrix(qp3)]
+    ∇contact_radius = [-cn; szeros(T,1,3); -ct * Dojo.skew(Dojo.vector_rotate(ωp25, qp3))] * cn'
+    ∇p = [cn * Dojo.rotation_matrix(qp3); szeros(T,1,3); ct * Dojo.skew(Dojo.vector_rotate(ωp25, qp3)) * Dojo.rotation_matrix(qp3)]
 
     ∇compμ = szeros(T,N½,Nd)
     ∇g = -[∇friction_coefficient ∇contact_radius ∇p]